6502-OS/spicode.s65

.include "system.h65"
.include "lcd.h65"
.include "math.h65"
.include "keypad.h65"
.import home:absolute
.segment "SPI"
.export CODE_START

CODE_START:
.assert * = $5000, error, "SPI Code not at $5000"
    lda '$'
    jsr lcd::print_char
    lda #<TEST_FMT
    sta ARG0
    lda #>TEST_FMT
    sta ARG1
    lda #<TEST_OUT
    sta ARG2
    lda #>TEST_OUT
    sta ARG3
    lda #$a9
    sta ARG4
    lda #$3c
    sta ARG5
    lda #$10
    sta ARG6
    jsr strf
    Print TEST_OUT
    stz kp::_DEBUG_VAL
@loop:
    lda kp::_DEBUG_VAL
    jeq home
    bra @loop

fmt_idx = $30
out_idx = $31
fmt_digit = $32
.proc strf
    stz out_idx
    stz fmt_idx
    stz fmt_digit
    ldx #0  ; index of format args
@loop:
    ldy fmt_idx
    lda (ARG0),y
    beq @null
    cmp #'%'
    beq @percent
@normal_char:  ; store A in output string
        ldy out_idx
        sta (ARG2),y
        inc fmt_idx
        inc out_idx
        beq @out_overflow
        bra @loop
@percent:  ; check for format in next position
        iny
        sty fmt_idx
        lda (ARG0),y  ; next char
        beq @null
        ; formats
        cmp #'9'
        ble @percent_number  ; numbers < letters
        cmp #'x'
        beq @format_hex1
        bra @normal_char
@percent_number:
            cmp #'1'
            blt @normal_char  ; NaN or zero
            ; todo covert from char
            jsr hex_char_to_int  ; only 0-9 supported
            ; A is now number of digits to convert
            sta fmt_digit
            iny
            sty fmt_idx
            lda (ARG0),y  ; next char
            beq @null
            cmp #'x'
            beq @format_hexN 
            bra @normal_char

@format_hexN:
                lda fmt_digit
                jsr int_to_hex_str
                bra @format_return

@format_hex1:   ; 1 byte hex -> 2 chars
            lda ARG4,x
            phx
            jsr int8_to_hex_str
            ldy out_idx
            sta (ARG2),y  ; most sig digit
            iny
            beq @out_overflow
            txa
            sta (ARG2),y  ; least sig digit
            iny
            beq @out_overflow
            sty out_idx
            plx
            inx  ; 1 byte of args handeled
            ; bra @format_return
@format_return:  ; increment fmt_idx to swallow the formating char
        inc fmt_idx
        bra @loop
@out_overflow:  ; store 0 in last position
    ldy #$ff
    sty out_idx
@store_null:
    lda #0
@null:  ; store and return
    ldy out_idx
    sta (ARG2),y
@rts:
    rts
.endproc


;********************************************************************************
; @function Convert a 1 byte number into two hex characters
; @param A: Number to convert
; @returns A: Most significant digit
; @returns X: Least significant digit
; @modifies A,X,Y
;********************************************************************************
.proc int8_to_hex_str
    pha
    and #%00001111
    tay
    ldx HEX_CHARS_UPPER,y
    pla
    div A,16
    and #%00001111
    tay
    lda HEX_CHARS_UPPER,y
    rts
.endproc

;********************************************************************************
; @function Convert any int into hex
; @param ARG2-3: Address of output string
; @param Y: Offset onto output string
; @param A: Number of digits to convert
; @param X: Offset onto ARG4 = start of int (big endian)
; @returns Y: New offset onto output string
; @returns A: 0
; @returns X: Offset onto ARG4 = past the end of number
; @modifies X,Y,A
;********************************************************************************
.proc int_to_hex_str
    bit
    beq @rts  ; check done
@loop:
    ; load next byte
    pha
        lda ARG4,x
        inx
        phx
            pha  ; copy byte
                div A,16  ; get first 4 bits = first digit
                and #%00001111
                phy
                    tay
                    lda HEX_CHARS_LOWER,y
                ply
                sta (ARG2),y 
                iny
            pla  ; get copy 
            and #%00001111  ; lower 4 bits = second digit
            phy
                tay
                lda HEX_CHARS_LOWER,y
            ply
            sta (ARG2),y 
            iny
        plx
    pla
    dec
    bne @loop
@rts:
    rts
.endproc



;********************************************************************************
; @function Convert a hex char into a binary number
; @details
;  The char must be one of [0-9,a-f,A-F].
;  All results are only valid if N == 0.
; @param A: Char to convert
; @returns N: 0 == success, 1 == invalid char
; @returns A: The converted number
;********************************************************************************
.proc hex_char_to_int
    sec
    sbc #'0'
    bmi @invalid  ; char was in [0, '0')
    cmp #10
    bcc @rts  ; A in [0, 10)
    ; char higher than '9'
    sbc #('A' - '0')
    bmi @invalid  ; char was in ('0', 'A')
    cmp #7
    bcc @hex_char  ; A in [0, 6]
    ; char higher than 'F'
    sbc #('a' - 'A')
    bmi @invalid  ; char was in ('F', 'a')
    cmp #17
    bcc @hex_char  ; A in [0, 6]
    ; char was in ('f', $ff]
@invalid:
    lda #$ff  ; sets N flag
    rts
@hex_char:
    ; carry is not set
    adc #10
@rts:
    rts
.endproc


;********************************************************************************
; @function Convert a number encoded as hexadecimal string to a binary number
; @details
;  The string must only consist of [0-9,a-f,A-F].
;  All results are only valid if N == 0.
; @param ARG0-1: Address of string
; @param A: Number of chars to convert
; @param Y: Offset onto string so that first char = (ARG0) + Y
; @returns N: 0 => success, 1 => invalid string
; @returns A: 0
; @returns X: Size of the number in bytes
; @returns Y: Offset onto string, past the end of the number
; @modifies X,Y,A
;********************************************************************************
.proc hex_to_int
    bit #%11111111  ; check if accumulator is zero
    beq @rts
    ldx #0
    bit #%00000001  ; check if accumulator is even
    beq @loop  ; even
    ; not even
    stz ARG2
    pha
    bra @less_sig_char
@loop:
    pha
        ; more significant char
        lda (ARG0),y  ; load next char
        iny
        jsr hex_char_to_int
        bmi @invalid
        rol
        rol
        rol
        rol
        sta ARG2,x
@less_sig_char:
        ; less significant char
        lda (ARG0),y  ; load next char
        iny
        jsr hex_char_to_int
        bmi @invalid
        ora ARG2,x
        sta ARG2,x
        inx
    pla
    dec
    bne @loop
@invalid:
@rts:
    rts

.endproc
    


TEST_FMT: .asciiz "%x -> %x -> %x:)"
TEST_OUT: .asciiz "TESTOUT"
HEX_CHARS_UPPER:    .byte "0123456789ABCDEF"
HEX_CHARS_LOWER:    .byte "0123456789abcdef"