From 014655f97bedfce6a2166e01755c3280fb7ab387 Mon Sep 17 00:00:00 2001 From: isiora Date: Wed, 24 Jan 2018 06:54:36 +0000 Subject: Added saving of banked register. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11396 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S | 125 +++++++++++++++++++++- 1 file changed, 120 insertions(+), 5 deletions(-) diff --git a/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S b/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S index 63acd6e34..66d3021f0 100644 --- a/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S +++ b/os/common/ports/ARMCAx-TZ/compilers/GCC/monitor.S @@ -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__) */ -- cgit v1.2.3