spmSPM:
movw r31:r30, r17:r16

rcall spmWait

in r20, SREG
cli

#ifdef __RAMPZ__
in r21, RAM_PZ
out RAM_PZ, r18
#endif

#ifdef __MEMSPM__
sts SPMREG, r22
#else
out SPMREG, r22
#endif

#ifdef __RAMPZ__
espm
#else
spm
#endif

;----------------------; Load address, encrypt block, send to usart
read_blockx: ;
ldi tmp2,st_ok ;
rcall us_tx ; Send command to the USART1
;----------------------;
lds zl,p_ah ; Init RAMPZ:Z pointer
out RAMPZ,zl ; Flash address RAMPZ0
lds zh,p_am ; Flash address M
lds zl,p_al ; Flash address L
ldi yl,low (bc_data) ; Encrypted AES data
ldi yh,high(bc_data) ; RAM pointer
;----------------------; Read 256 bytes from Flash
ldi loop3,0 ; amount
read_b_c: ;
elpm tmp2,z+ ; load FLASH byte, address=+1
st y+,tmp2 ; store byte in ram
dec loop3 ;
brne read_b_c ;
;----------------------; Encrypt block
ldi tmp,1 ; load aes ecncrypt mode
sts aesm,tmp ; AES Encryption
rcall _aesB ; Encrypt stored Block
;----------------------; Send block to USART1
ldi yl,low (bdc_data) ; Encrypted AES data
ldi yh,high(bdc_data) ; RAM pointer
ldi loop3,0 ; 256 bytes!
read_b_ce: ;
ld tmp2,y+ ;
rcall us_tx ; Send data to the USART1
dec loop3 ;
brne read_b_ce ;
;----------------------;
rjmp init_flags ; clear flags jump to boot_main

;----------------------;
write_blockx: ;
clr tmp ; selecl mode Decrypt
sts aesm,tmp ; save mode
rcall _aesB ; decrypr received block 256bytes
; save result block 256 bytes in (bdc_data)
lds zl,p_ah ; Init RAMPZ:Z pointer
out RAMPZ,zl ; Flash address RAMPZ0
lds zh,p_am ; Flash address M
lds zl,p_al ; Flash address L
;----------------------;
;ldi yl,low (bc_data) ; pure received data without decrypting
;ldi yh,high(bc_data) ; RAM pointer

ldi yl,low (bdc_data) ; Decrypted AES data
ldi yh,high(bdc_data) ; RAM pointer
rcall write_page ; Write page to flash!
wr_b_end: ;
rcall us_tx ; Send command to the USART1
rjmp init_flags ; clear flags jump to boot_main

; code from datasheet AT90CAN32/64/128
; Y = RAM data pointer
; RAMPZ0:Z = flash address pointer
; Edited lpm to ELPM!
; added Status wrote page!

Write_page: ; Write flash page 256 bytes
ldi tmp,(1<<PGERS)|(1<<SPMEN)
call Do_spm ; Page Erase
ldi tmp, (1<<RWWSRE)|(1<<SPMEN)
call Do_spm ; re-enable the RWW section
; transfer data from RAM to Flash page buffer
ldi xl,low (p_size) ; init loop variable
ldi xh,high(p_size) ; not required for PAGESIZEB<=256


Wrloop: ;
ld r0,Y+ ;
ld r1,Y+ ;
ldi tmp,(1<<SPMEN) ;
call Do_spm ;
adiw ZH:ZL,2 ;
sbiw xh:xl,2 ; use subi for PAGESIZEB<=256
brne Wrloop ;

;--------------------------; execute Page Write
subi ZL,low (p_size) ; restore pointer
sbci ZH,high(p_size) ; not required for PAGESIZEB<=256
ldi tmp, (1<<PGWRT) | (1<<SPMEN)
call Do_spm
; re-enable the RWW section
ldi tmp, (1<<RWWSRE) | (1<<SPMEN)
call Do_spm
; read back and check, optional
ldi xl,low (p_size) ; init loop variable
ldi xh,high(p_size) ; not required for PAGESIZEB<=256
subi YL,low (p_size) ; restore pointer
sbci YH,high(p_size) ;
;--------------------------;


Rdloop: ;
elpm r0,Z+ ; Load wrote data from flash
ld r1,Y+ ; Load right data from Ram
cpse r0,r1 ; compare and skip if equal
rjmp Loader_Error ; If not equal - STATUS ERROR!


rdl_p:
sbiw xh:xl,2 ; use subi for PAGESIZEB<=256
brne Rdloop ;
; return to RWW section
; verify that RWW section is safe to read

Return:
in tmp1,SPMCSR
sbrs tmp1,RWWSB ; If RWWSB is set, the RWW section is not ready yet
ldi tmp2,st_ok ;
ret
; re-enable the RWW section
ldi tmp,(1<<RWWSRE)|(1<<SPMEN)
call Do_spm ;
rjmp Return ;


Do_spm:

Wait_spm: ; check for previous SPM complete
in tmp1,SPMCSR
sbrc tmp1,SPMEN
rjmp Wait_spm
; input: spmcsrval determines SPM action

in tmp2,SREG ; disable interrupts if enabled, store status
cli

Wait_ee: ; check that no EEPROM write access is present
sbic EECR,EEWE ;
rjmp Wait_ee ;
out SPMCSR,tmp ; SPM timed sequence
spm
out SREG,tmp2 ; restore SREG (to enable interrupts if originally enabled)
ret ; return


loader_error: ;
ldi tmp2,st_err ; load status error
ret ; leave write_page