Improve some ARMv7-A/M floating point register save time; Add floating point register save logic for ARMv7-A

39 files changed, 693 insertions, 147 deletions

diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog
index 16b409c7b..1a1c0fbd1 100644
--- a/nuttx/ChangeLog
+++ b/nuttx/ChangeLog
@@ -5168,4 +5168,12 @@
 	* arch/arm/src/armv7-a/arm_vectors.S:  Force 8-byte stack alignment
 	  in interrupt handlers before calling C code.  Other ARM
 	  architectures need to do this as well (2013-7-23).
+	* arm/src/armv7-m/up_copyarmstate.c and armv7-a/up_copyarmstate.c:
+	  Added a new form of the register copy function that should save quit a
+	  bit of time for armv7-m (without common vectors) and with armv7-a
+	  (2013-7-23).
+	* arch/arm/src/armv7-a/arm_restorefpu.S, arm_savefpu.S, arm_doirq.c,
+	  arm_fullcontextrestore.S, arm_saveusercontext.S:  Add hardware
+	  floating point register save/restore logic for the Cortex-A5\
+	  (2013-7-23).
 
diff --git a/nuttx/arch/arm/include/armv7-a/irq.h b/nuttx/arch/arm/include/armv7-a/irq.h
index 761d46e8d..6aeba431e 100755
--- a/nuttx/arch/arm/include/armv7-a/irq.h
+++ b/nuttx/arch/arm/include/armv7-a/irq.h
@@ -60,9 +60,8 @@
  *   (1) stmia rx, {r0-r14}
  *   (2) then the PC and CPSR
  *
- * This results in the following set of indices that
- * can be used to access individual registers in the
- * xcp.regs array:
+ * This results in the following set of indices that can be used to access 
+ * individual registers in the xcp.regs array:
  */
 
 #define REG_R0              (0)
@@ -83,9 +82,85 @@
 #define REG_R15             (15)
 #define REG_CPSR            (16)
 
-#define XCPTCONTEXT_REGS    (17)
+#define ARM_CONTEXT_REGS    (17)
+
+/* If the MCU supports a floating point unit, then it will be necessary
+ * to save the state of the FPU status register and data registers on
+ * each context switch.  These registers are not saved during interrupt
+ * level processing, however. So, as a consequence, floating point
+ * operations may NOT be performed in interrupt handlers.
+ *
+ * The FPU provides an extension register file containing 32 single-
+ * precision registers. These can be viewed as:
+ *
+ * - Sixteen 64-bit doubleword registers, D0-D15
+ * - Thirty-two 32-bit single-word registers, S0-S31
+ *   S<2n> maps to the least significant half of D<n>
+ *   S<2n+1> maps to the most significant half of D<n>.
+ */
+
+#ifdef CONFIG_ARCH_FPU
+#  define REG_D0            (ARM_CONTEXT_REGS+0)  /* D0 */
+#  define REG_S0            (ARM_CONTEXT_REGS+0)  /* S0 */
+#  define REG_S1            (ARM_CONTEXT_REGS+1)  /* S1 */
+#  define REG_D1            (ARM_CONTEXT_REGS+2)  /* D1 */
+#  define REG_S2            (ARM_CONTEXT_REGS+2)  /* S2 */
+#  define REG_S3            (ARM_CONTEXT_REGS+3)  /* S3 */
+#  define REG_D2            (ARM_CONTEXT_REGS+4)  /* D2 */
+#  define REG_S4            (ARM_CONTEXT_REGS+4)  /* S4 */
+#  define REG_S5            (ARM_CONTEXT_REGS+5)  /* S5 */
+#  define REG_D3            (ARM_CONTEXT_REGS+6)  /* D3 */
+#  define REG_S6            (ARM_CONTEXT_REGS+6)  /* S6 */
+#  define REG_S7            (ARM_CONTEXT_REGS+7)  /* S7 */
+#  define REG_D4            (ARM_CONTEXT_REGS+8)  /* D4 */
+#  define REG_S8            (ARM_CONTEXT_REGS+8)  /* S8 */
+#  define REG_S9            (ARM_CONTEXT_REGS+9)  /* S9 */
+#  define REG_D5            (ARM_CONTEXT_REGS+10) /* D5 */
+#  define REG_S10           (ARM_CONTEXT_REGS+10) /* S10 */
+#  define REG_S11           (ARM_CONTEXT_REGS+11) /* S11 */
+#  define REG_D6            (ARM_CONTEXT_REGS+12) /* D6 */
+#  define REG_S12           (ARM_CONTEXT_REGS+12) /* S12 */
+#  define REG_S13           (ARM_CONTEXT_REGS+13) /* S13 */
+#  define REG_D7            (ARM_CONTEXT_REGS+14) /* D7 */
+#  define REG_S14           (ARM_CONTEXT_REGS+14) /* S14 */
+#  define REG_S15           (ARM_CONTEXT_REGS+15) /* S15 */
+#  define REG_D8            (ARM_CONTEXT_REGS+16) /* D8 */
+#  define REG_S16           (ARM_CONTEXT_REGS+16) /* S16 */
+#  define REG_S17           (ARM_CONTEXT_REGS+17) /* S17 */
+#  define REG_D9            (ARM_CONTEXT_REGS+18) /* D9 */
+#  define REG_S18           (ARM_CONTEXT_REGS+18) /* S18 */
+#  define REG_S19           (ARM_CONTEXT_REGS+19) /* S19 */
+#  define REG_D10           (ARM_CONTEXT_REGS+20) /* D10 */
+#  define REG_S20           (ARM_CONTEXT_REGS+20) /* S20 */
+#  define REG_S21           (ARM_CONTEXT_REGS+21) /* S21 */
+#  define REG_D11           (ARM_CONTEXT_REGS+22) /* D11 */
+#  define REG_S22           (ARM_CONTEXT_REGS+22) /* S22 */
+#  define REG_S23           (ARM_CONTEXT_REGS+23) /* S23 */
+#  define REG_D12           (ARM_CONTEXT_REGS+24) /* D12 */
+#  define REG_S24           (ARM_CONTEXT_REGS+24) /* S24 */
+#  define REG_S25           (ARM_CONTEXT_REGS+25) /* S25 */
+#  define REG_D13           (ARM_CONTEXT_REGS+26) /* D13 */
+#  define REG_S26           (ARM_CONTEXT_REGS+26) /* S26 */
+#  define REG_S27           (ARM_CONTEXT_REGS+27) /* S27 */
+#  define REG_D14           (ARM_CONTEXT_REGS+28) /* D14 */
+#  define REG_S28           (ARM_CONTEXT_REGS+28) /* S28 */
+#  define REG_S29           (ARM_CONTEXT_REGS+29) /* S29 */
+#  define REG_D15           (ARM_CONTEXT_REGS+30) /* D15 */
+#  define REG_S30           (ARM_CONTEXT_REGS+30) /* S30 */
+#  define REG_S31           (ARM_CONTEXT_REGS+31) /* S31 */
+#  define REG_FPSCR         (ARM_CONTEXT_REGS+32) /* Floating point status and control */
+#  define FPU_CONTEXT_REGS  (33)
+#else
+#  define FPU_CONTEXT_REGS  (0)
+#endif
+
+/* The total number of registers saved by software */
+
+#define XCPTCONTEXT_REGS    (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
 #define XCPTCONTEXT_SIZE    (4 * XCPTCONTEXT_REGS)
 
+/* Friendly register names */
+
 #define REG_A1              REG_R0
 #define REG_A2              REG_R1
 #define REG_A3              REG_R2
diff --git a/nuttx/arch/arm/src/arm/up_copystate.c b/nuttx/arch/arm/src/arm/up_copyfullstate.c
index fdc7246d9..3dffb8518 100644
--- a/nuttx/arch/arm/src/arm/up_copystate.c
+++ b/nuttx/arch/arm/src/arm/up_copyfullstate.c
@@ -1,7 +1,7 @@
 /****************************************************************************
- * arch/arm/src/arm/up_copystate.c
+ * arch/arm/src/arm/up_copyfullstate.c
  *
- *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007-2009, 2013 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -61,12 +61,12 @@
  ****************************************************************************/
 
 /****************************************************************************
- * Name: up_copystate
+ * Name: up_copyfullstate
  ****************************************************************************/
 
 /* A little faster than most memcpy's */
 
-void up_copystate(uint32_t *dest, uint32_t *src)
+void up_copyfullstate(uint32_t *dest, uint32_t *src)
 {
   int i;
 
diff --git a/nuttx/arch/arm/src/arm/up_doirq.c b/nuttx/arch/arm/src/arm/up_doirq.c
index 3b47a56b0..52355d43a 100644
--- a/nuttx/arch/arm/src/arm/up_doirq.c
+++ b/nuttx/arch/arm/src/arm/up_doirq.c
@@ -80,7 +80,7 @@ void up_doirq(int irq, uint32_t *regs)
 
   /* Nested interrupts are not supported in this implementation.  If you want
    * to implement nested interrupts, you would have to (1) change the way that
-   * current_regs is handled and (2) the design associated with
+   * current_regs is handled and (2) implement design changes associated with
    * CONFIG_ARCH_INTERRUPTSTACK.  The savestate variable will not work for
    * that purpose as implemented here because only the outermost nested
    * interrupt can result in a context switch (it can probably be deleted).
diff --git a/nuttx/arch/arm/src/arm/up_sigdeliver.c b/nuttx/arch/arm/src/arm/up_sigdeliver.c
index 9459bf7e9..be45b75d5 100644
--- a/nuttx/arch/arm/src/arm/up_sigdeliver.c
+++ b/nuttx/arch/arm/src/arm/up_sigdeliver.c
@@ -100,7 +100,7 @@ void up_sigdeliver(void)
 
   /* Save the real return state on the stack. */
 
-  up_copystate(regs, rtcb->xcp.regs);
+  up_copyfullstate(regs, rtcb->xcp.regs);
   regs[REG_PC]         = rtcb->xcp.saved_pc;
   regs[REG_CPSR]       = rtcb->xcp.saved_cpsr;
 
diff --git a/nuttx/arch/arm/src/armv6-m/up_copystate.c b/nuttx/arch/arm/src/armv6-m/up_copyfullstate.c
index 87df1190a..0747de452 100644
--- a/nuttx/arch/arm/src/armv6-m/up_copystate.c
+++ b/nuttx/arch/arm/src/armv6-m/up_copyfullstate.c
@@ -1,5 +1,5 @@
 /****************************************************************************
- * arch/arm/src/armv6-m/up_copystate.c
+ * arch/arm/src/armv6-m/up_copyfullstate.c
  *
  *   Copyright (C) 2013 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
@@ -61,12 +61,12 @@
  ****************************************************************************/
 
 /****************************************************************************
- * Name: up_copystate
+ * Name: up_copyfullstate
  ****************************************************************************/
 
 /* A little faster than most memcpy's */
 
-void up_copystate(uint32_t *dest, uint32_t *src)
+void up_copyfullstate(uint32_t *dest, uint32_t *src)
 {
   int i;
 
diff --git a/nuttx/arch/arm/src/armv6-m/up_sigdeliver.c b/nuttx/arch/arm/src/armv6-m/up_sigdeliver.c
index e688fa96f..5a7101cf2 100644
--- a/nuttx/arch/arm/src/armv6-m/up_sigdeliver.c
+++ b/nuttx/arch/arm/src/armv6-m/up_sigdeliver.c
@@ -105,7 +105,7 @@ void up_sigdeliver(void)
 
   /* Save the real return state on the stack. */
 
-  up_copystate(regs, rtcb->xcp.regs);
+  up_copyfullstate(regs, rtcb->xcp.regs);
   regs[REG_PC]         = rtcb->xcp.saved_pc;
   regs[REG_PRIMASK]    = rtcb->xcp.saved_primask;
   regs[REG_XPSR]       = rtcb->xcp.saved_xpsr;
diff --git a/nuttx/arch/arm/src/armv7-m/up_copystate.c b/nuttx/arch/arm/src/armv7-a/arm_copyarmstate.c
index e9eede8f9..219d01a1e 100644
--- a/nuttx/arch/arm/src/armv7-m/up_copystate.c
+++ b/nuttx/arch/arm/src/armv7-a/arm_copyarmstate.c
@@ -1,7 +1,7 @@
 /****************************************************************************
- * arch/arm/src/armv7-m/up_copystate.c
+ * arch/arm/src/armv7-a/arm_copyarmstate.c
  *
- *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2013 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -44,6 +44,8 @@
 #include "os_internal.h"
 #include "up_internal.h"
 
+#ifdef CONFIG_ARCH_FPU
+
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
@@ -61,12 +63,12 @@
  ****************************************************************************/
 
 /****************************************************************************
- * Name: up_copystate
+ * Name: up_copyarmstate
  ****************************************************************************/
 
 /* A little faster than most memcpy's */
 
-void up_copystate(uint32_t *dest, uint32_t *src)
+void up_copyarmstate(uint32_t *dest, uint32_t *src)
 {
   int i;
 
@@ -77,10 +79,11 @@ void up_copystate(uint32_t *dest, uint32_t *src)
 
   if (src != dest)
     {
-      for (i = 0; i < XCPTCONTEXT_REGS; i++)
+      for (i = 0; i < ARM_CONTEXT_REGS; i++)
         {
           *dest++ = *src++;
         }
     }
 }
 
+#endif /* CONFIG_ARCH_FPU */
diff --git a/nuttx/arch/arm/src/armv7-a/arm_copystate.c b/nuttx/arch/arm/src/armv7-a/arm_copyfullstate.c
index 42c46628d..cdc43a9bd 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_copystate.c
+++ b/nuttx/arch/arm/src/armv7-a/arm_copyfullstate.c
@@ -1,5 +1,5 @@
 /****************************************************************************
- * arch/arm/src/armv7-a/arm_copystate.c
+ * arch/arm/src/armv7-a/arm_copyfullstate.c
  *
  *   Copyright (C) 2013 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
@@ -61,12 +61,16 @@
  ****************************************************************************/
 
 /****************************************************************************
- * Name: up_copystate
+ * Name: up_copyfullstate
+ *
+ * Description:
+ *    Copy the entire register save area (including the floating point
+ *    registers if applicable).  This is a little faster than most memcpy's
+ *    since it does 32-bit transfers.
+ *
  ****************************************************************************/
 
-/* A little faster than most memcpy's */
-
-void up_copystate(uint32_t *dest, uint32_t *src)
+void up_copyfullstate(uint32_t *dest, uint32_t *src)
 {
   int i;
 
@@ -79,4 +83,3 @@ void up_copystate(uint32_t *dest, uint32_t *src)
       *dest++ = *src++;
     }
 }
-
diff --git a/nuttx/arch/arm/src/armv7-a/arm_doirq.c b/nuttx/arch/arm/src/armv7-a/arm_doirq.c
index eec228b91..d7ec6fc91 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_doirq.c
+++ b/nuttx/arch/arm/src/armv7-a/arm_doirq.c
@@ -76,21 +76,14 @@ void up_doirq(int irq, uint32_t *regs)
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
 #else
-  uint32_t *savestate;
-
-  /* Nested interrupts are not supported in this implementation.  If you want
-   * to implement nested interrupts, you would have to (1) change the way that
-   * current_regs is handled and (2) the design associated with
-   * CONFIG_ARCH_INTERRUPTSTACK.  The savestate variable will not work for
-   * that purpose as implemented here because only the outermost nested
-   * interrupt can result in a context switch (it can probably be deleted).
-   */
+  /* Nested interrupts are not supported */
+
+  DEBUGASSERT(current_regs == NULL);
 
   /* Current regs non-zero indicates that we are processing an interrupt;
    * current_regs is also used to manage interrupt level context switches.
    */
 
-  savestate    = (uint32_t*)current_regs;
   current_regs = regs;
 
   /* Mask and acknowledge the interrupt */
@@ -101,12 +94,24 @@ void up_doirq(int irq, uint32_t *regs)
 
   irq_dispatch(irq, regs);
 
-  /* Restore the previous value of current_regs.  NULL would indicate that
-   * we are no longer in an interrupt handler.  It will be non-NULL if we
-   * are returning from a nested interrupt.
+  /* Check for a context switch.  If a context switch occured, then
+   * current_regs will have a different value than it did on entry.  If an
+   * interrupt level context switch has occurred, then restore the floating
+   * point state before returning from the interrupt.
+   */
+
+  if (regs != current_regs)
+    {
+      /* Restore floating point registers */
+
+      up_restorefpu((uint32_t*)current_regs);
+    }
+
+  /* Set current_regs to NULL to indicate that we are no longer in an
+   * interrupt handler.
    */
 
-  current_regs = savestate;
+  current_regs = NULL;
 
   /* Unmask the last interrupt (global interrupts are still disabled) */
 
diff --git a/nuttx/arch/arm/src/armv7-a/arm_fullcontextrestore.S b/nuttx/arch/arm/src/armv7-a/arm_fullcontextrestore.S
index 6b0125612..1f10b8dde 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_fullcontextrestore.S
+++ b/nuttx/arch/arm/src/armv7-a/arm_fullcontextrestore.S
@@ -74,16 +74,41 @@
 
 	.globl	up_fullcontextrestore
 	.type	up_fullcontextrestore, function
+
 up_fullcontextrestore:
 
-	/* On entry, a1 (r0) holds address of the register save area */
+	/* On entry, a1 (r0) holds address of the register save area.  All other
+	 * registers are available for use.
+	 */
+
+#ifdef CONFIG_ARCH_FPU
+	/* First, restore the floating point registers.  Lets do this before we
+	 * restore the arm registers so that we have plentry of registers to
+	 * work with.
+	 */
+
+	add		r1, r0, #(4*REG_S0)		/* r1=Address of FP register storage */
+
+	/* Load all floating point registers.  Registers are loaded in numeric order,
+	 * s0, s1, ... in increasing address order.
+	 */
+
+	vldmia	r1!, {s0-s31}			/* Restore the full FP context */
+
+	/* Load the floating point control and status register.   At the end of the
+	 * vstmia, r1 will point to the FPCSR storage location.
+	 */
+
+	ldr		r2, [r1], #4			/* Fetch the floating point control and status register */
+	vmsr	fpscr, r2				/* Restore the FPCSR */
+#endif
 
 	/* Recover all registers except for r0, r1, R15, and CPSR */
 
 	add		r1, r0, #(4*REG_R2)		/* Offset to REG_R2 storage */
 	ldmia	r1, {r2-r14}			/* Recover registers */
 
-	/* Create a stack frame to hold the PC */
+	/* Create a stack frame to hold the some registers */
 
 	sub		sp, sp, #(3*4)			/* Frame for three registers */
 	ldr		r1, [r0, #(4*REG_R0)]	/* Fetch the stored r0 value */
diff --git a/nuttx/arch/arm/src/armv7-a/arm_head.S b/nuttx/arch/arm/src/armv7-a/arm_head.S
index acb87b248..6cf1a8fdf 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_head.S
+++ b/nuttx/arch/arm/src/armv7-a/arm_head.S
@@ -395,14 +395,11 @@ __start:
 
 	ldr		lr, .LCvstart			/* Abs. virtual address */
 
-	/* Configure the domain access register (see mmu.h).
-	 *
-	 *   Domains 0: Accesses are not checked
-	 *   Domains 1: Accesses are not checked
-	 *   Domains 2: Accesses checked against permissions in the TLB
+	/* Configure the domain access register (see mmu.h).  Only domain 0 is
+	 * supported and it uses the permissions in the TLB.
 	 */
 
-	mov		r0, #(DACR_MANAGER(0) | DACR_MANAGER(1) |  DACR_CLIENT(2))
+	mov		r0, #DACR_CLIENT(0)
 	mcr		CP15_DACR(r0)			/* Set domain access register */
 
 	/* Configure the system control register (see sctrl.h) */
diff --git a/nuttx/arch/arm/src/armv7-a/arm_restorefpu.S b/nuttx/arch/arm/src/armv7-a/arm_restorefpu.S
new file mode 100644
index 000000000..0d9a64066
--- /dev/null
+++ b/nuttx/arch/arm/src/armv7-a/arm_restorefpu.S
@@ -0,0 +1,108 @@
+/************************************************************************************
+ * arch/arm/src/armv7-a/arm_restorefpu.S
+ *
+ *   Copyright (C) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <arch/irq.h>
+
+#ifdef CONFIG_ARCH_FPU
+
+	.file		"arm_restorefpu.S"
+
+/************************************************************************************
+ * Preprocessor Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Global Symbols
+ ************************************************************************************/
+
+	.globl		up_restorefpu
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+	.text
+
+/************************************************************************************
+ * Name: up_restorefpu
+ *
+ * Description:
+ *   Given the pointer to a register save area (in R0), restore the state of the
+ *   floating point registers.
+ *
+ * C Function Prototype:
+ *   void up_restorefpu(const uint32_t *regs);
+ *
+ * Input Parameters:
+ *   regs - A pointer to the register save area containing the floating point
+ *     registers.
+ *
+ * Returned Value:
+ *   This function does not return anything explicitly.  However, it is called from
+ *   interrupt level assembly logic that assumes that r0 is preserved.
+ * 
+ ************************************************************************************/
+
+	.globl	up_restorefpu
+	.type	up_restorefpu, function
+
+up_restorefpu:
+
+	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
+
+	/* Load all floating point registers.  Registers are loaded in numeric order,
+	 * s0, s1, ... in increasing address order.
+	 */
+
+	vldmia	r1!, {s0-s31}			/* Restore the full FP context */
+
+	/* Load the floating point control and status register.   At the end of the
+	 * vstmia, r1 will point to the FPCSR storage location.
+	 */
+
+	ldr		r2, [r1], #4			/* Fetch the floating point control and status register */
+	vmsr	fpscr, r2				/* Restore the FPCSR */
+	bx		lr
+
+	.size	up_restorefpu, .-up_restorefpu
+#endif /* CONFIG_ARCH_FPU */
+	.end
+
diff --git a/nuttx/arch/arm/src/armv7-a/arm_savefpu.S b/nuttx/arch/arm/src/armv7-a/arm_savefpu.S
new file mode 100644
index 000000000..742db7848
--- /dev/null
+++ b/nuttx/arch/arm/src/armv7-a/arm_savefpu.S
@@ -0,0 +1,106 @@
+/************************************************************************************
+ * arch/arm/src/armv7-a/arm_savefpu.S
+ *
+ *   Copyright (C) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <arch/irq.h>
+
+#ifdef CONFIG_ARCH_FPU
+
+	.file		"arm_savefpu.S"
+
+/************************************************************************************
+ * Preprocessor Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Global Symbols
+ ************************************************************************************/
+
+	.globl		up_savefpu
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+	.text
+
+/************************************************************************************
+ * Name: up_savefpu
+ *
+ * Description:
+ *   Given the pointer to a register save area (in R0), save the state of the
+ *   floating point registers.
+ *
+ * C Function Prototype:
+ *   void up_savefpu(uint32_t *regs);
+ *
+ * Input Parameters:
+ *   regs - A pointer to the register save area in which to save the floating point
+ *     registers
+ *
+ * Returned Value:
+ *   None
+ * 
+ ************************************************************************************/
+
+	.globl	up_savefpu
+	.type	up_savefpu, function
+
+up_savefpu:
+
+	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
+
+	/* Store all floating point registers.  Registers are stored in numeric order,
+	 * s0, s1, ... in increasing address order.
+	 */
+
+	vstmia	r1!, {s0-s31}			/* Save the full FP context */
+
+	/* Store the floating point control and status register.  At the end of the
+	 * vstmia, r1 will point to the FPCSR storage location.
+	 */
+
+	vmrs	r2, fpscr				/* Fetch the FPCSR */
+	str		r2, [r1], #4			/* Save the floating point control and status register */
+	bx		lr
+
+	.size	up_savefpu, .-up_savefpu
+#endif /* CONFIG_ARCH_FPU */
+	.end
diff --git a/nuttx/arch/arm/src/armv7-a/arm_saveusercontext.S b/nuttx/arch/arm/src/armv7-a/arm_saveusercontext.S
index 427a08765..bdf16a7a9 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_saveusercontext.S
+++ b/nuttx/arch/arm/src/armv7-a/arm_saveusercontext.S
@@ -68,24 +68,23 @@
  * Public Functions
  ****************************************************************************/
 
+	.text
+
 /****************************************************************************
  * Name: up_saveusercontext
  ****************************************************************************/
 
-	.text
 	.globl	up_saveusercontext
 	.type	up_saveusercontext, function
+
 up_saveusercontext:
-	/* On entry, a1 (r0) holds address of struct xcptcontext.
-	 * Offset to the user region.
-	 */
 
-	/* Make sure that the return value will be non-zero (the
-	 * value of the other volatile registers don't matter --
-	 * r1-r3, ip).  This function is called throught the
-	 * normal C calling conventions and the values of these
-	 * registers cannot be assumed at the point of setjmp
-	 * return.
+	/* On entry, a1 (r0) holds address of struct xcptcontext */
+
+	/* Make sure that the return value will be non-zero (the value of the
+	 * other volatile registers don't matter -- r1-r3, ip).  This function
+	 * is called through the normal C calling conventions and the values of
+	 * these registers cannot be assumed at the point of setjmp return.
 	 */
 
 	mov		ip, #1
@@ -104,14 +103,38 @@ up_saveusercontext:
 	add		r1, r0, #(4*REG_CPSR)
 	str		r2, [r1]
 
-	/* Finally save the return address as the PC so that we
-	 * return to the exit from this function.
+	/* Save the return address as the PC so that we return to the exit from
+	 * this function.
 	 */
 
 	add		r1, r0, #(4*REG_PC)
 	str		lr, [r1]
 
-	/* Return 0 */
+	/* Save the floating point registers.
+	 * REVISIT:  Not all of the floating point registers need to be saved.
+	 * Some are volatile and need not be preserved across functions calls.
+	 * But right now, I can't find the definitive list of the volatile
+	 * floating point registers.
+	 */
+
+#ifdef CONFIG_ARCH_FPU
+	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
+
+	/* Store all floating point registers.  Registers are stored in numeric order,
+	 * s0, s1, ... in increasing address order.
+	 */
+
+	vstmia	r1!, {s0-s31}			/* Save the full FP context */
+
+	/* Store the floating point control and status register.  At the end of the
+	 * vstmia, r1 will point to the FPCSR storage location.
+	 */
+
+	vmrs	r2, fpscr				/* Fetch the FPCSR */
+	str		r2, [r1], #4			/* Save the floating point control and status register */
+#endif
+
+	/* Return 0 now indicating that this return is not a context switch */
 
 	mov		r0, #0			/* Return value == 0 */
 	mov		pc, lr			/* Return */
diff --git a/nuttx/arch/arm/src/armv7-a/arm_sigdeliver.c b/nuttx/arch/arm/src/armv7-a/arm_sigdeliver.c
index d2ad98474..a62e3e60e 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_sigdeliver.c
+++ b/nuttx/arch/arm/src/armv7-a/arm_sigdeliver.c
@@ -100,7 +100,7 @@ void up_sigdeliver(void)
 
   /* Save the real return state on the stack. */
 
-  up_copystate(regs, rtcb->xcp.regs);
+  up_copyfullstate(regs, rtcb->xcp.regs);
   regs[REG_PC]         = rtcb->xcp.saved_pc;
   regs[REG_CPSR]       = rtcb->xcp.saved_cpsr;
 
diff --git a/nuttx/arch/arm/src/armv7-a/arm_vectors.S b/nuttx/arch/arm/src/armv7-a/arm_vectors.S
index 172067163..74bffde6e 100644
--- a/nuttx/arch/arm/src/armv7-a/arm_vectors.S
+++ b/nuttx/arch/arm/src/armv7-a/arm_vectors.S
@@ -90,15 +90,15 @@ g_aborttmp:
 	.type	arm_vectorirq, %function
 
 arm_vectorirq:
-	/* On entry, we are in IRQ mode.  We are free to use
-	 * the IRQ mode r13 and r14.
+	/* On entry, we are in IRQ mode.  We are free to use the IRQ mode r13
+	 * and r14.
 	 */
 
-	ldr	r13, .Lirqtmp
-	sub	lr, lr, #4
-	str	lr, [r13]				/* Save lr_IRQ */
-	mrs	lr, spsr
-	str	lr, [r13, #4]			/* Save spsr_IRQ */
+	ldr		r13, .Lirqtmp
+	sub		lr, lr, #4
+	str		lr, [r13]				/* Save lr_IRQ */
+	mrs		lr, spsr
+	str		lr, [r13, #4]			/* Save spsr_IRQ */
 
 	/* Then switch back to SVC mode */
 
diff --git a/nuttx/arch/arm/src/armv7-a/mmu.h b/nuttx/arch/arm/src/armv7-a/mmu.h
index b0135de7a..08fa9231b 100644
--- a/nuttx/arch/arm/src/armv7-a/mmu.h
+++ b/nuttx/arch/arm/src/armv7-a/mmu.h
@@ -447,73 +447,43 @@
 
 #endif /* CONFIG_PAGING */
 
-/* Page Size Selections *************************************************************/
+/* MMU flags ************************************************************************/
 
-/* Create some friendly definitions to handle some differences between
- * small and tiny pages.
- */
-
-#if CONFIG_PAGING_PAGESIZE == 1024
-
-   /* Base of the L2 page table (aligned to 4Kb byte boundaries) */
-
-#  define PGTABLE_L2_BASE_PADDR PGTABLE_L2_FINE_PBASE
-#  define PGTABLE_L2_BASE_VADDR PGTABLE_L2_FINE_VBASE
-
-   /* Number of pages in an L2 table per L1 entry */
-
-#  define PTE_NPAGES            PTE_TINY_NPAGES
-
-   /* Mask to get the page table physical address from an L1 entry */
-
-#  define PG_L1_PADDRMASK       PMD_PTE_PADDR_MASK
+/* Create some friendly definitions to handle page table entries */
 
-   /* MMU Flags for each memory region */
-
-#  define MMU_L1_TEXTFLAGS      (PMD_TYPE_PTE)
-#  define MMU_L2_TEXTFLAGS      (PTE_TYPE_SMALL | PTE_AP_R12 | PTE_CACHEABLE)
-#  define MMU_L1_DATAFLAGS      (PMD_TYPE_PTE)
-#  define MMU_L2_DATAFLAGS      (PTE_TYPE_SMALL | PTE_AP_RW12 | PTE_CACHEABLE|PTE_B)
-#  define MMU_L2_ALLOCFLAGS     (PTE_TYPE_SMALL | PTE_AP_RW12)
-#  define MMU_L1_PGTABFLAGS     (PMD_TYPE_PTE)
-#  define MMU_L2_PGTABFLAGS     (PTE_TYPE_SMALL | PTE_AP_RW12)
-
-#  define MMU_L2_VECTRWFLAGS    (PTE_TYPE_SMALL | PTE_AP_RW12)
-#  define MMU_L2_VECTROFLAGS    (PTE_TYPE_SMALL | PTE_AP_R12 | PTE_CACHEABLE)
-
-#elif CONFIG_PAGING_PAGESIZE == 4096
-
-   /* Base of the L2 page table (aligned to 1Kb byte boundaries) */
-
-#  define PGTABLE_L2_BASE_PADDR PGTABLE_L2_PBASE
-#  define PGTABLE_L2_BASE_VADDR PGTABLE_L2_VBASE
+#if CONFIG_PAGING_PAGESIZE != 4096
+#  error "Unsupported value for CONFIG_PAGING_PAGESIZE"
+#endif
 
-   /* Number of pages in an L2 table per L1 entry */
+/* Base of the L2 page table (aligned to 1Kb byte boundaries) */
 
-#  define PTE_NPAGES            PTE_SMALL_NPAGES
+#define PGTABLE_L2_BASE_PADDR PGTABLE_L2_PBASE
+#define PGTABLE_L2_BASE_VADDR PGTABLE_L2_VBASE
 
-   /* Mask to get the page table physical address from an L1 entry */
+/* Number of pages in an L2 table per L1 entry */
 
-#  define PG_L1_PADDRMASK       PMD_SECT_PADDR_MASK
+#define PTE_NPAGES            PTE_SMALL_NPAGES
+#define PT_SIZE               (4*PTE_NPAGES)
 
-   /* MMU Flags for each memory region. */
+/* Mask to get the page table physical address from an L1 entry */
 
-#  define MMU_L1_TEXTFLAGS      (PMD_TYPE_PTE)
-#  define MMU_L2_TEXTFLAGS      (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRO|PTE_CACHEABLE)
-#  define MMU_L1_DATAFLAGS      (PMD_TYPE_PTE)
-#  define MMU_L2_DATAFLAGS      (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW|PTE_CACHEABLE|PTE_B)
-#  define MMU_L2_ALLOCFLAGS     (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
-#  define MMU_L1_PGTABFLAGS     (PMD_TYPE_PTE)
-#  define MMU_L2_PGTABFLAGS     (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
+#define PG_L1_PADDRMASK       PMD_SECT_PADDR_MASK
 
-#  define MMU_L2_VECTRWFLAGS    (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
-#  define MMU_L2_VECTROFLAGS    (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRO|PTE_CACHEABLE)
+/* MMU Flags for each type memory region. */
 
-#else
-#  error "Need extended definitions for CONFIG_PAGING_PAGESIZE"
-#endif
+#define MMU_L1_TEXTFLAGS      (PMD_TYPE_PTE)
+#define MMU_L2_TEXTFLAGS      (PTE_TYPE_SMALL | PTE_SMALL_AP_UNO_SRO | \
+                               PTE_CACHEABLE)
+#define MMU_L1_DATAFLAGS      (PMD_TYPE_PTE)
+#define MMU_L2_DATAFLAGS      (PTE_TYPE_SMALL | PTE_SMALL_AP_UNO_SRW | \
+                               PTE_CACHEABLE | PTE_B)
+#define MMU_L2_ALLOCFLAGS     (PTE_TYPE_SMALL | PTE_SMALL_AP_UNO_SRW)
+#define MMU_L1_PGTABFLAGS     (PMD_TYPE_PTE)
+#define MMU_L2_PGTABFLAGS     (PTE_TYPE_SMALL | PTE_SMALL_AP_UNO_SRW)
 
-#define PT_SIZE                 (4*PTE_NPAGES)
+#define MMU_L2_VECTRWFLAGS    (PTE_TYPE_SMALL | PTE_SMALL_AP_UNO_SRW)
+#define MMU_L2_VECTROFLAGS    (PTE_TYPE_SMALL | PTE_SMALL_AP_UNO_SRO | \
+                               PTE_CACHEABLE)
 
 /* Addresses of Memory Regions ******************************************************/
 
diff --git a/nuttx/arch/arm/src/armv7-m/up_copyarmstate.c b/nuttx/arch/arm/src/armv7-m/up_copyarmstate.c
new file mode 100644
index 000000000..94716939f
--- /dev/null
+++ b/nuttx/arch/arm/src/armv7-m/up_copyarmstate.c
@@ -0,0 +1,93 @@
+/****************************************************************************
+ * arch/arm/src/armv7-m/up_copyarmstate.c
+ *
+ *   Copyright (C) 2009, 2011, 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+
+#include "os_internal.h"
+#include "up_internal.h"
+
+#if defined(CONFIG_ARCH_FPU) && !defined(CONFIG_ARMV7M_CMNVECTOR)
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_copyarmstate
+ *
+ * Description:
+ *    Copy the ARM portion of the register save area (omitting the floating
+ *    point registers).  This is a little faster than most memcpy's since it
+ *    does 32-bit transfers.
+ *
+ ****************************************************************************/
+
+void up_copyarmstate(uint32_t *dest, uint32_t *src)
+{
+  int i;
+
+  /* In the Cortex-M3 model, the state is copied from the stack to the TCB,
+   * but only a reference is passed to get the state from the TCB.  So the
+   * following check avoids copying the TCB save area onto itself:
+   */
+
+  if (src != dest)
+    {
+      for (i = 0; i < SW_INT_REGS; i++)
+        {
+          *dest++ = *src++;
+        }
+    }
+}
+
+#endif /* CONFIG_ARCH_FPU && !CONFIG_ARMV7M_CMNVECTOR */
diff --git a/nuttx/arch/arm/src/armv7-m/up_copyfullstate.c b/nuttx/arch/arm/src/armv7-m/up_copyfullstate.c
new file mode 100644
index 000000000..7f320f842
--- /dev/null
+++ b/nuttx/arch/arm/src/armv7-m/up_copyfullstate.c
@@ -0,0 +1,89 @@
+/****************************************************************************
+ * arch/arm/src/armv7-m/up_copyfullstate.c
+ *
+ *   Copyright (C) 2009, 2011, 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+
+#include "os_internal.h"
+#include "up_internal.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_copyfullstate
+ *
+ * Description:
+ *    Copy the entire register save area (including the floating point
+ *    registers if applicable).  This is a little faster than most memcpy's
+ *    since it does 32-bit transfers.
+ *
+ ****************************************************************************/
+
+void up_copyfullstate(uint32_t *dest, uint32_t *src)
+{
+  int i;
+
+  /* In the Cortex-M3 model, the state is copied from the stack to the TCB,
+   * but only a reference is passed to get the state from the TCB.  So the
+   * following check avoids copying the TCB save area onto itself:
+   */
+
+  if (src != dest)
+    {
+      for (i = 0; i < XCPTCONTEXT_REGS; i++)
+        {
+          *dest++ = *src++;
+        }
+    }
+}
diff --git a/nuttx/arch/arm/src/armv7-m/up_sigdeliver.c b/nuttx/arch/arm/src/armv7-m/up_sigdeliver.c
index e9c7a9523..2319c48a7 100644
--- a/nuttx/arch/arm/src/armv7-m/up_sigdeliver.c
+++ b/nuttx/arch/arm/src/armv7-m/up_sigdeliver.c
@@ -100,7 +100,7 @@ void up_sigdeliver(void)
 
   /* Save the real return state on the stack. */
 
-  up_copystate(regs, rtcb->xcp.regs);
+  up_copyfullstate(regs, rtcb->xcp.regs);
   regs[REG_PC]         = rtcb->xcp.saved_pc;
 #ifdef CONFIG_ARMV7M_USEBASEPRI
   regs[REG_BASEPRI]    = rtcb->xcp.saved_basepri;
diff --git a/nuttx/arch/arm/src/c5471/Make.defs b/nuttx/arch/arm/src/c5471/Make.defs
index afa73cf6c..016cc36d7 100644
--- a/nuttx/arch/arm/src/c5471/Make.defs
+++ b/nuttx/arch/arm/src/c5471/Make.defs
@@ -36,7 +36,7 @@
 HEAD_ASRC	= up_nommuhead.S
 
 CMN_ASRCS	= up_saveusercontext.S up_fullcontextrestore.S vfork.S
-CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
+CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c \
 		  up_createstack.c up_dataabort.c up_mdelay.c up_udelay.c up_doirq.c \
 		  up_exit.c up_idle.c up_initialize.c up_initialstate.c \
 		  up_interruptcontext.c up_prefetchabort.c up_releasepending.c \
diff --git a/nuttx/arch/arm/src/calypso/Make.defs b/nuttx/arch/arm/src/calypso/Make.defs
index 1a1d14869..957604f36 100644
--- a/nuttx/arch/arm/src/calypso/Make.defs
+++ b/nuttx/arch/arm/src/calypso/Make.defs
@@ -40,7 +40,7 @@ HEAD_ASRC = calypso_head.S
 
 CMN_ASRCS = up_saveusercontext.S up_fullcontextrestore.S up_vectors.S \
             up_nommuhead.S vfork.S
-CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
+CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c \
             up_createstack.c up_dataabort.c up_mdelay.c up_udelay.c up_doirq.c \
             up_exit.c up_idle.c up_initialstate.c up_initialize.c \
             up_interruptcontext.c up_prefetchabort.c up_releasepending.c \
diff --git a/nuttx/arch/arm/src/common/up_internal.h b/nuttx/arch/arm/src/common/up_internal.h
index 5f3f5e63b..86e49004c 100644
--- a/nuttx/arch/arm/src/common/up_internal.h
+++ b/nuttx/arch/arm/src/common/up_internal.h
@@ -121,22 +121,46 @@
 #if defined(CONFIG_ARCH_CORTEXM0) || defined(CONFIG_ARCH_CORTEXM3) || \
     defined(CONFIG_ARCH_CORTEXM4)
 
+  /* If the floating point unit is present and enabled, then save the
+   * floating point registers as well as normal ARM registers.  This only
+   * applies if "lazy" floating point register save/restore is used
+   * (i.e., not CONFIG_ARMV7M_CMNVECTOR).
+   */
+
 #  if defined(CONFIG_ARCH_FPU) && !defined(CONFIG_ARMV7M_CMNVECTOR)
 #    define up_savestate(regs) \
        do { \
-         up_copystate(regs, (uint32_t*)current_regs); \
+         up_copyarmstate(regs, (uint32_t*)current_regs); \
          up_savefpu(regs); \
        } \
        while (0)
 #  else
-#    define up_savestate(regs)  up_copystate(regs, (uint32_t*)current_regs)
+#    define up_savestate(regs)  up_copyfullstate(regs, (uint32_t*)current_regs)
 #  endif
 #  define up_restorestate(regs) (current_regs = regs)
 
+/* Otherwise, for the ARM7, ARM9, and Cortex-A5.  The state is copied in full
+ * from stack to stack.  This is not very efficient.
+ */
+
 #else
 
-#  define up_savestate(regs)    up_copystate(regs, (uint32_t*)current_regs)
-#  define up_restorestate(regs) up_copystate((uint32_t*)current_regs, regs)
+  /* If the floating point unit is present and enabled, then save the
+   * floating point registers as well as normal ARM registers.  Only "lazy"
+   * floating point save/restore is supported.
+   */
+
+#  if defined(CONFIG_ARCH_FPU)
+#    define up_savestate(regs) \
+       do { \
+         up_copyarmstate(regs, (uint32_t*)current_regs); \
+         up_savefpu(regs); \
+       } \
+       while (0)
+#  else
+#    define up_savestate(regs)  up_copyfullstate(regs, (uint32_t*)current_regs)
+#  endif
+#  define up_restorestate(regs) up_copyfullstate((uint32_t*)current_regs, regs)
 
 #endif
 
@@ -244,7 +268,10 @@ void up_boot(void);
 
 /* Context switching */
 
-void up_copystate(uint32_t *dest, uint32_t *src);
+void up_copyfullstate(uint32_t *dest, uint32_t *src);
+#ifdef CONFIG_ARCH_FPU
+void up_copyarmstate(uint32_t *dest, uint32_t *src);
+#endif
 void up_decodeirq(uint32_t *regs);
 int  up_saveusercontext(uint32_t *saveregs);
 void up_fullcontextrestore(uint32_t *restoreregs) noreturn_function;
diff --git a/nuttx/arch/arm/src/dm320/Make.defs b/nuttx/arch/arm/src/dm320/Make.defs
index 651d9fea9..fc51170de 100644
--- a/nuttx/arch/arm/src/dm320/Make.defs
+++ b/nuttx/arch/arm/src/dm320/Make.defs
@@ -37,7 +37,7 @@ HEAD_ASRC	= up_head.S
 
 CMN_ASRCS	= up_cache.S up_fullcontextrestore.S up_saveusercontext.S \
 		  up_vectors.S up_vectoraddrexcptn.S up_vectortab.S vfork.S
-CMN_CSRCS	= up_assert.c up_blocktask.c up_copystate.c up_createstack.c \
+CMN_CSRCS	= up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c \
 		  up_dataabort.c up_mdelay.c up_udelay.c up_exit.c up_idle.c \
 		  up_initialize.c up_initialstate.c up_interruptcontext.c \
 		  up_prefetchabort.c up_releasepending.c up_releasestack.c \
diff --git a/nuttx/arch/arm/src/imx/Make.defs b/nuttx/arch/arm/src/imx/Make.defs
index 3cea29b9b..ef88e28cd 100644
--- a/nuttx/arch/arm/src/imx/Make.defs
+++ b/nuttx/arch/arm/src/imx/Make.defs
@@ -37,7 +37,7 @@ HEAD_ASRC	= up_head.S
 
 CMN_ASRCS	= up_cache.S up_fullcontextrestore.S up_saveusercontext.S \
 		  up_vectors.S up_vectoraddrexcptn.S up_vectortab.S vfork.S
-CMN_CSRCS	= up_assert.c up_blocktask.c up_copystate.c up_createstack.c \
+CMN_CSRCS	= up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c \
 		  up_dataabort.c up_mdelay.c up_udelay.c up_exit.c up_idle.c \
 		  up_initialize.c up_initialstate.c up_interruptcontext.c \
 		  up_prefetchabort.c up_releasepending.c up_releasestack.c \
diff --git a/nuttx/arch/arm/src/kinetis/Make.defs b/nuttx/arch/arm/src/kinetis/Make.defs
index 1583b3cc2..0429dfadd 100644
--- a/nuttx/arch/arm/src/kinetis/Make.defs
+++ b/nuttx/arch/arm/src/kinetis/Make.defs
@@ -42,7 +42,7 @@ HEAD_ASRC = kinetis_vectors.S
 CMN_ASRCS  = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S
 CMN_ASRCS += vfork.S
 
-CMN_CSRCS  = up_assert.c up_blocktask.c up_copystate.c up_createstack.c
+CMN_CSRCS  = up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c
 CMN_CSRCS += up_mdelay.c up_udelay.c up_exit.c up_initialize.c up_memfault.c
 CMN_CSRCS += up_initialstate.c up_interruptcontext.c up_modifyreg8.c
 CMN_CSRCS += up_modifyreg16.c up_modifyreg32.c up_releasestack.c
diff --git a/nuttx/arch/arm/src/kl/Make.defs b/nuttx/arch/arm/src/kl/Make.defs
index e43059399..2fa6049bf 100644
--- a/nuttx/arch/arm/src/kl/Make.defs
+++ b/nuttx/arch/arm/src/kl/Make.defs
@@ -38,7 +38,7 @@ HEAD_ASRC =
 CMN_ASRCS  = up_exception.S up_saveusercontext.S up_fullcontextrestore.S
 CMN_ASRCS += up_switchcontext.S vfork.S
 
-CMN_CSRCS  = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c
+CMN_CSRCS  = up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c
 CMN_CSRCS += up_createstack.c up_mdelay.c up_udelay.c up_exit.c
 CMN_CSRCS += up_initialize.c up_initialstate.c up_interruptcontext.c
 CMN_CSRCS += up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c
diff --git a/nuttx/arch/arm/src/lm/Make.defs b/nuttx/arch/arm/src/lm/Make.defs
index 1267e6cf9..141b5003a 100644
--- a/nuttx/arch/arm/src/lm/Make.defs
+++ b/nuttx/arch/arm/src/lm/Make.defs
@@ -38,7 +38,7 @@ HEAD_ASRC  = lm_vectors.S
 CMN_ASRCS  = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S
 CMN_ASRCS += vfork.S
 
-CMN_CSRCS  = up_assert.c up_blocktask.c up_copystate.c up_createstack.c
+CMN_CSRCS  = up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c
 CMN_CSRCS += up_mdelay.c up_udelay.c up_exit.c up_idle.c up_initialize.c
 CMN_CSRCS += up_initialstate.c up_interruptcontext.c up_memfault.c
 CMN_CSRCS += up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c
diff --git a/nuttx/arch/arm/src/lpc17xx/Make.defs b/nuttx/arch/arm/src/lpc17xx/Make.defs
index 4766e94f9..be695a777 100644
--- a/nuttx/arch/arm/src/lpc17xx/Make.defs
+++ b/nuttx/arch/arm/src/lpc17xx/Make.defs
@@ -49,7 +49,7 @@ CMN_UCSRCS =
 CMN_ASRCS  = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S
 CMN_ASRCS += vfork.S
 
-CMN_CSRCS  = up_assert.c up_blocktask.c up_copystate.c up_createstack.c
+CMN_CSRCS  = up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c
 CMN_CSRCS += up_mdelay.c up_udelay.c up_exit.c up_initialize.c up_memfault.c
 CMN_CSRCS += up_initialstate.c up_interruptcontext.c up_modifyreg8.c
 CMN_CSRCS += up_modifyreg16.c up_modifyreg32.c up_releasepending.c
@@ -90,6 +90,9 @@ endif
 
 ifeq ($(CONFIG_ARCH_FPU),y)
 CMN_ASRCS += up_fpu.S
+ifneq ($(CONFIG_ARMV7M_CMNVECTOR),y)
+CMN_CSRCS += up_copyarmstate.c
+endif
 endif
 
 # Required LPC17xx files
diff --git a/nuttx/arch/arm/src/lpc214x/Make.defs b/nuttx/arch/arm/src/lpc214x/Make.defs
index 34713b008..28c826eec 100644
--- a/nuttx/arch/arm/src/lpc214x/Make.defs
+++ b/nuttx/arch/arm/src/lpc214x/Make.defs
@@ -37,7 +37,7 @@ HEAD_ASRC	= lpc214x_head.S
 
 CMN_ASRCS	= up_saveusercontext.S up_fullcontextrestore.S up_vectors.S \
 			  vfork.S
-CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
+CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c \
 		  up_createstack.c up_dataabort.c up_mdelay.c up_udelay.c \
 		  up_exit.c up_idle.c up_initialize.c up_initialstate.c \
 		  up_interruptcontext.c up_prefetchabort.c up_releasepending.c \
diff --git a/nuttx/arch/arm/src/lpc2378/Make.defs b/nuttx/arch/arm/src/lpc2378/Make.defs
index cfd9dc79f..a245bc98a 100644
--- a/nuttx/arch/arm/src/lpc2378/Make.defs
+++ b/nuttx/arch/arm/src/lpc2378/Make.defs
@@ -42,7 +42,7 @@ HEAD_ASRC	= lpc23xx_head.S
 
 CMN_ASRCS	= up_saveusercontext.S up_fullcontextrestore.S up_vectors.S \
 			  vfork.S
-CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
+CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c \
 			  up_createstack.c up_dataabort.c up_mdelay.c up_udelay.c \
 			  up_exit.c up_idle.c up_initialize.c up_initialstate.c \
 			  up_interruptcontext.c up_prefetchabort.c up_releasepending.c \
diff --git a/nuttx/arch/arm/src/lpc31xx/Make.defs b/nuttx/arch/arm/src/lpc31xx/Make.defs
index 4e6f69299..f5bdf7a94 100644
--- a/nuttx/arch/arm/src/lpc31xx/Make.defs
+++ b/nuttx/arch/arm/src/lpc31xx/Make.defs
@@ -37,7 +37,7 @@ HEAD_ASRC	= up_head.S
 
 CMN_ASRCS	= up_cache.S up_fullcontextrestore.S up_saveusercontext.S \
 		  up_vectors.S up_vectoraddrexcptn.S up_vectortab.S vfork.S
-CMN_CSRCS	= up_assert.c up_blocktask.c up_copystate.c up_createstack.c \
+CMN_CSRCS	= up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c \
 		  up_dataabort.c up_mdelay.c up_udelay.c up_exit.c up_idle.c \
 		  up_initialize.c up_initialstate.c up_interruptcontext.c \
 		  up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c \
diff --git a/nuttx/arch/arm/src/lpc43xx/Make.defs b/nuttx/arch/arm/src/lpc43xx/Make.defs
index e3fe984b3..7b7a7e8c4 100644
--- a/nuttx/arch/arm/src/lpc43xx/Make.defs
+++ b/nuttx/arch/arm/src/lpc43xx/Make.defs
@@ -38,7 +38,7 @@ HEAD_ASRC  =
 CMN_ASRCS  = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S
 CMN_ASRCS += vfork.S
 
-CMN_CSRCS  = up_assert.c up_blocktask.c up_copystate.c
+CMN_CSRCS  = up_assert.c up_blocktask.c up_copyfullstate.c
 CMN_CSRCS += up_createstack.c up_mdelay.c up_udelay.c up_exit.c
 CMN_CSRCS += up_initialize.c up_initialstate.c up_interruptcontext.c
 CMN_CSRCS += up_memfault.c up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c
@@ -77,6 +77,9 @@ endif
 
 ifeq ($(CONFIG_ARCH_FPU),y)
 CMN_ASRCS += up_fpu.S
+ifneq ($(CONFIG_ARMV7M_CMNVECTOR),y)
+CMN_CSRCS += up_copyarmstate.c
+endif
 endif
 
 CHIP_ASRCS  = 
diff --git a/nuttx/arch/arm/src/nuc1xx/Make.defs b/nuttx/arch/arm/src/nuc1xx/Make.defs
index fa83d8aad..669d0877e 100644
--- a/nuttx/arch/arm/src/nuc1xx/Make.defs
+++ b/nuttx/arch/arm/src/nuc1xx/Make.defs
@@ -38,7 +38,7 @@ HEAD_ASRC =
 CMN_ASRCS  = up_exception.S up_saveusercontext.S up_fullcontextrestore.S
 CMN_ASRCS += up_switchcontext.S vfork.S
 
-CMN_CSRCS  = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c
+CMN_CSRCS  = up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c
 CMN_CSRCS += up_createstack.c up_mdelay.c up_udelay.c up_exit.c
 CMN_CSRCS += up_initialize.c up_initialstate.c up_interruptcontext.c
 CMN_CSRCS += up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c
diff --git a/nuttx/arch/arm/src/sam34/Make.defs b/nuttx/arch/arm/src/sam34/Make.defs
index 1db693145..725070805 100644
--- a/nuttx/arch/arm/src/sam34/Make.defs
+++ b/nuttx/arch/arm/src/sam34/Make.defs
@@ -44,7 +44,7 @@ CMN_UCSRCS =
 
 CMN_ASRCS  = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S
 CMN_ASRCS += vfork.S
-CMN_CSRCS  = up_assert.c up_blocktask.c up_copystate.c up_createstack.c
+CMN_CSRCS  = up_assert.c up_blocktask.c up_copyfullstate.c up_createstack.c
 CMN_CSRCS += up_mdelay.c up_udelay.c up_exit.c up_idle.c up_initialize.c
 CMN_CSRCS += up_initialstate.c up_interruptcontext.c up_memfault.c up_modifyreg8.c
 CMN_CSRCS += up_modifyreg16.c up_modifyreg32.c up_releasepending.c
diff --git a/nuttx/arch/arm/src/sama5/Make.defs b/nuttx/arch/arm/src/sama5/Make.defs
index 9dfd46a67..9f6b347a9 100644
--- a/nuttx/arch/arm/src/sama5/Make.defs
+++ b/nuttx/arch/arm/src/sama5/Make.defs
@@ -45,7 +45,7 @@ CMN_CSRCS += up_mdelay.c up_udelay.c
 CMN_CSRCS += up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c
 CMN_CSRCS += up_allocateheap.c
 
-CMN_CSRCS += arm_assert.c arm_blocktask.c arm_copystate.c arm_dataabort.c
+CMN_CSRCS += arm_assert.c arm_blocktask.c arm_copyfullstate.c arm_dataabort.c
 CMN_CSRCS += arm_doirq.c arm_initialstate.c arm_prefetchabort.c
 CMN_CSRCS += arm_releasepending.c arm_reprioritizertr.c
 CMN_CSRCS += arm_schedulesigaction.c arm_sigdeliver.c arm_syscall.c
@@ -60,6 +60,11 @@ ifeq ($(CONFIG_ELF),y)
 CMN_CSRCS += arm_elf.c
 endif
 
+ifeq ($(CONFIG_ARCH_FPU),y)
+CMN_ASRCS += arm_savefpu.S arm_restorefpu.S
+CMN_CSRCS += arm_copyarmstate.c
+endif
+
 CHIP_ASRCS  =
 
 CHIP_CSRCS  = sam_boot.c sam_clockconfig.c sam_gpio.c sam_irq.c
diff --git a/nuttx/arch/arm/src/stm32/Make.defs b/nuttx/arch/arm/src/stm32/Make.defs
index e4ff9e0fc..da27287f1 100644
--- a/nuttx/arch/arm/src/stm32/Make.defs
+++ b/nuttx/arch/arm/src/stm32/Make.defs
@@ -45,7 +45,7 @@ CMN_UCSRCS =
 CMN_ASRCS  = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S
 CMN_ASRCS += vfork.S
 
-CMN_CSRCS  = up_assert.c up_blocktask.c up_copystate.c
+CMN_CSRCS  = up_assert.c up_blocktask.c up_copyfullstate.c
 CMN_CSRCS += up_createstack.c up_mdelay.c up_udelay.c up_exit.c
 CMN_CSRCS += up_initialize.c up_initialstate.c up_interruptcontext.c
 CMN_CSRCS += up_memfault.c up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c
@@ -85,6 +85,9 @@ endif
 
 ifeq ($(CONFIG_ARCH_FPU),y)
 CMN_ASRCS += up_fpu.S
+ifneq ($(CONFIG_ARMV7M_CMNVECTOR),y)
+CMN_CSRCS += up_copyarmstate.c
+endif
 endif
 
 CHIP_ASRCS  = 
diff --git a/nuttx/arch/arm/src/str71x/Make.defs b/nuttx/arch/arm/src/str71x/Make.defs
index bde5ca003..b7a7e1bbc 100644
--- a/nuttx/arch/arm/src/str71x/Make.defs
+++ b/nuttx/arch/arm/src/str71x/Make.defs
@@ -37,7 +37,7 @@ HEAD_ASRC	= str71x_head.S
 
 CMN_ASRCS	= up_saveusercontext.S up_fullcontextrestore.S up_vectors.S \
 			  vfork.S
-CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
+CMN_CSRCS	= up_allocateheap.c up_assert.c up_blocktask.c up_copyfullstate.c \
 			  up_createstack.c up_dataabort.c up_mdelay.c up_udelay.c \
 			  up_exit.c up_idle.c up_initialize.c up_initialstate.c \
 			  up_interruptcontext.c up_prefetchabort.c up_releasepending.c \