score: Split SMP scheduler enqueue function

Extract code from _Scheduler_SMP_Enqueue_ordered() and move it to the
new function _Scheduler_SMP_Enqueue_scheduled_ordered() to avoid
untestable execution paths.

Add and use function _Scheduler_SMP_Unblock().

3 files changed, 220 insertions, 86 deletions

diff --git a/cpukit/score/include/rtems/score/schedulersmpimpl.h b/cpukit/score/include/rtems/score/schedulersmpimpl.h
index 3650e4c742..9bc1a3739b 100644
--- a/cpukit/score/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulersmpimpl.h
@@ -41,7 +41,8 @@ extern "C" {
  * - @ref SCHEDULER_SMP_NODE_READY.
  *
  * State transitions are triggered via basic operations
- * - _Scheduler_SMP_Enqueue_ordered(), and
+ * - _Scheduler_SMP_Enqueue_ordered(),
+ * - _Scheduler_SMP_Enqueue_scheduled_ordered(), and
  * - _Scheduler_SMP_Block().
  *
  * @dot
@@ -295,8 +296,7 @@ typedef void ( *Scheduler_SMP_Update )(
 
 typedef void ( *Scheduler_SMP_Enqueue )(
   Scheduler_Context *context,
-  Thread_Control *thread_to_enqueue,
-  bool has_processor_allocated
+  Thread_Control *thread_to_enqueue
 );
 
 static inline Scheduler_SMP_Context *_Scheduler_SMP_Get_self(
@@ -432,80 +432,102 @@ static inline Thread_Control *_Scheduler_SMP_Get_lowest_scheduled(
 /**
  * @brief Enqueues a thread according to the specified order function.
  *
+ * The thread must not be in the scheduled state.
+ *
  * @param[in] context The scheduler instance context.
  * @param[in] thread The thread to enqueue.
- * @param[in] has_processor_allocated The thread has a processor allocated.
  * @param[in] order The order function.
- * @param[in] get_highest_ready Function to get the highest ready node.
  * @param[in] insert_ready Function to insert a node into the set of ready
  * nodes.
  * @param[in] insert_scheduled Function to insert a node into the set of
  * scheduled nodes.
- * @param[in] move_from_ready_to_scheduled Function to move a node from the set
- * of ready nodes to the set of scheduled nodes.
  * @param[in] move_from_scheduled_to_ready Function to move a node from the set
  * of scheduled nodes to the set of ready nodes.
  */
 static inline void _Scheduler_SMP_Enqueue_ordered(
   Scheduler_Context *context,
   Thread_Control *thread,
-  bool has_processor_allocated,
   Chain_Node_order order,
-  Scheduler_SMP_Get_highest_ready get_highest_ready,
   Scheduler_SMP_Insert insert_ready,
   Scheduler_SMP_Insert insert_scheduled,
-  Scheduler_SMP_Move move_from_ready_to_scheduled,
   Scheduler_SMP_Move move_from_scheduled_to_ready
 )
 {
   Scheduler_SMP_Context *self = _Scheduler_SMP_Get_self( context );
+  Thread_Control *lowest_scheduled =
+    _Scheduler_SMP_Get_lowest_scheduled( self );
+
+  _Assert( lowest_scheduled != NULL);
+
+  /*
+   * NOTE: Do not exchange parameters to do the negation of the order check.
+   */
+  if ( ( *order )( &thread->Object.Node, &lowest_scheduled->Object.Node ) ) {
+    Scheduler_SMP_Node *lowest_scheduled_node =
+      _Scheduler_SMP_Node_get( lowest_scheduled );
+
+    _Scheduler_SMP_Node_change_state(
+      lowest_scheduled_node,
+      SCHEDULER_SMP_NODE_READY
+    );
+    _Scheduler_SMP_Allocate_processor( self, thread, lowest_scheduled );
+    ( *insert_scheduled )( &self->Base, thread );
+    ( *move_from_scheduled_to_ready )( &self->Base, lowest_scheduled );
+  } else {
+    ( *insert_ready )( &self->Base, thread );
+  }
+}
+
+/**
+ * @brief Enqueues a scheduled thread according to the specified order
+ * function.
+ *
+ * @param[in] context The scheduler instance context.
+ * @param[in] thread The thread to enqueue.
+ * @param[in] order The order function.
+ * @param[in] get_highest_ready Function to get the highest ready node.
+ * @param[in] insert_ready Function to insert a node into the set of ready
+ * nodes.
+ * @param[in] insert_scheduled Function to insert a node into the set of
+ * scheduled nodes.
+ * @param[in] move_from_ready_to_scheduled Function to move a node from the set
+ * of ready nodes to the set of scheduled nodes.
+ */
+static inline void _Scheduler_SMP_Enqueue_scheduled_ordered(
+  Scheduler_Context *context,
+  Thread_Control *thread,
+  Chain_Node_order order,
+  Scheduler_SMP_Get_highest_ready get_highest_ready,
+  Scheduler_SMP_Insert insert_ready,
+  Scheduler_SMP_Insert insert_scheduled,
+  Scheduler_SMP_Move move_from_ready_to_scheduled
+)
+{
+  Scheduler_SMP_Context *self = _Scheduler_SMP_Get_self( context );
   Scheduler_SMP_Node *node = _Scheduler_SMP_Node_get( thread );
+  Thread_Control *highest_ready = ( *get_highest_ready )( &self->Base );
 
-  if ( has_processor_allocated) {
-    Thread_Control *highest_ready = ( *get_highest_ready )( &self->Base );
-
-    _Assert( highest_ready != NULL);
-
-    /*
-     * The thread has been extracted from the scheduled chain.  We have to
-     * place it now on the scheduled or ready chain.
-     *
-     * NOTE: Do not exchange parameters to do the negation of the order check.
-     */
-    if ( !( *order )( &thread->Object.Node, &highest_ready->Object.Node ) ) {
-      _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
-      _Scheduler_SMP_Allocate_processor( self, highest_ready, thread );
-      ( *insert_ready )( &self->Base, thread );
-      ( *move_from_ready_to_scheduled )( &self->Base, highest_ready );
-    } else {
-      _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_SCHEDULED );
-      ( *insert_scheduled )( &self->Base, thread );
-    }
+  _Assert( highest_ready != NULL);
+
+  /*
+   * The thread has been extracted from the scheduled chain.  We have to place
+   * it now on the scheduled or ready set.
+   *
+   * NOTE: Do not exchange parameters to do the negation of the order check.
+   */
+  if ( !( *order )( &thread->Object.Node, &highest_ready->Object.Node ) ) {
+    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
+    _Scheduler_SMP_Allocate_processor( self, highest_ready, thread );
+    ( *insert_ready )( &self->Base, thread );
+    ( *move_from_ready_to_scheduled )( &self->Base, highest_ready );
   } else {
-    Thread_Control *lowest_scheduled =
-      _Scheduler_SMP_Get_lowest_scheduled( self );
-
-    _Assert( lowest_scheduled != NULL);
-
-    if ( ( *order )( &thread->Object.Node, &lowest_scheduled->Object.Node ) ) {
-      Scheduler_SMP_Node *lowest_scheduled_node =
-        _Scheduler_SMP_Node_get( lowest_scheduled );
-
-      _Scheduler_SMP_Node_change_state(
-        lowest_scheduled_node,
-        SCHEDULER_SMP_NODE_READY
-      );
-      _Scheduler_SMP_Allocate_processor( self, thread, lowest_scheduled );
-      ( *insert_scheduled )( &self->Base, thread );
-      ( *move_from_scheduled_to_ready )( &self->Base, lowest_scheduled );
-    } else {
-      _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
-      ( *insert_ready )( &self->Base, thread );
-    }
+    ( *insert_scheduled )( &self->Base, thread );
   }
 }
 
-static inline void _Scheduler_SMP_Extract_from_scheduled( Thread_Control *thread )
+static inline void _Scheduler_SMP_Extract_from_scheduled(
+  Thread_Control *thread
+)
 {
   _Chain_Extract_unprotected( &thread->Object.Node );
 }
@@ -563,6 +585,19 @@ static inline void _Scheduler_SMP_Block(
   }
 }
 
+static inline void _Scheduler_SMP_Unblock(
+  Scheduler_Context *context,
+  Thread_Control *thread,
+  Scheduler_SMP_Enqueue enqueue_fifo
+)
+{
+  Scheduler_SMP_Node *node = _Scheduler_SMP_Node_get( thread );
+
+  _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
+
+  ( *enqueue_fifo )( context, thread );
+}
+
 static inline void _Scheduler_SMP_Change_priority(
   Scheduler_Context *context,
   Thread_Control *thread,
@@ -571,24 +606,33 @@ static inline void _Scheduler_SMP_Change_priority(
   Scheduler_SMP_Extract extract_from_ready,
   Scheduler_SMP_Update update,
   Scheduler_SMP_Enqueue enqueue_fifo,
-  Scheduler_SMP_Enqueue enqueue_lifo
+  Scheduler_SMP_Enqueue enqueue_lifo,
+  Scheduler_SMP_Enqueue enqueue_scheduled_fifo,
+  Scheduler_SMP_Enqueue enqueue_scheduled_lifo
 )
 {
   Scheduler_SMP_Node *node = _Scheduler_SMP_Node_get( thread );
-  bool has_processor_allocated = node->state == SCHEDULER_SMP_NODE_SCHEDULED;
 
-  if ( has_processor_allocated ) {
+  if ( node->state == SCHEDULER_SMP_NODE_SCHEDULED ) {
     _Scheduler_SMP_Extract_from_scheduled( thread );
+
+    ( *update )( context, &node->Base, new_priority );
+
+    if ( prepend_it ) {
+      ( *enqueue_scheduled_lifo )( context, thread );
+    } else {
+      ( *enqueue_scheduled_fifo )( context, thread );
+    }
   } else {
     ( *extract_from_ready )( context, thread );
-  }
 
-  ( *update )( context, &node->Base, new_priority );
+    ( *update )( context, &node->Base, new_priority );
 
-  if ( prepend_it ) {
-    ( *enqueue_lifo )( context, thread, has_processor_allocated );
-  } else {
-    ( *enqueue_fifo )( context, thread, has_processor_allocated );
+    if ( prepend_it ) {
+      ( *enqueue_lifo )( context, thread );
+    } else {
+      ( *enqueue_fifo )( context, thread );
+    }
   }
 }
 
diff --git a/cpukit/score/src/schedulerprioritysmp.c b/cpukit/score/src/schedulerprioritysmp.c
index 956b7cf90f..56bb0ac91b 100644
--- a/cpukit/score/src/schedulerprioritysmp.c
+++ b/cpukit/score/src/schedulerprioritysmp.c
@@ -228,7 +228,6 @@ void _Scheduler_priority_SMP_Block(
 static void _Scheduler_priority_SMP_Enqueue_ordered(
   Scheduler_Context *context,
   Thread_Control *thread,
-  bool has_processor_allocated,
   Chain_Node_order order,
   Scheduler_SMP_Insert insert_ready,
   Scheduler_SMP_Insert insert_scheduled
@@ -237,26 +236,21 @@ static void _Scheduler_priority_SMP_Enqueue_ordered(
   _Scheduler_SMP_Enqueue_ordered(
     context,
     thread,
-    has_processor_allocated,
     order,
-    _Scheduler_priority_SMP_Get_highest_ready,
     insert_ready,
     insert_scheduled,
-    _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready
   );
 }
 
 static void _Scheduler_priority_SMP_Enqueue_lifo(
   Scheduler_Context *context,
-  Thread_Control *thread,
-  bool has_processor_allocated
+  Thread_Control *thread
 )
 {
   _Scheduler_priority_SMP_Enqueue_ordered(
     context,
     thread,
-    has_processor_allocated,
     _Scheduler_simple_Insert_priority_lifo_order,
     _Scheduler_priority_SMP_Insert_ready_lifo,
     _Scheduler_SMP_Insert_scheduled_lifo
@@ -265,14 +259,59 @@ static void _Scheduler_priority_SMP_Enqueue_lifo(
 
 static void _Scheduler_priority_SMP_Enqueue_fifo(
   Scheduler_Context *context,
-  Thread_Control *thread,
-  bool has_processor_allocated
+  Thread_Control *thread
 )
 {
   _Scheduler_priority_SMP_Enqueue_ordered(
     context,
     thread,
-    has_processor_allocated,
+    _Scheduler_simple_Insert_priority_fifo_order,
+    _Scheduler_priority_SMP_Insert_ready_fifo,
+    _Scheduler_SMP_Insert_scheduled_fifo
+  );
+}
+
+static void _Scheduler_priority_SMP_Enqueue_scheduled_ordered(
+  Scheduler_Context *context,
+  Thread_Control *thread,
+  Chain_Node_order order,
+  Scheduler_SMP_Insert insert_ready,
+  Scheduler_SMP_Insert insert_scheduled
+)
+{
+  _Scheduler_SMP_Enqueue_scheduled_ordered(
+    context,
+    thread,
+    order,
+    _Scheduler_priority_SMP_Get_highest_ready,
+    insert_ready,
+    insert_scheduled,
+    _Scheduler_priority_SMP_Move_from_ready_to_scheduled
+  );
+}
+
+static void _Scheduler_priority_SMP_Enqueue_scheduled_lifo(
+  Scheduler_Context *context,
+  Thread_Control *thread
+)
+{
+  _Scheduler_priority_SMP_Enqueue_scheduled_ordered(
+    context,
+    thread,
+    _Scheduler_simple_Insert_priority_lifo_order,
+    _Scheduler_priority_SMP_Insert_ready_lifo,
+    _Scheduler_SMP_Insert_scheduled_lifo
+  );
+}
+
+static void _Scheduler_priority_SMP_Enqueue_scheduled_fifo(
+  Scheduler_Context *context,
+  Thread_Control *thread
+)
+{
+  _Scheduler_priority_SMP_Enqueue_scheduled_ordered(
+    context,
+    thread,
     _Scheduler_simple_Insert_priority_fifo_order,
     _Scheduler_priority_SMP_Insert_ready_fifo,
     _Scheduler_SMP_Insert_scheduled_fifo
@@ -286,7 +325,11 @@ void _Scheduler_priority_SMP_Unblock(
 {
   Scheduler_Context *context = _Scheduler_Get_context( scheduler );
 
-  _Scheduler_priority_SMP_Enqueue_fifo( context, thread, false );
+  _Scheduler_SMP_Unblock(
+    context,
+    thread,
+    _Scheduler_priority_SMP_Enqueue_fifo
+  );
 }
 
 void _Scheduler_priority_SMP_Change_priority(
@@ -306,7 +349,9 @@ void _Scheduler_priority_SMP_Change_priority(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_SMP_Do_update,
     _Scheduler_priority_SMP_Enqueue_fifo,
-    _Scheduler_priority_SMP_Enqueue_lifo
+    _Scheduler_priority_SMP_Enqueue_lifo,
+    _Scheduler_priority_SMP_Enqueue_scheduled_fifo,
+    _Scheduler_priority_SMP_Enqueue_scheduled_lifo
   );
 }
 
@@ -321,7 +366,7 @@ void _Scheduler_priority_SMP_Yield(
   _ISR_Disable( level );
 
   _Scheduler_SMP_Extract_from_scheduled( thread );
-  _Scheduler_priority_SMP_Enqueue_fifo( context, thread, true );
+  _Scheduler_priority_SMP_Enqueue_scheduled_fifo( context, thread );
 
   _ISR_Enable( level );
 }
diff --git a/cpukit/score/src/schedulersimplesmp.c b/cpukit/score/src/schedulersimplesmp.c
index a743cf9fff..d5d3908f30 100644
--- a/cpukit/score/src/schedulersimplesmp.c
+++ b/cpukit/score/src/schedulersimplesmp.c
@@ -164,7 +164,6 @@ void _Scheduler_simple_SMP_Block(
 static void _Scheduler_simple_SMP_Enqueue_ordered(
   Scheduler_Context *context,
   Thread_Control *thread,
-  bool has_processor_allocated,
   Chain_Node_order order,
   Scheduler_SMP_Insert insert_ready,
   Scheduler_SMP_Insert insert_scheduled
@@ -173,26 +172,21 @@ static void _Scheduler_simple_SMP_Enqueue_ordered(
   _Scheduler_SMP_Enqueue_ordered(
     context,
     thread,
-    has_processor_allocated,
     order,
-    _Scheduler_simple_SMP_Get_highest_ready,
     insert_ready,
     insert_scheduled,
-    _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
     _Scheduler_simple_SMP_Move_from_scheduled_to_ready
   );
 }
 
 static void _Scheduler_simple_SMP_Enqueue_lifo(
   Scheduler_Context *context,
-  Thread_Control *thread,
-  bool has_processor_allocated
+  Thread_Control *thread
 )
 {
   _Scheduler_simple_SMP_Enqueue_ordered(
     context,
     thread,
-    has_processor_allocated,
     _Scheduler_simple_Insert_priority_lifo_order,
     _Scheduler_simple_SMP_Insert_ready_lifo,
     _Scheduler_SMP_Insert_scheduled_lifo
@@ -201,14 +195,59 @@ static void _Scheduler_simple_SMP_Enqueue_lifo(
 
 static void _Scheduler_simple_SMP_Enqueue_fifo(
   Scheduler_Context *context,
-  Thread_Control *thread,
-  bool has_processor_allocated
+  Thread_Control *thread
 )
 {
   _Scheduler_simple_SMP_Enqueue_ordered(
     context,
     thread,
-    has_processor_allocated,
+    _Scheduler_simple_Insert_priority_fifo_order,
+    _Scheduler_simple_SMP_Insert_ready_fifo,
+    _Scheduler_SMP_Insert_scheduled_fifo
+  );
+}
+
+static void _Scheduler_simple_SMP_Enqueue_scheduled_ordered(
+  Scheduler_Context *context,
+  Thread_Control *thread,
+  Chain_Node_order order,
+  Scheduler_SMP_Insert insert_ready,
+  Scheduler_SMP_Insert insert_scheduled
+)
+{
+  _Scheduler_SMP_Enqueue_scheduled_ordered(
+    context,
+    thread,
+    order,
+    _Scheduler_simple_SMP_Get_highest_ready,
+    insert_ready,
+    insert_scheduled,
+    _Scheduler_simple_SMP_Move_from_ready_to_scheduled
+  );
+}
+
+static void _Scheduler_simple_SMP_Enqueue_scheduled_lifo(
+  Scheduler_Context *context,
+  Thread_Control *thread
+)
+{
+  _Scheduler_simple_SMP_Enqueue_scheduled_ordered(
+    context,
+    thread,
+    _Scheduler_simple_Insert_priority_lifo_order,
+    _Scheduler_simple_SMP_Insert_ready_lifo,
+    _Scheduler_SMP_Insert_scheduled_lifo
+  );
+}
+
+static void _Scheduler_simple_SMP_Enqueue_scheduled_fifo(
+  Scheduler_Context *context,
+  Thread_Control *thread
+)
+{
+  _Scheduler_simple_SMP_Enqueue_scheduled_ordered(
+    context,
+    thread,
     _Scheduler_simple_Insert_priority_fifo_order,
     _Scheduler_simple_SMP_Insert_ready_fifo,
     _Scheduler_SMP_Insert_scheduled_fifo
@@ -222,7 +261,11 @@ void _Scheduler_simple_SMP_Unblock(
 {
   Scheduler_Context *context = _Scheduler_Get_context( scheduler );
 
-  _Scheduler_simple_SMP_Enqueue_fifo( context, thread, false );
+  _Scheduler_SMP_Unblock(
+    context,
+    thread,
+    _Scheduler_simple_SMP_Enqueue_fifo
+  );
 }
 
 void _Scheduler_simple_SMP_Change_priority(
@@ -242,7 +285,9 @@ void _Scheduler_simple_SMP_Change_priority(
     _Scheduler_simple_SMP_Extract_from_ready,
     _Scheduler_simple_SMP_Do_update,
     _Scheduler_simple_SMP_Enqueue_fifo,
-    _Scheduler_simple_SMP_Enqueue_lifo
+    _Scheduler_simple_SMP_Enqueue_lifo,
+    _Scheduler_simple_SMP_Enqueue_scheduled_fifo,
+    _Scheduler_simple_SMP_Enqueue_scheduled_lifo
   );
 }
 
@@ -257,7 +302,7 @@ void _Scheduler_simple_SMP_Yield(
   _ISR_Disable( level );
 
   _Scheduler_SMP_Extract_from_scheduled( thread );
-  _Scheduler_simple_SMP_Enqueue_fifo( context, thread, true );
+  _Scheduler_simple_SMP_Enqueue_scheduled_fifo( context, thread );
 
   _ISR_Enable( level );
 }