andreika-git
/
ChibiOS
mirror of https://github.com/andreika-git/ChibiOS.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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 2018-01-24 06:54:36 +00:00
parent 240b09a926
commit 014655f97b
1 changed files with 120 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S
View File

@ -73,10 +73,61 @@
.set MSG_TIMEOUT, -1 .set MSG_TIMEOUT, -1
.set MSG_RESET, -2 .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 .global _ns_thread
.section .text .section .text
.code 32 .code 32
.balign 4 .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 * Monitor vectors
*/ */
@ -94,25 +145,53 @@ _monitor_vectors:
* SMC entry * SMC entry
*/ */
sm_call: sm_call:
stmfd sp!, {r3}
ldr r12, =MON_S_SCR // enter in the secure world ldr r12, =MON_S_SCR // enter in the secure world
mcr p15, 0, r12, c1, c1, 0 mcr p15, 0, r12, c1, c1, 0
ands r0, r0 // OS special service, ands r0, r0 // OS special service,
// 0 == jump trampoline to non secure world // 0 == jump trampoline to non secure world
// r1 contains the address where it jumps // r1 contains the address where it jumps
beq 1f 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 stmfd sp!, {lr} // save lr
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
bl _dbg_check_lock
#endif
bl smcEntry // call the C smc handler 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 msr CPSR_c, #MODE_MON | I_BIT | F_BIT // switch to monitor mode
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 mcr p15, 0, r12, c1, c1, 0
ldmfd sp!, {r3}
subs pc, lr, #0 // return from smc subs pc, lr, #0 // return from smc
1: 1:
mov lr, r1 // use the address in r1 as return address mov lr, r1 // use the address in r1 as return address
// in the non secure world // in the non secure world
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 mcr p15, 0, r12, c1, c1, 0
ldmfd sp!, {r3}
subs pc, lr, #0 // return from smc subs pc, lr, #0 // return from smc
/* /*
@ -131,8 +210,26 @@ sm_fiq:
stmfd sp!, {r0} stmfd sp!, {r0}
ldr r0, =MON_S_SCR // enter in the secure world ldr r0, =MON_S_SCR // enter in the secure world
mcr p15, 0, r0, c1, c1, 0 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_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 ldr r0, =MON_NS_SCR // set non-secure SCR before return
mcr p15, 0, r0, c1, c1, 0 mcr p15, 0, r0, c1, c1, 0
ldmfd sp!, {r0} ldmfd sp!, {r0}
@ -147,11 +244,11 @@ sm_fiq:
* Because we are running in secure state, we are sure that * Because we are running in secure state, we are sure that
* the main thread is suspended in the smc handler. * the main thread is suspended in the smc handler.
* The main thread is then resumed with MSG_TIMEOUT * 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. * secure state via a smc.
* *
*/ */
sm_irq: sm_irq:
// check point: SCR.NS == 0 // check point: SCR.NS == 0
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT msr CPSR_c, #MODE_SYS | I_BIT | F_BIT
stmfd sp!, {r0-r3, r12, lr} // save scratch registers and lr stmfd sp!, {r0-r3, r12, lr} // save scratch registers and lr
@ -185,7 +282,25 @@ sm_fiq:
.global _ns_trampoline .global _ns_trampoline
_ns_trampoline: _ns_trampoline:
mov r1, r0 mov r1, r0
ldr r0, =sm_secctx
sm_store_ooctx_regs r0
cps #MODE_SYS
ldr r0, =#0 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 smc #0
#endif /* !defined(__DOXYGEN__) */ #endif /* !defined(__DOXYGEN__) */