diff --git a/arch/arm/src/armv7-a/arm_doirq.c b/arch/arm/src/armv7-a/arm_doirq.c
index fa3e1045823..f13995e9b58 100644
--- a/arch/arm/src/armv7-a/arm_doirq.c
+++ b/arch/arm/src/armv7-a/arm_doirq.c
@@ -51,6 +51,7 @@
 #include "up_internal.h"
 
 #include "group/group.h"
+#include "irq/irq.h"
 #include "gic.h"
 
 /****************************************************************************
@@ -120,6 +121,17 @@ static inline uint32_t *_arm_doirq(int irq, uint32_t *regs)
 
       (void)group_addrenv(NULL);
 #endif
+
+#ifdef CONFIG_SMP
+      /* In the SMP configuration, critical section management uses a
+       * "voting" algorithm with current task on each CPU casting its
+       * "vote" by the state of the TCB irqcount flag.  That irqcount
+       * for the current task on this CPU will be different is a
+       * context switch occurrred.
+       */
+
+      irq_restore_lock();
+#endif
     }
 #endif
 
diff --git a/arch/arm/src/armv7-a/arm_fullcontextrestore.S b/arch/arm/src/armv7-a/arm_fullcontextrestore.S
index 64f74c8a98f..0c0eea4d716 100644
--- a/arch/arm/src/armv7-a/arm_fullcontextrestore.S
+++ b/arch/arm/src/armv7-a/arm_fullcontextrestore.S
@@ -77,6 +77,7 @@ up_fullcontextrestore:
 	 */
 
 #ifdef CONFIG_ARCH_FPU
+
 	/* First, restore the floating point registers.  Lets do this before we
 	 * restore the ARM registers so that we have plenty of registers to
 	 * work with.
@@ -96,9 +97,11 @@ up_fullcontextrestore:
 
 	ldr		r2, [r1], #4			/* Fetch the floating point control and status register */
 	vmsr	fpscr, r2				/* Restore the FPCSR */
+
 #endif
 
 #ifdef CONFIG_BUILD_KERNEL
+
 	/* For the kernel build, we need to be able to transition gracefully
 	 * between kernel- and user-mode tasks.  Here we do that with a system
 	 * call; the system call will execute in kernel mode and but can return
@@ -116,10 +119,73 @@ up_fullcontextrestore:
 	bx		lr						/* Unnecessary ... will not return */
 
 #else
+
 	/* For a flat build, we can do all of this here... Just think of this as
 	 * a longjmp() all on steriods.
 	 */
 
+#ifdef CONFIG_SMP
+
+	/* Recover all registers except for the volatile registers {r0-r3, r12}
+	 * and r14 (lr).
+	 */
+
+	add		r1, r0, #(4*REG_R4)		/* Offset to REG_R2 storage */
+	ldmia	r1, {r4-r11}			/* Recover registers */
+
+	/* Recover the stack pointer (r13) */
+
+	add		sp, r0, #(4*REG_SP)		/* Recover the stack pointer */
+
+	/* Create a stack from to preserve the structure pointer and some
+	 * additional registers.  We need to have everything preserved on the
+	 * stack when irq_restore_lock(0) is called.
+	 */
+
+	sub		sp, sp, #(4*9)			/* Frame for eight registers */
+
+	str		r0, [sp, #(4*0)]		/* Save the structure pointer at the top of the stack */
+	ldr		r1, [r0, #(4*REG_R2)]	/* Fetch the stored r2 value */
+	str		r1, [sp, #(4*1)]		/* Save it in the stack */
+	ldr		r1, [r0, #(4*REG_R3)]	/* Fetch the stored r3 value */
+	str		r1, [sp, #(4*2)]		/* Save it in the stack */
+	ldr		r1, [r0, #(4*REG_R12)]	/* Fetch the stored r12 value */
+	str		r1, [sp, #(4*3)]		/* Save it in the stack */
+	ldr		r1, [r0, #(4*REG_R14)]	/* Fetch the stored r14 value */
+	str		r1, [sp, #(4*4)]		/* Save it in the stack */
+	ldr		r1, [r0, #(4*REG_CPSR)]	/* Fetch the stored CPSR value */
+	str		r1, [sp, #(4*5)]		/* Save it in the stack */
+
+	ldr		r1, [r0, #(4*REG_R0)]	/* Fetch the stored r0 value */
+	str		r1, [sp, #(4*6)]		/* Save it in the stack */
+	ldr		r1, [r0, #(4*REG_R1)]	/* Fetch the stored r1 value */
+	str		r1, [sp, #(4*7)]		/* Save it in the stack */
+	ldr		r1, [r0, #(4*REG_PC)]	/* Fetch the stored pc value */
+	str		r1, [sp, #(4*8)]		/* Save it at the bottom of the frame */
+
+	/* In the SMP configuration, critical section management uses a
+	 * "voting" algorithm with current task on each CPU casting its
+	 * "vote" by the state of the TCB irqcount flag.  That irqcount
+	 * for the current task on this CPU will be different is a
+	 * context switch occurrred.
+	 */
+
+	bl		irq_restore_lock
+
+	/* Recover the structure pointer and most of the volatile structures
+	 * that were saved on the stack.
+	 */
+
+	ldr		r0,  [sp, #(4*0)]		/* Recover the structure pointer at the top of the stack */
+	ldr		r2,  [sp, #(4*1)]		/* Recover R2 */
+	ldr		r3,  [sp, #(4*2)]		/* Recover R3 */
+	ldr		r12, [sp, #(4*3)]		/* Recover R12 */
+	ldr		r14, [sp, #(4*4)]		/* Recover R14 */
+	ldr		r1,  [sp, #(4*5)]		/* Recover the save CPSR in r1 */
+	add		sp,   sp, #(4*6)		/* Discard 6 of the allocated 9 storage locations */
+
+#else
+
 	/* Recover all registers except for r0, r1, R15, and CPSR */
 
 	add		r1, r0, #(4*REG_R2)		/* Offset to REG_R2 storage */
@@ -129,11 +195,16 @@ up_fullcontextrestore:
 
 	sub		sp, sp, #(3*4)			/* Frame for three registers */
 	ldr		r1, [r0, #(4*REG_R0)]	/* Fetch the stored r0 value */
-	str		r1, [sp]				/* Save it at the top of the stack */
+	str		r1, [sp, #(4*0)]		/* Save it at the top of the stack */
 	ldr		r1, [r0, #(4*REG_R1)]	/* Fetch the stored r1 value */
-	str		r1, [sp, #4]			/* Save it in the stack */
+	str		r1, [sp, #(4*1)]		/* Save it in the stack */
 	ldr		r1, [r0, #(4*REG_PC)]	/* Fetch the stored pc value */
-	str		r1, [sp, #8]			/* Save it at the bottom of the frame */
+	str		r1, [sp, #(4*2)]		/* Save it at the bottom of the frame */
+
+	/* Recover the saved CPSR value in r1 */
+
+	ldr		r1, [r0, #(4*REG_CPSR)]	/* Fetch the stored CPSR value */
+#endif
 
 	/* Now we can restore the CPSR.  We wait until we are completely
 	 * finished with the context save data to do this. Restore the CPSR
@@ -142,14 +213,13 @@ up_fullcontextrestore:
 	 * disabled.
 	 */
 
-	ldr		r1, [r0, #(4*REG_CPSR)]	/* Fetch the stored CPSR value */
 	msr		cpsr, r1				/* Set the CPSR */
 
 	/* Now recover r0 and r1 */
 
 	ldr		r0, [sp]
 	ldr		r1, [sp, #4]
-	add		sp, sp, #(2*4)
+	add		sp, sp, #(4*2)
 
 	/* Then return to the address at the stop of the stack,
 	 * destroying the stack frame
diff --git a/arch/arm/src/armv7-a/arm_testset.S b/arch/arm/src/armv7-a/arm_testset.S
index e89cbb3adc5..638736e4d67 100644
--- a/arch/arm/src/armv7-a/arm_testset.S
+++ b/arch/arm/src/armv7-a/arm_testset.S
@@ -75,7 +75,7 @@
  *   This function must be provided via the architecture-specific logoic.
  *
  * Input Parameters:
- *   lock - The address of spinlock object.
+ *   lock - The address of spinlock object (r0).
  *
  * Returned Value:
  *   The spinlock is always locked upon return.  The value of previous value
@@ -84,6 +84,8 @@
  *   obtain the lock) or SP_UNLOCKED if the spinlock was previously unlocked
  *   (meaning that we successfully obtained the lock)
  *
+ * Modifies: r1, r2, and lr
+ *
  ****************************************************************************/
 
 	.globl	up_testset
@@ -98,7 +100,7 @@ up_testset:
 1:
 	ldrexb	r2, [r0]			/* Test if spinlock is locked or not */
 	cmp		r2, r1				/* Already locked? */
-	beq		2f					/* If alrady locked, return SP_LOCKED */
+	beq		2f					/* If already locked, return SP_LOCKED */
 
 	/* Not locked ... attempt to lock it */