Moved monitor code in monitor.S

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

1 files changed, 7 insertions, 139 deletions

diff --git a/os/common/ports/ARMCAx-TZ/compilers/GCC/chcoreasm.S b/os/common/ports/ARMCAx-TZ/compilers/GCC/chcoreasm.S
index ecb06a8c2..58b83b9b5 100644
--- a/os/common/ports/ARMCAx-TZ/compilers/GCC/chcoreasm.S
+++ b/os/common/ports/ARMCAx-TZ/compilers/GCC/chcoreasm.S
@@ -55,17 +55,8 @@
                 .set    MODE_UND, 0x1B

                 .set    MODE_SYS, 0x1F

 

-                .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

-

-                .equ    MON_S_SCR,  SCR_IRQ

-                .equ    MON_NS_SCR, SCR_FIQ|SCR_NS

+                .set    I_BIT,      0x80

+                .set    F_BIT,      0x40

 

                 .text

 

@@ -94,10 +85,10 @@ _port_switch_arm:
  *      |     r2     |  | External context: IRQ handler frame

  *      |     r1     |  |

  *      |     r0     |  |

- *      |   LR_IRQ   |  |   (user code return address)(could be in non-secure space)

+ *      |   LR_FIQ   |  |   (user code return address)

  *      |   PSR_USR  | -+   (user code status)

  *      |    ....    | <- chSchDoReschedule() stack frame, optimize it for space

- *      |     LR     | -+   (system code return address)(always in secure space)

+ *      |     LR     | -+   (system code return address)

  *      |     r11    |  |

  *      |     r10    |  |

  *      |     r9     |  |

@@ -109,132 +100,9 @@ _port_switch_arm:
  * Low  +------------+

  *

  */

-

-/*

- * We are facing an architecure with security extension exploited.

- * The following two monitor execution paths are taken by the execution units

- * running in secure state when an irq is fired (Mon_Irq_Handler), and in non-secure

- * state when a fiq interrupt is fired (Mon_Fiq_Handler).

- * They originate by the monitor irq/fiq vector and run 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, IRQs are taken locally

- *   SCR.FW  == 0 and SCR.FIQ == 0 and SCR.IRQ == 1 in secure state,

- *      ie FIQs are taken locally, IRQs are taken to monitor mode

- *   MVBAR holds the address of the monitor vectors base.

- *   The code and the stacks memory reside both in secure memory.

- */

                 .balign 16

                 .code   32

-                .global Mon_Fiq_Handler

-                .global Mon_Irq_Handler

                 .global Fiq_Handler

-Mon_Irq_Handler:

-                // 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.

-                //

-                // This procedure is challenging, because the irq must be

-                // executed in the non secure context and must return in this

-                // secure context. The non secure IRQ handler has

-                // the responsibility to return into secure state via a smc.

-                //

-                // The thread is interrupted in the same way that local FIQ.

-                // The frame is created in the system stack of the current thread

-                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.

-

-                /* Save the s_ctx e recover the ns_ctx */

-

-                // Re-establish the original conditions

-                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

-                                                // and serve the IRQ

-/*

- *

- */

-Mon_Fiq_Handler:

-                // Here the fiq is taken from non-secure state, via the FIQ vector

-                // that is in the mon vector table.

-                // Current mode is monitor (so current state is secure).

-                // Note also that we never leave secure state while sec FIQ was disabled,

-                // then it's always safe to process the FIQ here.

-                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, save scratch registers

-                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

-                // relative to non secure context,

-                // 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}

-                mov		r0, sp

-                msr     CPSR_c, #MODE_MON | I_BIT | F_BIT

-                stmfd	sp!, {r0}				// address of ns_ctx in mon stack

-                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.

-                mov		r0, sp

-

-                // 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

-                ldmfd	sp!, {r0}				// ns_ctx no longer exists, cleanup mon stack

-                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 mon 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

@@ -256,7 +124,7 @@ _fiq_ret_arm:
                 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.

+                stmfd   sp!, {r0, r1}           // Push R0=SPSR, R1=LR_FIQ.

 

                 // Context switch.

 #if CH_DBG_SYSTEM_STATE_CHECK

@@ -268,7 +136,7 @@ _fiq_ret_arm:
 #endif

 

                 // Re-establish the IRQ conditions again.

-                ldmfd   sp!, {r0, r1}           // Pop R0=SPSR, R1=LR_IRQ.

+                ldmfd   sp!, {r0, r1}           // Pop R0=SPSR, R1=LR_FIQ.

                 msr     CPSR_c, #MODE_FIQ | I_BIT | F_BIT

                 msr     SPSR_fsxc, r0

                 mov     lr, r1

@@ -288,7 +156,7 @@ _port_thread_start:
 #if CH_DBG_SYSTEM_STATE_CHECK

                 bl      _dbg_check_unlock

 #endif

-				msr     CPSR_c, #MODE_SYS | I_BIT

+				msr     CPSR_c, #MODE_SYS

                 mov     r0, r5

                 mov     lr, pc

                 bx      r4