diff --git a/arch/arm/include/armv7-a/irq.h b/arch/arm/include/armv7-a/irq.h
index abd083346b6..098ff0e697b 100644
--- a/arch/arm/include/armv7-a/irq.h
+++ b/arch/arm/include/armv7-a/irq.h
@@ -66,10 +66,8 @@
 #define REG_R12             (12)
 #define REG_R13             (13)
 #define REG_R14             (14)
-#define REG_R15             (15)
-#define REG_CPSR            (16)
 
-#define ARM_CONTEXT_REGS    (17)
+#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
@@ -162,9 +160,12 @@
 #  define FPU_CONTEXT_REGS  (0)
 #endif
 
+#define REG_R15             (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
+#define REG_CPSR            (REG_R15 + 1)
+
 /* The total number of registers saved by software */
 
-#define XCPTCONTEXT_REGS    (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
+#define XCPTCONTEXT_REGS    (REG_CPSR + 1)
 #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 64faba4f183..b8aaa2c8a8c 100644
--- a/arch/arm/include/armv7-r/irq.h
+++ b/arch/arm/include/armv7-r/irq.h
@@ -66,10 +66,8 @@
 #define REG_R12             (12)
 #define REG_R13             (13)
 #define REG_R14             (14)
-#define REG_R15             (15)
-#define REG_CPSR            (16)
 
-#define ARM_CONTEXT_REGS    (17)
+#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
@@ -162,9 +160,12 @@
 #  define FPU_CONTEXT_REGS  (0)
 #endif
 
+#define REG_R15             (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
+#define REG_CPSR            (REG_R15 + 1)
+
 /* The total number of registers saved by software */
 
-#define XCPTCONTEXT_REGS    (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
+#define XCPTCONTEXT_REGS    (REG_CPSR + 1)
 #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 178b6cca7c3..430ce50645b 100644
--- a/arch/arm/src/armv7-a/arm_vectors.S
+++ b/arch/arm/src/armv7-a/arm_vectors.S
@@ -171,6 +171,12 @@
 	.type	arm_vectorirq, %function
 
 arm_vectorirq:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #4
+	srsdb		sp, #PSR_MODE_SYS
+
 	/* Switch to SYS mode */
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -186,24 +192,15 @@ arm_vectorirq:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_IRQ			/* Switch back IRQ mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #4
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -265,18 +262,16 @@ arm_vectorirq:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, IRQ mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is IRQ mode */
 
 	mov		r14, r0				/* (IRQ) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (IRQ) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* 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
 .Lirqstacktop:
@@ -299,6 +294,11 @@ arm_vectorirq:
 	.type	arm_vectorsvc, %function
 
 arm_vectorsvc:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	srsdb		sp, #PSR_MODE_SYS
+
 	/* Switch to SYS mode */
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -314,24 +314,15 @@ arm_vectorsvc:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_SVC			/* Switch back SVC mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	mov		r3, r14				/* Save r14 as the PC as well */
-	mrs		r4, spsr			/* Get the saved CPSR */
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -395,18 +386,16 @@ arm_vectorsvc:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, SYS mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is SVC mode */
 
 	mov		r14, r0				/* (SVC) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (SVC) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectorsvc, . - arm_vectorsvc
 
 	.align	5
@@ -427,6 +416,12 @@ arm_vectorsvc:
 	.type	arm_vectordata, %function
 
 arm_vectordata:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #8
+	srsdb		sp, #PSR_MODE_SYS
+
 	/* Switch to SYS mode */
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -442,24 +437,15 @@ arm_vectordata:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_ABT			/* Switch back ABT mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #8
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -498,18 +484,16 @@ arm_vectordata:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, ABT mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is ABT mode */
 
 	mov		r14, r0				/* (ABT) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (ABT) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectordata, . - arm_vectordata
 
 	.align	5
@@ -530,6 +514,12 @@ arm_vectordata:
 	.type	arm_vectorprefetch, %function
 
 arm_vectorprefetch:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #4
+	srsdb		sp, #PSR_MODE_SYS
+
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
 	/* Create a context structure.  First set aside a stack frame
@@ -539,24 +529,15 @@ arm_vectorprefetch:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_ABT			/* Switch back ABT mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #4
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -595,18 +576,16 @@ arm_vectorprefetch:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, ABT mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is ABT mode */
 
 	mov		r14, r0				/* (ABT) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (ABT) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectorprefetch, . - arm_vectorprefetch
 
 	.align	5
@@ -625,6 +604,11 @@ arm_vectorprefetch:
 	.type	arm_vectorundefinsn, %function
 
 arm_vectorundefinsn:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	srsdb		sp, #PSR_MODE_SYS
+
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
 	/* Create a context structure.  First set aside a stack frame
@@ -634,24 +618,15 @@ arm_vectorundefinsn:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_UND			/* Switch back UND mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	mov		r3, lr
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -688,18 +663,16 @@ arm_vectorundefinsn:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, UND mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is UND mode */
 
 	mov		r14, r0				/* (UND) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (UND) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectorundefinsn, . - arm_vectorundefinsn
 
 	.align	5
@@ -721,6 +694,12 @@ arm_vectorundefinsn:
 
 arm_vectorfiq:
 #ifdef CONFIG_ARMV7A_DECODEFIQ
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #4
+	srsdb		sp, #PSR_MODE_SYS
+
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
 	/* Create a context structure.  First set aside a stack frame
@@ -730,24 +709,15 @@ arm_vectorfiq:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_FIQ			/* Switch back FIQ mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #4
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -796,18 +766,16 @@ arm_vectorfiq:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, FIQ mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is FIQ mode */
 
 	mov		r14, r0				/* (FIQ) r14=Register storage area */
 	ldmia		r14!, {r0-r7}			/* Restore common r0-r7 */
 	ldmia		r14, {r8-r14}^			/* Restore user mode r8-r14 */
-	add		r14, r14, #(4*7)		/* (FIQ) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* 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
 .Lfiqstacktop:
diff --git a/arch/arm/src/armv7-r/arm_vectors.S b/arch/arm/src/armv7-r/arm_vectors.S
index 6939900ad67..25de3c6af7c 100644
--- a/arch/arm/src/armv7-r/arm_vectors.S
+++ b/arch/arm/src/armv7-r/arm_vectors.S
@@ -125,6 +125,12 @@
 	.type	arm_vectorirq, %function
 
 arm_vectorirq:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #4
+	srsdb		sp, #PSR_MODE_SYS
+
 	/* Switch to SYS mode */
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -140,24 +146,15 @@ arm_vectorirq:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_IRQ			/* Switch back IRQ mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #4
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -219,18 +216,16 @@ arm_vectorirq:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, IRQ mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is IRQ mode */
 
 	mov		r14, r0				/* (IRQ) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (IRQ) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 .Lirqstacktop:
@@ -253,6 +248,11 @@ arm_vectorirq:
 	.type	arm_vectorsvc, %function
 
 arm_vectorsvc:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	srsdb		sp, #PSR_MODE_SYS
+
 	/* Switch to SYS mode */
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -268,24 +268,15 @@ arm_vectorsvc:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_SVC			/* Switch back SVC mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	mov		r3, r14				/* Save r14 as the PC as well */
-	mrs		r4, spsr			/* Get the saved CPSR */
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -349,18 +340,16 @@ arm_vectorsvc:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, SYS mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is SVC mode */
 
 	mov		r14, r0				/* (SVC) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (SVC) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectorsvc, . - arm_vectorsvc
 
 	.align	5
@@ -381,6 +370,12 @@ arm_vectorsvc:
 	.type	arm_vectordata, %function
 
 arm_vectordata:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #8
+	srsdb		sp, #PSR_MODE_SYS
+
 	/* Switch to SYS mode */
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -396,24 +391,15 @@ arm_vectordata:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_ABT			/* Switch back ABT mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #8
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -452,18 +438,16 @@ arm_vectordata:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, ABT mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is ABT mode */
 
 	mov		r14, r0				/* (ABT) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (ABT) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectordata, . - arm_vectordata
 
 	.align	5
@@ -484,6 +468,12 @@ arm_vectordata:
 	.type	arm_vectorprefetch, %function
 
 arm_vectorprefetch:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #4
+	srsdb		sp, #PSR_MODE_SYS
+
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
 	/* Create a context structure.  First set aside a stack frame
@@ -493,24 +483,15 @@ arm_vectorprefetch:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_ABT			/* Switch back ABT mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #4
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -549,18 +530,16 @@ arm_vectorprefetch:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, ABT mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is ABT mode */
 
 	mov		r14, r0				/* (ABT) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (ABT) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectorprefetch, . - arm_vectorprefetch
 
 	.align	5
@@ -579,6 +558,11 @@ arm_vectorprefetch:
 	.type	arm_vectorundefinsn, %function
 
 arm_vectorundefinsn:
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	srsdb		sp, #PSR_MODE_SYS
+
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
 	/* Create a context structure.  First set aside a stack frame
@@ -588,24 +572,15 @@ arm_vectorundefinsn:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_UND			/* Switch back UND mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	mov		r3, lr
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -642,18 +617,16 @@ arm_vectorundefinsn:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, UND mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is UND mode */
 
 	mov		r14, r0				/* (UND) r14=Register storage area */
 	ldmia		r14!, {r0-r12}			/* Restore common r0-r12 */
 	ldmia		r14, {r13, r14}^		/* Restore user mode r13/r14 */
-	add		r14, r14, #(4*2)		/* (UND) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 	.size	arm_vectorundefinsn, . - arm_vectorundefinsn
 
 	.align	5
@@ -675,6 +648,12 @@ arm_vectorundefinsn:
 
 arm_vectorfiq:
 #ifdef CONFIG_ARMV7R_DECODEFIQ
+
+	/* Save the LR and SPSR onto the SYS mode stack before switch. */
+
+	sub		lr, lr, #4
+	srsdb		sp, #PSR_MODE_SYS
+
 	cpsid		if, #PSR_MODE_SYS		/* Switch to SYS mode */
 
 	/* Create a context structure.  First set aside a stack frame
@@ -684,24 +663,15 @@ arm_vectorfiq:
 	sub		sp, sp, #XCPTCONTEXT_SIZE
 	stmia		sp, {r0-r12}			/* Save the SYS mode regs */
 
-	cps		#PSR_MODE_FIQ			/* Switch back FIQ mode */
-
-	/* Get the values for r15(pc) and CPSR in r3 and r4 */
-
-	sub		r3, lr, #4
-	mrs		r4, spsr
-
-	cps		#PSR_MODE_SYS			/* Then switch back to SYS mode */
-
 	/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
 
 	add		r1, sp, #XCPTCONTEXT_SIZE
 	mov		r2, r14
 
-	/* Save r13(sp), r14(lr), r15(pc), and the CPSR */
+	/* Save r13(sp), r14(lr) */
 
 	add		r0, sp, #(4*REG_SP)		/* Offset to pc, cpsr storage */
-	stmia		r0, {r1-r4}
+	stmia		r0, {r1-r2}
 
 #ifdef CONFIG_ARCH_FPU
 	/* Save the state of the floating point registers. */
@@ -750,18 +720,16 @@ arm_vectorfiq:
 	 * context switch is required.
 	 */
 
-	/* Restore the CPSR, FIQ mode registers and return */
-
-	ldr		r1, [r0, #(4*REG_CPSR)]		/* Fetch the return SPSR */
-	msr		spsr_cxsf, r1			/* Set the return mode SPSR */
-
 	/* Life is simple when everything is FIQ mode */
 
 	mov		r14, r0				/* (FIQ) r14=Register storage area */
 	ldmia		r14!, {r0-r7}			/* Restore common r0-r7 */
 	ldmia		r14, {r8-r14}^			/* Restore user mode r8-r14 */
-	add		r14, r14, #(4*7)		/* (FIQ) r14=address of r15 storage */
-	ldmia		r14, {r15}^			/* Return */
+
+	/* Restore the CPSR, SYS mode registers and return. */
+
+	add		r14, r14, #(4*(REG_R15-REG_R13))
+	rfeia		r14
 
 #if CONFIG_ARCH_INTERRUPTSTACK > 7
 .Lfiqstacktop: