diff --git a/sched/task/task_restart.c b/sched/task/task_restart.c
index 51d8020d08059..65c08624a59ae 100644
--- a/sched/task/task_restart.c
+++ b/sched/task/task_restart.c
@@ -38,136 +38,104 @@
 #include "signal/signal.h"
 #include "task/task.h"
 
+#ifndef CONFIG_BUILD_KERNEL
+
 /****************************************************************************
- * Private Functions
+ * Private Type Declarations
  ****************************************************************************/
 
+#ifdef CONFIG_SMP
+struct restart_arg_s
+{
+  pid_t pid;
+  cpu_set_t saved_affinity;
+  uint16_t saved_flags;
+  bool need_restore;
+};
+
 /****************************************************************************
- * Name: nxtask_restart
- *
- * Description:
- *   This function "restarts" a task.  The task is first terminated and then
- *   reinitialized with same ID, priority, original entry point, stack size,
- *   and parameters it had when it was first started.
- *
- * Input Parameters:
- *   pid - The task ID of the task to delete.  An ID of zero signifies the
- *         calling task.
- *
- * Returned Value:
- *   Zero (OK) on success; or negated errno on failure
- *
- *   This function can fail if:
- *   (1) A pid of zero or the pid of the calling task is provided
- *      (functionality not implemented)
- *   (2) The pid is not associated with any task known to the system.
- *
+ * Private Functions
  ****************************************************************************/
 
-#ifndef CONFIG_BUILD_KERNEL
-static int nxtask_restart(pid_t pid)
+static int restart_handler(FAR void *cookie)
 {
-  FAR struct tcb_s *rtcb;
-  FAR struct task_tcb_s *tcb;
-  FAR dq_queue_t *tasklist;
+  FAR struct restart_arg_s *arg = cookie;
+  FAR struct tcb_s *tcb;
   irqstate_t flags;
-  int ret;
-#ifdef CONFIG_SMP
-  int cpu;
-#endif
 
-  /* Check if the task to restart is the calling task */
+  flags = enter_critical_section();
 
-  rtcb = this_task();
-  if ((pid == 0) || (pid == rtcb->pid))
+  /* tcb that we want restart */
+
+  tcb = nxsched_get_tcb(arg->pid);
+  if (!tcb || tcb->task_state == TSTATE_TASK_INVALID ||
+      (tcb->flags & TCB_FLAG_EXIT_PROCESSING) != 0)
     {
-      /* Not implemented */
+      /* There is no TCB with this pid or, if there is, it is not a task. */
 
-      ret = -ENOSYS;
-      goto errout;
+      leave_critical_section(flags);
+      return -ESRCH;
     }
 
-  /* We are restarting some other task than ourselves.  Make sure that the
-   * task does not change its state while we are executing.  In the single
-   * CPU state this could be done by disabling pre-emption.  But we will
-   * a little stronger medicine on the SMP case:  The task make be running
-   * on another CPU.
-   */
+  if (arg->need_restore)
+    {
+      tcb->affinity = arg->saved_affinity;
+      tcb->flags = arg->saved_flags;
+    }
 
-  flags = enter_critical_section();
+  nxsched_remove_readytorun(tcb, false);
 
-  /* Find for the TCB associated with matching pid  */
+  leave_critical_section(flags);
 
-  tcb = (FAR struct task_tcb_s *)nxsched_get_tcb(pid);
-#ifndef CONFIG_DISABLE_PTHREAD
-  if (!tcb || (tcb->cmn.flags & TCB_FLAG_TTYPE_MASK) ==
-      TCB_FLAG_TTYPE_PTHREAD)
-#else
-  if (!tcb)
+  return OK;
+}
 #endif
-    {
-      /* There is no TCB with this pid or, if there is, it is not a task. */
 
-      ret = -ESRCH;
-      goto errout_with_lock;
-    }
-
-#ifdef CONFIG_SMP
-  /* If the task is running on another CPU, then pause that CPU.  We can
-   * then manipulate the TCB of the restarted task and when we resume the
-   * that CPU, the restart take effect.
-   */
-
-  cpu = nxsched_pause_cpu(&tcb->cmn);
-#endif /* CONFIG_SMP */
+/****************************************************************************
+ * Name: nxtask_reset_task
+ *
+ * Description:
+ *   We use this function to reset tcb
+ *
+ ****************************************************************************/
 
+static void nxtask_reset_task(FAR struct tcb_s *tcb, bool remove)
+{
   /* Try to recover from any bad states */
 
-  nxtask_recover((FAR struct tcb_s *)tcb);
+  nxtask_recover(tcb);
 
   /* Kill any children of this thread */
 
 #ifdef HAVE_GROUP_MEMBERS
-  group_kill_children((FAR struct tcb_s *)tcb);
+  group_kill_children(tcb);
 #endif
 
   /* Remove the TCB from whatever list it is in.  After this point, the TCB
    * should no longer be accessible to the system
    */
 
-#ifdef CONFIG_SMP
-  if ((FAR struct tcb_s *)tcb == g_delivertasks[tcb->cmn.cpu])
+  if (remove)
     {
-      g_delivertasks[tcb->cmn.cpu] = NULL;
+      nxsched_remove_readytorun(tcb, false);
     }
-  else
-    {
-      tasklist = TLIST_HEAD(&tcb->cmn, tcb->cmn.cpu);
-      dq_rem((FAR dq_entry_t *)tcb, tasklist);
-    }
-#else
-  tasklist = TLIST_HEAD(&tcb->cmn);
-  dq_rem((FAR dq_entry_t *)tcb, tasklist);
-#endif
-
-  tcb->cmn.task_state = TSTATE_TASK_INVALID;
 
   /* Deallocate anything left in the TCB's signal queues */
 
-  nxsig_cleanup((FAR struct tcb_s *)tcb);  /* Deallocate Signal lists */
-  sigemptyset(&tcb->cmn.sigprocmask);      /* Reset sigprocmask */
+  nxsig_cleanup(tcb);             /* Deallocate Signal lists */
+  sigemptyset(&tcb->sigprocmask); /* Reset sigprocmask */
 
   /* Reset the current task priority  */
 
-  tcb->cmn.sched_priority = tcb->cmn.init_priority;
+  tcb->sched_priority = tcb->init_priority;
 
   /* The task should restart with pre-emption disabled and not in a critical
    * section.
    */
 
-  tcb->cmn.lockcount = 0;
+  tcb->lockcount = 0;
 #ifdef CONFIG_SMP
-  tcb->cmn.irqcount  = 0;
+  tcb->irqcount  = 0;
 #endif
 
   /* Reset the base task priority and the number of pending
@@ -175,44 +143,141 @@ static int nxtask_restart(pid_t pid)
    */
 
 #ifdef CONFIG_PRIORITY_INHERITANCE
-  tcb->cmn.base_priority = tcb->cmn.init_priority;
-  tcb->cmn.boost_priority = 0;
+  tcb->base_priority = tcb->init_priority;
+  tcb->boost_priority = 0;
 #endif
 
   /* Re-initialize the processor-specific portion of the TCB.  This will
    * reset the entry point and the start-up parameters
    */
 
-  up_initial_state((FAR struct tcb_s *)tcb);
+  up_initial_state(tcb);
 
   /* Add the task to the inactive task list */
 
   dq_addfirst((FAR dq_entry_t *)tcb, list_inactivetasks());
-  tcb->cmn.task_state = TSTATE_TASK_INACTIVE;
+  tcb->task_state = TSTATE_TASK_INACTIVE;
+}
 
-#ifdef CONFIG_SMP
-  /* Resume the paused CPU (if any) */
+/****************************************************************************
+ * Name: nxtask_restart
+ *
+ * Description:
+ *   This function "restarts" a task.  The task is first terminated and then
+ *   reinitialized with same ID, priority, original entry point, stack size,
+ *   and parameters it had when it was first started.
+ *
+ * Input Parameters:
+ *   pid - The task ID of the task to delete.  An ID of zero signifies the
+ *         calling task.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; or negated errno on failure
+ *
+ *   This function can fail if:
+ *   (1) A pid of zero or the pid of the calling task is provided
+ *      (functionality not implemented)
+ *   (2) The pid is not associated with any task known to the system.
+ *
+ ****************************************************************************/
+
+static int nxtask_restart(pid_t pid)
+{
+  FAR struct tcb_s *rtcb;
+  FAR struct tcb_s *tcb;
+  irqstate_t flags;
+  int ret;
+
+  /* We are restarting some other task than ourselves.  Make sure that the
+   * task does not change its state while we are executing.  In the single
+   * CPU state this could be done by disabling pre-emption.  But we will
+   * a little stronger medicine on the SMP case:  The task make be running
+   * on another CPU.
+   */
+
+  flags = enter_critical_section();
+
+  /* Check if the task to restart is the calling task */
+
+  rtcb = this_task();
+  if (pid == 0 || pid == rtcb->pid)
+    {
+      /* Not implemented */
+
+      ret = -ENOSYS;
+      goto errout_with_lock;
+    }
+
+  /* Find for the TCB associated with matching pid  */
+
+  tcb = nxsched_get_tcb(pid);
+#ifndef CONFIG_DISABLE_PTHREAD
+  if (!tcb || (tcb->flags & TCB_FLAG_TTYPE_MASK) ==
+      TCB_FLAG_TTYPE_PTHREAD)
+#else
+  if (!tcb)
+#endif
+    {
+      /* There is no TCB with this pid or, if there is, it is not a task. */
+
+      ret = -ESRCH;
+      goto errout_with_lock;
+    }
 
-  if (cpu >= 0)
+#ifdef CONFIG_SMP
+  if (tcb->task_state == TSTATE_TASK_RUNNING &&
+      tcb->cpu != this_cpu())
     {
-      ret = up_cpu_resume(cpu);
-      if (ret < 0)
+      struct restart_arg_s arg;
+
+      if ((tcb->flags & TCB_FLAG_CPU_LOCKED) != 0)
+        {
+          arg.pid = tcb->pid;
+          arg.need_restore = false;
+        }
+      else
         {
+          arg.pid = tcb->pid;
+          arg.saved_flags = tcb->flags;
+          arg.saved_affinity = tcb->affinity;
+          arg.need_restore = true;
+
+          tcb->flags |= TCB_FLAG_CPU_LOCKED;
+          CPU_SET(tcb->cpu, &tcb->affinity);
+        }
+
+      nxsched_smp_call_single(tcb->cpu, restart_handler, &arg, true);
+
+      tcb = nxsched_get_tcb(pid);
+      if (!tcb || tcb->task_state != TSTATE_TASK_INVALID ||
+          (tcb->flags & TCB_FLAG_EXIT_PROCESSING) != 0)
+        {
+          ret = -ESRCH;
           goto errout_with_lock;
         }
+
+      DEBUGASSERT(tcb->task_state != TSTATE_TASK_RUNNING);
+      nxtask_reset_task(tcb, false);
+      leave_critical_section(flags);
+
+      /* Activate the task. */
+
+      nxtask_activate(tcb);
+
+      return OK;
     }
 #endif /* CONFIG_SMP */
 
+  nxtask_reset_task(tcb, true);
   leave_critical_section(flags);
 
   /* Activate the task. */
 
-  nxtask_activate((FAR struct tcb_s *)tcb);
+  nxtask_activate(tcb);
   return OK;
 
 errout_with_lock:
   leave_critical_section(flags);
-errout:
   return ret;
 }