From 8626b939363019cc72dd51b4bb12720b744f4d8c Mon Sep 17 00:00:00 2001 From: isiora Date: Tue, 4 Jul 2017 15:12:01 +0000 Subject: Added monitor code to switch between secure and non-secure state git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@10301 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- .../ports/ARMCMAx-TZ/compilers/GCC/chcoreasm.S | 94 ++++++++++++++++++++-- 1 file changed, 89 insertions(+), 5 deletions(-) (limited to 'os/common/ports') diff --git a/os/common/ports/ARMCMAx-TZ/compilers/GCC/chcoreasm.S b/os/common/ports/ARMCMAx-TZ/compilers/GCC/chcoreasm.S index 52b6be5e0..bb743a2f5 100644 --- a/os/common/ports/ARMCMAx-TZ/compilers/GCC/chcoreasm.S +++ b/os/common/ports/ARMCMAx-TZ/compilers/GCC/chcoreasm.S @@ -50,13 +50,22 @@ .set MODE_FIQ, 0x11 .set MODE_IRQ, 0x12 .set MODE_SVC, 0x13 - .set MODE_MONITOR, 0x16 + .set MODE_MON, 0x16 .set MODE_ABT, 0x17 .set MODE_UND, 0x1B .set MODE_SYS, 0x1F - .equ I_BIT, 0x80 - .equ F_BIT, 0x40 + .equ I_BIT, 0x80 + .equ F_BIT, 0x40 + .equ SCR_NS, 0x01 + .equ SCR_IRQ, 0x02 + .equ SCR_FIQ, 0x04 + .equ SCR_EA, 0x08 + .equ SCR_FW, 0x10 + .equ SCR_AW, 0x20 + + .set MON_S_SCR, 0 + .set MON_NS_SCR, SCR_FIQ|SCR_NS .text @@ -85,10 +94,10 @@ _port_switch_arm: * | r2 | | External context: IRQ handler frame * | r1 | | * | r0 | | - * | LR_IRQ | | (user code return address) + * | LR_IRQ | | (user code return address)(could be in non-secure space) * | PSR_USR | -+ (user code status) * | .... | <- chSchDoReschedule() stack frame, optimize it for space - * | LR | -+ (system code return address) + * | LR | -+ (system code return address)(always in secure space) * | r11 | | * | r10 | | * | r9 | | @@ -98,11 +107,86 @@ _port_switch_arm: * | r5 | | * SP-> | r4 | -+ * Low +------------+ + * + */ + +/* + * We are on an architecures with security extension exploited. + * The following execution path is taken by the execution units running in + * non-secure state when a fiq interrupt is fired. + * It originates by the monitor fiq vector and runs in monitor mode, + * ie in secure state. + * It assumes the following, set at boot time, or wherever it needs: + * SCR.FW == 0 and SCR.FIQ == 1 and SCR.IRQ == 0 in non-secure state, + * ie FIQs are taken to monitor mode, IRQ locally + * SCR.FIQ == 0 and SCR.IRQ == 1 and CPSR.I == 1 in secure state, + * ie the secure code takes fiq locally and runs always with IRQ disabled(?) + * MVBAR holds the address of the monitor vectors base. + * The code and the stacks memory reside in secure memory. */ .balign 16 .code 32 + .global Mon_Fiq_Handler .global Fiq_Handler +Mon_Fiq_Handler: + // here the fiq is taken from non-secure state + // current mode is monitor (so current state is secure) + stmfd sp!, {lr} // save lr into monitor stack + ldr lr, =#MON_S_SCR // set secure SCR before to switch to FIQ mode + mrc p15, 0, lr, c1, c1, 0 + cpsid if, #MODE_FIQ // secure FIQ mode + stmfd sp!, {r0-r3, r12} // IRQ frame + ldr r0, =ARM_IRQ_VECTOR_REG + ldr r0, [r0] + ldr lr, =_mon_fiq_ret_arm // ISR return point. + bx r0 // Calling the ISR. +_mon_fiq_ret_arm: + cmp r0, #0 + ldmfd sp!, {r0-r3, r12} + cpsid if, #MODE_MON + ldr lr, =#MON_NS_SCR // set non-secure SCR before return + mrceq p15, 0, lr, c1, c1, 0 // only if it will return + ldmfd sp!, {lr} + subeqs pc, lr, #4 // No reschedule, returns. + + // Now the frame is created in the system stack, the IRQ + // and monitor stacks are empty, the state is secure. + msr CPSR_c, #MODE_SYS | I_BIT | F_BIT + stmfd sp!, {r0-r3, r12, lr} + msr CPSR_c, #MODE_MON | I_BIT | F_BIT + mrs r0, SPSR + mov r1, lr + msr CPSR_c, #MODE_SYS | I_BIT | F_BIT + stmfd sp!, {r0, r1} // Push R0=SPSR, R1=LR_IRQ. + + // Context switch. +#if CH_DBG_SYSTEM_STATE_CHECK + bl _dbg_check_lock +#endif + bl chSchDoReschedule +#if CH_DBG_SYSTEM_STATE_CHECK + bl _dbg_check_unlock +#endif + + // Re-establish the IRQ conditions again. + ldmfd sp!, {r0, r1} // Pop R0=SPSR, R1=LR_IRQ. + msr CPSR_c, #MODE_MON | I_BIT | F_BIT + msr SPSR_fsxc, r0 + mov lr, r1 + msr CPSR_c, #MODE_SYS | I_BIT | F_BIT + ldmfd sp!, {r0-r3, r12, lr} + msr CPSR_c, #MODE_MON | I_BIT | F_BIT + stmfd sp!, {lr} // save lr into monitor stack + ldr lr, =#MON_NS_SCR // set non-secure SCR before return + mrc p15, 0, lr, c1, c1, 0 + ldmfd sp!, {lr} + subs pc, lr, #4 // return into non-secure world +/* + * + */ Fiq_Handler: + // the fiq is taken locally from secure state + // current mode is fiq stmfd sp!, {r0-r3, r12, lr} ldr r0, =ARM_IRQ_VECTOR_REG ldr r0, [r0] -- cgit v1.2.3