; 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:
	sec		; Yes, so prepare for a non borrowing subtract.
	sbc #'a'-10	; Get the numeric value from this digit.
	bra @chkbase	; Check if the value matches the base.
@digit:
	sec		; Prepare for a non borrowing subtract.
	sbc #'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.
	clc		; Prepare for a non carrying add.
	adc 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:
	phy.q		; Preserve Y.
	pha.q		; Set the temp variable to the argument.
	and #0		; Reset A.
	tay		; Reset Y.
@loop:
	lda (sp+1), y	; Are we at the end of the string?
	beq @end	; Yes, so we're done.
	iny		; No, so increment the index.
	bra @loop	; Keep looping.
@end:
	pla.q		; Get the argument back.
	tya		; Get the return value.
	ply.q		; Get the preserved value back.
	rts		; End of strlen.


strcmp:
	ldb #0		; Reset B.
	tba		; Reset A.
	phy.q		; Preserve Y.
	tay		; Reset Y.
@loop:
	ldb #0		; Set the islong flag to false.
	lda (sp+25), y	; Are we at the end of the first string?
	beq cmpr	; Yes, so check if we're too short, or too long.
	ldb #1		; No, so set the islong flag to true.
	cmp (sp+19), 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.

strccmp:
strcasecmp:
	ldb #0		; Reset B.
	tba		; Reset A.
	phy.q		; Preserve Y.
	tay		; Reset Y.
@loop:
	ldb #0		; Set the islong flag to false.
	lda (sp+25), y	; Are we at the end of the first string?
	beq cmpr	; Yes, so check if we're too short, or too long.
	ldb #1		; No, so set the islong flag to true.
	jsr tolower	; Convert the character of string 1 to lowercase.
	phb		; Preserve the islong flag.
	pha		; Preserve the converted character.
	lda (sp+19), y	; Get the character of the second string.
	jsr tolower	; Convert the character of string 2 to lowercase.
	tab		; Place it in B.
	pla		; Get the character of string 1 back.
	cmp b		; Is the character of both strings, the same?
	plb		; Get the islong flag back.
	bne cmpr	; No, so check if we're too short, or too long.
	iny		; Yes, so increment the index.
	bra @loop	; Keep looping.

cmpr:
	lda (sp+17), y	; Are we at the end of the second string?
	beq @islong	; Yes, so check the islong flag.
@isshort:
	lda (sp+25), y	; No, but are we at the end of the first string?
	beq @short	; Yes, so return -1.
@islong:
	cpb #1		; Is the islong flag true?
	bne @equ	; No, so return 0.
@long:
	lda #1		; Yes, so return 1.
	bra @end	; We are done.
@equ:
	lda #0		; Return 0.
	bra @end	; We are done.
@short:
	lda #$FF	; Return -1.
@end:
	ply.q		; Get the preserved value back.
	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: A = size in bytes to allocate.
; Output: A = Pointer to allocated memory.
; Caller preserved registers: none.
; Callie preserved registers: Y.

malloc:
	phy.q		; Preserve Y.
	pha.q		; Preserve the size.
	and #0		; Reset A.
	tay		; Reset Y.
	sbs #$10	; Allocate 2 local variables onto the stack.
	lda.q sp+17	; Get the size.
	ora.d sp+21	; Is the size zero?
	beq @end	; Yes, so we're done.
	lda.q sp+17	; No, so get back the size.
	clc		; Prepare for a non carrying add.
	adc #ublk	; Add the size of a used block struct, to the size.
	sta.q sp+17	; Set the actual size.
	lda.q heapf	; Get the first heap entry.
	sta.q sp+9	; Save it in the second local variable.
	bra @checkblk	; Check the block.
@findfblk:
	ldy #fblk.size	; Get the size of this free block.
	lda.q (sp+9), y	;
	sec		; Prepare for a non borrowing subtract.
	sbc.q sp+17	; 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.
	ldy #fblk.next	; Get the next free block.
	lda.q (sp+9), y	;
	sta.q sp+9	; Set the current free block, to the next free block.
@checkblk:
	ora.d sp+13	; Is the address of this block, zero?
	bne @findfblk	; No, so keep looping.
	lda.q heapptr	; Yes, so get the current top of the heap.
	clc		; Prepare for a non carrying add.
	adc.q sp+17	; Add the top of the heap, with the passed 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:
	pha.q		; Preserve heapptr + size.
	lda.q heapptr	; Get the previous top of the heap.
	sta.q sp+17	; Set the current free block to the previous top of the heap.
	pla.q		; Restore heapptr + size.
	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.
	ldy #fblk.prev	; No, so get the previous free block.
	lda.q (sp+9), y	;
	sta.q sp+1	; Set the third local variable, to the previous free block.
	ora.d sp+5	; Is the previous block, set to zero?
	beq @setheapf	; Yes, so set the first block.
	ldy #fblk.next	; No, so get the next free block.
	lda.q (sp+9), y	;
	sta.q (sp+1), y	; Set the previous block's next block, to the next block.
	bra @chknxtblk	; Check the next block.
@setheapf:
	lda.q (sp+9), y	; Get the next block.
	sta.q heapf	; Set the first heap entry to it.
@chknxtblk:
	lda.q (sp+9), y	; Get the next free block.
	sta.q sp+1	; Set the third local variable to the next free block.
	ora.d sp+5	; Is the next block, zero?
	beq @setheapl	; Yes, so set the least heap entry.
	ldy #fblk.prev	; No, so get the current previous block.
	lda.q (sp+9), y	;
	sta.q (sp+1), 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.
	lda.q (sp+9), y	;
	sta.q heapl	; 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.
	lda.q (sp+9), y	;
	sec		; Prepare for a non borrowing subtract.
	sbc.q sp+17	; Subtract the current block's size from the passed size.
	sta.q (sp+9), y	; Set the current block's size to the subtracted size.
	clc		; Prepare for a non carrying add.
	adc.q sp+9	; Add the current block's size to the pointer of the current block.
	sta.q sp+9	; Set the current block to the space above it (return value).
@setsize:
	ldy #ublk.size	; Start setting the used block's size, to the passed size.
	lda.q sp+17	; Get the passed size.
	sta.q (sp+9), y	; Set the used block's size to the passed size.
@retptr:
	ldy #ublk.start	; Start setting the used block's starting address, to the used block.
	lda.q sp+9	; Get the used block.
	sta.q (sp+9), y	; Set the used block's starting address, to the used block.
	clc		; Prepare for a non carrying add.
	adc #ublk	; Return the pointer to the real memory block.
@end:
	ads #$18	; Clean up the stack frame.
	ply.q		; Restore Y.
	rts		; End of malloc.


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


; memcpy: memory to memory copy.
; Input: A = Destination pointer. B = Source pointer. Y = Number of bytes to copy.
; Output: A = Destination pointer.
; Caller preserved registers: none.
; Callie preserved registers: none.

memcpy:
	pha.q		; Preserve the return value.
	pha.q		; Push the destination pointer on the stack.
	phb.q		; Push the source pointer on the stack.
	phy.q		; Push the size on the stack.
	and #0		; Reset A.
	tab		; Reset B.
	cpy #0		; Is the size zero?
	beq @end	; Yes, so we're done.
@loop:
	lda (sp+9)	; Get a byte from the source.
	sta (sp+17)	; Copy it to the destination.
	inc.q sp+9	; Increment the source pointer.
	inc.q sp+17	; Increment the destination pointer.
	dey		; Decrement the size.
	beq @end	; The size is zero, so we're done.
	bra @loop	; Keep looping.
@end:
	ads #$18	; Clean up the stack frame.
	pla.q		; Restore the return value.
	rts		; End of memcpy.