diff --git a/arch/arm/src/armv7-a/arm_cpustart.c b/arch/arm/src/armv7-a/arm_cpustart.c
index 7e86312f98e92..b6c69859705a1 100644
--- a/arch/arm/src/armv7-a/arm_cpustart.c
+++ b/arch/arm/src/armv7-a/arm_cpustart.c
@@ -37,6 +37,28 @@
 
 #ifdef CONFIG_SMP
 
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define MODE_FIQ    0x11
+#define MODE_IRQ    0x12
+#define MODE_ABORT  0x17
+#define MODE_UNDEF  0x1B
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* The following variables are allocated in arm_vectors.S */
+
+extern uint32_t g_irqtmp;
+extern uint32_t g_undeftmp;
+extern uint32_t g_aborttmp;
+#ifdef CONFIG_ARMV7A_DECODEFIQ
+extern uint32_t g_fiqtmp;
+#endif
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -68,6 +90,24 @@ static inline void arm_registerdump(FAR struct tcb_s *tcb)
 # define arm_registerdump(tcb)
 #endif
 
+/****************************************************************************
+ * Name: set_stack
+ ****************************************************************************/
+
+static void set_stack(uint32_t mode, uintptr_t stack)
+{
+  uint32_t reg = 0x0;
+
+  asm volatile (
+    "mrs %0, cpsr \n"     /* Save cpsr */
+    "msr cpsr_c, %2 \n"   /* Switch to IRQ mode */
+    "mov r13, %1 \n"      /* Set the IRQ mode stack */
+    "msr cpsr, %0 \n"     /* Restore cpsr */
+    : "+r" (reg)
+    : "r" (stack), "r" (mode)
+    : "memory");
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -91,8 +131,18 @@ static inline void arm_registerdump(FAR struct tcb_s *tcb)
 int arm_start_handler(int irq, FAR void *context, FAR void *arg)
 {
   FAR struct tcb_s *tcb = this_task();
+  int cpu = this_cpu();
+
+  sinfo("CPU%d Started\n", cpu);
 
-  sinfo("CPU%d Started\n", this_cpu());
+  /* Set stack for each mode on this CPU */
+
+#ifdef CONFIG_ARMV7A_DECODEFIQ
+  set_stack(MODE_FIQ, (uintptr_t)&g_fiqtmp + (8  * cpu));
+#endif
+  set_stack(MODE_IRQ, (uintptr_t)&g_irqtmp + (8  * cpu));
+  set_stack(MODE_ABORT, (uintptr_t)&g_aborttmp + (8  * cpu));
+  set_stack(MODE_UNDEF, (uintptr_t)&g_undeftmp + (8  * cpu));
 
 #ifdef CONFIG_SCHED_INSTRUMENTATION
   /* Notify that this CPU has started */
diff --git a/arch/arm/src/armv7-a/arm_vectors.S b/arch/arm/src/armv7-a/arm_vectors.S
index a90266d47b395..ffb7f0c1750c6 100644
--- a/arch/arm/src/armv7-a/arm_vectors.S
+++ b/arch/arm/src/armv7-a/arm_vectors.S
@@ -40,23 +40,56 @@
  ****************************************************************************/
 
 	.data
+
+	.globl	g_irqtmp
 g_irqtmp:
+#ifdef CONFIG_SMP
+	.rept	CONFIG_SMP_NCPUS
+	.word	0		/* Saved lr */
+	.word	0		/* Saved spsr */
+	.endr
+#else
 	.word	0		/* Saved lr */
 	.word	0		/* Saved spsr */
+#endif
 
+	.globl	g_undeftmp
 g_undeftmp:
+#ifdef CONFIG_SMP
+	.rept	CONFIG_SMP_NCPUS
 	.word	0		/* Saved lr */
 	.word	0		/* Saved spsr */
+	.endr
+#else
+	.word	0		/* Saved lr */
+	.word	0		/* Saved spsr */
+#endif
 
+	.globl	g_aborttmp
 g_aborttmp:
+#ifdef CONFIG_SMP
+	.rept	CONFIG_SMP_NCPUS
+	.word	0		/* Saved lr */
+	.word	0		/* Saved spsr */
+	.endr
+#else
 	.word	0		/* Saved lr */
 	.word	0		/* Saved spsr */
+#endif
 
 #ifdef CONFIG_ARMV7A_DECODEFIQ
+	.globl	g_fiqtmp
 g_fiqtmp:
+#ifdef CONFIG_SMP
+	.rept	CONFIG_SMP_NCPUS
+	.word	0		/* Saved lr */
+	.word	0		/* Saved spsr */
+	.endr
+#else
 	.word	0		/* Saved lr */
 	.word	0		/* Saved spsr */
 #endif
+#endif
 
 #if CONFIG_ARCH_INTERRUPTSTACK > 7 && defined(CONFIG_ARMV7A_HAVE_GICv2)
 
@@ -147,7 +180,11 @@ arm_vectorirq:
 	 * and r14.
 	 */
 
-	ldr		r13, .Lirqtmp
+	cmp		r13, #0				/* IRQ stack is set? */
+	bne		.Lirqstackset
+	ldr		r13, .Lirqtmp			/* Set IRQ stack to irqtmp */
+
+.Lirqstackset:
 	sub		lr, lr, #4
 	str		lr, [r13]			/* Save lr_IRQ */
 	mrs		lr, spsr
@@ -173,6 +210,13 @@ arm_vectorirq:
 	/* Get the values for r15(pc) and CPSR in r3 and r4 */
 
 	ldr		r0, .Lirqtmp			/* Points to temp storage */
+
+#ifdef CONFIG_SMP
+	cpuindex	r1				/* r1 = cpuindex */
+	lsls		r1, r1, #3			/* r1 = (cpuindex * 8) */
+	add		r0, r0, r1			/* r0 = g_irqtmp + (8 * cpuindex) */
+#endif
+
 	ldmia		r0, {r3, r4}			/* Recover r3=lr_IRQ, r4=spsr_IRQ */
 
 #ifdef CONFIG_BUILD_KERNEL
@@ -630,7 +674,11 @@ arm_vectorprefetch:
 	 * r13 and r14
 	 */
 
+	cmp		r13, #0				/* ABORT stack is set? */
+	bne		.Labortstackset
 	ldr		r13, .Lpaborttmp		/* Points to temp storage */
+
+.Labortstackset:
 	sub		lr, lr, #4			/* Fixup return */
 	str		lr, [r13]			/* Save in temp storage */
 	mrs		lr, spsr			/* Get SPSR */
@@ -652,6 +700,13 @@ arm_vectorprefetch:
 	/* Get the values for r15(pc) and CPSR in r3 and r4 */
 
 	ldr		r0, .Lpaborttmp			/* Points to temp storage */
+
+#ifdef CONFIG_SMP
+	cpuindex	r1				/* r1 = cpuindex */
+	lsls		r1, r1, #3			/* r1 = (cpuindex * 8) */
+	add		r0, r0, r1			/* r0 = g_aborttmp + (8 * cpuindex) */
+#endif
+
 	ldmia		r0, {r3, r4}			/* Recover r3=lr_ABT, r4=spsr_ABT */
 
 #ifdef CONFIG_BUILD_KERNEL
@@ -774,7 +829,12 @@ arm_vectorundefinsn:
 	 * r13 and r14
 	 */
 
+	cmp		r13, #0				/* UNDEF stack is set? */
+	bne		.Lundefstackset
 	ldr		r13, .Lundeftmp			/* Points to temp storage */
+
+.Lundefstackset:
+	sub		lr, lr, #4			/* Fixup return */
 	str		lr, [r13]			/* Save in temp storage */
 	mrs		lr, spsr			/* Get SPSR */
 	str		lr, [r13, #4]			/* Save in temp storage */
@@ -795,6 +855,13 @@ arm_vectorundefinsn:
 	/* Get the values for r15(pc) and CPSR in r3 and r4 */
 
 	ldr		r0, .Lundeftmp			/* Points to temp storage */
+
+#ifdef CONFIG_SMP
+	cpuindex	r1				/* r1 = cpuindex */
+	lsls		r1, r1, #3			/* r1 = (cpuindex * 8) */
+	add		r0, r0, r1			/* r0 = g_undeftmp + (8 * cpuindex) */
+#endif
+
 	ldmia		r0, {r3, r4}			/* Recover r3=lr_UND, r4=spsr_UND */
 
 #ifdef CONFIG_BUILD_KERNEL
@@ -916,7 +983,11 @@ arm_vectorfiq:
 #ifdef CONFIG_ARMV7A_DECODEFIQ
 	/* On entry we are free to use the FIQ mode registers r8 through r14 */
 
+	cmp		r13, #0				/* FIQ stack is set? */
+	bne		.Lfiqstackset
 	ldr		r13, .Lfiqtmp			/* Points to temp storage */
+
+.Lfiqstackset:
 	sub		lr, lr, #4			/* Fixup return */
 	str		lr, [r13]			/* Save in temp storage */
 	mrs		lr, spsr			/* Get SPSR_fiq */
@@ -938,6 +1009,13 @@ arm_vectorfiq:
 	/* Get the values for r15(pc) and CPSR in r3 and r4 */
 
 	ldr		r0, .Lfiqtmp			/* Points to temp storage */
+
+#ifdef CONFIG_SMP
+	cpuindex	r1				/* r1 = cpuindex */
+	lsls		r1, r1, #3			/* r1 = (cpuindex * 8) */
+	add		r0, r0, r1			/* r0 = g_fiqtmp + (8 * cpuindex) */
+#endif
+
 	ldmia		r0, {r3, r4}			/* Recover r3=lr_SVC, r4=spsr_SVC */
 
 #ifdef CONFIG_BUILD_KERNEL
diff --git a/arch/arm/src/imx6/chip.h b/arch/arm/src/imx6/chip.h
index 45f84fa8336d1..7043d66d9019a 100644
--- a/arch/arm/src/imx6/chip.h
+++ b/arch/arm/src/imx6/chip.h
@@ -66,7 +66,7 @@
  *
  ****************************************************************************/
 
-#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+#if defined(CONFIG_SMP)
   .macro  cpuindex, index
   mrc  p15, 0, \index, c0, c0, 5  /* Read the MPIDR */
   and  \index, \index, #3         /* Bits 0-1=CPU ID */