score: Fine grained locking for semaphores

Update #2273.

3 files changed, 34 insertions, 20 deletions

diff --git a/cpukit/score/include/rtems/score/coresemimpl.h b/cpukit/score/include/rtems/score/coresemimpl.h
index 3e1d481ab8..75851ee0d1 100644
--- a/cpukit/score/include/rtems/score/coresemimpl.h
+++ b/cpukit/score/include/rtems/score/coresemimpl.h
@@ -141,13 +141,16 @@ RTEMS_INLINE_ROUTINE void _CORE_semaphore_Destroy(
  *         with this instance of a SuperCore Semaphore
  *  @param[in] api_semaphore_mp_support is the routine to invoke if the
  *         thread unblocked is remote
+ *  @param[in] lock_context is a temporary variable used to contain the ISR
+ *        disable level cookie
  *
  *  @retval an indication of whether the routine succeeded or failed
  */
 CORE_semaphore_Status _CORE_semaphore_Surrender(
   CORE_semaphore_Control                *the_semaphore,
   Objects_Id                             id,
-  CORE_semaphore_API_mp_support_callout  api_semaphore_mp_support
+  CORE_semaphore_API_mp_support_callout  api_semaphore_mp_support,
+  ISR_lock_Context                      *lock_context
 );
 
 /**
@@ -228,21 +231,20 @@ RTEMS_INLINE_ROUTINE void _CORE_semaphore_Seize_isr_disable(
   /* disabled when you get here */
 
   executing->Wait.return_code = CORE_SEMAPHORE_STATUS_SUCCESSFUL;
+  _Thread_queue_Acquire_critical( &the_semaphore->Wait_queue, lock_context );
   if ( the_semaphore->count != 0 ) {
     the_semaphore->count -= 1;
-    _ISR_lock_ISR_enable( lock_context );
+    _Thread_queue_Release( &the_semaphore->Wait_queue, lock_context );
     return;
   }
 
   if ( !wait ) {
-    _ISR_lock_ISR_enable( lock_context );
+    _Thread_queue_Release( &the_semaphore->Wait_queue, lock_context );
     executing->Wait.return_code = CORE_SEMAPHORE_STATUS_UNSATISFIED_NOWAIT;
     return;
   }
 
-  _Thread_Disable_dispatch();
   executing->Wait.id = id;
-  _Thread_queue_Acquire_critical( &the_semaphore->Wait_queue, lock_context );
   _Thread_queue_Enqueue_critical(
     &the_semaphore->Wait_queue,
     executing,
@@ -251,7 +253,6 @@ RTEMS_INLINE_ROUTINE void _CORE_semaphore_Seize_isr_disable(
     CORE_SEMAPHORE_TIMEOUT,
     lock_context
   );
-  _Thread_Enable_dispatch();
 }
 
 /** @} */
diff --git a/cpukit/score/src/coresemsurrender.c b/cpukit/score/src/coresemsurrender.c
index 58ba6a3fdf..9b3d8fd4be 100644
--- a/cpukit/score/src/coresemsurrender.c
+++ b/cpukit/score/src/coresemsurrender.c
@@ -20,34 +20,46 @@
 #endif
 
 #include <rtems/score/coresemimpl.h>
-#include <rtems/score/objectimpl.h>
 
 CORE_semaphore_Status _CORE_semaphore_Surrender(
   CORE_semaphore_Control                *the_semaphore,
   Objects_Id                             id,
-  CORE_semaphore_API_mp_support_callout  api_semaphore_mp_support
+  CORE_semaphore_API_mp_support_callout  api_semaphore_mp_support,
+  ISR_lock_Context                      *lock_context
 )
 {
   Thread_Control *the_thread;
-  ISR_Level       level;
   CORE_semaphore_Status status;
 
   status = CORE_SEMAPHORE_STATUS_SUCCESSFUL;
 
-  if ( (the_thread = _Thread_queue_Dequeue(&the_semaphore->Wait_queue)) ) {
+  _Thread_queue_Acquire_critical( &the_semaphore->Wait_queue, lock_context );
+
+  the_thread = _Thread_queue_First_locked( &the_semaphore->Wait_queue );
+  if ( the_thread != NULL ) {
+#if defined(RTEMS_MULTIPROCESSING)
+    _Thread_Dispatch_disable();
+#endif
+
+    _Thread_queue_Extract_critical(
+      &the_semaphore->Wait_queue,
+      the_thread,
+      lock_context
+    );
 
 #if defined(RTEMS_MULTIPROCESSING)
     if ( !_Objects_Is_local_id( the_thread->Object.id ) )
       (*api_semaphore_mp_support) ( the_thread, id );
-#endif
 
+    _Thread_Dispatch_enable( _Per_CPU_Get() );
+#endif
   } else {
-    _ISR_Disable( level );
-      if ( the_semaphore->count < the_semaphore->Attributes.maximum_count )
-        the_semaphore->count += 1;
-      else
-        status = CORE_SEMAPHORE_MAXIMUM_COUNT_EXCEEDED;
-    _ISR_Enable( level );
+    if ( the_semaphore->count < the_semaphore->Attributes.maximum_count )
+      the_semaphore->count += 1;
+    else
+      status = CORE_SEMAPHORE_MAXIMUM_COUNT_EXCEEDED;
+
+    _Thread_queue_Release( &the_semaphore->Wait_queue, lock_context );
   }
 
   return status;
diff --git a/cpukit/score/src/mpci.c b/cpukit/score/src/mpci.c
index 7fd77f7597..6b3bc7fbd8 100644
--- a/cpukit/score/src/mpci.c
+++ b/cpukit/score/src/mpci.c
@@ -329,9 +329,10 @@ Thread _MPCI_Receive_server(
 
 void _MPCI_Announce ( void )
 {
-  _Thread_Disable_dispatch();
-  (void) _CORE_semaphore_Surrender( &_MPCI_Semaphore, 0, 0 );
-  _Thread_Enable_dispatch();
+  ISR_lock_Context lock_context;
+
+  _ISR_lock_ISR_disable( &lock_context );
+  (void) _CORE_semaphore_Surrender( &_MPCI_Semaphore, 0, 0, &lock_context );
 }
 
 void _MPCI_Internal_packets_Send_process_packet (