3 files changed, 56 insertions, 58 deletions

diff --git a/cpukit/include/rtems/score/coremuteximpl.h b/cpukit/include/rtems/score/coremuteximpl.h
index 426c4c5a95..757efbde9b 100644
--- a/cpukit/include/rtems/score/coremuteximpl.h
+++ b/cpukit/include/rtems/score/coremuteximpl.h
@@ -529,10 +529,7 @@ RTEMS_INLINE_ROUTINE Status_Control _CORE_ceiling_mutex_Surrender(
   Thread_queue_Context       *queue_context
 )
 {
-  unsigned int      nest_level;
-  ISR_lock_Context  lock_context;
-  Per_CPU_Control  *cpu_self;
-  Thread_Control   *new_owner;
+  unsigned int nest_level;
 
   _CORE_mutex_Acquire_critical( &the_mutex->Recursive.Mutex, queue_context );
 
@@ -549,53 +546,13 @@ RTEMS_INLINE_ROUTINE Status_Control _CORE_ceiling_mutex_Surrender(
     return STATUS_SUCCESSFUL;
   }
 
-  _Thread_Resource_count_decrement( executing );
-
-  _Thread_queue_Context_clear_priority_updates( queue_context );
-  _Thread_Wait_acquire_default_critical( executing, &lock_context );
-  _Thread_Priority_remove(
+  return _Thread_queue_Surrender_priority_ceiling(
+    &the_mutex->Recursive.Mutex.Wait_queue.Queue,
     executing,
     &the_mutex->Priority_ceiling,
-    queue_context
-  );
-  _Thread_Wait_release_default_critical( executing, &lock_context );
-
-  new_owner = _Thread_queue_First_locked(
-    &the_mutex->Recursive.Mutex.Wait_queue,
+    queue_context,
     CORE_MUTEX_TQ_OPERATIONS
   );
-  _CORE_mutex_Set_owner( &the_mutex->Recursive.Mutex, new_owner );
-
-  cpu_self = _Thread_Dispatch_disable_critical(
-    &queue_context->Lock_context.Lock_context
-  );
-
-  if ( new_owner != NULL ) {
-#if defined(RTEMS_MULTIPROCESSING)
-    if ( _Objects_Is_local_id( new_owner->Object.id ) )
-#endif
-    {
-      _Thread_Resource_count_increment( new_owner );
-      _Thread_Priority_add(
-        new_owner,
-        &the_mutex->Priority_ceiling,
-        queue_context
-      );
-    }
-
-    _Thread_queue_Extract_critical(
-      &the_mutex->Recursive.Mutex.Wait_queue.Queue,
-      CORE_MUTEX_TQ_OPERATIONS,
-      new_owner,
-      queue_context
-    );
-  } else {
-    _CORE_mutex_Release( &the_mutex->Recursive.Mutex, queue_context );
-  }
-
-  _Thread_Priority_update( queue_context );
-  _Thread_Dispatch_enable( cpu_self );
-  return STATUS_SUCCESSFUL;
 }
 
 /** @} */
diff --git a/cpukit/include/rtems/score/coresemimpl.h b/cpukit/include/rtems/score/coresemimpl.h
index ba4825bce9..40b58cbda7 100644
--- a/cpukit/include/rtems/score/coresemimpl.h
+++ b/cpukit/include/rtems/score/coresemimpl.h
@@ -133,23 +133,21 @@ RTEMS_INLINE_ROUTINE Status_Control _CORE_semaphore_Surrender(
   Thread_queue_Context          *queue_context
 )
 {
-  Thread_Control *the_thread;
-  Status_Control  status;
+  Status_Control      status;
+  Thread_queue_Heads *heads;
 
   status = STATUS_SUCCESSFUL;
 
   _CORE_semaphore_Acquire_critical( the_semaphore, queue_context );
 
-  the_thread = _Thread_queue_First_locked(
-    &the_semaphore->Wait_queue,
-    operations
-  );
-  if ( the_thread != NULL ) {
-    _Thread_queue_Extract_critical(
+  heads = the_semaphore->Wait_queue.Queue.heads;
+
+  if ( heads != NULL ) {
+    _Thread_queue_Surrender_no_priority(
       &the_semaphore->Wait_queue.Queue,
-      operations,
-      the_thread,
-      queue_context
+      heads,
+      queue_context,
+      operations
     );
   } else {
     if ( the_semaphore->count < maximum_count )
diff --git a/cpukit/include/rtems/score/threadqimpl.h b/cpukit/include/rtems/score/threadqimpl.h
index 44efc1fcd0..2465fc4499 100644
--- a/cpukit/include/rtems/score/threadqimpl.h
+++ b/cpukit/include/rtems/score/threadqimpl.h
@@ -1080,6 +1080,49 @@ void _Thread_queue_Surrender(
   const Thread_queue_Operations *operations
 );
 
+/**
+ * @brief Surrenders the thread queue previously owned by the thread to the
+ * first enqueued thread.
+ *
+ * The owner of the thread queue must be set to NULL by the caller.
+ *
+ * This function releases the thread queue lock.  In addition it performs a
+ * thread dispatch if necessary.
+ *
+ * @param[in, out] queue The actual thread queue.
+ * @param heads The thread queue heads.  It must not be NULL.
+ * @param queue_context The thread queue context of the lock acquire.
+ * @param operations The thread queue operations.
+ */
+void _Thread_queue_Surrender_no_priority(
+  Thread_queue_Queue            *queue,
+  Thread_queue_Heads            *heads,
+  Thread_queue_Context          *queue_context,
+  const Thread_queue_Operations *operations
+);
+
+/**
+ * @brief Surrenders the thread queue previously owned by the thread to the
+ * first enqueued thread.
+ *
+ * The owner of the thread queue must be set to NULL by the caller.
+ *
+ * This function releases the thread queue lock.  In addition it performs a
+ * thread dispatch if necessary.
+ *
+ * @param[in, out] queue The actual thread queue.
+ * @param heads The thread queue heads.  It must not be NULL.
+ * @param queue_context The thread queue context of the lock acquire.
+ * @param operations The thread queue operations.
+ */
+Status_Control _Thread_queue_Surrender_priority_ceiling(
+  Thread_queue_Queue            *queue,
+  Thread_Control                *executing,
+  Priority_Node                 *ceiling_priority,
+  Thread_queue_Context          *queue_context,
+  const Thread_queue_Operations *operations
+);
+
 #if defined(RTEMS_SMP)
 /**
  * @brief Surrenders the thread queue previously owned by the thread to the