score: Add Thread_queue_Control::Lock

Move the complete thread queue enqueue procedure into
_Thread_queue_Enqueue_critical().  It is possible to use the thread
queue lock to protect state of the object embedding the thread queue.
This enables per object fine grained locking in the future.

Delete _Thread_queue_Enter_critical_section().

Update #2273.

18 files changed, 234 insertions, 140 deletions

diff --git a/cpukit/score/include/rtems/score/corebarrierimpl.h b/cpukit/score/include/rtems/score/corebarrierimpl.h
index 124ecabf05..e8b330dcb6 100644
--- a/cpukit/score/include/rtems/score/corebarrierimpl.h
+++ b/cpukit/score/include/rtems/score/corebarrierimpl.h
@@ -21,6 +21,7 @@
 
 #include <rtems/score/corebarrier.h>
 #include <rtems/score/thread.h>
+#include <rtems/score/threadqimpl.h>
 #include <rtems/score/watchdog.h>
 
 #ifdef __cplusplus
@@ -83,6 +84,13 @@ void _CORE_barrier_Initialize(
   CORE_barrier_Attributes    *the_barrier_attributes
 );
 
+RTEMS_INLINE_ROUTINE void _CORE_barrier_Destroy(
+  CORE_barrier_Control *the_barrier
+)
+{
+  _Thread_queue_Destroy( &the_barrier->Wait_queue );
+}
+
 /**
  *  @brief Wait for the barrier.
  *
diff --git a/cpukit/score/include/rtems/score/coremuteximpl.h b/cpukit/score/include/rtems/score/coremuteximpl.h
index e019b0abab..a6811680ff 100644
--- a/cpukit/score/include/rtems/score/coremuteximpl.h
+++ b/cpukit/score/include/rtems/score/coremuteximpl.h
@@ -119,6 +119,11 @@ CORE_mutex_Status _CORE_mutex_Initialize(
   bool                          initially_locked
 );
 
+RTEMS_INLINE_ROUTINE void _CORE_mutex_Destroy( CORE_mutex_Control *the_mutex )
+{
+  _Thread_queue_Destroy( &the_mutex->Wait_queue );
+}
+
 /**
  *  @brief Attempt to receive a unit from the_mutex.
  *
diff --git a/cpukit/score/include/rtems/score/corerwlockimpl.h b/cpukit/score/include/rtems/score/corerwlockimpl.h
index 331510ba02..e619574d89 100644
--- a/cpukit/score/include/rtems/score/corerwlockimpl.h
+++ b/cpukit/score/include/rtems/score/corerwlockimpl.h
@@ -21,6 +21,7 @@
 
 #include <rtems/score/corerwlock.h>
 #include <rtems/score/thread.h>
+#include <rtems/score/threadqimpl.h>
 #include <rtems/score/watchdog.h>
 
 #ifdef __cplusplus
@@ -87,6 +88,13 @@ void _CORE_RWLock_Initialize(
   CORE_RWLock_Attributes    *the_rwlock_attributes
 );
 
+RTEMS_INLINE_ROUTINE void _CORE_RWLock_Destroy(
+  CORE_RWLock_Control *the_rwlock
+)
+{
+  _Thread_queue_Destroy( &the_rwlock->Wait_queue );
+}
+
 /**
  *  @brief Obtain RWLock for reading.
  *
diff --git a/cpukit/score/include/rtems/score/coresemimpl.h b/cpukit/score/include/rtems/score/coresemimpl.h
index 6a3a212f34..fac9f689e4 100644
--- a/cpukit/score/include/rtems/score/coresemimpl.h
+++ b/cpukit/score/include/rtems/score/coresemimpl.h
@@ -98,6 +98,13 @@ void _CORE_semaphore_Initialize(
   uint32_t                         initial_value
 );
 
+RTEMS_INLINE_ROUTINE void _CORE_semaphore_Destroy(
+  CORE_semaphore_Control *the_semaphore
+)
+{
+  _Thread_queue_Destroy( &the_semaphore->Wait_queue );
+}
+
 #if defined(RTEMS_SCORE_CORESEM_ENABLE_SEIZE_BODY)
   /**
    *  This routine attempts to receive a unit from @a the_semaphore.
@@ -234,16 +241,14 @@ RTEMS_INLINE_ROUTINE void _CORE_semaphore_Seize_isr_disable(
   }
 
   _Thread_Disable_dispatch();
-  _Thread_queue_Enter_critical_section( &the_semaphore->Wait_queue );
-  executing->Wait.queue          = &the_semaphore->Wait_queue;
-  executing->Wait.id             = id;
-  _ISR_lock_ISR_enable( lock_context );
-
-  _Thread_queue_Enqueue(
+  executing->Wait.id = id;
+  _Thread_queue_Acquire_critical( &the_semaphore->Wait_queue, lock_context );
+  _Thread_queue_Enqueue_critical(
     &the_semaphore->Wait_queue,
     executing,
     STATES_WAITING_FOR_SEMAPHORE,
-    timeout
+    timeout,
+    lock_context
   );
   _Thread_Enable_dispatch();
 }
diff --git a/cpukit/score/include/rtems/score/threadq.h b/cpukit/score/include/rtems/score/threadq.h
index 00b9221356..1f99ffde04 100644
--- a/cpukit/score/include/rtems/score/threadq.h
+++ b/cpukit/score/include/rtems/score/threadq.h
@@ -20,6 +20,7 @@
 #define _RTEMS_SCORE_THREADQ_H
 
 #include <rtems/score/chain.h>
+#include <rtems/score/isrlock.h>
 #include <rtems/score/states.h>
 #include <rtems/score/threadsync.h>
 #include <rtems/score/rbtree.h>
@@ -62,6 +63,18 @@ typedef struct {
     /** This is the set of threads for priority discipline waiting. */
     RBTree_Control Priority;
   } Queues;
+
+  /**
+   * @brief Lock to protect this thread queue.
+   *
+   * It may be used to protect additional state of the object embedding this
+   * thread queue.
+   *
+   * @see _Thread_queue_Acquire(), _Thread_queue_Acquire_critical() and
+   * _Thread_queue_Release().
+   */
+  ISR_LOCK_MEMBER( Lock )
+
   /** This field is used to manage the critical section. */
   Thread_blocking_operation_States sync_state;
   /** This field indicates the thread queue's blocking discipline. */
diff --git a/cpukit/score/include/rtems/score/threadqimpl.h b/cpukit/score/include/rtems/score/threadqimpl.h
index 6fc38cfa5b..3595dc5d85 100644
--- a/cpukit/score/include/rtems/score/threadqimpl.h
+++ b/cpukit/score/include/rtems/score/threadqimpl.h
@@ -31,6 +31,31 @@ extern "C" {
  */
 /**@{*/
 
+RTEMS_INLINE_ROUTINE void _Thread_queue_Acquire_critical(
+  Thread_queue_Control *the_thread_queue,
+  ISR_lock_Context     *lock_context
+)
+{
+  _ISR_lock_Acquire( &the_thread_queue->Lock, lock_context );
+}
+
+RTEMS_INLINE_ROUTINE void _Thread_queue_Acquire(
+  Thread_queue_Control *the_thread_queue,
+  ISR_lock_Context     *lock_context
+)
+{
+  _ISR_lock_ISR_disable( lock_context );
+  _Thread_queue_Acquire_critical( the_thread_queue, lock_context );
+}
+
+RTEMS_INLINE_ROUTINE void _Thread_queue_Release(
+  Thread_queue_Control *the_thread_queue,
+  ISR_lock_Context     *lock_context
+)
+{
+  _ISR_lock_Release_and_ISR_enable( &the_thread_queue->Lock, lock_context );
+}
+
 /**
  *  The following type defines the callout used when a remote task
  *  is extracted from a local thread queue.
@@ -55,26 +80,48 @@ Thread_Control *_Thread_queue_Dequeue(
 );
 
 /**
- *  @brief Blocks a thread and places it on a thread.
+ * @brief Blocks a thread and places it on a thread queue.
  *
- *  This routine blocks a thread, places it on a thread, and optionally
- *  starts a timeout timer.
+ * This routine blocks a thread, places it on a thread queue, and optionally
+ * starts a watchdog in case the timeout interval is not WATCHDOG_NO_TIMEOUT.
  *
- *  @param[in] the_thread_queue pointer to threadq
- *  @param[in] the_thread the thread to enqueue
- *  @param[in] state is the new state of the thread
- *  @param[in] timeout interval to wait
+ * The caller must be the owner of the thread queue lock.  This function will
+ * release the thread queue lock and register it as the new thread lock.
  *
- *  - INTERRUPT LATENCY:
- *    + single case
+ * @param[in] the_thread_queue The thread queue.
+ * @param[in] the_thread The thread to enqueue.
+ * @param[in] state The new state of the thread.
+ * @param[in] timeout Interval to wait.  Use WATCHDOG_NO_TIMEOUT to block
+ * potentially forever.
+ * @param[in] lock_context The lock context of the lock acquire.
  */
-void _Thread_queue_Enqueue(
+void _Thread_queue_Enqueue_critical(
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread,
   States_Control        state,
-  Watchdog_Interval     timeout
+  Watchdog_Interval     timeout,
+  ISR_lock_Context     *lock_context
 );
 
+RTEMS_INLINE_ROUTINE void _Thread_queue_Enqueue(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread,
+  States_Control        state,
+  Watchdog_Interval     timeout
+)
+{
+  ISR_lock_Context lock_context;
+
+  _Thread_queue_Acquire( the_thread_queue, &lock_context );
+  _Thread_queue_Enqueue_critical(
+    the_thread_queue,
+    the_thread,
+    state,
+    timeout,
+    &lock_context
+  );
+}
+
 /**
  *  @brief Extracts thread from thread queue.
  *
@@ -116,15 +163,30 @@ void _Thread_queue_Extract_with_proxy(
 );
 
 /**
- *  @brief Gets a pointer to the "first" thread on the_thread_queue.
+ * @brief Returns the first thread on the thread queue if it exists, otherwise
+ * @c NULL (locked).
+ *
+ * The caller must be the owner of the thread queue lock.
+ *
+ * @param[in] the_thread_queue The thread queue.
  *
- *  This function returns a pointer to the "first" thread
- *  on the_thread_queue.  The "first" thread is selected
- *  based on the discipline of the_thread_queue.
+ * @retval NULL No thread is present on the thread queue.
+ * @retval first The first thread on the thread queue according to the enqueue
+ * order.
+ */
+Thread_Control *_Thread_queue_First_locked(
+  Thread_queue_Control *the_thread_queue
+);
+
+/**
+ * @brief Returns the first thread on the thread queue if it exists, otherwise
+ * @c NULL.
  *
- *  @param[in] the_thread_queue pointer to thread queue
+ * @param[in] the_thread_queue The thread queue.
  *
- *  @retval first thread or NULL
+ * @retval NULL No thread is present on the thread queue.
+ * @retval first The first thread on the thread queue according to the enqueue
+ * order.
  */
 Thread_Control *_Thread_queue_First(
   Thread_queue_Control *the_thread_queue
@@ -165,6 +227,13 @@ void _Thread_queue_Initialize(
   uint32_t                      timeout_status
 );
 
+RTEMS_INLINE_ROUTINE void _Thread_queue_Destroy(
+  Thread_queue_Control *the_thread_queue
+)
+{
+  _ISR_lock_Destroy( &the_thread_queue->Lock );
+}
+
 /**
  *  @brief Thread queue timeout.
  *
@@ -212,17 +281,6 @@ RBTree_Compare_result _Thread_queue_Compare_priority(
   const RBTree_Node *right
 );
 
-/**
- * This routine is invoked to indicate that the specified thread queue is
- * entering a critical section.
- */
-RTEMS_INLINE_ROUTINE void _Thread_queue_Enter_critical_section (
-  Thread_queue_Control *the_thread_queue
-)
-{
-  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
-}
-
 /**@}*/
 
 #ifdef __cplusplus
diff --git a/cpukit/score/src/corebarrierwait.c b/cpukit/score/src/corebarrierwait.c
index 6267ae67c0..30abf0c17a 100644
--- a/cpukit/score/src/corebarrierwait.c
+++ b/cpukit/score/src/corebarrierwait.c
@@ -32,30 +32,28 @@ void _CORE_barrier_Wait(
   CORE_barrier_API_mp_support_callout  api_barrier_mp_support
 )
 {
-  ISR_Level       level;
+  ISR_lock_Context lock_context;
 
   executing->Wait.return_code = CORE_BARRIER_STATUS_SUCCESSFUL;
-  _ISR_Disable( level );
+  _Thread_queue_Acquire( &the_barrier->Wait_queue, &lock_context );
   the_barrier->number_of_waiting_threads++;
   if ( _CORE_barrier_Is_automatic( &the_barrier->Attributes ) ) {
     if ( the_barrier->number_of_waiting_threads ==
 	 the_barrier->Attributes.maximum_count) {
       executing->Wait.return_code = CORE_BARRIER_STATUS_AUTOMATICALLY_RELEASED;
-      _ISR_Enable( level );
+      _Thread_queue_Release( &the_barrier->Wait_queue, &lock_context );
       _CORE_barrier_Release( the_barrier, id, api_barrier_mp_support );
       return;
     }
   }
 
-  _Thread_queue_Enter_critical_section( &the_barrier->Wait_queue );
-  executing->Wait.queue          = &the_barrier->Wait_queue;
-  executing->Wait.id             = id;
-  _ISR_Enable( level );
+  executing->Wait.id = id;
 
-  _Thread_queue_Enqueue(
+  _Thread_queue_Enqueue_critical(
     &the_barrier->Wait_queue,
     executing,
     STATES_WAITING_FOR_BARRIER,
-    timeout
+    timeout,
+    &lock_context
   );
 }
diff --git a/cpukit/score/src/coremsgclose.c b/cpukit/score/src/coremsgclose.c
index 987f038cb3..17ecc4428e 100644
--- a/cpukit/score/src/coremsgclose.c
+++ b/cpukit/score/src/coremsgclose.c
@@ -54,4 +54,5 @@ void _CORE_message_queue_Close(
 
   (void) _Workspace_Free( the_message_queue->message_buffers );
 
+  _Thread_queue_Destroy( &the_message_queue->Wait_queue );
 }
diff --git a/cpukit/score/src/coremsgseize.c b/cpukit/score/src/coremsgseize.c
index db8d558ca6..45105ca19b 100644
--- a/cpukit/score/src/coremsgseize.c
+++ b/cpukit/score/src/coremsgseize.c
@@ -36,15 +36,15 @@ void _CORE_message_queue_Seize(
   Watchdog_Interval                timeout
 )
 {
-  ISR_Level                          level;
+  ISR_lock_Context                   lock_context;
   CORE_message_queue_Buffer_control *the_message;
 
   executing->Wait.return_code = CORE_MESSAGE_QUEUE_STATUS_SUCCESSFUL;
-  _ISR_Disable( level );
+  _Thread_queue_Acquire( &the_message_queue->Wait_queue, &lock_context );
   the_message = _CORE_message_queue_Get_pending_message( the_message_queue );
   if ( the_message != NULL ) {
     the_message_queue->number_of_pending_messages -= 1;
-    _ISR_Enable( level );
+    _Thread_queue_Release( &the_message_queue->Wait_queue, &lock_context );
 
     *size_p = the_message->Contents.size;
     executing->Wait.count =
@@ -109,23 +109,21 @@ void _CORE_message_queue_Seize(
   }
 
   if ( !wait ) {
-    _ISR_Enable( level );
+    _Thread_queue_Release( &the_message_queue->Wait_queue, &lock_context );
     executing->Wait.return_code = CORE_MESSAGE_QUEUE_STATUS_UNSATISFIED_NOWAIT;
     return;
   }
 
-  _Thread_queue_Enter_critical_section( &the_message_queue->Wait_queue );
-  executing->Wait.queue = &the_message_queue->Wait_queue;
   executing->Wait.id = id;
   executing->Wait.return_argument_second.mutable_object = buffer;
   executing->Wait.return_argument = size_p;
   /* Wait.count will be filled in with the message priority */
-  _ISR_Enable( level );
 
-  _Thread_queue_Enqueue(
+  _Thread_queue_Enqueue_critical(
     &the_message_queue->Wait_queue,
     executing,
     STATES_WAITING_FOR_MESSAGE,
-    timeout
+    timeout,
+    &lock_context
   );
 }
diff --git a/cpukit/score/src/coremsgsubmit.c b/cpukit/score/src/coremsgsubmit.c
index 4437856b17..0bcebe04af 100644
--- a/cpukit/score/src/coremsgsubmit.c
+++ b/cpukit/score/src/coremsgsubmit.c
@@ -120,16 +120,10 @@ CORE_message_queue_Status _CORE_message_queue_Submit(
      *  would be to use this variable prior to here.
      */
     {
-      ISR_Level        level;
-
-      _ISR_Disable( level );
-      _Thread_queue_Enter_critical_section( &the_message_queue->Wait_queue );
-      executing->Wait.queue = &the_message_queue->Wait_queue;
       executing->Wait.id = id;
       executing->Wait.return_argument_second.immutable_object = buffer;
       executing->Wait.option = (uint32_t) size;
       executing->Wait.count = submit_type;
-      _ISR_Enable( level );
 
       _Thread_queue_Enqueue(
         &the_message_queue->Wait_queue,
diff --git a/cpukit/score/src/coremutexseize.c b/cpukit/score/src/coremutexseize.c
index 3fc3765ef4..67954b697b 100644
--- a/cpukit/score/src/coremutexseize.c
+++ b/cpukit/score/src/coremutexseize.c
@@ -53,27 +53,35 @@ void _CORE_mutex_Seize_interrupt_blocking(
   ISR_lock_Context    *lock_context
 )
 {
-  _Thread_queue_Enter_critical_section( &the_mutex->Wait_queue );
-  executing->Wait.queue = &the_mutex->Wait_queue;
   _Thread_Disable_dispatch();
-  _ISR_lock_ISR_enable( lock_context );
 
   if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ) {
     Thread_Control *holder = the_mutex->holder;
 
+    /*
+     * To enable interrupts here works only since we own the Giant lock and
+     * only threads are allowed to seize and surrender mutexes with the
+     * priority inheritance protocol.
+     */
+    _ISR_lock_ISR_enable( lock_context );
+
     _Scheduler_Change_priority_if_higher(
       _Scheduler_Get( holder ),
       holder,
       executing->current_priority,
       false
     );
+
+    _ISR_lock_ISR_disable( lock_context );
   }
 
-  _Thread_queue_Enqueue(
+  _Thread_queue_Acquire_critical( &the_mutex->Wait_queue, lock_context );
+  _Thread_queue_Enqueue_critical(
     &the_mutex->Wait_queue,
     executing,
     STATES_WAITING_FOR_MUTEX,
-    timeout
+    timeout,
+    lock_context
   );
 
   _Thread_Enable_dispatch();
diff --git a/cpukit/score/src/corerwlockobtainread.c b/cpukit/score/src/corerwlockobtainread.c
index 203680f6dc..59c1097354 100644
--- a/cpukit/score/src/corerwlockobtainread.c
+++ b/cpukit/score/src/corerwlockobtainread.c
@@ -32,7 +32,7 @@ void _CORE_RWLock_Obtain_for_reading(
   CORE_RWLock_API_mp_support_callout   api_rwlock_mp_support
 )
 {
-  ISR_Level       level;
+  ISR_lock_Context lock_context;
 
   /*
    *  If unlocked, then OK to read.
@@ -40,21 +40,21 @@ void _CORE_RWLock_Obtain_for_reading(
    *  If any thread is waiting, then we wait.
    */
 
-  _ISR_Disable( level );
+  _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;
-	_ISR_Enable( level );
+	_Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
 	executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
 	return;
 
       case CORE_RWLOCK_LOCKED_FOR_READING: {
         Thread_Control *waiter;
-        waiter = _Thread_queue_First( &the_rwlock->Wait_queue );
+        waiter = _Thread_queue_First_locked( &the_rwlock->Wait_queue );
         if ( !waiter ) {
 	  the_rwlock->number_of_readers += 1;
-	  _ISR_Enable( level );
+	  _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
 	  executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
           return;
         }
@@ -69,7 +69,7 @@ void _CORE_RWLock_Obtain_for_reading(
      */
 
     if ( !wait ) {
-      _ISR_Enable( level );
+      _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
       executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
       return;
     }
@@ -78,18 +78,16 @@ void _CORE_RWLock_Obtain_for_reading(
      *  We need to wait to enter this critical section
      */
 
-    _Thread_queue_Enter_critical_section( &the_rwlock->Wait_queue );
-    executing->Wait.queue       = &the_rwlock->Wait_queue;
     executing->Wait.id          = id;
     executing->Wait.option      = CORE_RWLOCK_THREAD_WAITING_FOR_READ;
     executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
-    _ISR_Enable( level );
 
-    _Thread_queue_Enqueue(
+    _Thread_queue_Enqueue_critical(
        &the_rwlock->Wait_queue,
        executing,
        STATES_WAITING_FOR_RWLOCK,
-       timeout
+       timeout,
+       &lock_context
     );
 
     /* return to API level so it can dispatch and we block */
diff --git a/cpukit/score/src/corerwlockobtainwrite.c b/cpukit/score/src/corerwlockobtainwrite.c
index 3499bcd4c8..409d31aeec 100644
--- a/cpukit/score/src/corerwlockobtainwrite.c
+++ b/cpukit/score/src/corerwlockobtainwrite.c
@@ -32,7 +32,7 @@ void _CORE_RWLock_Obtain_for_writing(
   CORE_RWLock_API_mp_support_callout   api_rwlock_mp_support
 )
 {
-  ISR_Level       level;
+  ISR_lock_Context lock_context;
 
   /*
    *  If unlocked, then OK to read.
@@ -41,13 +41,13 @@ void _CORE_RWLock_Obtain_for_writing(
    *  If any thread is waiting, then we wait.
    */
 
-  _ISR_Disable( level );
+  _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;
-	_ISR_Enable( level );
-	executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
-	return;
+        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;
 
       case CORE_RWLOCK_LOCKED_FOR_READING:
       case CORE_RWLOCK_LOCKED_FOR_WRITING:
@@ -59,7 +59,7 @@ void _CORE_RWLock_Obtain_for_writing(
      */
 
     if ( !wait ) {
-      _ISR_Enable( level );
+      _Thread_queue_Release( &the_rwlock->Wait_queue, &lock_context );
       executing->Wait.return_code = CORE_RWLOCK_UNAVAILABLE;
       return;
     }
@@ -68,18 +68,16 @@ void _CORE_RWLock_Obtain_for_writing(
      *  We need to wait to enter this critical section
      */
 
-    _Thread_queue_Enter_critical_section( &the_rwlock->Wait_queue );
-    executing->Wait.queue       = &the_rwlock->Wait_queue;
     executing->Wait.id          = id;
     executing->Wait.option      = CORE_RWLOCK_THREAD_WAITING_FOR_WRITE;
     executing->Wait.return_code = CORE_RWLOCK_SUCCESSFUL;
-    _ISR_Enable( level );
 
-    _Thread_queue_Enqueue(
+    _Thread_queue_Enqueue_critical(
        &the_rwlock->Wait_queue,
        executing,
        STATES_WAITING_FOR_RWLOCK,
-       timeout
+       timeout,
+       &lock_context
     );
 
     /* return to API level so it can dispatch and we block */
diff --git a/cpukit/score/src/coresemseize.c b/cpukit/score/src/coresemseize.c
index 9c0db96204..d7ab28e779 100644
--- a/cpukit/score/src/coresemseize.c
+++ b/cpukit/score/src/coresemseize.c
@@ -34,13 +34,13 @@ void _CORE_semaphore_Seize(
   Watchdog_Interval       timeout
 )
 {
-  ISR_Level       level;
+  ISR_lock_Context lock_context;
 
   executing->Wait.return_code = CORE_SEMAPHORE_STATUS_SUCCESSFUL;
-  _ISR_Disable( level );
+  _Thread_queue_Acquire( &the_semaphore->Wait_queue, &lock_context );
   if ( the_semaphore->count != 0 ) {
     the_semaphore->count -= 1;
-    _ISR_Enable( level );
+    _Thread_queue_Release( &the_semaphore->Wait_queue, &lock_context );
     return;
   }
 
@@ -50,7 +50,7 @@ void _CORE_semaphore_Seize(
    *  the semaphore was not available and the caller never blocked.
    */
   if ( !wait ) {
-    _ISR_Enable( level );
+    _Thread_queue_Release( &the_semaphore->Wait_queue, &lock_context );
     executing->Wait.return_code = CORE_SEMAPHORE_STATUS_UNSATISFIED_NOWAIT;
     return;
   }
@@ -59,15 +59,13 @@ void _CORE_semaphore_Seize(
    *  If the semaphore is not available and the caller is willing to
    *  block, then we now block the caller with optional timeout.
    */
-  _Thread_queue_Enter_critical_section( &the_semaphore->Wait_queue );
-  executing->Wait.queue = &the_semaphore->Wait_queue;
-  executing->Wait.id    = id;
-  _ISR_Enable( level );
-  _Thread_queue_Enqueue(
+  executing->Wait.id = id;
+  _Thread_queue_Enqueue_critical(
     &the_semaphore->Wait_queue,
     executing,
     STATES_WAITING_FOR_SEMAPHORE,
-    timeout
+    timeout,
+    &lock_context
   );
 }
 #endif
diff --git a/cpukit/score/src/mpci.c b/cpukit/score/src/mpci.c
index 424bcb4d33..9915aa2d12 100644
--- a/cpukit/score/src/mpci.c
+++ b/cpukit/score/src/mpci.c
@@ -206,8 +206,6 @@ uint32_t   _MPCI_Send_request_packet (
 
     (*_MPCI_table->send_packet)( destination, the_packet );
 
-    _Thread_queue_Enter_critical_section( &_MPCI_Remote_blocked_threads );
-
     /*
      *  See if we need a default timeout
      */
diff --git a/cpukit/score/src/threadq.c b/cpukit/score/src/threadq.c
index 0ea0c82d83..47c294e968 100644
--- a/cpukit/score/src/threadq.c
+++ b/cpukit/score/src/threadq.c
@@ -55,10 +55,11 @@ void _Thread_queue_Initialize(
   the_thread_queue->timeout_status = timeout_status;
   the_thread_queue->sync_state     = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
 
+  _ISR_lock_Initialize( &the_thread_queue->Lock, "Thread Queue" );
+
   if ( the_discipline == THREAD_QUEUE_DISCIPLINE_PRIORITY ) {
     _RBTree_Initialize_empty( &the_thread_queue->Queues.Priority );
   } else { /* must be THREAD_QUEUE_DISCIPLINE_FIFO */
     _Chain_Initialize_empty( &the_thread_queue->Queues.Fifo );
   }
-
 }
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index 590865d8dd..76eaa30dde 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -24,18 +24,6 @@
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/watchdogimpl.h>
 
-ISR_LOCK_DEFINE( static, _Thread_queue_Lock, "Thread Queue" )
-
-static void _Thread_queue_Acquire( ISR_lock_Context *lock_context )
-{
-  _ISR_lock_ISR_disable_and_acquire( &_Thread_queue_Lock, lock_context );
-}
-
-static void _Thread_queue_Release( ISR_lock_Context *lock_context )
-{
-  _ISR_lock_Release_and_ISR_enable( &_Thread_queue_Lock, lock_context );
-}
-
 /**
  *  @brief Finalize a blocking operation.
  *
@@ -50,8 +38,9 @@ static void _Thread_queue_Release( ISR_lock_Context *lock_context )
  *  @param[in] lock_context is the previous ISR disable level
  */
 static void _Thread_blocking_operation_Finalize(
-  Thread_Control   *the_thread,
-  ISR_lock_Context *lock_context
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread,
+  ISR_lock_Context     *lock_context
 )
 {
   /*
@@ -59,16 +48,18 @@ static void _Thread_blocking_operation_Finalize(
    */
   the_thread->Wait.queue = NULL;
 
+  _Thread_Lock_restore_default( the_thread );
+
   /*
    *  If the sync state is timed out, this is very likely not needed.
    *  But better safe than sorry when it comes to critical sections.
    */
   if ( _Watchdog_Is_active( &the_thread->Timer ) ) {
     _Watchdog_Deactivate( &the_thread->Timer );
-    _Thread_queue_Release( lock_context );
+    _Thread_queue_Release( the_thread_queue, lock_context );
     _Watchdog_Remove_ticks( &the_thread->Timer );
   } else
-    _Thread_queue_Release( lock_context );
+    _Thread_queue_Release( the_thread_queue, lock_context );
 
   /*
    *  Global objects with thread queue's should not be operated on from an
@@ -101,16 +92,23 @@ static void _Thread_queue_Requeue_priority(
   );
 }
 
-void _Thread_queue_Enqueue(
+void _Thread_queue_Enqueue_critical(
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread,
   States_Control        state,
-  Watchdog_Interval     timeout
+  Watchdog_Interval     timeout,
+  ISR_lock_Context     *lock_context
 )
 {
-  ISR_lock_Context                 lock_context;
   Thread_blocking_operation_States sync_state;
 
+  _Thread_Lock_set( the_thread, &the_thread_queue->Lock );
+
+  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
+  the_thread->Wait.queue = the_thread_queue;
+
+  _Thread_queue_Release( the_thread_queue, lock_context );
+
 #if defined(RTEMS_MULTIPROCESSING)
   if ( _Thread_MP_Is_receive( the_thread ) && the_thread->receive_packet )
     the_thread = _Thread_MP_Allocate_proxy( state );
@@ -140,7 +138,7 @@ void _Thread_queue_Enqueue(
    * should be completed or the thread has had its blocking condition
    * satisfied before we got here.
    */
-  _Thread_queue_Acquire( &lock_context );
+  _Thread_queue_Acquire( the_thread_queue, lock_context );
 
   sync_state = the_thread_queue->sync_state;
   the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
@@ -155,7 +153,6 @@ void _Thread_queue_Enqueue(
         &the_thread->Object.Node
       );
     } else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
-      _Thread_Lock_set( the_thread, &_Thread_queue_Lock );
       _Thread_Priority_set_change_handler(
         the_thread,
         _Thread_queue_Requeue_priority,
@@ -169,9 +166,8 @@ void _Thread_queue_Enqueue(
       );
     }
 
-    the_thread->Wait.queue = the_thread_queue;
     the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
-    _Thread_queue_Release( &lock_context );
+    _Thread_queue_Release( the_thread_queue, lock_context );
   } else {
     /* Cancel a blocking operation due to ISR */
 
@@ -180,7 +176,7 @@ void _Thread_queue_Enqueue(
         sync_state == THREAD_BLOCKING_OPERATION_SATISFIED
     );
 
-    _Thread_blocking_operation_Finalize( the_thread, &lock_context );
+    _Thread_blocking_operation_Finalize( the_thread_queue, the_thread, lock_context );
   }
 }
 
@@ -190,16 +186,19 @@ void _Thread_queue_Extract_with_return_code(
 )
 {
   Thread_queue_Control *the_thread_queue;
-  ISR_lock_Context lock_context;
+  ISR_lock_Control     *lock;
+  ISR_lock_Context      lock_context;
 
-  _Thread_queue_Acquire( &lock_context );
+  lock = _Thread_Lock_acquire( the_thread, &lock_context );
 
   the_thread_queue = the_thread->Wait.queue;
   if ( the_thread_queue == NULL ) {
-    _Thread_queue_Release( &lock_context );
+    _Thread_Lock_release( lock, &lock_context );
     return;
   }
 
+  _SMP_Assert( lock == &the_thread_queue->Lock );
+
   if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
     _Chain_Extract_unprotected( &the_thread->Object.Node );
   } else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
@@ -208,7 +207,6 @@ void _Thread_queue_Extract_with_return_code(
       &the_thread->RBNode
     );
     _Thread_Priority_restore_default_change_handler( the_thread );
-    _Thread_Lock_restore_default( the_thread );
   }
 
   the_thread->Wait.return_code = return_code;
@@ -218,7 +216,7 @@ void _Thread_queue_Extract_with_return_code(
    *
    * NOTE: This is invoked with interrupts still disabled.
    */
-  _Thread_blocking_operation_Finalize( the_thread, &lock_context );
+  _Thread_blocking_operation_Finalize( the_thread_queue, the_thread, &lock_context );
 }
 
 void _Thread_queue_Extract( Thread_Control *the_thread )
@@ -238,7 +236,7 @@ Thread_Control *_Thread_queue_Dequeue(
   Thread_blocking_operation_States  sync_state;
 
   the_thread = NULL;
-  _Thread_queue_Acquire( &lock_context );
+  _Thread_queue_Acquire( the_thread_queue, &lock_context );
 
   /*
    * Invoke the discipline specific dequeue method.
@@ -255,7 +253,6 @@ Thread_Control *_Thread_queue_Dequeue(
     if ( first ) {
       the_thread = THREAD_RBTREE_NODE_TO_THREAD( first );
       _Thread_Priority_restore_default_change_handler( the_thread );
-      _Thread_Lock_restore_default( the_thread );
     }
   }
 
@@ -270,7 +267,7 @@ Thread_Control *_Thread_queue_Dequeue(
       the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SATISFIED;
       the_thread = _Thread_Executing;
     } else {
-      _Thread_queue_Release( &lock_context );
+      _Thread_queue_Release( the_thread_queue, &lock_context );
       return NULL;
     }
   }
@@ -280,7 +277,7 @@ Thread_Control *_Thread_queue_Dequeue(
    *
    * NOTE: This is invoked with interrupts still disabled.
    */
-  _Thread_blocking_operation_Finalize( the_thread, &lock_context );
+  _Thread_blocking_operation_Finalize( the_thread_queue, the_thread, &lock_context );
 
   return the_thread;
 }
diff --git a/cpukit/score/src/threadqfirst.c b/cpukit/score/src/threadqfirst.c
index 5d97ae156d..f43b9abbd0 100644
--- a/cpukit/score/src/threadqfirst.c
+++ b/cpukit/score/src/threadqfirst.c
@@ -20,20 +20,16 @@
 
 #include <rtems/score/threadqimpl.h>
 #include <rtems/score/chainimpl.h>
-#include <rtems/score/isrlevel.h>
 #include <rtems/score/threadimpl.h>
 
-Thread_Control *_Thread_queue_First(
+Thread_Control *_Thread_queue_First_locked(
   Thread_queue_Control *the_thread_queue
 )
 {
-  ISR_Level       level;
   Thread_Control *thread;
 
   thread = NULL;
 
-  _ISR_Disable( level );
-
   if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
     if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) )
       thread = (Thread_Control *) _Chain_First(&the_thread_queue->Queues.Fifo);
@@ -45,7 +41,19 @@ Thread_Control *_Thread_queue_First(
       thread = THREAD_RBTREE_NODE_TO_THREAD( first );
   }
 
-  _ISR_Enable( level );
-
   return thread;
 }
+
+Thread_Control *_Thread_queue_First(
+  Thread_queue_Control *the_thread_queue
+)
+{
+  Thread_Control   *the_thread;
+  ISR_lock_Context  lock_context;
+
+  _Thread_queue_Acquire( the_thread_queue, &lock_context );
+  the_thread = _Thread_queue_First_locked( the_thread_queue );
+  _Thread_queue_Release( the_thread_queue, &lock_context );
+
+  return the_thread;
+}