Fixed r12 (ip) saving.

Code cleanup.

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

1 files changed, 56 insertions, 62 deletions

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