Fixed r12 (ip) saving.
Code cleanup. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11417 35acf78f-673a-0410-8e92-d51de3d6d3f4
This commit is contained in:
isiora
parent
6f0c84af5e
commit
6082e361d8
|
|
@ -56,9 +56,6 @@
|
|||
.set MODE_UND, 0x1B
|
||||
.set MODE_SYS, 0x1F
|
||||
|
||||
.set I_BIT, 0x80
|
||||
.set F_BIT, 0x40
|
||||
|
||||
.set SCR_NS, 0x01
|
||||
.set SCR_IRQ, 0x02
|
||||
.set SCR_FIQ, 0x04
|
||||
|
|
@ -69,12 +66,10 @@
|
|||
.set MON_S_SCR, (SCR_IRQ) // (SCR_EA|SCR_IRQ)
|
||||
.set MON_NS_SCR, (SCR_FIQ|SCR_NS)
|
||||
|
||||
.set MSG_OK, 0
|
||||
.set MSG_TIMEOUT, -1
|
||||
.set MSG_RESET, -2
|
||||
.set SMC_SVC_INTR, -1
|
||||
|
||||
.comm sm_secctx, 32*4, 4
|
||||
.comm sm_nsecctx, 32*4, 4
|
||||
.comm sm_secctx, 20*4, 4
|
||||
.comm sm_nsecctx, 20*4, 4
|
||||
|
||||
.global _ns_thread
|
||||
.section .text
|
||||
|
|
@ -87,7 +82,7 @@
|
|||
* Store out of context registers in a world area pointed by rm
|
||||
*/
|
||||
.macro sm_store_ooctx_regs rm
|
||||
// cpsxx #MODE_SYS // Assume mode SYS
|
||||
// cps #MODE_SYS // Assume mode SYS
|
||||
stm \rm!, {sp, lr}
|
||||
cps #MODE_FIQ
|
||||
mrs r12, spsr
|
||||
|
|
@ -109,7 +104,7 @@
|
|||
* Retrieve out of context registers from a world area pointed by rm
|
||||
*/
|
||||
.macro sm_load_ooctx_regs rm
|
||||
// cpsxx #MODE_SYS // Assume mode SYS
|
||||
// cps #MODE_SYS // Assume mode SYS
|
||||
ldm \rm!, {sp, lr}
|
||||
cps #MODE_FIQ
|
||||
ldm \rm!, {r12, sp, lr}
|
||||
|
|
@ -144,54 +139,53 @@ _monitor_vectors:
|
|||
* SMC entry
|
||||
*/
|
||||
sm_call:
|
||||
stmfd sp!, {r3}
|
||||
ldr r12, =MON_S_SCR // enter in the secure world
|
||||
stmfd sp!, {r3, r12}
|
||||
ldr r12, =MON_S_SCR // enter in the secure world
|
||||
mcr p15, 0, r12, c1, c1, 0
|
||||
ands r0, r0 // OS special service,
|
||||
// 0 == jump trampoline to non secure world
|
||||
// r1 contains the address where it jumps
|
||||
ands r0, r0 // OS special service,
|
||||
// 0 == jump trampoline to non secure world
|
||||
// r1 contains the address where it jumps
|
||||
beq 1f
|
||||
|
||||
mrs r3, SPSR
|
||||
mov r12, lr
|
||||
stmfd sp!, {r3, r12} // push r3=spsr_mon, r12=lr_mon.
|
||||
stmfd sp!, {r3, r12} // push r3=spsr_mon, r12=lr_mon.
|
||||
|
||||
cpsid if, #MODE_SYS // ints disabled
|
||||
cps #MODE_SYS // switch to sys mode, ints disabled
|
||||
ldr r3, =sm_nsecctx
|
||||
sm_store_ooctx_regs r3
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
cps #MODE_SYS
|
||||
ldr r3, =sm_secctx
|
||||
sm_load_ooctx_regs r3
|
||||
|
||||
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT // switch to sys mode
|
||||
|
||||
bl smcEntry // call the C smc handler
|
||||
cps #MODE_SYS
|
||||
bl smcEntry // call the C smc handler
|
||||
|
||||
ldr r3, =sm_secctx
|
||||
sm_store_ooctx_regs r3
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
cps #MODE_SYS
|
||||
ldr r3, =sm_nsecctx
|
||||
sm_load_ooctx_regs r3
|
||||
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT // switch to monitor mode
|
||||
cps #MODE_MON // switch to monitor mode
|
||||
|
||||
ldmfd sp!, {r3, r12} // pop r3=spsr_mon, r12=lr_mon.
|
||||
ldmfd sp!, {r3, r12} // pop r3=spsr_mon, r12=lr_mon.
|
||||
msr SPSR_fsxc, r3
|
||||
mov lr, r12
|
||||
|
||||
ldr r12, =MON_NS_SCR // enter in the non-secure world
|
||||
ldr r12, =MON_NS_SCR // enter in the non-secure world
|
||||
mcr p15, 0, r12, c1, c1, 0
|
||||
ldmfd sp!, {r3}
|
||||
subs pc, lr, #0 // return from smc
|
||||
ldmfd sp!, {r3, r12}
|
||||
subs pc, lr, #0 // return from smc
|
||||
1:
|
||||
mov lr, r1 // use the address in r1 as return address
|
||||
// in the non secure world
|
||||
ldr r12, =MON_NS_SCR // enter in the non-secure world
|
||||
mov lr, r1 // use the address in r1 as return address
|
||||
// in the non secure world
|
||||
ldr r12, =MON_NS_SCR // enter in the non-secure world
|
||||
mcr p15, 0, r12, c1, c1, 0
|
||||
ldmfd sp!, {r3}
|
||||
subs pc, lr, #0 // return from smc
|
||||
ldmfd sp!, {r3, r12}
|
||||
subs pc, lr, #0 // return from smc
|
||||
|
||||
/*
|
||||
* FIQ entry
|
||||
|
|
@ -205,41 +199,41 @@ sm_call:
|
|||
* without scheduling.
|
||||
*/
|
||||
sm_fiq:
|
||||
// check point: SCR.NS == 1
|
||||
stmfd sp!, {r0}
|
||||
ldr r0, =MON_S_SCR // enter in the secure world
|
||||
// check point: SCR.NS == 1
|
||||
stmfd sp!, {r0, r12}
|
||||
ldr r0, =MON_S_SCR // enter in the secure world
|
||||
mcr p15, 0, r0, c1, c1, 0
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
cps #MODE_SYS
|
||||
ldr r0, =sm_nsecctx
|
||||
sm_store_ooctx_regs r0
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
cps #MODE_SYS
|
||||
ldr r0, =sm_secctx
|
||||
sm_load_ooctx_regs r0
|
||||
|
||||
msr CPSR_c, #MODE_SYS | I_BIT // FIQ enabled, served via base table
|
||||
cpsie f, #MODE_SYS // FIQ enabled, served via base table
|
||||
|
||||
cpsid if, #MODE_SYS // the handler returns here.
|
||||
cpsid f, #MODE_SYS // the handler returns here.
|
||||
ldr r0, =sm_secctx
|
||||
sm_store_ooctx_regs r0
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
cps #MODE_SYS
|
||||
ldr r0, =sm_nsecctx
|
||||
sm_load_ooctx_regs r0
|
||||
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT // switch to monitor mode
|
||||
ldr r0, =MON_NS_SCR // set non-secure SCR before return
|
||||
cps #MODE_MON // switch to monitor mode
|
||||
ldr r0, =MON_NS_SCR // set non-secure SCR before return
|
||||
mcr p15, 0, r0, c1, c1, 0
|
||||
ldmfd sp!, {r0}
|
||||
subs pc, lr, #4 // return into non-secure world
|
||||
ldmfd sp!, {r0, r12}
|
||||
subs pc, lr, #4 // return into non-secure world
|
||||
/*
|
||||
* IRQ entry
|
||||
*
|
||||
* Here the IRQ is taken from secure state.
|
||||
* Current mode is monitor (so current state is secure),
|
||||
* the previous mode and status is in mon.spsr and
|
||||
* the return address+4 is in mon.lr.
|
||||
* the previous mode and status is in spsr_mon and
|
||||
* the return address+4 is in lr_mon.
|
||||
* Because we are running in secure state, we are sure that
|
||||
* the main thread is suspended in the smc handler.
|
||||
* The main thread is then resumed with MSG_TIMEOUT
|
||||
|
|
@ -248,35 +242,35 @@ sm_fiq:
|
|||
*
|
||||
*/
|
||||
sm_irq:
|
||||
// check point: SCR.NS == 0
|
||||
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT
|
||||
stmfd sp!, {r0-r3, r12, lr} // save scratch registers and lr
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT
|
||||
mrs r0, SPSR
|
||||
// check point: SCR.NS == 0
|
||||
cps #MODE_SYS
|
||||
stmfd sp!, {r0-r3, r12, lr} // save scratch registers and lr
|
||||
cps #MODE_MON // switch to monitor mode
|
||||
mrs r0, spsr
|
||||
mov r1, lr
|
||||
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT
|
||||
stmfd sp!, {r0, r1} // push R0=SPSR, R1=LR_MON.
|
||||
// check point: ns_tread != 0
|
||||
cps #MODE_SYS
|
||||
stmfd sp!, {r0, r1} // push r0=spsr_mon, r1=lr_mon.
|
||||
// check point: ns_tread != 0
|
||||
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
|
||||
bl _dbg_check_lock
|
||||
#endif
|
||||
ldr r0, =_ns_thread
|
||||
mov r1, #MSG_TIMEOUT
|
||||
bl chThdResumeS // resume the ns_thread and serve the IRQ
|
||||
// into non-secure world
|
||||
mov r1, #SMC_SVC_INTR
|
||||
bl chThdResumeS // resume the ns_thread and serve the IRQ
|
||||
// into non-secure world
|
||||
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
|
||||
bl _dbg_check_unlock
|
||||
#endif
|
||||
// The ns_thread reentered smc, that set SRC.NS to 0
|
||||
// re-establish the original conditions
|
||||
ldmfd sp!, {r0, r1} // pop R0=SPSR, R1=LR_MON.
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT
|
||||
ldmfd sp!, {r0, r1} // pop r0=spsr_mon, r1=lr_mon.
|
||||
cps #MODE_MON // switch to monitor mode
|
||||
msr SPSR_fsxc, r0
|
||||
mov lr, r1
|
||||
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT
|
||||
cps #MODE_SYS
|
||||
ldmfd sp!, {r0-r3, r12, lr}
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT
|
||||
subs pc, lr, #4 // return into secure world
|
||||
cps #MODE_MON // switch to monitor mode
|
||||
subs pc, lr, #4 // return into secure world
|
||||
|
||||
.global _ns_trampoline
|
||||
_ns_trampoline:
|
||||
|
|
|
|||
Loading…
Reference in New Issue