From ac8402ddd6e4a8eb6defb98220d39d4c20a6f025 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Mon, 27 Jun 2016 10:20:34 +0200
Subject: [PATCH] score: Simplify _Thread_queue_Boost_priority()

Raise the priority under thread queue lock protection and omit the
superfluous thread queue priority change, since the thread is extracted
anyway.  The unblock operation will pick up the new priority.

Update #2412.
Update #2556.
Update #2765.
---
 cpukit/score/src/coremutexsurrender.c | 14 +++++++-------
 cpukit/score/src/mutex.c              |  2 +-
 cpukit/score/src/threadqops.c         | 26 +++++++++++++++-----------
 3 files changed, 23 insertions(+), 19 deletions(-)

diff --git a/cpukit/score/src/coremutexsurrender.c b/cpukit/score/src/coremutexsurrender.c
index 6604be89da..aed17ab33b 100644
--- a/cpukit/score/src/coremutexsurrender.c
+++ b/cpukit/score/src/coremutexsurrender.c
@@ -38,13 +38,6 @@ Status_Control _CORE_mutex_Surrender_slow(
 
     _CORE_mutex_Set_owner( the_mutex, new_owner );
 
-    unblock = _Thread_queue_Extract_locked(
-      &the_mutex->Wait_queue.Queue,
-      operations,
-      new_owner,
-      queue_context
-    );
-
 #if defined(RTEMS_MULTIPROCESSING)
     if ( _Objects_Is_local_id( new_owner->Object.id ) )
 #endif
@@ -53,6 +46,13 @@ Status_Control _CORE_mutex_Surrender_slow(
       _Thread_queue_Boost_priority( &the_mutex->Wait_queue.Queue, new_owner );
     }
 
+    unblock = _Thread_queue_Extract_locked(
+      &the_mutex->Wait_queue.Queue,
+      operations,
+      new_owner,
+      queue_context
+    );
+
     _Thread_queue_Unblock_critical(
       unblock,
       &the_mutex->Wait_queue.Queue,
diff --git a/cpukit/score/src/mutex.c b/cpukit/score/src/mutex.c
index 525992afe5..4b95262f18 100644
--- a/cpukit/score/src/mutex.c
+++ b/cpukit/score/src/mutex.c
@@ -136,13 +136,13 @@ static void _Mutex_Release_slow(
 
     mutex->Queue.Queue.owner = first;
     ++first->resource_count;
+    _Thread_queue_Boost_priority( &mutex->Queue.Queue, first );
     unblock = _Thread_queue_Extract_locked(
       &mutex->Queue.Queue,
       operations,
       first,
       queue_context
     );
-    _Thread_queue_Boost_priority( &mutex->Queue.Queue, first );
     _Thread_queue_Unblock_critical(
       unblock,
       &mutex->Queue.Queue,
diff --git a/cpukit/score/src/threadqops.c b/cpukit/score/src/threadqops.c
index 5a00ee6fe1..83abf0ded0 100644
--- a/cpukit/score/src/threadqops.c
+++ b/cpukit/score/src/threadqops.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2015, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -318,16 +318,20 @@ void _Thread_queue_Boost_priority(
 {
   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 );
+  if ( !_Chain_Has_only_one_node( &heads->Heads.Fifo ) ) {
+    const Scheduler_Control *scheduler;
+    Scheduler_Node          *own_node;
+    Priority_Control         boost_priority;
+
+    the_thread->priority_restore_hint = true;
+    _Atomic_Fence( ATOMIC_ORDER_ACQ_REL );
+
+    scheduler = _Scheduler_Get_own( the_thread );
+    own_node = _Scheduler_Thread_get_own_node( the_thread );
+    boost_priority = _Scheduler_Map_priority( scheduler, PRIORITY_PSEUDO_ISR );
+    _Scheduler_Node_set_priority( own_node, boost_priority, false );
+
+    the_thread->current_priority = boost_priority;
   }
 }
 #endif