8 files changed, 208 insertions, 11 deletions

diff --git a/cpukit/score/src/coremutexseize.c b/cpukit/score/src/coremutexseize.c
index c19d868cf0..dace2137c8 100644
--- a/cpukit/score/src/coremutexseize.c
+++ b/cpukit/score/src/coremutexseize.c
@@ -61,13 +61,8 @@ void _CORE_mutex_Seize_interrupt_blocking(
 
   executing = _Thread_Executing;
   if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ) {
-    if ( the_mutex->holder->Priority_node.current_priority > executing->Priority_node.current_priority ) {
-      _Thread_Change_priority(
-        the_mutex->holder,
-        executing->Priority_node.current_priority,
-        false
-      );
-    }
+    _Thread_Enqueue_priority_node( executing, the_mutex );
+    _Thread_Evaluate_priority( the_mutex->holder );
   }
 
   the_mutex->blocked_count++;
diff --git a/cpukit/score/src/coremutexsurrender.c b/cpukit/score/src/coremutexsurrender.c
index 2ed2afebb7..56d504531e 100644
--- a/cpukit/score/src/coremutexsurrender.c
+++ b/cpukit/score/src/coremutexsurrender.c
@@ -124,6 +124,9 @@ CORE_mutex_Status _CORE_mutex_Surrender(
 #endif
     holder->resource_count--;
   }
+  if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ) {
+    _Thread_Release_inherited_priority( the_mutex );
+  }
   the_mutex->holder    = NULL;
   the_mutex->holder_id = 0;
 
@@ -132,8 +135,7 @@ 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 ( _CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) ) {
 #ifdef __RTEMS_STRICT_ORDER_MUTEX__
     if(the_mutex->queue.priority_before != holder->Priority_node.current_priority)
       _Thread_Change_priority(holder,the_mutex->queue.priority_before,true);
diff --git a/cpukit/score/src/threadchangepriority.c b/cpukit/score/src/threadchangepriority.c
index d109dd9ec6..c3dc3ffa80 100644
--- a/cpukit/score/src/threadchangepriority.c
+++ b/cpukit/score/src/threadchangepriority.c
@@ -88,8 +88,10 @@ void _Thread_Change_priority(
    *  Do not bother recomputing all the priority related information if
    *  we are not REALLY changing priority.
    */
- if ( the_thread->Priority_node.current_priority != new_priority )
+  if ( the_thread->Priority_node.current_priority != new_priority ) {
     _Thread_Set_priority( the_thread, new_priority );
+    _Thread_Requeue_priority_node( the_thread );
+  }
 
   _ISR_Disable( level );
 
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index a02c72958b..3fda5eaf3d 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -192,7 +192,11 @@ bool _Thread_Initialize(
   #if defined(RTEMS_ITRON_API)
     the_thread->suspend_count         = 0;
   #endif
-  the_thread->Priority_node.real_priority           = priority;
+  _Chain_Set_off_chain( &the_thread->Priority_node.Node );
+  the_thread->Priority_node.real_priority    = priority;
+  the_thread->Priority_node.current_priority = priority;
+  the_thread->Priority_node.waiting_to_hold  = NULL;
+  _Chain_Initialize_empty( &the_thread->Priority_node.Inherited_priorities );
   the_thread->Start.initial_priority  = priority;
   _Thread_Set_priority( the_thread, priority );
 
diff --git a/cpukit/score/src/threadprioritynode.c b/cpukit/score/src/threadprioritynode.c
new file mode 100644
index 0000000000..69bbe14972
--- /dev/null
+++ b/cpukit/score/src/threadprioritynode.c
@@ -0,0 +1,167 @@
+/*
+ * Thread Priority Node
+ * 
+ * Copyright (C) 2017. Gedare Bloom.
+ *
+ * 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/system.h>
+#include <rtems/config.h>
+#include <rtems/score/chain.h>
+#include <rtems/score/coremutex.h>
+#include <rtems/score/thread.h>
+
+/**
+ * This routine adds a thread's Priority_node to the mutex holder's
+ * Inherited_priorities list, and sets the waiting_to_hold field to the
+ * mutex.
+ *
+ * Returns true if the enqueued node is at the head of the holder's
+ * Inherited priorities, and therefore the holder's priority may change.
+ */
+bool _Thread_Enqueue_priority_node(
+  Thread_Control     *the_thread,
+  CORE_mutex_Control *the_mutex
+)
+{
+  Chain_Node *iter;
+  Thread_Priority_node *thread_pn, *holder_pn;
+  Priority_Control task_priority, iter_priority;
+  
+  thread_pn = &the_thread->Priority_node;
+  holder_pn = &the_mutex->holder->Priority_node;
+
+  thread_pn->waiting_to_hold = the_mutex;
+  iter = holder_pn->Inherited_priorities.first;
+  task_priority = thread_pn->current_priority;
+
+  /* Find the first node in holder's Inherited_priorities list with
+   * lower priority than the task, or find the end of the list. */
+  while ( !_Chain_Is_tail( &holder_pn->Inherited_priorities, iter) ) {
+    iter_priority = ((Thread_Priority_node *)iter)->current_priority;
+    if ( iter_priority > task_priority ) {
+      break;
+    }
+    iter = _Chain_Next( iter );
+  }
+  _Chain_Insert_unprotected( _Chain_Previous( iter ), &thread_pn->Node );
+
+  /* If the_thread is at the start of the holder's Inherited_priorites list,
+   * then may need to update the holder's current priority. */
+  if ( _Chain_Is_first( &thread_pn->Node ) ) {
+    return true;
+  }
+  return false;
+}
+
+/**
+ * This routine sets the thread's priority_node->current_priority to the
+ * minimum of the real_priority and the first node of Inherited_priorities,
+ * if any.
+ */
+void _Thread_Evaluate_priority(
+  Thread_Control *the_thread
+)
+{
+  Chain_Node *first;
+  Thread_Priority_node *first_pn, *priority_node;
+  Priority_Control current_priority, min_priority;
+
+  /* Start with the_thread, and iterate through the holder threads that
+   * the_thread is waiting on. If any thread's priority does not change,
+   * then stop iterating. */
+  while ( the_thread != NULL ) {
+    priority_node = &the_thread->Priority_node;
+    current_priority = priority_node->current_priority;
+    min_priority = priority_node->real_priority;
+    first = _Chain_First( &priority_node->Inherited_priorities );
+    if ( !_Chain_Is_tail( &priority_node->Inherited_priorities, first ) ) {
+      first_pn = (Thread_Priority_node *)first;
+      if ( min_priority > first_pn->current_priority ) {
+        min_priority = first_pn->current_priority;
+      }
+    }
+    if ( current_priority < min_priority ) {
+      _Thread_Change_priority( the_thread, min_priority, true );
+    } else if ( current_priority > min_priority ) {
+      _Thread_Change_priority( the_thread, min_priority, false );
+    } else {
+      break;
+    }
+    if ( the_thread->Priority_node.waiting_to_hold != NULL ) {
+      the_thread = the_thread->Priority_node.waiting_to_hold->holder;
+    } else {
+      the_thread = NULL;
+    }
+  }
+}
+
+/**
+ * This routine removes a thread's Priority_node from an
+ * Inherited_priorities list, updates the mutex holder's
+ * current_priority if needed, and clears the mutex field.
+ *
+ * Returns the mutex that the_thread was waiting on.
+ */
+CORE_mutex_Control* _Thread_Dequeue_priority_node(
+  Thread_Priority_node *thread_pn
+)
+{
+  CORE_mutex_Control *the_mutex = thread_pn->waiting_to_hold;
+
+  if ( the_mutex == NULL ) {
+    return NULL;
+  }
+
+  _Chain_Extract_unprotected( &thread_pn->Node );
+  thread_pn->waiting_to_hold = NULL;
+
+  return the_mutex;
+}
+
+void _Thread_Requeue_priority_node(
+  Thread_Control *the_thread
+)
+{
+  ISR_Level       level;
+  CORE_mutex_Control *mutex;
+  
+  _ISR_Disable( level );
+  /* TODO: refactor? */
+  mutex = _Thread_Dequeue_priority_node( &the_thread->Priority_node );
+  if ( mutex != NULL ) {
+    _Thread_Enqueue_priority_node( the_thread, mutex );
+  }
+  _ISR_Enable( level );
+}
+
+/**
+ * This routine removes the inherited priorities from the mutex being
+ * released and updates the holder's priority if necessary.
+ */
+void _Thread_Release_inherited_priority(
+  CORE_mutex_Control *the_mutex
+)
+{
+  Chain_Control *ip = &the_mutex->holder->Priority_node.Inherited_priorities;
+  Chain_Node *cn;
+  Thread_Priority_node *tpn;
+
+  for (cn = _Chain_First(ip); !_Chain_Is_tail(ip, cn); cn = _Chain_Next(cn)) {
+    tpn = (Thread_Priority_node*)cn;
+    if ( tpn->waiting_to_hold == the_mutex ) {
+      _Chain_Extract(cn);
+      tpn->waiting_to_hold = NULL;
+    }
+  }
+
+  _Thread_Evaluate_priority( the_mutex->holder );
+}
+
diff --git a/cpukit/score/src/threadqextractpriority.c b/cpukit/score/src/threadqextractpriority.c
index f314f75da9..5c8188d661 100644
--- a/cpukit/score/src/threadqextractpriority.c
+++ b/cpukit/score/src/threadqextractpriority.c
@@ -18,6 +18,7 @@
 
 #include <rtems/system.h>
 #include <rtems/score/chain.h>
+#include <rtems/score/coremutex.h>
 #include <rtems/score/isr.h>
 #include <rtems/score/object.h>
 #include <rtems/score/states.h>
@@ -57,6 +58,7 @@ bool _Thread_queue_Extract_priority_helper(
   Chain_Node     *new_first_node;
   Chain_Node     *new_second_node;
   Chain_Node     *last_node;
+  CORE_mutex_Control *mutex;
 
   the_node = (Chain_Node *) the_thread;
   _ISR_Disable( level );
@@ -106,6 +108,9 @@ bool _Thread_queue_Extract_priority_helper(
     return true;
   }
 
+  mutex = _Thread_Dequeue_priority_node( &the_thread->Priority_node );
+  _Thread_Evaluate_priority( mutex->holder );
+
   if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
     _ISR_Enable( level );
   } else {
diff --git a/cpukit/score/src/threadqflush.c b/cpukit/score/src/threadqflush.c
index b777aa3fa1..47bff8834d 100644
--- a/cpukit/score/src/threadqflush.c
+++ b/cpukit/score/src/threadqflush.c
@@ -18,6 +18,7 @@
 
 #include <rtems/system.h>
 #include <rtems/score/chain.h>
+#include <rtems/score/coremutex.h>
 #include <rtems/score/isr.h>
 #include <rtems/score/object.h>
 #include <rtems/score/states.h>
@@ -50,6 +51,7 @@ void _Thread_queue_Flush(
 )
 {
   Thread_Control *the_thread;
+  CORE_mutex_Control *mutex;
 
   while ( (the_thread = _Thread_queue_Dequeue( the_thread_queue )) ) {
 #if defined(RTEMS_MULTIPROCESSING)
@@ -58,5 +60,9 @@ void _Thread_queue_Flush(
     else
 #endif
       the_thread->Wait.return_code = status;
+    if ( the_thread->Priority_node.waiting_to_hold != NULL ) {
+      mutex = _Thread_Dequeue_priority_node( &the_thread->Priority_node );
+      _Thread_Evaluate_priority( mutex->holder );
+    }
   }
 }
diff --git a/cpukit/score/src/threadreset.c b/cpukit/score/src/threadreset.c
index fd55ca320f..464a611391 100644
--- a/cpukit/score/src/threadreset.c
+++ b/cpukit/score/src/threadreset.c
@@ -19,6 +19,7 @@
 #include <rtems/system.h>
 #include <rtems/score/apiext.h>
 #include <rtems/score/context.h>
+#include <rtems/score/coremutex.h>
 #include <rtems/score/interr.h>
 #include <rtems/score/isr.h>
 #include <rtems/score/object.h>
@@ -46,6 +47,8 @@ void _Thread_Reset(
   Thread_Entry_numeric_type  numeric_argument
 )
 {
+  CORE_mutex_Control *mutex;
+
   the_thread->resource_count   = 0;
   #if defined(RTEMS_ITRON_API)
     the_thread->suspend_count  = 0;
@@ -63,6 +66,19 @@ void _Thread_Reset(
       (void) _Watchdog_Remove( &the_thread->Timer );
   }
 
+  if ( the_thread->Priority_node.waiting_to_hold != NULL ) {
+    mutex = _Thread_Dequeue_priority_node( &the_thread->Priority_node );
+    _Thread_Evaluate_priority( mutex->holder );
+  }
+
+  while ( !_Chain_Is_empty( &the_thread->Priority_node.Inherited_priorities ) ) {
+    _Thread_Dequeue_priority_node(
+      ((Thread_Priority_node*)_Chain_First(
+        &the_thread->Priority_node.Inherited_priorities
+      ))
+    );
+  }
+
   if ( the_thread->Priority_node.current_priority != the_thread->Start.initial_priority ) {
     the_thread->Priority_node.real_priority = the_thread->Start.initial_priority;
     _Thread_Set_priority( the_thread, the_thread->Start.initial_priority );