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