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.

5 files changed, 82 insertions, 45 deletions

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
+};