5 files changed, 259 insertions, 48 deletions

diff --git a/cpukit/score/src/scheduleredfsmp.c b/cpukit/score/src/scheduleredfsmp.c
index 0b0ee6ed21..28266dd13d 100644
--- a/cpukit/score/src/scheduleredfsmp.c
+++ b/cpukit/score/src/scheduleredfsmp.c
@@ -409,10 +409,36 @@ static inline void _Scheduler_EDF_SMP_Move_from_ready_to_scheduled(
   );
 }
 
+static inline Scheduler_Node *_Scheduler_EDF_SMP_Get_idle( void *arg )
+{
+  Scheduler_EDF_SMP_Context *self;
+  Scheduler_Node            *lowest_ready;
+
+  self = _Scheduler_EDF_SMP_Get_self( arg );
+  lowest_ready = (Scheduler_Node *) _RBTree_Maximum( &self->Ready[ 0 ].Queue );
+  _Assert( lowest_ready != NULL );
+  _RBTree_Extract( &self->Ready[ 0 ].Queue, &lowest_ready->Node.RBTree );
+  _Chain_Initialize_node( &lowest_ready->Node.Chain );
+
+  return lowest_ready;
+}
+
+static inline void _Scheduler_EDF_SMP_Release_idle(
+  Scheduler_Node *node,
+  void           *arg
+)
+{
+  Scheduler_EDF_SMP_Context *self;
+
+  self = _Scheduler_EDF_SMP_Get_self( arg );
+  _RBTree_Initialize_node( &node->Node.RBTree );
+  _RBTree_Append( &self->Ready[ 0 ].Queue, &node->Node.RBTree );
+}
+
 static inline void _Scheduler_EDF_SMP_Allocate_processor(
   Scheduler_Context *context,
   Scheduler_Node    *scheduled_base,
-  Scheduler_Node    *victim_base,
+  Thread_Control    *victim_thread,
   Per_CPU_Control   *victim_cpu
 )
 {
@@ -420,7 +446,7 @@ static inline void _Scheduler_EDF_SMP_Allocate_processor(
   Scheduler_EDF_SMP_Node        *scheduled;
   uint8_t                        rqi;
 
-  (void) victim_base;
+  (void) victim_thread;
   self = _Scheduler_EDF_SMP_Get_self( context );
   scheduled = _Scheduler_EDF_SMP_Node_downcast( scheduled_base );
   rqi = scheduled->ready_queue_index;
@@ -471,7 +497,9 @@ void _Scheduler_EDF_SMP_Block(
     _Scheduler_EDF_SMP_Extract_from_ready,
     _Scheduler_EDF_SMP_Get_highest_ready,
     _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_EDF_SMP_Allocate_processor
+    _Scheduler_EDF_SMP_Allocate_processor,
+    _Scheduler_EDF_SMP_Get_idle,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -489,8 +517,11 @@ static inline bool _Scheduler_EDF_SMP_Enqueue(
     _Scheduler_EDF_SMP_Insert_ready,
     _Scheduler_EDF_SMP_Insert_scheduled,
     _Scheduler_EDF_SMP_Move_from_scheduled_to_ready,
+    _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
     _Scheduler_EDF_SMP_Get_lowest_scheduled,
-    _Scheduler_EDF_SMP_Allocate_processor
+    _Scheduler_EDF_SMP_Allocate_processor,
+    _Scheduler_EDF_SMP_Get_idle,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -510,7 +541,9 @@ static inline void _Scheduler_EDF_SMP_Enqueue_scheduled(
     _Scheduler_EDF_SMP_Insert_ready,
     _Scheduler_EDF_SMP_Insert_scheduled,
     _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_EDF_SMP_Allocate_processor
+    _Scheduler_EDF_SMP_Allocate_processor,
+    _Scheduler_EDF_SMP_Get_idle,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -527,7 +560,8 @@ void _Scheduler_EDF_SMP_Unblock(
     thread,
     node,
     _Scheduler_EDF_SMP_Do_update,
-    _Scheduler_EDF_SMP_Enqueue
+    _Scheduler_EDF_SMP_Enqueue,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -546,7 +580,8 @@ static inline bool _Scheduler_EDF_SMP_Do_ask_for_help(
     _Scheduler_EDF_SMP_Insert_scheduled,
     _Scheduler_EDF_SMP_Move_from_scheduled_to_ready,
     _Scheduler_EDF_SMP_Get_lowest_scheduled,
-    _Scheduler_EDF_SMP_Allocate_processor
+    _Scheduler_EDF_SMP_Allocate_processor,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -616,7 +651,9 @@ void _Scheduler_EDF_SMP_Withdraw_node(
     _Scheduler_EDF_SMP_Extract_from_ready,
     _Scheduler_EDF_SMP_Get_highest_ready,
     _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_EDF_SMP_Allocate_processor
+    _Scheduler_EDF_SMP_Allocate_processor,
+    _Scheduler_EDF_SMP_Get_idle,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -649,7 +686,9 @@ void _Scheduler_EDF_SMP_Clean_sticky(
     _Scheduler_EDF_SMP_Extract_from_ready,
     _Scheduler_EDF_SMP_Get_highest_ready,
     _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_EDF_SMP_Allocate_processor
+    _Scheduler_EDF_SMP_Allocate_processor,
+    _Scheduler_EDF_SMP_Get_idle,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -695,7 +734,9 @@ Thread_Control *_Scheduler_EDF_SMP_Remove_processor(
     cpu,
     _Scheduler_EDF_SMP_Extract_from_scheduled,
     _Scheduler_EDF_SMP_Extract_from_ready,
-    _Scheduler_EDF_SMP_Enqueue
+    _Scheduler_EDF_SMP_Enqueue,
+    _Scheduler_EDF_SMP_Get_idle,
+    _Scheduler_EDF_SMP_Release_idle
   );
 }
 
@@ -833,7 +874,9 @@ Status_Control _Scheduler_EDF_SMP_Set_affinity(
       _Scheduler_EDF_SMP_Get_highest_ready,
       _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
       _Scheduler_EDF_SMP_Enqueue,
-      _Scheduler_EDF_SMP_Allocate_processor
+      _Scheduler_EDF_SMP_Allocate_processor,
+      _Scheduler_EDF_SMP_Get_idle,
+      _Scheduler_EDF_SMP_Release_idle
     );
   }
 
diff --git a/cpukit/score/src/schedulerpriorityaffinitysmp.c b/cpukit/score/src/schedulerpriorityaffinitysmp.c
index 648c243589..7d971ef8d1 100644
--- a/cpukit/score/src/schedulerpriorityaffinitysmp.c
+++ b/cpukit/score/src/schedulerpriorityaffinitysmp.c
@@ -190,7 +190,9 @@ void _Scheduler_priority_affinity_SMP_Block(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_affinity_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 
   /*
@@ -262,8 +264,11 @@ static bool _Scheduler_priority_affinity_SMP_Enqueue_fifo(
     _Scheduler_priority_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
+    _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
     _Scheduler_priority_affinity_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -342,7 +347,10 @@ static void _Scheduler_priority_affinity_SMP_Check_for_migrations(
       lowest_scheduled,
       _Scheduler_SMP_Insert_scheduled,
       _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
-      _Scheduler_SMP_Allocate_processor_exact
+      _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
+      _Scheduler_SMP_Allocate_processor_exact,
+      _Scheduler_priority_SMP_Get_idle,
+      _Scheduler_priority_SMP_Release_idle
     );
   }
 }
@@ -363,7 +371,8 @@ void _Scheduler_priority_affinity_SMP_Unblock(
     thread,
     node,
     _Scheduler_priority_SMP_Do_update,
-    _Scheduler_priority_affinity_SMP_Enqueue_fifo
+    _Scheduler_priority_affinity_SMP_Enqueue_fifo,
+    _Scheduler_priority_SMP_Release_idle
   );
 
   /*
@@ -390,8 +399,11 @@ static bool _Scheduler_priority_affinity_SMP_Enqueue(
     _Scheduler_priority_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
+    _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
     _Scheduler_priority_affinity_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -416,7 +428,9 @@ static void _Scheduler_priority_affinity_SMP_Enqueue_scheduled(
     _Scheduler_priority_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -435,7 +449,8 @@ static bool _Scheduler_priority_affinity_SMP_Do_ask_for_help(
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -510,7 +525,9 @@ void _Scheduler_priority_affinity_SMP_Withdraw_node(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_affinity_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -543,7 +560,9 @@ void _Scheduler_priority_affinity_SMP_Clean_sticky(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_affinity_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -575,7 +594,9 @@ Thread_Control *_Scheduler_priority_affinity_SMP_Remove_processor(
     cpu,
     _Scheduler_SMP_Extract_from_scheduled,
     _Scheduler_priority_SMP_Extract_from_ready,
-    _Scheduler_priority_affinity_SMP_Enqueue
+    _Scheduler_priority_affinity_SMP_Enqueue,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
diff --git a/cpukit/score/src/schedulerprioritysmp.c b/cpukit/score/src/schedulerprioritysmp.c
index 7262f48e8e..a33b44587f 100644
--- a/cpukit/score/src/schedulerprioritysmp.c
+++ b/cpukit/score/src/schedulerprioritysmp.c
@@ -50,6 +50,7 @@ void _Scheduler_priority_SMP_Initialize( const Scheduler_Control *scheduler )
     _Scheduler_priority_SMP_Get_context( scheduler );
 
   _Scheduler_SMP_Initialize( &self->Base );
+  self->idle_ready_queue = &self->Ready[ scheduler->maximum_priority ];
   _Priority_bit_map_Initialize( &self->Bit_map );
   _Scheduler_priority_Ready_queue_initialize(
     &self->Ready[ 0 ],
@@ -118,7 +119,9 @@ void _Scheduler_priority_SMP_Block(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -136,8 +139,11 @@ static bool _Scheduler_priority_SMP_Enqueue(
     _Scheduler_priority_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
+    _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -157,7 +163,9 @@ static void _Scheduler_priority_SMP_Enqueue_scheduled(
     _Scheduler_priority_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -174,7 +182,8 @@ void _Scheduler_priority_SMP_Unblock(
     thread,
     node,
     _Scheduler_priority_SMP_Do_update,
-    _Scheduler_priority_SMP_Enqueue
+    _Scheduler_priority_SMP_Enqueue,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -193,7 +202,8 @@ static bool _Scheduler_priority_SMP_Do_ask_for_help(
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -263,7 +273,9 @@ void _Scheduler_priority_SMP_Withdraw_node(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -296,7 +308,9 @@ void _Scheduler_priority_SMP_Clean_sticky(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
@@ -328,7 +342,9 @@ Thread_Control *_Scheduler_priority_SMP_Remove_processor(
     cpu,
     _Scheduler_SMP_Extract_from_scheduled,
     _Scheduler_priority_SMP_Extract_from_ready,
-    _Scheduler_priority_SMP_Enqueue
+    _Scheduler_priority_SMP_Enqueue,
+    _Scheduler_priority_SMP_Get_idle,
+    _Scheduler_priority_SMP_Release_idle
   );
 }
 
diff --git a/cpukit/score/src/schedulersimplesmp.c b/cpukit/score/src/schedulersimplesmp.c
index b7814b0fcb..086d00094d 100644
--- a/cpukit/score/src/schedulersimplesmp.c
+++ b/cpukit/score/src/schedulersimplesmp.c
@@ -172,6 +172,30 @@ static void _Scheduler_simple_SMP_Extract_from_ready(
   _Chain_Extract_unprotected( &node_to_extract->Node.Chain );
 }
 
+static inline Scheduler_Node *_Scheduler_simple_SMP_Get_idle( void *arg )
+{
+  Scheduler_simple_SMP_Context *self =
+    _Scheduler_simple_SMP_Get_self( arg );
+  Scheduler_Node *lowest_ready = (Scheduler_Node *) _Chain_Last( &self->Ready );
+
+  _Assert( &lowest_ready->Node.Chain != _Chain_Head( &self->Ready ) );
+  _Chain_Extract_unprotected( &lowest_ready->Node.Chain );
+
+  return lowest_ready;
+}
+
+static inline void _Scheduler_simple_SMP_Release_idle(
+  Scheduler_Node *node,
+  void           *arg
+)
+{
+  Scheduler_simple_SMP_Context *self;
+
+  self = _Scheduler_simple_SMP_Get_self( arg );
+
+  _Chain_Append_unprotected( &self->Ready, &node->Node.Chain );
+}
+
 void _Scheduler_simple_SMP_Block(
   const Scheduler_Control *scheduler,
   Thread_Control          *thread,
@@ -188,7 +212,9 @@ void _Scheduler_simple_SMP_Block(
     _Scheduler_simple_SMP_Extract_from_ready,
     _Scheduler_simple_SMP_Get_highest_ready,
     _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -206,8 +232,11 @@ static bool _Scheduler_simple_SMP_Enqueue(
     _Scheduler_simple_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_simple_SMP_Move_from_scheduled_to_ready,
+    _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -227,7 +256,9 @@ static void _Scheduler_simple_SMP_Enqueue_scheduled(
     _Scheduler_simple_SMP_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -244,7 +275,8 @@ void _Scheduler_simple_SMP_Unblock(
     thread,
     node,
     _Scheduler_simple_SMP_Do_update,
-    _Scheduler_simple_SMP_Enqueue
+    _Scheduler_simple_SMP_Enqueue,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -263,7 +295,8 @@ static bool _Scheduler_simple_SMP_Do_ask_for_help(
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_simple_SMP_Move_from_scheduled_to_ready,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -333,7 +366,9 @@ void _Scheduler_simple_SMP_Withdraw_node(
     _Scheduler_simple_SMP_Extract_from_ready,
     _Scheduler_simple_SMP_Get_highest_ready,
     _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -366,7 +401,9 @@ void _Scheduler_simple_SMP_Clean_sticky(
     _Scheduler_simple_SMP_Extract_from_ready,
     _Scheduler_simple_SMP_Get_highest_ready,
     _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
@@ -398,7 +435,9 @@ Thread_Control *_Scheduler_simple_SMP_Remove_processor(
     cpu,
     _Scheduler_SMP_Extract_from_scheduled,
     _Scheduler_simple_SMP_Extract_from_ready,
-    _Scheduler_simple_SMP_Enqueue
+    _Scheduler_simple_SMP_Enqueue,
+    _Scheduler_simple_SMP_Get_idle,
+    _Scheduler_simple_SMP_Release_idle
   );
 }
 
diff --git a/cpukit/score/src/schedulerstrongapa.c b/cpukit/score/src/schedulerstrongapa.c
index 6c7ab942fa..36ceaeddc1 100644
--- a/cpukit/score/src/schedulerstrongapa.c
+++ b/cpukit/score/src/schedulerstrongapa.c
@@ -157,14 +157,14 @@ static inline Scheduler_Node *_Scheduler_strong_APA_Get_scheduled(
 static inline void _Scheduler_strong_APA_Allocate_processor(
   Scheduler_Context *context,
   Scheduler_Node    *scheduled_base,
-  Scheduler_Node    *victim_base,
+  Thread_Control    *victim_thread,
   Per_CPU_Control   *victim_cpu
 )
 {
   Scheduler_strong_APA_Node    *scheduled;
   Scheduler_strong_APA_Context *self;
 
-  (void) victim_base;
+  (void) victim_thread;
 
   scheduled = _Scheduler_strong_APA_Node_downcast( scheduled_base );
   self = _Scheduler_strong_APA_Get_self( context );
@@ -270,6 +270,63 @@ static inline Scheduler_Node * _Scheduler_strong_APA_Find_highest_ready(
   return highest_ready;
 }
 
+static inline Scheduler_Node *_Scheduler_strong_APA_Get_idle( void *arg )
+{
+  Scheduler_strong_APA_Context *self;
+  Scheduler_strong_APA_Node    *lowest_ready = NULL;
+  Priority_Control              max_priority_num;
+  const Chain_Node             *tail;
+  Chain_Node                   *next;
+
+  self = _Scheduler_strong_APA_Get_self( arg );
+  tail = _Chain_Immutable_tail( &self->Ready );
+  next = _Chain_First( &self->Ready );
+  max_priority_num = 0;
+
+  while ( next != tail ) {
+    Scheduler_strong_APA_Node *node;
+    Scheduler_SMP_Node_state   curr_state;
+
+    node = (Scheduler_strong_APA_Node*) STRONG_SCHEDULER_NODE_OF_CHAIN( next );
+    curr_state = _Scheduler_SMP_Node_state( &node->Base.Base );
+
+    if ( curr_state == SCHEDULER_SMP_NODE_READY ) {
+      Priority_Control curr_priority;
+
+      curr_priority = _Scheduler_Node_get_priority( &node->Base.Base );
+
+      if ( curr_priority > max_priority_num ) {
+        max_priority_num = curr_priority;
+        lowest_ready = node;
+      }
+    }
+
+    next = _Chain_Next( next );
+  }
+
+  _Assert( lowest_ready != NULL );
+  _Chain_Extract_unprotected( &lowest_ready->Ready_node );
+  _Chain_Set_off_chain( &lowest_ready->Ready_node );
+
+  return &lowest_ready->Base.Base;
+}
+
+static inline void _Scheduler_strong_APA_Release_idle(
+  Scheduler_Node *node_base,
+  void           *arg
+)
+{
+  Scheduler_strong_APA_Context *self;
+  Scheduler_strong_APA_Node    *node;
+
+  self = _Scheduler_strong_APA_Get_self( arg );
+  node = _Scheduler_strong_APA_Node_downcast( node_base );
+
+  if ( _Chain_Is_node_off_chain( &node->Ready_node ) ) {
+    _Chain_Append_unprotected( &self->Ready, &node->Ready_node );
+  }
+}
+
 static inline void  _Scheduler_strong_APA_Move_from_ready_to_scheduled(
   Scheduler_Context *context,
   Scheduler_Node    *ready_to_scheduled
@@ -386,16 +443,24 @@ static inline Scheduler_Node *_Scheduler_strong_APA_Get_highest_ready(
      * So there is need for task shifting.
      */
     while ( node->cpu_to_preempt != filter_cpu ) {
+      Thread_Control *next_node_idle;
+
       curr_node = &node->Base.Base;
       next_node = _Scheduler_strong_APA_Get_scheduled(
                     self,
                     node->cpu_to_preempt
                   );
+      next_node_idle = _Scheduler_Release_idle_thread_if_necessary(
+        next_node,
+        _Scheduler_strong_APA_Release_idle,
+        context
+      );
 
       (void) _Scheduler_SMP_Preempt(
                context,
                curr_node,
                next_node,
+               next_node_idle,
                _Scheduler_strong_APA_Allocate_processor
              );
 
@@ -587,6 +652,8 @@ static inline bool _Scheduler_strong_APA_Do_enqueue(
   self = _Scheduler_strong_APA_Get_self( context );
   CPU = self->CPU;
 
+  _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
+
   node_priority = _Scheduler_Node_get_priority( node );
   node_priority = SCHEDULER_PRIORITY_PURIFY( node_priority );
 
@@ -633,7 +700,10 @@ static inline bool _Scheduler_strong_APA_Do_enqueue(
       next_node,
       _Scheduler_SMP_Insert_scheduled,
       _Scheduler_strong_APA_Move_from_scheduled_to_ready,
-      _Scheduler_strong_APA_Allocate_processor
+      _Scheduler_strong_APA_Move_from_ready_to_scheduled,
+      _Scheduler_strong_APA_Allocate_processor,
+      _Scheduler_strong_APA_Get_idle,
+      _Scheduler_strong_APA_Release_idle
     );
 
     curr_node = next_node;
@@ -641,13 +711,21 @@ static inline bool _Scheduler_strong_APA_Do_enqueue(
     curr_strong_node = _Scheduler_strong_APA_Node_downcast( curr_node );
 
     while ( curr_node != lowest_reachable ) {
+      Thread_Control *next_node_idle;
+
       curr_CPU = curr_strong_node->cpu_to_preempt;
       next_node = _Scheduler_strong_APA_Get_scheduled( self, curr_CPU );
+      next_node_idle = _Scheduler_Release_idle_thread_if_necessary(
+        next_node,
+        _Scheduler_strong_APA_Release_idle,
+        context
+      );
       /* curr_node preempts the next_node; */
       _Scheduler_SMP_Preempt(
         context,
         curr_node,
         next_node,
+        next_node_idle,
         _Scheduler_strong_APA_Allocate_processor
       );
 
@@ -755,7 +833,9 @@ static inline void _Scheduler_strong_APA_Enqueue_scheduled(
     _Scheduler_strong_APA_Insert_ready,
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_strong_APA_Move_from_ready_to_scheduled,
-    _Scheduler_strong_APA_Allocate_processor
+    _Scheduler_strong_APA_Allocate_processor,
+    _Scheduler_strong_APA_Get_idle,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -774,7 +854,8 @@ static inline bool _Scheduler_strong_APA_Do_ask_for_help(
     _Scheduler_SMP_Insert_scheduled,
     _Scheduler_strong_APA_Move_from_scheduled_to_ready,
     _Scheduler_strong_APA_Get_lowest_scheduled,
-    _Scheduler_strong_APA_Allocate_processor
+    _Scheduler_strong_APA_Allocate_processor,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -842,7 +923,9 @@ void _Scheduler_strong_APA_Block(
     _Scheduler_strong_APA_Extract_from_ready,
     _Scheduler_strong_APA_Get_highest_ready,
     _Scheduler_strong_APA_Move_from_ready_to_scheduled,
-    _Scheduler_strong_APA_Allocate_processor
+    _Scheduler_strong_APA_Allocate_processor,
+    _Scheduler_strong_APA_Get_idle,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -859,7 +942,8 @@ void _Scheduler_strong_APA_Unblock(
     thread,
     node,
     _Scheduler_strong_APA_Do_update,
-    _Scheduler_strong_APA_Enqueue
+    _Scheduler_strong_APA_Enqueue,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -933,7 +1017,9 @@ void _Scheduler_strong_APA_Withdraw_node(
     _Scheduler_strong_APA_Extract_from_ready,
     _Scheduler_strong_APA_Get_highest_ready,
     _Scheduler_strong_APA_Move_from_ready_to_scheduled,
-    _Scheduler_strong_APA_Allocate_processor
+    _Scheduler_strong_APA_Allocate_processor,
+    _Scheduler_strong_APA_Get_idle,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -966,7 +1052,9 @@ void _Scheduler_strong_APA_Clean_sticky(
     _Scheduler_strong_APA_Extract_from_ready,
     _Scheduler_strong_APA_Get_highest_ready,
     _Scheduler_strong_APA_Move_from_ready_to_scheduled,
-    _Scheduler_strong_APA_Allocate_processor
+    _Scheduler_strong_APA_Allocate_processor,
+    _Scheduler_strong_APA_Get_idle,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -1028,7 +1116,9 @@ Thread_Control *_Scheduler_strong_APA_Remove_processor(
     cpu,
     _Scheduler_strong_APA_Extract_from_scheduled,
     _Scheduler_strong_APA_Extract_from_ready,
-    _Scheduler_strong_APA_Enqueue
+    _Scheduler_strong_APA_Enqueue,
+    _Scheduler_strong_APA_Get_idle,
+    _Scheduler_strong_APA_Release_idle
   );
 }
 
@@ -1089,7 +1179,9 @@ Status_Control _Scheduler_strong_APA_Set_affinity(
    _Scheduler_strong_APA_Get_highest_ready,
    _Scheduler_strong_APA_Move_from_ready_to_scheduled,
    _Scheduler_strong_APA_Enqueue,
-   _Scheduler_strong_APA_Allocate_processor
+   _Scheduler_strong_APA_Allocate_processor,
+   _Scheduler_strong_APA_Get_idle,
+   _Scheduler_strong_APA_Release_idle
  );
 
   return STATUS_SUCCESSFUL;