score: Avoid Giant lock for CORE rwlock

Update #2555.

4 files changed, 190 insertions, 154 deletions

diff --git a/cpukit/score/include/rtems/score/corerwlockimpl.h b/cpukit/score/include/rtems/score/corerwlockimpl.h
index e727ad62c2..ed59d696b6 100644
--- a/cpukit/score/include/rtems/score/corerwlockimpl.h
+++ b/cpukit/score/include/rtems/score/corerwlockimpl.h
@@ -86,6 +86,22 @@ RTEMS_INLINE_ROUTINE void _CORE_RWLock_Destroy(
   _Thread_queue_Destroy( &the_rwlock->Wait_queue );
 }
 
+RTEMS_INLINE_ROUTINE void _CORE_RWLock_Acquire_critical(
+  CORE_RWLock_Control *the_rwlock,
+  ISR_lock_Context    *lock_context
+)
+{
+  _Thread_queue_Acquire_critical( &the_rwlock->Wait_queue, lock_context );
+}
+
+RTEMS_INLINE_ROUTINE void _CORE_RWLock_Release(
+  CORE_RWLock_Control *the_rwlock,
+  ISR_lock_Context    *lock_context
+)
+{
+  _Thread_queue_Release( &the_rwlock->Wait_queue, lock_context );
+}
+
 /**
  *  @brief Obtain RWLock for reading.
  *
@@ -103,7 +119,8 @@ void _CORE_RWLock_Seize_for_reading(
   CORE_RWLock_Control *the_rwlock,
   Thread_Control      *executing,
   bool                 wait,
-  Watchdog_Interval    timeout
+  Watchdog_Interval    timeout,
+  ISR_lock_Context    *lock_context
 );
 
 /**
@@ -122,7 +139,8 @@ void _CORE_RWLock_Seize_for_writing(
   CORE_RWLock_Control *the_rwlock,
   Thread_Control      *executing,
   bool                 wait,
-  Watchdog_Interval    timeout
+  Watchdog_Interval    timeout,
+  ISR_lock_Context    *lock_context
 );
 
 /**
@@ -137,7 +155,7 @@ void _CORE_RWLock_Seize_for_writing(
  */
 CORE_RWLock_Status _CORE_RWLock_Surrender(
   CORE_RWLock_Control *the_rwlock,
-  Thread_Control      *executing
+  ISR_lock_Context    *lock_context
 );
 
 /** @} */
diff --git a/cpukit/score/src/corerwlockobtainread.c b/cpukit/score/src/corerwlockobtainread.c
index 3d3757144f..fcbaf4a829 100644
--- a/cpukit/score/src/corerwlockobtainread.c
+++ b/cpukit/score/src/corerwlockobtainread.c
@@ -27,70 +27,68 @@ void _CORE_RWLock_Seize_for_reading(
   CORE_RWLock_Control *the_rwlock,
   Thread_Control      *executing,
   bool                 wait,
-  Watchdog_Interval    timeout
+  Watchdog_Interval    timeout,
+  ISR_lock_Context    *lock_context
 )
 {
-  ISR_lock_Context lock_context;
-
   /*
    *  If unlocked, then OK to read.
    *  If locked for reading and no waiters, then OK to read.
    *  If any thread is waiting, then we wait.
    */
 
-  _Thread_queue_Acquire( &the_rwlock->Wait_queue, &lock_context );
-    switch ( the_rwlock->current_state ) {
-      case CORE_RWLOCK_UNLOCKED:
-	the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_READING;
-	the_rwlock->number_of_readers += 1;
-	_Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
-	executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
-	return;
+  _CORE_RWLock_Acquire_critical( the_rwlock, lock_context );
+
+  switch ( the_rwlock->current_state ) {
+    case CORE_RWLOCK_UNLOCKED:
+      the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_READING;
+      the_rwlock->number_of_readers += 1;
+      _CORE_RWLock_Release( the_rwlock, lock_context );
+      executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
+      return;
 
-      case CORE_RWLOCK_LOCKED_FOR_READING: {
-        Thread_Control *waiter;
-        waiter = _Thread_queue_First_locked(
-          &the_rwlock->Wait_queue,
-          CORE_RWLOCK_TQ_OPERATIONS
-        );
-        if ( !waiter ) {
-	  the_rwlock->number_of_readers += 1;
-	  _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
-	  executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
-          return;
-        }
-        break;
+    case CORE_RWLOCK_LOCKED_FOR_READING: {
+      Thread_Control *waiter;
+      waiter = _Thread_queue_First_locked(
+        &the_rwlock->Wait_queue,
+        CORE_RWLOCK_TQ_OPERATIONS
+      );
+      if ( !waiter ) {
+        the_rwlock->number_of_readers += 1;
+        _CORE_RWLock_Release( the_rwlock, lock_context );
+        executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
+        return;
       }
-      case CORE_RWLOCK_LOCKED_FOR_WRITING:
-        break;
+      break;
     }
+    case CORE_RWLOCK_LOCKED_FOR_WRITING:
+      break;
+  }
 
-    /*
-     *  If the thread is not willing to wait, then return immediately.
-     */
-
-    if ( !wait ) {
-      _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
-      executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
-      return;
-    }
+  /*
+   *  If the thread is not willing to wait, then return immediately.
+   */
 
-    /*
-     *  We need to wait to enter this critical section
-     */
+  if ( !wait ) {
+    _CORE_RWLock_Release( the_rwlock, lock_context );
+    executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
+    return;
+  }
 
-    executing->Wait.option      = CORE_RWLOCK_THREAD_WAITING_FOR_READ;
-    executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
+  /*
+   *  We need to wait to enter this critical section
+   */
 
-    _Thread_queue_Enqueue_critical(
-       &the_rwlock->Wait_queue.Queue,
-       CORE_RWLOCK_TQ_OPERATIONS,
-       executing,
-       STATES_WAITING_FOR_RWLOCK,
-       timeout,
-       CORE_RWLOCK_TIMEOUT,
-       &lock_context
-    );
+  executing->Wait.option      = CORE_RWLOCK_THREAD_WAITING_FOR_READ;
+  executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
 
-    /* return to API level so it can dispatch and we block */
+  _Thread_queue_Enqueue_critical(
+     &the_rwlock->Wait_queue.Queue,
+     CORE_RWLOCK_TQ_OPERATIONS,
+     executing,
+     STATES_WAITING_FOR_RWLOCK,
+     timeout,
+     CORE_RWLOCK_TIMEOUT,
+     lock_context
+  );
 }
diff --git a/cpukit/score/src/corerwlockobtainwrite.c b/cpukit/score/src/corerwlockobtainwrite.c
index 07fe03acaa..e1bb1bd14a 100644
--- a/cpukit/score/src/corerwlockobtainwrite.c
+++ b/cpukit/score/src/corerwlockobtainwrite.c
@@ -24,14 +24,13 @@
 #include <rtems/score/watchdog.h>
 
 void _CORE_RWLock_Seize_for_writing(
-  CORE_RWLock_Control                 *the_rwlock,
-  Thread_Control                      *executing,
-  bool                                 wait,
-  Watchdog_Interval                    timeout
+  CORE_RWLock_Control *the_rwlock,
+  Thread_Control      *executing,
+  bool                 wait,
+  Watchdog_Interval    timeout,
+  ISR_lock_Context    *lock_context
 )
 {
-  ISR_lock_Context lock_context;
-
   /*
    *  If unlocked, then OK to read.
    *  Otherwise, we have to block.
@@ -39,45 +38,44 @@ void _CORE_RWLock_Seize_for_writing(
    *  If any thread is waiting, then we wait.
    */
 
-  _Thread_queue_Acquire( &the_rwlock->Wait_queue, &lock_context );
-    switch ( the_rwlock->current_state ) {
-      case CORE_RWLOCK_UNLOCKED:
-        the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_WRITING;
-        _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
-        executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
-        return;
+  _CORE_RWLock_Acquire_critical( the_rwlock, lock_context );
 
-      case CORE_RWLOCK_LOCKED_FOR_READING:
-      case CORE_RWLOCK_LOCKED_FOR_WRITING:
-        break;
-    }
+  switch ( the_rwlock->current_state ) {
+    case CORE_RWLOCK_UNLOCKED:
+      the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_WRITING;
+      _CORE_RWLock_Release( the_rwlock, lock_context );
+      executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
+      return;
 
-    /*
-     *  If the thread is not willing to wait, then return immediately.
-     */
+    case CORE_RWLOCK_LOCKED_FOR_READING:
+    case CORE_RWLOCK_LOCKED_FOR_WRITING:
+      break;
+  }
 
-    if ( !wait ) {
-      _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
-      executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
-      return;
-    }
+  /*
+   *  If the thread is not willing to wait, then return immediately.
+   */
 
-    /*
-     *  We need to wait to enter this critical section
-     */
+  if ( !wait ) {
+    _CORE_RWLock_Release( the_rwlock, lock_context );
+    executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
+    return;
+  }
 
-    executing->Wait.option      = CORE_RWLOCK_THREAD_WAITING_FOR_WRITE;
-    executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
+  /*
+   *  We need to wait to enter this critical section
+   */
 
-    _Thread_queue_Enqueue_critical(
-       &the_rwlock->Wait_queue.Queue,
-       CORE_RWLOCK_TQ_OPERATIONS,
-       executing,
-       STATES_WAITING_FOR_RWLOCK,
-       timeout,
-       CORE_RWLOCK_TIMEOUT,
-       &lock_context
-    );
+  executing->Wait.option      = CORE_RWLOCK_THREAD_WAITING_FOR_WRITE;
+  executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
 
-    /* return to API level so it can dispatch and we block */
+  _Thread_queue_Enqueue_critical(
+     &the_rwlock->Wait_queue.Queue,
+     CORE_RWLOCK_TQ_OPERATIONS,
+     executing,
+     STATES_WAITING_FOR_RWLOCK,
+     timeout,
+     CORE_RWLOCK_TIMEOUT,
+     lock_context
+  );
 }
diff --git a/cpukit/score/src/corerwlockrelease.c b/cpukit/score/src/corerwlockrelease.c
index d8180fce6c..3ace00da8e 100644
--- a/cpukit/score/src/corerwlockrelease.c
+++ b/cpukit/score/src/corerwlockrelease.c
@@ -19,19 +19,65 @@
 #include "config.h"
 #endif
 
-#include <rtems/system.h>
 #include <rtems/score/corerwlockimpl.h>
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/watchdog.h>
+#include <rtems/score/assert.h>
+
+static bool _CORE_RWLock_Is_waiting_for_reading(
+  const Thread_Control *the_thread
+)
+{
+  if ( the_thread->Wait.option == CORE_RWLOCK_THREAD_WAITING_FOR_READ ) {
+    return true;
+  } else {
+    _Assert( the_thread->Wait.option == CORE_RWLOCK_THREAD_WAITING_FOR_WRITE );
+    return false;
+  }
+}
+
+static Thread_Control *_CORE_RWLock_Flush_filter(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  CORE_RWLock_Control *the_rwlock;
+
+  the_rwlock = RTEMS_CONTAINER_OF(
+    queue,
+    CORE_RWLock_Control,
+    Wait_queue.Queue
+  );
+
+  switch ( the_rwlock->current_state ) {
+    case CORE_RWLOCK_LOCKED_FOR_READING:
+      if ( _CORE_RWLock_Is_waiting_for_reading( the_thread ) ) {
+        the_rwlock->number_of_readers += 1;
+      } else {
+        the_thread = NULL;
+      }
+      break;
+    case CORE_RWLOCK_LOCKED_FOR_WRITING:
+      the_thread = NULL;
+      break;
+    default:
+      _Assert( the_rwlock->current_state == CORE_RWLOCK_UNLOCKED );
+      if ( _CORE_RWLock_Is_waiting_for_reading( the_thread ) ) {
+        the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_READING;
+        the_rwlock->number_of_readers = 1;
+      } else {
+        the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_WRITING;
+      }
+      break;
+  }
+
+  return the_thread;
+}
 
 CORE_RWLock_Status _CORE_RWLock_Surrender(
   CORE_RWLock_Control *the_rwlock,
-  Thread_Control      *executing
+  ISR_lock_Context    *lock_context
 )
 {
-  ISR_Level       level;
-  Thread_Control *next;
-
   /*
    *  If unlocked, then OK to read.
    *  Otherwise, we have to block.
@@ -39,67 +85,43 @@ CORE_RWLock_Status _CORE_RWLock_Surrender(
    *  If any thread is waiting, then we wait.
    */
 
-  _ISR_Disable( level );
-    if ( the_rwlock->current_state == CORE_RWLOCK_UNLOCKED){
-      _ISR_Enable( level );
-      executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
-      return CORE_RWLOCK_SUCCESSFUL;
-    }
-    if ( the_rwlock->current_state == CORE_RWLOCK_LOCKED_FOR_READING ) {
-	the_rwlock->number_of_readers -= 1;
-	if ( the_rwlock->number_of_readers != 0 ) {
-          /* must be unlocked again */
-	  _ISR_Enable( level );
-          return CORE_RWLOCK_SUCCESSFUL;
-        }
-    }
-
-    /* CORE_RWLOCK_LOCKED_FOR_WRITING or READING with readers */
-    executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
+  _CORE_RWLock_Acquire_critical( the_rwlock, lock_context );
 
-    /*
-     * Implicitly transition to "unlocked" and find another thread interested
-     * in obtaining this rwlock.
-     */
-    the_rwlock->current_state = CORE_RWLOCK_UNLOCKED;
-  _ISR_Enable( level );
+  if ( the_rwlock->current_state == CORE_RWLOCK_UNLOCKED){
+    /* This is an error at the caller site */
+    _CORE_RWLock_Release( the_rwlock, lock_context );
+    return CORE_RWLOCK_SUCCESSFUL;
+  }
 
-  next = _Thread_queue_Dequeue(
-    &the_rwlock->Wait_queue,
-    CORE_RWLOCK_TQ_OPERATIONS,
-    NULL,
-    0
-  );
+  if ( the_rwlock->current_state == CORE_RWLOCK_LOCKED_FOR_READING ) {
+    the_rwlock->number_of_readers -= 1;
 
-  if ( next ) {
-    if ( next->Wait.option == CORE_RWLOCK_THREAD_WAITING_FOR_WRITE ) {
-      the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_WRITING;
+    if ( the_rwlock->number_of_readers != 0 ) {
+      /* must be unlocked again */
+      _CORE_RWLock_Release( the_rwlock, lock_context );
       return CORE_RWLOCK_SUCCESSFUL;
     }
-
-    /*
-     * Must be CORE_RWLOCK_THREAD_WAITING_FOR_READING
-     */
-    the_rwlock->number_of_readers += 1;
-    the_rwlock->current_state = CORE_RWLOCK_LOCKED_FOR_READING;
-
-    /*
-     * Now see if more readers can be let go.
-     */
-    while ( 1 ) {
-      next = _Thread_queue_First(
-        &the_rwlock->Wait_queue,
-        CORE_RWLOCK_TQ_OPERATIONS
-      );
-      if ( !next ||
-           next->Wait.option == CORE_RWLOCK_THREAD_WAITING_FOR_WRITE )
-        return CORE_RWLOCK_SUCCESSFUL;
-      the_rwlock->number_of_readers += 1;
-      _Thread_queue_Extract( next );
-    }
   }
 
-  /* indentation is to match _ISR_Disable at top */
+  _Assert(
+    the_rwlock->current_state == CORE_RWLOCK_LOCKED_FOR_WRITING
+      || ( the_rwlock->current_state == CORE_RWLOCK_LOCKED_FOR_READING
+        && the_rwlock->number_of_readers == 0 )
+  );
+
+  /*
+   * Implicitly transition to "unlocked" and find another thread interested
+   * in obtaining this rwlock.
+   */
+  the_rwlock->current_state = CORE_RWLOCK_UNLOCKED;
 
+  _Thread_queue_Flush_critical(
+    &the_rwlock->Wait_queue.Queue,
+    CORE_RWLOCK_TQ_OPERATIONS,
+    _CORE_RWLock_Flush_filter,
+    NULL,
+    0,
+    lock_context
+  );
   return CORE_RWLOCK_SUCCESSFUL;
 }