Thread Queue Priority Discipline Reimplemented with RBTree

6 files changed, 69 insertions, 259 deletions

diff --git a/cpukit/score/src/threadq.c b/cpukit/score/src/threadq.c
index 69b687fe1c..0ffbfada09 100644
--- a/cpukit/score/src/threadq.c
+++ b/cpukit/score/src/threadq.c
@@ -6,7 +6,7 @@
  */
 
 /*
- *  COPYRIGHT (c) 1989-2008.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -22,6 +22,28 @@
 #include <rtems/score/chainimpl.h>
 #include <rtems/score/scheduler.h>
 
+#include <rtems/score/rbtreeimpl.h>
+
+int _Thread_queue_Compare_priority(
+  const RBTree_Node *left,
+  const RBTree_Node *right
+)
+{
+  Priority_Control left_priority = _RBTree_Container_of
+    (left,Thread_Control,RBNode)->current_priority;
+  Priority_Control right_priority = _RBTree_Container_of
+    (right,Thread_Control,RBNode)->current_priority;
+
+  /*
+   * SuperCore priorities use lower numbers to indicate greater importance.
+   */
+  if ( left_priority == right_priority )
+    return 0;
+  if ( left_priority < right_priority )
+    return -1;
+  return 1;
+}
+
 void _Thread_queue_Initialize(
   Thread_queue_Control         *the_thread_queue,
   Thread_queue_Disciplines      the_discipline,
@@ -39,12 +61,7 @@ void _Thread_queue_Initialize(
   the_thread_queue->sync_state     = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
 
   if ( the_discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY ) {
-    uint32_t   index;
-
-    for( index=0 ;
-         index < TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS ;
-         index++)
-      _Chain_Initialize_empty( &the_thread_queue->Queues.Priority[index] );
+    _RBTree_Initialize_empty( &the_thread_queue->Queues.Priority );
   } else { /* must be THREAD_QUEUE_DISCIPLINE_FIFO */
     _Chain_Initialize_empty( &the_thread_queue->Queues.Fifo );
   }
diff --git a/cpukit/score/src/threadqdequeuepriority.c b/cpukit/score/src/threadqdequeuepriority.c
index affd75817d..cfae72fa71 100644
--- a/cpukit/score/src/threadqdequeuepriority.c
+++ b/cpukit/score/src/threadqdequeuepriority.c
@@ -6,7 +6,7 @@
  */
 
 /*
- *  COPYRIGHT (c) 1989-2008.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -19,7 +19,7 @@
 #endif
 
 #include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
+#include <rtems/score/rbtreeimpl.h>
 #include <rtems/score/isrlevel.h>
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/watchdogimpl.h>
@@ -28,67 +28,27 @@ Thread_Control *_Thread_queue_Dequeue_priority(
   Thread_queue_Control *the_thread_queue
 )
 {
-  uint32_t        index;
   ISR_Level       level;
   Thread_Control *the_thread = NULL;  /* just to remove warnings */
-  Thread_Control *new_first_thread;
-  Chain_Node     *head;
-  Chain_Node     *tail;
-  Chain_Node     *new_first_node;
-  Chain_Node     *new_second_node;
-  Chain_Node     *last_node;
-  Chain_Node     *next_node;
-  Chain_Node     *previous_node;
+  RBTree_Node    *first;
 
   _ISR_Disable( level );
-  for( index=0 ;
-       index < TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS ;
-       index++ ) {
-    if ( !_Chain_Is_empty( &the_thread_queue->Queues.Priority[ index ] ) ) {
-      the_thread = (Thread_Control *) _Chain_First(
-        &the_thread_queue->Queues.Priority[ index ]
-      );
-      goto dequeue;
-    }
+
+  first = _RBTree_Get( &the_thread_queue->Queues.Priority, RBT_LEFT );
+  if ( !first ) {
+    /*
+     * We did not find a thread to unblock.
+     */
+    _ISR_Enable( level );
+    return NULL;
   }
 
   /*
-   * We did not find a thread to unblock.
+   * We found a thread to unblock.
    */
-  _ISR_Enable( level );
-  return NULL;
 
-dequeue:
+  the_thread = _RBTree_Container_of( first, Thread_Control, RBNode );
   the_thread->Wait.queue = NULL;
-  new_first_node   = _Chain_First( &the_thread->Wait.Block2n );
-  new_first_thread = (Thread_Control *) new_first_node;
-  next_node        = the_thread->Object.Node.next;
-  previous_node    = the_thread->Object.Node.previous;
-
-  if ( !_Chain_Is_empty( &the_thread->Wait.Block2n ) ) {
-    last_node       = _Chain_Last( &the_thread->Wait.Block2n );
-    new_second_node = new_first_node->next;
-
-    previous_node->next      = new_first_node;
-    next_node->previous      = new_first_node;
-    new_first_node->next     = next_node;
-    new_first_node->previous = previous_node;
-
-    if ( !_Chain_Has_only_one_node( &the_thread->Wait.Block2n ) ) {
-                                                /* > two threads on 2-n */
-      head = _Chain_Head( &new_first_thread->Wait.Block2n );
-      tail = _Chain_Tail( &new_first_thread->Wait.Block2n );
-
-      new_second_node->previous = head;
-      head->next = new_second_node;
-      tail->previous = last_node;
-      last_node->next = tail;
-    }
-  } else {
-    previous_node->next = next_node;
-    next_node->previous = previous_node;
-  }
-
   if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
     _ISR_Enable( level );
   } else {
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index 5d5f4578a5..2230f10321 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -6,10 +6,7 @@
  */
 
 /*
- *  Thread Queue Handler
- *
- *
- *  COPYRIGHT (c) 1989-2008.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
diff --git a/cpukit/score/src/threadqenqueuepriority.c b/cpukit/score/src/threadqenqueuepriority.c
index c4b41ba9bc..a8d2bff740 100644
--- a/cpukit/score/src/threadqenqueuepriority.c
+++ b/cpukit/score/src/threadqenqueuepriority.c
@@ -1,12 +1,12 @@
 /**
- *  @file
+ * @file
  *
- *  @brief Thread Queue Enqueue Priority
- *  @ingroup ScoreThreadQ
+ * @brief Thread Queue Enqueue Priority
+ * @ingroup ScoreThreadQ
  */
 
 /*
- *  COPYRIGHT (c) 1989-2009.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -19,154 +19,35 @@
 #endif
 
 #include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
 #include <rtems/score/isrlevel.h>
 #include <rtems/score/statesimpl.h>
 
-/*
- *  Support the user forcing the unrolling to be disabled.
- */
-#if __RTEMS_DO_NOT_UNROLL_THREADQ_ENQUEUE_PRIORITY__
-  #undef CPU_UNROLL_ENQUEUE_PRIORITY
-  #define CPU_UNROLL_ENQUEUE_PRIORITY FALSE
-#endif
-
 Thread_blocking_operation_States _Thread_queue_Enqueue_priority (
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread,
   ISR_Level            *level_p
 )
 {
-  Priority_Control     search_priority;
-  Thread_Control      *search_thread;
-  ISR_Level            level;
-  Chain_Control       *header;
-  uint32_t             header_index;
-  Chain_Node          *the_node;
-  Chain_Node          *next_node;
-  Chain_Node          *previous_node;
-  Chain_Node          *search_node;
-  Priority_Control     priority;
-  States_Control       block_state;
-
-  _Chain_Initialize_empty( &the_thread->Wait.Block2n );
-
-  priority     = the_thread->current_priority;
-  header_index = _Thread_queue_Header_number( priority );
-  header       = &the_thread_queue->Queues.Priority[ header_index ];
-  block_state  = the_thread_queue->state;
-
-  if ( _Thread_queue_Is_reverse_search( priority ) )
-    goto restart_reverse_search;
+  Thread_blocking_operation_States sync_state;
+  ISR_Level                        level;
 
-restart_forward_search:
-  search_priority = PRIORITY_MINIMUM - 1;
   _ISR_Disable( level );
-  search_thread = (Thread_Control *) _Chain_First( header );
-  while ( !_Chain_Is_tail( header, (Chain_Node *)search_thread ) ) {
-    search_priority = search_thread->current_priority;
-    if ( priority <= search_priority )
-      break;
-
-#if ( CPU_UNROLL_ENQUEUE_PRIORITY == TRUE )
-    search_thread = (Thread_Control *) search_thread->Object.Node.next;
-    if ( _Chain_Is_tail( header, (Chain_Node *)search_thread ) )
-      break;
-    search_priority = search_thread->current_priority;
-    if ( priority <= search_priority )
-      break;
-#endif
-    _ISR_Flash( level );
-    if ( !_States_Are_set( search_thread->current_state, block_state) ) {
-      _ISR_Enable( level );
-      goto restart_forward_search;
-    }
-    search_thread =
-       (Thread_Control *)search_thread->Object.Node.next;
-  }
-
-  if ( the_thread_queue->sync_state !=
-       THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED )
-    goto synchronize;
-
-  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
 
-  if ( priority == search_priority )
-    goto equal_priority;
-
-  search_node   = (Chain_Node *) search_thread;
-  previous_node = search_node->previous;
-  the_node      = (Chain_Node *) the_thread;
-
-  the_node->next         = search_node;
-  the_node->previous     = previous_node;
-  previous_node->next    = the_node;
-  search_node->previous  = the_node;
-  the_thread->Wait.queue = the_thread_queue;
-  _ISR_Enable( level );
-  return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
-
-restart_reverse_search:
-  search_priority     = PRIORITY_MAXIMUM + 1;
-
-  _ISR_Disable( level );
-  search_thread = (Thread_Control *) _Chain_Last( header );
-  while ( !_Chain_Is_head( header, (Chain_Node *)search_thread ) ) {
-    search_priority = search_thread->current_priority;
-    if ( priority >= search_priority )
-      break;
-#if ( CPU_UNROLL_ENQUEUE_PRIORITY == TRUE )
-    search_thread = (Thread_Control *) search_thread->Object.Node.previous;
-    if ( _Chain_Is_head( header, (Chain_Node *)search_thread ) )
-      break;
-    search_priority = search_thread->current_priority;
-    if ( priority >= search_priority )
-      break;
-#endif
-    _ISR_Flash( level );
-    if ( !_States_Are_set( search_thread->current_state, block_state) ) {
+    sync_state = the_thread_queue->sync_state;
+    the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
+    if (sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) {
+      _RBTree_Insert(
+        &the_thread_queue->Queues.Priority,
+        &the_thread->RBNode,
+        _Thread_queue_Compare_priority,
+        false
+      );
+      the_thread->Wait.queue = the_thread_queue;
+      the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
       _ISR_Enable( level );
-      goto restart_reverse_search;
+      return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
     }
-    search_thread = (Thread_Control *)
-                         search_thread->Object.Node.previous;
-  }
-
-  if ( the_thread_queue->sync_state !=
-       THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED )
-    goto synchronize;
-
-  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
-
-  if ( priority == search_priority )
-    goto equal_priority;
-
-  search_node = (Chain_Node *) search_thread;
-  next_node   = search_node->next;
-  the_node    = (Chain_Node *) the_thread;
-
-  the_node->next          = next_node;
-  the_node->previous      = search_node;
-  search_node->next       = the_node;
-  next_node->previous    = the_node;
-  the_thread->Wait.queue = the_thread_queue;
-  _ISR_Enable( level );
-  return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
-
-equal_priority:               /* add at end of priority group */
-  search_node   = _Chain_Tail( &search_thread->Wait.Block2n );
-  previous_node = search_node->previous;
-  the_node      = (Chain_Node *) the_thread;
-
-  the_node->next         = search_node;
-  the_node->previous     = previous_node;
-  previous_node->next    = the_node;
-  search_node->previous  = the_node;
-  the_thread->Wait.queue = the_thread_queue;
-  _ISR_Enable( level );
-  return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
 
-synchronize:
   /*
    *  An interrupt completed the thread's blocking request.
    *  For example, the blocking thread could have been given
@@ -175,5 +56,5 @@ synchronize:
    *  WARNING! Returning with interrupts disabled!
    */
   *level_p = level;
-  return the_thread_queue->sync_state;
+  return sync_state;
 }
diff --git a/cpukit/score/src/threadqextractpriority.c b/cpukit/score/src/threadqextractpriority.c
index 2f2555be33..dc793b3cb5 100644
--- a/cpukit/score/src/threadqextractpriority.c
+++ b/cpukit/score/src/threadqextractpriority.c
@@ -6,7 +6,7 @@
  */
 
 /*
- *  COPYRIGHT (c) 1989-2008.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -19,7 +19,7 @@
 #endif
 
 #include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
+#include <rtems/score/rbtreeimpl.h>
 #include <rtems/score/isrlevel.h>
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/watchdogimpl.h>
@@ -31,17 +31,7 @@ void _Thread_queue_Extract_priority_helper(
 )
 {
   ISR_Level       level;
-  Chain_Node     *head;
-  Chain_Node     *tail;
-  Chain_Node     *the_node;
-  Chain_Node     *next_node;
-  Chain_Node     *previous_node;
-  Thread_Control *new_first_thread;
-  Chain_Node     *new_first_node;
-  Chain_Node     *new_second_node;
-  Chain_Node     *last_node;
 
-  the_node = (Chain_Node *) the_thread;
   _ISR_Disable( level );
   if ( !_States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
     _ISR_Enable( level );
@@ -51,40 +41,14 @@ void _Thread_queue_Extract_priority_helper(
   /*
    *  The thread was actually waiting on a thread queue so let's remove it.
    */
-
-  next_node     = the_node->next;
-  previous_node = the_node->previous;
-
-  if ( !_Chain_Is_empty( &the_thread->Wait.Block2n ) ) {
-    new_first_node   = _Chain_First( &the_thread->Wait.Block2n );
-    new_first_thread = (Thread_Control *) new_first_node;
-    last_node        = _Chain_Last( &the_thread->Wait.Block2n );
-    new_second_node  = new_first_node->next;
-
-    previous_node->next      = new_first_node;
-    next_node->previous      = new_first_node;
-    new_first_node->next     = next_node;
-    new_first_node->previous = previous_node;
-
-    if ( !_Chain_Has_only_one_node( &the_thread->Wait.Block2n ) ) {
-                                        /* > two threads on 2-n */
-      head = _Chain_Head( &new_first_thread->Wait.Block2n );
-      tail = _Chain_Tail( &new_first_thread->Wait.Block2n );
-
-      new_second_node->previous = head;
-      head->next = new_second_node;
-      tail->previous = last_node;
-      last_node->next = tail;
-    }
-  } else {
-    previous_node->next = next_node;
-    next_node->previous = previous_node;
-  }
+  _RBTree_Extract(
+    &the_thread->Wait.queue->Queues.Priority,
+    &the_thread->RBNode
+  );
 
   /*
    *  If we are not supposed to touch timers or the thread's state, return.
    */
-
   if ( requeuing ) {
     _ISR_Enable( level );
     return;
diff --git a/cpukit/score/src/threadqfirstpriority.c b/cpukit/score/src/threadqfirstpriority.c
index 46f708e0c3..9a0bb60157 100644
--- a/cpukit/score/src/threadqfirstpriority.c
+++ b/cpukit/score/src/threadqfirstpriority.c
@@ -2,15 +2,11 @@
  * @file
  *
  * @brief Returns Highest Priority Thread on Thread Queue
- *
  * @ingroup ScoreThreadQ
  */
 
 /*
- *  Thread Queue Handler
- *
- *
- *  COPYRIGHT (c) 1989-2008.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -23,21 +19,16 @@
 #endif
 
 #include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
+#include <rtems/score/rbtreeimpl.h>
 
 Thread_Control *_Thread_queue_First_priority (
   Thread_queue_Control *the_thread_queue
 )
 {
-  uint32_t   index;
+  RBTree_Node *first;
 
-  for( index=0 ;
-       index < TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS ;
-       index++ ) {
-    if ( !_Chain_Is_empty( &the_thread_queue->Queues.Priority[ index ] ) )
-      return (Thread_Control *) _Chain_First(
-        &the_thread_queue->Queues.Priority[ index ]
-      );
-  }
+  first = _RBTree_First( &the_thread_queue->Queues.Priority, RBT_LEFT );
+  if ( first )
+    return _RBTree_Container_of( first, Thread_Control, RBNode );
   return NULL;
 }