score: Fine grained locking for semaphores

Update #2273.

7 files changed, 75 insertions, 34 deletions

diff --git a/cpukit/posix/include/rtems/posix/semaphoreimpl.h b/cpukit/posix/include/rtems/posix/semaphoreimpl.h
index 783289aa44..eeea51c488 100644
--- a/cpukit/posix/include/rtems/posix/semaphoreimpl.h
+++ b/cpukit/posix/include/rtems/posix/semaphoreimpl.h
@@ -89,6 +89,20 @@ RTEMS_INLINE_ROUTINE POSIX_Semaphore_Control *_POSIX_Semaphore_Get (
     _Objects_Get( &_POSIX_Semaphore_Information, (Objects_Id)*id, location );
 }
 
+RTEMS_INLINE_ROUTINE POSIX_Semaphore_Control *
+_POSIX_Semaphore_Get_interrupt_disable(
+  sem_t             *id,
+  Objects_Locations *location,
+  ISR_lock_Context  *lock_context
+)
+{
+  return (POSIX_Semaphore_Control *) _Objects_Get_isr_disable(
+    &_POSIX_Semaphore_Information,
+    (Objects_Id)*id,
+    location,
+    lock_context
+  );
+}
 
 /**
  *  @brief POSIX Semaphore Create Support
diff --git a/cpukit/posix/src/sempost.c b/cpukit/posix/src/sempost.c
index f139c9ab67..9c8c673ddb 100644
--- a/cpukit/posix/src/sempost.c
+++ b/cpukit/posix/src/sempost.c
@@ -37,8 +37,13 @@ int sem_post(
 {
   POSIX_Semaphore_Control          *the_semaphore;
   Objects_Locations                 location;
+  ISR_lock_Context                  lock_context;
 
-  the_semaphore = _POSIX_Semaphore_Get( sem, &location );
+  the_semaphore = _POSIX_Semaphore_Get_interrupt_disable(
+    sem,
+    &location,
+    &lock_context
+  );
   switch ( location ) {
 
     case OBJECTS_LOCAL:
@@ -46,12 +51,12 @@ int sem_post(
         &the_semaphore->Semaphore,
         the_semaphore->Object.id,
 #if defined(RTEMS_MULTIPROCESSING)
-        NULL         /* POSIX Semaphores are local only */
+        NULL,        /* POSIX Semaphores are local only */
 #else
-        NULL
+        NULL,
 #endif
+        &lock_context
       );
-      _Objects_Put( &the_semaphore->Object );
       return 0;
 
 #if defined(RTEMS_MULTIPROCESSING)
diff --git a/cpukit/rtems/src/semobtain.c b/cpukit/rtems/src/semobtain.c
index 0edac96264..ca13bea080 100644
--- a/cpukit/rtems/src/semobtain.c
+++ b/cpukit/rtems/src/semobtain.c
@@ -56,10 +56,10 @@ rtems_status_code rtems_semaphore_obtain(
       attribute_set = the_semaphore->attribute_set;
       wait = !_Options_Is_no_wait( option_set );
 #if defined(RTEMS_SMP)
-      _Thread_Disable_dispatch();
       if ( _Attributes_Is_multiprocessor_resource_sharing( attribute_set ) ) {
         MRSP_Status mrsp_status;
 
+        _Thread_Disable_dispatch();
         _ISR_lock_ISR_enable( &lock_context );
         mrsp_status = _MRSP_Obtain(
           &the_semaphore->Core_control.mrsp,
@@ -73,6 +73,9 @@ rtems_status_code rtems_semaphore_obtain(
       } else
 #endif
       if ( !_Attributes_Is_counting_semaphore( attribute_set ) ) {
+#if defined(RTEMS_SMP)
+        _Thread_Disable_dispatch();
+#endif
         _CORE_mutex_Seize(
           &the_semaphore->Core_control.mutex,
           executing,
@@ -98,10 +101,6 @@ rtems_status_code rtems_semaphore_obtain(
         timeout,
         &lock_context
       );
-#if defined(RTEMS_SMP)
-      _Thread_Enable_dispatch();
-#endif
-      _Objects_Put_for_get_isr_disable( &the_semaphore->Object );
       return _Semaphore_Translate_core_semaphore_return_code(
                   executing->Wait.return_code );
 
diff --git a/cpukit/rtems/src/semrelease.c b/cpukit/rtems/src/semrelease.c
index 37a05b2507..1f0581eec1 100644
--- a/cpukit/rtems/src/semrelease.c
+++ b/cpukit/rtems/src/semrelease.c
@@ -62,37 +62,46 @@ rtems_status_code rtems_semaphore_release(
   CORE_mutex_Status           mutex_status;
   CORE_semaphore_Status       semaphore_status;
   rtems_attribute             attribute_set;
+  ISR_lock_Context            lock_context;
 
-  the_semaphore = _Semaphore_Get( id, &location );
+  the_semaphore = _Semaphore_Get_interrupt_disable(
+    id,
+    &location,
+    &lock_context
+  );
   switch ( location ) {
 
     case OBJECTS_LOCAL:
       attribute_set = the_semaphore->attribute_set;
 #if defined(RTEMS_SMP)
       if ( _Attributes_Is_multiprocessor_resource_sharing( attribute_set ) ) {
+        _Thread_Disable_dispatch();
+        _ISR_lock_ISR_enable( &lock_context );
         MRSP_Status mrsp_status = _MRSP_Release(
           &the_semaphore->Core_control.mrsp,
           _Thread_Get_executing()
         );
-        _Objects_Put( &the_semaphore->Object );
+        _Thread_Enable_dispatch();
         return _Semaphore_Translate_MRSP_status_code( mrsp_status );
       } else
 #endif
       if ( !_Attributes_Is_counting_semaphore( attribute_set ) ) {
+        _Thread_Disable_dispatch();
+        _ISR_lock_ISR_enable( &lock_context );
         mutex_status = _CORE_mutex_Surrender(
           &the_semaphore->Core_control.mutex,
           id,
           MUTEX_MP_SUPPORT
         );
-        _Objects_Put( &the_semaphore->Object );
+        _Thread_Enable_dispatch();
         return _Semaphore_Translate_core_mutex_return_code( mutex_status );
       } else {
         semaphore_status = _CORE_semaphore_Surrender(
           &the_semaphore->Core_control.semaphore,
           id,
-          MUTEX_MP_SUPPORT
+          MUTEX_MP_SUPPORT,
+          &lock_context
         );
-        _Objects_Put( &the_semaphore->Object );
         return
           _Semaphore_Translate_core_semaphore_return_code( semaphore_status );
       }
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 (