; ***************************************************************************
; 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.   *
; ***************************************************************************


; Needed vars:
; - HEAP_START
; - HEAP_SIZE


; ***************************************************************************
; defines



.equ HEAP_HEADER_BIT_USED = 0 



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

.dseg


heapPtr:     .byte 2
heapUsed:    .byte 2
heapFree:    .byte 2
heapDblFree: .byte 1



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

.cseg



; ---------------------------------------------------------------------------
; @routine Heap_Init

Heap_Init:
  ldi xl, LOW(HEAP_START)
  ldi xh, HIGH(HEAP_START)
  ldi r24, LOW((HEAP_SIZE-8) & 0xfffc)  ; size minus chunk header, chunk footer, start, end
  ldi r25, HIGH((HEAP_SIZE-8) & 0xfffc) ; we allocate multiples of 4 bytes
  clr r16
  sts heapUsed, r16
  sts heapUsed+1, r16
  ; write start header (two zero bytes)
  st X+, r16
  st X+, r16
  ; X now points to first chunk
  sts heapPtr, xl            ; store global vars
  sts heapPtr+1, xh
  sts heapFree, r24
  sts heapFree+1, r25
  ; store header of first chunk (bit0=0, meaning: free chunk)
  st X+, r24
  st X+, r25
  add xl, r24
  adc xh, r25
  ; store footer of first chunk
  st X+, r24
  st X+, r25
  ; write end header (two zero bytes)
  st X+, r16
  st X+, r16
  ; okay
  sec
  ret
; @end



; ---------------------------------------------------------------------------
; @routine Heap_Alloc
;
; @param r25:r24 number of bytes to alloc
; @return CFLAG set of okay, cleared otherwise
; @return X start of allocated memory
; @clobbers r16, r17, r18, r19, r24, r25, X

Heap_Alloc:
  rjmp heapAllocFirstFit
; @end



; ---------------------------------------------------------------------------
; @routine Heap_AllocAndZero
;
; Allocates data on the heap and presets all bytes with zero.
;
; @param r25:r24 number of bytes to alloc
; @return CFLAG set of okay, cleared otherwise
; @return X start of allocated memory
; @clobbers r16, r17, r18, r19, r24, r25, X

Heap_AllocAndZero:
  push r24
    push r25
      rcall heapAllocFirstFit
    pop r25
  pop r24
  brcc Heap_AllocAndZero_ret
  push xl
    push xh
      clr r18
Heap_AllocAndZero_loop:
        st X+, r18
        sbiw r25:r24, 1
      brne Heap_AllocAndZero_loop
    pop xh
  pop xl
  sec
Heap_AllocAndZero_ret:
  ret
; @end




; ---------------------------------------------------------------------------
; @routine heapAllocFirstFit
;
; @param r25:r24 number of bytes to alloc
; @return CFLAG set if okay, cleared otherwise
; @return X start of allocated memory
; @clobbers r16, r17, r18, r19, r24, r25, X

heapAllocFirstFit:
  adiw r25:r24, 3 ; align size to next multiple of 4
  andi r24, 0xfc  ; mask lower two bits
  
  lds xl, heapPtr
  lds xh, heapPtr+1

heapAllocFirstFit_loop:
    ; read chunk header
    ld r18, X+
    ld r19, X+                      ; X now points to first data byte of chunk
    ; zero-header found?
    mov r16, r18
    or r16, r19
    breq heapAllocFirstFit_memFull  ; yes, memory full
    ; chunk in use?
    sbrc r18, HEAP_HEADER_BIT_USED
      rjmp heapAllocFirstFit_next ; jump if used
    ; current chunk free, check size
    andi r18, 0xfc                ; clear lower two bits
    mov r16, r18
    mov r17, r19
    sub r16, r24
    sbc r17, r25                  ; r17:r16=remainder
    brcs heapAllocFirstFit_next   ; too small
    ; chunk size is okay, use it
    rcall heapUseChunk
    sec
    ret
heapAllocFirstFit_next:
    andi r18, 0xfc
    add xl, r18
    adc xh, r19
    adiw xh:xl, 2 ; skip footer
  rjmp heapAllocFirstFit_loop  

heapAllocFirstFit_memFull:
  bigcall Led1_SetFastTiming
  clc
  ret
; @end



; ---------------------------------------------------------------------------
; @routine heapUseChunk
;
; @param r25:r24 wanted size
; @param r17:r16 size of current chunk minus wanted size (remainder)
; @param X points to data of current chunk
; @clobbers 

heapUseChunk:
  push xl
    push xh
      tst r17
      brne heapUseChunk_split
      cpi r16, 8                     ; at least 8 bytes left?
      brcs heapUseChunk_directAlloc  ; nope, use full chunk
heapUseChunk_split:
      sbiw xh:xl, 2                  ; go back to chunk header
      ; X points to start of chunk header
      mov r18, r24
      sbr r18, (1<<HEAP_HEADER_BIT_USED)
      ; write new chunk start header
      st X+, r18                ; with used bit set
      st X+, r25                ; X now points to start of chunk data
      add xl, r24
      adc xh, r25
      ; X now points to the start of chunk footer
      st X+, r18                ; write new chunk footer (with USED bit set)
      st X+, r25                ; X now points behind chunk end header
      ; create chunk header for new free chunk
      subi r16, 4               ; sub chunk header and footer from size
      sbci r17, 0
      andi r16, 0xfc            ; clear USED bit
      ; store new heap pointer
      sts heapPtr, xl
      sts heapPtr+1, xh
      ; write new chunk header
      st X+, r16                ; write new chunk header (with USED bit cleared)
      st X+, r17                ; X now points to begin of next chunk data
      add xl, r16
      adc xh, r17               ; X now points behind chunk end header
      st X+, r16                ; write new chunk footer (with USED bit cleared)
      st X+, r17                ; X points to next chunk header
      ; update stats, pop X and return
      rjmp heapUseChunk_statsPopRet
heapUseChunk_directAlloc:
      ld r17, -X                          ; load allocated size from chunk header
      ld r16, -X
      mov r18, r16
      sbr r18, (1<<HEAP_HEADER_BIT_USED)  
      st X, r18                           ; set USED bit in chunk header
      adiw xh:xl, 2                       ; skip chunk header
      add xl, r16                         ; skip chunk data
      adc xh, r17
      st X, r18                           ; set USED bit in chunk footer
      adiw xh:xl, 2                       ; X points to next chunk header
      ; store new heap pointer
      sts heapPtr, xl
      sts heapPtr+1, xh
      ; update stats, pop X and return
heapUseChunk_statsPopRet:
      rcall heapAdjustStatsAlloc ; (r16, r17)
    pop xh
  pop xl
  sec
  ret
; @end



; ---------------------------------------------------------------------------
; @routine heapAdjustStatsAlloc
;
; @param r25:r24 size of allocated chunk
; @clobbers r16, r17

heapAdjustStatsAlloc:
  ; adjust heapUsed
  lds r16, heapUsed
  lds r17, heapUsed+1
  add r16, r24
  adc r17, r25
  sts heapUsed, r16
  sts heapUsed+1, r17
  
  ; adjust heapFree
  lds r16, heapFree
  lds r17, heapFree+1
  sub r16, r24
  sbc r17, r25
  sts heapFree, r16
  sts heapFree+1, r17
  ret
; @end



; ---------------------------------------------------------------------------
; @routine heapAdjustStatsFree
;
; @param r25:r24 size of released chunk
; @clobbers r16, r17

heapAdjustStatsFree:
  ; adjust heapUsed
  lds r16, heapUsed
  lds r17, heapUsed+1
  sub r16, r24
  sbc r17, r25
  sts heapUsed, r16
  sts heapUsed+1, r17
  
  ; adjust heapFree
  lds r16, heapFree
  lds r17, heapFree+1
  add r16, r24
  adc r17, r25
  sts heapFree, r16
  sts heapFree+1, r17
  ret
; @end



; ---------------------------------------------------------------------------
; @routine Heap_Free
;
; @param X pointer to previously allocated data
; @clobbers r16, r17, r18, r19, r24, r25, X

Heap_Free:
  sbiw xh:xl, 2 ; go back to chunk header
  ld r24, X+    ; read allocated size (and USED bit)
  ld r25, X+
  sbrs r24, HEAP_HEADER_BIT_USED
    rjmp Heap_Free_dblFree              ; not in use (double free!!)
  cbr r24, (1<<HEAP_HEADER_BIT_USED)    ; clear USED bit
  st -X, r25                            ; write back chunk header
  st -X, r24                            ; X now points to begin of chunk header
  push xl
    push xh
      adiw xh:xl, 2                     ; go skip chunk header
      add xl, r24                       ; skip data
      adc xh, r25
      st X+, r24                        ; write chunk footer
      st X+, r25                        ; X points to the next chunk header now
      rcall heapAdjustStatsFree         ; update stats (r16, r17)
      rcall heapCoalecseUp              ; try to coalecse following chunk with this (r16, r17, r18, r19, r24, r25, X)
    pop xh
  pop xl
  rcall heapCoalecseUp                  ; try to coalesce this chunk with predecessor (r16, r17, r18, r19, r24, r25, X)
  rjmp Heap_Free_ret
Heap_Free_dblFree:
  lds r24, heapDblFree
  lds r25, heapDblFree+1
  adiw r25:r24, 1
  breq Heap_Free_ret
  sts heapDblFree, r24
  sts heapDblFree+1, r25
Heap_Free_ret:
  ret
; @end




; ---------------------------------------------------------------------------
; @routine heapCoalecseUp
;
; Check whether the given chunk and the previous one are both free.
; If so combine them into a common free chunk.
;
; @param X pointer to chunk header
; @clobbers r16, r17, r18, r19, r24, r25, X

heapCoalecseUp:
  ; read current chunk header
  ld r24, X+
  ld r25, X
  sbiw xh:xl, 3
  ; read previous chunk footer
  ld r18, X+
  ld r19, X+
  ; current chunk=end_of_heap?
  mov r16, r24
  or r16, r25
  breq heapCoalecseUp_done
  ; current chunk in use?
  sbrc r24, HEAP_HEADER_BIT_USED
    rjmp heapCoalecseUp_done
  ; previous chunk=begin of heap?
  mov r16, r18
  or r16, r19
  breq heapCoalecseUp_done
  ; previous chunk in use?
  sbrc r18, HEAP_HEADER_BIT_USED
    rjmp heapCoalecseUp_done
  
  ; clear lower two bits
  andi r18, 0xfc
  andi r24, 0xfc
  
  ; now:
  ; - both chunks are free and can be combined
  ; - X points to beginning of current header

  ; calc combined size (sizes of both chunks plus 1 header and 1 footer)
  add r24, r18
  adc r25, r19
  ldi r16, 4
  clr r17
  add r24, r16
  adc r25, r17  ; r25:r24=combined size

  ; check for heapPtr
  lds r16, heapPtr
  lds r17, heapPtr+1
  sub r16, xl
  sbc r17, xh
  or r16, r17    ; r16=0 if heapPtr=current chunk

  sbiw xh:xl, 4  ; subtract previous footer and header from pos
  sub xl, r18    ; subtract size of previous chunk from pos
  sbc xh, r19    ; X now points to beginning of previous chunk
  
  ; test heapPtr
  tst r16
  brne heapCoalecseUp_l1
  ; adjust heapPtr
  sts heapPtr, xl
  sts heapPtr+1, xh
heapCoalecseUp_l1:
  ; write new combined header
  st X+, r24
  st X+, r25
  add xl, r24
  adc xh, r25
  ; write new combined footer
  st X+, r24
  st X+, r25
heapCoalecseUp_done:
  ret
; @end