Thread Queue Priority Discipline Reimplemented with RBTree

10 files changed, 107 insertions, 325 deletions

diff --git a/cpukit/configure.ac b/cpukit/configure.ac
index e87aa4a94a..19e5b810a6 100644
--- a/cpukit/configure.ac
+++ b/cpukit/configure.ac
@@ -248,12 +248,6 @@ RTEMS_CPUOPT([__RTEMS_DO_NOT_INLINE_CORE_MUTEX_SEIZE__],
   [1],
   [disable inlining _Thread_Enable_dispatch])
 
-## This improves both the size and coverage analysis.
-RTEMS_CPUOPT([__RTEMS_DO_NOT_UNROLL_THREADQ_ENQUEUE_PRIORITY__],
-  [test x"${RTEMS_DO_NOT_UNROLL_THREADQ_ENQUEUE_PRIORITY}" = x"1"],
-  [1],
-  [disable inlining _Thread_queue_Enqueue_priority])
-
 ## This gives the same behavior as 4.8 and older
 RTEMS_CPUOPT([__RTEMS_STRICT_ORDER_MUTEX__],
   [test x"${ENABLE_STRICT_ORDER_MUTEX}" = x"1"],
diff --git a/cpukit/score/include/rtems/score/thread.h b/cpukit/score/include/rtems/score/thread.h
index 0d9025fdfa..be357895dd 100644
--- a/cpukit/score/include/rtems/score/thread.h
+++ b/cpukit/score/include/rtems/score/thread.h
@@ -304,6 +304,8 @@ typedef struct {
 typedef struct {
   /** This field is the object management structure for each proxy. */
   Objects_Control          Object;
+  /** This field is used to enqueue the thread on RBTrees. */
+  RBTree_Node              RBNode;
   /** This field is the current execution state of this proxy. */
   States_Control           current_state;
   /** This field is the current priority state of this proxy. */
@@ -541,6 +543,8 @@ typedef struct {
 struct Thread_Control_struct {
   /** This field is the object management structure for each thread. */
   Objects_Control          Object;
+  /** This field is used to enqueue the thread on RBTrees. */
+  RBTree_Node              RBNode;
   /** This field is the current execution state of this thread. */
   States_Control           current_state;
   /** This field is the current priority state of this thread. */
diff --git a/cpukit/score/include/rtems/score/threadq.h b/cpukit/score/include/rtems/score/threadq.h
index c8f2aa44bf..35e0f1d617 100644
--- a/cpukit/score/include/rtems/score/threadq.h
+++ b/cpukit/score/include/rtems/score/threadq.h
@@ -8,7 +8,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,7 @@
 #include <rtems/score/chain.h>
 #include <rtems/score/states.h>
 #include <rtems/score/threadsync.h>
+#include <rtems/score/rbtree.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -48,22 +49,6 @@ typedef enum {
 }   Thread_queue_Disciplines;
 
 /**
- *  This is one of the constants used to manage the priority queues.
- *
- *  There are four chains used to maintain a priority -- each chain
- *  manages a distinct set of task priorities.  The number of chains
- *  is determined by TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS.
- *  The following set must be consistent.
- *
- *  The set below configures 4 headers -- each contains 64 priorities.
- *  Header x manages priority range (x*64) through ((x*64)+63).  If
- *  the priority is more than half way through the priority range it
- *  is in, then the search is performed from the rear of the chain.
- *  This halves the search time to find the insertion point.
- */
-#define TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS 4
-
-/**
  *  This is the structure used to manage sets of tasks which are blocked
  *  waiting to acquire a resource.
  */
@@ -74,8 +59,8 @@ typedef struct {
   union {
     /** This is the FIFO discipline list. */
     Chain_Control Fifo;
-    /** This is the set of lists for priority discipline waiting. */
-    Chain_Control Priority[TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS];
+    /** This is the set of threads for priority discipline waiting. */
+    RBTree_Control Priority;
   } Queues;
   /** This field is used to manage the critical section. */
   Thread_blocking_operation_States sync_state;
diff --git a/cpukit/score/include/rtems/score/threadqimpl.h b/cpukit/score/include/rtems/score/threadqimpl.h
index 91f09383f2..0acc66ee6c 100644
--- a/cpukit/score/include/rtems/score/threadqimpl.h
+++ b/cpukit/score/include/rtems/score/threadqimpl.h
@@ -8,7 +8,7 @@
  */
 
 /*
- *  COPYRIGHT (c) 1989-2009.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -37,18 +37,6 @@ extern "C" {
 #define THREAD_QUEUE_WAIT_FOREVER  WATCHDOG_NO_TIMEOUT
 
 /**
- *  This is one of the constants used to manage the priority queues.
- *  @ref TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS for more details.
- */
-#define TASK_QUEUE_DATA_PRIORITIES_PER_HEADER      64
-
-/**
- *  This is one of the constants used to manage the priority queues.
- *  @ref TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS for more details.
- */
-#define TASK_QUEUE_DATA_REVERSE_SEARCH_MASK        0x20
-
-/**
  *  The following type defines the callout used when a remote task
  *  is extracted from a local thread queue.
  */
@@ -117,13 +105,25 @@ void _Thread_queue_Enqueue_with_handler(
  *  and cancels any timeouts associated with this blocking.
  *
  *  @param[in] the_thread_queue is the pointer to the ThreadQ header
- *  @param[in] the_thread is the pointer to a thread control block that is to be removed
+ *  @param[in] the_thread is the pointer to a thread control block that
+ *      is to be removed
  */
 void _Thread_queue_Extract(
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread
 );
 
+/**
+ *  @brief Extracts thread from thread queue (w/return code).
+ *
+ *  This routine removes @a the_thread from @a the_thread_queue
+ *  and cancels any timeouts associated with this blocking.
+ *
+ *  @param[in] the_thread_queue is the pointer to the ThreadQ header
+ *  @param[in] the_thread is the pointer to a thread control block that
+ *      is to be removed
+ *  @param[in] return_code specifies the status to be returned.
+ */
 void _Thread_queue_Extract_with_return_code(
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread,
@@ -227,8 +227,7 @@ Thread_Control *_Thread_queue_Dequeue_priority(
  *          well as filling in *@ level_p with the previous interrupt level.
  *
  *  - INTERRUPT LATENCY:
- *    + forward less than
- *    + forward equal
+ *    + single case
  */
 Thread_blocking_operation_States _Thread_queue_Enqueue_priority (
   Thread_queue_Control *the_thread_queue,
@@ -245,8 +244,9 @@ Thread_blocking_operation_States _Thread_queue_Enqueue_priority (
  *  @param[in] the_thread pointer to a thread control block
  *  @param[in] requeuing true if requeuing and should not alter
  *         timeout or state
+ *
  *  - INTERRUPT LATENCY:
- *    + EXTRACT_PRIORITY
+ *    + single case
  *
  *  @retval true The extract operation was performed by the executing context.
  *  @retval false Otherwise.
@@ -262,9 +262,9 @@ void _Thread_queue_Extract_priority_helper(
  *
  * This macro wraps the underlying call and hides the requeuing argument.
  */
-
 #define _Thread_queue_Extract_priority( _the_thread, _return_code ) \
   _Thread_queue_Extract_priority_helper( _the_thread, _return_code, false )
+
 /**
  *  @brief Get highest priority thread on the_thread_queue.
  *
@@ -377,35 +377,24 @@ void _Thread_queue_Process_timeout(
 );
 
 /**
- * This function returns the index of the priority chain on which
- * a thread of the_priority should be placed.
- */
-
-RTEMS_INLINE_ROUTINE uint32_t   _Thread_queue_Header_number (
-  Priority_Control the_priority
-)
-{
-  return (the_priority / TASK_QUEUE_DATA_PRIORITIES_PER_HEADER);
-}
-
-/**
- * This function returns true if the_priority indicates that the
- * enqueue search should start at the front of this priority
- * group chain, and false if the search should start at the rear.
+ * @brief Compare two thread's priority for RBTree Insertion.
+ *
+ * @param[in] left points to the left thread's RBnode
+ * @param[in] right points to the right thread's RBnode
+ *
+ * @retval 1 The @left node is more important than @right node.
+ * @retval 0 The @left node is of equal importance with @right node.
+ * @retval 1 The @left node is less important than @right node.
  */
-
-RTEMS_INLINE_ROUTINE bool _Thread_queue_Is_reverse_search (
-  Priority_Control the_priority
-)
-{
-  return ( the_priority & TASK_QUEUE_DATA_REVERSE_SEARCH_MASK );
-}
+int _Thread_queue_Compare_priority(
+  const RBTree_Node *left,
+  const RBTree_Node *right
+);
 
 /**
  * This routine is invoked to indicate that the specified thread queue is
  * entering a critical section.
  */
-
 RTEMS_INLINE_ROUTINE void _Thread_queue_Enter_critical_section (
   Thread_queue_Control *the_thread_queue
 )
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;
 }