.include "declare.s"

.org 0

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

free:
	pha.q			; Preserve A.
	phb.q			; Preserve B.
	phx.q			; Preserve X.
	phy.q			; Preserve Y.
	phe.q			; Preserve E.
	and #0			; Reset A.
	tab			; Reset B.
	tax			; Reset X.
	tay			; Reset Y.
	mov a, d		; Get the passed pointer.
	bne @getrealblk		; The pointer isn't NULL, so get the real block.
	bra @end		; The pointer is NULL, so we're done.
@getrealblk:
	sub #8			; Get the start of the used block.
	mov.q a, (a)		; Get the start of the real block.
	mov.q b, (a+ublk.size)	; Get the size of the real block.
	lea e, (a+b)		; Add the size of the real block with the start of the real block.
	cmp.q e, heapptr	; Is this block on top of the heap?
	bne @heapadd		; No, so add it to the free block list.
@dectop:
	mov.q heapptr, (a)	; Set the top of the heap to the start of the previous real block.
@chklastblk:
	mov.q b, heapl		; Get the last free list entry.
	beq @end		; The free list is empty, so we're done.
	ldy #fblk.size		; Get the size of the block.
	mov.q a, (b+fblk.size)	; Get the size of the block.
	lea e, (a+b)		; Add the size of the block, with the address of the block entry.
	cmp.q e, heapptr	; Is the last block on top of the heap?
	bne @end		; No, so we're done.
@delblk:
	mov.q heapptr, b	; Yes, so remove the last block.
@correctblk:
	mov.q a, (b+fblk.prev)	; Get the previous block.
	sta.q heapl		; Set the last block to the previous block.
	bne @delnxtblk		; The previous block isn't NULL, so delete the next block.
	sta.q heapf		; The previous block is NULL, so empty the free list.
	bra @end		; We are done.
@delnxtblk:
	lea e, (a+fblk.next)	; Delete the next block.
	stz.q (e)		;
@end:
	ple.q			; Restore E.
	ply.q			; Restore Y.
	plx.q			; Restore X.
	plb.q			; Restore B.
	pla.q			; Restore A.
	rts			; End of free.


@heapadd:
	mov.q y, heapf		; Get the first block.
	bne @srchflst		; The Free list isn't empty, so start searching the free list.
@empty:
	mov.q (a+fblk.next), y	; Clear the next block.
	mov.q (a+fblk.prev), y	; Clear the previous block.
	sta.q heapf		; Set the first block entry to the current block.
	sta.q heapl		; Set the last block entry to the current block.
	bra @end		; We are done.
@srchflst:
@loop:
	cmp y, a		; 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:
	tyx			; Set the left pointer, to the right pointer.
	mov.q y, (y+fblk.next)	; Set the current right pointer to the next right pointer.
	bne @loop		; The next right pointer isn't NULL, so keep looping.
@st_lmerge2:
	mov.q (a+fblk.next), y	; Clear the next block.
	sta.q heapl		; Set the last free block entry to it.
	bra @chklmerge2		; Do the left block merge.
@chkrmerge:
	lea e, (a+b)		; Add the size of the current block, to the current block.
	cmp e, y		; Is the current block the same as the right block?
	bne @normerge		; No, so don't merge the right block.
@rmerge:
	mov e, b		; Get the size of the current block.
	add.q e, (y+fblk.size)	; Add the size of the current block, with the size of the right pointer.
	mov.q (a+fblk.size), e	; Set the size of the current block, to the new size.
@rmerge2:
	lea fblk.next		; Get the next right pointer.
	mov.q (a+e), (y+e)	; Set the next block, to the next right pointer.
	mov.q b, (y+e)		; Save the next block.
	beq @setheapl		; The next block is NULL, so set the last block.
@setprev:
	mov.q (b+fblk.prev), a	; Set the previous block to the current block.
	bra @chklmerge		; Do the left block merge.
@setheapl:
	sta.q heapl		; Set the last block to the current block.
	bra @chklmerge		; Do the left block merge.
@normerge:
	mov.q (a+fblk.next), y	; Set the next block to the right pointer.
	mov.q (y+fblk.prev), a	; Set the previous right pointer to the current block.
@chklmerge:
	and x, x		; Is the left pointer NULL?
	bne @chklmerge2		; No, so keep checking.
@newstart:
	mov.q (a+fblk.prev), x	;
	sta.q heapf		; Set the first block, to the current block.
	bra @end2		; We are done.
@chklmerge2:
	mov.q e, (x+fblk.size)	; Get the size of the left block.
	add e, x		; Add the size of the left block, to the left pointer.
	cmp e, a		; Is the left block adjacent?
	bne @nolmerge		; No, so don't merge the left block.
@lmerge:
	lea fblk.size		; Set the offset to the block size.
	add.q (x+e), (a+e)	; Add the size of the left block, with the size of the current block.
@lmerge2:
	lea fblk.next		; Set the offset to the next block.
	mov.q (x+e), (a+e)	; Set the next left pointer, to the next block.
	mov.q b, (a+e)		; Get the next block.
	beq @newlast		; The next block is NULL, so set the last block.
@lprev:
	mov.q (b+fblk.prev), x	; Set the next left pointer's previous pointer to the left pointer.
	bra @end2		; We are done.
@newlast:
	stx.q heapl		; Set the last block, to the left pointer.
	bra @end2		; We are done.
@nolmerge:
	mov.q (x+fblk.next), a	; Set the next left pointer, to the current block.
@nolmerge2:
	mov.q (a+fblk.prev), x	; Set the previous block, to the left pointer.
@end2:
	ple.q			; Restore E.
	ply.q			; Restore Y.
	plb.q			; Restore B.
	pla.q			; Restore A.
	rts			; End of free.

a
q