diff --git a/arch/arm/src/armv7-a/arm_copyarmstate.c b/arch/arm/src/armv7-a/arm_copyarmstate.c
index 219d01a1e7..5feebfee9e 100644
--- a/arch/arm/src/armv7-a/arm_copyarmstate.c
+++ b/arch/arm/src/armv7-a/arm_copyarmstate.c
@@ -64,6 +64,11 @@
 
 /****************************************************************************
  * Name: up_copyarmstate
+ *
+ * Description:
+ *    Copy the ARM portion of the register save area (omitting the floating
+ *    point registers) and save the floating pointer register directly.
+ *
  ****************************************************************************/
 
 /* A little faster than most memcpy's */
@@ -79,6 +84,16 @@ void up_copyarmstate(uint32_t *dest, uint32_t *src)
 
   if (src != dest)
     {
+      /* Save the floating point registers: This will initialize the floating
+       * registers at indices ARM_CONTEXT_REGS through (XCPTCONTEXT_REGS-1)
+       */
+
+      up_savefpu(dest);
+
+      /* Then copy all of the ARM registers (mitting the floating point
+       * registers).  Indices: 0 through (ARM_CONTEXT_REGS-1).
+       */
+
       for (i = 0; i < ARM_CONTEXT_REGS; i++)
         {
           *dest++ = *src++;
diff --git a/arch/arm/src/armv7-a/arm_restorefpu.S b/arch/arm/src/armv7-a/arm_restorefpu.S
index 0d9a640667..b138ee9795 100644
--- a/arch/arm/src/armv7-a/arm_restorefpu.S
+++ b/arch/arm/src/armv7-a/arm_restorefpu.S
@@ -78,7 +78,7 @@
  * 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
diff --git a/arch/arm/src/armv7-a/arm_savefpu.S b/arch/arm/src/armv7-a/arm_savefpu.S
index 742db78486..df6ba65c66 100644
--- a/arch/arm/src/armv7-a/arm_savefpu.S
+++ b/arch/arm/src/armv7-a/arm_savefpu.S
@@ -77,7 +77,7 @@
  *
  * Returned Value:
  *   None
- * 
+ *
  ************************************************************************************/
 
 	.globl	up_savefpu
diff --git a/arch/arm/src/armv7-m/Toolchain.defs b/arch/arm/src/armv7-m/Toolchain.defs
index ec8988a2d8..f129c19133 100644
--- a/arch/arm/src/armv7-m/Toolchain.defs
+++ b/arch/arm/src/armv7-m/Toolchain.defs
@@ -157,9 +157,7 @@ endif
 ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),ATOLLIC)
   CROSSDEV ?= arm-atollic-eabi-
   ARCROSSDEV ?= arm-atollic-eabi-
-  ifneq ($(CONFIG_WINDOWS_NATIVE),y)
-    WINTOOL = y
-  endif
+  MAXOPTIMIZATION = -Os
   ifeq ($(CONFIG_ARCH_CORTEXM4),y)
     ifeq ($(CONFIG_ARCH_FPU),y)
       ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard
@@ -169,6 +167,9 @@ ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),ATOLLIC)
   else ifeq ($(CONFIG_ARCH_CORTEXM3),y)
     ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft
   endif
+  ifneq ($(CONFIG_WINDOWS_NATIVE),y)
+    WINTOOL = y
+  endif
 endif
 
 # NuttX buildroot under Linux or Cygwin
@@ -191,6 +192,7 @@ endif
 ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),CODEREDL)
   CROSSDEV ?= arm-none-eabi-
   ARCROSSDEV ?= arm-none-eabi-
+  MAXOPTIMIZATION = -Os
   ifeq ($(CONFIG_ARCH_CORTEXM4),y)
     ifeq ($(CONFIG_ARCH_FPU),y)
       ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard
@@ -207,9 +209,7 @@ endif
 ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),CODEREDW)
   CROSSDEV ?= arm-none-eabi-
   ARCROSSDEV ?= arm-none-eabi-
-  ifneq ($(CONFIG_WINDOWS_NATIVE),y)
-    WINTOOL = y
-  endif
+  MAXOPTIMIZATION = -Os
   ifeq ($(CONFIG_ARCH_CORTEXM4),y)
     ifeq ($(CONFIG_ARCH_FPU),y)
       ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard
@@ -219,6 +219,9 @@ ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),CODEREDW)
   else ifeq ($(CONFIG_ARCH_CORTEXM3),y)
     ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft
   endif
+  ifneq ($(CONFIG_WINDOWS_NATIVE),y)
+    WINTOOL = y
+  endif
 endif
 
 # CodeSourcery under Linux
@@ -235,10 +238,19 @@ endif
 ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),CODESOURCERYW)
   CROSSDEV ?= arm-none-eabi-
   ARCROSSDEV ?= arm-none-eabi-
+  MAXOPTIMIZATION = -Os
+  ifeq ($(CONFIG_ARCH_CORTEXM4),y)
+    ifeq ($(CONFIG_ARCH_FPU),y)
+      ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard
+    else
+      ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfloat-abi=soft
+    endif
+  else ifeq ($(CONFIG_ARCH_CORTEXM3),y)
+    ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft
+  endif
   ifneq ($(CONFIG_WINDOWS_NATIVE),y)
     WINTOOL = y
   endif
-  ARCHCPUFLAGS = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft
 endif
 
 # devkitARM under Windows
diff --git a/arch/arm/src/armv7-m/up_copyarmstate.c b/arch/arm/src/armv7-m/up_copyarmstate.c
index 94716939f7..32c7fce06f 100644
--- a/arch/arm/src/armv7-m/up_copyarmstate.c
+++ b/arch/arm/src/armv7-m/up_copyarmstate.c
@@ -67,8 +67,7 @@
  *
  * 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.
+ *    point registers) and save the floating pointer register directly.
  *
  ****************************************************************************/
 
@@ -83,10 +82,33 @@ void up_copyarmstate(uint32_t *dest, uint32_t *src)
 
   if (src != dest)
     {
+      /* Save the floating point registers: This will initialize the floating
+       * registers at indices SW_INT_REGS through (SW_INT_REGS+SW_FPU_REGS-1)
+       */
+
+      up_savefpu(dest);
+
+      /* Save the block of ARM registers that were saved by the interrupt
+       * handling logic.  Indices: 0 through (SW_INT_REGS-1).
+       */
+
       for (i = 0; i < SW_INT_REGS; i++)
         {
           *dest++ = *src++;
         }
+
+      /* Skip over the floating point registers and save the block of ARM
+       * registers that were saved by the hardware when the interrupt was
+       * taken.  Indices: (SW_INT_REGS+SW_FPU_REGS) through (HW_XCPT_REGS-1)
+       */
+
+      src  += SW_FPU_REGS;
+      dest += SW_FPU_REGS;
+
+      for (i = 0; i < HW_XCPT_REGS; i++)
+        {
+          *dest++ = *src++;
+        }
     }
 }
 
diff --git a/arch/arm/src/common/up_internal.h b/arch/arm/src/common/up_internal.h
index 86e49004c2..495b51b24f 100644
--- a/arch/arm/src/common/up_internal.h
+++ b/arch/arm/src/common/up_internal.h
@@ -128,12 +128,7 @@
    */
 
 #  if defined(CONFIG_ARCH_FPU) && !defined(CONFIG_ARMV7M_CMNVECTOR)
-#    define up_savestate(regs) \
-       do { \
-         up_copyarmstate(regs, (uint32_t*)current_regs); \
-         up_savefpu(regs); \
-       } \
-       while (0)
+#    define up_savestate(regs)  up_copyarmstate(regs, (uint32_t*)current_regs)
 #  else
 #    define up_savestate(regs)  up_copyfullstate(regs, (uint32_t*)current_regs)
 #  endif
@@ -151,12 +146,7 @@
    */
 
 #  if defined(CONFIG_ARCH_FPU)
-#    define up_savestate(regs) \
-       do { \
-         up_copyarmstate(regs, (uint32_t*)current_regs); \
-         up_savefpu(regs); \
-       } \
-       while (0)
+#    define up_savestate(regs)  up_copyarmstate(regs, (uint32_t*)current_regs)
 #  else
 #    define up_savestate(regs)  up_copyfullstate(regs, (uint32_t*)current_regs)
 #  endif