From 8172ebaa5bdc90d010ccd07dd4b4ed18af5be0f7 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sat, 17 Dec 2011 19:25:30 +0000
Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3629
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/ports/GCC/ARMCMx/chcore_v7m.c | 61 ++++++++++++++++++++++------------------
 1 file changed, 33 insertions(+), 28 deletions(-)

(limited to 'os')

diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index 71d6faf9b..04f9ab7e8 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -111,28 +111,31 @@ void PendSVVector(void) {
  * @brief   Port-related initialization code.
  */
 void _port_init(void) {
-  uint32_t reg;
 
   /* Initialization of the vector table and priority related settings.*/
   SCB_VTOR = CORTEX_VTOR_INIT;
   SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(0);
 
 #if CORTEX_USE_FPU
-  /* CP10 and CP11 set to full access.*/
-  SCB_CPACR |= 0x00F00000;
+  {
+    uint32_t reg;
 
-  /* Enables FPU context save/restore on exception entry/exit (FPCA bit).*/
-  asm volatile ("mrs     %0, CONTROL" : "=r" (reg) : : "memory");
-  reg |= 4;
-  asm volatile ("msr     CONTROL, %0" : : "r" (reg) : "memory");
+    /* CP10 and CP11 set to full access.*/
+    SCB_CPACR |= 0x00F00000;
 
-  /* FPSCR and FPDSCR initially zero.*/
-  reg = 0;
-  asm volatile ("vmsr    FPSCR, %0" : : "r" (reg) : "memory");
-  SCB_FPDSCR = reg;
+    /* Enables FPU context save/restore on exception entry/exit (FPCA bit).*/
+    asm volatile ("mrs     %0, CONTROL" : "=r" (reg) : : "memory");
+    reg |= 4;
+    asm volatile ("msr     CONTROL, %0" : : "r" (reg) : "memory");
 
-  /* Initializing the FPU context save in lazy mode.*/
-  SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;
+    /* FPSCR and FPDSCR initially zero.*/
+    reg = 0;
+    asm volatile ("vmsr    FPSCR, %0" : : "r" (reg) : "memory");
+    SCB_FPDSCR = reg;
+
+    /* Initializing the FPU context save in lazy mode.*/
+    SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;
+  }
 #endif
 
   /* Initialization of the system vectors used by the port.*/
@@ -155,19 +158,6 @@ void _port_irq_epilogue(void) {
     /* Current PSP value.*/
     asm volatile ("mrs     %0, PSP" : "=r" (ctxp) : : "memory");
 
-#if CORTEX_USE_FPU
-    {
-      uint32_t fpccr;
-
-      /* Saving the special register SCB_FPCCR.*/
-      ctxp->fpccr = (regarm_t)(fpccr = SCB_FPCCR);
-
-      /* Now the FPCCR is modified in order to not restore the FPU status
-         from the artificial return context.*/
-      SCB_FPCCR = fpccr | FPCCR_LSPACT;
-    }
-#endif
-
     /* Adding an artificial exception return context, there is no need to
        populate it fully.*/
     ctxp--;
@@ -177,10 +167,12 @@ void _port_irq_epilogue(void) {
     /* The exit sequence is different depending on if a preemption is
        required or not.*/
     if (chSchIsPreemptionRequired()) {
-      /* Preemption is required we need to trigger a lazy FPU state save
-         and enforce a context switch.*/
+      /* Preemption is required we need to enforce a context switch.*/
       ctxp->pc = _port_switch_from_isr;
+#if CORTEX_USE_FPU
+      /* Triggering a lazy FPU state save.*/
       asm volatile ("vmrs    APSR_nzcv, FPSCR" : : : "memory");
+#endif
     }
     else {
       /* Preemption not required, we just need to exit the exception
@@ -189,6 +181,19 @@ void _port_irq_epilogue(void) {
       ctxp->pc = _port_exit_from_isr;
     }
 
+#if CORTEX_USE_FPU
+    {
+      uint32_t fpccr;
+
+      /* Saving the special register SCB_FPCCR.*/
+      ctxp->fpccr = (regarm_t)(fpccr = SCB_FPCCR);
+
+      /* Now the FPCCR is modified in order to not restore the FPU status
+         from the artificial return context.*/
+      SCB_FPCCR = fpccr | FPCCR_LSPACT;
+    }
+#endif
+
     /* Note, returning without unlocking is intentional, this is done in
        order to keep the rest of the context switching atomic.*/
     return;
-- 
cgit v1.2.3