/* Using the indirect addressing mode instructions ld and st
 * write a subroutine to restore registers  r0 to r15 from a 
 * stack buffer located at SRAM 0x04FF
 * Hint: SRAM address 0x0000 to 0x0001F map to registers r0 to r31 
 */

.INCLUDE <m328pdef.inc>

Setup:
    ldi  YH,0x05   ; YH = r29, initialize register stack pointer
	ldi  YL,0x00   ; YL = r28, pre-decrement, first saved location is 04FF

    rcall PreReg0_20
	rcall SaveReg0_15
	rcall Clr0_15
	rcall RestoreReg15_0

done:
	rjmp  done


RestoreReg15_0:
    clr  XH      ; point X at register r15
	ldi  XL, 16  ; after pre-decrement (ld  -X)
    ldi  r24,16  ; loop 16 times (r15...r0) 
pop_loop:
    ld   r25,Y+
    st   -X, r25
	dec  r24
	brne pop_loop
	ret 

/* 
 * Save registers  r0 to r15 onto Stack Buffer 0x04FF
 */
SaveReg0_15:
    clr  XH      ; r27
	clr  XL      ; r26
	clr  r25     ; starting with r0
    ldi  r24,16  ; save 16 registers
save_reg:  
    ld   r25, X+
	st   -Y, r25
	dec  r24
	brne save_reg
    ret

/* 
 * Preload registers  r0 to r20 with values 21 to 1
 */
PreReg0_20:
    clr  XH      ; r27
	clr  XL      ; r26
    ldi  r24,21  ; clear registers r0 to r20
reg_loop:  
    st   X+,r24
	dec  r24
	brne reg_loop
	ret

/* 
 * Clear registers  r0 to r15
 */
Clr0_15:
    clr  XH      ; r27
	clr  XL      ; r26
	clr  r25
    ldi  r24,16  ; clear registers r0 to r15 
clr_loop:  
    st   X+,r25
	dec  r24
	brne clr_loop
	ret