NetBSD/sys/arch/arm32/boot/swiv.S

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