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

1 files changed, 33 insertions, 28 deletions

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;