swiv

; Copyright 1996 Acorn Computers Ltd
;
; Licensed under the Apache License, Version 2.0 (the "License");
; you may not use this file except in compliance with the License.
; You may obtain a copy of the License at
;
;     http://www.apache.org/licenses/LICENSE-2.0
;
; Unless required by applicable law or agreed to in writing, software
; distributed under the License is distributed on an "AS IS" BASIS,
; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
; See the License for the specific language governing permissions and
; limitations under the License.
;
r0              RN      0
r1              RN      1
r2              RN      2
r3              RN      3
r4              RN      4
r5              RN      5
r6              RN      6
r7              RN      7
r8              RN      8
r9              RN      9
r10             RN      10
r11             RN      11
r12             RN      12
sp              RN      13
lr              RN      14
pc              RN      15


                GET     s.h_StrongA

                AREA    |C$$code|, CODE, READONLY

        EXPORT  |_swix|
        EXPORT  |_swi|

  [ StrongARM
    ; tedious static _swi(x) entry handling, to avoid generating dynamic code, and
    ; requiring an expensive XOS_SynchroniseCodeAreas

|_swix|
        ORR     r0, r0, #&20000
        TST     r1, #&FF0                 ; check for use of input regs. 4 to 9, or of block param
        BNE     swix_even_more_tedious    ; if so, do full stuff

        STMFD   sp!, {r2, r3}             ; put 1st two variadic args on stack
        STMDB   sp!, {r1, r4-r9, lr}      ; save stuff

        SUB     sp, sp, #5*4              ; so we can use tail code common with dynamic version (and room for regs stash)

        ADD     r14, sp, #(5+8)*4         ; r14 -> input args
        MOV     r12, r0                   ; target SWI code
        STR     fp, [sp]                  ; stash fp

        MOV     r11, r1
        TST     r11, #&001
        LDRNE   r0, [r14], #4
        TST     r11, #&002
        LDRNE   r1, [r14], #4
        TST     r11, #&004
        LDRNE   r2, [r14], #4
        TST     r11, #&008
        LDRNE   r3, [r14], #4

        STR     r14, [sp, #4]             ; stash args ptr
        LDR     fp, [sp, #0]              ; get fp and lr saying something useful in case
        LDR     lr, [sp, #48]             ; SWI aborts or throws an error.
        SWI     XOS_CallASWIR12
        LDR     ip, [sp, #4]              ; restore (ip -> args)
        B       SWIXReturn

swix_even_more_tedious
|_swi|
        STMFD   sp!, {r2, r3}             ; put 1st two variadic args on stack
        STMDB   sp!, {r1, r4-r9, lr}      ; save stuff

        SUB     sp, sp, #5*4              ; so we can use tail code common with dynamic version (and room for regs stash)

        ADD     r14, sp, #(5+8)*4         ; r14 -> input args
        MOV     r12, r0                   ; target SWI code
        STR     fp, [sp]                  ; stash fp


        MOV     r11, r1
        TST     r11, #&001
        LDRNE   r0, [r14], #4
        TST     r11, #&002
        LDRNE   r1, [r14], #4
        TST     r11, #&004
        LDRNE   r2, [r14], #4
        TST     r11, #&008
        LDRNE   r3, [r14], #4
        TST     r11, #&010
        LDRNE   r4, [r14], #4
        TST     r11, #&020
        LDRNE   r5, [r14], #4
        TST     r11, #&040
        LDRNE   r6, [r14], #4
        TST     r11, #&080
        LDRNE   r7, [r14], #4
        TST     r11, #&100
        LDRNE   r8, [r14], #4
        TST     r11, #&200
        LDRNE   r9, [r14], #4

        STR     r14, [sp, #4]             ; stash args ptr
        TST     r11, #&800                ; use of block parameter input?
        BLNE    swi_blockhead             ; if so, handle it and...
        LDRNE   r14, [sp, #4]             ; ...restore arg ptr

        TST     r12, #&20000              ; if non X SWI, could be a return value register
        BEQ     swi_beyond_a_joke

        LDR     fp, [sp, #0]              ; get fp and lr saying something useful in case
        LDR     lr, [sp, #48]             ; SWI aborts or throws an error.
        SWI     XOS_CallASWIR12
        LDR     ip, [sp, #4]              ; restore (ip -> args)
        B       SWIXReturn

swi_beyond_a_joke
;so we have to deal with a return value then
        LDR     fp, [sp, #0]              ; get fp and lr saying something useful in case
        LDR     lr, [sp, #48]             ; SWI aborts or throws an error.
        SWI     XOS_CallASWIR12
        LDR     ip, [sp, #4]              ; restore (ip -> args)
        STR     pc, [sp, #4*4]!
        LDR     lr, [sp, #1*4]
;right, if R0 is also required as an output param, we'd better sort that first
        TST     lr,#&80000000
        BEQ     swi_beyond_a_joke_R0safe
        LDRNE   lr, [r12], #4
        STRNE   r0, [lr]
        LDR     lr, [sp, #1*4]
        BIC     lr,lr,#&80000000       ;done it now
        STR     lr, [sp, #1*4]
swi_beyond_a_joke_R0safe
        ANDS    lr, lr, #&000F0000     ;select return value register
        BEQ     SWIReturn2
        CMP     lr, #&00010000
        MOVEQ   r0, r1
        CMP     lr, #&00020000
        MOVEQ   r0, r2
        CMP     lr, #&00030000
        MOVEQ   r0, r3
        CMP     lr, #&00040000
        MOVEQ   r0, r4
        CMP     lr, #&00050000
        MOVEQ   r0, r5
        CMP     lr, #&00060000
        MOVEQ   r0, r6
        CMP     lr, #&00070000
        MOVEQ   r0, r7
        CMP     lr, #&00080000
        MOVEQ   r0, r8
        CMP     lr, #&00090000
        MOVEQ   r0, r9
        CMP     lr, #&000F0000         ;for goodness sake!
        LDREQ   r0, [sp]
        B       SWIReturn2

swi_blockhead
        STMFD   sp!, {r10-r12, lr}
        LDR     r12, [sp, #(4+1)*4]    ;pick up args ptr from stack
;r12 currently -> first output arg, so crank it past them
        MOVS    r11, r11, ASL #1
        ADDCS   r12, r12, #4           ;tests R0 output bit
        ADDMI   r12, r12, #4           ;tests R1 output bit
        MOV     r10, #5                ;5 more reg bit pairs to go (includes PC and one dummy)
swi_blockhead1
        MOVS    r11, r11, ASL #2
        ADDCS   r12, r12, #4
        ADDMI   r12, r12, #4
        SUBS    r10, r10, #1
        BNE     swi_blockhead1
;now r12 -> parameter block args on stack
        LDR     r11, [sp,#4]
        ANDS    r11, r11, #&f000       ;select reg for parameter block pointer
        MOVEQ   r0, r12
        CMP     r11, #&1000
        MOVEQ   r1, r12
        CMP     r11, #&2000
        MOVEQ   r2, r12
        CMP     r11, #&3000
        MOVEQ   r3, r12
        CMP     r11, #&4000
        MOVEQ   r4, r12
        CMP     r11, #&5000
        MOVEQ   r5, r12
        CMP     r11, #&6000
        MOVEQ   r6, r12
        CMP     r11, #&7000
        MOVEQ   r7, r12
        CMP     r11, #&8000
        MOVEQ   r8, r12
        CMP     r11, #&9000
        MOVEQ   r9, r12

        LDMFD   sp!, {r10-r12, pc}^ ;must restore flags

  ] ; StrongARM


  [ :LNOT: StrongARM

|_swi|

; Construct a stack frame that looks something like this:
;       LDMIA   r12!, {r0..rn}      ; Or NOP if no input regs
;       ADD     Rb, R12, #Nout * 4  ; Or NOP if no parameter block
;       SWI     xxxxxx
;       MOV     R0, Rn              ; Use ADD because Rn is correct bitfield
;       B       SWIReturn
;       saved r4-r11,lr
;       saved r1
;       saved input values (r2...rn)

        STMFD   sp!, {r2-r3}            ; Save r1 and put 1st two variadic args on stack
        STMDB   sp!, {r1, r4-r9, lr}
        ADR     r6, SWIReturn-4
        B       swix0
|_swix|
        ORR     r0, r0, #&20000
        STMDB   sp!, {r2-r3}
        STMDB   sp!, {r1, r4-r9, lr}
        ADR     r6, SWIXReturn-4
swix0
        ORR     r3, r0, #&ef000000      ; Construct SWI instruction
        MOV     r0, r1, LSL #22         ; Construct LDMIA R12!, {regs} instruction
        MOVS    r0, r0, LSR #22         ; {regs} = {} (IE no input regs) we must not
        ORRNE   r0, r0, #&e8000000      ; use an LDMIA R12!, {} instruction as this is an
        ORRNE   r0, r0, #&00bc0000      ; invalid instruction, we use a suitable NOP instead.
        MOV     r5, r1, LSR #16
        AND     r5, r5, #&f
        ORR     r5, r5, #&e1000000
        ORR     r5, r5, #&a00000
        ANDS    r2, r1, #&800
        BLNE    BuildBlockInst
        SUB     r6, r6, sp
        MOV     r6, r6, LSR #2
        BIC     r6, r6, #&ff000000
        ADD     r6, r6, #&ea000000
        STMDB   sp!, {r0,r2,r3,r5,r6}
        ADD     r12, sp, #(5+8)*4       ; Point R12 at input regs on stack.
        MOV     pc, sp                  ; Call routine on stack

SWIReturn
        STR     pc, [sp, #4*4]!
  ] ; not StrongARM
SWIReturn2
        LDR     lr, [sp, #1*4]
        MOVS    lr, lr, ASL #1          ; Shift out setting C if R0 to be written, N
        LDRCS   lr, [r12], #4           ; if R1 to be written.
        STRCS   r0, [lr]
        LDRMI   lr, [r12], #4
        STRMI   r1, [lr]
        LDR     lr, [sp, #1*4]
        B       ReturnTail
SWIXReturn
        STR     pc, [sp, #4*4]!
        LDR     lr, [sp, #1*4]
        BVS     VSetReturn
        MOVS    lr, lr, ASL #1          ; Shift out setting C if R0 to be written, N
        LDRCS   lr, [r12], #4           ; if R1 to be written.
        STRCS   r0, [lr]
        LDRMI   lr, [r12], #4
        STRMI   r1, [lr]
        LDR     lr, [sp, #1*4]
        TST     lr, #&f0000
        MOVEQ   r0, #0
ReturnTail
        MOVS    lr, lr, ASL #3          ; Shift 2 bits each time for the next 2 regs
        LDRCS   r1, [r12], #4
        STRCS   r2, [r1]
        LDRMI   r1, [r12], #4
        STRMI   r3, [r1]
        AND     lr, lr, #&ff000000
        MOVS    lr, lr, ASL #2
        LDRCS   r1, [r12], #4
        STRCS   r4, [r1]
        BEQ     VSetReturn              ; Typically saves 16S - (3S + 1N)
        LDRMI   r1, [r12], #4
        STRMI   r5, [r1]
        MOVS    lr, lr, ASL #2
        LDRCS   r1, [r12], #4
        STRCS   r6, [r1]
        LDRMI   r1, [r12], #4
        STRMI   r7, [r1]
        MOVS    lr, lr, ASL #2
        LDRCS   r1, [r12], #4
        STRCS   r8, [r1]
        LDRMI   r1, [r12], #4
        STRMI   r9, [r1]
        MOVS    lr, lr, ASL #2
        LDRCS   r9, [sp]
        LDRCS   r1, [r12], #4
        STRCS   r9, [r1]
VSetReturn
        ADD     sp, sp, #2*4
        LDMIA   sp!, {r4-r9,lr}
        ADD     sp, sp, #2 * 4
        MOVS    pc, lr

  [ :LNOT: StrongARM
BuildBlockInst
        MOV     r4, #6
        AND     r2, r1, #&f000
        ORR     r2, r2, #&e2000000
        ORR     r2, r2, #&008c0000
BuildBlockInst1
        MOVS    r1, r1, ASL #2
        ADDCS   r2, r2, #4
        ADDMI   r2, r2, #4
        SUBS    r4, r4, #1
        BNE     BuildBlockInst1
        MOVS    pc, lr
  ]

        END