score: Add node to insert to Chain_Node_order

This allows to use additional members of the nodes for comparision.

Update #4534.

6 files changed, 59 insertions, 21 deletions

diff --git a/cpukit/include/rtems/score/chainimpl.h b/cpukit/include/rtems/score/chainimpl.h
index 234dd1d74e..6aaa89237d 100644
--- a/cpukit/include/rtems/score/chainimpl.h
+++ b/cpukit/include/rtems/score/chainimpl.h
@@ -826,7 +826,8 @@ RTEMS_INLINE_ROUTINE bool _Chain_Get_with_empty_check_unprotected(
  * @retval false Otherwise.
  */
 typedef bool ( *Chain_Node_order )(
-  const void       *left,
+  const void       *key,
+  const Chain_Node *left,
   const Chain_Node *right
 );
 
@@ -848,18 +849,20 @@ typedef bool ( *Chain_Node_order )(
 RTEMS_INLINE_ROUTINE void _Chain_Insert_ordered_unprotected(
   Chain_Control    *the_chain,
   Chain_Node       *to_insert,
-  const void       *left,
+  const void       *key,
   Chain_Node_order  order
 )
 {
   const Chain_Node *tail = _Chain_Immutable_tail( the_chain );
+  Chain_Node *previous = _Chain_Head( the_chain );
   Chain_Node *next = _Chain_First( the_chain );
 
-  while ( next != tail && !( *order )( left, next ) ) {
+  while ( next != tail && !( *order )( key, to_insert, next ) ) {
+    previous = next;
     next = _Chain_Next( next );
   }
 
-  _Chain_Insert_unprotected( _Chain_Previous( next ), to_insert );
+  _Chain_Insert_unprotected( previous, to_insert );
 }
 
 /**
diff --git a/cpukit/include/rtems/score/schedulersimpleimpl.h b/cpukit/include/rtems/score/schedulersimpleimpl.h
index 08ad7b8c66..9d762e058a 100644
--- a/cpukit/include/rtems/score/schedulersimpleimpl.h
+++ b/cpukit/include/rtems/score/schedulersimpleimpl.h
@@ -48,21 +48,26 @@ RTEMS_INLINE_ROUTINE Scheduler_simple_Context *
 /**
  * @brief Checks if the priority is less or equal than the priority of the node.
  *
- * @param to_insert The priority to check whether it is less or equal than @a next.
- * @param next The Chain node to compare the priority of.
+ * @param key is the priority to compare.
+ *
+ * @param to_insert is the chain node to insert.
+ *
+ * @param next is the chain node to compare the priority of.
  *
  * @retval true @a to_insert is smaller or equal than the priority of @a next.
  * @retval false @a to_insert is greater than the priority of @a next.
  */
 RTEMS_INLINE_ROUTINE bool _Scheduler_simple_Priority_less_equal(
-  const void       *to_insert,
+  const void       *key,
+  const Chain_Node *to_insert,
   const Chain_Node *next
 )
 {
   const unsigned int   *priority_to_insert;
   const Thread_Control *thread_next;
 
-  priority_to_insert = (const unsigned int *) to_insert;
+  (void) to_insert;
+  priority_to_insert = (const unsigned int *) key;
   thread_next = (const Thread_Control *) next;
 
   return *priority_to_insert <= _Thread_Get_priority( thread_next );
diff --git a/cpukit/include/rtems/score/schedulersmpimpl.h b/cpukit/include/rtems/score/schedulersmpimpl.h
index 6fb97c86b2..ab20a554f3 100644
--- a/cpukit/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/include/rtems/score/schedulersmpimpl.h
@@ -374,21 +374,26 @@ static inline void _Scheduler_SMP_Do_nothing_register_idle(
 /**
  * @brief Checks if @a to_insert is less or equal than the priority of the chain node.
  *
- * @param to_insert The priority to compare.
- * @param next The chain node to compare the priority of.
+ * @param key is the priority to compare.
+ *
+ * @param to_insert is the chain node to insert.
+ *
+ * @param next is the chain node to compare the priority of.
  *
  * @retval true @a to_insert is less or equal than the priority of @a next.
  * @retval false @a to_insert is greater than the priority of @a next.
  */
 static inline bool _Scheduler_SMP_Priority_less_equal(
-  const void       *to_insert,
+  const void       *key,
+  const Chain_Node *to_insert,
   const Chain_Node *next
 )
 {
   const Priority_Control   *priority_to_insert;
   const Scheduler_SMP_Node *node_next;
 
-  priority_to_insert = (const Priority_Control *) to_insert;
+  (void) to_insert;
+  priority_to_insert = (const Priority_Control *) key;
   node_next = (const Scheduler_SMP_Node *) next;
 
   return *priority_to_insert <= node_next->priority;
@@ -931,7 +936,13 @@ static inline bool _Scheduler_SMP_Enqueue(
 
   lowest_scheduled = ( *get_lowest_scheduled )( context, node );
 
-  if ( ( *order )( &insert_priority, &lowest_scheduled->Node.Chain ) ) {
+  if (
+    ( *order )(
+      &insert_priority,
+      &node->Node.Chain,
+      &lowest_scheduled->Node.Chain
+    )
+  ) {
     _Scheduler_SMP_Enqueue_to_scheduled(
       context,
       node,
@@ -1007,8 +1018,11 @@ static inline void _Scheduler_SMP_Enqueue_scheduled(
      * it now on the scheduled or ready set.
      */
     if (
-      node->sticky_level > 0
-        && ( *order )( &insert_priority, &highest_ready->Node.Chain )
+      node->sticky_level > 0 && ( *order )(
+        &insert_priority,
+        &node->Node.Chain,
+        &highest_ready->Node.Chain
+      )
     ) {
       if ( node_idle != NULL ) {
         Thread_Control   *owner;
@@ -1532,7 +1546,13 @@ static inline bool _Scheduler_SMP_Ask_for_help(
 
       insert_priority = _Scheduler_SMP_Node_priority( node );
 
-      if ( ( *order )( &insert_priority, &lowest_scheduled->Node.Chain ) ) {
+      if (
+        ( *order )(
+          &insert_priority,
+          &node->Node.Chain,
+          &lowest_scheduled->Node.Chain
+        )
+      ) {
         Thread_Control *lowest_scheduled_idle;
 
         _Thread_Scheduler_cancel_need_for_help(
diff --git a/cpukit/score/src/coremsginsert.c b/cpukit/score/src/coremsginsert.c
index 14b023d9e1..d9e88ae0eb 100644
--- a/cpukit/score/src/coremsginsert.c
+++ b/cpukit/score/src/coremsginsert.c
@@ -24,14 +24,16 @@
 
 #if defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
 static bool _CORE_message_queue_Order(
-  const void       *left,
+  const void       *key,
+  const Chain_Node *left,
   const Chain_Node *right
 )
 {
    const int                       *left_priority;
    const CORE_message_queue_Buffer *right_message;
 
-   left_priority = (const int *) left;
+   (void) left;
+   left_priority = (const int *) key;
    right_message = (const CORE_message_queue_Buffer *) right;
 
    return *left_priority <
diff --git a/cpukit/score/src/schedulerpriorityaffinitysmp.c b/cpukit/score/src/schedulerpriorityaffinitysmp.c
index 7d971ef8d1..63d9ae47b1 100644
--- a/cpukit/score/src/schedulerpriorityaffinitysmp.c
+++ b/cpukit/score/src/schedulerpriorityaffinitysmp.c
@@ -52,12 +52,13 @@
  */
 
 static bool _Scheduler_priority_affinity_SMP_Priority_less_equal(
-  const void       *to_insert,
+  const void       *key,
+  const Chain_Node *to_insert,
   const Chain_Node *next
 )
 {
   return next != NULL
-    && _Scheduler_SMP_Priority_less_equal( to_insert, next );
+    && _Scheduler_SMP_Priority_less_equal( key, to_insert, next );
 }
 
 static Scheduler_priority_affinity_SMP_Node *
@@ -326,6 +327,7 @@ static void _Scheduler_priority_affinity_SMP_Check_for_migrations(
     if (
       _Scheduler_SMP_Priority_less_equal(
         &lowest_scheduled_priority,
+        &lowest_scheduled->Node.Chain,
         &highest_ready->Node.Chain
       )
     ) {
diff --git a/testsuites/sptests/spchain/init.c b/testsuites/sptests/spchain/init.c
index 51278d52f3..ceb4f07b72 100644
--- a/testsuites/sptests/spchain/init.c
+++ b/testsuites/sptests/spchain/init.c
@@ -426,8 +426,14 @@ static void test_chain_node_count(void)
   }
 }
 
-static bool test_order( const void *left, const Chain_Node *right )
+static bool test_order(
+  const void       *key,
+  const Chain_Node *left,
+  const Chain_Node *right
+)
 {
+  rtems_test_assert( key == left );
+
   return (uintptr_t) left < (uintptr_t) right;
 }