diff --git a/sched/sched/sched.h b/sched/sched/sched.h
index db20164b90e..cd75865003e 100644
--- a/sched/sched/sched.h
+++ b/sched/sched/sched.h
@@ -577,7 +577,7 @@ static inline_function int nxsched_select_cpu(cpu_set_t affinity)
   int i;
 
   minprio = SCHED_PRIORITY_MAX;
-  cpu     = CONFIG_SMP_NCPUS;
+  cpu     = 0xff;
 
   for (i = 0; i < CONFIG_SMP_NCPUS; i++)
     {
@@ -600,8 +600,7 @@ static inline_function int nxsched_select_cpu(cpu_set_t affinity)
               DEBUGASSERT(rtcb->sched_priority == 0);
               return i;
             }
-          else if (rtcb->sched_priority <= minprio &&
-                   !nxsched_islocked_tcb(rtcb))
+          else if (rtcb->sched_priority <= minprio)
             {
               DEBUGASSERT(rtcb->sched_priority > 0);
               minprio = rtcb->sched_priority;
@@ -610,6 +609,7 @@ static inline_function int nxsched_select_cpu(cpu_set_t affinity)
         }
     }
 
+  DEBUGASSERT(cpu != 0xff);
   return cpu;
 }
 #  endif
diff --git a/sched/sched/sched_addreadytorun.c b/sched/sched/sched_addreadytorun.c
index d4bc4e66d87..2724a1bd5ac 100644
--- a/sched/sched/sched_addreadytorun.c
+++ b/sched/sched/sched_addreadytorun.c
@@ -163,7 +163,7 @@ bool nxsched_switch_running(int cpu, bool switch_equal)
       return false;
     }
 
-  if (switch_equal)
+  if (switch_equal && sched_priority > 0)
     {
       sched_priority--;
     }
@@ -255,6 +255,7 @@ bool nxsched_add_readytorun(FAR struct tcb_s *btcb)
   bool doswitch = false;
   int target_cpu = btcb->flags & TCB_FLAG_CPU_LOCKED ? btcb->cpu :
     nxsched_select_cpu(btcb->affinity);
+  FAR struct tcb_s *tcb = current_task(target_cpu);
 
   /* Add the btcb to the ready to run list, and try to run it on the target
    * CPU
@@ -263,15 +264,13 @@ bool nxsched_add_readytorun(FAR struct tcb_s *btcb)
   btcb->task_state = TSTATE_TASK_READYTORUN;
   nxsched_add_prioritized(btcb, list_readytorun());
 
-  if (target_cpu < CONFIG_SMP_NCPUS)
-    {
-      FAR struct tcb_s *tcb = current_task(target_cpu);
+  /* In some cases, such as setaffinity, cpu need to be used. */
 
-      if (tcb->sched_priority < btcb->sched_priority)
-        {
-          doswitch = nxsched_deliver_task(this_cpu(), target_cpu,
-                                          SWITCH_HIGHER);
-        }
+  btcb->cpu = target_cpu;
+  if (tcb->sched_priority < btcb->sched_priority)
+    {
+      doswitch = nxsched_deliver_task(this_cpu(), target_cpu,
+                                      SWITCH_HIGHER);
     }
 
   return doswitch;
diff --git a/sched/sched/sched_process_delivered.c b/sched/sched/sched_process_delivered.c
index 663161de754..36effcd028d 100644
--- a/sched/sched/sched_process_delivered.c
+++ b/sched/sched/sched_process_delivered.c
@@ -105,7 +105,7 @@ void nxsched_process_delivered(int cpu)
         {
           int target_cpu = tcb->flags & TCB_FLAG_CPU_LOCKED ?
             tcb->cpu : nxsched_select_cpu(tcb->affinity);
-          if (target_cpu < CONFIG_SMP_NCPUS && target_cpu != cpu &&
+          if (target_cpu != cpu &&
               current_task(target_cpu)->sched_priority < tcb->sched_priority)
             {
               nxsched_deliver_task(cpu, target_cpu, priority);
diff --git a/sched/sched/sched_removereadytorun.c b/sched/sched/sched_removereadytorun.c
index 0166c443fe6..88b11263a44 100644
--- a/sched/sched/sched_removereadytorun.c
+++ b/sched/sched/sched_removereadytorun.c
@@ -162,12 +162,13 @@ static void nxsched_remove_running(FAR struct tcb_s *tcb)
   nxttcb->task_state = TSTATE_TASK_RUNNING;
   g_assignedtasks[cpu] = nxttcb;
   up_update_task(nxttcb);
+
+  nxsched_switch_running(tcb->cpu, false);
 }
 
 void nxsched_remove_self(FAR struct tcb_s *tcb)
 {
   nxsched_remove_running(tcb);
-  nxsched_switch_running(tcb->cpu, false);
 }
 
 bool nxsched_remove_readytorun(FAR struct tcb_s *tcb)