Fix bugs in lazy FPU register saving

git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@4427 42af7a65-404d-4744-a932-0658087f49c3

8 files changed, 98 insertions, 117 deletions

diff --git a/apps/examples/ostest/fpu.c b/apps/examples/ostest/fpu.c
index ca61c0089..56cee45ee 100644
--- a/apps/examples/ostest/fpu.c
+++ b/apps/examples/ostest/fpu.c
@@ -43,6 +43,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <string.h>
 #include <sched.h>
 
 #include "ostest.h"
@@ -88,6 +89,11 @@
 #endif
 
 /* Other defintions ****************************************************/
+/* We'll keep all data using 32-bit values only to force 32-bit alignment.
+ * This logic has not real notion of the underlying representation.
+ */
+
+#define FPU_WORDSIZE ((CONFIG_EXAMPLES_OSTEST_FPUSIZE+3)>>2)
 
 #ifndef NULL
 # define NULL (void*)0
@@ -101,32 +107,18 @@
  * to be provided:
  */
 
-/* Given a uint8_t array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this
- * function will return the current FPU registers.
- */
-
-extern void arch_getfpu(FAR uint8_t *fpusave);
-
-/* Given a uint8_t array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this
- * function will set the current FPU regisers to match the provided
- * register save set.
- */
-
-extern void arch_setfpu(FAR const uint8_t *fpusave);
-
-/* Given a uint8_t array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE and a
- * seed value, this function will set the FPU registers to a known
- * values for testing purposes.  The contents of the FPU registers
- * must be uniqe for each sed value.
+/* Given an array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this function
+ * will return the current FPU registers.
  */
 
-extern void arch_initfpu(FAR uint8_t *fpusave, int seed);
+extern void arch_getfpu(FAR uint32_t *fpusave);
 
-/* Given two uint8_t arrays of size CONFIG_EXAMPLES_OSTEST_FPUSIZE this
+/* Given two arrays of size CONFIG_EXAMPLES_OSTEST_FPUSIZE this
  * function will compare then an return true if they are identical.
  */
 
-extern bool arch_cmpfpu(FAR const uint8_t *fpusave1, FAR const uint8_t *fpusave2);
+extern bool arch_cmpfpu(FAR const uint32_t *fpusave1,
+                        FAR const uint32_t *fpusave2);
 
 /***********************************************************************
  * Private Data
@@ -138,24 +130,25 @@ static uint8_t g_fpuno;
  * Private Functions
  ***********************************************************************/
 
-static void fpu_dump(FAR uint8_t *buffer, FAR const char *msg)
+static void fpu_dump(FAR uint32_t *buffer, FAR const char *msg)
 {
   int i, j, k;
 
   printf("%s (%p):\n", msg, buffer);
-  for (i = 0; i < CONFIG_EXAMPLES_OSTEST_FPUSIZE; i += 32)
+  for (i = 0; i < FPU_WORDSIZE; i += 8)
     {
-      printf("%04x: ", i);
-      for (j = 0; j < 32; j++)
+      printf("    %04x: ", i);
+      for (j = 0; j < 8; j++)
         {
           k = i + j;
 
-          if (k < CONFIG_EXAMPLES_OSTEST_FPUSIZE)
+          if (k < FPU_WORDSIZE)
             {
-              printf("%02x ", buffer[k]);
+              printf("%08x ", buffer[k]);
             }
           else
             {
+              printf("\n");
               break;
             }
         }
@@ -165,11 +158,13 @@ static void fpu_dump(FAR uint8_t *buffer, FAR const char *msg)
 
 static int fpu_task(int argc, char *argv[])
 {
-  uint8_t fpusave1[CONFIG_EXAMPLES_OSTEST_FPUSIZE];
-  uint8_t fpusave2[CONFIG_EXAMPLES_OSTEST_FPUSIZE];
+  uint32_t fpusave1[FPU_WORDSIZE];
+  uint32_t fpusave2[FPU_WORDSIZE];
+  double val1;
+  double val2;
+  double val3;
+  double val4;
   int id;
-  int seed;
-  int incr;
   int i;
 
   /* Which are we? */
@@ -178,30 +173,51 @@ static int fpu_task(int argc, char *argv[])
   id = (int)(++g_fpuno);
   sched_unlock();
 
-  seed = id << 24 | id << 16 | id << 8 | id;
-  incr = seed;
+  /* Seed the flowing point values */
+
+  val1 = (double)id;
 
   for (i = 0; i < CONFIG_EXAMPLES_OSTEST_FPULOOPS; i++)
     {
-      /* Initialize the FPU registers to a known value */
+      printf("FPU#%d: pass %d\n", id, i+1);
+      fflush(stdout);
+
+      /* Set the FPU register save arrays to a known-but-illogical values so 
+       * that we can verify that reading of the registers actually occurs.
+       */
+
+      memset(fpusave1, 0xff, sizeof(fpusave1));
+      memset(fpusave2, 0xff, sizeof(fpusave1));
+
+      /* Prevent context switches while we set up some stuff */
 
       sched_lock();
-      arch_initfpu(fpusave1, seed);
-      arch_setfpu(fpusave1);
 
-      /* Check that the FPU registers were set as we expected */
+      /* Do some trivial floating point operations that should cause some
+       * changes to floating point resters
+       */
+
+      val4 = 3.1415926 * val1;     /* Multiple by Pi */
+      val3 = val4 + 1.61803398874; /* Add the golden ratio */
+      val2 = val3 / 2.7182;        /* Divide by Euler's constant */
+      val1 = val2 + 1.0;           /* Plus one */
+ 
+      /* Sample the floating point registers */
+
+      arch_getfpu(fpusave1);
+
+      /* Re-read and verify the FPU registers consistently without corruption */
 
       arch_getfpu(fpusave2);
       if (!arch_cmpfpu(fpusave1, fpusave2))
         {
-          printf("ERROR FPU#%d: fpusave1 and fpusave2 do not match (BEFORE waiting)\n");
-          fpu_dump(fpusave1, "Values written:");
-          fpu_dump(fpusave2, "Values read:");
-          sched_unlock();
+          printf("ERROR FPU#%d: fpusave1 and fpusave2 do not match\n", id);
+          fpu_dump(fpusave1, "Values after math operations (fpusave1)");
+          fpu_dump(fpusave2, "Values after verify re-read (fpusave2)");
           return EXIT_FAILURE;
         }
 
-      /* Now unlock and sleep for a while */
+      /* Now unlock and sleep for a while -- this should result in some context switches */
 
       sched_unlock();
       usleep(CONFIG_EXAMPLES_OSTEST_FPUMSDELAY * 1000);
@@ -210,21 +226,18 @@ static int fpu_task(int argc, char *argv[])
        * point registers are still correctly set.
        */
 
-      sched_lock();
       arch_getfpu(fpusave2);
       if (!arch_cmpfpu(fpusave1, fpusave2))
         {
-          printf("ERROR FPU#%d: fpusave1 and fpusave2 do not match (AFTER waiting)\n");
-          fpu_dump(fpusave1, "Values written:");
-          fpu_dump(fpusave2, "Values read:");
-          sched_unlock();
+          printf("ERROR FPU#%d: fpusave1 and fpusave2 do not match\n", id);
+          fpu_dump(fpusave1, "Values before waiting (fpusave1)");
+          fpu_dump(fpusave2, "Values after waiting (fpusave2)");
           return EXIT_FAILURE;
         }
-
-      seed += incr;
     }
 
-  printf("FPU#%d: Succeeded\n");
+  printf("FPU#%d: Succeeded\n", id);
+  fflush(stdout);
   return EXIT_SUCCESS;
 }
 #endif /* HAVE_FPU */
@@ -253,6 +266,7 @@ void fpu_test(void)
     {
       printf("fpu_test: Started task FPU#1 at PID=%d\n", task1);
     }
+  fflush(stdout);
   usleep(250);
 
   printf("Starting task FPU#2\n");
@@ -268,6 +282,7 @@ void fpu_test(void)
 
   /* Wait for each task to complete */
 
+  fflush(stdout);
   (void)waitpid(task1, &statloc, 0);
   (void)waitpid(task2, &statloc, 0);
 
diff --git a/nuttx/arch/arm/src/armv7-m/up_blocktask.c b/nuttx/arch/arm/src/armv7-m/up_blocktask.c
index e2a612a18..896476ed2 100755
--- a/nuttx/arch/arm/src/armv7-m/up_blocktask.c
+++ b/nuttx/arch/arm/src/armv7-m/up_blocktask.c
@@ -1,8 +1,8 @@
 /****************************************************************************
  * arch/arm/src/armv7-m/up_blocktask.c
  *
- *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *   Copyright (C) 2007-2009, 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/nuttx/arch/arm/src/armv7-m/up_fpu.S b/nuttx/arch/arm/src/armv7-m/up_fpu.S
index fd6449c2f..707420f06 100644
--- a/nuttx/arch/arm/src/armv7-m/up_fpu.S
+++ b/nuttx/arch/arm/src/armv7-m/up_fpu.S
@@ -68,7 +68,7 @@
  ************************************************************************************/
 
 /************************************************************************************
- * Name: up_restorefpu
+ * Name: up_savefpu
  *
  * Description:
  *   Given the pointer to a register save area (in R0), save the state of the
diff --git a/nuttx/arch/arm/src/armv7-m/up_releasepending.c b/nuttx/arch/arm/src/armv7-m/up_releasepending.c
index 20b953543..2f0d4dc39 100755
--- a/nuttx/arch/arm/src/armv7-m/up_releasepending.c
+++ b/nuttx/arch/arm/src/armv7-m/up_releasepending.c
@@ -1,8 +1,8 @@
 /****************************************************************************
  *  arch/arm/src/armv7-m/up_releasepending.c
  *
- *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *   Copyright (C) 2007-2009, 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -84,15 +84,14 @@ void up_release_pending(void)
   /* sched_lock(); */
   if (sched_mergepending())
     {
-      /* The currently active task has changed!  We will need to
-       * switch contexts.  First check if we are operating in
-       * interrupt context:
+      /* The currently active task has changed!  We will need to switch
+       * contexts.  First check if we are operating in interrupt context.
        */
 
       if (current_regs)
         {
-          /* Yes, then we have to do things differently.
-           * Just copy the current_regs into the OLD rtcb.
+          /* Yes, then we have to do things differently. Just copy the
+           * current_regs into the OLD rtcb.
            */
 
            up_savestate(rtcb->xcp.regs);
diff --git a/nuttx/arch/arm/src/armv7-m/up_reprioritizertr.c b/nuttx/arch/arm/src/armv7-m/up_reprioritizertr.c
index 9ac2d1145..f1c961b15 100755
--- a/nuttx/arch/arm/src/armv7-m/up_reprioritizertr.c
+++ b/nuttx/arch/arm/src/armv7-m/up_reprioritizertr.c
@@ -1,8 +1,8 @@
 /****************************************************************************
  *  arch/arm/src/armv7-m/up_reprioritizertr.c
  *
- *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *   Copyright (C) 2007-2009, 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -103,8 +103,8 @@ void up_reprioritize_rtr(_TCB *tcb, uint8_t priority)
       slldbg("TCB=%p PRI=%d\n", tcb, priority);
 
       /* Remove the tcb task from the ready-to-run list.
-       * sched_removereadytorun will return true if we just
-       * remove the head of the ready to run list.
+       * sched_removereadytorun will return true if we just removed the head
+       * of the ready to run list.
        */
 
       switch_needed = sched_removereadytorun(tcb);
@@ -113,17 +113,18 @@ void up_reprioritize_rtr(_TCB *tcb, uint8_t priority)
 
       tcb->sched_priority = (uint8_t)priority;
 
-      /* Return the task to the specified blocked task list.
-       * sched_addreadytorun will return true if the task was
-       * added to the new list.  We will need to perform a context
-       * switch only if the EXCLUSIVE or of the two calls is non-zero
-       * (i.e., one and only one the calls changes the head of the
-       * ready-to-run list).
+      /* Return the task to the ready-to-run task list. sched_addreadytorun
+       * will return true if the task was added to the head of ready-to-run
+       * list.  We will need to perform a context switch only if the
+       * EXCLUSIVE or of the two calls is non-zero (i.e., one and only one
+       * the calls changes the head of the ready-to-run list).
        */
 
       switch_needed ^= sched_addreadytorun(tcb);
 
-      /* Now, perform the context switch if one is needed */
+      /* Now, perform the context switch if one is needed (i.e. if the head
+       * of the ready-to-run list is no longer the same).
+       */
 
       if (switch_needed)
         {
diff --git a/nuttx/arch/arm/src/armv7-m/up_schedulesigaction.c b/nuttx/arch/arm/src/armv7-m/up_schedulesigaction.c
index f392a08d7..9e6dbd14b 100644
--- a/nuttx/arch/arm/src/armv7-m/up_schedulesigaction.c
+++ b/nuttx/arch/arm/src/armv7-m/up_schedulesigaction.c
@@ -1,8 +1,8 @@
 /****************************************************************************
  * arch/arm/src/armv7-m/up_schedulesigaction.c
  *
- *   Copyright (C) 2009-2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *   Copyright (C) 2009-2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/nuttx/arch/arm/src/armv7-m/up_svcall.c b/nuttx/arch/arm/src/armv7-m/up_svcall.c
index 949efb1af..5a4d64fe2 100644
--- a/nuttx/arch/arm/src/armv7-m/up_svcall.c
+++ b/nuttx/arch/arm/src/armv7-m/up_svcall.c
@@ -320,6 +320,9 @@ int up_svcall(int irq, FAR void *context)
         {
           DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
           memcpy((uint32_t*)regs[REG_R1], regs, XCPTCONTEXT_SIZE);
+#if defined(CONFIG_ARCH_FPU) && !defined(CONFIG_ARMV7M_CMNVECTOR)
+          up_savefpu((uint32_t*)regs[REG_R1]);
+#endif
           current_regs = (uint32_t*)regs[REG_R2];
         }
         break;
diff --git a/nuttx/configs/stm3240g-eval/src/up_ostest.c b/nuttx/configs/stm3240g-eval/src/up_ostest.c
index 477b67caf..a30999d09 100644
--- a/nuttx/configs/stm3240g-eval/src/up_ostest.c
+++ b/nuttx/configs/stm3240g-eval/src/up_ostest.c
@@ -77,66 +77,29 @@
 /************************************************************************************
  * Public Functions
  ************************************************************************************/
-/* Given a uint8_t array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this
- * function will return the current FPU registers.
+/* Given an array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this function will return
+ * the current FPU registers.
  */
 
-void arch_getfpu(FAR uint8_t *fpusave)
+void arch_getfpu(FAR uint32_t *fpusave)
 {
   irqstate_t flags;
   uint32_t regs[XCPTCONTEXT_REGS];
 
   flags = irqsave();
-  up_savefpu(regs);
+  up_savefpu(regs);  /* Saves the context of the FPU registers to memory */
   irqrestore(flags);
 
   memcpy(fpusave, &regs[REG_S0], (4*SW_FPU_REGS));
 }
 
-/* Given a uint8_t array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this
- * function will set the current FPU regisers to match the provided
- * register save set.
+/* Given two arrays of size CONFIG_EXAMPLES_OSTEST_FPUSIZE this function
+ * will compare then an return true if they are identical.
  */
 
-void arch_setfpu(FAR const uint8_t *fpusave)
+bool arch_cmpfpu(FAR const uint32_t *fpusave1, FAR const uint32_t *fpusave2)
 {
-  irqstate_t flags;
-  uint32_t regs[XCPTCONTEXT_REGS];
-
-  memcpy(&regs[REG_S0], fpusave, (4*SW_FPU_REGS));
-
-  flags = irqsave();
-  up_restorefpu(regs);
-  irqrestore(flags);
-}
-
-/* Given a uint8_t array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE and a
- * seed value, this function will set the FPU registers to a known
- * values for testing purposes.  The contents of the FPU registers
- * must be uniqe for each sed value.
- */
-
-void arch_initfpu(FAR uint8_t *fpusave, int seed)
-{
-  FAR uint32_t *dest = (FAR uint32_t *)fpusave;
-  uint32_t mask = 0x01010101;
-  uint32_t incr = 0x01010101;
-  int i;
-
-  for (i = 0; i < 32; i++)
-    {
-      *dest = (uint32_t)seed ^ mask;
-      mask += incr;
-    }
-}
-
-/* Given two uint8_t arrays of size CONFIG_EXAMPLES_OSTEST_FPUSIZE this
- * function will compare then an return true if they are identical.
- */
-
-bool arch_cmpfpu(FAR const uint8_t *fpusave1, FAR const uint8_t *fpusave2)
-{
-  return memcmp(fpusave1, fpusave2, (4*32)) == 0;
+  return memcmp(fpusave1, fpusave2, (4*SW_FPU_REGS)) == 0;
 }
 
 #endif /* HAVE_FPU */