;> FileCore05

; LEAF ROUTINES

; ======
; ClearV
; ======

ClearV
        BICS    PC, LR, #V_bit

; ====
; SetV
; ====

SetV
        ORRS    PC, LR, #V_bit

; ========
; SetVOnR0
; ========

SetVOnR0
        CMPS    R0, #0
        BICEQS  PC, LR, #V_bit
        ORRNES  PC, LR, #V_bit


; =============
; PullLinkKeepV
; =============

PullLinkKeepV
        Pull    "LR"
        BICVCS  PC, LR, #V_bit
        ORRVSS  PC, LR, #V_bit


; ============
; DiscAddToRec
; ============

; entry R3 disc address
; exit LR -> disc rec

DiscAddToRec
        Push    "LR"
        MOV     LR, R3, LSR #(32-3)
        DiscRecPtr  LR,LR
        Pull    "PC"


; ======
; Divide
; ======

; entry: R0=Numerator, R1=Denominator
; exit:  R0=Result, R1=Remainder

Divide  ROUT
        Push    "R2,LR"

        MOV     LR, R1          ;temp = den
        CMPS    LR, R0, LSR #1  ;shift temp left unless it would be > num
10
        MOVLS   LR, LR, LSL #1
        CMPLS   LR, R0, LSR #1
        BLS     %BT10
        MOV     R2, #0          ;init result
20
        CMPS    R0, LR          ;if subtraction possible
        SUBCS   R0, R0, LR      ; do it
        ADC     R2, R2, R2      ;shift bit into result
        MOV     LR, LR, LSR #1  ;move temp right
        CMPS    LR, R1          ;loop if not done
        BHS     %BT20

        MOV     R1, R0          ;mod
        MOV     R0, R2          ;div

        Pull    "R2,PC",,^


; =========
; BlockMove
; =========

; entry: R0=source start, R1=dest start, R2=length

BlockMove ROUT
 [ Debug9
        mess    ,"source  |dest    |length  - enter BlockMove",NL
        wrhex   R0
        wrhex   R1
        wrhex   R2
        mess    ,NL
 ]
        Push    "R0-R12,LR"
        TEQS    R2, #0
        Pull    "R0-R12,PC",EQ,^
        CMPS    R1, R0
        BHI     %FT50           ;upwards move
05                      ;bytes at a time until source aligned
        TSTNE   R0, #2_11
        LDRNEB  LR, [R0], #1
        STRNEB  LR, [R1], #1
        SUBNES  R2, R2, #1
        BNE     %BT05

        ANDS    LR, R1, #2_11
        BEQ     %FT30           ;same alignment

;downwards mis-aligned move
        SUBS    R2, R2, #9*4    ;enough left to worth being clever ?
        BLO     %FT35           ;if not

        LDR     R3, [R0], #4    ;get a word from source
10
        STRB    R3, [R1], #1    ;put bytes to dest until aligned
        MOV     R3, R3, LSR #8
        TSTS    R1, #2_11
        BNE     %BT10
        MOV     LR, LR, LSL #3  ;# bits left in R3
        RSB     R12,LR, #32     ;# bits free in R3
15
        LDMIA   R0!,{R4-R11}    ;load 8 words
                         ;do 8 word shift, spare bits in R3
        ORR     R3, R3, R4, LSL LR
        MOV     R4, R4, LSR R12

        ORR     R4, R4, R5, LSL LR
        MOV     R5, R5, LSR R12

        ORR     R5, R5, R6, LSL LR
        MOV     R6, R6, LSR R12

        ORR     R6, R6, R7, LSL LR
        MOV     R7, R7, LSR R12

        ORR     R7, R7, R8, LSL LR
        MOV     R8, R8, LSR R12

        ORR     R8, R8, R9, LSL LR
        MOV     R9, R9, LSR R12

        ORR     R9, R9, R10,LSL LR
        MOV     R10,R10,LSR R12

        ORR     R10,R10,R11,LSL LR
        STMIA   R1!,{R3-R10}
        MOV     R3, R11,LSR R12

        SUBS    R2, R2, #8*4
        BPL     %BT15
        ADD     R2, R2, #8*4
20                              ;write out spare bytes from shift
        STRB    R3, [R1], #1
        MOV     R3, R3, LSR #8
        SUBS    LR, LR, #8
        BNE     %BT20
        B       %FT40           ;do any bytes left

;downwards aligned move

25
        LDMIA   R0!, {R3-R11}   ;move 9 aligned words at a time
        STMIA   R1!, {R3-R11}
30
        SUBS    R2, R2, #9*4
        BPL     %BT25
35
        ADD     R2, R2, #9*4
40                      ;move any odd bytes left
        SUBS    R2, R2, #1
        LDRPLB  LR, [R0], #1
        STRPLB  LR, [R1], #1
        BPL     %BT40
 [ Debug9
        mess    ,"leave BlockMove",NL
 ]
        Pull    "R0-R12,PC",,^

; upwards move
50
        ADD     R0, R0, R2      ;source end
        ADD     R1, R1, R2      ;dest end
55                      ;bytes at a time until source aligned
        TSTNES  R0, #2_11
        LDRNEB  LR, [R0,#-1]!
        STRNEB  LR, [R1,#-1]!
        SUBNES  R2, R2, #1
        BNE     %BT55

        ANDS    LR, R1, #2_11   ;dest mis-alignment
        BEQ     %FT80           ;same alignment

;upwards mis-aligned move

        SUBS    R2, R2, #9*4    ;enough left to worth being clever ?
        BLO     %FT85           ;if not

        LDR     R11,[R0,#-4]!   ;get a word from source
60
        MOV     R10,R11,LSR #24
        STRB    R10,[R1,#-1]!   ;put bytes to dest until aligned
        MOV     R11,R11,LSL #8
        TSTS    R1, #2_11
        BNE     %BT60
        MOV     LR, LR, LSL #3  ;# bits free in R11
        RSB     R12,LR, #32     ;# bits left in R11
65
        LDMDB   R0!,{R3-R10}    ;load 8 words
                        ;do 8 word shift, spare bits in R11
        ORR     R11,R11,R10,LSR R12
        MOV     R10,R10,LSL LR

        ORR     R10,R10,R9,LSR R12
        MOV     R9, R9, LSL LR

        ORR     R9, R9, R8, LSR R12
        MOV     R8, R8, LSL LR

        ORR     R8, R8, R7, LSR R12
        MOV     R7, R7, LSL LR

        ORR     R7, R7, R6, LSR R12
        MOV     R6, R6, LSL LR

        ORR     R6, R6, R5, LSR R12
        MOV     R5, R5, LSL LR

        ORR     R5, R5, R4,LSR R12
        MOV     R4, R4, LSL LR

        ORR     R4, R4, R3, LSR R12
        STMDB   R1!, {R4-R11}
        MOV     R11, R3, LSL LR

        SUBS    R2, R2, #8*4
        BPL     %BT65
        ADD     R2, R2, #8*4
70                      ;write out spare bytes from shift
        MOV     LR, R11,LSR #24
        STRB    LR, [R1,#-1]!
        MOV     R11,R11,LSL #8
        SUBS    R12,R12,#8
        BNE     %BT70
        B       %FT90           ;do any bytes left

;upwards aligned move
75
        LDMDB   R0!, {R3-R11}   ;move 9 aligned words at a time
        STMDB   R1!, {R3-R11}
80
        SUBS    R2, R2, #9*4
        BPL     %BT75
85
        ADD     R2, R2, #9*4
90                      ;move any odd bytes left
        SUBS    R2, R2,#1
        LDRPLB  LR, [R0,#-1]!
        STRPLB  LR, [R1,#-1]!
        BPL     %BT90
95
 [ Debug9
        mess    ,"leave BlockMove",NL
 ]
        Pull    "R0-R12,PC",,^


; ========
; Move256n
; ========

;Quick move when aligned, non overlapping move of length 256n

;entry
; R0=source start
; R1=dest start
; R2=length

;exit R0-R2 updated

Move256n ROUT
        Push    "R3-R11,SB,LR"
10
        LDMIA   R0!, {R3-R11,SB,LR}     ;11*4
        STMIA   R1!, {R3-R11,SB,LR}

        LDMIA   R0!, {R3-R11,SB,LR}     ;22*4
        STMIA   R1!, {R3-R11,SB,LR}

        LDMIA   R0!, {R3-R11,SB,LR}     ;33*4
        STMIA   R1!, {R3-R11,SB,LR}

        LDMIA   R0!, {R3-R11,SB,LR}     ;44*4
        STMIA   R1!, {R3-R11,SB,LR}

        LDMIA   R0!, {R3-R11,SB,LR}     ;55*4
        STMIA   R1!, {R3-R11,SB,LR}

        LDMIA   R0!, {R3-R11}           ;64*4=256
        STMIA   R1!, {R3-R11}

        SUBS    R2, R2, #256
        BGT     %BT10
        Pull    "R3-R11,SB,PC",,^



; =======
; ZeroRam
; =======

; Clear a block of RAM to 0

; entry: R0=start, R1=length

ZeroRam ROUT

 [ Debug9
        mess    ,"start   |length  - enter ZeroRam",NL
        wrhex   R0
        wrhex   R1
        mess    ,NL
 ]

        Push    "R0-R12,LR"
        TEQS    R1, #0
        BEQ     %FT95
        MOV     R2, #0
10                      ;byte at a time until aligned
        TSTS    R0, #2_11
        STRNEB  R2, [R0],#1
        SUBNES  R1, R1, #1
        BNE     %BT10

        SUBS    R1, R1, #16*4   ;enough to be worth being clever
        ADDLO   R1, R1, #16*4
        BLO     %FT40           ; if not
        MOV     R3, #0
        MOV     R4, #0
        MOV     R5, #0

        STMIA   R0!,{R2-R5}     ;good since 256 MOD 12 = 4, 1024 MOD 12 = 4
        LDMDB   R0, {R6-R9}
        LDMDB   R0, {R10-R12,LR}
20
        STMIA   R0!,{R2-R12,LR}
        SUBS    R1, R1, #12*4
        BPL     %BT20
30
        ADD     R1, R1, #12*4
40
        SUBS    R1, R1, #1
        STRPLB  R2, [R0],#1
        BPL     %BT40
95
 [ Debug9
        mess    ,"leave ZeroRam",NL
 ]
        Pull    "R0-R12,PC",,^


; =====
; Read3
; =====

; Read 3 bytes from unknown alignment

; entry: R0 -> first byte (LO)

; exit:  LR =  24 bit result

Read3   ROUT
        Push    "R0-R1,LR"
        BIC     R1,R0,#3
        LDMIA   R1,{R1,LR}
        ANDS    R0,R0,#3
        MOVNE   R0,R0,LSL #3
        MOVNE   R1,R1,LSR R0
        RSBNE   R0,R0,#32
        ORRNE   R1,R1,LR,LSL R0
        BIC     LR,R1,#&FF000000
        Pull    "R0-R1,PC",,^


; ========
; ReadWord
; ========

; Load 4 byte word from unknown alignment

; entry: R0 -> first byte (LO)

; exit:  LR =  32 bit result

ReadWord ROUT
        Push    "R0-R1,LR"
        BIC     R1,R0,#3
        LDMIA   R1,{R1,LR}
        ANDS    R0,R0,#3
        MOVNE   R0,R0,LSL #3
        MOVNE   R1,R1,LSR R0
        RSBNE   R0,R0,#32
        ORRNE   R1,R1,LR,LSL R0
        MOV     LR,R1
        Pull    "R0-R1,PC",,^


; ======
; Write3
; ======

; Write 3 byte number at unknown alignment

; entry: R0 -> first byte (LO), R1=3 bytes to write

Write3  ROUT
        Push    "R0-R2,LR"
        MOV     R2,#3
        B        WriteCommon


; =========
; WriteWord
; =========

; Write 4 byte word at unknown alignment

; entry: R0 -> first byte (LO), R1=byte to write


WriteWord ROUT
        Push    "R0-R2,LR"
        MOV     R2,#4
WriteCommon
        STRB    R1,[R0],#1
        MOV     R1,R1,ROR #8
        SUBS    R2,R2,#1
        BNE     WriteCommon
        Pull    "R0-R2,PC",,^


; ===========
; CheckEscape
; ===========

; exit: No Escape => Z=0
;          Escape => Z=1, R0=EscapeErr

CheckEscape
        Push    "R1,LR"
        LDR     R1,=ESC_Status
        LDRB    R1,[R1]
        EOR     R1,R1,#EscapeBit
        TSTS    R1,#EscapeBit
        MOVEQ   R0,#IntEscapeErr
        Pull    "R1,PC"

; ===========
; GetRandomId
; ===========

; entry -

; exit lr = a 'random' Id
GetRandomId ROUT
        Push    "r0,r1,lr"
        MOV     r0, #IOC
        STRB    r1, [r0, #Timer0LR]
        LDRB    r1, [r0, #Timer0CL]
        LDRB    lr, [r0, #Timer0CH]
        ORR     lr, lr, r1, ASL #8
        Pull    "r0,r1,pc"

 [ NewFs

; ===========
; NewRandomId
; ===========

; entry r0->start of 1K-sector new free space map

NewRandomId
        Push    "r1,lr"
 [ DebugE
        DREG    r0, "<-NewRandomId(",cc
        DLINE   ")"
 ]
        BL      GetRandomId
        STRB    lr, [r0, #ZoneHead+DiscId]
        MOV     lr, lr, LSR #8
        STRB    lr, [r0, #ZoneHead+DiscId+1]
        MOV     r1, #SzNewFloppyFs
        BL      SetNewCheck             ;(r0,r1)
        Pull    "r1,pc",,^
 ]


; ===========
; OldRandomId
; ===========

; entry R0->start of old free space map

OldRandomId
        Push    "r1-r2,lr"
        BL      GetRandomId
        STRB    lr, [r0, #OldId]
        MOV     lr, lr, LSR #8
        STRB    lr, [r0, #OldId+1]
        MOV     r1, #SzOldFs/2
        ADD     r0, r0, r1
        BL      CheckSum        ;(r0,r1->r2,V)
        STRB    r2, [r0, #Check1-SzOldFs/2]
        Pull    "r1-r2,pc",,^


; ====
; Sort
; ====

; sort a table of words into ascending order
; R0 start
; R1 end

Sort
        Push    "R2-R6,LR"              ;SHELL SORT TABLE
        SUB     R3,R1,R0                ;table length
        CMPS    R3,#2*4
        BLO     %FT30                   ;only sort if 2 or more entries
        CMPS    R3,#13*4
        MOVLS   R2,#1*4
        MOVHI   R2,#4*4
        CMPS    R3,#40*4
        MOVHI   R2,#13*4
15
        ADD     R3,R0,R2
        MOV     R6,R3
20
        MOV     R4,R3
        LDR     R5,[R3],#4
25
        LDR     LR,[R4,-R2]
        CMPS    LR,R5
        STRHS   LR,[R4],-R2
        CMPHSS  R4,R6
        BHS     %BT25
        STR     R5,[R4]
        CMPS    R3,R1
        BLO     %BT20
        CMPS    R2,#4*4
        MOVEQ   R2,#1*4
        MOVHI   R2,#4*4
        BHS     %BT15
30
        Pull    "R2-R6,PC",,^

        LTORG
        END
