score: Priority inherit thread queue operations

Move the priority change due to priority interitance to the thread queue
enqueue operation to simplify the locking on SMP configurations.

Update #2412.
Update #2556.
Update #2765.

9 files changed, 116 insertions, 77 deletions

diff --git a/cpukit/score/include/rtems/score/coremuteximpl.h b/cpukit/score/include/rtems/score/coremuteximpl.h
index 339834bc37..f48524a7b6 100644
--- a/cpukit/score/include/rtems/score/coremuteximpl.h
+++ b/cpukit/score/include/rtems/score/coremuteximpl.h
@@ -36,6 +36,9 @@ extern "C" {
 
 #define CORE_MUTEX_TQ_OPERATIONS &_Thread_queue_Operations_priority
 
+#define CORE_MUTEX_TQ_PRIORITY_INHERIT_OPERATIONS \
+  &_Thread_queue_Operations_priority_inherit
+
 RTEMS_INLINE_ROUTINE void _CORE_mutex_Initialize(
   CORE_mutex_Control *the_mutex
 )
diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index 5323e2c85b..92968a2e8e 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -512,37 +512,6 @@ void _Thread_Raise_priority(
 );
 
 /**
- * @brief Inherit the priority of a thread.
- *
- * It changes the current priority of the inheritor thread to the current priority
- * of the ancestor thread if it is higher than the current priority of the inheritor
- * thread.  In this case the inheritor thread is appended to its new priority group
- * in its scheduler instance.
- *
- * On SMP configurations, the priority is changed to PRIORITY_PSEUDO_ISR in
- * case the own schedulers of the inheritor and ancestor thread differ (priority
- * boosting).
- *
- * @param[in] inheritor The thread to inherit the priority.
- * @param[in] ancestor The thread to bequeath its priority to the inheritor
- *   thread.
- */
-#if defined(RTEMS_SMP)
-void _Thread_Inherit_priority(
-  Thread_Control *inheritor,
-  Thread_Control *ancestor
-);
-#else
-RTEMS_INLINE_ROUTINE void _Thread_Inherit_priority(
-  Thread_Control *inheritor,
-  Thread_Control *ancestor
-)
-{
-  _Thread_Raise_priority( inheritor, ancestor->current_priority );
-}
-#endif
-
-/**
  * @brief Sets the current to the real priority of a thread.
  *
  * Sets the priority restore hint to false.
diff --git a/cpukit/score/include/rtems/score/threadq.h b/cpukit/score/include/rtems/score/threadq.h
index b3b8fec980..75aab20993 100644
--- a/cpukit/score/include/rtems/score/threadq.h
+++ b/cpukit/score/include/rtems/score/threadq.h
@@ -43,6 +43,8 @@ extern "C" {
 
 typedef struct _Thread_Control Thread_Control;
 
+typedef struct Thread_queue_Path Thread_queue_Path;
+
 #if defined(RTEMS_MULTIPROCESSING)
 /**
  * @brief Multiprocessing (MP) support callout for thread queue operations.
@@ -239,6 +241,10 @@ typedef void ( *Thread_queue_Priority_change_operation )(
 /**
  * @brief Thread queue enqueue operation.
  *
+ * A potential thread to update the priority due to priority inheritance is
+ * returned via the thread queue path.  This thread is handed over to
+ * _Thread_Update_priority().
+ *
  * @param[in] queue The actual thread queue.
  * @param[in] the_thread The thread to enqueue on the queue.
  *
@@ -246,7 +252,8 @@ typedef void ( *Thread_queue_Priority_change_operation )(
  */
 typedef void ( *Thread_queue_Enqueue_operation )(
   Thread_queue_Queue *queue,
-  Thread_Control     *the_thread
+  Thread_Control     *the_thread,
+  Thread_queue_Path  *path
 );
 
 /**
diff --git a/cpukit/score/include/rtems/score/threadqimpl.h b/cpukit/score/include/rtems/score/threadqimpl.h
index e7db7bb6c0..1f17f1a4f1 100644
--- a/cpukit/score/include/rtems/score/threadqimpl.h
+++ b/cpukit/score/include/rtems/score/threadqimpl.h
@@ -36,6 +36,27 @@ extern "C" {
 /**@{*/
 
 /**
+ * @brief Representation of a thread queue path from a start thread queue to
+ * the terminal thread queue.
+ *
+ * The start thread queue is determined by the object on which a thread intends
+ * to block.  The terminal thread queue is the thread queue reachable via
+ * thread queue links those owner is not blocked on a thread queue.  The thread
+ * queue links are determined by the thread queue owner and thread wait queue
+ * relationships.
+ */
+struct Thread_queue_Path {
+  /**
+   * @brief A potential thread to update the priority via
+   * _Thread_Update_priority().
+   *
+   * This thread is determined by thread queues which support priority
+   * inheritance.
+   */
+  Thread_Control *update_priority;
+};
+
+/**
  * @brief Thread queue with a layout compatible to struct _Thread_queue_Queue
  * defined in Newlib <sys/lock.h>.
  */
@@ -888,6 +909,8 @@ extern const Thread_queue_Operations _Thread_queue_Operations_FIFO;
 
 extern const Thread_queue_Operations _Thread_queue_Operations_priority;
 
+extern const Thread_queue_Operations _Thread_queue_Operations_priority_inherit;
+
 /**@}*/
 
 #ifdef __cplusplus
diff --git a/cpukit/score/src/coremutexseize.c b/cpukit/score/src/coremutexseize.c
index c468c07a7c..dacb274bdf 100644
--- a/cpukit/score/src/coremutexseize.c
+++ b/cpukit/score/src/coremutexseize.c
@@ -54,8 +54,6 @@ Status_Control _CORE_mutex_Seize_slow(
   _CORE_mutex_Release( the_mutex, queue_context );
 #endif
 
-  _Thread_Inherit_priority( owner, executing );
-
 #if defined(RTEMS_SMP)
   _Thread_queue_Context_set_expected_level( queue_context, 1 );
 #else
@@ -66,7 +64,7 @@ Status_Control _CORE_mutex_Seize_slow(
 
   _Thread_queue_Enqueue_critical(
     &the_mutex->Wait_queue.Queue,
-    CORE_MUTEX_TQ_OPERATIONS,
+    CORE_MUTEX_TQ_PRIORITY_INHERIT_OPERATIONS,
     executing,
     STATES_WAITING_FOR_MUTEX,
     queue_context
diff --git a/cpukit/score/src/mutex.c b/cpukit/score/src/mutex.c
index 4b95262f18..12a4f292e1 100644
--- a/cpukit/score/src/mutex.c
+++ b/cpukit/score/src/mutex.c
@@ -26,7 +26,7 @@
 #include <rtems/score/threadqimpl.h>
 #include <rtems/score/todimpl.h>
 
-#define MUTEX_TQ_OPERATIONS &_Thread_queue_Operations_priority
+#define MUTEX_TQ_OPERATIONS &_Thread_queue_Operations_priority_inherit
 
 typedef struct {
   Thread_queue_Syslock_queue Queue;
@@ -107,7 +107,6 @@ static void _Mutex_Acquire_slow(
   Thread_queue_Context *queue_context
 )
 {
-  _Thread_Inherit_priority( owner, executing );
   _Thread_queue_Context_set_expected_level( queue_context, 1 );
   _Thread_queue_Enqueue_critical(
     &mutex->Queue.Queue,
diff --git a/cpukit/score/src/threadchangepriority.c b/cpukit/score/src/threadchangepriority.c
index d895ee6dbe..e3cf4a165a 100644
--- a/cpukit/score/src/threadchangepriority.c
+++ b/cpukit/score/src/threadchangepriority.c
@@ -142,40 +142,6 @@ void _Thread_Raise_priority(
   );
 }
 
-#if defined(RTEMS_SMP)
-static bool _Thread_Inherit_priority_filter(
-  Thread_Control   *inheritor,
-  Priority_Control *new_priority,
-  void             *arg
-)
-{
-  Thread_Control *ancestor = arg;
-
-  if ( _Scheduler_Get_own( inheritor ) == _Scheduler_Get_own( ancestor ) ) {
-    *new_priority = ancestor->current_priority;
-  }
-
-  return _Thread_Priority_less_than(
-    inheritor->current_priority,
-    *new_priority
-  );
-}
-
-void _Thread_Inherit_priority(
-  Thread_Control *inheritor,
-  Thread_Control *ancestor
-)
-{
-  _Thread_Change_priority(
-    inheritor,
-    PRIORITY_PSEUDO_ISR,
-    ancestor,
-    _Thread_Inherit_priority_filter,
-    false
-  );
-}
-#endif
-
 static bool _Thread_Restore_priority_filter(
   Thread_Control   *the_thread,
   Priority_Control *new_priority,
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index 1528b48170..818073c2b1 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -42,8 +42,9 @@ void _Thread_queue_Enqueue_critical(
   Thread_queue_Context          *queue_context
 )
 {
-  Per_CPU_Control *cpu_self;
-  bool             success;
+  Thread_queue_Path  path;
+  Per_CPU_Control   *cpu_self;
+  bool               success;
 
 #if defined(RTEMS_MULTIPROCESSING)
   if ( _Thread_MP_Is_receive( the_thread ) && the_thread->receive_packet ) {
@@ -57,7 +58,7 @@ void _Thread_queue_Enqueue_critical(
   _Thread_Wait_set_queue( the_thread, queue );
   _Thread_Wait_set_operations( the_thread, operations );
 
-  ( *operations->enqueue )( queue, the_thread );
+  ( *operations->enqueue )( queue, the_thread, &path );
 
   _Thread_Wait_flags_set( the_thread, THREAD_QUEUE_INTEND_TO_BLOCK );
   cpu_self = _Thread_Dispatch_disable_critical( &queue_context->Lock_context );
@@ -124,6 +125,7 @@ void _Thread_queue_Enqueue_critical(
     _Thread_Remove_timer_and_unblock( the_thread, queue );
   }
 
+  _Thread_Update_priority( path.update_priority );
   _Thread_Dispatch_enable( cpu_self );
 }
 
diff --git a/cpukit/score/src/threadqops.c b/cpukit/score/src/threadqops.c
index 83abf0ded0..c288ceeca9 100644
--- a/cpukit/score/src/threadqops.c
+++ b/cpukit/score/src/threadqops.c
@@ -39,7 +39,7 @@ static void _Thread_queue_Do_nothing_extract(
   /* Do nothing */
 }
 
-static void _Thread_queue_Queue_enqueue(
+static Thread_queue_Heads *_Thread_queue_Queue_enqueue(
   Thread_queue_Queue *queue,
   Thread_Control     *the_thread,
   void             ( *initialize )( Thread_queue_Heads * ),
@@ -62,6 +62,8 @@ static void _Thread_queue_Queue_enqueue(
   _Chain_Prepend_unprotected( &heads->Free_chain, &spare_heads->Free_node );
 
   ( *enqueue )( heads, the_thread );
+
+  return heads;
 }
 
 static void _Thread_queue_Queue_extract(
@@ -116,9 +118,12 @@ static void _Thread_queue_FIFO_do_extract(
 
 static void _Thread_queue_FIFO_enqueue(
   Thread_queue_Queue *queue,
-  Thread_Control     *the_thread
+  Thread_Control     *the_thread,
+  Thread_queue_Path  *path
 )
 {
+  path->update_priority = NULL;
+
   _Thread_queue_Queue_enqueue(
     queue,
     the_thread,
@@ -266,9 +271,12 @@ static void _Thread_queue_Priority_do_extract(
 
 static void _Thread_queue_Priority_enqueue(
   Thread_queue_Queue *queue,
-  Thread_Control     *the_thread
+  Thread_Control     *the_thread,
+  Thread_queue_Path  *path
 )
 {
+  path->update_priority = NULL;
+
   _Thread_queue_Queue_enqueue(
     queue,
     the_thread,
@@ -310,6 +318,63 @@ static Thread_Control *_Thread_queue_Priority_first(
   return THREAD_RBTREE_NODE_TO_THREAD( first );
 }
 
+static void _Thread_queue_Priority_inherit_enqueue(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread,
+  Thread_queue_Path  *path
+)
+{
+  Thread_queue_Heads *heads;
+  Thread_Control     *owner;
+  Priority_Control    priority;
+
+  heads = _Thread_queue_Queue_enqueue(
+    queue,
+    the_thread,
+    _Thread_queue_Priority_do_initialize,
+    _Thread_queue_Priority_do_enqueue
+  );
+
+  owner = queue->owner;
+
+#if defined(RTEMS_SMP)
+  if ( _Chain_Has_only_one_node( &heads->Heads.Fifo ) ) {
+    priority = the_thread->current_priority;
+  } else {
+    priority = _Scheduler_Map_priority(
+      _Scheduler_Get_own( the_thread ),
+      PRIORITY_PSEUDO_ISR
+    );
+  }
+#else
+  (void) heads;
+
+  priority = the_thread->current_priority;
+#endif
+
+  if ( priority < owner->current_priority ) {
+    Scheduler_Node *own_node;
+
+    path->update_priority = owner;
+
+    owner->priority_restore_hint = true;
+    _Atomic_Fence( ATOMIC_ORDER_ACQ_REL );
+
+    own_node = _Scheduler_Thread_get_own_node( owner );
+    _Scheduler_Node_set_priority( own_node, priority, false );
+
+    owner->current_priority = priority;
+
+    ( *owner->Wait.operations->priority_change )(
+      owner,
+      priority,
+      owner->Wait.queue
+    );
+  } else {
+    path->update_priority = NULL;
+  }
+}
+
 #if defined(RTEMS_SMP)
 void _Thread_queue_Boost_priority(
   Thread_queue_Queue *queue,
@@ -359,3 +424,10 @@ const Thread_queue_Operations _Thread_queue_Operations_priority = {
   .extract = _Thread_queue_Priority_extract,
   .first = _Thread_queue_Priority_first
 };
+
+const Thread_queue_Operations _Thread_queue_Operations_priority_inherit = {
+  .priority_change = _Thread_queue_Priority_priority_change,
+  .enqueue = _Thread_queue_Priority_inherit_enqueue,
+  .extract = _Thread_queue_Priority_extract,
+  .first = _Thread_queue_Priority_first
+};