; ***************************************************************************
; copyright   : (C) 2025 by Martin Preuss
; email       : martin@libchipcard.de
;
; ***************************************************************************
; * This file is part of the project "AqHome".                              *
; * Please see toplevel file COPYING of that project for license details.   *
; ***************************************************************************

; Code for large pagesizes (128 and larger)


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

.cseg



; ---------------------------------------------------------------------------
; @routine Flash_Init
;
; @clobbers R16

Flash_Init:
  ldi r16, 0xff
  sts flashPageStart, r16
  sts flashPageStart+1, r16
  ret
; @end



; ---------------------------------------------------------------------------
; @routine Flash_Fini
;
; @clobbers (R0, R1, R16, R18, R19, R20, R21, R24, R25, X)

Flash_Fini:
  push r15
    in r15, SREG
    cli
    rcall flashEndPage             ; (R0, R1, R16, R18, R19, R20, R24, R25, X)
    out SREG, r15
  pop r15
  ret
; @end



; ---------------------------------------------------------------------------
; @routine Flash_WriteData
;
; @param Z destination address in FLASH memory (byte address as for LPM!)
; @param Y source address
; @param r17 number of bytes to write
; @clobbers (R0, R1, R16, R18, R19, R20, R24, R25, X)

Flash_WriteData:
  push r15
    in r15, SREG
    cli
Flash_WriteData_loop:
      rcall flashWriteData       ; (R0, R1, R16, R18, R19, R20, R24, R25, X)
      tst r17
    brne Flash_WriteData_loop
    out SREG, r15
  pop r15
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashWriteData
;
; Interrupts must be disabled!
;
; @return Z next dest position
; @return Y next source position
; @return R17 remainder of bytes to flash
; @param Z position to flash data to
; @param Y source pointer
; @param R17 number of bytes to flash
; @clobbers R16, R18, R24, R25, X (R0, R1, R19, R20)

flashWriteData:
  rcall flashCheckCurrentValid         ; (r18, r19)
  brcc flashWriteData_beginPage        ; jump if this is the first call (no page in work)

  rcall flashWriteCheckPage            ; (r18, r19)
  brcs flashWriteData_calcPosAndLength ; still inside current page, jump
  rcall flashEndPage                   ; (R0, R1, R16, R18, R19, R20, R24, R25, X)
flashWriteData_beginPage:
  rcall flashBeginPage                 ; (r16, r24, r25, X)
flashWriteData_calcPosAndLength:
  rcall flashCalcPosAndLength          ; r18=bytes_to_write, x=pos_in_buffer (r19, r24, r25)
  ; X=abs pos in buffer, r18=bytes to read, r17=bytes initially requested
  ; prepare data for return
  sub r17, r18                         ; r17 holds remainder
  add zl, r18                          ; Z points to next data
  adc zh, r18                          ; only add carry flag
  sub zh, r18
  ; actually write data into buffer
flashWriteData_copyLoop:
    ld r16, Y+
    st X+, r16
    dec r18
  brne flashWriteData_copyLoop
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashCalcPosAndLength
;
; @param  Z   destination pos
; @param  R17 number of bytes to write
; @return X   absolute write position inside buffer
; @return R18 bytes to write
; @clobbers r19, r24, r25

flashCalcPosAndLength:
  ; calc offset into buffer
  mov r24, zl
  mov r25, zh
  andi r24, LOW(FLASH_PAGESIZE-1)
  andi r25, HIGH(FLASH_PAGESIZE-1)   ; R25:r24=pos inside page
  ldi r18, LOW(FLASH_PAGESIZE)
  ldi r19, HIGH(FLASH_PAGESIZE)
  sub r18, r24
  sbc r19, r25                       ; R19:R18=FLASH_PAGESIZE-relPos
  tst r19                            ; high byte zero?
  breq flashCalcPosAndLength_8bit    ; yes, already 8-bit
  ldi r18, 255                       ; clip size to 8 bit
flashCalcPosAndLength_8bit:
  cp r17, r18                        ; r18>r17?
  brcc flashCalcPosAndLength_sizeOk  ; nope: jump
  mov r18, r17                       ; clip r18 to size requested
flashCalcPosAndLength_sizeOk:        ; r18=bytes to write
  ldi xl, LOW(flashPageBuffer)       ; set X to pos within page buffer
  ldi xh, HIGH(flashPageBuffer)
  add xl, r24
  adc xh, r25
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashWriteCheckPage
;
; check whether the given address is inside the current page in buffer
;
; @return CFLAG set if same page, cleared otherwise
; @param Z address to write to
; @clobbers r18, r19

flashWriteCheckPage:
  ; check low byte of page
  mov r18, zl
  lds r19, flashPageStart
  eor r18, r19
  cbr r18, LOW(FLASH_PAGESIZE-1)
  brne flashWriteCheckPage_ClcRet

  ; check high byte of page
  mov r18, zh
  lds r19, flashPageStart+1
  eor r18, r19
  cbr r18, HIGH(FLASH_PAGESIZE-1)
  brne flashWriteCheckPage_ClcRet
  sec
  ret
flashWriteCheckPage_ClcRet:
  clc
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashCheckCurrentValid
;
; check whether the page buffer contains a valid page (e.g. flashPageStart is not 0xffff)
;
; @return CFLAG set if current page is valid, cleared otherwise
; @clobbers r18, r19

flashCheckCurrentValid:
  lds r18, flashPageStart
  lds r19, flashPageStart+1
  and r18, r19
  inc r18
  breq flashCheckCurrentValid_ClcRet
  sec
  ret
flashCheckCurrentValid_ClcRet:
  clc
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashBeginPage
;
; Interrupts must be disabled!
;
; @param Z Address to read from (byte address as for LPM!)
; @clobbers (r16, r24, r25, X)

flashBeginPage:
  push zl
    push zh
      cbr zl, LOW(FLASH_PAGESIZE-1)
      cbr zh, HIGH(FLASH_PAGESIZE-1)
      sts flashPageStart, zl
      sts flashPageStart+1, zh
      rcall flashReadPageIntoSram  ; (r16, r24, r25, X, Z)
    pop zh
  pop zl
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashEndPage
;
; Interrupts must be disabled!
;
; @clobbers (R0, R1, R16, R18, R19, R20, R24, R25, X)

flashEndPage:
  rcall flashCheckCurrentValid ; (r18, r19)
  brcs flashEndPage_write
  ret
flashEndPage_write:
  push zl
    push zh
      rcall flashErasePage  ; (R16, R20, Z)
      rcall flashWritePage  ; (r0, r1, r20, r24, r25, X, Z)
    pop zh
  pop zl
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashReadPageIntoSram
;
; @param Z Address to read from (byte address as for LPM!)
; @clobbers r16, r24, r25, X, Z

flashReadPageIntoSram:
  ldi xl, LOW(flashPageBuffer)
  ldi xh, HIGH(flashPageBuffer)
  ldi r24, LOW(FLASH_PAGESIZE)
  ldi r25, HIGH(FLASH_PAGESIZE)
flashReadPageIntoSram_loop:
    lpm r16, Z+ 
    st X+, r16
    sbiw r25:r24, 1
  brne flashReadPageIntoSram_loop
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashErasePage
;
; Interrupts must be disabled!
;
; @clobbers R16, R20, Z

flashErasePage:
  lds zl, flashPageStart
  lds zh, flashPageStart+1
  ldi r20, (1<<PGERS) + (1<<SPMEN)          ; enable next SPM, erase page (R1/R0 ignored)
  rcall flashDoSpm                          ; (R16)
  ret
; @end



; ---------------------------------------------------------------------------
; @routine flashDoSpm
;
; wait until possible previous SPM finished and then issue another SPM.
;
; @param R20 value for register SPMCR
; @param R0  low value for SPM
; @param R1  high value for SPM
; @param Z   address for SPM (byte address!)
; @clobbers R16

flashDoSpm:
flashDoSpm_wait:         ; wait for possibly previous SPM to complete
.ifdef SPMCSR
    in r16, SPMCSR
.else
    in r16, SPMCR
.endif
    sbrc r16, SPMEN
  rjmp flashDoSpm_wait
  ; SPM timed sequence
.ifdef SPMCSR
  out SPMCSR, r20
.else
  out SPMCR, r20
.endif
  spm
  ret
; @end


