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

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 2018-01-30 22:33:11 +00:00
parent 6f0c84af5e
commit 6082e361d8
1 changed files with 56 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,9 +56,6 @@
.set MODE_UND, 0x1B .set MODE_UND, 0x1B
.set MODE_SYS, 0x1F .set MODE_SYS, 0x1F
.set I_BIT, 0x80
.set F_BIT, 0x40
.set SCR_NS, 0x01 .set SCR_NS, 0x01
.set SCR_IRQ, 0x02 .set SCR_IRQ, 0x02
.set SCR_FIQ, 0x04 .set SCR_FIQ, 0x04
@ -69,12 +66,10 @@
.set MON_S_SCR, (SCR_IRQ) // (SCR_EA|SCR_IRQ) .set MON_S_SCR, (SCR_IRQ) // (SCR_EA|SCR_IRQ)
.set MON_NS_SCR, (SCR_FIQ|SCR_NS) .set MON_NS_SCR, (SCR_FIQ|SCR_NS)
.set MSG_OK, 0 .set SMC_SVC_INTR, -1
.set MSG_TIMEOUT, -1
.set MSG_RESET, -2
.comm sm_secctx, 32*4, 4 .comm sm_secctx, 20*4, 4
.comm sm_nsecctx, 32*4, 4 .comm sm_nsecctx, 20*4, 4
.global _ns_thread .global _ns_thread
.section .text .section .text
@ -87,7 +82,7 @@
* Store out of context registers in a world area pointed by rm * Store out of context registers in a world area pointed by rm
*/ */
.macro sm_store_ooctx_regs rm .macro sm_store_ooctx_regs rm
// cpsxx #MODE_SYS // Assume mode SYS // cps #MODE_SYS // Assume mode SYS
stm \rm!, {sp, lr} stm \rm!, {sp, lr}
cps #MODE_FIQ cps #MODE_FIQ
mrs r12, spsr mrs r12, spsr
@ -109,7 +104,7 @@
* Retrieve out of context registers from a world area pointed by rm * Retrieve out of context registers from a world area pointed by rm
*/ */
.macro sm_load_ooctx_regs rm .macro sm_load_ooctx_regs rm
// cpsxx #MODE_SYS // Assume mode SYS // cps #MODE_SYS // Assume mode SYS
ldm \rm!, {sp, lr} ldm \rm!, {sp, lr}
cps #MODE_FIQ cps #MODE_FIQ
ldm \rm!, {r12, sp, lr} ldm \rm!, {r12, sp, lr}
@ -144,54 +139,53 @@ _monitor_vectors:
* SMC entry * SMC entry
*/ */
sm_call: sm_call:
stmfd sp!, {r3} stmfd sp!, {r3, r12}
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
mrs r3, SPSR mrs r3, SPSR
mov r12, lr 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 ldr r3, =sm_nsecctx
sm_store_ooctx_regs r3 sm_store_ooctx_regs r3
cpsid if, #MODE_SYS cps #MODE_SYS
ldr r3, =sm_secctx ldr r3, =sm_secctx
sm_load_ooctx_regs r3 sm_load_ooctx_regs r3
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT // switch to sys mode cps #MODE_SYS
bl smcEntry // call the C smc handler
bl smcEntry // call the C smc handler
ldr r3, =sm_secctx ldr r3, =sm_secctx
sm_store_ooctx_regs r3 sm_store_ooctx_regs r3
cpsid if, #MODE_SYS cps #MODE_SYS
ldr r3, =sm_nsecctx ldr r3, =sm_nsecctx
sm_load_ooctx_regs r3 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 msr SPSR_fsxc, r3
mov lr, r12 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 mcr p15, 0, r12, c1, c1, 0
ldmfd sp!, {r3} ldmfd sp!, {r3, r12}
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} ldmfd sp!, {r3, r12}
subs pc, lr, #0 // return from smc subs pc, lr, #0 // return from smc
/* /*
* FIQ entry * FIQ entry
@ -205,41 +199,41 @@ sm_call:
* without scheduling. * without scheduling.
*/ */
sm_fiq: sm_fiq:
// check point: SCR.NS == 1 // check point: SCR.NS == 1
stmfd sp!, {r0} stmfd sp!, {r0, r12}
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 cps #MODE_SYS
ldr r0, =sm_nsecctx ldr r0, =sm_nsecctx
sm_store_ooctx_regs r0 sm_store_ooctx_regs r0
cpsid if, #MODE_SYS cps #MODE_SYS
ldr r0, =sm_secctx ldr r0, =sm_secctx
sm_load_ooctx_regs r0 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 ldr r0, =sm_secctx
sm_store_ooctx_regs r0 sm_store_ooctx_regs r0
cpsid if, #MODE_SYS cps #MODE_SYS
ldr r0, =sm_nsecctx ldr r0, =sm_nsecctx
sm_load_ooctx_regs r0 sm_load_ooctx_regs r0
msr CPSR_c, #MODE_MON | I_BIT | F_BIT // switch to monitor mode cps #MODE_MON // 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, r12}
subs pc, lr, #4 // return into non-secure world subs pc, lr, #4 // return into non-secure world
/* /*
* IRQ entry * IRQ entry
* *
* Here the IRQ is taken from secure state. * Here the IRQ is taken from secure state.
* Current mode is monitor (so current state is secure), * Current mode is monitor (so current state is secure),
* the previous mode and status is in mon.spsr and * the previous mode and status is in spsr_mon and
* the return address+4 is in mon.lr. * the return address+4 is in lr_mon.
* 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
@ -248,35 +242,35 @@ sm_fiq:
* *
*/ */
sm_irq: sm_irq:
// check point: SCR.NS == 0 // check point: SCR.NS == 0
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT cps #MODE_SYS
stmfd sp!, {r0-r3, r12, lr} // save scratch registers and lr stmfd sp!, {r0-r3, r12, lr} // save scratch registers and lr
msr CPSR_c, #MODE_MON | I_BIT | F_BIT cps #MODE_MON // switch to monitor mode
mrs r0, SPSR mrs r0, spsr
mov r1, lr mov r1, lr
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT cps #MODE_SYS
stmfd sp!, {r0, r1} // push R0=SPSR, R1=LR_MON. stmfd sp!, {r0, r1} // push r0=spsr_mon, r1=lr_mon.
// check point: ns_tread != 0 // check point: ns_tread != 0
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE) #if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
bl _dbg_check_lock bl _dbg_check_lock
#endif #endif
ldr r0, =_ns_thread ldr r0, =_ns_thread
mov r1, #MSG_TIMEOUT mov r1, #SMC_SVC_INTR
bl chThdResumeS // resume the ns_thread and serve the IRQ bl chThdResumeS // resume the ns_thread and serve the IRQ
// into non-secure world // into non-secure world
#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE) #if (CH_DBG_SYSTEM_STATE_CHECK == TRUE)
bl _dbg_check_unlock bl _dbg_check_unlock
#endif #endif
// The ns_thread reentered smc, that set SRC.NS to 0 // The ns_thread reentered smc, that set SRC.NS to 0
// re-establish the original conditions // re-establish the original conditions
ldmfd sp!, {r0, r1} // pop R0=SPSR, R1=LR_MON. ldmfd sp!, {r0, r1} // pop r0=spsr_mon, r1=lr_mon.
msr CPSR_c, #MODE_MON | I_BIT | F_BIT cps #MODE_MON // switch to monitor mode
msr SPSR_fsxc, r0 msr SPSR_fsxc, r0
mov lr, r1 mov lr, r1
msr CPSR_c, #MODE_SYS | I_BIT | F_BIT cps #MODE_SYS
ldmfd sp!, {r0-r3, r12, lr} ldmfd sp!, {r0-r3, r12, lr}
msr CPSR_c, #MODE_MON | I_BIT | F_BIT cps #MODE_MON // switch to monitor mode
subs pc, lr, #4 // return into secure world subs pc, lr, #4 // return into secure world
.global _ns_trampoline .global _ns_trampoline
_ns_trampoline: _ns_trampoline: