malloc for Game Boy

; Chunk header:
; word - Size of chunk
; word - Ptr to next chunk (NULL means last chunk). Bit 7 indicates if chunk is in use (set if so)
HEADER_SIZE equ 4

; Allocates a block of memory with the demanded size, using a best-fit strategy.
; @param bc The size of the block to be allocated.
; @return C Set if an error occurred.
; @return hl A pointer to the allocated data. Undefined if C is set
malloc:
    ; Temporarily add header size to requested size
    ld a, c
    add a, HEADER_SIZE
    ld c, a
    adc a, b
    sub c
    ld b, a

    ; Find block to split
    ld hl, Heap
    ld de, 0 ; No chunk found
.searchHeap
    ld a, [hli]
    ldh [hChunkSizeLow], a
    ld a, [hli]
    bit 7, a
    jr nz, .skip ; Used chunk
    ; Check if chunk size is sufficient
    cp b
    jr c, .skip
    jr nz, .suitable
    ldh a, [hChunkSizeLow]
    cp c
    jr c, .skip
    jr z, .perfectFit ; Just mark the chunk as used and return
 .suitable
    ; This chunk works, but is it better than the current one?
    dec hl
    dec hl
    ld a, d
    or e
    jr z, .firstChunk ; Anything is better than nothing
    ; Check if this chunk is smaller than the candidate one
    ; Compare high bytes
    inc de
    inc hl
    ld a, [de]
    dec de
    cp [hl]
    dec hl
    jr c, .better
    jr nz, .incThenSkip
    ; Compare low bytes
    ld a, [de]
    cp [hl]
    jr nc, .incThenSkip
.better
.firstChunk
    ld d, h
    ld e, l
.incThenSkip
    inc hl
    inc hl
.skip
    ld a, [hli]
    ld h, [hl]
    ld l, a
    or h
    jr nz, .searchHeap

    ; Check if a chunk has been found
    ld a, d
    or e
    jr nz, .chunkFound
    ld a, ENOMEM ; Or whatever else
    ld [wErrno], a
    scf
    ret

.perfectFit
    dec hl
    set 7, [hl]
    inc hl
    inc hl
    inc hl
    xor a
    ld [wErrno], a
    ret

.chunkFound

    ; Set pointer to new block to current block pointer + size
    ld h, d
    ld l, e

    ld a, e
    add a, c
    ld e, a
    ld a, d
    adc a, b
    ld d, a

    ; Set size of new block to current block size - size; set it to be free
    ld a, [hli]
    sub c
    ld [de], a
    inc de
    ld a, [hld]
    sbc b
    ; bit 7 should be reset, normally...
    ld [de], a
    inc de

    ; Set size of the current block to requested size (subtracting header size), and mark it as in use
    ld a, c
    sub HEADER_SIZE
    ld [hli], a
    ld a, b
    sbc 0
    or $80
    ld [hli], a

    ; Set next pointer of new block to next pointer of current block
    ld a, [hli]
    ld [de], a
    inc de
    ld a, [hld]
    ld [de], a

    ; Set next pointer of current block to point to new block
    ld a, e
    sub HEADER_SIZE - 1
    ld [hli], a
    ld a, d
    sbc 0
    ld [hli], a

    xor a
    ret