diff --git a/arch/arm/include/armv7-a/cp15.h b/arch/arm/include/armv7-a/cp15.h
index 9367f3ec28021..09d7e5b1783d2 100644
--- a/arch/arm/include/armv7-a/cp15.h
+++ b/arch/arm/include/armv7-a/cp15.h
@@ -275,4 +275,6 @@
      value;                             \
   })                                    \
 
+#define CP15_MODIFY(v,m,a) CP15_SET(a, ((CP15_GET(a) & ~(m)) | ((uintptr_t)(v) & (m))))
+
 #endif /* __ARCH_ARM_SRC_ARMV7_A_CP15_H */
diff --git a/arch/arm/include/armv7-a/irq.h b/arch/arm/include/armv7-a/irq.h
index afdf5615520b3..62425feddbfb1 100644
--- a/arch/arm/include/armv7-a/irq.h
+++ b/arch/arm/include/armv7-a/irq.h
@@ -472,18 +472,16 @@ static inline_function uint32_t up_getsp(void)
   return sp;
 }
 
-noinstrument_function
-static inline_function bool up_interrupt_context(void)
-{
-  return (bool)CP15_GET(TPIDRPRW);
-}
-
 noinstrument_function
 static inline_function void up_set_interrupt_context(bool flag)
 {
-  CP15_SET(TPIDRPRW, flag);
+  CP15_MODIFY(flag, 1ul, TPIDRPRW);
 }
 
+#define up_this_task()         ((struct tcb_s *)(CP15_GET(TPIDRPRW) & ~1ul))
+#define up_update_task(t)      CP15_MODIFY(t, ~1ul, TPIDRPRW)
+#define up_interrupt_context() (CP15_GET(TPIDRPRW) & 1)
+
 /****************************************************************************
  * Public Data
  ****************************************************************************/
diff --git a/arch/arm/include/armv7-r/cp15.h b/arch/arm/include/armv7-r/cp15.h
index 3f1d23e2b97e6..fef41b769af59 100644
--- a/arch/arm/include/armv7-r/cp15.h
+++ b/arch/arm/include/armv7-r/cp15.h
@@ -218,4 +218,6 @@
      value;                             \
   })                                    \
 
+#define CP15_MODIFY(v,m,a) CP15_SET(a, ((CP15_GET(a) & ~(m)) | ((uintptr_t)(v) & (m))))
+
 #endif /* __ARCH_ARM_SRC_ARMV7_R_CP15_H */
diff --git a/arch/arm/include/armv7-r/irq.h b/arch/arm/include/armv7-r/irq.h
index 12470257a6941..3c404d13666eb 100644
--- a/arch/arm/include/armv7-r/irq.h
+++ b/arch/arm/include/armv7-r/irq.h
@@ -467,18 +467,16 @@ static inline_function uint32_t up_getsp(void)
   return sp;
 }
 
-noinstrument_function
-static inline_function bool up_interrupt_context(void)
-{
-  return (bool)CP15_GET(TPIDRPRW);
-}
-
 noinstrument_function
 static inline_function void up_set_interrupt_context(bool flag)
 {
-  CP15_SET(TPIDRPRW, flag);
+  CP15_MODIFY(flag, 1ul, TPIDRPRW);
 }
 
+#define up_this_task()         ((struct tcb_s *)(CP15_GET(TPIDRPRW) & ~1ul))
+#define up_update_task(t)      CP15_MODIFY(t, ~1ul, TPIDRPRW)
+#define up_interrupt_context() (CP15_GET(TPIDRPRW) & 1)
+
 /****************************************************************************
  * Public Data
  ****************************************************************************/
diff --git a/arch/arm/include/armv8-r/cp15.h b/arch/arm/include/armv8-r/cp15.h
index ab8ec421c5d5b..9214c439eaf3a 100644
--- a/arch/arm/include/armv8-r/cp15.h
+++ b/arch/arm/include/armv8-r/cp15.h
@@ -237,4 +237,6 @@
      _value;                             \
   })                                    \
 
+#define CP15_MODIFY(v,m,a) CP15_SET(a, ((CP15_GET(a) & ~(m)) | ((uintptr_t)(v) & (m))))
+
 #endif /* __ARCH_ARM_SRC_ARMV8_R_CP15_H */
diff --git a/arch/arm/include/armv8-r/irq.h b/arch/arm/include/armv8-r/irq.h
index 62962399c5d5c..6f49b40a2aaee 100644
--- a/arch/arm/include/armv8-r/irq.h
+++ b/arch/arm/include/armv8-r/irq.h
@@ -467,18 +467,16 @@ static inline_function uint32_t up_getsp(void)
   return sp;
 }
 
-noinstrument_function
-static inline_function bool up_interrupt_context(void)
-{
-  return (bool)CP15_GET(TPIDRPRW);
-}
-
 noinstrument_function
 static inline_function void up_set_interrupt_context(bool flag)
 {
-  CP15_SET(TPIDRPRW, flag);
+  CP15_MODIFY(flag, 1ul, TPIDRPRW);
 }
 
+#define up_this_task()         ((struct tcb_s *)(CP15_GET(TPIDRPRW) & ~1ul))
+#define up_update_task(t)      CP15_MODIFY(t, ~1ul, TPIDRPRW)
+#define up_interrupt_context() (CP15_GET(TPIDRPRW) & 1)
+
 /****************************************************************************
  * Public Data
  ****************************************************************************/
diff --git a/arch/arm/src/armv7-a/arm_scu.c b/arch/arm/src/armv7-a/arm_scu.c
index f82101e35238d..78322f911e4a8 100644
--- a/arch/arm/src/armv7-a/arm_scu.c
+++ b/arch/arm/src/armv7-a/arm_scu.c
@@ -29,6 +29,7 @@
 #include <stdint.h>
 
 #include <arch/irq.h>
+#include <sched/sched.h>
 
 #include "arm_internal.h"
 #include "cp15_cacheops.h"
@@ -57,6 +58,14 @@ void arm_enable_smp(int cpu)
 {
   uint32_t regval;
 
+  /* We need to confirm that current_task has been initialized. */
+
+  while (!current_task(this_cpu()));
+
+  /* Init idle task to percpu reg */
+
+  up_update_task(current_task(cpu));
+
   /* Handle actions unique to CPU0 which comes up first */
 
   if (cpu == 0)
diff --git a/arch/arm/src/armv7-r/arm_scu.c b/arch/arm/src/armv7-r/arm_scu.c
index 4e5760695337f..75913b445af6a 100644
--- a/arch/arm/src/armv7-r/arm_scu.c
+++ b/arch/arm/src/armv7-r/arm_scu.c
@@ -29,6 +29,7 @@
 #include <stdint.h>
 
 #include <arch/irq.h>
+#include <sched/sched.h>
 
 #include "arm_internal.h"
 #include "cp15_cacheops.h"
@@ -57,6 +58,14 @@ void arm_enable_smp(int cpu)
 {
   uint32_t regval;
 
+  /* We need to confirm that current_task has been initialized. */
+
+  while (!current_task(this_cpu()));
+
+  /* Init idle task to percpu reg */
+
+  up_update_task(current_task(cpu));
+
   /* Handle actions unique to CPU0 which comes up first */
 
   if (cpu == 0)