Added saving of banked register.
git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11396 35acf78f-673a-0410-8e92-d51de3d6d3f4
This commit is contained in:
isiora
parent
240b09a926
commit
014655f97b
|
|
@ -73,10 +73,61 @@
|
|||
.set MSG_TIMEOUT, -1
|
||||
.set MSG_RESET, -2
|
||||
|
||||
// .comm sm_secctx, 32*4, 4
|
||||
// .comm sm_nsecctx, 32*4, 4
|
||||
.global sm_secctx
|
||||
.global sm_nsecctx
|
||||
.global _ns_thread
|
||||
.section .text
|
||||
.code 32
|
||||
.balign 4
|
||||
/*
|
||||
* Helper macros
|
||||
*/
|
||||
/*
|
||||
* Store out of context registers in a world area pointed by r0
|
||||
*/
|
||||
.macro sm_store_ooctx_regs rm
|
||||
// cpsxx #MODE_SYS // Assume mode SYS
|
||||
stm \rm!, {sp, lr}
|
||||
cps #MODE_FIQ
|
||||
mrs r12, spsr
|
||||
stm \rm!, {r12, sp, lr}
|
||||
cps #MODE_IRQ
|
||||
mrs r12, spsr
|
||||
stm \rm!, {r12, sp, lr}
|
||||
cps #MODE_ABT
|
||||
mrs r12, spsr
|
||||
stm \rm!, {r12, sp, lr}
|
||||
cps #MODE_SVC
|
||||
mrs r12, spsr
|
||||
stm \rm!, {r12, sp, lr}
|
||||
cps #MODE_UND
|
||||
mrs r12, spsr
|
||||
stm \rm!, {r12, sp, lr}
|
||||
.endm
|
||||
/*
|
||||
* Retrieve out of context registers from a world area pointed by r0
|
||||
*/
|
||||
.macro sm_load_ooctx_regs rm
|
||||
// cpsxx #MODE_SYS // Assume mode SYS
|
||||
ldm \rm!, {sp, lr}
|
||||
cps #MODE_FIQ
|
||||
ldm \rm!, {r12, sp, lr}
|
||||
msr spsr_fsxc, r12
|
||||
cps #MODE_IRQ
|
||||
ldm \rm!, {r12, sp, lr}
|
||||
msr spsr_fsxc, r12
|
||||
cps #MODE_ABT
|
||||
ldm \rm!, {r12, sp, lr}
|
||||
msr spsr_fsxc, r12
|
||||
cps #MODE_SVC
|
||||
ldm \rm!, {r12, sp, lr}
|
||||
msr spsr_fsxc, r12
|
||||
cps #MODE_UND
|
||||
ldm \rm!, {r12, sp, lr}
|
||||
msr spsr_fsxc, r12
|
||||
.endm
|
||||
/*
|
||||
* Monitor vectors
|
||||
*/
|
||||
|
|
@ -94,25 +145,53 @@ _monitor_vectors:
|
|||
* SMC entry
|
||||
*/
|
||||
sm_call:
|
||||
stmfd sp!, {r3}
|
||||
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
|
||||
beq 1f
|
||||
msr CPSR_c, #MODE_SYS | I_BIT // switch to sys mode, foreign int disabled
|
||||
|
||||
cpsid if, #MODE_SYS // ints disabled
|
||||
ldr r3, =sm_nsecctx
|
||||
sm_store_ooctx_regs r3
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
ldr r3, =sm_secctx
|
||||
sm_load_ooctx_regs r3
|
||||
|
||||
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT // switch to sys mode
|
||||
stmfd sp!, {lr} // save lr
|
||||
|
||||
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
|
||||
bl _dbg_check_lock
|
||||
#endif
|
||||
bl smcEntry // call the C smc handler
|
||||
ldmfd sp!, {lr} // restore lr
|
||||
|
||||
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
|
||||
bl _dbg_check_unlock
|
||||
#endif
|
||||
ldmfd sp!, {lr} // restore lr
|
||||
|
||||
ldr r3, =sm_secctx
|
||||
sm_store_ooctx_regs r3
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
ldr r3, =sm_nsecctx
|
||||
sm_load_ooctx_regs r3
|
||||
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT // switch to monitor mode
|
||||
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
|
||||
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
|
||||
mcr p15, 0, r12, c1, c1, 0
|
||||
ldmfd sp!, {r3}
|
||||
subs pc, lr, #0 // return from smc
|
||||
|
||||
/*
|
||||
|
|
@ -131,8 +210,26 @@ sm_fiq:
|
|||
stmfd sp!, {r0}
|
||||
ldr r0, =MON_S_SCR // enter in the secure world
|
||||
mcr p15, 0, r0, c1, c1, 0
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
ldr r0, =sm_nsecctx
|
||||
sm_store_ooctx_regs r0
|
||||
|
||||
cpsid if, #MODE_SYS
|
||||
ldr r0, =sm_secctx
|
||||
sm_load_ooctx_regs r0
|
||||
|
||||
msr CPSR_c, #MODE_SYS | I_BIT // FIQ enabled, served via base table
|
||||
msr CPSR_c, #MODE_MON | I_BIT | F_BIT // the handler returns here. Switch to monitor mode
|
||||
|
||||
cpsid if, #MODE_SYS // the handler returns here.
|
||||
ldr r0, =sm_secctx
|
||||
sm_store_ooctx_regs r0
|
||||
|
||||
cpsid if, #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
|
||||
mcr p15, 0, r0, c1, c1, 0
|
||||
ldmfd sp!, {r0}
|
||||
|
|
@ -147,11 +244,11 @@ sm_fiq:
|
|||
* 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
|
||||
* The non secure IRQ handler has then the responsibility to return into
|
||||
* The non secure world has then the responsibility to return into
|
||||
* secure state via a smc.
|
||||
*
|
||||
*/
|
||||
sm_irq:
|
||||
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
|
||||
|
|
@ -185,7 +282,25 @@ sm_fiq:
|
|||
.global _ns_trampoline
|
||||
_ns_trampoline:
|
||||
mov r1, r0
|
||||
|
||||
ldr r0, =sm_secctx
|
||||
sm_store_ooctx_regs r0
|
||||
cps #MODE_SYS
|
||||
|
||||
ldr r0, =#0
|
||||
mov r2, r0
|
||||
mov r3, r0
|
||||
mov r4, r0
|
||||
mov r5, r0
|
||||
mov r6, r0
|
||||
mov r7, r0
|
||||
mov r8, r0
|
||||
mov r9, r0
|
||||
mov r10, r0
|
||||
mov r11, r0
|
||||
mov r12, r0
|
||||
mov sp, r0
|
||||
mov lr, r0
|
||||
smc #0
|
||||
|
||||
#endif /* !defined(__DOXYGEN__) */
|
||||
|
|
|
|||
Loading…
Reference in New Issue