diff --git a/arch/arm/include/armv7-a/irq.h b/arch/arm/include/armv7-a/irq.h
index 098ff0e697b..6c12500911c 100644
--- a/arch/arm/include/armv7-a/irq.h
+++ b/arch/arm/include/armv7-a/irq.h
@@ -51,24 +51,6 @@
  * individual registers in the xcp.regs array:
  */
 
-#define REG_R0              (0)
-#define REG_R1              (1)
-#define REG_R2              (2)
-#define REG_R3              (3)
-#define REG_R4              (4)
-#define REG_R5              (5)
-#define REG_R6              (6)
-#define REG_R7              (7)
-#define REG_R8              (8)
-#define REG_R9              (9)
-#define REG_R10             (10)
-#define REG_R11             (11)
-#define REG_R12             (12)
-#define REG_R13             (13)
-#define REG_R14             (14)
-
-#define ARM_CONTEXT_REGS    (15)
-
 /* 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
@@ -85,87 +67,104 @@
  */
 
 #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_D0            (0)  /* D0 */
+#  define REG_S0            (0)  /* S0 */
+#  define REG_S1            (1)  /* S1 */
+#  define REG_D1            (2)  /* D1 */
+#  define REG_S2            (2)  /* S2 */
+#  define REG_S3            (3)  /* S3 */
+#  define REG_D2            (4)  /* D2 */
+#  define REG_S4            (4)  /* S4 */
+#  define REG_S5            (5)  /* S5 */
+#  define REG_D3            (6)  /* D3 */
+#  define REG_S6            (6)  /* S6 */
+#  define REG_S7            (7)  /* S7 */
+#  define REG_D4            (8)  /* D4 */
+#  define REG_S8            (8)  /* S8 */
+#  define REG_S9            (9)  /* S9 */
+#  define REG_D5            (10) /* D5 */
+#  define REG_S10           (10) /* S10 */
+#  define REG_S11           (11) /* S11 */
+#  define REG_D6            (12) /* D6 */
+#  define REG_S12           (12) /* S12 */
+#  define REG_S13           (13) /* S13 */
+#  define REG_D7            (14) /* D7 */
+#  define REG_S14           (14) /* S14 */
+#  define REG_S15           (15) /* S15 */
+#  define REG_D8            (16) /* D8 */
+#  define REG_S16           (16) /* S16 */
+#  define REG_S17           (17) /* S17 */
+#  define REG_D9            (18) /* D9 */
+#  define REG_S18           (18) /* S18 */
+#  define REG_S19           (19) /* S19 */
+#  define REG_D10           (20) /* D10 */
+#  define REG_S20           (20) /* S20 */
+#  define REG_S21           (21) /* S21 */
+#  define REG_D11           (22) /* D11 */
+#  define REG_S22           (22) /* S22 */
+#  define REG_S23           (23) /* S23 */
+#  define REG_D12           (24) /* D12 */
+#  define REG_S24           (24) /* S24 */
+#  define REG_S25           (25) /* S25 */
+#  define REG_D13           (26) /* D13 */
+#  define REG_S26           (26) /* S26 */
+#  define REG_S27           (27) /* S27 */
+#  define REG_D14           (28) /* D14 */
+#  define REG_S28           (28) /* S28 */
+#  define REG_S29           (29) /* S29 */
+#  define REG_D15           (30) /* D15 */
+#  define REG_S30           (30) /* S30 */
+#  define REG_S31           (31) /* S31 */
 #  ifdef CONFIG_ARM_HAVE_DPFPU32
-#    define REG_D16         (ARM_CONTEXT_REGS+32) /* D16 */
-#    define REG_D17         (ARM_CONTEXT_REGS+34) /* D17 */
-#    define REG_D18         (ARM_CONTEXT_REGS+36) /* D18 */
-#    define REG_D19         (ARM_CONTEXT_REGS+38) /* D19 */
-#    define REG_D20         (ARM_CONTEXT_REGS+40) /* D20 */
-#    define REG_D21         (ARM_CONTEXT_REGS+42) /* D21 */
-#    define REG_D22         (ARM_CONTEXT_REGS+44) /* D22 */
-#    define REG_D23         (ARM_CONTEXT_REGS+46) /* D23 */
-#    define REG_D24         (ARM_CONTEXT_REGS+48) /* D24 */
-#    define REG_D25         (ARM_CONTEXT_REGS+50) /* D25 */
-#    define REG_D26         (ARM_CONTEXT_REGS+52) /* D26 */
-#    define REG_D27         (ARM_CONTEXT_REGS+54) /* D27 */
-#    define REG_D28         (ARM_CONTEXT_REGS+56) /* D28 */
-#    define REG_D29         (ARM_CONTEXT_REGS+58) /* D29 */
-#    define REG_D30         (ARM_CONTEXT_REGS+60) /* D30 */
-#    define REG_D31         (ARM_CONTEXT_REGS+62) /* D31 */
-#    define REG_FPSCR       (ARM_CONTEXT_REGS+64) /* Floating point status and control */
+#    define REG_D16         (32) /* D16 */
+#    define REG_D17         (34) /* D17 */
+#    define REG_D18         (36) /* D18 */
+#    define REG_D19         (38) /* D19 */
+#    define REG_D20         (40) /* D20 */
+#    define REG_D21         (42) /* D21 */
+#    define REG_D22         (44) /* D22 */
+#    define REG_D23         (46) /* D23 */
+#    define REG_D24         (48) /* D24 */
+#    define REG_D25         (50) /* D25 */
+#    define REG_D26         (52) /* D26 */
+#    define REG_D27         (54) /* D27 */
+#    define REG_D28         (56) /* D28 */
+#    define REG_D29         (58) /* D29 */
+#    define REG_D30         (60) /* D30 */
+#    define REG_D31         (62) /* D31 */
+#    define REG_FPSCR       (64) /* Floating point status and control */
 #    define FPU_CONTEXT_REGS  (65)
 #  else
-#    define REG_FPSCR       (ARM_CONTEXT_REGS+32) /* Floating point status and control */
+#    define REG_FPSCR       (32) /* Floating point status and control */
 #    define FPU_CONTEXT_REGS  (33)
 #  endif
 #else
 #  define FPU_CONTEXT_REGS  (0)
 #endif
 
-#define REG_R15             (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
-#define REG_CPSR            (REG_R15 + 1)
+#define REG_R13             (FPU_CONTEXT_REGS+0)
+#define REG_R14             (FPU_CONTEXT_REGS+1)
+#define REG_R0              (FPU_CONTEXT_REGS+2)
+#define REG_R1              (FPU_CONTEXT_REGS+3)
+#define REG_R2              (FPU_CONTEXT_REGS+4)
+#define REG_R3              (FPU_CONTEXT_REGS+5)
+#define REG_R4              (FPU_CONTEXT_REGS+6)
+#define REG_R5              (FPU_CONTEXT_REGS+7)
+#define REG_R6              (FPU_CONTEXT_REGS+8)
+#define REG_R7              (FPU_CONTEXT_REGS+9)
+#define REG_R8              (FPU_CONTEXT_REGS+10)
+#define REG_R9              (FPU_CONTEXT_REGS+11)
+#define REG_R10             (FPU_CONTEXT_REGS+12)
+#define REG_R11             (FPU_CONTEXT_REGS+13)
+#define REG_R12             (FPU_CONTEXT_REGS+14)
+#define REG_R15             (FPU_CONTEXT_REGS+15)
+#define REG_CPSR            (FPU_CONTEXT_REGS+16)
+
+#define ARM_CONTEXT_REGS    (17)
 
 /* The total number of registers saved by software */
 
-#define XCPTCONTEXT_REGS    (REG_CPSR + 1)
+#define XCPTCONTEXT_REGS    (FPU_CONTEXT_REGS + ARM_CONTEXT_REGS)
 #define XCPTCONTEXT_SIZE    (4 * XCPTCONTEXT_REGS)
 
 /* Friendly register names */
diff --git a/arch/arm/include/armv7-r/irq.h b/arch/arm/include/armv7-r/irq.h
index b8aaa2c8a8c..8fe73706a4a 100644
--- a/arch/arm/include/armv7-r/irq.h
+++ b/arch/arm/include/armv7-r/irq.h
@@ -51,24 +51,6 @@
  * individual registers in the xcp.regs array:
  */
 
-#define REG_R0              (0)
-#define REG_R1              (1)
-#define REG_R2              (2)
-#define REG_R3              (3)
-#define REG_R4              (4)
-#define REG_R5              (5)
-#define REG_R6              (6)
-#define REG_R7              (7)
-#define REG_R8              (8)
-#define REG_R9              (9)
-#define REG_R10             (10)
-#define REG_R11             (11)
-#define REG_R12             (12)
-#define REG_R13             (13)
-#define REG_R14             (14)
-
-#define ARM_CONTEXT_REGS    (15)
-
 /* 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
@@ -85,87 +67,104 @@
  */
 
 #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_D0            (0)  /* D0 */
+#  define REG_S0            (0)  /* S0 */
+#  define REG_S1            (1)  /* S1 */
+#  define REG_D1            (2)  /* D1 */
+#  define REG_S2            (2)  /* S2 */
+#  define REG_S3            (3)  /* S3 */
+#  define REG_D2            (4)  /* D2 */
+#  define REG_S4            (4)  /* S4 */
+#  define REG_S5            (5)  /* S5 */
+#  define REG_D3            (6)  /* D3 */
+#  define REG_S6            (6)  /* S6 */
+#  define REG_S7            (7)  /* S7 */
+#  define REG_D4            (8)  /* D4 */
+#  define REG_S8            (8)  /* S8 */
+#  define REG_S9            (9)  /* S9 */
+#  define REG_D5            (10) /* D5 */
+#  define REG_S10           (10) /* S10 */
+#  define REG_S11           (11) /* S11 */
+#  define REG_D6            (12) /* D6 */
+#  define REG_S12           (12) /* S12 */
+#  define REG_S13           (13) /* S13 */
+#  define REG_D7            (14) /* D7 */
+#  define REG_S14           (14) /* S14 */
+#  define REG_S15           (15) /* S15 */
+#  define REG_D8            (16) /* D8 */
+#  define REG_S16           (16) /* S16 */
+#  define REG_S17           (17) /* S17 */
+#  define REG_D9            (18) /* D9 */
+#  define REG_S18           (18) /* S18 */
+#  define REG_S19           (19) /* S19 */
+#  define REG_D10           (20) /* D10 */
+#  define REG_S20           (20) /* S20 */
+#  define REG_S21           (21) /* S21 */
+#  define REG_D11           (22) /* D11 */
+#  define REG_S22           (22) /* S22 */
+#  define REG_S23           (23) /* S23 */
+#  define REG_D12           (24) /* D12 */
+#  define REG_S24           (24) /* S24 */
+#  define REG_S25           (25) /* S25 */
+#  define REG_D13           (26) /* D13 */
+#  define REG_S26           (26) /* S26 */
+#  define REG_S27           (27) /* S27 */
+#  define REG_D14           (28) /* D14 */
+#  define REG_S28           (28) /* S28 */
+#  define REG_S29           (29) /* S29 */
+#  define REG_D15           (30) /* D15 */
+#  define REG_S30           (30) /* S30 */
+#  define REG_S31           (31) /* S31 */
 #  ifdef CONFIG_ARM_HAVE_DPFPU32
-#    define REG_D16         (ARM_CONTEXT_REGS+32) /* D16 */
-#    define REG_D17         (ARM_CONTEXT_REGS+34) /* D17 */
-#    define REG_D18         (ARM_CONTEXT_REGS+36) /* D18 */
-#    define REG_D19         (ARM_CONTEXT_REGS+38) /* D19 */
-#    define REG_D20         (ARM_CONTEXT_REGS+40) /* D20 */
-#    define REG_D21         (ARM_CONTEXT_REGS+42) /* D21 */
-#    define REG_D22         (ARM_CONTEXT_REGS+44) /* D22 */
-#    define REG_D23         (ARM_CONTEXT_REGS+46) /* D23 */
-#    define REG_D24         (ARM_CONTEXT_REGS+48) /* D24 */
-#    define REG_D25         (ARM_CONTEXT_REGS+50) /* D25 */
-#    define REG_D26         (ARM_CONTEXT_REGS+52) /* D26 */
-#    define REG_D27         (ARM_CONTEXT_REGS+54) /* D27 */
-#    define REG_D28         (ARM_CONTEXT_REGS+56) /* D28 */
-#    define REG_D29         (ARM_CONTEXT_REGS+58) /* D29 */
-#    define REG_D30         (ARM_CONTEXT_REGS+60) /* D30 */
-#    define REG_D31         (ARM_CONTEXT_REGS+62) /* D31 */
-#    define REG_FPSCR       (ARM_CONTEXT_REGS+64) /* Floating point status and control */
+#    define REG_D16         (32) /* D16 */
+#    define REG_D17         (34) /* D17 */
+#    define REG_D18         (36) /* D18 */
+#    define REG_D19         (38) /* D19 */
+#    define REG_D20         (40) /* D20 */
+#    define REG_D21         (42) /* D21 */
+#    define REG_D22         (44) /* D22 */
+#    define REG_D23         (46) /* D23 */
+#    define REG_D24         (48) /* D24 */
+#    define REG_D25         (50) /* D25 */
+#    define REG_D26         (52) /* D26 */
+#    define REG_D27         (54) /* D27 */
+#    define REG_D28         (56) /* D28 */
+#    define REG_D29         (58) /* D29 */
+#    define REG_D30         (60) /* D30 */
+#    define REG_D31         (62) /* D31 */
+#    define REG_FPSCR       (64) /* Floating point status and control */
 #    define FPU_CONTEXT_REGS  (65)
 #  else
-#    define REG_FPSCR       (ARM_CONTEXT_REGS+32) /* Floating point status and control */
+#    define REG_FPSCR       (32) /* Floating point status and control */
 #    define FPU_CONTEXT_REGS  (33)
 #  endif
 #else
 #  define FPU_CONTEXT_REGS  (0)
 #endif
 
-#define REG_R15             (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
-#define REG_CPSR            (REG_R15 + 1)
+#define REG_R13             (FPU_CONTEXT_REGS+0)
+#define REG_R14             (FPU_CONTEXT_REGS+1)
+#define REG_R0              (FPU_CONTEXT_REGS+2)
+#define REG_R1              (FPU_CONTEXT_REGS+3)
+#define REG_R2              (FPU_CONTEXT_REGS+4)
+#define REG_R3              (FPU_CONTEXT_REGS+5)
+#define REG_R4              (FPU_CONTEXT_REGS+6)
+#define REG_R5              (FPU_CONTEXT_REGS+7)
+#define REG_R6              (FPU_CONTEXT_REGS+8)
+#define REG_R7              (FPU_CONTEXT_REGS+9)
+#define REG_R8              (FPU_CONTEXT_REGS+10)
+#define REG_R9              (FPU_CONTEXT_REGS+11)
+#define REG_R10             (FPU_CONTEXT_REGS+12)
+#define REG_R11             (FPU_CONTEXT_REGS+13)
+#define REG_R12             (FPU_CONTEXT_REGS+14)
+#define REG_R15             (FPU_CONTEXT_REGS+15)
+#define REG_CPSR            (FPU_CONTEXT_REGS+16)
+
+#define ARM_CONTEXT_REGS    (17)
 
 /* The total number of registers saved by software */
 
-#define XCPTCONTEXT_REGS    (REG_CPSR + 1)
+#define XCPTCONTEXT_REGS    (FPU_CONTEXT_REGS + ARM_CONTEXT_REGS)
 #define XCPTCONTEXT_SIZE    (4 * XCPTCONTEXT_REGS)
 
 /* Friendly register names */
diff --git a/arch/arm/src/armv7-a/arm_vectors.S b/arch/arm/src/armv7-a/arm_vectors.S
index 430ce50645b..9cc6fe4d94e 100644
--- a/arch/arm/src/armv7-a/arm_vectors.S
+++ b/arch/arm/src/armv7-a/arm_vectors.S
@@ -102,19 +102,18 @@
 	 * s0, s1, ... in increasing address order.
 	 */
 
-#ifdef CONFIG_ARM_DPFPU32
-	vstmia.64	\out!, {d0-d15}			/* Save the full FP context */
-	vstmia.64	\out!, {d16-d31}
-#else
-	vstmia		\out!, {s0-s31}			/* Save the full FP context */
-#endif
-
-	/* Store the floating point control and status register.  At the end of the
-	 * vstmia, r1 will point to the FPSCR storage location.
-	 */
+	/* Store the floating point control and status register. */
 
 	vmrs		\tmp, fpscr			/* Fetch the FPSCR */
-	str		\tmp, [\out], #4		/* Save the floating point control and status register */
+	str		\tmp, [\out, #-4]! 		/* Save the floating point control and status register */
+
+#ifdef CONFIG_ARM_DPFPU32
+	vstmdb.64	\out!, {d16-d31}		/* Save the full FP context */
+	vstmdb.64	\out!, {d0-d15}
+#else
+	vstmdb		\out!, {s0-s31}			/* Save the full FP context */
+#endif
+
 	.endm
 #endif
 
@@ -175,7 +174,7 @@ arm_vectorirq:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #4
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	/* Switch to SYS mode */
 
@@ -189,24 +188,19 @@ arm_vectorirq:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the IRQ handler with interrupts disabled. */
@@ -214,20 +208,15 @@ arm_vectorirq:
 	mov		fp, #0				/* Init frame pointer */
 	mov		r0, sp				/* Get r0=xcp */
 
+	mov		r4, sp				/* Save the SP in a preserved register */
+
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 	/* Call arm_decodeirq() on the interrupt stack */
 
 	setirqstack	r1, r3				/* SP = interrupt stack top */
-	str		r0, [sp, #-4]!			/* Save the xcp address at SP-4 then update SP */
-	mov		r4, sp				/* Save the SP in a preserved register */
-	bic		sp, sp, #7			/* Force 8-byte alignment */
-	bl		arm_decodeirq			/* Call the handler */
-	ldr		sp, [r4]			/* Restore the user stack pointer */
 #else
 	/* Call arm_decodeirq() on the user stack */
 
-	mov		r4, sp				/* Save the SP in a preserved register */
-
 	/* If the interrupt stack is disabled, reserve xcpcontext to ensure
 	 * that signal processing can have a separate xcpcontext to handle
 	 * signal context (reference: arm_schedulesigaction.c):
@@ -239,17 +228,16 @@ arm_vectorirq:
 	 */
 
 	sub		sp, sp, #XCPTCONTEXT_SIZE	/* Reserve signal context */
+#endif
 
 	bic		sp, sp, #7			/* Force 8-byte alignment */
 	bl		arm_decodeirq			/* Call the handler */
 	mov		sp, r4				/* Restore the possibly unaligned stack pointer */
-#endif
 
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back IRQ mode and return with shadow SPSR */
@@ -264,13 +252,12 @@ arm_vectorirq:
 
 	/* Life is simple when everything is IRQ mode */
 
-	mov		r14, r0				/* (IRQ) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 
 #if !defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
@@ -297,7 +284,7 @@ arm_vectorsvc:
 
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	/* Switch to SYS mode */
 
@@ -311,24 +298,19 @@ arm_vectorsvc:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the SVC handler with interrupts disabled.
@@ -338,20 +320,15 @@ arm_vectorsvc:
 	mov		fp, #0				/* Init frame pointer */
 	mov		r0, sp				/* Get r0=xcp */
 
+	mov		r4, sp				/* Save the SP in a preserved register */
+
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 	/* Call arm_syscall() on the interrupt stack */
 
 	setirqstack	r1, r3				/* SP = interrupt stack top */
-	str		r0, [sp, #-4]!			/* Save the xcp address at SP-4 then update SP */
-	mov		r4, sp				/* Save the SP in a preserved register */
-	bic		sp, sp, #7			/* Force 8-byte alignment */
-	bl		arm_syscall			/* Call the handler */
-	ldr		sp, [r4]			/* Restore the user stack pointer */
 #else
 	/* Call arm_syscall() on the user stack */
 
-	mov		r4, sp				/* Save the SP in a preserved register */
-
 	/* If the interrupt stack is disabled, reserve xcpcontext to ensure
 	 * that signal processing can have a separate xcpcontext to handle
 	 * signal context (reference: arm_schedulesigaction.c):
@@ -363,17 +340,16 @@ arm_vectorsvc:
 	 */
 
 	sub		sp, sp, #XCPTCONTEXT_SIZE	/* Reserve signal context */
+#endif
 
 	bic		sp, sp, #7			/* Force 8-byte alignment */
 	bl		arm_syscall			/* Call the handler */
 	mov		sp, r4				/* Restore the possibly unaligned stack pointer */
-#endif
 
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back SVC mode and return with shadow SPSR */
@@ -388,13 +364,12 @@ arm_vectorsvc:
 
 	/* Life is simple when everything is SVC mode */
 
-	mov		r14, r0				/* (SVC) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectorsvc, . - arm_vectorsvc
 
@@ -420,7 +395,7 @@ arm_vectordata:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #8
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	/* Switch to SYS mode */
 
@@ -434,24 +409,19 @@ arm_vectordata:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the data abort handler with interrupts disabled.
@@ -470,8 +440,7 @@ arm_vectordata:
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back ABT mode and return with shadow SPSR */
@@ -486,13 +455,12 @@ arm_vectordata:
 
 	/* Life is simple when everything is ABT mode */
 
-	mov		r14, r0				/* (ABT) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectordata, . - arm_vectordata
 
@@ -518,7 +486,7 @@ arm_vectorprefetch:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #4
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
@@ -526,24 +494,19 @@ arm_vectorprefetch:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the prefetch abort handler with interrupts disabled.
@@ -562,8 +525,7 @@ arm_vectorprefetch:
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back ABT mode and return with shadow SPSR */
@@ -578,13 +540,12 @@ arm_vectorprefetch:
 
 	/* Life is simple when everything is ABT mode */
 
-	mov		r14, r0				/* (ABT) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectorprefetch, . - arm_vectorprefetch
 
@@ -607,7 +568,7 @@ arm_vectorundefinsn:
 
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
@@ -615,24 +576,19 @@ arm_vectorundefinsn:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the undef insn handler with interrupts disabled.
@@ -649,8 +605,7 @@ arm_vectorundefinsn:
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back UND mode and return with shadow SPSR */
@@ -665,13 +620,12 @@ arm_vectorundefinsn:
 
 	/* Life is simple when everything is UND mode */
 
-	mov		r14, r0				/* (UND) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectorundefinsn, . - arm_vectorundefinsn
 
@@ -698,7 +652,7 @@ arm_vectorfiq:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #4
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
@@ -706,24 +660,19 @@ arm_vectorfiq:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the FIQ handler with interrupts disabled. */
@@ -731,29 +680,22 @@ arm_vectorfiq:
 	mov		fp, #0				/* Init frame pointer */
 	mov		r0, sp				/* Get r0=xcp */
 
+	mov		r4, sp				/* Save the SP in a preserved register */
+
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 	/* Call arm_decodefiq() on the interrupt stack */
 
-	setfiqstack	r1, r4				/* SP = interrupt stack top */
-	str		r0, [sp, #-4]!			/* Save the xcp address at SP-4 then update SP */
-	mov		r4, sp				/* Save the SP in a preserved register */
-	bic		sp, sp, #7			/* Force 8-byte alignment */
-	bl		arm_decodefiq			/* Call the handler */
-	ldr		sp, [r4]			/* Restore the user stack pointer */
-#else
-	/* Call arm_decodefiq() on the user stack */
+	setfiqstack	r1, r3				/* SP = interrupt stack top */
+#endif
 
-	mov		r4, sp				/* Save the SP in a preserved register */
 	bic		sp, sp, #7			/* Force 8-byte alignment */
 	bl		arm_decodefiq			/* Call the handler */
 	mov		sp, r4				/* Restore the possibly unaligned stack pointer */
-#endif
 
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back FIQ mode and return with shadow SPSR */
@@ -768,13 +710,14 @@ arm_vectorfiq:
 
 	/* Life is simple when everything is FIQ mode */
 
-	mov		r14, r0				/* (FIQ) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r7}			/* Restore common r0-r7 */
-	ldmia		r14, {r8-r14}^			/* Restore user mode r8-r14 */
+	ldmia		r14, {r8-r12}^			/* Restore user mode r8-r12 */
+	add		r14, r14, #20
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 
 #if !defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
diff --git a/arch/arm/src/armv7-r/arm_vectors.S b/arch/arm/src/armv7-r/arm_vectors.S
index 25de3c6af7c..5f4fa8604e6 100644
--- a/arch/arm/src/armv7-r/arm_vectors.S
+++ b/arch/arm/src/armv7-r/arm_vectors.S
@@ -56,19 +56,18 @@
 	 * s0, s1, ... in increasing address order.
 	 */
 
-#ifdef CONFIG_ARM_DPFPU32
-	vstmia.64	\out!, {d0-d15}			/* Save the full FP context */
-	vstmia.64	\out!, {d16-d31}
-#else
-	vstmia		\out!, {s0-s31}			/* Save the full FP context */
-#endif
-
-	/* Store the floating point control and status register.  At the end of the
-	 * vstmia, r1 will point to the FPSCR storage location.
-	 */
+	/* Store the floating point control and status register. */
 
 	vmrs		\tmp, fpscr			/* Fetch the FPSCR */
-	str		\tmp, [\out], #4		/* Save the floating point control and status register */
+	str		\tmp, [\out, #-4]! 		/* Save the floating point control and status register */
+
+#ifdef CONFIG_ARM_DPFPU32
+	vstmdb.64	\out!, {d16-d31}		/* Save the full FP context */
+	vstmdb.64	\out!, {d0-d15}
+#else
+	vstmdb		\out!, {s0-s31}			/* Save the full FP context */
+#endif
+
 	.endm
 #endif
 
@@ -129,7 +128,7 @@ arm_vectorirq:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #4
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	/* Switch to SYS mode */
 
@@ -143,24 +142,19 @@ arm_vectorirq:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the IRQ handler with interrupts disabled. */
@@ -168,20 +162,15 @@ arm_vectorirq:
 	mov		fp, #0				/* Init frame pointer */
 	mov		r0, sp				/* Get r0=xcp */
 
+	mov		r4, sp				/* Save the SP in a preserved register */
+
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 	/* Call arm_decodeirq() on the interrupt stack */
 
 	ldr		sp, .Lirqstacktop		/* SP = interrupt stack top */
-	str		r0, [sp, #-4]!			/* Save the xcp address at SP-4 then update SP */
-	mov		r4, sp				/* Save the SP in a preserved register */
-	bic		sp, sp, #7			/* Force 8-byte alignment */
-	bl		arm_decodeirq			/* Call the handler */
-	ldr		sp, [r4]			/* Restore the user stack pointer */
 #else
 	/* Call arm_decodeirq() on the user stack */
 
-	mov		r4, sp				/* Save the SP in a preserved register */
-
 	/* If the interrupt stack is disabled, reserve xcpcontext to ensure
 	 * that signal processing can have a separate xcpcontext to handle
 	 * signal context (reference: arm_schedulesigaction.c):
@@ -193,17 +182,16 @@ arm_vectorirq:
 	 */
 
 	sub		sp, sp, #XCPTCONTEXT_SIZE	/* Reserve signal context */
+#endif
 
 	bic		sp, sp, #7			/* Force 8-byte alignment */
 	bl		arm_decodeirq			/* Call the handler */
 	mov		sp, r4				/* Restore the possibly unaligned stack pointer */
-#endif
 
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back IRQ mode and return with shadow SPSR */
@@ -218,13 +206,12 @@ arm_vectorirq:
 
 	/* Life is simple when everything is IRQ mode */
 
-	mov		r14, r0				/* (IRQ) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
@@ -251,7 +238,7 @@ arm_vectorsvc:
 
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	/* Switch to SYS mode */
 
@@ -265,24 +252,19 @@ arm_vectorsvc:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the SVC handler with interrupts disabled.
@@ -292,20 +274,15 @@ arm_vectorsvc:
 	mov		fp, #0				/* Init frame pointer */
 	mov		r0, sp				/* Get r0=xcp */
 
+	mov		r4, sp				/* Save the SP in a preserved register */
+
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 	/* Call arm_syscall() on the interrupt stack */
 
 	ldr		sp, .Lirqstacktop		/* SP = interrupt stack top */
-	str		r0, [sp, #-4]!			/* Save the xcp address at SP-4 then update SP */
-	mov		r4, sp				/* Save the SP in a preserved register */
-	bic		sp, sp, #7			/* Force 8-byte alignment */
-	bl		arm_syscall			/* Call the handler */
-	ldr		sp, [r4]			/* Restore the user stack pointer */
 #else
 	/* Call arm_syscall() on the user stack */
 
-	mov		r4, sp				/* Save the SP in a preserved register */
-
 	/* If the interrupt stack is disabled, reserve xcpcontext to ensure
 	 * that signal processing can have a separate xcpcontext to handle
 	 * signal context (reference: arm_schedulesigaction.c):
@@ -317,17 +294,16 @@ arm_vectorsvc:
 	 */
 
 	sub		sp, sp, #XCPTCONTEXT_SIZE	/* Reserve signal context */
+#endif
 
 	bic		sp, sp, #7			/* Force 8-byte alignment */
 	bl		arm_syscall			/* Call the handler */
 	mov		sp, r4				/* Restore the possibly unaligned stack pointer */
-#endif
 
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back SVC mode and return with shadow SPSR */
@@ -342,13 +318,12 @@ arm_vectorsvc:
 
 	/* Life is simple when everything is SVC mode */
 
-	mov		r14, r0				/* (SVC) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectorsvc, . - arm_vectorsvc
 
@@ -374,7 +349,7 @@ arm_vectordata:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #8
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	/* Switch to SYS mode */
 
@@ -388,24 +363,19 @@ arm_vectordata:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the data abort handler with interrupts disabled.
@@ -424,8 +394,7 @@ arm_vectordata:
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back ABT mode and return with shadow SPSR */
@@ -440,13 +409,12 @@ arm_vectordata:
 
 	/* Life is simple when everything is ABT mode */
 
-	mov		r14, r0				/* (ABT) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectordata, . - arm_vectordata
 
@@ -472,7 +440,7 @@ arm_vectorprefetch:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #4
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
@@ -480,24 +448,19 @@ arm_vectorprefetch:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the prefetch abort handler with interrupts disabled.
@@ -516,8 +479,7 @@ arm_vectorprefetch:
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back ABT mode and return with shadow SPSR */
@@ -532,13 +494,12 @@ arm_vectorprefetch:
 
 	/* Life is simple when everything is ABT mode */
 
-	mov		r14, r0				/* (ABT) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectorprefetch, . - arm_vectorprefetch
 
@@ -561,7 +522,7 @@ arm_vectorundefinsn:
 
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
@@ -569,24 +530,19 @@ arm_vectorundefinsn:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the undef insn handler with interrupts disabled.
@@ -603,8 +559,7 @@ arm_vectorundefinsn:
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back UND mode and return with shadow SPSR */
@@ -619,13 +574,12 @@ arm_vectorundefinsn:
 
 	/* Life is simple when everything is UND mode */
 
-	mov		r14, r0				/* (UND) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
-	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 	.size	arm_vectorundefinsn, . - arm_vectorundefinsn
 
@@ -652,7 +606,7 @@ arm_vectorfiq:
 	/* Save the LR and SPSR onto the SYS mode stack before switch. */
 
 	sub		lr, lr, #4
-	srsdb		sp, #PSR_MODE_SYS
+	srsdb		sp!, #PSR_MODE_SYS
 
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
@@ -660,24 +614,19 @@ arm_vectorfiq:
 	 * and store r0-r12 into the frame.
 	 */
 
-	sub		sp, sp, #XCPTCONTEXT_SIZE
-	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
+	stmdb		sp!, {r0-r12}			/* Save the SYS mode regs */
 
-	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
+	/* Get the correct values of USR/SYS r13(sp) in r1 and
+	 * save r13 and r14 into the frame.
+	 */
 
-	add		r1, sp, #XCPTCONTEXT_SIZE
-	mov		r2, r14
-
-	/* Save r13(sp), r14(lr) */
-
-	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r2}
+	add		r1, sp, #(XCPTCONTEXT_SIZE-4*REG_R0)
+	stmdb		sp!, {r1, r14}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
 
-	add		r0, sp, #(4*REG_S0)		/* R1=Address of FP register storage */
-	savefpu		r0, r1
+	savefpu		sp, r1
 #endif
 
 	/* Then call the FIQ handler with interrupts disabled. */
@@ -685,29 +634,22 @@ arm_vectorfiq:
 	mov		fp, #0				/* Init frame pointer */
 	mov		r0, sp				/* Get r0=xcp */
 
+	mov		r4, sp				/* Save the SP in a preserved register */
+
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 	/* Call arm_decodefiq() on the interrupt stack */
 
 	ldr		sp, .Lfiqstacktop		/* SP = interrupt stack top */
-	str		r0, [sp, #-4]!			/* Save the xcp address at SP-4 then update SP */
-	mov		r4, sp				/* Save the SP in a preserved register */
-	bic		sp, sp, #7			/* Force 8-byte alignment */
-	bl		arm_decodefiq			/* Call the handler */
-	ldr		sp, [r4]			/* Restore the user stack pointer */
-#else
-	/* Call arm_decodefiq() on the user stack */
+#endif
 
-	mov		r4, sp				/* Save the SP in a preserved register */
 	bic		sp, sp, #7			/* Force 8-byte alignment */
 	bl		arm_decodefiq			/* Call the handler */
 	mov		sp, r4				/* Restore the possibly unaligned stack pointer */
-#endif
 
 #ifdef CONFIG_ARCH_FPU
 	/* Restore the state of the floating point registers. */
 
-	add		r1, r0, #(4*REG_S0)		/* R1=Address of FP register storage */
-	restorefpu	r1, r2
+	restorefpu	r0, r2
 #endif
 
 	/* Switch back FIQ mode and return with shadow SPSR */
@@ -722,13 +664,14 @@ arm_vectorfiq:
 
 	/* Life is simple when everything is FIQ mode */
 
-	mov		r14, r0				/* (FIQ) r14=Register storage area */
+	ldmia		r0, {r13, r14}^			/* Restore user mode r13 and r14 */
+	add		r14, r0, #8
 	ldmia		r14!, {r0-r7}			/* Restore common r0-r7 */
-	ldmia		r14, {r8-r14}^			/* Restore user mode r8-r14 */
+	ldmia		r14, {r8-r12}^			/* Restore user mode r8-r12 */
+	add		r14, r14, #20
 
 	/* Restore the CPSR, SYS mode registers and return. */
 
-	add		r14, r14, #(4*(REG_R15-REG_R13))
 	rfeia		r14
 
 #if CONFIG_ARCH_INTERRUPTSTACK > 7