; Simple libc implementation for the SuB Suite

strtoull:
	phy.q		; Preserve Y.
	and #0		; Reset A.
	tay		; Reset Y.
	pha.q		; Reset the value buffer.
@loop:
	lda (sp+26), y	; Get a character from the string.
	pha		; Preserve the character.
	jsr isdigit	; Is this character, a digit?
	pla		; Get the character back.
	bne @digit	; Yes, so extract the value from it.
	jsr tolower	; No, so convert the character to lowercase.
	pha		; Preserve the character.
	jsr islower	; Is this an alphabetical character?
	pla		; Get the character back.
	beq @end	; No, so we're done.
@alpha:
	sub #'a'-10	; Yes, so get the numeric value from this digit.
	bra @chkbase	; Check if the value matches the base.
@digit:
	sub #'0'	; Get the numeric value from this digit.
@chkbase:
	cmp sp+25	; Does the value match the base?
	bcs @end	; No, so we're done.
@addval:
	tab		; Save the digit value.
	lda.q sp+1	; Get the value from the value buffer.
	mul sp+25	; Multiply the value by the base.
	add a, b	; Add the digit value to the total value.
	sta.q sp+1	; Place the value in the value buffer.
	iny		; Increment the string index.
	and #0		; Reset A.
	bra @loop	; Keep looping.
@end:
	pla.q		; Get the value buffer back.
	ply.q		; Get Y back.
	ldb #0		; Reset B.
	rts		; End of strtoull.


strlen:
	and #0		; Reset A.
@loop:
	inc		; Increment the index.
	cmp (d+a-1), #0	; Are we at the end of the string?
	bne @loop	; No, so keep looping.
@end:
	dec		; Decrement the length to not include the terminator.
	rts		; End of strlen.


strcmp:
	phb.q		; Preserve B.
	phx.q		; Preserve X.
	phy.q		; Preserve Y.
	and #0		; Reset A.
	tab		; Reset B.
	tax		; Reset X.
	tay		; Reset Y.
@loop:
	ldx #0		; Set the islong flag to false.
	mov a, (d+y)	; Are we at the end of the first string?
	beq cmpr	; Yes, so check if we're too short, or too long.
	ldx #1		; No, so set the islong flag to true.
	cmp a, (s+y)	; Is the character of both strings, the same?
	bne cmpr	; No, so check if we're too short, or too long.
	iny		; Yes, so increment the index.
	bra @loop	; Keep looping.

strcasecmp:
	phb.q		; Preserve B.
	phx.q		; Preserve X.
	phy.q		; Preserve Y.
	and #0		; Reset A.
	tab		; Reset B.
	tax		; Reset X.
	inx		; Set X to 1.
	tay		; Reset Y.
@loop:
	dex		; Set the islong flag to false.
	mov a, (d+y)	; Are we at the end of the first string?
	beq cmpr	; Yes, so check if we're too short, or too long.
	inx		; No, so set the islong flag to true.
	jsr tolower	; Convert the character of string 1 to lowercase.
	tab		; Save the converted character.
	mov a, (s+y)	; Get the character of the second string.
	jsr tolower	; Convert the character of string 2 to lowercase.
	cmp b		; Is the character of both strings, the same?
	bne cmpr	; No, so check if we're too short, or too long.
	iny		; Yes, so increment the index.
	bra @loop	; Keep looping.

cmpr:
	mov a, (s+y)	; Are we at the end of the second string?
	beq @islong	; Yes, so check the islong flag.
@isshort:
	mov a, (d+y)	; No, but are we at the end of the first string?
	bne @islong	; No, so check if the islong flag is true.
	lda #$FF	; Yes, so return -1.
	bra @end	; We are done.
@islong:
	cpx #1		; Is the islong flag true?
	set a, eq	; Return true if it is.
@end:
	ply.q		; Restore Y.
	plx.q		; Restore X.
	plb.q		; Restore B.
	rts		; End of cmpr.


isdigit:
	sec		; Prepare for a non carrying subtraction.
	sbc #'0'	; Subtract $30 from the passed character.
	and #$FF	; Make sure that we have only one byte.
	cmp #10		; Is the subtracted value, less than 10?
	lcc #1		; Yes, so return true.
	lcs #0		; No, so return false.
;	bcs @false	; No, so return false.
;@true:
;	lda #1		; Yes, so return true.
;	bra @end	; We are done.
;@false:
;	lda #0		; Return false.
;@end:
	rts		; End of isdigit.

isxdigit:
	pha		; Preserve the character.
	jsr isdigit	; Is this character, a decimal digit?
	pla		; Get the character back.
	bne @end	; Yes, so return true.
@alpha:
	ora #$20	; No, so convert it to lowercase.
	sub #'a'	; Subtract $61 from the character.
	and #$FF	; Make sure that we have only one byte.
	cmp #6		; Is the subtracted value, less than 6?
	lcc #1		; Yes, so return true.
	lcs #0		; No, so return false.
;	bcs @false	; No, so return false.
;@true:
;	lda #1		; Yes, so return true.
;	bra @end	; We are done.
;@false:
;	lda #0		; Return false.
@end:
	rts		; End of isxdigit.


isupper:
	sub #'A'	; Subtract $41 from the passed character.
	bra isletter	; Check if it's less than 26.
islower:
	sub #'a'	; Subtract $61 from the passed character.
isletter:
	and #$FF	; Make sure that we have only one byte.
	cmp #26		; Is the subtracted value, less than 26?
	lcc #1		; Yes, so return true.
	lcs #0		; No, so return false
	rts		; End of isletter.


tolower:
	pha		; Preserve the character.
	jsr isupper	; Is this character, an uppercase character?
	pla		; Get the character back.
	beq @end	; No, so we're done.
@lower:
	ora #$20	; Yes, so convert it to lowercase.
@end:
	rts		; End of tolower.


toupper:
	pha		; Preserve the character.
	jsr islower	; Is this character, a lowercase character?
	pla		; Get the character back.
	beq @end	; No, so we're done.
@upper:
	and #$5F	; Yes, so convert it to uppercase.
@end:
	rts		; End of toupper.


; malloc: Dynamically allocate memory.
; Input: D = size in bytes to allocate.
; Output: A = Pointer to allocated memory.
; Caller preserved registers: D.
; Callee preserved registers: B, X, Y.

malloc:
	phb.q			; Preserve B.
	phx.q			; Preserve X.
	phy.q			; Preserve Y.
	xor b, b		; Reset B.
	mov x, b		; Reset X.
	txy			; Reset Y.
	mov a, d		; Get the size.
	beq @end		; The size is zero, so we're done.
	add #ublk		; Add the size of a used block struct, to the size.
	cmp #ublk		; Is the total size smaller than the size of a used block struct?
	bcs @getheapf		; No, so get the first heap entry.
@toosmall:
	lda #ublk		; Yes, so set the total size to the size of a used block struct.
@getheapf:
	mov d, a		; Place the new size into the passed size.
	ldb.q heapf		; Get the first heap entry.
	bra @checkblk		; Check the block.
@findfblk:
	mov.q a, (b+fblk.size)	; Get the size of this free block.
	sub a, d		; Subtract the size of this free block, with the passed size.
	bcs @blkfound		; The result is less than, or equal to the free size, so return the found block.
	mov.q b, (b+fblk.next)	; Set the current free block, to the next free block.
@checkblk:
	bne @findfblk		; The address of this block is non NULL, so keep looping.
	mov a, d		; Get the passed size.
	add.q heapptr		; Add the top of the heap, with the passed size.
;	mov d, a		; Save the new size.
	bcs @outofspace		; The result overflowed, so return zero.
	cmp.q heapend		; Is the top of the heap, less than, or equal to the max heap size?
	bcc @inctop		; Yes, so increase the current top of the heap.
	beq @inctop		;
@outofspace:
	and #0			; Return zero.
	bra @end		; We are done.
@inctop:

	ldb.q heapptr		; Set the current free block to the previous top of the heap.
	sta.q heapptr		; Increment the current top of the heap by the passed size.
	bra @setsize		; Set the size, and start address into the used space of the block.
@blkfound:
	bne @sliceblk		; The block is big enough to slice.
	cmp #fblk		; Is the block big enough to slice?
	bcs @sliceblk		; Yes, so slice the block.
	mov.q x, (b+fblk.prev)	; Get the previous free block.
	beq @setheapf		; The previous block is NULL, so set the first block.
	ldy #fblk.next		; No, so set the offset to the next free block.
	mov.q (x+y), (b+y)	; Set the previous block's next block, to the next block.
	bra @chknxtblk		; Check the next block.
@setheapf:
	ldy #fblk.next		; Set the offset to the next block.
	mov.q heapf, (b+y)	; Set the first heap entry to the next block.
@chknxtblk:
	mov.q x, (b+y)		; Get the next free block.
	beq @setheapl		; The next block is NULL, so set the least heap entry.
	ldy #fblk.prev		; Get the current previous block.
	mov.q (x+y), (b+y)	; Set the next block's previous block to the current previous block.
	bra @retptr		; Return the pointer.
@setheapl:
	ldy #fblk.prev		; Get the current previous block.
	mov.q heapl, (b+y)	; Set the last heap entry to the current previous block.
	bra @retptr		; Return the pointer.
@sliceblk:
	ldy #fblk.size		; Get the size of the current block.
	sub.q (b+y), d		; Subtract the current block's size from the passed size.
	add.q b, (b+y)		; Add the current block's size to the pointer of the current block.
@setsize:
	mov.q (b+ublk.size), d	; Set the used block's size to the passed size.
@retptr:
	mov.q (b+ublk.start), b	; Set the used block's starting address, to the used block.
	add b, #ublk		; Get the pointer to the real memory block.
	tba			; Return the pointer to the real memory block.
@end:
	ply.q			; Restore Y.
	plx.q			; Restore X.
	plb.q			; Restore B.
	rts			; End of malloc.


; free: Free allocated memory.
; Input: D = Pointer to allocated memory.
; Output: none.
; Caller preserved registers: D.
; Callee 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.
	cpe.q heapptr		; Is this block on top of the heap?
	bne @heapadd		; No, so add it to the free block list.
@dectop:
	sta.q heapptr		; Set the top of the heap to the start of the current real block.
@chklastblk:
	ldb.q 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.
	cpe.q heapptr		; Is the last block on top of the heap?
	bne @end		; No, so we're done.
@delblk:
	stb.q heapptr		; 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:
	ldy.q 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.

; memcpy: memory to memory copy.
; Input: D = Destination pointer. S = Source pointer. F = Number of bytes to copy.
; Output: A = Destination pointer.
; Caller preserved registers: D, S, F.
; Callee preserved registers: B.

;memcpy:
;;	inc step		;
;	phb.q			; Preserve B.
;	phx.q			; Preserve X.
;	xor b, b		; Reset B.
;	mov a, f		; Get the size.
;	and #7			; Get the size, mod 8.
;	mov x, f		; Preserve the size.
;	lsr f, #3		; Divide the size by 8.
;	beq @rem		; The quotient is zero, so copy the remaining number of bytes.
;@loop:
;	mov.q (d+8*b), (s+8*b)	; Copy 8 bytes from the source, into the destination.
;	inb			; Increment the counter.
;	cmp b, f		; Is the counter the same as the size?
;	bne @loop		; No, so keep looping.
;@rem:
;	lsl f, #3		; Multiply the quotient by 8.
;	cmp #0			; Is the remainder zero?
;	beq @end		; Yes, so we're done.
;	lea b, 8		; No, so set our offset to 8.
;	sub b, a		; Subtract 8 by the remainder.
;	and #0			; Reset A.
;	dec			; Set the bitmask to -1.
;	lsl b, #3		; Multiply the offset by 8.
;	lsr b			; Shift the bitmask right by the offset.
;	mov.q b, (s+f)		; Get 8 bytes from the source.
;	xor.q b, (d+f)		; Mask out the destination bytes.
;	and b			; Get The masked source.
;	xor.q (d+f), a		; Copy the remaining bytes from the source, into the destination.
;@end:
;	mov f, x		; Restore the size.
;	mov a, d		; Set the return value to the destination pointer.
;	plx.q			; Restore X.
;	plb.q			; Restore B.
;	rts			; End of memcpy.


;memcpy:
;	phb.q			; Preserve B.
;	phx.q			; Preserve X.
;	phy.q			; Preserve Y.
;	mov b, f		; Get the size.
;	mov x, d		; Get the destination.
;	mov y, s		; Get the source.
;	txa			; Set the return value to the destination pointer.
;@copy:
;	mmv.r			; Copy the size bytes from the source, into the destination.
;@end:
;	ply.q			; Restore Y.
;	plx.q			; Restore X.
;	plb.q			; Restore B.
;	rts			; End of memcpy.


memcpy:
	phb.q			; Preserve B.
	mov a, d		; Set the return value to the destination pointer.
	and f, f		; Is the size zero?
	beq @end		; The size is zero, so we're done.
	xor b, b		; Reset B.
@loop:
	mov (d+b), (s+b)	; Copy a byte from the source, into the destination.
	inb			; Increment the counter.
	cmp b, f		; Is the counter the same as the size?
	bne @loop		; The size is non-zero, so keep looping.
@end:
	plb.q			; Restore B.
	rts			; End of memcpy.

; memcpy_back: memory to memory copy from end to start.
; Input: D = Destination pointer. S = Source pointer. F = Number of bytes to copy.
; Output: A = Destination pointer.
; Caller preserved registers: D, S, F.
; Callee preserved registers: B.

memcpy_back:
	phb.q			; Preserve B.
	mov a, d		; Set the return value to the destination pointer.
	mov b, f		; Copy the size into B.
@loop:
	cpb #0			; Is the size zero?
	beq @end		; Yes, so we're done.
	deb			; No, so decrement the size.
	mov (d+b), (s+b)	; Copy a byte from the source, into the destination.
	bra @loop		; Keep looping.
@end:
	plb.q			; Restore B.
	rts			; End of memcpy.

; memcpy_dir: memory to memory copy with selectable direction.
; Input: D = Destination pointer. S = Source pointer. F = Number of bytes to copy.
; C = direction flag, 0 = start to end, 1 = end to start.
; Output: A = Destination pointer.
; Caller preserved registers: D, S, F, C.
; Callee preserved registers: none.

memcpy_dir:
	and c, c		; Is the direction flag set?
	bne @back		; Yes, so copy from end to start.
@forward:
	jsr memcpy		; Copy from start to end.
	bra @end		; We are done.
@back:
	jsr memcpy_back		; Copy from end to start.
@end:
	rts			; End of memcpy_dir.

; memset: Set memory to some value.
; Input: D = Destination pointer. S = Constant value. F = Number of bytes to set.
; Output: A = Destination pointer.
; Caller preserved registers: D, S, F.
; Callee preserved registers: B.

;memset:
;;	inc step		;
;	phb.q			; Preserve B.
;	phx.q			; Preserve X.
;	lea b, 8		; Set the loop counter to 8.
;	psh s			; Get the constant.
;@const_loop:
;	lsl s, #8		; Make room for the next byte.
;	mov s, (sp+1)		; Get the next byte.
;	deb			; Decrement the loop counter.
;	bne @const_loop		; The counter is non zero, so keep looping.
;	ins			; Clean up the frame.
;@init:
;	mov a, f		; Get the size.
;	and #7			; Get the size, mod 8.
;	mov x, f		; Preserve the size.
;	lsr f, #3		; Divide the size by 8.
;	beq @rem		; The quotient is zero, so copy the remaining number of bytes.
;@loop:
;	mov.q (d+8*b), s	; Set all 8 destination bytes, to the constant value.
;	inb			; Increment the counter.
;	cmp b, f		; Is the counter the same as the size?
;	bne @loop		; No, so keep looping.
;@rem:
;	lsl f, #3		; Multiply the quotient by 8.
;	cmp #0			; Is the remainder zero?
;	beq @end		; Yes, so we're done.
;	lea b, 8		; No, so set our offset to 8.
;	sub b, a		; Subtract 8 by the remainder.
;	and #0			; Reset A.
;	dec			; Set the bitmask to -1.
;	lsl b, #3		; Multiply the offset by 8.
;	lsr b			; Shift the bitmask right by the offset.
;	mov.q b, s		; Get 8 bytes from the constant value.
;	xor.q b, (d+f)		; Mask out the destination bytes.
;	and b			; Get The masked source.
;	xor.q (d+f), a		; Copy the remaining bytes from the source, into the destination.
;@end:
;	mov f, x		; Restore the size.
;	mov a, d		; Set the return value to the destination pointer.
;	plx.q			; Restore X.
;	plb.q			; Restore B.
;	rts			; End of memset.


memset:
	phb.q			; Preserve B.
	xor b, b		; Reset B.
	mov a, d		; Set the return value to the destination pointer.
	cmp f, #0		; Is the size zero?
	beq @end		; Yes, so we're done.
@loop:
	mov (d+b), s		; Set the destination byte, to the constant value.
	inb			; Increment the counter.
	cmp b, f		; Is the counter the same as the size?
	bne @loop		; No, so keep looping.
@end:
	plb.q			; Restore B.
	rts			; End of memset.


; max: Get the largest of two integers.
; Input: D = First value, S = Second value.
; Output: A = Largest of the two values.
; Caller preserved registers: D, S.
; Callee preserved registers: None.

max:
	mov a, s		; Set the return value to the second value.
	cmp d, s		; Is the first value greater than the second value?
	beq @end		; No, so we're done.
	mcs a, d		; Set the return value to the first value if so.
@end:
	rts			; End of max.


; min: Get the smallest of two integers.
; Input: D = First value, S = Second value.
; Output: A = Smallest of the two values.
; Caller preserved registers: D, S.
; Callee preserved registers: None.

min:
	mov a, s		; Set the return value to the second value.
	cmp d, s		; Is the first value less than the second value?
	mcc a, d		; Set the return value to the first value if so.
@end:
	rts			; End of min.