score: Implement priority boosting

7 files changed, 124 insertions, 6 deletions

diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index 46568813a4..1092b65f9b 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -437,6 +437,37 @@ 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/threadqimpl.h b/cpukit/score/include/rtems/score/threadqimpl.h
index bf01eb7404..510f886844 100644
--- a/cpukit/score/include/rtems/score/threadqimpl.h
+++ b/cpukit/score/include/rtems/score/threadqimpl.h
@@ -522,6 +522,31 @@ RTEMS_INLINE_ROUTINE void _Thread_queue_Destroy(
 }
 
 /**
+ * @brief Boosts the priority of the thread if threads of another scheduler
+ * instance are enqueued on the thread queue.
+ *
+ * The thread queue must use the priority waiting discipline.
+ *
+ * @param[in] queue The actual thread queue.
+ * @param[in] the_thread The thread to boost the priority if necessary.
+ */
+#if defined(RTEMS_SMP)
+void _Thread_queue_Boost_priority(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread
+);
+#else
+RTEMS_INLINE_ROUTINE void _Thread_queue_Boost_priority(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread
+)
+{
+  (void) queue;
+  (void) the_thread;
+}
+#endif
+
+/**
  * @brief Compare two thread's priority for RBTree Insertion.
  *
  * @param[in] left points to the left thread's RBnode
diff --git a/cpukit/score/src/coremutexseize.c b/cpukit/score/src/coremutexseize.c
index ddc5d6b982..8059659a42 100644
--- a/cpukit/score/src/coremutexseize.c
+++ b/cpukit/score/src/coremutexseize.c
@@ -75,7 +75,7 @@ void _CORE_mutex_Seize_interrupt_blocking(
     _Thread_queue_Release( &the_mutex->Wait_queue, lock_context );
 #endif
 
-    _Thread_Raise_priority( holder, executing->current_priority );
+    _Thread_Inherit_priority( holder, executing );
 
 #if !defined(RTEMS_SMP)
     _Thread_queue_Acquire( &the_mutex->Wait_queue, lock_context );
diff --git a/cpukit/score/src/coremutexsurrender.c b/cpukit/score/src/coremutexsurrender.c
index d3f965d55d..7d9c57f98b 100644
--- a/cpukit/score/src/coremutexsurrender.c
+++ b/cpukit/score/src/coremutexsurrender.c
@@ -209,6 +209,7 @@ CORE_mutex_Status _CORE_mutex_Surrender(
         case CORE_MUTEX_DISCIPLINES_PRIORITY_INHERIT:
           _CORE_mutex_Push_priority( the_mutex, the_thread );
           the_thread->resource_count++;
+          _Thread_queue_Boost_priority( &the_mutex->Wait_queue.Queue, the_thread );
           break;
         case CORE_MUTEX_DISCIPLINES_PRIORITY_CEILING:
           _CORE_mutex_Push_priority( the_mutex, the_thread );
diff --git a/cpukit/score/src/mutex.c b/cpukit/score/src/mutex.c
index ae637dda84..f03bab76ee 100644
--- a/cpukit/score/src/mutex.c
+++ b/cpukit/score/src/mutex.c
@@ -112,9 +112,7 @@ static void _Mutex_Acquire_slow(
   ISR_lock_Context  *lock_context
 )
 {
-  /* Priority inheritance */
-  _Thread_Raise_priority( owner, executing->current_priority );
-
+  _Thread_Inherit_priority( owner, executing );
   _Thread_queue_Enqueue_critical(
     &mutex->Queue.Queue,
     MUTEX_TQ_OPERATIONS,
@@ -136,15 +134,22 @@ static void _Mutex_Release_slow(
 {
   if (heads != NULL) {
     const Thread_queue_Operations *operations;
-    Thread_Control *first;
+    Thread_Control                *first;
+    bool                           unblock;
 
     operations = MUTEX_TQ_OPERATIONS;
     first = ( *operations->first )( heads );
 
     mutex->owner = first;
-    _Thread_queue_Extract_critical(
+    unblock = _Thread_queue_Extract_locked(
       &mutex->Queue.Queue,
       operations,
+      first
+    );
+    _Thread_queue_Boost_priority( &mutex->Queue.Queue, first );
+    _Thread_queue_Unblock_critical(
+      unblock,
+      &mutex->Queue.Queue,
       first,
       lock_context
     );
diff --git a/cpukit/score/src/threadchangepriority.c b/cpukit/score/src/threadchangepriority.c
index 8f5d14f412..35e5e5bfbc 100644
--- a/cpukit/score/src/threadchangepriority.c
+++ b/cpukit/score/src/threadchangepriority.c
@@ -110,6 +110,40 @@ 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/threadqops.c b/cpukit/score/src/threadqops.c
index 07473f50bd..7b017793ed 100644
--- a/cpukit/score/src/threadqops.c
+++ b/cpukit/score/src/threadqops.c
@@ -293,6 +293,28 @@ static Thread_Control *_Thread_queue_Priority_first(
   return THREAD_RBTREE_NODE_TO_THREAD( first );
 }
 
+#if defined(RTEMS_SMP)
+void _Thread_queue_Boost_priority(
+  Thread_queue_Queue *queue,
+  Thread_Control     *the_thread
+)
+{
+  Thread_queue_Heads *heads = queue->heads;
+
+  if (
+    heads != NULL
+      && (
+        !_Chain_Has_only_one_node( &heads->Heads.Fifo )
+          || _RBTree_Is_empty(
+            &_Thread_queue_Priority_queue( heads, the_thread )->Queue
+          )
+      )
+  ) {
+    _Thread_Raise_priority( the_thread, PRIORITY_PSEUDO_ISR );
+  }
+}
+#endif
+
 const Thread_queue_Operations _Thread_queue_Operations_default = {
   .priority_change = _Thread_queue_Do_nothing_priority_change,
   .extract = _Thread_queue_Do_nothing_extract