CP4 FPU support apparently working.

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

2 files changed, 26 insertions, 29 deletions

diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index aedaeea10..936823755 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -33,10 +33,10 @@
  */

 #if CORTEX_USE_FPU || defined(__DOXYGEN__)

 #define PUSH_CONTEXT() {                                                    \

-  asm volatile ("vpush   {s16-s31}"                                         \

-                : : : "memory");                                            \

   asm volatile ("push    {r4, r5, r6, r7, r8, r9, r10, r11, lr}"            \

                 : : : "memory");                                            \

+  asm volatile ("vpush   {s16-s31}"                                         \

+                : : : "memory");                                            \

 }

 #else /* !CORTEX_USE_FPU */

 #define PUSH_CONTEXT() {                                                    \

@@ -50,10 +50,10 @@
  */

 #if CORTEX_USE_FPU || defined(__DOXYGEN__)

 #define POP_CONTEXT() {                                                     \

-  asm volatile ("pop     {r4, r5, r6, r7, r8, r9, r10, r11, pc}"            \

-                : : : "memory");                                            \

   asm volatile ("vpop    {s16-s31}"                                         \

                 : : : "memory");                                            \

+  asm volatile ("pop     {r4, r5, r6, r7, r8, r9, r10, r11, pc}"            \

+                : : : "memory");                                            \

 }

 #else /* !CORTEX_USE_FPU */

 #define POP_CONTEXT() {                                                     \

@@ -99,21 +99,20 @@ CH_IRQ_HANDLER(SysTickVector) {
  */

 void SVCallVector(void) {

   uint32_t *psp;

-  register struct extctx *ctxp;

 

   /* Current PSP value.*/

   asm volatile ("mrs     %0, PSP" : "=r" (psp) : : "memory");

 

+  /* Discarding the current exception context and positioning the stack to

+     point to the real one.*/

+  psp = (uint32_t *)((struct extctx *)psp + 1);

+

 #if CORTEX_USE_FPU

   /* Restoring the special registers SCB_FPCCR and FPCAR.*/

   SCB_FPCAR = *psp++;

   SCB_FPCCR = *psp++;

 #endif

-

-  /* Discarding the current exception context and positioning the stack to

-     point to the real one.*/

-  ctxp = (struct extctx *)psp + 1;

-  asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");

+  asm volatile ("msr     PSP, %0" : : "r" (psp) : "memory");

   port_unlock_from_isr();

 }

 #endif /* !CORTEX_SIMPLIFIED_PRIORITY */

@@ -127,21 +126,20 @@ void SVCallVector(void) {
  */

 void PendSVVector(void) {

   uint32_t *psp;

-  register struct extctx *ctxp;

 

   /* Current PSP value.*/

   asm volatile ("mrs     %0, PSP" : "=r" (psp) : : "memory");

 

+  /* Discarding the current exception context and positioning the stack to

+     point to the real one.*/

+  psp = (uint32_t *)((struct extctx *)psp + 1);

+

 #if CORTEX_USE_FPU

   /* Restoring the special registers SCB_FPCCR and FPCAR.*/

   SCB_FPCAR = *psp++;

   SCB_FPCCR = *psp++;

 #endif

-

-  /* Discarding the current exception context and positioning the stack to

-     point to the real one.*/

-  ctxp = (struct extctx *)psp + 1;

-  asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");

+  asm volatile ("msr     PSP, %0" : : "r" (psp) : "memory");

 }

 #endif /* CORTEX_SIMPLIFIED_PRIORITY */

 

@@ -170,7 +168,7 @@ void _port_init(void) {
   SCB_FPDSCR = reg;

 

   /* Initializing the FPU context save in lazy mode.*/

-  SCB_FPCCR = FPCCR_LSPEN;

+  SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;

 #endif

 

   /* Initialization of the system vectors used by the port.*/

@@ -210,7 +208,7 @@ void _port_irq_epilogue(void) {
 

     /* Adding an artificial exception return context, there is no need to

        populate it fully.*/

-    ctxp = (struct extctx *)psp - 1;

+    ctxp = ((struct extctx *)psp) - 1;

     asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");

     ctxp->pc = _port_switch_from_isr;

     ctxp->xpsr = (regarm_t)0x01000000;

diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.h b/os/ports/GCC/ARMCMx/chcore_v7m.h
index 3cd2b9e18..8dd42f909 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.h
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.h
@@ -47,7 +47,7 @@
  * @details Activating this option will make the Kernel work in compact mode.

  */

 #if !defined(CORTEX_USE_FPU)

-#define CORTEX_USE_FPU                  FALSE/*CORTEX_HAS_FPU*/

+#define CORTEX_USE_FPU                  CORTEX_HAS_FPU

 #elif CORTEX_USE_FPU && !CORTEX_HAS_FPU

 /* This setting requires an FPU presence check in case it is externally

    redefined.*/

@@ -183,22 +183,12 @@ struct extctx {
   regarm_t      s13;

   regarm_t      s14;

   regarm_t      s15;

-  regarm_t      s16;

   regarm_t      fpscr;

   regarm_t      reserved;

 #endif /* CORTEX_USE_FPU */

 };

 

 struct intctx {

-  regarm_t      r4;

-  regarm_t      r5;

-  regarm_t      r6;

-  regarm_t      r7;

-  regarm_t      r8;

-  regarm_t      r9;

-  regarm_t      r10;

-  regarm_t      r11;

-  regarm_t      lr;

 #if CORTEX_USE_FPU || defined(__DOXYGEN__)

   regarm_t      s16;

   regarm_t      s17;

@@ -217,6 +207,15 @@ struct intctx {
   regarm_t      s30;

   regarm_t      s31;

 #endif /* CORTEX_USE_FPU */

+  regarm_t      r4;

+  regarm_t      r5;

+  regarm_t      r6;

+  regarm_t      r7;

+  regarm_t      r8;

+  regarm_t      r9;

+  regarm_t      r10;

+  regarm_t      r11;

+  regarm_t      lr;

 };

 #endif