2007-03-05 Joel Sherrill <joel@OARcorp.com>

	PR 1222/cpukit
	* score/Makefile.am, score/include/rtems/score/coremutex.h,
	score/include/rtems/score/threadq.h,
	score/inline/rtems/score/coremutex.inl, score/src/coremsgsubmit.c,
	score/src/coremutexsurrender.c, score/src/threadchangepriority.c,
	score/src/threadclearstate.c, score/src/threadhandler.c,
	score/src/threadinitialize.c, score/src/threadqdequeuefifo.c,
	score/src/threadqdequeuepriority.c, score/src/threadqenqueue.c,
	score/src/threadqenqueuefifo.c, score/src/threadqenqueuepriority.c,
	score/src/threadqextractfifo.c, score/src/threadqextractpriority.c,
	score/src/threadsetstate.c: Enhance so that when the prioirity of a
	thread that is blocked on a priority based thread queue is changed,
	that its placement in the queue is reevaluated based upon the new
	priority. This enhancement includes modifications to the SuperCore as
	well as new test cases.
	* score/src/threadqrequeue.c: New file.

20 files changed, 300 insertions, 103 deletions

diff --git a/cpukit/ChangeLog b/cpukit/ChangeLog
index dabfdeb3c6..f9c80cbf00 100644
--- a/cpukit/ChangeLog
+++ b/cpukit/ChangeLog
@@ -1,5 +1,24 @@
 2007-03-05	Joel Sherrill <joel@OARcorp.com>
 
+	PR 1222/cpukit
+	* score/Makefile.am, score/include/rtems/score/coremutex.h,
+	score/include/rtems/score/threadq.h,
+	score/inline/rtems/score/coremutex.inl, score/src/coremsgsubmit.c,
+	score/src/coremutexsurrender.c, score/src/threadchangepriority.c,
+	score/src/threadclearstate.c, score/src/threadhandler.c,
+	score/src/threadinitialize.c, score/src/threadqdequeuefifo.c,
+	score/src/threadqdequeuepriority.c, score/src/threadqenqueue.c,
+	score/src/threadqenqueuefifo.c, score/src/threadqenqueuepriority.c,
+	score/src/threadqextractfifo.c, score/src/threadqextractpriority.c,
+	score/src/threadsetstate.c: Enhance so that when the prioirity of a
+	thread that is blocked on a priority based thread queue is changed,
+	that its placement in the queue is reevaluated based upon the new
+	priority. This enhancement includes modifications to the SuperCore as
+	well as new test cases.
+	* score/src/threadqrequeue.c: New file.
+
+2007-03-05	Joel Sherrill <joel@OARcorp.com>
+
 	* sapi/src/exinit.c: Fix spacing.
 
 2007-03-05	Joel Sherrill <joel@OARcorp.com>
diff --git a/cpukit/score/Makefile.am b/cpukit/score/Makefile.am
index a837abe0bd..bf5e15eb1a 100644
--- a/cpukit/score/Makefile.am
+++ b/cpukit/score/Makefile.am
@@ -136,7 +136,8 @@ libscore_a_SOURCES += src/threadq.c src/threadqdequeue.c \
     src/threadqenqueuepriority.c src/threadqextract.c \
     src/threadqextractfifo.c src/threadqextractpriority.c \
     src/threadqextractwithproxy.c src/threadqfirst.c src/threadqfirstfifo.c \
-    src/threadqfirstpriority.c src/threadqflush.c src/threadqtimeout.c
+    src/threadqfirstpriority.c src/threadqflush.c src/threadqrequeue.c \
+    src/threadqtimeout.c
 
 ## TOD_C_FILES
 libscore_a_SOURCES += src/coretod.c src/coretodset.c src/coretodtickle.c \
diff --git a/cpukit/score/include/rtems/score/coremutex.h b/cpukit/score/include/rtems/score/coremutex.h
index 4015ac97e4..1d2d38db59 100644
--- a/cpukit/score/include/rtems/score/coremutex.h
+++ b/cpukit/score/include/rtems/score/coremutex.h
@@ -249,7 +249,7 @@ RTEMS_INLINE_ROUTINE int _CORE_mutex_Seize_interrupt_trylock(
 );
 
 /**
- *  @brief Seize Mutex with Blockign
+ *  @brief Seize Mutex with Blocking
  *
  *  This routine performs the blocking portion of a mutex obtain.
  *  It is an actual subroutine and is not implemented as something
@@ -280,6 +280,16 @@ void _CORE_mutex_Seize_interrupt_blocking(
  *  @note If the mutex is called from an interrupt service routine,
  *        with context switching disabled, or before multitasking,
  *        then a fatal error is generated. 
+ *
+ *
+ *  The logic on this routine is as follows:
+ *
+ *  * If incorrect system state
+ *      return an error 
+ *  * If mutex is available without any contention or blocking
+ *      obtain it with interrupts disabled and returned
+ *  * If the caller is willing to wait
+ *      then they are blocked. 
  */
 #define _CORE_mutex_Seize( \
   _the_mutex, _id, _wait, _timeout, _level ) \
diff --git a/cpukit/score/include/rtems/score/threadq.h b/cpukit/score/include/rtems/score/threadq.h
index 8744887344..7b609285eb 100644
--- a/cpukit/score/include/rtems/score/threadq.h
+++ b/cpukit/score/include/rtems/score/threadq.h
@@ -94,6 +94,20 @@ void _Thread_queue_Enqueue_with_handler(
   Thread_queue_Timeout_callout handler
 );
 
+/**
+ *  @brief Thread queue Requeue
+ *
+ *  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.
+ */
+void _Thread_queue_Requeue(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread
+);
+
 /** @brief  Thread queue Extract
  *
  *  This routine removes the_thread from the_thread_queue
@@ -169,16 +183,27 @@ void _Thread_queue_Enqueue_priority(
   Thread_Control       *the_thread
 );
 
-/** @brief  Thread queue Extract priority
+/** @brief  Thread queue Extract priority Helper
  *
  *  This routine removes the_thread from the_thread_queue
  *  and cancels any timeouts associated with this blocking.
  */
-void _Thread_queue_Extract_priority(
+void _Thread_queue_Extract_priority_helper(
   Thread_queue_Control *the_thread_queue,
-  Thread_Control       *the_thread
+  Thread_Control       *the_thread,
+  boolean               requeuing 
 );
 
+/**
+ * @brief Thread queue Extract priority
+ *
+ * This macro wraps the underlying call and hides the requeuing argument.
+ */
+
+#define _Thread_queue_Extract_priority( _the_thread_queue, _the_thread ) \
+  _Thread_queue_Extract_priority_helper( _the_thread_queue, _the_thread, FALSE )
+
+
 /** @brief  Thread queue First priority
  *
  *  This function returns a pointer to the "first" thread
diff --git a/cpukit/score/inline/rtems/score/coremutex.inl b/cpukit/score/inline/rtems/score/coremutex.inl
index 2bde4a888f..342428ccbb 100644
--- a/cpukit/score/inline/rtems/score/coremutex.inl
+++ b/cpukit/score/inline/rtems/score/coremutex.inl
@@ -143,25 +143,29 @@ RTEMS_INLINE_ROUTINE int _CORE_mutex_Seize_interrupt_trylock(
     if ( !_CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) ) {
         _ISR_Enable( level );
         return 0;
-    }
-    /* else must be CORE_MUTEX_DISCIPLINES_PRIORITY_CEILING */
+    } /* else must be CORE_MUTEX_DISCIPLINES_PRIORITY_CEILING
+       *
+       * we possibly bump the priority of the current holder -- which
+       * happens to be _Thread_Executing.
+       */
     {
-       Priority_Control  ceiling;
-       Priority_Control  current;
+      Priority_Control  ceiling;
+      Priority_Control  current;
+
+      ceiling = the_mutex->Attributes.priority_ceiling;
+      current = executing->current_priority;
+      if ( current == ceiling ) {
+        _ISR_Enable( level );
+        return 0;
+      }
 
-       ceiling = the_mutex->Attributes.priority_ceiling;
-       current = executing->current_priority;
-       if ( current == ceiling ) {
-         _ISR_Enable( level );
-         return 0;
-       }
-       if ( current > ceiling ) {
+      if ( current > ceiling ) {
         _Thread_Disable_dispatch();
         _ISR_Enable( level );
         _Thread_Change_priority(
           the_mutex->holder,
           the_mutex->Attributes.priority_ceiling,
-          FALSE
+         FALSE
         );
         _Thread_Enable_dispatch();
         return 0;
@@ -177,6 +181,11 @@ RTEMS_INLINE_ROUTINE int _CORE_mutex_Seize_interrupt_trylock(
     return 0;
   }
 
+  /*
+   *  At this point, we know the mutex was not available.  If this thread
+   *  is the thread that has locked the mutex, let's see if we are allowed
+   *  to nest access.
+   */
   if ( _Thread_Is_executing( the_mutex->holder ) ) {
     switch ( the_mutex->Attributes.lock_nesting_behavior ) {
       case CORE_MUTEX_NESTING_ACQUIRES:
@@ -192,6 +201,10 @@ RTEMS_INLINE_ROUTINE int _CORE_mutex_Seize_interrupt_trylock(
     }
   }
 
+  /*
+   *  The mutex is not available and the caller must deal with the possibility
+   *  of blocking.
+   */
   return 1;
 }
 
diff --git a/cpukit/score/src/coremsgsubmit.c b/cpukit/score/src/coremsgsubmit.c
index a1ac6d5b87..2f1ea815ca 100644
--- a/cpukit/score/src/coremsgsubmit.c
+++ b/cpukit/score/src/coremsgsubmit.c
@@ -88,7 +88,7 @@ CORE_message_queue_Status _CORE_message_queue_Submit(
         the_thread->Wait.return_argument,
         size
       );
-      *(uint32_t   *)the_thread->Wait.return_argument_1 = size;
+      *(size_t *)the_thread->Wait.return_argument_1 = size;
       the_thread->Wait.count = submit_type;
 
 #if defined(RTEMS_MULTIPROCESSING)
@@ -168,7 +168,7 @@ CORE_message_queue_Status _CORE_message_queue_Submit(
     executing->Wait.queue              = &the_message_queue->Wait_queue;
     executing->Wait.id                 = id;
     executing->Wait.return_argument    = buffer;
-    executing->Wait.return_argument_1  = (void *)size;
+    executing->Wait.option             = size;
     executing->Wait.count              = submit_type;
     _ISR_Enable( level );
 
diff --git a/cpukit/score/src/coremutexsurrender.c b/cpukit/score/src/coremutexsurrender.c
index e6f52e229d..839c0af5bf 100644
--- a/cpukit/score/src/coremutexsurrender.c
+++ b/cpukit/score/src/coremutexsurrender.c
@@ -6,7 +6,7 @@
  *  This package is the implementation of the Mutex Handler.
  *  This handler provides synchronization and mutual exclusion capabilities.
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -90,6 +90,10 @@ CORE_mutex_Status _CORE_mutex_Surrender(
     }
   }
 
+  /*
+   *  Formally release the mutex before possibly transferring it to a
+   *  blocked thread.
+   */
   if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ||
        _CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) )
     holder->resource_count--;
@@ -101,7 +105,6 @@ CORE_mutex_Status _CORE_mutex_Surrender(
    *  inherited priority must be lowered if this is the last
    *  mutex (i.e. resource) this task has.
    */
-
   if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ||
        _CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) ) {
     if ( holder->resource_count == 0 &&
@@ -110,6 +113,10 @@ CORE_mutex_Status _CORE_mutex_Surrender(
     }
   }
 
+  /*
+   *  Now we check if another thread was waiting for this mutex.  If so,
+   *  transfer the mutex to that thread.
+   */
   if ( ( the_thread = _Thread_queue_Dequeue( &the_mutex->Wait_queue ) ) ) {
 
 #if defined(RTEMS_MULTIPROCESSING)
diff --git a/cpukit/score/src/threadchangepriority.c b/cpukit/score/src/threadchangepriority.c
index 89d0d2cd5f..9847f54f80 100644
--- a/cpukit/score/src/threadchangepriority.c
+++ b/cpukit/score/src/threadchangepriority.c
@@ -2,7 +2,7 @@
  *  Thread Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -55,8 +55,8 @@ void _Thread_Change_priority(
   boolean           prepend_it
 )
 {
-  ISR_Level level;
-  /* boolean   do_prepend = FALSE; */
+  ISR_Level      level;
+  States_Control state;
 
   /*
    *  If this is a case where prepending the task to its priority is
@@ -66,13 +66,6 @@ void _Thread_Change_priority(
    *  change calls (e.g. rtems_task_set_priority) should always do an
    *  append not a prepend.
    */
-
-  /*
-   *  Techically, the prepend should conditional on the thread lowering
-   *  its priority but that does allow cxd2004 of the acvc 2.0.1 to
-   *  pass with rtems 4.0.0.  This should change when gnat redoes its
-   *  priority scheme.
-   */
 /*
   if ( prepend_it &&
        _Thread_Is_executing( the_thread ) &&
@@ -80,26 +73,46 @@ void _Thread_Change_priority(
     prepend_it = TRUE;
 */
 
+  /*
+   * Set a transient state for the thread so it is pulled off the Ready chains.
+   * This will prevent it from being scheduled no matter what happens in an
+   * ISR.
+   */
   _Thread_Set_transient( the_thread );
 
-  if ( the_thread->current_priority != new_priority )
+  /*
+   *  Do not bother recomputing all the priority related information if
+   *  we are not REALLY changing priority.
+   */
+ if ( the_thread->current_priority != new_priority )
     _Thread_Set_priority( the_thread, new_priority );
 
   _ISR_Disable( level );
 
-  the_thread->current_state =
-    _States_Clear( STATES_TRANSIENT, the_thread->current_state );
-
-  if ( ! _States_Is_ready( the_thread->current_state ) ) {
-    /*
-     *  XXX If a task is to be reordered while blocked on a priority
-     *  XXX priority ordered thread queue, then this is where that
-     *  XXX should occur.
-     */
+  /*
+   *  If the thread has more than STATES_TRANSIENT set, then it is blocked,
+   *  If it is blocked on a thread queue, then we need to requeue it.
+   */
+  state = the_thread->current_state;
+  if ( state != STATES_TRANSIENT ) {
+    if ( _States_Is_waiting_on_thread_queue( state ) ) {
+       _ISR_Enable( level ); 
+      _Thread_queue_Requeue( the_thread->Wait.queue, the_thread );
+      _ISR_Disable( level );  /* redisable so state is cleared with ISR off */
+    }
+    the_thread->current_state = _States_Clear( STATES_TRANSIENT, state );
     _ISR_Enable( level );
     return;
   }
 
+  /*
+   *  Interrupts are STILL disabled.
+   *  We now know the thread will be in the READY state when we remove
+   *  the TRANSIENT state.  So we have to place it on the appropriate
+   *  Ready Queue with interrupts off.
+   */
+  the_thread->current_state = _States_Clear( STATES_TRANSIENT, state );
+
   _Priority_Add_to_bit_map( &the_thread->Priority_map );
   if ( prepend_it )
     _Chain_Prepend_unprotected( the_thread->ready, &the_thread->Object.Node );
@@ -108,6 +121,10 @@ void _Thread_Change_priority(
 
   _ISR_Flash( level );
 
+  /*
+   *  We altered the set of thread priorities.  So let's figure out
+   *  who is the heir and if we need to switch to them.
+   */
   _Thread_Calculate_heir();
 
   if ( !_Thread_Is_executing_also_the_heir() &&
diff --git a/cpukit/score/src/threadclearstate.c b/cpukit/score/src/threadclearstate.c
index bb7deffe06..8a175fd8cd 100644
--- a/cpukit/score/src/threadclearstate.c
+++ b/cpukit/score/src/threadclearstate.c
@@ -73,6 +73,12 @@ void _Thread_Clear_state(
 
         _ISR_Flash( level );
 
+        /*
+         *  If the thread that was unblocked is more important than the heir,
+         *  then we have a new heir.  In addition, if the current thread
+         *  is preemptible or we are waking up one of the "pseudo-ISR" system
+         *  threads, then we need to do a context switch.
+         */
         if ( the_thread->current_priority < _Thread_Heir->current_priority ) {
           _Thread_Heir = the_thread;
           if ( _Thread_Executing->is_preemptible ||
diff --git a/cpukit/score/src/threadhandler.c b/cpukit/score/src/threadhandler.c
index c6cf23e660..b7708e2b03 100644
--- a/cpukit/score/src/threadhandler.c
+++ b/cpukit/score/src/threadhandler.c
@@ -98,7 +98,6 @@ void _Thread_Handler( void )
 #endif
 #endif
 
-
   /*
    * Take care that 'begin' extensions get to complete before
    * 'switch' extensions can run.  This means must keep dispatch
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index 0ed4cdfe69..40e786f954 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -2,7 +2,7 @@
  *  Thread Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -190,6 +190,7 @@ boolean _Thread_Initialize(
   the_thread->Start.isr_level        = isr_level;
 
   the_thread->current_state          = STATES_DORMANT;
+  the_thread->Wait.queue             = NULL;
   the_thread->resource_count         = 0;
   the_thread->suspend_count          = 0;
   the_thread->real_priority          = priority;
diff --git a/cpukit/score/src/threadqdequeuefifo.c b/cpukit/score/src/threadqdequeuefifo.c
index ff76bff3d9..787ec41074 100644
--- a/cpukit/score/src/threadqdequeuefifo.c
+++ b/cpukit/score/src/threadqdequeuefifo.c
@@ -2,7 +2,7 @@
  *  Thread Queue Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -55,6 +55,7 @@ Thread_Control *_Thread_queue_Dequeue_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 );
diff --git a/cpukit/score/src/threadqdequeuepriority.c b/cpukit/score/src/threadqdequeuepriority.c
index 1b02a71158..4da18165d9 100644
--- a/cpukit/score/src/threadqdequeuepriority.c
+++ b/cpukit/score/src/threadqdequeuepriority.c
@@ -2,7 +2,7 @@
  *  Thread Queue Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -81,6 +81,7 @@ Thread_Control *_Thread_queue_Dequeue_priority(
   }
 
 dequeue:
+  the_thread->Wait.queue = NULL;
   new_first_node   = the_thread->Wait.Block2n.first;
   new_first_thread = (Thread_Control *) new_first_node;
   next_node        = the_thread->Object.Node.next;
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index 3cbf76800b..2cc1141993 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -2,7 +2,7 @@
  *  Thread Queue Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -60,8 +60,14 @@ void _Thread_queue_Enqueue_with_handler(
     the_thread = _Thread_MP_Allocate_proxy( the_thread_queue->state );
   else
 #endif
-    _Thread_Set_state( the_thread, the_thread_queue->state );
+  /*
+   *  Set the blocking state for this thread queue in the thread.
+   */
+  _Thread_Set_state( the_thread, the_thread_queue->state );
 
+  /*
+   *  If the thread wants to timeout, then schedule its timer.
+   */ 
   if ( timeout ) {
     _Watchdog_Initialize(
        &the_thread->Timer,
@@ -73,6 +79,9 @@ void _Thread_queue_Enqueue_with_handler(
     _Watchdog_Insert_ticks( &the_thread->Timer, timeout );
   }
 
+  /*
+   *  Now enqueue the thread per the discipline for this thread queue.
+   */ 
   switch( the_thread_queue->discipline ) {
     case THREAD_QUEUE_DISCIPLINE_FIFO:
       _Thread_queue_Enqueue_fifo( the_thread_queue, the_thread );
diff --git a/cpukit/score/src/threadqenqueuefifo.c b/cpukit/score/src/threadqenqueuefifo.c
index b4cba6a532..c7260807ed 100644
--- a/cpukit/score/src/threadqenqueuefifo.c
+++ b/cpukit/score/src/threadqenqueuefifo.c
@@ -67,17 +67,20 @@ void _Thread_queue_Enqueue_fifo (
         &the_thread_queue->Queues.Fifo,
         &the_thread->Object.Node
       );
+      the_thread->Wait.queue = the_thread_queue;
       _ISR_Enable( level );
       return;
 
     case THREAD_QUEUE_TIMEOUT:
       the_thread->Wait.return_code = the_thread->Wait.queue->timeout_status;
+      the_thread->Wait.queue = NULL;
       _ISR_Enable( level );
       break;
 
     case THREAD_QUEUE_SATISFIED:
       if ( _Watchdog_Is_active( &the_thread->Timer ) ) {
         _Watchdog_Deactivate( &the_thread->Timer );
+      the_thread->Wait.queue = NULL;
         _ISR_Enable( level );
         (void) _Watchdog_Remove( &the_thread->Timer );
       } else
diff --git a/cpukit/score/src/threadqenqueuepriority.c b/cpukit/score/src/threadqenqueuepriority.c
index 5c49a4c6d8..9974a3a674 100644
--- a/cpukit/score/src/threadqenqueuepriority.c
+++ b/cpukit/score/src/threadqenqueuepriority.c
@@ -108,10 +108,11 @@ restart_forward_search:
   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_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;
 
@@ -153,10 +154,11 @@ restart_reverse_search:
   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_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;
 
@@ -165,10 +167,11 @@ equal_priority:               /* add at end of priority group */
   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_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;
 
@@ -193,12 +196,14 @@ synchronize:
 
     case THREAD_QUEUE_TIMEOUT:
       the_thread->Wait.return_code = the_thread->Wait.queue->timeout_status;
+      the_thread->Wait.queue = NULL;
       _ISR_Enable( level );
       break;
 
     case THREAD_QUEUE_SATISFIED:
       if ( _Watchdog_Is_active( &the_thread->Timer ) ) {
         _Watchdog_Deactivate( &the_thread->Timer );
+        the_thread->Wait.queue = NULL;
         _ISR_Enable( level );
         (void) _Watchdog_Remove( &the_thread->Timer );
       } else
diff --git a/cpukit/score/src/threadqextractfifo.c b/cpukit/score/src/threadqextractfifo.c
index 145bf314a3..f7ecf3e270 100644
--- a/cpukit/score/src/threadqextractfifo.c
+++ b/cpukit/score/src/threadqextractfifo.c
@@ -2,7 +2,7 @@
  *  Thread Queue Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -58,6 +58,8 @@ void _Thread_queue_Extract_fifo(
 
   _Chain_Extract_unprotected( &the_thread->Object.Node );
 
+  the_thread->Wait.queue = NULL;
+
   if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
     _ISR_Enable( level );
   } else {
diff --git a/cpukit/score/src/threadqextractpriority.c b/cpukit/score/src/threadqextractpriority.c
index dfadc25425..ef45fb4423 100644
--- a/cpukit/score/src/threadqextractpriority.c
+++ b/cpukit/score/src/threadqextractpriority.c
@@ -2,7 +2,7 @@
  *  Thread Queue Handler
  *
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2006.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -35,6 +35,7 @@
  *  Input parameters:
  *    the_thread_queue - pointer to a threadq header
  *    the_thread       - pointer to a thread control block
+ *    requeuing        - TRUE if requeuing and should not alter timeout or state
  *
  *  Output parameters: NONE
  *
@@ -42,12 +43,13 @@
  *    EXTRACT_PRIORITY
  */
 
-void _Thread_queue_Extract_priority(
+void _Thread_queue_Extract_priority_helper(
   Thread_queue_Control *the_thread_queue,
-  Thread_Control       *the_thread
+  Thread_Control       *the_thread,
+  boolean               requeuing
 )
 {
-  ISR_Level              level;
+  ISR_Level       level;
   Chain_Node     *the_node;
   Chain_Node     *next_node;
   Chain_Node     *previous_node;
@@ -58,50 +60,63 @@ void _Thread_queue_Extract_priority(
 
   the_node = (Chain_Node *) the_thread;
   _ISR_Disable( level );
-  if ( _States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
-    next_node     = the_node->next;
-    previous_node = the_node->previous;
+  if ( !_States_Is_waiting_on_thread_queue( the_thread->current_state ) ) {
+    _ISR_Enable( level );
+    return;
+  }
 
-    if ( !_Chain_Is_empty( &the_thread->Wait.Block2n ) ) {
-      new_first_node   = the_thread->Wait.Block2n.first;
-      new_first_thread = (Thread_Control *) new_first_node;
-      last_node        = the_thread->Wait.Block2n.last;
-      new_second_node  = new_first_node->next;
+  /*
+   *  The thread was actually waiting on a thread queue so let's remove it.
+   */
 
-      previous_node->next      = new_first_node;
-      next_node->previous      = new_first_node;
-      new_first_node->next     = next_node;
-      new_first_node->previous = previous_node;
+  next_node     = the_node->next;
+  previous_node = the_node->previous;
 
-      if ( !_Chain_Has_only_one_node( &the_thread->Wait.Block2n ) ) {
-                                          /* > two threads on 2-n */
-        new_second_node->previous =
-                  _Chain_Head( &new_first_thread->Wait.Block2n );
-        new_first_thread->Wait.Block2n.first = new_second_node;
+  if ( !_Chain_Is_empty( &the_thread->Wait.Block2n ) ) {
+    new_first_node   = the_thread->Wait.Block2n.first;
+    new_first_thread = (Thread_Control *) new_first_node;
+    last_node        = the_thread->Wait.Block2n.last;
+    new_second_node  = new_first_node->next;
 
-        new_first_thread->Wait.Block2n.last = last_node;
-        last_node->next = _Chain_Tail( &new_first_thread->Wait.Block2n );
-      }
-    } else {
-      previous_node->next = next_node;
-      next_node->previous = previous_node;
-    }
+    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 */
+      new_second_node->previous =
+                _Chain_Head( &new_first_thread->Wait.Block2n );
+      new_first_thread->Wait.Block2n.first = new_second_node;
 
-    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 );
+      new_first_thread->Wait.Block2n.last = last_node;
+      last_node->next = _Chain_Tail( &new_first_thread->Wait.Block2n );
     }
+  } else {
+    previous_node->next = next_node;
+    next_node->previous = previous_node;
+  }
 
-#if defined(RTEMS_MULTIPROCESSING)
-    if ( !_Objects_Is_local_id( the_thread->Object.id ) )
-      _Thread_MP_Free_proxy( the_thread );
-#endif
+  /*
+   *  If we are not supposed to touch timers or the thread's state, return.
+   */
+
+  if ( requeuing ) {
+    _ISR_Enable( level );
+    return;
   }
-  else
+
+  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/threadqrequeue.c b/cpukit/score/src/threadqrequeue.c
new file mode 100644
index 0000000000..96434639ef
--- /dev/null
+++ b/cpukit/score/src/threadqrequeue.c
@@ -0,0 +1,63 @@
+/*
+ *  Thread Queue Handler
+ *
+ *
+ *  COPYRIGHT (c) 1989-2006.
+ *  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.OARcorp.com/rtems/license.html.
+ *
+ *  $Id$
+ */
+
+#include <rtems/system.h>
+#include <rtems/score/chain.h>
+#include <rtems/score/isr.h>
+#include <rtems/score/object.h>
+#include <rtems/score/states.h>
+#include <rtems/score/thread.h>
+#include <rtems/score/threadq.h>
+#include <rtems/score/tqdata.h>
+
+/*PAGE
+ *
+ *  _Thread_queue_Requeue
+ *
+ *  This routine is invoked when a thread changes priority and should be
+ *  moved to a different position on the thread queue.
+ *
+ *  Input parameters:
+ *    the_thread_queue - pointer to a threadq header
+ *    the_thread       - pointer to a thread control block
+ *
+ *  Output parameters: NONE
+ *
+ *  INTERRUPT LATENCY: NONE
+ */
+
+void _Thread_queue_Requeue(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread
+)
+{
+  /* just in case the thread really wasn't blocked here */
+  if ( !the_thread_queue )
+    return;
+
+  switch ( the_thread_queue->discipline ) {
+    case THREAD_QUEUE_DISCIPLINE_FIFO:
+      /* queueing by FIFO -- nothing to do */
+      break;
+    case THREAD_QUEUE_DISCIPLINE_PRIORITY: {
+      Thread_queue_Control *tq = the_thread_queue;
+
+      _Thread_queue_Enter_critical_section( tq );
+        _Thread_queue_Extract_priority_helper( tq, the_thread, TRUE );
+        _Thread_queue_Enqueue_priority( tq, the_thread );
+      }
+      break;
+   }
+}
+
diff --git a/cpukit/score/src/threadsetstate.c b/cpukit/score/src/threadsetstate.c
index f1cc469f1c..9538ffaa27 100644
--- a/cpukit/score/src/threadsetstate.c
+++ b/cpukit/score/src/threadsetstate.c
@@ -50,10 +50,10 @@
 
 void _Thread_Set_state(
   Thread_Control *the_thread,
-  States_Control         state
+  States_Control  state
 )
 {
-  ISR_Level             level;
+  ISR_Level      level;
   Chain_Control *ready;
 
   ready = the_thread->ready;