score: Implement SMP-specific priority queue

6 files changed, 128 insertions, 16 deletions

diff --git a/cpukit/sapi/include/confdefs.h b/cpukit/sapi/include/confdefs.h
index 4b438ff540..66c8c7e750 100644
--- a/cpukit/sapi/include/confdefs.h
+++ b/cpukit/sapi/include/confdefs.h
@@ -1008,9 +1008,10 @@ const rtems_libio_helper rtems_fs_init_helper =
     CONFIGURE_SCHEDULER_CONTROLS
   };
 
+  #define CONFIGURE_SCHEDULER_COUNT RTEMS_ARRAY_SIZE( _Scheduler_Table )
+
   #if defined(RTEMS_SMP)
-    const size_t _Scheduler_Count =
-      RTEMS_ARRAY_SIZE( _Scheduler_Table );
+    const size_t _Scheduler_Count = CONFIGURE_SCHEDULER_COUNT;
 
     const Scheduler_Assignment _Scheduler_Assignments[] = {
       #if defined(CONFIGURE_SMP_SCHEDULER_ASSIGNMENTS)
@@ -2970,7 +2971,7 @@ const rtems_libio_helper rtems_fs_init_helper =
   ( \
     _Configure_Object_RAM(_tasks, sizeof(Configuration_Thread_control)) \
       + _Configure_From_workspace(_Configure_Max_Objects(_tasks) \
-        * sizeof(Thread_queue_Heads)) \
+        * THREAD_QUEUE_HEADS_SIZE(CONFIGURE_SCHEDULER_COUNT)) \
       + _Configure_Max_Objects(_number_FP_tasks) \
         * _Configure_From_workspace(CONTEXT_FP_SIZE) \
   )
diff --git a/cpukit/score/include/rtems/score/threadq.h b/cpukit/score/include/rtems/score/threadq.h
index 06ba9f3f9d..cc07daed57 100644
--- a/cpukit/score/include/rtems/score/threadq.h
+++ b/cpukit/score/include/rtems/score/threadq.h
@@ -42,6 +42,26 @@ extern "C" {
 typedef struct _Thread_Control Thread_Control;
 
 /**
+ * @brief Thread priority queue.
+ */
+typedef struct {
+#if defined(RTEMS_SMP)
+  /**
+   * @brief Node to enqueue this queue in the FIFO chain of the corresponding
+   * heads structure.
+   *
+   * @see Thread_queue_Heads::Heads::Fifo.
+   */
+  Chain_Node Node;
+#endif
+
+  /**
+   * @brief The actual thread priority queue.
+   */
+  RBTree_Control Queue;
+} Thread_queue_Priority_queue;
+
+/**
  * @brief Thread queue heads.
  *
  * Each thread is equipped with spare thread queue heads in case it is not
@@ -61,13 +81,19 @@ typedef struct _Thread_queue_Heads {
   union {
     /**
      * @brief This is the FIFO discipline list.
+     *
+     * On SMP configurations this FIFO is used to enqueue the per scheduler
+     * instance priority queues of this structure.  This ensures FIFO fairness
+     * among the highest priority thread of each scheduler instance.
      */
     Chain_Control Fifo;
 
+#if !defined(RTEMS_SMP)
     /**
      * @brief This is the set of threads for priority discipline waiting.
      */
-    RBTree_Control Priority;
+    Thread_queue_Priority_queue Priority;
+#endif
   } Heads;
 
   /**
@@ -81,8 +107,24 @@ typedef struct _Thread_queue_Heads {
    * the thread queue heads dedicated to the thread queue of an object.
    */
   Chain_Node Free_node;
+
+#if defined(RTEMS_SMP)
+  /**
+   * @brief One priority queue per scheduler instance.
+   */
+  Thread_queue_Priority_queue Priority[ RTEMS_ZERO_LENGTH_ARRAY ];
+#endif
 } Thread_queue_Heads;
 
+#if defined(RTEMS_SMP)
+  #define THREAD_QUEUE_HEADS_SIZE( scheduler_count ) \
+    ( sizeof( Thread_queue_Heads ) \
+      + ( scheduler_count ) * sizeof( Thread_queue_Priority_queue ) )
+#else
+  #define THREAD_QUEUE_HEADS_SIZE( scheduler_count ) \
+    sizeof( Thread_queue_Heads )
+#endif
+
 typedef struct {
   /**
    * @brief The thread queue heads.
diff --git a/cpukit/score/include/rtems/score/threadqimpl.h b/cpukit/score/include/rtems/score/threadqimpl.h
index 3828f415a6..bf01eb7404 100644
--- a/cpukit/score/include/rtems/score/threadqimpl.h
+++ b/cpukit/score/include/rtems/score/threadqimpl.h
@@ -22,6 +22,7 @@
 #include <rtems/score/threadq.h>
 #include <rtems/score/chainimpl.h>
 #include <rtems/score/rbtreeimpl.h>
+#include <rtems/score/scheduler.h>
 #include <rtems/score/thread.h>
 
 #ifdef __cplusplus
@@ -51,6 +52,21 @@ typedef struct {
 #endif
 } Thread_queue_Syslock_queue;
 
+RTEMS_INLINE_ROUTINE void _Thread_queue_Heads_initialize(
+  Thread_queue_Heads *heads
+)
+{
+#if defined(RTEMS_SMP)
+  size_t i;
+
+  for ( i = 0; i < _Scheduler_Count; ++i ) {
+    _RBTree_Initialize_empty( &heads->Priority[ i ].Queue );
+  }
+#endif
+
+  _Chain_Initialize_empty( &heads->Free_chain );
+}
+
 RTEMS_INLINE_ROUTINE void _Thread_queue_Queue_initialize(
   Thread_queue_Queue *queue
 )
diff --git a/cpukit/score/src/thread.c b/cpukit/score/src/thread.c
index e1d6d5c231..1ad7a59f32 100644
--- a/cpukit/score/src/thread.c
+++ b/cpukit/score/src/thread.c
@@ -20,6 +20,7 @@
 
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/interr.h>
+#include <rtems/score/scheduler.h>
 #include <rtems/score/wkspace.h>
 
 #define THREAD_OFFSET_ASSERT( field ) \
@@ -73,7 +74,7 @@ void _Thread_Initialize_information(
     &information->Free_thread_queue_heads,
     _Workspace_Allocate_or_fatal_error,
     _Objects_Maximum_per_allocation( maximum ),
-    sizeof( Thread_queue_Heads )
+    THREAD_QUEUE_HEADS_SIZE( _Scheduler_Count )
   );
 }
 
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index 9a796e9bcc..bdb4370f47 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -142,12 +142,12 @@ bool _Thread_Initialize(
     &information->Free_thread_queue_heads,
     _Workspace_Allocate,
     _Objects_Extend_size( &information->Objects ),
-    sizeof( *the_thread->Wait.spare_heads )
+    THREAD_QUEUE_HEADS_SIZE( _Scheduler_Count )
   );
   if ( the_thread->Wait.spare_heads == NULL ) {
     goto failed;
   }
-  _Chain_Initialize_empty( &the_thread->Wait.spare_heads->Free_chain );
+  _Thread_queue_Heads_initialize( the_thread->Wait.spare_heads );
 
   /*
    *  Initialize the thread timer
diff --git a/cpukit/score/src/threadqops.c b/cpukit/score/src/threadqops.c
index d9dc94440e..07473f50bd 100644
--- a/cpukit/score/src/threadqops.c
+++ b/cpukit/score/src/threadqops.c
@@ -20,6 +20,7 @@
 #include <rtems/score/assert.h>
 #include <rtems/score/chainimpl.h>
 #include <rtems/score/rbtreeimpl.h>
+#include <rtems/score/schedulerimpl.h>
 
 static void _Thread_queue_Do_nothing_priority_change(
   Thread_Control     *the_thread,
@@ -150,22 +151,41 @@ static Thread_Control *_Thread_queue_FIFO_first(
   return THREAD_CHAIN_NODE_TO_THREAD( first );
 }
 
+static Thread_queue_Priority_queue *_Thread_queue_Priority_queue(
+  Thread_queue_Heads   *heads,
+  const Thread_Control *the_thread
+)
+{
+#if defined(RTEMS_SMP)
+  return &heads->Priority[
+    _Scheduler_Get_index( _Scheduler_Get_own( the_thread ) )
+  ];
+#else
+  (void) the_thread;
+
+  return &heads->Heads.Priority;
+#endif
+}
+
 static void _Thread_queue_Priority_priority_change(
   Thread_Control     *the_thread,
   Priority_Control    new_priority,
   Thread_queue_Queue *queue
 )
 {
-  Thread_queue_Heads *heads = queue->heads;
+  Thread_queue_Heads          *heads = queue->heads;
+  Thread_queue_Priority_queue *priority_queue;
 
   _Assert( heads != NULL );
 
+  priority_queue = _Thread_queue_Priority_queue( heads, the_thread );
+
   _RBTree_Extract(
-    &heads->Heads.Priority,
+    &priority_queue->Queue,
     &the_thread->Wait.Node.RBTree
   );
   _RBTree_Insert(
-    &heads->Heads.Priority,
+    &priority_queue->Queue,
     &the_thread->Wait.Node.RBTree,
     _Thread_queue_Compare_priority,
     false
@@ -176,7 +196,11 @@ static void _Thread_queue_Priority_do_initialize(
   Thread_queue_Heads *heads
 )
 {
+#if defined(RTEMS_SMP)
+  _Chain_Initialize_empty( &heads->Heads.Fifo );
+#else
   _RBTree_Initialize_empty( &heads->Heads.Priority );
+#endif
 }
 
 static void _Thread_queue_Priority_do_enqueue(
@@ -184,8 +208,17 @@ static void _Thread_queue_Priority_do_enqueue(
   Thread_Control     *the_thread
 )
 {
+  Thread_queue_Priority_queue *priority_queue =
+    _Thread_queue_Priority_queue( heads, the_thread );
+
+#if defined(RTEMS_SMP)
+  if ( _RBTree_Is_empty( &priority_queue->Queue ) ) {
+    _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node );
+  }
+#endif
+
   _RBTree_Insert(
-    &heads->Heads.Priority,
+    &priority_queue->Queue,
     &the_thread->Wait.Node.RBTree,
     _Thread_queue_Compare_priority,
     false
@@ -197,10 +230,21 @@ static void _Thread_queue_Priority_do_extract(
   Thread_Control     *the_thread
 )
 {
+  Thread_queue_Priority_queue *priority_queue =
+    _Thread_queue_Priority_queue( heads, the_thread );
+
   _RBTree_Extract(
-    &heads->Heads.Priority,
+    &priority_queue->Queue,
     &the_thread->Wait.Node.RBTree
   );
+
+#if defined(RTEMS_SMP)
+  _Chain_Extract_unprotected( &priority_queue->Node );
+
+  if ( !_RBTree_Is_empty( &priority_queue->Queue ) ) {
+    _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node );
+  }
+#endif
 }
 
 static void _Thread_queue_Priority_enqueue(
@@ -232,11 +276,19 @@ static Thread_Control *_Thread_queue_Priority_first(
   Thread_queue_Heads *heads
 )
 {
-  RBTree_Control *priority_queue = &heads->Heads.Priority;
-  RBTree_Node    *first;
+  Thread_queue_Priority_queue *priority_queue;
+  RBTree_Node                 *first;
+
+#if defined(RTEMS_SMP)
+  _Assert( !_Chain_Is_empty( &heads->Heads.Fifo ) );
+  priority_queue = (Thread_queue_Priority_queue *)
+    _Chain_First( &heads->Heads.Fifo );
+#else
+  priority_queue = &heads->Heads.Priority;
+#endif
 
-  _Assert( !_RBTree_Is_empty( priority_queue ) );
-  first = _RBTree_Minimum( priority_queue );
+  _Assert( !_RBTree_Is_empty( &priority_queue->Queue ) );
+  first = _RBTree_Minimum( &priority_queue->Queue );
 
   return THREAD_RBTREE_NODE_TO_THREAD( first );
 }