; ***************************************************************************
; data


.dseg

flashDataBegin:
  flashCurrentAddress:     .byte 2
  flashLastReceivedMsg:    .byte 2
flashDataEnd:




; ***************************************************************************
; code


.cseg


; ---------------------------------------------------------------------------
; start flashing a page
;
; IN:
; - Z: Address to work on (byte address as for LPM!)

Flash_StartPage:
  push zh
    push zl
      ldi r16, ((PAGESIZE*2)-1)
      and r16, zl
      rcall Flash_ReadPageIntoPageBuffer
    pop zl
  pop zh
  ret



; ---------------------------------------------------------------------------
; finish flashing a page
;
; IN:
; - Z: Address to work on (byte address as for LPM!)

Flash_FinishPage:
  ldi r16, ((PAGESIZE*2)-1)
  and r16, zl
  rcall Flash_ErasePage
  rcall Flash_WritePage
  ret



; ---------------------------------------------------------------------------
; Flash_ReadPageIntoPageBuffer
;
;
; IN:
; - Z: Address to read from (byte address as for LPM!)
; OUT:
; - nothing
; MODIFIED REGISTERS: R0, R1, R15, R16, R20, R24, R25, Z

Flash_ReadPageIntoPageBuffer:
  in r15, SREG
  cli
    ldi r24, LOW(PAGESIZE*2)
    ldi r25, HIGH(PAGESIZE*2)
Flash_ReadPageIntoPageBuffer_loop:
      lpm r0, Z+                                ; read source data from FLASH (low)
      lpm r1, Z                                 ; read source data from FLASH (high)
      sbiw ZH:ZL, 1                             ; rewind Z address for following SPM
      ldi r20, (1<<SPMEN)                       ; enable next SPM, write into temp page buffer
      rcall utilsDoSpm                          ; (R16)
      adiw ZH:ZL, 2
      sbiw r25:r24, 2                           ;use subi for PAGESIZEB<=256
    brne Flash_ReadPageIntoPageBuffer_loop
  out SREG, r15
  ret



; ---------------------------------------------------------------------------
; Flash_WriteIntoPage
;
;
; IN:
; - Z: Address to write to (byte address as for LPM!)
; - R1:R0: Word to write into page
; OUT:
; - Z: address for next write
; MODIFIED REGISTERS: R15, R16

Flash_WriteIntoPage:
  in r15, SREG
  cli
  ldi r20, (1<<SPMEN)                       ; enable next SPM, write into temp page buffer
  rcall utilsDoSpm                          ; (R16)
  ldi r16, 2
  add zl, r16
  dec r16                                   ; doesn't affect carry flag
  dec r16
  adc zh, r16
  out SREG, r15
  ret



; ---------------------------------------------------------------------------
; Flash_ErasePage
;
;
; IN:
; - Z: Address of the page to erase.
; OUT:
; - nothing
; MODIFIED REGISTERS: R0, R1, R15, R16, R18, R19, R20

Flash_ErasePage:
  in r15, SREG
  cli
    ldi r20, (1<<PGERS) + (1<<SPMEN)          ; enable next SPM, erase page (R1/R0 ignored)
    rcall utilsDoSpm                          ; (R16)
  out SREG, r15
  ret



; ---------------------------------------------------------------------------
; Flash_WritePage
;
;
; IN:
; - Z: Address of the page to erase.
; OUT:
; - nothing
; MODIFIED REGISTERS: R0, R1, R15, R16, R18, R19, R20

Flash_WritePage:
  in r15, SREG
  cli
    ldi r20, (1<<PGWRT) + (1<<SPMEN)          ; enable next SPM, write page (R1/R0 ignored)
    rcall utilsDoSpm                          ; (R16)
  out SREG, r15
  ret



; ---------------------------------------------------------------------------
; utilsDoSpm
;
; wait until possible previous SPM finished and then issue another SPM.
;
; IN:
; - R20: value for register SPMCR
; - R0:  low value for SPM
; - R1:  high value for SPM
; - Z  : address for SPM (byte address!)
; OUT:
; - nothing
; REGS: R16

utilsDoSpm:
wait:                ; wait for possibly previous SPM to complete
    in r16, SPMCSR
    sbrc r16, SPMEN
  rjmp wait
  ; SPM timed sequence
  out SPMCSR, r20
  spm
  ret