From 6100dc08a6a9f4592f324204bad26ba11239a030 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Tue, 20 Dec 2011 18:51:28 +0000
Subject: Cortex-M4 FPU support working.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3644 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/ports/GCC/ARMCMx/chcore_v7m.c       |  2 ++
 readme.txt                             |  3 +-
 testhal/STM32F4xx/IRQ_STORM_FPU/main.c | 55 +++++++++++++++++++++-------------
 3 files changed, 38 insertions(+), 22 deletions(-)

diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index e4ea3a72e..ca45f4f1a 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -76,6 +76,7 @@ void SVCallVector(void) {
 #if CORTEX_USE_FPU
   /* Restoring the special register SCB_FPCCR.*/
   SCB_FPCCR = (uint32_t)ctxp->fpccr;
+  SCB_FPCAR = SCB_FPCAR + sizeof (struct extctx);
 #endif
   asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");
   port_unlock_from_isr();
@@ -102,6 +103,7 @@ void PendSVVector(void) {
 #if CORTEX_USE_FPU
   /* Restoring the special register SCB_FPCCR.*/
   SCB_FPCCR = (uint32_t)ctxp->fpccr;
+  SCB_FPCAR = SCB_FPCAR + sizeof (struct extctx);
 #endif
   asm volatile ("msr     PSP, %0" : : "r" (ctxp) : "memory");
 }
diff --git a/readme.txt b/readme.txt
index 060f1c022..a6912b249 100644
--- a/readme.txt
+++ b/readme.txt
@@ -81,7 +81,8 @@
   structures and stacks in the CCM RAM instead normal RAM. It is done using
   a special .ld file that can be customized to decide how to allocate data
   in the various RAM sections.
-- NEW: Added support for the Cortex-M4 FPU (default when the FPU is present).
+- NEW: Added experimental support for the Cortex-M4 FPU (default when the
+  FPU is present).
 - NEW: Improved I2C driver model and STM32 implementation by Barthess.
 
 *** 2.3.4 ***
diff --git a/testhal/STM32F4xx/IRQ_STORM_FPU/main.c b/testhal/STM32F4xx/IRQ_STORM_FPU/main.c
index 73c98bf6f..a41c931e1 100644
--- a/testhal/STM32F4xx/IRQ_STORM_FPU/main.c
+++ b/testhal/STM32F4xx/IRQ_STORM_FPU/main.c
@@ -39,10 +39,6 @@ float ff2(float par1, float par2, float par3, float par4);
 #define ITERATIONS      100
 #endif
 
-#ifndef NUM_THREADS
-#define NUM_THREADS     4
-#endif
-
 /*===========================================================================*/
 /* Test related code.                                                        */
 /*===========================================================================*/
@@ -50,9 +46,9 @@ float ff2(float par1, float par2, float par3, float par4);
 static bool_t saturated;
 
 /*
- * Test worker threads.
+ * Test worker thread.
  */
-static WORKING_AREA(waWorkerThread[NUM_THREADS], 128);
+static WORKING_AREA(waWorkerThread, 128);
 static msg_t WorkerThread(void *arg) {
 
   (void)arg;
@@ -71,11 +67,34 @@ static msg_t WorkerThread(void *arg) {
   }
 }
 
+/*
+ * Test periodic thread.
+ */
+static WORKING_AREA(waPeriodicThread, 128);
+static msg_t PeriodicThread(void *arg) {
+
+  (void)arg;
+
+  while(1) {
+    float f1, f2, f3, f4, f5;
+
+    f1 = ff1(4);
+    f2 = ff1(5);
+    f3 = ff1(6);
+    f5 = f1 + f2 + f3;
+    f4 = ff1(7);
+    f5 = ff2(f5, f4, f5, f4);
+    if (f5 != 484)
+      chSysHalt();
+    chThdSleepSeconds(1);
+  }
+}
+
 /*
  * GPT2 callback.
  */
 static void gpt2cb(GPTDriver *gptp) {
-/*  float f1, f2, f3, f4, f5;
+  float f1, f2, f3, f4, f5;
 
   (void)gptp;
 
@@ -86,14 +105,14 @@ static void gpt2cb(GPTDriver *gptp) {
   f4 = ff1(5);
   f5 = ff2(f5, f4, f5, f4);
   if (f5 != 196)
-    chSysHalt();*/
+    chSysHalt();
 }
 
 /*
  * GPT3 callback.
  */
 static void gpt3cb(GPTDriver *gptp) {
-/*  float f1, f2, f3, f4, f5;
+  float f1, f2, f3, f4, f5;
 
   (void)gptp;
 
@@ -104,8 +123,7 @@ static void gpt3cb(GPTDriver *gptp) {
   f4 = ff1(4);
   f5 = ff2(f5, f4, f5, f4);
   if (f5 != 100)
-    chSysHalt();*/
-  volatile float f1 = ff1(1);
+    chSysHalt();
 }
 
 /*
@@ -185,14 +203,12 @@ int main(void) {
   gptStart(&GPTD3, &gpt3cfg);
 
   /*
-   * Initializes the mailboxes and creates the worker threads.
+   * Initializes the worker threads.
    */
-/*  for (i = 0; i < NUM_THREADS; i++) {
-    chThdCreateStatic(waWorkerThread[i], sizeof waWorkerThread[i],
-                      NORMALPRIO - 20, WorkerThread, (void *)i);
-  }*/
-  chThdCreateStatic(waWorkerThread[0], sizeof waWorkerThread[0],
-                    NORMALPRIO - 20, WorkerThread, (void *)0);
+  chThdCreateStatic(waWorkerThread, sizeof waWorkerThread,
+                    NORMALPRIO - 20, WorkerThread, NULL);
+  chThdCreateStatic(waPeriodicThread, sizeof waPeriodicThread,
+                    NORMALPRIO - 10, PeriodicThread, NULL);
 
   /*
    * Test procedure.
@@ -236,9 +252,6 @@ int main(void) {
   print("*** Randomize:    ");
   printn(RANDOMIZE);
   println("");
-  print("*** Threads:      ");
-  printn(NUM_THREADS);
-  println("");
 
   println("");
   worst = 0;
-- 
cgit v1.2.3