1 files changed, 124 insertions, 23 deletions

diff --git a/cpukit/score/src/threadqops.c b/cpukit/score/src/threadqops.c
index e0be4918b6..d19067fab1 100644
--- a/cpukit/score/src/threadqops.c
+++ b/cpukit/score/src/threadqops.c
@@ -17,6 +17,7 @@
 #endif
 
 #include <rtems/score/threadimpl.h>
+#include <rtems/score/assert.h>
 #include <rtems/score/chainimpl.h>
 #include <rtems/score/rbtreeimpl.h>
 
@@ -37,37 +38,110 @@ static void _Thread_queue_Do_nothing_extract(
   /* Do nothing */
 }
 
-static void _Thread_queue_FIFO_initialize(
-  Thread_queue_Queue *queue
+static void _Thread_queue_Queue_enqueue(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread,
+  void             ( *initialize )( Thread_queue_Heads * ),
+  void             ( *enqueue )( Thread_queue_Heads *, Thread_Control * )
 )
 {
-  _Chain_Initialize_empty( &queue->Heads.Fifo );
+  Thread_queue_Heads *heads = queue->heads;
+  Thread_queue_Heads *spare_heads = the_thread->Wait.spare_heads;
+
+  the_thread->Wait.spare_heads = NULL;
+
+  if ( heads == NULL ) {
+    _Assert( spare_heads != NULL );
+    _Assert( _Chain_Is_empty( &spare_heads->Free_chain ) );
+    heads = spare_heads;
+    queue->heads = heads;
+    ( *initialize )( heads );
+  }
+
+  _Chain_Prepend_unprotected( &heads->Free_chain, &spare_heads->Free_node );
+
+  ( *enqueue )( heads, the_thread );
 }
 
-static void _Thread_queue_FIFO_enqueue(
+static void _Thread_queue_Queue_extract(
   Thread_queue_Queue *queue,
+  Thread_Control     *the_thread,
+  void             ( *extract )( Thread_queue_Heads *, Thread_Control * )
+)
+{
+  Thread_queue_Heads *heads = queue->heads;
+
+  _Assert( heads != NULL );
+
+  the_thread->Wait.spare_heads = RTEMS_CONTAINER_OF(
+    _Chain_Get_first_unprotected( &heads->Free_chain ),
+    Thread_queue_Heads,
+    Free_node
+  );
+
+  if ( _Chain_Is_empty( &heads->Free_chain ) ) {
+    queue->heads = NULL;
+  }
+
+  ( *extract )( heads, the_thread );
+}
+
+static void _Thread_queue_FIFO_do_initialize(
+  Thread_queue_Heads *heads
+)
+{
+  _Chain_Initialize_empty( &heads->Heads.Fifo );
+}
+
+static void _Thread_queue_FIFO_do_enqueue(
+  Thread_queue_Heads *heads,
   Thread_Control     *the_thread
 )
 {
   _Chain_Append_unprotected(
-    &queue->Heads.Fifo,
+    &heads->Heads.Fifo,
     &the_thread->Wait.Node.Chain
   );
 }
 
+static void _Thread_queue_FIFO_do_extract(
+  Thread_queue_Heads *heads,
+  Thread_Control     *the_thread
+)
+{
+  _Chain_Extract_unprotected( &the_thread->Wait.Node.Chain );
+}
+
+static void _Thread_queue_FIFO_enqueue(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread
+)
+{
+  _Thread_queue_Queue_enqueue(
+    queue,
+    the_thread,
+    _Thread_queue_FIFO_do_initialize,
+    _Thread_queue_FIFO_do_enqueue
+  );
+}
+
 static void _Thread_queue_FIFO_extract(
   Thread_queue_Queue *queue,
   Thread_Control     *the_thread
 )
 {
-  _Chain_Extract_unprotected( &the_thread->Wait.Node.Chain );
+  _Thread_queue_Queue_extract(
+    queue,
+    the_thread,
+    _Thread_queue_FIFO_do_extract
+  );
 }
 
 static Thread_Control *_Thread_queue_FIFO_first(
-  Thread_queue_Queue *queue
+  Thread_queue_Heads *heads
 )
 {
-  Chain_Control *fifo = &queue->Heads.Fifo;
+  Chain_Control *fifo = &heads->Heads.Fifo;
 
   return _Chain_Is_empty( fifo ) ?
     NULL : THREAD_CHAIN_NODE_TO_THREAD( _Chain_First( fifo ) );
@@ -79,56 +153,85 @@ static void _Thread_queue_Priority_priority_change(
   Thread_queue_Queue *queue
 )
 {
+  Thread_queue_Heads *heads = queue->heads;
+
+  _Assert( heads != NULL );
+
   _RBTree_Extract(
-    &queue->Heads.Priority,
+    &heads->Heads.Priority,
     &the_thread->Wait.Node.RBTree
   );
   _RBTree_Insert(
-    &queue->Heads.Priority,
+    &heads->Heads.Priority,
     &the_thread->Wait.Node.RBTree,
     _Thread_queue_Compare_priority,
     false
   );
 }
 
-static void _Thread_queue_Priority_initialize(
-  Thread_queue_Queue *queue
+static void _Thread_queue_Priority_do_initialize(
+  Thread_queue_Heads *heads
 )
 {
-  _RBTree_Initialize_empty( &queue->Heads.Priority );
+  _RBTree_Initialize_empty( &heads->Heads.Priority );
 }
 
-static void _Thread_queue_Priority_enqueue(
-  Thread_queue_Queue *queue,
+static void _Thread_queue_Priority_do_enqueue(
+  Thread_queue_Heads *heads,
   Thread_Control     *the_thread
 )
 {
   _RBTree_Insert(
-    &queue->Heads.Priority,
+    &heads->Heads.Priority,
     &the_thread->Wait.Node.RBTree,
     _Thread_queue_Compare_priority,
     false
   );
 }
 
-static void _Thread_queue_Priority_extract(
-  Thread_queue_Queue *queue,
+static void _Thread_queue_Priority_do_extract(
+  Thread_queue_Heads *heads,
   Thread_Control     *the_thread
 )
 {
   _RBTree_Extract(
-    &queue->Heads.Priority,
+    &heads->Heads.Priority,
     &the_thread->Wait.Node.RBTree
   );
 }
 
+static void _Thread_queue_Priority_enqueue(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread
+)
+{
+  _Thread_queue_Queue_enqueue(
+    queue,
+    the_thread,
+    _Thread_queue_Priority_do_initialize,
+    _Thread_queue_Priority_do_enqueue
+  );
+}
+
+static void _Thread_queue_Priority_extract(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread
+)
+{
+  _Thread_queue_Queue_extract(
+    queue,
+    the_thread,
+    _Thread_queue_Priority_do_extract
+  );
+}
+
 static Thread_Control *_Thread_queue_Priority_first(
-  Thread_queue_Queue *queue
+  Thread_queue_Heads *heads
 )
 {
   RBTree_Node *first;
 
-  first = _RBTree_First( &queue->Heads.Priority, RBT_LEFT );
+  first = _RBTree_First( &heads->Heads.Priority, RBT_LEFT );
 
   return first != NULL ? THREAD_RBTREE_NODE_TO_THREAD( first ) : NULL;
 }
@@ -145,7 +248,6 @@ const Thread_queue_Operations _Thread_queue_Operations_default = {
 
 const Thread_queue_Operations _Thread_queue_Operations_FIFO = {
   .priority_change = _Thread_queue_Do_nothing_priority_change,
-  .initialize = _Thread_queue_FIFO_initialize,
   .enqueue = _Thread_queue_FIFO_enqueue,
   .extract = _Thread_queue_FIFO_extract,
   .first = _Thread_queue_FIFO_first
@@ -153,7 +255,6 @@ const Thread_queue_Operations _Thread_queue_Operations_FIFO = {
 
 const Thread_queue_Operations _Thread_queue_Operations_priority = {
   .priority_change = _Thread_queue_Priority_priority_change,
-  .initialize = _Thread_queue_Priority_initialize,
   .enqueue = _Thread_queue_Priority_enqueue,
   .extract = _Thread_queue_Priority_extract,
   .first = _Thread_queue_Priority_first