diff --git a/arch/arm/src/arm/arm_sigdeliver.c b/arch/arm/src/arm/arm_sigdeliver.c
index 22589d1e6e7..b55e820604f 100644
--- a/arch/arm/src/arm/arm_sigdeliver.c
+++ b/arch/arm/src/arm/arm_sigdeliver.c
@@ -97,5 +97,7 @@ void arm_sigdeliver(void)
   /* Then restore the correct state for this thread of execution. */
 
   board_autoled_off(LED_SIGNAL);
+
+  g_running_tasks[this_cpu()] = NULL;
   arm_fullcontextrestore(regs);
 }
diff --git a/arch/arm/src/arm/arm_syscall.c b/arch/arm/src/arm/arm_syscall.c
index 8a5849e4d2d..a69cc31941c 100644
--- a/arch/arm/src/arm/arm_syscall.c
+++ b/arch/arm/src/arm/arm_syscall.c
@@ -54,19 +54,23 @@
 
 uint32_t *arm_syscall(uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  FAR struct tcb_s *tcb = this_task();
   uint32_t cmd;
-  int cpu;
 
   /* Nested interrupts are not supported */
 
   DEBUGASSERT(up_current_regs() == NULL);
 
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
+
   /* Current regs non-zero indicates that we are processing an interrupt;
    * current_regs is also used to manage interrupt level context switches.
    */
 
-  tcb->xcp.regs = regs;
   up_set_current_regs(regs);
 
   /* The SYSCALL command is in R0 on entry.  Parameters follow in R1..R7 */
@@ -118,11 +122,6 @@ uint32_t *arm_syscall(uint32_t *regs)
        */
 
       case SYS_switch_context:
-        {
-          DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          up_set_current_regs((uint32_t *)regs[REG_R2]);
-        }
         break;
 
       default:
@@ -134,7 +133,7 @@ uint32_t *arm_syscall(uint32_t *regs)
         break;
     }
 
-  if (regs != tcb->xcp.regs)
+  if (*running_task != tcb)
     {
 #ifdef CONFIG_ARCH_ADDRENV
       /* Make sure that the address environment for the previously
@@ -145,25 +144,16 @@ uint32_t *arm_syscall(uint32_t *regs)
 
       addrenv_switch(NULL);
 #endif
-
-      /* Record the new "running" task.  g_running_tasks[] is only used by
-       * assertion logic for reporting crashes.
-       */
-
-      cpu = this_cpu();
-      tcb = current_task(cpu);
-
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[cpu]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
-      g_running_tasks[cpu] = tcb;
+      *running_task = tcb;
 
       /* Restore the cpu lock */
 
-      restore_critical_section(tcb, cpu);
-      regs = up_current_regs();
+      restore_critical_section(tcb, this_cpu());
     }
 
   /* Set current_regs to NULL to indicate that we are no longer in an
diff --git a/arch/arm/src/armv6-m/arm_doirq.c b/arch/arm/src/armv6-m/arm_doirq.c
index 3ec4f182a55..8e8f0de89b6 100644
--- a/arch/arm/src/armv6-m/arm_doirq.c
+++ b/arch/arm/src/armv6-m/arm_doirq.c
@@ -56,7 +56,13 @@ void exception_direct(void)
 
 uint32_t *arm_doirq(int irq, uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  FAR struct tcb_s *tcb;
+
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
 
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
@@ -80,13 +86,9 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
 #endif
 
       up_irq_save();
-      g_running_tasks[this_cpu()]->xcp.regs = regs;
     }
   else
     {
-      /* Dispatch irq */
-
-      tcb->xcp.regs = regs;
       irq_dispatch(irq, regs);
     }
 
@@ -110,7 +112,7 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
 
   /* Update scheduler parameters */
 
-  nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+  nxsched_suspend_scheduler(*running_task);
   nxsched_resume_scheduler(tcb);
 
   /* Record the new "running" task when context switch occurred.
@@ -118,7 +120,7 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
    * crashes.
    */
 
-  g_running_tasks[this_cpu()] = tcb;
+  *running_task = tcb;
   regs = tcb->xcp.regs;
 #endif
 
diff --git a/arch/arm/src/armv6-m/arm_svcall.c b/arch/arm/src/armv6-m/arm_svcall.c
index 55aa6ea07f1..b86b11f98eb 100644
--- a/arch/arm/src/armv6-m/arm_svcall.c
+++ b/arch/arm/src/armv6-m/arm_svcall.c
@@ -119,6 +119,7 @@ int arm_svcall(int irq, void *context, void *arg)
 {
   struct tcb_s *tcb = this_task();
   uint32_t *regs = (uint32_t *)context;
+  uint32_t *new_regs = regs;
   uint32_t cmd;
 
   cmd = regs[REG_R0];
@@ -167,6 +168,7 @@ int arm_svcall(int irq, void *context, void *arg)
       case SYS_restore_context:
         {
           DEBUGASSERT(regs[REG_R1] != 0);
+          new_regs = (uint32_t *)regs[REG_R1];
           tcb->xcp.regs = (uint32_t *)regs[REG_R1];
         }
         break;
@@ -191,8 +193,7 @@ int arm_svcall(int irq, void *context, void *arg)
       case SYS_switch_context:
         {
           DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_R2];
+          new_regs = (uint32_t *)regs[REG_R2];
         }
         break;
 
@@ -446,12 +447,12 @@ int arm_svcall(int irq, void *context, void *arg)
    * switch.
    */
 
-  if (regs != tcb->xcp.regs)
+  if (regs != new_regs)
     {
       restore_critical_section(tcb, this_cpu());
 
 #ifdef CONFIG_DEBUG_SYSCALL_INFO
-      regs = (uint32_t *)tcb->xcp.regs;
+      regs = new_regs;
 
       svcinfo("SVCall Return:\n");
       svcinfo("  R0: %08x %08x %08x %08x %08x %08x %08x %08x\n",
diff --git a/arch/arm/src/armv7-a/arm_sigdeliver.c b/arch/arm/src/armv7-a/arm_sigdeliver.c
index 98ac0e115b6..1867aa88446 100644
--- a/arch/arm/src/armv7-a/arm_sigdeliver.c
+++ b/arch/arm/src/armv7-a/arm_sigdeliver.c
@@ -160,5 +160,7 @@ void arm_sigdeliver(void)
   leave_critical_section(regs[REG_CPSR]);
   rtcb->irqcount--;
 #endif
+
+  g_running_tasks[this_cpu()] = NULL;
   arm_fullcontextrestore(regs);
 }
diff --git a/arch/arm/src/armv7-a/arm_syscall.c b/arch/arm/src/armv7-a/arm_syscall.c
index a1c8fdced17..c1347104487 100644
--- a/arch/arm/src/armv7-a/arm_syscall.c
+++ b/arch/arm/src/armv7-a/arm_syscall.c
@@ -159,9 +159,9 @@ static void dispatch_syscall(void)
 
 uint32_t *arm_syscall(uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
   struct tcb_s *tcb = this_task();
   uint32_t cmd;
-  int cpu;
 #ifdef CONFIG_BUILD_KERNEL
   uint32_t cpsr;
 #endif
@@ -170,7 +170,10 @@ uint32_t *arm_syscall(uint32_t *regs)
 
   DEBUGASSERT(up_current_regs() == NULL);
 
-  tcb->xcp.regs = regs;
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
 
   /* Current regs non-zero indicates that we are processing an interrupt;
    * current_regs is also used to manage interrupt level context switches.
@@ -296,11 +299,6 @@ uint32_t *arm_syscall(uint32_t *regs)
        */
 
       case SYS_switch_context:
-        {
-          DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_R2];
-        }
         break;
 
       /* R0=SYS_task_start:  This a user task start
@@ -566,7 +564,7 @@ uint32_t *arm_syscall(uint32_t *regs)
         break;
     }
 
-  if (regs != tcb->xcp.regs)
+  if (*running_task != tcb)
     {
 #ifdef CONFIG_ARCH_ADDRENV
       /* Make sure that the address environment for the previously
@@ -578,23 +576,20 @@ uint32_t *arm_syscall(uint32_t *regs)
       addrenv_switch(NULL);
 #endif
 
-      cpu = this_cpu();
-      tcb = current_task(cpu);
-
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[cpu]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task.  g_running_tasks[] is only used by
        * assertion logic for reporting crashes.
        */
 
-      g_running_tasks[cpu] = tcb;
+      *running_task = tcb;
 
       /* Restore the cpu lock */
 
-      restore_critical_section(tcb, cpu);
+      restore_critical_section(tcb, this_cpu());
       regs = tcb->xcp.regs;
     }
 
diff --git a/arch/arm/src/armv7-m/arm_doirq.c b/arch/arm/src/armv7-m/arm_doirq.c
index 3f585442291..94a0f3e4f4d 100644
--- a/arch/arm/src/armv7-m/arm_doirq.c
+++ b/arch/arm/src/armv7-m/arm_doirq.c
@@ -56,7 +56,13 @@ void exception_direct(void)
 
 uint32_t *arm_doirq(int irq, uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  FAR struct tcb_s *tcb;
+
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
 
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
@@ -80,13 +86,9 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
 #endif
 
       up_irq_save();
-      g_running_tasks[this_cpu()]->xcp.regs = regs;
     }
   else
     {
-      /* Dispatch irq */
-
-      tcb->xcp.regs = regs;
       irq_dispatch(irq, regs);
     }
 
@@ -110,7 +112,7 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
 
   /* Update scheduler parameters */
 
-  nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+  nxsched_suspend_scheduler(*running_task);
   nxsched_resume_scheduler(tcb);
 
   /* Record the new "running" task when context switch occurred.
@@ -118,7 +120,7 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
    * crashes.
    */
 
-  g_running_tasks[this_cpu()] = tcb;
+  *running_task = tcb;
   regs = tcb->xcp.regs;
 #endif
 
diff --git a/arch/arm/src/armv7-m/arm_svcall.c b/arch/arm/src/armv7-m/arm_svcall.c
index 18a578f7cc3..3e3d6088708 100644
--- a/arch/arm/src/armv7-m/arm_svcall.c
+++ b/arch/arm/src/armv7-m/arm_svcall.c
@@ -127,6 +127,7 @@ int arm_svcall(int irq, void *context, void *arg)
 {
   struct tcb_s *tcb = this_task();
   uint32_t *regs = (uint32_t *)context;
+  uint32_t *new_regs = regs;
   uint32_t cmd;
 
   cmd = regs[REG_R0];
@@ -176,6 +177,7 @@ int arm_svcall(int irq, void *context, void *arg)
       case SYS_restore_context:
         {
           DEBUGASSERT(regs[REG_R1] != 0);
+          new_regs = (uint32_t *)regs[REG_R1];
           tcb->xcp.regs = (uint32_t *)regs[REG_R1];
         }
         break;
@@ -200,8 +202,7 @@ int arm_svcall(int irq, void *context, void *arg)
       case SYS_switch_context:
         {
           DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_R2];
+          new_regs = (uint32_t *)regs[REG_R2];
         }
         break;
 
@@ -456,12 +457,12 @@ int arm_svcall(int irq, void *context, void *arg)
    * switch.
    */
 
-  if (regs != tcb->xcp.regs)
+  if (regs != new_regs)
     {
       restore_critical_section(tcb, this_cpu());
 
 #ifdef CONFIG_DEBUG_SYSCALL_INFO
-      regs = (uint32_t *)tcb->xcp.regs;
+      regs = new_regs;
 
       svcinfo("SVCall Return:\n");
       svcinfo("  R0: %08x %08x %08x %08x %08x %08x %08x %08x\n",
diff --git a/arch/arm/src/armv7-r/arm_sigdeliver.c b/arch/arm/src/armv7-r/arm_sigdeliver.c
index 706f6ad5551..5a63458b9b0 100644
--- a/arch/arm/src/armv7-r/arm_sigdeliver.c
+++ b/arch/arm/src/armv7-r/arm_sigdeliver.c
@@ -157,5 +157,7 @@ void arm_sigdeliver(void)
   leave_critical_section(regs[REG_CPSR]);
   rtcb->irqcount--;
 #endif
+
+  g_running_tasks[this_cpu()] = NULL;
   arm_fullcontextrestore(regs);
 }
diff --git a/arch/arm/src/armv7-r/arm_syscall.c b/arch/arm/src/armv7-r/arm_syscall.c
index 2467c032077..7115cdd3a8a 100644
--- a/arch/arm/src/armv7-r/arm_syscall.c
+++ b/arch/arm/src/armv7-r/arm_syscall.c
@@ -156,10 +156,9 @@ static void dispatch_syscall(void)
 
 uint32_t *arm_syscall(uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
-
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  FAR struct tcb_s *tcb = this_task();
   uint32_t cmd;
-  int cpu;
 #ifdef CONFIG_BUILD_PROTECTED
   uint32_t cpsr;
 #endif
@@ -168,7 +167,10 @@ uint32_t *arm_syscall(uint32_t *regs)
 
   DEBUGASSERT(up_current_regs() == NULL);
 
-  tcb->xcp.regs = regs;
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
 
   /* Current regs non-zero indicates that we are processing an interrupt;
    * current_regs is also used to manage interrupt level context switches.
@@ -294,11 +296,6 @@ uint32_t *arm_syscall(uint32_t *regs)
        */
 
       case SYS_switch_context:
-        {
-          DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_R2];
-        }
         break;
 
       /* R0=SYS_task_start:  This a user task start
@@ -564,24 +561,22 @@ uint32_t *arm_syscall(uint32_t *regs)
         break;
     }
 
-  if (regs != tcb->xcp.regs)
+  if (*running_task != tcb)
     {
-      cpu = this_cpu();
-
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[cpu]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task.  g_running_tasks[] is only used by
        * assertion logic for reporting crashes.
        */
 
-      g_running_tasks[cpu] = tcb;
+      *running_task = tcb;
 
       /* Restore the cpu lock */
 
-      restore_critical_section(tcb, cpu);
+      restore_critical_section(tcb, this_cpu());
       regs = tcb->xcp.regs;
     }
 
diff --git a/arch/arm/src/armv8-m/arm_doirq.c b/arch/arm/src/armv8-m/arm_doirq.c
index df51d7c6fb9..5f23362a541 100644
--- a/arch/arm/src/armv8-m/arm_doirq.c
+++ b/arch/arm/src/armv8-m/arm_doirq.c
@@ -67,7 +67,13 @@ void exception_direct(void)
 
 uint32_t *arm_doirq(int irq, uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  FAR struct tcb_s *tcb;
+
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
 
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
@@ -91,13 +97,9 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
 #endif
 
       up_irq_save();
-      g_running_tasks[this_cpu()]->xcp.regs = regs;
     }
   else
     {
-      /* Dispatch irq */
-
-      tcb->xcp.regs = regs;
       irq_dispatch(irq, regs);
     }
 
@@ -121,7 +123,7 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
 
   /* Update scheduler parameters */
 
-  nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+  nxsched_suspend_scheduler(*running_task);
   nxsched_resume_scheduler(tcb);
 
   /* Record the new "running" task when context switch occurred.
@@ -129,7 +131,7 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
    * crashes.
    */
 
-  g_running_tasks[this_cpu()] = tcb;
+  *running_task = tcb;
   regs = tcb->xcp.regs;
 #endif
 
diff --git a/arch/arm/src/armv8-m/arm_svcall.c b/arch/arm/src/armv8-m/arm_svcall.c
index 168b071e3b4..e020c335730 100644
--- a/arch/arm/src/armv8-m/arm_svcall.c
+++ b/arch/arm/src/armv8-m/arm_svcall.c
@@ -127,6 +127,7 @@ int arm_svcall(int irq, void *context, void *arg)
 {
   struct tcb_s *tcb = this_task();
   uint32_t *regs = (uint32_t *)context;
+  uint32_t *new_regs = regs;
   uint32_t cmd;
 
   cmd = regs[REG_R0];
@@ -176,6 +177,7 @@ int arm_svcall(int irq, void *context, void *arg)
       case SYS_restore_context:
         {
           DEBUGASSERT(regs[REG_R1] != 0);
+          new_regs = (uint32_t *)regs[REG_R1];
           tcb->xcp.regs = (uint32_t *)regs[REG_R1];
         }
         break;
@@ -200,8 +202,7 @@ int arm_svcall(int irq, void *context, void *arg)
       case SYS_switch_context:
         {
           DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_R2];
+          new_regs = (uint32_t *)regs[REG_R2];
         }
         break;
 
@@ -456,12 +457,12 @@ int arm_svcall(int irq, void *context, void *arg)
    * switch.
    */
 
-  if (regs != tcb->xcp.regs)
+  if (regs != new_regs)
     {
       restore_critical_section(tcb, this_cpu());
 
 #ifdef CONFIG_DEBUG_SYSCALL_INFO
-      regs = (uint32_t *)tcb->xcp.regs;
+      regs = new_regs;
 
       svcinfo("SVCall Return:\n");
       svcinfo("  R0: %08x %08x %08x %08x %08x %08x %08x %08x\n",
diff --git a/arch/arm/src/armv8-r/arm_sigdeliver.c b/arch/arm/src/armv8-r/arm_sigdeliver.c
index a6837079a3c..e4a8ad59353 100644
--- a/arch/arm/src/armv8-r/arm_sigdeliver.c
+++ b/arch/arm/src/armv8-r/arm_sigdeliver.c
@@ -155,5 +155,7 @@ void arm_sigdeliver(void)
   leave_critical_section(regs[REG_CPSR]);
   rtcb->irqcount--;
 #endif
+
+  g_running_tasks[this_cpu()] = NULL;
   arm_fullcontextrestore(regs);
 }
diff --git a/arch/arm/src/armv8-r/arm_syscall.c b/arch/arm/src/armv8-r/arm_syscall.c
index 87ff43dc849..338613e9f78 100644
--- a/arch/arm/src/armv8-r/arm_syscall.c
+++ b/arch/arm/src/armv8-r/arm_syscall.c
@@ -156,9 +156,9 @@ static void dispatch_syscall(void)
 
 uint32_t *arm_syscall(uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  FAR struct tcb_s *tcb = this_task();
   uint32_t cmd;
-  int cpu;
 #ifdef CONFIG_BUILD_PROTECTED
   uint32_t cpsr;
 #endif
@@ -167,7 +167,10 @@ uint32_t *arm_syscall(uint32_t *regs)
 
   DEBUGASSERT(up_current_regs() == NULL);
 
-  tcb->xcp.regs = regs;
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
 
   /* Current regs non-zero indicates that we are processing an interrupt;
    * current_regs is also used to manage interrupt level context switches.
@@ -293,11 +296,6 @@ uint32_t *arm_syscall(uint32_t *regs)
        */
 
       case SYS_switch_context:
-        {
-          DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
-          *(uint32_t **)regs[REG_R1] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_R2];
-        }
         break;
 
       /* R0=SYS_task_start:  This a user task start
@@ -563,24 +561,22 @@ uint32_t *arm_syscall(uint32_t *regs)
         break;
     }
 
-  if (regs != tcb->xcp.regs)
+  if (*running_task != tcb)
     {
-      cpu = this_cpu();
-
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[cpu]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task.  g_running_tasks[] is only used by
        * assertion logic for reporting crashes.
        */
 
-      g_running_tasks[cpu] = tcb;
+      *running_task = tcb;
 
       /* Restore the cpu lock */
 
-      restore_critical_section(tcb, cpu);
+      restore_critical_section(tcb, this_cpu());
       regs = tcb->xcp.regs;
     }
 
diff --git a/arch/arm64/include/irq.h b/arch/arm64/include/irq.h
index 778d741c74a..1c837c5dfd8 100644
--- a/arch/arm64/include/irq.h
+++ b/arch/arm64/include/irq.h
@@ -410,14 +410,15 @@ static inline void up_irq_restore(irqstate_t flags)
 #define up_update_task(t)      modify_sysreg(t, ~1ul, tpidr_el1)
 #define up_interrupt_context() (read_sysreg(tpidr_el1) & 1)
 
-#define up_switch_context(tcb, rtcb)                              \
-  do {                                                            \
-    if (!up_interrupt_context())                                  \
-      {                                                           \
-        sys_call2(SYS_switch_context, (uintptr_t)&rtcb->xcp.regs, \
-                  (uintptr_t)tcb->xcp.regs);                      \
-      }                                                           \
-  } while (0)
+#define up_switch_context(tcb, rtcb)                                      \
+  do                                                                      \
+    {                                                                     \
+      if (!up_interrupt_context())                                        \
+        {                                                                 \
+          sys_call2(SYS_switch_context, (uintptr_t)rtcb, (uintptr_t)tcb); \
+        }                                                                 \
+    }                                                                     \
+  while (0)
 
 /****************************************************************************
  * Name: up_getusrpc
diff --git a/arch/arm64/src/common/arm64_exit.c b/arch/arm64/src/common/arm64_exit.c
index ba81dbb97f8..793533f76eb 100644
--- a/arch/arm64/src/common/arm64_exit.c
+++ b/arch/arm64/src/common/arm64_exit.c
@@ -76,5 +76,5 @@ void up_exit(int status)
 
   /* Then switch contexts */
 
-  arm64_fullcontextrestore(tcb->xcp.regs);
+  arm64_fullcontextrestore(tcb);
 }
diff --git a/arch/arm64/src/common/arm64_internal.h b/arch/arm64/src/common/arm64_internal.h
index de5bc209806..5badadb9af2 100644
--- a/arch/arm64/src/common/arm64_internal.h
+++ b/arch/arm64/src/common/arm64_internal.h
@@ -112,10 +112,10 @@
 
 /* Context switching */
 
-#define arm64_fullcontextrestore(restoreregs) \
+#define arm64_fullcontextrestore(next) \
   do \
     { \
-      sys_call1(SYS_restore_context, (uintptr_t)restoreregs); \
+      sys_call1(SYS_restore_context, (uintptr_t)next); \
     } \
   while (1)
 
diff --git a/arch/arm64/src/common/arm64_sigdeliver.c b/arch/arm64/src/common/arm64_sigdeliver.c
index fff7e1ccee5..3161f8859cb 100644
--- a/arch/arm64/src/common/arm64_sigdeliver.c
+++ b/arch/arm64/src/common/arm64_sigdeliver.c
@@ -160,5 +160,7 @@ void arm64_sigdeliver(void)
   leave_critical_section(flags);
   rtcb->irqcount--;
 #endif
-  arm64_fullcontextrestore(rtcb->xcp.regs);
+
+  g_running_tasks[this_cpu()] = NULL;
+  arm64_fullcontextrestore(rtcb);
 }
diff --git a/arch/arm64/src/common/arm64_syscall.c b/arch/arm64/src/common/arm64_syscall.c
index a192cabafba..f969c4eb5d3 100644
--- a/arch/arm64/src/common/arm64_syscall.c
+++ b/arch/arm64/src/common/arm64_syscall.c
@@ -154,14 +154,19 @@ uintptr_t dispatch_syscall(unsigned int nbr, uintptr_t parm1,
 
 uint64_t *arm64_syscall(uint64_t *regs)
 {
-  uint64_t            *ret_regs = regs;
-  uint64_t             cmd;
-  struct tcb_s        *tcb;
-  int cpu;
+  int cpu = this_cpu();
+  struct tcb_s **running_task = &g_running_tasks[cpu];
+  struct tcb_s *tcb = this_task();
+  uint64_t cmd;
 #ifdef CONFIG_BUILD_KERNEL
   uint64_t             spsr;
 #endif
 
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
+
   /* Nested interrupts are not supported */
 
   DEBUGASSERT(regs);
@@ -182,32 +187,21 @@ uint64_t *arm64_syscall(uint64_t *regs)
        * At this point, the following values are saved in context:
        *
        *   x0 = SYS_restore_context
-       *   x1 = restoreregs( xcp->regs, callee saved register save area)
+       *   x1 = next
        */
 
       case SYS_restore_context:
-        {
-          /* Replace 'regs' with the pointer to the register set in
-           * regs[REG_R1].  On return from the system call, that register
-           * set will determine the restored context.
-           */
-
-          ret_regs = (uint64_t *)regs[REG_X1];
-          regs[REG_X1] = 0; /* set the saveregs = 0 */
-
-          DEBUGASSERT(ret_regs);
-        }
         break;
 
       /* x0 = SYS_switch_context:  This a switch context command:
        *
-       * void arm64_switchcontext(uint64_t *saveregs, uint64_t *restoreregs);
+       * void arm64_switchcontext(struct tcb_s *prev, struct tcb_s *next);
        *
        * At this point, the following values are saved in context:
        *
        *   x0 = SYS_switch_context
-       *   x1 = saveregs (xcp->regs, callee saved register save area)
-       *   x2 = restoreregs (xcp->regs, callee saved register save area)
+       *   x1 = prev
+       *   x2 = next
        *
        * In this case, we do both: We save the context registers to the save
        * register area reference by the saved contents of x1 and then set
@@ -216,12 +210,6 @@ uint64_t *arm64_syscall(uint64_t *regs)
        */
 
       case SYS_switch_context:
-        {
-          DEBUGASSERT(regs[REG_X1] != 0 && regs[REG_X2] != 0);
-          *(uint64_t **)regs[REG_X1] = regs;
-
-          ret_regs = (uint64_t *)regs[REG_X2];
-        }
         break;
 
 #ifdef CONFIG_BUILD_KERNEL
@@ -328,15 +316,13 @@ uint64_t *arm64_syscall(uint64_t *regs)
       default:
         {
           svcerr("ERROR: Bad SYS call: 0x%" PRIx64 "\n", cmd);
-          ret_regs = 0;
           return 0;
         }
         break;
     }
 
-  if ((uint64_t *)regs != ret_regs)
+  if (*running_task != tcb)
     {
-      cpu = this_cpu();
       tcb = current_task(cpu);
 
 #ifdef CONFIG_ARCH_ADDRENV
@@ -351,19 +337,19 @@ uint64_t *arm64_syscall(uint64_t *regs)
 
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[cpu]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task.  g_running_tasks[] is only used by
        * assertion logic for reporting crashes.
        */
 
-      g_running_tasks[cpu] = tcb;
+      *running_task = tcb;
 
       /* Restore the cpu lock */
 
       restore_critical_section(tcb, cpu);
     }
 
-  return ret_regs;
+  return tcb->xcp.regs;
 }
diff --git a/arch/avr/src/avr/avr_doirq.c b/arch/avr/src/avr/avr_doirq.c
index c25e8b68d12..0bc80302c1b 100644
--- a/arch/avr/src/avr/avr_doirq.c
+++ b/arch/avr/src/avr/avr_doirq.c
@@ -57,6 +57,13 @@
 
 uint8_t *avr_doirq(uint8_t irq, uint8_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      avr_copystate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/avr/src/avr/avr_switchcontext.c b/arch/avr/src/avr/avr_switchcontext.c
index 0b3bd586ed9..bd7194f3062 100644
--- a/arch/avr/src/avr/avr_switchcontext.c
+++ b/arch/avr/src/avr/avr_switchcontext.c
@@ -85,6 +85,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Switch context to the context of the task at the head of the
        * ready to run list.
        */
diff --git a/arch/avr/src/avr32/avr_doirq.c b/arch/avr/src/avr32/avr_doirq.c
index 37d05289df6..3798248c626 100644
--- a/arch/avr/src/avr32/avr_doirq.c
+++ b/arch/avr/src/avr32/avr_doirq.c
@@ -58,6 +58,13 @@
 
 uint32_t *avr_doirq(int irq, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      avr_copystate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/avr/src/avr32/avr_switchcontext.c b/arch/avr/src/avr32/avr_switchcontext.c
index ea150f66331..71316ed163f 100644
--- a/arch/avr/src/avr32/avr_switchcontext.c
+++ b/arch/avr/src/avr32/avr_switchcontext.c
@@ -99,6 +99,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       avr_switchcontext(rtcb->xcp.regs, tcb->xcp.regs);
diff --git a/arch/ceva/src/common/ceva_doirq.c b/arch/ceva/src/common/ceva_doirq.c
index c977dc138e8..f5131596166 100644
--- a/arch/ceva/src/common/ceva_doirq.c
+++ b/arch/ceva/src/common/ceva_doirq.c
@@ -59,6 +59,13 @@ uint32_t *ceva_doirq(int irq, uint32_t *regs)
     }
   else
     {
+      struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+      if (*running_task != NULL)
+        {
+          (*running_task)->xcp.regs = regs;
+        }
+
       /* Current regs non-zero indicates that we are processing an interrupt;
        * current_regs is also used to manage interrupt level context
        * switches.
@@ -80,7 +87,7 @@ uint32_t *ceva_doirq(int irq, uint32_t *regs)
         {
           /* Update scheduler parameters */
 
-          nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+          nxsched_suspend_scheduler(*running_task);
           nxsched_resume_scheduler(this_task());
 
           /* Record the new "running" task when context switch occurred.
diff --git a/arch/ceva/src/common/ceva_exit.c b/arch/ceva/src/common/ceva_exit.c
index 08d60544c11..b096ff491fb 100644
--- a/arch/ceva/src/common/ceva_exit.c
+++ b/arch/ceva/src/common/ceva_exit.c
@@ -78,6 +78,8 @@ void _exit(int status)
 
   sched_resume_scheduler(tcb);
 
+  g_running_tasks[this_cpu()] = tcb;
+
   /* Then switch contexts */
 
   ceva_fullcontextrestore(tcb->xcp.regs);
diff --git a/arch/hc/src/common/hc_doirq.c b/arch/hc/src/common/hc_doirq.c
index 838358ebd38..2142687a456 100644
--- a/arch/hc/src/common/hc_doirq.c
+++ b/arch/hc/src/common/hc_doirq.c
@@ -58,6 +58,13 @@
 
 uint8_t *hc_doirq(int irq, uint8_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      hc_copystate((*running_task)->xcp.regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/hc/src/common/hc_switchcontext.c b/arch/hc/src/common/hc_switchcontext.c
index 7ea2c275abd..f75ee8576b8 100644
--- a/arch/hc/src/common/hc_switchcontext.c
+++ b/arch/hc/src/common/hc_switchcontext.c
@@ -102,6 +102,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       hc_fullcontextrestore(tcb->xcp.regs);
diff --git a/arch/mips/src/mips32/mips_doirq.c b/arch/mips/src/mips32/mips_doirq.c
index 36c3151ccc2..8a08dc8c0ce 100644
--- a/arch/mips/src/mips32/mips_doirq.c
+++ b/arch/mips/src/mips32/mips_doirq.c
@@ -58,6 +58,13 @@
 
 uint32_t *mips_doirq(int irq, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      mips_copystate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/mips/src/pic32mx/pic32mx_decodeirq.c b/arch/mips/src/pic32mx/pic32mx_decodeirq.c
index 49f0c8b00d6..3b653f8ea88 100644
--- a/arch/mips/src/pic32mx/pic32mx_decodeirq.c
+++ b/arch/mips/src/pic32mx/pic32mx_decodeirq.c
@@ -70,12 +70,18 @@
 
 uint32_t *pic32mx_decodeirq(uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
 #ifdef CONFIG_PIC32MX_NESTED_INTERRUPTS
   uint32_t *savestate;
 #endif
   uint32_t regval;
   int irq;
 
+  if (*running_task != NULL)
+    {
+      mips_copystate((*running_task)->xcp.regs, regs);
+    }
+
   /* If the board supports LEDs, turn on an LED now to indicate that we are
    * processing an interrupt.
    */
diff --git a/arch/mips/src/pic32mx/pic32mx_exception.c b/arch/mips/src/pic32mx/pic32mx_exception.c
index 98f8c6dfbd0..85f8bf8c954 100644
--- a/arch/mips/src/pic32mx/pic32mx_exception.c
+++ b/arch/mips/src/pic32mx/pic32mx_exception.c
@@ -53,11 +53,17 @@
 
 uint32_t *pic32mx_exception(uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
 #ifdef CONFIG_DEBUG_FEATURES
   uint32_t cause;
   uint32_t epc;
 #endif
 
+  if (*running_task != NULL)
+    {
+      mips_copystate((*running_task)->xcp.regs, regs);
+    }
+
   /* If the board supports LEDs, turn on an LED now to indicate that we are
    * processing an interrupt.
    */
diff --git a/arch/mips/src/pic32mz/pic32mz_decodeirq.c b/arch/mips/src/pic32mz/pic32mz_decodeirq.c
index d64ff57c216..4c742e26465 100644
--- a/arch/mips/src/pic32mz/pic32mz_decodeirq.c
+++ b/arch/mips/src/pic32mz/pic32mz_decodeirq.c
@@ -69,6 +69,13 @@
 
 uint32_t *pic32mz_decodeirq(uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      mips_copystate((*running_task)->xcp.regs, regs);
+    }
+
 #ifdef CONFIG_PIC32MZ_NESTED_INTERRUPTS
   uint32_t *savestate;
 #endif
diff --git a/arch/mips/src/pic32mz/pic32mz_exception.c b/arch/mips/src/pic32mz/pic32mz_exception.c
index 4cca474cdb3..d16da4c1c34 100644
--- a/arch/mips/src/pic32mz/pic32mz_exception.c
+++ b/arch/mips/src/pic32mz/pic32mz_exception.c
@@ -52,11 +52,17 @@
 
 uint32_t *pic32mz_exception(uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
 #ifdef CONFIG_DEBUG_FEATURES
   uint32_t cause;
   uint32_t epc;
 #endif
 
+  if (*running_task != NULL)
+    {
+      mips_copystate((*running_task)->xcp.regs, regs);
+    }
+
   /* If the board supports LEDs, turn on an LED now to indicate that we are
    * processing an interrupt.
    */
diff --git a/arch/misoc/src/lm32/lm32_doirq.c b/arch/misoc/src/lm32/lm32_doirq.c
index 3472df00bec..079429b4d8a 100644
--- a/arch/misoc/src/lm32/lm32_doirq.c
+++ b/arch/misoc/src/lm32/lm32_doirq.c
@@ -45,6 +45,13 @@
 
 uint32_t *lm32_doirq(int irq, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      lm32_copystate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 
   /* Current regs non-zero indicates that we are processing an interrupt;
diff --git a/arch/misoc/src/minerva/minerva_doirq.c b/arch/misoc/src/minerva/minerva_doirq.c
index 0a4cca5d7ea..815e1c242cb 100644
--- a/arch/misoc/src/minerva/minerva_doirq.c
+++ b/arch/misoc/src/minerva/minerva_doirq.c
@@ -45,6 +45,13 @@
 
 uint32_t *minerva_doirq(int irq, uint32_t * regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      minerva_copystate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 
   /* Current regs non-zero indicates that we are processing an interrupt;
diff --git a/arch/or1k/src/common/or1k_doirq.c b/arch/or1k/src/common/or1k_doirq.c
index 42bb4f6a03a..ecd6e33a43c 100644
--- a/arch/or1k/src/common/or1k_doirq.c
+++ b/arch/or1k/src/common/or1k_doirq.c
@@ -41,6 +41,13 @@
 
 uint32_t *or1k_doirq(int irq, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      or1k_copyfullstate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/or1k/src/common/or1k_switchcontext.c b/arch/or1k/src/common/or1k_switchcontext.c
index c19cf847ed9..1e0921e2512 100644
--- a/arch/or1k/src/common/or1k_switchcontext.c
+++ b/arch/or1k/src/common/or1k_switchcontext.c
@@ -106,6 +106,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       or1k_fullcontextrestore(tcb->xcp.regs);
diff --git a/arch/renesas/src/common/renesas_doirq.c b/arch/renesas/src/common/renesas_doirq.c
index 06af0e84c4e..a6503087c4a 100644
--- a/arch/renesas/src/common/renesas_doirq.c
+++ b/arch/renesas/src/common/renesas_doirq.c
@@ -58,6 +58,13 @@
 
 uint32_t *renesas_doirq(int irq, uint32_t * regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      renesas_copystate((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/renesas/src/common/renesas_switchcontext.c b/arch/renesas/src/common/renesas_switchcontext.c
index b4a79c922b2..9ff14868c91 100644
--- a/arch/renesas/src/common/renesas_switchcontext.c
+++ b/arch/renesas/src/common/renesas_switchcontext.c
@@ -102,6 +102,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       renesas_fullcontextrestore(tcb->xcp.regs);
diff --git a/arch/risc-v/src/common/riscv_doirq.c b/arch/risc-v/src/common/riscv_doirq.c
index bbb1db9fb0a..dd8e094cd93 100644
--- a/arch/risc-v/src/common/riscv_doirq.c
+++ b/arch/risc-v/src/common/riscv_doirq.c
@@ -99,7 +99,7 @@ uintreg_t *riscv_doirq(int irq, uintreg_t *regs)
    * returning from the interrupt.
    */
 
-  if ((*running_task) != tcb)
+  if (*running_task != tcb)
     {
 #ifdef CONFIG_ARCH_ADDRENV
       /* Make sure that the address environment for the previously
@@ -113,7 +113,7 @@ uintreg_t *riscv_doirq(int irq, uintreg_t *regs)
 
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task when context switch occurred.
diff --git a/arch/risc-v/src/common/supervisor/riscv_perform_syscall.c b/arch/risc-v/src/common/supervisor/riscv_perform_syscall.c
index aa36a12c65d..00fd5f54b2e 100644
--- a/arch/risc-v/src/common/supervisor/riscv_perform_syscall.c
+++ b/arch/risc-v/src/common/supervisor/riscv_perform_syscall.c
@@ -54,7 +54,7 @@ void *riscv_perform_syscall(uintreg_t *regs)
   riscv_swint(0, regs, NULL);
   tcb = this_task();
 
-  if ((*running_task) != tcb)
+  if (*running_task != tcb)
     {
 #ifdef CONFIG_ARCH_ADDRENV
       /* Make sure that the address environment for the previously
@@ -67,7 +67,7 @@ void *riscv_perform_syscall(uintreg_t *regs)
 #endif
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task.  g_running_tasks[] is only used by
diff --git a/arch/sim/src/sim/sim_doirq.c b/arch/sim/src/sim/sim_doirq.c
index 5adcf15cdfa..5d75d76bf20 100644
--- a/arch/sim/src/sim/sim_doirq.c
+++ b/arch/sim/src/sim/sim_doirq.c
@@ -63,6 +63,13 @@ void *sim_doirq(int irq, void *context)
   sim_saveusercontext(regs, ret);
   if (ret == 0)
     {
+      struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+      if (*running_task != NULL)
+        {
+          sim_copyfullstate((*running_task)->xcp.regs, regs);
+        }
+
       up_set_current_regs(regs);
 
       /* Deliver the IRQ */
diff --git a/arch/sim/src/sim/sim_switchcontext.c b/arch/sim/src/sim/sim_switchcontext.c
index fc8d9d703df..83a845ad6f4 100644
--- a/arch/sim/src/sim/sim_switchcontext.c
+++ b/arch/sim/src/sim/sim_switchcontext.c
@@ -105,6 +105,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       restore_critical_section(tcb, this_cpu());
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       sim_fullcontextrestore(tcb->xcp.regs);
diff --git a/arch/sparc/src/sparc_v8/sparc_v8_doirq.c b/arch/sparc/src/sparc_v8/sparc_v8_doirq.c
index f596139c34e..05e1ed77057 100644
--- a/arch/sparc/src/sparc_v8/sparc_v8_doirq.c
+++ b/arch/sparc/src/sparc_v8/sparc_v8_doirq.c
@@ -58,6 +58,13 @@
 
 uint32_t *sparc_doirq(int irq, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      trap_flush_task((*running_task)->xcp.regs, regs);
+    }
+
   board_autoled_on(LED_INIRQ);
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
diff --git a/arch/tricore/src/common/tricore_doirq.c b/arch/tricore/src/common/tricore_doirq.c
index f3ed8151ea7..975ad21fef8 100644
--- a/arch/tricore/src/common/tricore_doirq.c
+++ b/arch/tricore/src/common/tricore_doirq.c
@@ -44,12 +44,19 @@
 
 IFX_INTERRUPT_INTERNAL(tricore_doirq, 0, 255)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   PANIC();
 #else
   Ifx_CPU_ICR icr;
   uintptr_t *regs;
 
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
+
   icr.U = __mfcr(CPU_ICR);
   regs = (uintptr_t *)__mfcr(CPU_PCXI);
 
diff --git a/arch/x86/src/common/x86_switchcontext.c b/arch/x86/src/common/x86_switchcontext.c
index 2ab1e358d2c..b1d7f1147c1 100644
--- a/arch/x86/src/common/x86_switchcontext.c
+++ b/arch/x86/src/common/x86_switchcontext.c
@@ -102,6 +102,10 @@ void up_switch_context(struct tcb_s *tcb, struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       x86_fullcontextrestore(tcb->xcp.regs);
diff --git a/arch/x86/src/qemu/qemu_handlers.c b/arch/x86/src/qemu/qemu_handlers.c
index 091d75ff74b..ab9093d9963 100644
--- a/arch/x86/src/qemu/qemu_handlers.c
+++ b/arch/x86/src/qemu/qemu_handlers.c
@@ -76,6 +76,7 @@ static void idt_outb(uint8_t val, uint16_t addr)
 #ifndef CONFIG_SUPPRESS_INTERRUPTS
 static uint32_t *common_handler(int irq, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
   board_autoled_on(LED_INIRQ);
 
   /* Current regs non-zero indicates that we are processing an interrupt;
@@ -87,6 +88,11 @@ static uint32_t *common_handler(int irq, uint32_t *regs)
   DEBUGASSERT(up_current_regs() == NULL);
   up_set_current_regs(regs);
 
+  if (*running_task != NULL)
+    {
+      x86_savestate((*running_task)->xcp.regs);
+    }
+
   /* Deliver the IRQ */
 
   irq_dispatch(irq, regs);
@@ -118,7 +124,7 @@ static uint32_t *common_handler(int irq, uint32_t *regs)
 
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(this_task());
 
       /* Record the new "running" task when context switch occurred.
@@ -126,7 +132,7 @@ static uint32_t *common_handler(int irq, uint32_t *regs)
        * crashes.
        */
 
-      g_running_tasks[this_cpu()] = this_task();
+      *running_task = this_task();
     }
 
   /* If a context switch occurred while processing the interrupt then
diff --git a/arch/x86_64/src/intel64/intel64_handlers.c b/arch/x86_64/src/intel64/intel64_handlers.c
index c17f8fc8c28..2d234b21bcd 100644
--- a/arch/x86_64/src/intel64/intel64_handlers.c
+++ b/arch/x86_64/src/intel64/intel64_handlers.c
@@ -62,9 +62,15 @@
 #ifndef CONFIG_SUPPRESS_INTERRUPTS
 static uint64_t *common_handler(int irq, uint64_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
   struct tcb_s *tcb;
   int cpu;
 
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
+
   /* Current regs non-zero indicates that we are processing an interrupt;
    * g_current_regs is also used to manage interrupt level context switches.
    *
@@ -99,7 +105,7 @@ static uint64_t *common_handler(int irq, uint64_t *regs)
       /* Update scheduler parameters */
 
       cpu = this_cpu();
-      nxsched_suspend_scheduler(g_running_tasks[cpu]);
+      nxsched_suspend_scheduler(*running_task);
       nxsched_resume_scheduler(this_task());
 
       /* Record the new "running" task when context switch occurred.
@@ -146,6 +152,13 @@ static uint64_t *common_handler(int irq, uint64_t *regs)
 
 uint64_t *isr_handler(uint64_t *regs, uint64_t irq)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
+
   board_autoled_on(LED_INIRQ);
 
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
diff --git a/arch/xtensa/src/common/xtensa_assert.c b/arch/xtensa/src/common/xtensa_assert.c
index 0b51bd603ac..e3a6abbff20 100644
--- a/arch/xtensa/src/common/xtensa_assert.c
+++ b/arch/xtensa/src/common/xtensa_assert.c
@@ -65,6 +65,13 @@
 
 void xtensa_panic(int xptcode, uint32_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      (*running_task)->xcp.regs = regs;
+    }
+
   up_set_current_regs(regs);
 
   /* We get here when a un-dispatch-able, irrecoverable exception occurs */
diff --git a/arch/xtensa/src/common/xtensa_irqdispatch.c b/arch/xtensa/src/common/xtensa_irqdispatch.c
index c3023994c8d..46e848c777c 100644
--- a/arch/xtensa/src/common/xtensa_irqdispatch.c
+++ b/arch/xtensa/src/common/xtensa_irqdispatch.c
@@ -44,7 +44,8 @@
 
 uint32_t *xtensa_irq_dispatch(int irq, uint32_t *regs)
 {
-  struct tcb_s *tcb = this_task();
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+  struct tcb_s *tcb;
 
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   board_autoled_on(LED_INIRQ);
@@ -64,11 +65,9 @@ uint32_t *xtensa_irq_dispatch(int irq, uint32_t *regs)
 
   up_set_current_regs(regs);
 
-  if (irq != XTENSA_IRQ_SWINT)
+  if (*running_task != NULL)
     {
-      /* we are not trigger by syscall */
-
-      tcb->xcp.regs = regs;
+      (*running_task)->xcp.regs = regs;
     }
 
   /* Deliver the IRQ */
@@ -80,7 +79,7 @@ uint32_t *xtensa_irq_dispatch(int irq, uint32_t *regs)
    * current_regs will have a different value than it did on entry.
    */
 
-  if (regs != tcb->xcp.regs)
+  if (*running_task != tcb)
     {
 #ifdef CONFIG_ARCH_ADDRENV
       /* Make sure that the address environment for the previously
@@ -94,16 +93,15 @@ uint32_t *xtensa_irq_dispatch(int irq, uint32_t *regs)
 
       /* Update scheduler parameters */
 
-      nxsched_suspend_scheduler(g_running_tasks[this_cpu()]);
-      nxsched_resume_scheduler(this_task());
+      nxsched_suspend_scheduler(*running_task);
+      nxsched_resume_scheduler(tcb);
 
       /* Record the new "running" task when context switch occurred.
        * g_running_tasks[] is only used by assertion logic for reporting
        * crashes.
        */
 
-      g_running_tasks[this_cpu()] = tcb;
-      regs = tcb->xcp.regs;
+      *running_task = tcb;
     }
 
   /* Set current_regs to NULL to indicate that we are no longer in an
@@ -114,5 +112,5 @@ uint32_t *xtensa_irq_dispatch(int irq, uint32_t *regs)
 #endif
 
   board_autoled_off(LED_INIRQ);
-  return regs;
+  return tcb->xcp.regs;
 }
diff --git a/arch/xtensa/src/common/xtensa_swint.c b/arch/xtensa/src/common/xtensa_swint.c
index 943a7724307..fc48a03f3e0 100644
--- a/arch/xtensa/src/common/xtensa_swint.c
+++ b/arch/xtensa/src/common/xtensa_swint.c
@@ -59,12 +59,12 @@ int xtensa_swint(int irq, void *context, void *arg)
 {
   uint32_t *regs = (uint32_t *)context;
   struct tcb_s *tcb = this_task();
+  uintptr_t *new_regs = regs;
   uint32_t cmd;
 
   DEBUGASSERT(regs != NULL);
 
   cmd = regs[REG_A2];
-  tcb->xcp.regs = regs;
 
   /* The syscall software interrupt is called with A2 = system call command
    * and A3..A9 = variable number of arguments depending on the system call.
@@ -119,6 +119,7 @@ int xtensa_swint(int irq, void *context, void *arg)
       case SYS_restore_context:
         {
           DEBUGASSERT(regs[REG_A3] != 0);
+          new_regs = (uint32_t *)regs[REG_A3];
           tcb->xcp.regs = (uint32_t *)regs[REG_A3];
         }
         break;
@@ -142,9 +143,8 @@ int xtensa_swint(int irq, void *context, void *arg)
 
       case SYS_switch_context:
         {
-          DEBUGASSERT(regs[REG_A3] != 0 && regs[REG_A4] != 0);
-          *(uint32_t **)regs[REG_A3] = regs;
-          tcb->xcp.regs = (uint32_t *)regs[REG_A4];
+          DEBUGASSERT(regs[REG_A4] != 0);
+          new_regs = (uint32_t *)regs[REG_A4];
         }
         break;
 
@@ -432,10 +432,10 @@ int xtensa_swint(int irq, void *context, void *arg)
    */
 
 #ifdef CONFIG_DEBUG_SYSCALL_INFO
-  if (regs != tcb->xcp.regs)
+  if (regs != new_regs)
     {
       svcinfo("SYSCALL Return: Context switch!\n");
-      up_dump_register(tcb->xcp.regs);
+      up_dump_register(new_regs);
     }
   else
     {
@@ -443,7 +443,7 @@ int xtensa_swint(int irq, void *context, void *arg)
     }
 #endif
 
-  if (regs != tcb->xcp.regs)
+  if (regs != new_regs)
     {
       restore_critical_section(this_task(), this_cpu());
     }
diff --git a/arch/z16/src/common/z16_doirq.c b/arch/z16/src/common/z16_doirq.c
index 233f7788580..a9ed9605e3e 100644
--- a/arch/z16/src/common/z16_doirq.c
+++ b/arch/z16/src/common/z16_doirq.c
@@ -58,8 +58,14 @@ FAR chipreg_t *z16_doirq(int irq, FAR chipreg_t *regs)
 #else
   if ((unsigned)irq < NR_IRQS)
     {
+      struct tcb_s **running_task = &g_running_tasks[this_cpu()];
       FAR chipreg_t *savestate;
 
+      if (*running_task != NULL)
+        {
+          z16_copystate((*running_task)->xcp.regs, regs)
+        }
+
       /* Nested interrupts are not supported in this implementation.  If
        * you want to implement nested interrupts, you would have to (1)
        * change the way that g_current_regs is handled and (2) the design
diff --git a/arch/z16/src/common/z16_switchcontext.c b/arch/z16/src/common/z16_switchcontext.c
index f076ba37011..e703b996ffb 100644
--- a/arch/z16/src/common/z16_switchcontext.c
+++ b/arch/z16/src/common/z16_switchcontext.c
@@ -93,6 +93,10 @@ void up_switch_context(FAR struct tcb_s *tcb, FAR struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       RESTORE_USERCONTEXT(tcb);
diff --git a/arch/z16/src/z16f/z16f_sysexec.c b/arch/z16/src/z16f/z16f_sysexec.c
index 916aa679f61..5d38d2a410d 100644
--- a/arch/z16/src/z16f/z16f_sysexec.c
+++ b/arch/z16/src/z16f/z16f_sysexec.c
@@ -47,8 +47,14 @@
 
 void z16f_sysexec(FAR chipreg_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
   uint16_t excp;
 
+  if (*running_task != NULL)
+    {
+      z16_copystate((*running_task)->xcp.regs, regs)
+    }
+
   /* Save that register reference so that it can be used for built-in
    * diagnostics.
    */
diff --git a/arch/z80/src/common/z80_doirq.c b/arch/z80/src/common/z80_doirq.c
index 81bbc2b51b3..8429a41bfc8 100644
--- a/arch/z80/src/common/z80_doirq.c
+++ b/arch/z80/src/common/z80_doirq.c
@@ -44,6 +44,13 @@
 
 FAR chipreg_t *z80_doirq(uint8_t irq, FAR chipreg_t *regs)
 {
+  struct tcb_s **running_task = &g_running_tasks[this_cpu()];
+
+  if (*running_task != NULL)
+    {
+      z80_copystate((*running_task)->xcp.regs, regs)
+    }
+
   board_autoled_on(LED_INIRQ);
 
   DECL_SAVESTATE();
diff --git a/arch/z80/src/common/z80_switchcontext.c b/arch/z80/src/common/z80_switchcontext.c
index fde1cd3c0d1..08fdab13788 100644
--- a/arch/z80/src/common/z80_switchcontext.c
+++ b/arch/z80/src/common/z80_switchcontext.c
@@ -105,6 +105,10 @@ void up_switch_context(FAR struct tcb_s *tcb, FAR struct tcb_s *rtcb)
 
       nxsched_resume_scheduler(tcb);
 
+      /* Record the new "running" task */
+
+      g_running_tasks[this_cpu()] = tcb;
+
       /* Then switch contexts */
 
       RESTORE_USERCONTEXT(tcb);
diff --git a/fs/procfs/fs_procfstcbinfo.c b/fs/procfs/fs_procfstcbinfo.c
index e3253582733..08c1140b791 100644
--- a/fs/procfs/fs_procfstcbinfo.c
+++ b/fs/procfs/fs_procfstcbinfo.c
@@ -179,17 +179,17 @@ static int tcbinfo_close(FAR struct file *filep)
 }
 
 /****************************************************************************
- * Name: tcbinfo_current_regs
+ * Name: tcbinfo_running_regs
  *
  * Description:
- *   A special version of up_current_regs() that is non-optimized.
+ *   A special version of running_regs() that is non-optimized.
  *
  ****************************************************************************/
 
 nooptimiziation_function
-FAR static void *tcbinfo_current_regs(void)
+FAR static void *tcbinfo_running_regs(void)
 {
-  return up_current_regs();
+  return running_regs();
 }
 
 /****************************************************************************
@@ -215,8 +215,8 @@ static ssize_t tcbinfo_read(FAR struct file *filep, FAR char *buffer,
     {
       linesize = procfs_snprintf(attr->line, TCBINFO_LINELEN,
                                  "pointer %p size %zu current regs %p\n",
-                                  &g_tcbinfo, sizeof(struct tcbinfo_s),
-                                  tcbinfo_current_regs());
+                                 &g_tcbinfo, sizeof(struct tcbinfo_s),
+                                 tcbinfo_running_regs());
 
       /* Save the linesize in case we are re-entered with f_pos > 0 */
 
diff --git a/include/nuttx/sched.h b/include/nuttx/sched.h
index 960847b5b46..21cd765819a 100644
--- a/include/nuttx/sched.h
+++ b/include/nuttx/sched.h
@@ -244,6 +244,8 @@
 #  define this_cpu()                 (0)
 #endif
 
+#define running_regs()               ((void *)(g_running_tasks[this_cpu()]->xcp.regs))
+
 /****************************************************************************
  * Public Type Definitions
  ****************************************************************************/
@@ -872,6 +874,12 @@ EXTERN clock_t g_premp_max[CONFIG_SMP_NCPUS];
 EXTERN clock_t g_crit_max[CONFIG_SMP_NCPUS];
 #endif /* CONFIG_SCHED_CRITMONITOR_MAXTIME_CSECTION >= 0 */
 
+/* g_running_tasks[] holds a references to the running task for each CPU.
+ * It is valid only when up_interrupt_context() returns true.
+ */
+
+EXTERN FAR struct tcb_s *g_running_tasks[CONFIG_SMP_NCPUS];
+
 EXTERN const struct tcbinfo_s g_tcbinfo;
 
 /****************************************************************************
diff --git a/libs/libc/gdbstub/lib_gdbstub.c b/libs/libc/gdbstub/lib_gdbstub.c
index 45660591f5c..b54a8ab7803 100644
--- a/libs/libc/gdbstub/lib_gdbstub.c
+++ b/libs/libc/gdbstub/lib_gdbstub.c
@@ -1039,7 +1039,7 @@ static void gdb_get_registers(FAR struct gdb_state_s *state)
     {
       if (up_interrupt_context())
         {
-          reg = (FAR uint8_t *)up_current_regs();
+          reg = (FAR uint8_t *)running_regs();
         }
       else
         {
diff --git a/sched/init/nx_start.c b/sched/init/nx_start.c
index d45f68bbf15..01ba4df823f 100644
--- a/sched/init/nx_start.c
+++ b/sched/init/nx_start.c
@@ -129,7 +129,7 @@ dq_queue_t g_assignedtasks[CONFIG_SMP_NCPUS];
 FAR struct tcb_s *g_delivertasks[CONFIG_SMP_NCPUS];
 #endif
 
-/* g_running_tasks[] holds a references to the running task for each cpu.
+/* g_running_tasks[] holds a references to the running task for each CPU.
  * It is valid only when up_interrupt_context() returns true.
  */
 
diff --git a/sched/misc/assert.c b/sched/misc/assert.c
index 215f61991b4..103d9412ecd 100644
--- a/sched/misc/assert.c
+++ b/sched/misc/assert.c
@@ -301,7 +301,7 @@ static void dump_stacks(FAR struct tcb_s *rtcb, uintptr_t sp)
       /* Try to restore SP from current_regs if assert from interrupt. */
 
       tcbstack_sp = up_interrupt_context() ?
-                    up_getusrsp((FAR void *)up_current_regs()) : 0;
+                    up_getusrsp((FAR void *)running_regs()) : 0;
       if (tcbstack_sp < tcbstack_base || tcbstack_sp >= tcbstack_top)
         {
           tcbstack_sp = 0;
@@ -600,7 +600,7 @@ static void dump_deadlock(void)
 
 static noreturn_function int pause_cpu_handler(FAR void *arg)
 {
-  memcpy(g_last_regs[this_cpu()], up_current_regs(), sizeof(g_last_regs[0]));
+  memcpy(g_last_regs[this_cpu()], running_regs(), sizeof(g_last_regs[0]));
   g_cpu_paused[this_cpu()] = true;
   up_flush_dcache_all();
   while (1);
diff --git a/sched/misc/coredump.c b/sched/misc/coredump.c
index 4f39b51fa4f..1de2fb5f2b6 100644
--- a/sched/misc/coredump.c
+++ b/sched/misc/coredump.c
@@ -309,7 +309,7 @@ static void elf_emit_tcb_note(FAR struct elf_dumpinfo_s *cinfo,
     {
       if (up_interrupt_context())
         {
-          regs = (FAR uintptr_t *)up_current_regs();
+          regs = (FAR uintptr_t *)running_regs();
         }
       else
         {
diff --git a/sched/sched/sched.h b/sched/sched/sched.h
index d8ffdf2accb..a9784930610 100644
--- a/sched/sched/sched.h
+++ b/sched/sched/sched.h
@@ -197,12 +197,6 @@ extern dq_queue_t g_assignedtasks[CONFIG_SMP_NCPUS];
 
 extern FAR struct tcb_s *g_delivertasks[CONFIG_SMP_NCPUS];
 
-/* g_running_tasks[] holds a references to the running task for each cpu.
- * It is valid only when up_interrupt_context() returns true.
- */
-
-extern FAR struct tcb_s *g_running_tasks[CONFIG_SMP_NCPUS];
-
 /* This is the list of all tasks that are ready-to-run, but cannot be placed
  * in the g_readytorun list because:  (1) They are higher priority than the
  * currently active task at the head of the g_readytorun list, and (2) the