Thread Queue: Merge discipline subroutines into main methods

There was a lot of duplication between the discipline subroutines.
With the transition to RBTrees for priority discipline, there were
only a few lines of source code manipulating the data structure
for FIFO and priority. Thus is made sense to fold these back
into the main methods.

As part of doing this all of the tests for discipline were changed
to be in the same order.

14 files changed, 216 insertions, 539 deletions

diff --git a/cpukit/score/src/threadchangepriority.c b/cpukit/score/src/threadchangepriority.c
index 8ddaa0bdc7..ca2c5871af 100644
--- a/cpukit/score/src/threadchangepriority.c
+++ b/cpukit/score/src/threadchangepriority.c
@@ -7,7 +7,7 @@
  */
 
 /*
- *  COPYRIGHT (c) 1989-2011.
+ *  COPYRIGHT (c) 1989-2014.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -23,53 +23,6 @@
 #include <rtems/score/schedulerimpl.h>
 #include <rtems/score/threadqimpl.h>
 
-/**
- *  @brief Invoked when a thread changes priority and is blocked.
- *
- *  This routine is invoked when a thread changes priority and is
- *  blocked on a thread queue.  If the queue is priority ordered,
- *  the_thread is removed from the_thread_queue and reinserted using
- *  its new priority.  This method has no impact on the state of the_thread
- *  or of any timeouts associated with this blocking.
- *
- *  @param[in] the_thread_queue pointer to a threadq header
- *  @param[in] the_thread pointer to a thread control block
- */
-static void _Thread_queue_Requeue(
-  Thread_queue_Control *the_thread_queue,
-  Thread_Control       *the_thread
-)
-{
-  /*
-   * Just in case the thread really wasn't blocked on a thread queue
-   * when we get here.
-   */
-  if ( !the_thread_queue )
-    return;
-
-  /*
-   * If queueing by FIFO, there is nothing to do. This only applies to
-   * priority blocking discipline.
-   */
-  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY ) {
-    Thread_queue_Control *tq = the_thread_queue;
-    ISR_Level             level;
-    ISR_Level             level_ignored;
-
-    _ISR_Disable( level );
-    if ( _States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
-      _Thread_queue_Enter_critical_section( tq );
-      _Thread_queue_Extract_priority_helper(
-        the_thread,
-        the_thread->Wait.return_code,
-        true
-      );
-      (void) _Thread_queue_Enqueue_priority( tq, the_thread, &level_ignored );
-    }
-    _ISR_Enable( level );
-  }
-}
-
 void _Thread_Change_priority(
   Thread_Control   *the_thread,
   Priority_Control  new_priority,
diff --git a/cpukit/score/src/threadqdequeue.c b/cpukit/score/src/threadqdequeue.c
index 93d5c07a1b..3b55e52e83 100644
--- a/cpukit/score/src/threadqdequeue.c
+++ b/cpukit/score/src/threadqdequeue.c
@@ -7,7 +7,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,33 +19,73 @@
 #include "config.h"
 #endif
 
+#include <rtems/score/chainimpl.h>
+#include <rtems/score/threadimpl.h>
 #include <rtems/score/threadqimpl.h>
 #include <rtems/score/isrlevel.h>
+#include <rtems/score/rbtreeimpl.h>
+#include <rtems/score/watchdogimpl.h>
 
 Thread_Control *_Thread_queue_Dequeue(
   Thread_queue_Control *the_thread_queue
 )
 {
-  Thread_Control *(*dequeue_p)( Thread_queue_Control * );
   Thread_Control *the_thread;
   ISR_Level       level;
   Thread_blocking_operation_States  sync_state;
 
-  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY )
-    dequeue_p = _Thread_queue_Dequeue_priority;
-  else /* must be THREAD_QUEUE_DISCIPLINE_FIFO */
-    dequeue_p = _Thread_queue_Dequeue_fifo;
-
-  the_thread = (*dequeue_p)( the_thread_queue );
+  the_thread = NULL;
   _ISR_Disable( level );
-    if ( !the_thread ) {
-      sync_state = the_thread_queue->sync_state;
-      if ( (sync_state == THREAD_BLOCKING_OPERATION_TIMEOUT) ||
-           (sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) ) {
-        the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SATISFIED;
-        the_thread = _Thread_Executing;
-      }
+
+  /*
+   * Invoke the discipline specific dequeue method.
+   */
+  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
+    if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) ) {
+      the_thread = (Thread_Control *)
+       _Chain_Get_first_unprotected( &the_thread_queue->Queues.Fifo );
+    }
+  } else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
+    RBTree_Node    *first;
+
+    first = _RBTree_Get( &the_thread_queue->Queues.Priority, RBT_LEFT );
+    if ( first ) {
+      the_thread = _RBTree_Container_of( first, Thread_Control, RBNode );
+    }
+  }
+
+  /*
+   * We did not find a thread to unblock.
+   */
+  if ( !the_thread ) {
+    sync_state = the_thread_queue->sync_state;
+    if ( (sync_state == THREAD_BLOCKING_OPERATION_TIMEOUT) ||
+         (sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) ) {
+      the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SATISFIED;
+      the_thread = _Thread_Executing;
     }
-  _ISR_Enable( level );
+    _ISR_Enable( level );
+    return NULL;
+  }
+
+  /*
+   * We found a thread to unblock.
+   */
+  the_thread->Wait.queue = NULL;
+  if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
+    _ISR_Enable( level );
+  } else {
+    _Watchdog_Deactivate( &the_thread->Timer );
+    _ISR_Enable( level );
+    (void) _Watchdog_Remove( &the_thread->Timer );
+  }
+
+  _Thread_Unblock( the_thread );
+
+#if defined(RTEMS_MULTIPROCESSING)
+  if ( !_Objects_Is_local_id( the_thread->Object.id ) )
+    _Thread_MP_Free_proxy( the_thread );
+#endif
+
   return the_thread;
 }
diff --git a/cpukit/score/src/threadqdequeuefifo.c b/cpukit/score/src/threadqdequeuefifo.c
deleted file mode 100644
index 3500e37425..0000000000
--- a/cpukit/score/src/threadqdequeuefifo.c
+++ /dev/null
@@ -1,61 +0,0 @@
-/**
- *  @file
- *
- *  @brief Thread Queue Dequeue FIFO
- *  @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2008.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
-#include <rtems/score/isrlevel.h>
-#include <rtems/score/threadimpl.h>
-#include <rtems/score/watchdogimpl.h>
-
-Thread_Control *_Thread_queue_Dequeue_fifo(
-  Thread_queue_Control *the_thread_queue
-)
-{
-  ISR_Level              level;
-  Thread_Control *the_thread;
-
-  _ISR_Disable( level );
-  if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) ) {
-
-    the_thread = (Thread_Control *)
-       _Chain_Get_first_unprotected( &the_thread_queue->Queues.Fifo );
-
-    the_thread->Wait.queue = NULL;
-    if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
-      _ISR_Enable( level );
-      _Thread_Unblock( the_thread );
-    } else {
-      _Watchdog_Deactivate( &the_thread->Timer );
-      _ISR_Enable( level );
-      (void) _Watchdog_Remove( &the_thread->Timer );
-      _Thread_Unblock( the_thread );
-    }
-
-#if defined(RTEMS_MULTIPROCESSING)
-    if ( !_Objects_Is_local_id( the_thread->Object.id ) )
-      _Thread_MP_Free_proxy( the_thread );
-#endif
-
-    return the_thread;
-  }
-
-  _ISR_Enable( level );
-  return NULL;
-}
diff --git a/cpukit/score/src/threadqdequeuepriority.c b/cpukit/score/src/threadqdequeuepriority.c
deleted file mode 100644
index cfae72fa71..0000000000
--- a/cpukit/score/src/threadqdequeuepriority.c
+++ /dev/null
@@ -1,67 +0,0 @@
-/**
- * @file
- *
- * @brief  Thread Queue Dequeue Priority
- * @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2014.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/rbtreeimpl.h>
-#include <rtems/score/isrlevel.h>
-#include <rtems/score/threadimpl.h>
-#include <rtems/score/watchdogimpl.h>
-
-Thread_Control *_Thread_queue_Dequeue_priority(
-  Thread_queue_Control *the_thread_queue
-)
-{
-  ISR_Level       level;
-  Thread_Control *the_thread = NULL;  /* just to remove warnings */
-  RBTree_Node    *first;
-
-  _ISR_Disable( level );
-
-  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 found a thread to unblock.
-   */
-
-  the_thread = _RBTree_Container_of( first, Thread_Control, RBNode );
-  the_thread->Wait.queue = NULL;
-  if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
-    _ISR_Enable( level );
-  } else {
-    _Watchdog_Deactivate( &the_thread->Timer );
-    _ISR_Enable( level );
-    (void) _Watchdog_Remove( &the_thread->Timer );
-  }
-
-  _Thread_Unblock( the_thread );
-
-#if defined(RTEMS_MULTIPROCESSING)
-  if ( !_Objects_Is_local_id( the_thread->Object.id ) )
-    _Thread_MP_Free_proxy( the_thread );
-#endif
-  return( the_thread );
-}
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index 2230f10321..0fa5fa6ae4 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -32,11 +32,6 @@ void _Thread_queue_Enqueue_with_handler(
 {
   ISR_Level                         level;
   Thread_blocking_operation_States  sync_state;
-  Thread_blocking_operation_States (*enqueue_p)(
-    Thread_queue_Control *,
-    Thread_Control *,
-    ISR_Level *
-  );
 
 #if defined(RTEMS_MULTIPROCESSING)
   if ( _Thread_MP_Is_receive( the_thread ) && the_thread->receive_packet )
@@ -63,14 +58,38 @@ void _Thread_queue_Enqueue_with_handler(
   }
 
   /*
-   *  Now enqueue the thread per the discipline for this thread queue.
+   * Now initiate the enqueuing and checking if the blocking operation
+   * should be completed or the thread has had its blocking condition
+   * satisfied before we got here.
    */
-  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY )
-    enqueue_p = _Thread_queue_Enqueue_priority;
-  else /* must be THREAD_QUEUE_DISCIPLINE_FIFO */
-    enqueue_p = _Thread_queue_Enqueue_fifo;
+  _ISR_Disable( level );
 
-  sync_state = (*enqueue_p)( the_thread_queue, the_thread, &level );
-  if ( sync_state != THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED )
+  sync_state = the_thread_queue->sync_state;
+  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
+
+  if ( sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED ) {
+    /*
+     * Invoke the discipline specific enqueue method.
+     */
+    if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
+      _Chain_Append_unprotected(
+        &the_thread_queue->Queues.Fifo,
+        &the_thread->Object.Node
+      );
+    } else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
+      _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 );
+    return;
+  } else { /* sync_state != THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED ) */
     _Thread_blocking_operation_Cancel( sync_state, the_thread, level );
+  }
 }
diff --git a/cpukit/score/src/threadqenqueuefifo.c b/cpukit/score/src/threadqenqueuefifo.c
deleted file mode 100644
index 6124af4847..0000000000
--- a/cpukit/score/src/threadqenqueuefifo.c
+++ /dev/null
@@ -1,59 +0,0 @@
-/**
- *  @file
- *
- *  @brief  Thread queue Enqueue FIFO
- *  @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2008.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
-#include <rtems/score/isrlevel.h>
-
-Thread_blocking_operation_States _Thread_queue_Enqueue_fifo (
-  Thread_queue_Control *the_thread_queue,
-  Thread_Control       *the_thread,
-  ISR_Level            *level_p
-)
-{
-  Thread_blocking_operation_States sync_state;
-  ISR_Level                        level;
-
-  _ISR_Disable( level );
-
-    sync_state = the_thread_queue->sync_state;
-    the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
-    if (sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) {
-      _Chain_Append_unprotected(
-        &the_thread_queue->Queues.Fifo,
-        &the_thread->Object.Node
-      );
-      the_thread->Wait.queue = the_thread_queue;
-
-      the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
-      _ISR_Enable( level );
-      return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
-    }
-
-  /*
-   *  An interrupt completed the thread's blocking request.
-   *  For example, the blocking thread could have been given
-   *  the mutex by an ISR or timed out.
-   *
-   *  WARNING! Returning with interrupts disabled!
-   */
-  *level_p = level;
-  return sync_state;
-}
diff --git a/cpukit/score/src/threadqenqueuepriority.c b/cpukit/score/src/threadqenqueuepriority.c
deleted file mode 100644
index a8d2bff740..0000000000
--- a/cpukit/score/src/threadqenqueuepriority.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/**
- * @file
- *
- * @brief Thread Queue Enqueue Priority
- * @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2014.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/isrlevel.h>
-#include <rtems/score/statesimpl.h>
-
-Thread_blocking_operation_States _Thread_queue_Enqueue_priority (
-  Thread_queue_Control *the_thread_queue,
-  Thread_Control       *the_thread,
-  ISR_Level            *level_p
-)
-{
-  Thread_blocking_operation_States sync_state;
-  ISR_Level                        level;
-
-  _ISR_Disable( level );
-
-    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 );
-      return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
-    }
-
-  /*
-   *  An interrupt completed the thread's blocking request.
-   *  For example, the blocking thread could have been given
-   *  the mutex by an ISR or timed out.
-   *
-   *  WARNING! Returning with interrupts disabled!
-   */
-  *level_p = level;
-  return sync_state;
-}
diff --git a/cpukit/score/src/threadqextract.c b/cpukit/score/src/threadqextract.c
index 0a9c9d4932..bc7d34686a 100644
--- a/cpukit/score/src/threadqextract.c
+++ b/cpukit/score/src/threadqextract.c
@@ -7,7 +7,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,10 @@
 #include "config.h"
 #endif
 
+#include <rtems/score/chainimpl.h>
+#include <rtems/score/threadimpl.h>
 #include <rtems/score/threadqimpl.h>
+#include <rtems/score/watchdogimpl.h>
 
 void _Thread_queue_Extract_with_return_code(
   Thread_queue_Control *the_thread_queue,
@@ -27,14 +30,41 @@ void _Thread_queue_Extract_with_return_code(
   uint32_t              return_code
 )
 {
-  /*
-   * Can not use indirect function pointer here since Extract priority
-   * is a macro and the underlying methods do not have the same signature.
-   */
-  if  ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY )
-    return _Thread_queue_Extract_priority( the_thread, return_code );
-  else /* must be THREAD_QUEUE_DISCIPLINE_FIFO */
-    return _Thread_queue_Extract_fifo( the_thread, return_code );
+  ISR_Level level;
+
+  _ISR_Disable( level );
+
+  if ( !_States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
+    _ISR_Enable( level );
+    return;
+  }
+
+  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
+    _Chain_Extract_unprotected( &the_thread->Object.Node );
+  } else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
+    _RBTree_Extract(
+      &the_thread->Wait.queue->Queues.Priority,
+      &the_thread->RBNode
+    );
+  }
+
+  the_thread->Wait.queue = NULL;
+  the_thread->Wait.return_code = return_code;
+
+  if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
+    _ISR_Enable( level );
+  } else {
+    _Watchdog_Deactivate( &the_thread->Timer );
+    _ISR_Enable( level );
+    (void) _Watchdog_Remove( &the_thread->Timer );
+  }
+
+  _Thread_Unblock( the_thread );
+
+#if defined(RTEMS_MULTIPROCESSING)
+  if ( !_Objects_Is_local_id( the_thread->Object.id ) )
+    _Thread_MP_Free_proxy( the_thread );
+#endif
 
 }
 
diff --git a/cpukit/score/src/threadqextractfifo.c b/cpukit/score/src/threadqextractfifo.c
deleted file mode 100644
index 80fc7f3964..0000000000
--- a/cpukit/score/src/threadqextractfifo.c
+++ /dev/null
@@ -1,61 +0,0 @@
-/**
- * @file
- *
- * @brief Removes a Thread from a Thread  Queue
- *
- * @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2008.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
-#include <rtems/score/isrlevel.h>
-#include <rtems/score/threadimpl.h>
-#include <rtems/score/watchdogimpl.h>
-
-void _Thread_queue_Extract_fifo(
-  Thread_Control       *the_thread,
-  uint32_t              return_code
-)
-{
-  ISR_Level level;
-
-  _ISR_Disable( level );
-
-  if ( !_States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
-    _ISR_Enable( level );
-    return;
-  }
-
-  _Chain_Extract_unprotected( &the_thread->Object.Node );
-
-  the_thread->Wait.queue = NULL;
-  the_thread->Wait.return_code = return_code;
-
-  if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
-    _ISR_Enable( level );
-  } else {
-    _Watchdog_Deactivate( &the_thread->Timer );
-    _ISR_Enable( level );
-    (void) _Watchdog_Remove( &the_thread->Timer );
-  }
-
-  _Thread_Unblock( the_thread );
-
-#if defined(RTEMS_MULTIPROCESSING)
-  if ( !_Objects_Is_local_id( the_thread->Object.id ) )
-    _Thread_MP_Free_proxy( the_thread );
-#endif
-}
diff --git a/cpukit/score/src/threadqextractpriority.c b/cpukit/score/src/threadqextractpriority.c
deleted file mode 100644
index dc793b3cb5..0000000000
--- a/cpukit/score/src/threadqextractpriority.c
+++ /dev/null
@@ -1,73 +0,0 @@
-/**
- * @file
- *
- * @brief Thread queue Extract priority Helper
- * @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2014.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/rbtreeimpl.h>
-#include <rtems/score/isrlevel.h>
-#include <rtems/score/threadimpl.h>
-#include <rtems/score/watchdogimpl.h>
-
-void _Thread_queue_Extract_priority_helper(
-  Thread_Control       *the_thread,
-  uint32_t              return_code,
-  bool                  requeuing
-)
-{
-  ISR_Level       level;
-
-  _ISR_Disable( level );
-  if ( !_States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
-    _ISR_Enable( level );
-    return;
-  }
-
-  /*
-   *  The thread was actually waiting on a thread queue so let's remove it.
-   */
-  _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;
-  }
-
-  the_thread->Wait.queue = NULL;
-  the_thread->Wait.return_code = return_code;
-
-  if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
-    _ISR_Enable( level );
-  } else {
-    _Watchdog_Deactivate( &the_thread->Timer );
-    _ISR_Enable( level );
-    (void) _Watchdog_Remove( &the_thread->Timer );
-  }
-  _Thread_Unblock( the_thread );
-
-#if defined(RTEMS_MULTIPROCESSING)
-  if ( !_Objects_Is_local_id( the_thread->Object.id ) )
-    _Thread_MP_Free_proxy( the_thread );
-#endif
-}
diff --git a/cpukit/score/src/threadqfirst.c b/cpukit/score/src/threadqfirst.c
index 7b7999665d..39f7c3f5b3 100644
--- a/cpukit/score/src/threadqfirst.c
+++ b/cpukit/score/src/threadqfirst.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
@@ -18,18 +18,33 @@
 #include "config.h"
 #endif
 
+#include <rtems/score/chainimpl.h>
+#include <rtems/score/isrlevel.h>
 #include <rtems/score/threadqimpl.h>
 
 Thread_Control *_Thread_queue_First(
   Thread_queue_Control *the_thread_queue
 )
 {
-  Thread_Control * (*first_p)(Thread_queue_Control *);
+  ISR_Level       level;
+  Thread_Control *thread;
 
-  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY )
-      first_p = _Thread_queue_First_priority;
-  else /* must be THREAD_QUEUE_DISCIPLINE_FIFO */
-      first_p = _Thread_queue_First_fifo;
+  thread = NULL;
 
-  return (*first_p)( the_thread_queue );
+  _ISR_Disable( level );
+
+  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
+    if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) )
+      thread = (Thread_Control *) _Chain_First(&the_thread_queue->Queues.Fifo);
+  } else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
+    RBTree_Node *first;
+
+    first = _RBTree_First( &the_thread_queue->Queues.Priority, RBT_LEFT );
+    if ( first )
+      thread = _RBTree_Container_of( first, Thread_Control, RBNode );
+  }
+
+  _ISR_Enable( level );
+
+  return thread;
 }
diff --git a/cpukit/score/src/threadqfirstfifo.c b/cpukit/score/src/threadqfirstfifo.c
deleted file mode 100644
index df8f158a53..0000000000
--- a/cpukit/score/src/threadqfirstfifo.c
+++ /dev/null
@@ -1,32 +0,0 @@
-/**
- *  @file
- *
- *  @brief Thread Queue First FIFO
- *  @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2008.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/chainimpl.h>
-
-Thread_Control *_Thread_queue_First_fifo(
-  Thread_queue_Control *the_thread_queue
-)
-{
-  if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) )
-    return (Thread_Control *) _Chain_First( &the_thread_queue->Queues.Fifo );
-
-  return NULL;
-}
diff --git a/cpukit/score/src/threadqfirstpriority.c b/cpukit/score/src/threadqfirstpriority.c
deleted file mode 100644
index 9a0bb60157..0000000000
--- a/cpukit/score/src/threadqfirstpriority.c
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * @file
- *
- * @brief Returns Highest Priority Thread on Thread Queue
- * @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2014.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/rbtreeimpl.h>
-
-Thread_Control *_Thread_queue_First_priority (
-  Thread_queue_Control *the_thread_queue
-)
-{
-  RBTree_Node *first;
-
-  first = _RBTree_First( &the_thread_queue->Queues.Priority, RBT_LEFT );
-  if ( first )
-    return _RBTree_Container_of( first, Thread_Control, RBNode );
-  return NULL;
-}
diff --git a/cpukit/score/src/threadqrequeue.c b/cpukit/score/src/threadqrequeue.c
new file mode 100644
index 0000000000..791a59e73c
--- /dev/null
+++ b/cpukit/score/src/threadqrequeue.c
@@ -0,0 +1,67 @@
+/**
+ * @file
+ *
+ * @brief Thread Queue Requeue
+ * @ingroup ScoreThreadQ
+ */
+
+/*
+ *  COPYRIGHT (c) 1989-2014.
+ *  On-Line Applications Research Corporation (OAR).
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <rtems/score/threadqimpl.h>
+#include <rtems/score/isrlevel.h>
+#include <rtems/score/threadimpl.h>
+#include <rtems/score/watchdogimpl.h>
+
+void _Thread_queue_Requeue(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread
+)
+{
+  /*
+   * Just in case the thread really wasn't blocked on a thread queue
+   * when we get here.
+   */
+  if ( !the_thread_queue )
+    return;
+
+  /*
+   * If queueing by FIFO, there is nothing to do. This only applies to
+   * priority blocking discipline.
+   */
+  if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY ) {
+    Thread_queue_Control *tq = the_thread_queue;
+    ISR_Level             level;
+
+    _ISR_Disable( level );
+    if ( _States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
+      _Thread_queue_Enter_critical_section( tq );
+
+      /* extract the thread */
+      _RBTree_Extract(
+        &the_thread->Wait.queue->Queues.Priority,
+        &the_thread->RBNode
+      );
+
+      /* enqueue the thread at the new priority */
+      _RBTree_Insert(
+        &the_thread_queue->Queues.Priority,
+        &the_thread->RBNode,
+        _Thread_queue_Compare_priority,
+        false
+      );
+    }
+    _ISR_Enable( level );
+  }
+}
+