score: Add scheduler node table for each thread

Update #2556.

4 files changed, 60 insertions, 17 deletions

diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index dea1888a51..2fdc01a695 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -772,7 +772,11 @@ RTEMS_INLINE_ROUTINE Scheduler_Node *_Scheduler_Thread_get_node(
   const Thread_Control *the_thread
 )
 {
+#if defined(RTEMS_SMP)
   return the_thread->Scheduler.node;
+#else
+  return the_thread->Scheduler.nodes;
+#endif
 }
 
 RTEMS_INLINE_ROUTINE void _Scheduler_Thread_set_priority(
@@ -1377,7 +1381,8 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
   Priority_Control         priority
 )
 {
-  Scheduler_Node *own_node;
+  Scheduler_Node *new_scheduler_node;
+  Scheduler_Node *old_scheduler_node;
 
   if (
     _Thread_Owns_resources( the_thread )
@@ -1386,22 +1391,34 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
     return STATUS_RESOURCE_IN_USE;
   }
 
-  own_node = _Thread_Scheduler_get_own_node( the_thread );
-  _Priority_Plain_extract( &own_node->Wait.Priority, &the_thread->Real_priority );
+  old_scheduler_node = _Thread_Scheduler_get_own_node( the_thread );
+  _Priority_Plain_extract(
+    &old_scheduler_node->Wait.Priority,
+    &the_thread->Real_priority
+  );
 
-  if ( !_Priority_Is_empty( &own_node->Wait.Priority ) ) {
+  if ( !_Priority_Is_empty( &old_scheduler_node->Wait.Priority ) ) {
     _Priority_Plain_insert(
-      &own_node->Wait.Priority,
+      &old_scheduler_node->Wait.Priority,
       &the_thread->Real_priority,
       the_thread->Real_priority.priority
     );
     return STATUS_RESOURCE_IN_USE;
   }
 
+#if defined(RTEMS_SMP)
+  new_scheduler_node = _Thread_Scheduler_get_node_by_index(
+    the_thread,
+    _Scheduler_Get_index( new_scheduler )
+  );
+#else
+  new_scheduler_node = old_scheduler_node;
+#endif
+
   the_thread->Start.initial_priority = priority;
   _Priority_Node_set_priority( &the_thread->Real_priority, priority );
   _Priority_Initialize_one(
-    &own_node->Wait.Priority,
+    &new_scheduler_node->Wait.Priority,
     &the_thread->Real_priority
   );
 
@@ -1420,15 +1437,11 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
         _Scheduler_Block( the_thread );
       }
 
-      _Scheduler_Node_destroy( old_scheduler, own_node );
       the_thread->Scheduler.own_control = new_scheduler;
       the_thread->Scheduler.control = new_scheduler;
-      _Scheduler_Node_initialize(
-        new_scheduler,
-        own_node,
-        the_thread,
-        priority
-      );
+      the_thread->Scheduler.own_node = new_scheduler_node;
+      the_thread->Scheduler.node = new_scheduler_node;
+      _Scheduler_Node_set_priority( new_scheduler_node, priority, false );
 
       if ( _States_Is_ready( current_state ) ) {
         _Scheduler_Unblock( the_thread );
@@ -1439,7 +1452,7 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
   }
 #endif
 
-  _Scheduler_Node_set_priority( own_node, priority, false );
+  _Scheduler_Node_set_priority( new_scheduler_node, priority, false );
   _Scheduler_Update_priority( the_thread );
   return STATUS_SUCCESSFUL;
 }
diff --git a/cpukit/score/include/rtems/score/schedulernode.h b/cpukit/score/include/rtems/score/schedulernode.h
index 9827d21e64..2954db5350 100644
--- a/cpukit/score/include/rtems/score/schedulernode.h
+++ b/cpukit/score/include/rtems/score/schedulernode.h
@@ -188,6 +188,15 @@ typedef struct Scheduler_Node {
   } Priority;
 } Scheduler_Node;
 
+#if defined(RTEMS_SMP)
+/**
+ * @brief The size of a scheduler node.
+ *
+ * This value is provided via <rtems/confdefs.h>.
+ */
+extern const size_t _Scheduler_Node_size;
+#endif
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */
diff --git a/cpukit/score/include/rtems/score/thread.h b/cpukit/score/include/rtems/score/thread.h
index 393d431e0e..17aeacde29 100644
--- a/cpukit/score/include/rtems/score/thread.h
+++ b/cpukit/score/include/rtems/score/thread.h
@@ -271,7 +271,6 @@ typedef struct {
    * priority and ask for help operations.
    */
   Scheduler_Node *own_node;
-#endif
 
   /**
    * @brief The scheduler node of this thread.
@@ -284,12 +283,18 @@ typedef struct {
    */
   Scheduler_Node *node;
 
-#if defined(RTEMS_SMP)
   /**
    * @brief The processor assigned by the current scheduler.
    */
   struct Per_CPU_Control *cpu;
 #endif
+
+  /**
+   * @brief The scheduler nodes of this thread.
+   *
+   * Each thread has a scheduler node for each scheduler instance.
+   */
+  Scheduler_Node *nodes;
 } Thread_Scheduler_control;
 
 /**
diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index 7f9dccf5e2..09af9c15dd 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -993,7 +993,23 @@ RTEMS_INLINE_ROUTINE Scheduler_Node *_Thread_Scheduler_get_own_node(
 #if defined(RTEMS_SMP)
   return the_thread->Scheduler.own_node;
 #else
-  return the_thread->Scheduler.node;
+  return the_thread->Scheduler.nodes;
+#endif
+}
+
+RTEMS_INLINE_ROUTINE Scheduler_Node *_Thread_Scheduler_get_node_by_index(
+  const Thread_Control *the_thread,
+  size_t                scheduler_index
+)
+{
+#if defined(RTEMS_SMP)
+  return (Scheduler_Node *)
+    ( (uintptr_t) the_thread->Scheduler.nodes
+      + scheduler_index * _Scheduler_Node_size );
+#else
+  _Assert( scheduler_index == 0 );
+  (void) scheduler_index;
+  return the_thread->Scheduler.nodes;
 #endif
 }