C02/include/intlib.a02

; C02 module intlib.h02 assembly language subroutines
; Requires 
; external zero page words DSTPTR and SRCPTR 
; and external locations TEMP0, TEMP1, TEMP2, and TEMP3 

        SUBROUTINE INTLIB

;iabs(n) - Get Integer ABSolute Value
;Args: Y,X = Integer to get Absolute Value Of
;Sets: TEMP1, TEMP2
;Affects: C, N, Z
;Returns: A = Absolute Value of Argument
IABS:   CPY #$80        ;If Negative (High Bit Set)
        BCC .RETURN     ;  Carry will Already be Set
        JSR SAVRXY      ;  Copy LSB, MSB to TEMP1. TEMP2
        LDA #0          ;  Subtract LSB
        SBC TEMP1       ;    from 0
        TAX             ;    and Copy to X Register
        LDA #0          ;  Subtract MSB
        SBC TEMP2       ;    from 0
        TAY             ;    and Copy to Y Register
.RETURN RTS

;imax(i) - Get MAXimum of Two Integers
;Args: Y,X = Second Integer
;Uses: SRCPTR = First Integer
;Affects: N,Z,C
;Returns: Y,X = Larger of the Two Arguments
IMAX:   CPY SRCPTR+1    ;If Y < SRCPTR MSB
        BCC .GETSRC     ;  Return SRCPTR
        CPX SRCPTR       ;IF X >= SRCPTR LSB
        BCS .RETURN     ;  Return Argument
.GETSRC JMP GETSRC      ;Return Integer in SRCPTR

;imin(i) - Get MINimum of Two Integers
;Args: Y,X = Second Integer
;Uses: SRCPTR = First Integer
;Affects: N,Z,C
;Returns: Y,X = Larger of the Two Arguments
IMIN:   CPY SRCPTR+1       ;If Y < SRCPTR+1
        BCC .RETURN     ;  Return Argument
        BNE .GETSRC     ;If Y > SRCPTR+1 Return SRCPTR
        CPX SRCPTR       ;If X >= SRCPTR
        BCS .GETSRC     ; Return SRCPTR 
        RTS             ;Return Argument

;iaddc(c,i) - Add Byte c to Integer i
IADDC:  JSR SETSRC      ;Save Integer and Clear Y
        TAX             ;Copy Byte to LSB and drop into IADD

;iadd(d) - ADD Integer d to from Integer g
;Args: Y,X = Addend
;Requires: setsrc(g) - Augend
;Sets: TEMP1,TEMP2 = Addend
;Affects: Z,C
;Returns: A = Carry
;         Y,X = Sum
;         N = Sign of Result
IADD:   CLC         ;Clear Carry for Addition
        TXA         ;Add Addend LSB
        ADC SRCPTR   ;to Augend LSB
        TAX         ;and Copy to X
        TYA         ;Add Addend MSB
        ADC SRCPTR+1   ;to Augebd MSB
        TAY         ;and Copy to Y
        LDA #0      ;Set Overflow to 0
        ROL         ;  Rotate Carry (Same as Adding it)
        RTS         ;  and Return

;isub(s) - SUBtract Integer s from Integer m 
;Args: Y,X = Subtrahend
;Requires: setsrc(m) - Minuend
;Sets: TEMP1,TEMP2 = Subtrahend
;Affects: Z,C
;Returns: A = Carry
;         Y,X = Difference 
;         N = Sign of Result
ISUB:   JSR SAVRXY  ;Store Subtrahend in TEMP1,TEMP2
        SEC         ;Set Carry for Subtraction
        LDA SRCPTR   ;Load Minuend LSB 
        SBC TEMP1   ;Subtract Subtrahend LSB
        TAX         ;Copy Difference LSB to X
        LDA SRCPTR+1   ;Load Minuend MSB 
        SBC TEMP2   ;Subtract Subtrahend MSB
        TAY         ;Copy Difference MSB to Y
        LDA #0      ;Set Overflow Byte to 0
        SBC #0      ;  Subtract Carry
        RTS         ;  and Return

;imult(m) - MULTiply Two Integers
;Args: Y,X - Multiplier
;Requires: DSTPTR = Multiplicand
;Sets: TEMP0-TEMP3 = 32 Bit Product
;Destroys: SRCPTR
;Affects: A,C,Z,N
;Returns: A,Y,X = 24 Bit Product
IMULT:  JSR SETSRC      ;Save Multiplier
        STY TEMP0+2     ;Clear Upper Bits of Product
        STY TEMP0+3 
        LDX #16         ;Rotate Through 16 Bits
.MSHFTR LSR SRCPTR+1     ;Divide Multiplier by 2 
        ROR SRCPTR
        BCC .MROTR      ;If Shifted out Bit is 1
        LDA TEMP0+2     ;  Add Multiplicand
        CLC             ;  to Upper Half of Product
        ADC DSTPTR
        STA TEMP0+2
        LDA TEMP0+3
        ADC DSTPTR+1
        STA TEMP0+3 
.MROTR  ROR TEMP0+3 
        ROR TEMP0+2
        ROR TEMP0+1 
        ROR TEMP0 
        DEX             ;Decrement Counter
        BNE .MSHFTR     ;and Process Next Bit
        LDX TEMP0       
        LDY TEMP1       ;Return Low 24 Bits of 
        LDA TEMP2       ;Product in A, Y, and X
        RTS

;idiv(d) - Integer DIVide
;Args: Y,X - Divisor
;Requires: DSTPTR = Dividend
;Sets: SRCPTR = Divisor
;      DSTPTR = Quotient
;      TEMP1,TEMP2 = Remainder
;Affects: A,C,Z,N
;Returns: Y,X = 16 Bit Quotient
IDIV:   JSR .IDIV       ;Do Division and
        JMP GETDST      ;Return Quotient

;imod(d) - Integer MODulus
;Args: Y,X - Divisor
;Requires: DSTPTR = Dividend
;Sets: SRCPTR = Divisor
;      DSTPTR = Quotient
;      TEMP1,TEMP2 = Remainder
;Affects: A,C,Z,N
;Returns: Y,X = 16 Bit Remainder
IMOD:   JSR .IDIV       ;Do Division and
        JMP RESRXY      ;Return Remainder

.IDIV   JSR SETSRC      ;Save Divisor
        STY TEMP1
        STY TEMP1+1
        LDX #16	        ;repeat for each bit: ...
.IDLOOP	ASL DSTPTR      ;dividend lb & hb*2, msb -> Carry
        ROL DSTPTR+1      
        ROL TEMP1       ;remainder lb & hb * 2 + msb from carry
        ROL TEMP1+1       
        LDA TEMP1       
        SEC
        SBC SRCPTR      ;subtract divisor to see if it fits in
        TAY	            ;lb result -> Y, for we may need it later
        LDA TEMP1+1       
        SBC SRCPTR+1       
        BCC .IDSKIP     ;if carry=0 then divisor didn't fit in yet
        STA TEMP1+1     ;else save substraction result as new remainder,
        STY TEMP1       
        INC DSTPTR      ;and INCrement result cause divisor fit in 1 times
.IDSKIP DEX
        BNE .IDLOOP
        RTS

;ishftl(n,i) - Shift Integer i to the Left n Bits
;Sets: TEMP1, TEMP2 = LSB, MSB of Result
;Affects: A,Y,N,Z,C
;Returns: A = Bits Shifted out of Integer
;         Y,X = Shifted Integer
ISHFTL: JSR SAVRXY      ;Save X,Y in TEMP1,TEMP2
        TAY             ;Set Counter to Number of Bits
        BEQ .RESRXY     ;If Zero, Return
        LDA #0          ;Clear Overflow
.LSLOOP ASL TEMP1       ;Shift LSB to Left
        ROL TEMP2       ;Rotate MSB to Left
        ROL             ;Rotate Carry into A
        DEY             ;Decrement Counter
        BNE .LSLOOP     ;  and Loop if Not 0
        BEQ .RESRXY     ;Return Shifted Integer

;ishftr(n,i) - Shift Integer i to the Right n Bits
;Sets: TEMP1, TEMP2 = LSB, MSB of Result
;Affects: A,Y,N,Z,C
;Returns: A = Bits Shifted out of Integer
;         Y,X = Shifted Integer
ISHFTR: JSR SAVRXY      ;Save X,Y in TEMP1,TEMP2
        TAY             ;Set Counter to Number of Bits
        BEQ .RESRXY     ;If Zero, Return
        LDA #0          ;Clear Overflow
.RSLOOP LSR TEMP2       ;Shift MSB to Right
        ROR TEMP1       ;Rotate LSB to Right
        ROR             ;Rotate Carry into A
        DEY             ;Decrement Counter
        BNE .RSLOOP     ;  and Loop if Not 0
        BEQ .RESRXY     ;Load Shifted Integer and Return

;atoi(&s) - ASCII string TO Integer
;Args: Y,X = Address of String to Convert
;Sets: TEMP1, TEMP2 = Integer Value
;Affects: TEMP0 
;Returns: A = Number of Digits 
;         Y,X = Integer Value
ATOI:   JSR SETSRC      ;Initialize Source String
        STY TEMP1       ;Initialize Result
        STY TEMP2
.AILOOP LDA (SRCPTR),Y   ;Get Next Character
        CMP #$30        ;If Less Than '0'
        BCC .AIDONE     ;  Exit
        CMP #$3A        ;If Greater Than '9'
        BCS .AIDONE     ;  Exit
        AND #$0F        ;Convert to Binary Nybble
        STA TEMP0       ;  and Save It
        LDA TEMP1       ;Load Result
        LDX TEMP2
        ASL TEMP1       ;Multiply by 5 by
        ROL TEMP2
        ASL TEMP1       ;  Multiplying by 4
        ROL TEMP2
        CLC             ;  And Adding Itself
        ADC TEMP1
        STA TEMP1
        TXA
        ADC TEMP2
        STA TEMP2
        ASL TEMP1       ;Multiply that by 2
        ROL TEMP2
        LDA TEMP0       ;Get Saved Nybble
        CLC             ;and Add to Result
        ADC TEMP1       ;Add Saved Nybble
        STA TEMP1       ;  and Store Result
        LDA #0
        ADC TEMP2
        STA TEMP2
        INY             ;Increment Index
        BPL .AILOOP     ;  and Loop
.AIDONE TYA             ;Return Number of Digits
.RESRXY JMP RESRXY      ;and Integer Value

;itoa(n) - Integer TO ASCII string
;Args: Y,X = Integer Value to Convert
;Uses: DSTPTR = Destination String
;Affects: X
;Returns: A,Y = Length of String
ITOA:   JSR CVIBCD      ;Convert Integer to Packed BCD
        LDY #0          ;Initialize Index into String
        STY TEMP3
.ITOAA  LDY #4          ;Set Initial Digit Number
.IAZERO JSR UPBCDI      ;Unpack Digit Y
        BNE .IASKIP     ;If Zero
        DEY             ;  Decrement Digit Number
        BNE .IAZERO     ;  If Not Zero Loop
        BEQ .IASKIP     ;  Else .IDSKIP Unpack 
.IALOOP JSR UPBCDI      ;Unpack Digit #Y
.IASKIP TAX             ;Save Digit in X
        TYA             ;Push Digit Number into Stack
        PHA
        TXA             ;and Restore Digit
        LDY TEMP3       ;Get Index into String
        ORA #$30        ;Convert Digit to ASCII
        STA (DSTPTR),Y  ;and Store in String
        INC TEMP3       ;Increment Index into String
        PLA             ;Pull Digit Number off Stack
        TAY
        DEY             ;Decrement Digit Number
        BPL .IALOOP       ;Loop if >= Zero
        LDA #0          ;Terminate String
        STA (DSTPTR),Y
        TYA             ;Return String Length
        RTS

;upbcdi() - UnPack digits from BCD Integer
;  Assumes that TEMP0, TEMP1, and TEMP2
;  are in consecutive memory locations
;Args: Y = Digit Number to Unpack (0-5)
;Uses: TEMP0 = Low Byte
;      TEMP1 = Middle Byte
;      TEMP2 = High Nybble
;Affects: X,N,Z
;Returns: A = Unpacked Digit
UPBCDI: PHP
        TYA             ;Divide Digit Number by 2,
        LSR             ;  Setting Carry 
        TAX             ;  if Digit Number is Odd
        LDA TEMP0,X     ;Load BCD Byte
        BCC .UPSKIP     ;If Digit Number is Odd
        LSR             ;  Shift High Nybble to Low Nybble
        LSR   
        LSR   
        LSR   
.UPSKIP PLP   
        AND #$0F        ;Strip Off High Nybble
        RTS

;cvibcd(int) - ConVert Integer to packed Binary Coded Decimal
;Args: Y,X - Integer to Convert
;Sets: TEMP0 = Tens and Ones Digit
;      TEMP1 = Thousands and Hundreds Digit
;      TEMP2 = Ten-Thousands Digit
;Affects: A
CVIBCD: LDA #0      ;Clear BCD Bytes
        STA TEMP0
        STA TEMP1
        STA TEMP2
        PHP         ;Save Status Register
        SEI         ;Disable Interrupts
        SED         ;Set Decimal Mode
        TYA         ;Push MSB onto Stack
        PHA
        TXA         ;Push LSB onto Stack
        PHA
        TSX         ;Copy Stack Pointer to X
        LDY #16     ;Process 16 bits of Binary
.CVLOOP ASL $101,X  ;Shift High Bit Into Carry
        ROL $102,X
        LDA TEMP0   ;Add 6 Digit BCD Number Itself 
        ADC TEMP0   ;  Effectively Multiplying It by 2
        STA TEMP0   ;  and Adding in the Shifted Out Bit
        LDA TEMP1
        ADC TEMP1
        STA TEMP1
        LDA TEMP2
        ADC TEMP2
        STA TEMP2
        DEY         ;Decrement Counter and
        BNE .CVLOOP ;  Process Next Bit
        PLA         ;Restore X and Y Registers
        PLA
        PLP         ;Restore Status Register
        RTS 

        ENDSUBROUTINE