From c17fbc9313e4dfdfa29824d6fc0b4844df811dad Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sun, 11 Dec 2011 16:20:32 +0000
Subject: CP4 FPU support apparently working.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3592 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/ports/GCC/ARMCMx/chcore_v7m.c | 34 ++++++++++++++++------------------
 os/ports/GCC/ARMCMx/chcore_v7m.h | 21 ++++++++++-----------
 2 files changed, 26 insertions(+), 29 deletions(-)

(limited to 'os/ports')

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
 
-- 
cgit v1.2.3