81 lines
2.3 KiB
ArmAsm
81 lines
2.3 KiB
ArmAsm
|
#include "regs.h"
|
||
|
|
|
||
|
|
.text
|
||
|
|
|
||
|
|
SWIReturnInst:
|
||
|
|
ldr pc, [sp, #0*4]
|
||
|
|
|
||
|
|
.global _swix
|
||
|
|
_swix:
|
||
|
|
orr r0, r0, #0x20000
|
||
|
|
|
||
|
|
.global _swi
|
||
|
|
_swi:
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Construct a stack frame that looks something like this:
|
||
|
|
* returnval
|
||
|
|
* LDMIA r12!, {r0..rn}
|
||
|
|
* SWI xxxxxx
|
||
|
|
* LDR pc, [sp]
|
||
|
|
* saved r4-r11,lr
|
||
|
|
* saved r1
|
||
|
|
* saved input values (r2...rn)
|
||
|
|
*/
|
||
|
|
|
||
|
|
mov pc, lr
|
||
|
|
|
||
|
|
stmfd sp!, {r1-r3} /* Save r1 and put 1st two variadic args on stack */
|
||
|
|
bic r2, r0, #0xff000000
|
||
|
|
orr r2, r2, #0xef000000 /* Construct SWI instruction */
|
||
|
|
adr r0, SWIReturn
|
||
|
|
tst r1, #0x20000 /* bit for write flags */
|
||
|
|
adrne r0, SWIReturnFlags
|
||
|
|
bic r1, r1, #0xff000000 /* Construct LDMIA R12!, {regs} instruction, if */
|
||
|
|
bics r1, r1, #0x00ff0000 /* {regs} = {} (IE no input regs) we must not */
|
||
|
|
orrne r1, r1, #0xe8000000 /* use an LDMIA R12!, {} instruction as this is an */
|
||
|
|
orrne r1, r1, #0x00bc0000 /* invalid instruction, we use a suitable NOP instead */
|
||
|
|
moveq r1, #0 /* 0 = opcode for ANDEQ r0, r0, r0 (a suitable NOP) */
|
||
|
|
ldr r3, SWIReturnInst
|
||
|
|
stmfd sp!, {r0-r9, r11, lr} /* Save regs and set up SWI call routine (in R0-R3) */
|
||
|
|
add r12, sp, #(12+1)*4 /* Point R12 at input regs on stack. */
|
||
|
|
add pc, sp, #4 /* Call routine on stack */
|
||
|
|
SWIReturnFlags:
|
||
|
|
ldr r11, [r12], #4
|
||
|
|
str pc, [r11] /* write flags */
|
||
|
|
SWIReturn:
|
||
|
|
ldr lr, [sp, #(12+0)*4] /* Fetch reg mask again */
|
||
|
|
movs lr, lr, asl #1 /* Shift out setting C if R0 to be written, N */
|
||
|
|
ldrcs r11, [r12], #4 /* if R1 to be written. */
|
||
|
|
strcs r0, [r11]
|
||
|
|
ldrmi r11, [r12], #4
|
||
|
|
strmi r1, [r11]
|
||
|
|
movs lr, lr, asl #2 /* Shift 2 bits each time for the next 2 regs */
|
||
|
|
ldrcs r11, [r12], #4
|
||
|
|
strcs r2, [r11]
|
||
|
|
ldrmi r11, [r12], #4
|
||
|
|
strmi r3, [r11]
|
||
|
|
movs lr, lr, asl #2
|
||
|
|
ldrcs r11, [r12], #4
|
||
|
|
strcs r4, [r11]
|
||
|
|
ldrmi r11, [r12], #4
|
||
|
|
strmi r5, [r11]
|
||
|
|
movs lr, lr, asl #2
|
||
|
|
ldrcs r11, [r12], #4
|
||
|
|
strcs r6, [r11]
|
||
|
|
ldrmi r11, [r12], #4
|
||
|
|
strmi r7, [r11]
|
||
|
|
movs lr, lr, asl #2
|
||
|
|
ldrcs r11, [r12], #4
|
||
|
|
strcs r8, [r11]
|
||
|
|
ldrmi r11, [r12], #4
|
||
|
|
strmi r9, [r11]
|
||
|
|
ldr r1, [sp, #2*4]
|
||
|
|
tst r1, #0x20000 /* X-bit clear */
|
||
|
|
cmpeq pc, #0x80000000 /* SET V flag if so, so R0 not cleared */
|
||
|
|
movvc r0, #0 /* Clear R0 if no error (or X-bit clear) */
|
||
|
|
add sp, sp, #4*4 /* Drop SWI call routine */
|
||
|
|
ldmia sp!, {r4-r9,r11,lr}
|
||
|
|
add sp, sp, #3*4 /* Drop saved R1 and 1st two variadic args. */
|
||
|
|
movs pc, lr
|