Added saving of banked register.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11396 35acf78f-673a-0410-8e92-d51de3d6d3f4

1 files 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,11 +73,62 @@
                 .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
  */
 				.global _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__) */