.include "declare.s"

.org 0

; free: Free allocated memory.
; Input: A = Pointer to allocated memory.
; Output: none.
; Caller preserved registers: none.
; Callie preserved registers: A, B, Y.

free:
	pha.q		; Preserve A.
	phb.q		; Preserve B.
	phy.q		; Preserve Y.
	pha.q		; Push the pointer argument to the stack.
	and #0		; Reset A.
	tay		; Reset Y.
	sbs #$18	; Allocate 3 local variables onto the stack.
	lda.q sp+25	; Get the passed pointer.
	ora.d sp+29	; Is the passed pointer NULL?
	bne @getrealblk	; No, so get the real block.
	bra @end	; Yes, so we're done.
@getrealblk:
	lda.q sp+25	; Get the passed pointer.
	sec		; Prepare for a non borrowing subtract.
	sbc #8		; Move the passed pointer back by one.
	sta.q sp+25	; Set the first local variable to the start of the used block.
	lda.q (sp+25)	; Get the real block address.
	sta.q sp+25	; Set the passed pointer to the start of the real block.
	ldy #ublk.size	; Get the size of the real block.
	lda.q (sp+25), y;
	sta.q sp+17	; Save the size of the real block.
	clc		; Prepare for a non carrying add.
	adc.q sp+25	; Add the size of the real block with the start of the real block.
	cmp.q heapptr	; Is this block on top of the heap?
	bne @heapadd	; No, so add it to the free block list.
@dectop:
	lda.q sp+25	; Get the block.
	sta.q heapptr	; Set the top of the heap to it.
@chklastblk:
	lda.q heapl	; Get the last free list entry.
	sta.q sp+17	; Copy it into the second local variable.
	ora.d sp+21	; Is the free list empty?
	beq @end	; Yes, so we're done.
	lda.q sp+17	; No, so Get the last free list entry.
	ldy #fblk.size	; Get the size of the block.
	lda.q (sp+17), y;
	clc		; Prepare for a non carrying add.
	adc.q sp+17	; Add the size of the block, with the address of the block entry.
	cmp.q heapptr	; Is the last block on top of the heap?
	bne @end	; No, so we're done.
@delblk:
	lda.q sp+17	; Yes, so remove the last block.
	sta.q heapptr	;
@correctblk:
	ldy #fblk.prev	; Get the previous block.
	lda.q (sp+17), y;
	sta.q sp+25	; Save the previous block for now.
	sta.q heapl	; Set the last block to the previous block.
	ora.d sp+29	; Is the previous block non NULL?
	bne @delnxtblk	; Yes, so delete the next block.
	sta.q heapf	; No, so empty the free list.
	bra @end	; We are done.
@delnxtblk:
	and #0		; Reset A.
	ldy #fblk.next	; Delete the next block.
	sta.q (sp+25), y;
@end:
	ads #$20	; Clean up the stack frame.
	ply.q		; Restore Y.
	plb.q		; Restore B.
	pla.q		; Restore A.
	rts		; End of free.


@heapadd:
	lda.q heapf	; Get the first block.
	sta.q sp+9	; Copy it into the third local variable.
	ora.d sp+13	; Is the free list empty?
	bne @srchflst	; No, so start searching the free list.
@empty:
	ldy #fblk.next	; Clear the next block.
	sta.q (sp+25), y;
	ldy #fblk.prev	; Clear the previous block.
	sta.q (sp+25), y;
	lda.q sp+25	; Reset A.
	sta.q heapf	; Clear the first block entry.
	sta.q heapf	; Clear the last block entry.
	bra @end	; We are done.
@srchflst:
	and #0		; Reset A.
	sta.q sp+1	; Reset the left pointer.
	ldy #fblk.next	; Setup for the loop.
@loop:
	lda.q sp+9	; Get the right pointer.
	cmp.q sp+25	; Is the right pointer at, or below the current block?
	beq @nextright	; Yes, so get the next right pointer.
	bcs @chkrmerge	; No, so do the right block merge.
@nextright:
	sta.q sp+1	; Set the left pointer, to the right pointer.
	lda.q (sp+9), y	; Get the next right pointer.
	sta.q sp+9	; Set the current right pointer to the next right pointer.
	ora.d sp+13	; Is the next right pointer NULL?
	bne @loop	; No, so keep looping.
@st_lmerge2:
	sta.q (sp+25), y; Clear the next block.
	lda.q sp+25	; Get the current block.
	sta.q heapl	; Set the last free block entry to it.
	bra @chklmerge2	; Do the left block merge.
@chkrmerge:
	lda.q sp+25	; Get the current block.
	clc		; Prepare for a non carrying add.
	adc.q sp+17	; Add the size of the current block, to the current block.
	cmp.q sp+9	; Is the right pointer NULL?
	bne @normerge	; No, so don't merge the right block.
@rmerge:
	ldy #fblk.size	; Get the size of the right pointer.
	lda.q sp+17	; Get the size of the current block.
	clc		; Prepare for a non carrying add.
	adc.q (sp+9), y	; Add the size of the current block, with the size of the right pointer.
	sta.q (sp+25), y; Set the size of the current block, to the new size.
@rmerge2:
	ldy #fblk.next	; Get the next right pointer.
	lda.q (sp+9), y	;
	sta.q (sp+25), y; Set the next block, to the next right pointer.
	sta.q sp+17	; Save the next block in the second local variable.
	ora.d sp+21	; Is the next block NULL?
	beq @setheapl	; Yes, so set the last block.
@setprev:
	lda.q sp+25	; Get the current block.
	ldy #fblk.prev	; Set the previous block to the current block.
	sta.q (sp+17), y;
	bra @chklmerge	; Do the left block merge.
@setheapl:
	lda.q sp+25	; Get the current block.
	sta.q heapl	; Set the last block to the current block.
	bra @chklmerge	; Do the left block merge.
@normerge:
	lda.q sp+9	; Get the right pointer.
	ldy #fblk.next	; Set the next block to the right pointer.
	sta.q (sp+25), y;
	lda.q sp+25	; Get the current block.
	ldy #fblk.prev	; Set the previous right pointer to the current block.
	lda.q (sp+9), y	;
@chklmerge:
	lda.q sp+1	; Get the left pointer.
	ora.d sp+5	; Is the left pointer NULL?
	bne @chklmerge2	; No, so keep checking.
@newstart:
	ldy #fblk.prev	; Clear the previous block.
	sta.q (sp+25), y;
	lda.q sp+25	; Get the current block.
	sta.q heapf	; Set the first block, to the current block.
	bra @end2	; We are done.
@chklmerge2:
	ldy #fblk.size	; Get the size of the left block.
	lda.q (sp+1), y	;
	clc		; Prepare for a non carrying add.
	adc.q sp+1	; Add the size of the left block, to the left pointer.
	cmp.q sp+25	; Is the left block adjacent?
	bne @nolmerge	; No, so don't merge the left block.
@lmerge:
	lda.q (sp+1), y	; Get the size of the left block.
	clc		; Prepare for a non carrying add.
	ldb.q (sp+25), y; Get the size of the current block.
	adc b		; Add the size of the left block, with the size of the current block.
	sta.q (sp+1), y	; Set the size of the left block to the new size.
@lmerge2:
	ldy #fblk.next	; Get the next block.
	lda.q (sp+25), y;
	sta.q (sp+1), y	; Set the next left pointer, to the next block.
	sta.q sp+17	; Set the second local variable to the next block.
	ora.d sp+21	; Is the next left pointer NULL?
	beq @newlast	; Yes, so set the last block.
@lprev:
	lda.q sp+1	; Get the left pointer.
	ldy #fblk.prev	; Set the next left pointer's previous pointer to the left pointer.
	sta.q (sp+17), y;
	bra @end2	; We are done.
@newlast:
	lda.q sp+1	; Get the left pointer.
	sta.q heapl	; Set the last block, to the left pointer.
	bra @end2	; We are done.
@nolmerge:
	lda.q sp+25	; Get the current block.
	ldy #fblk.next	; Set the next left pointer, to the current block.
	sta.q (sp+1), y	;
@nolmerge2:
	lda.q sp+1	; Get the left pointer.
	ldy #fblk.prev	; Set the previous block, to the left pointer.
	sta.q (sp+25), y;
@end2:
	ads #$20	; Clean up the stack frame.
	ply.q		; Restore Y.
	plb.q		; Restore B.
	pla.q		; Restore A.
	rts		; End of free.

a
q