score: New thread queue implementation

Use thread wait flags for synchronization.  The enqueue operation is now
part of the initial critical section.  This is the key change and
enables fine grained locking on SMP for objects using a thread queue
like semaphores and message queues.

Update #2273.

8 files changed, 130 insertions, 287 deletions

diff --git a/cpukit/score/src/threadq.c b/cpukit/score/src/threadq.c
index 61832c19a2..cdb99498fe 100644
--- a/cpukit/score/src/threadq.c
+++ b/cpukit/score/src/threadq.c
@@ -52,7 +52,6 @@ void _Thread_queue_Initialize(
   const Thread_queue_Operations *operations;
 
   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" );
 
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index be08ffcbf1..d02f2ee073 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -20,58 +20,28 @@
 
 #include <rtems/score/threadqimpl.h>
 #include <rtems/score/assert.h>
+#include <rtems/score/threaddispatch.h>
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/watchdogimpl.h>
 
-/**
- *  @brief Finalize a blocking operation.
- *
- *  This method is used to finalize a blocking operation that was
- *  satisfied. It may be used with thread queues or any other synchronization
- *  object that uses the blocking states and watchdog times for timeout.
- *
- *  This method will restore the previous ISR disable level during the cancel
- *  operation.  Thus it is an implicit _ISR_Enable().
- *
- *  @param[in] the_thread is the thread whose blocking is canceled
- *  @param[in] lock_context is the previous ISR disable level
- */
-static void _Thread_blocking_operation_Finalize(
-  Thread_queue_Control *the_thread_queue,
-  Thread_Control       *the_thread,
-  ISR_lock_Context     *lock_context
-)
-{
-  /*
-   * The thread is not waiting on anything after this completes.
-   */
-  _Thread_Wait_set_queue( the_thread, NULL );
-  _Thread_Wait_restore_default_operations( the_thread );
+#define THREAD_QUEUE_INTEND_TO_BLOCK \
+  (THREAD_WAIT_CLASS_OBJECT | THREAD_WAIT_STATE_INTEND_TO_BLOCK)
 
-  _Thread_Lock_restore_default( the_thread );
+#define THREAD_QUEUE_BLOCKED \
+  (THREAD_WAIT_CLASS_OBJECT | THREAD_WAIT_STATE_BLOCKED)
 
-  /*
-   *  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( the_thread_queue, lock_context );
-    _Watchdog_Remove_ticks( &the_thread->Timer );
-  } else
-    _Thread_queue_Release( the_thread_queue, lock_context );
-
-  /*
-   *  Global objects with thread queue's should not be operated on from an
-   *  ISR.  But the sync code still must allow short timeouts to be processed
-   *  correctly.
-   */
+#define THREAD_QUEUE_READY_AGAIN \
+  (THREAD_WAIT_CLASS_OBJECT | THREAD_WAIT_STATE_READY_AGAIN)
 
+static void _Thread_queue_Unblock( Thread_Control *the_thread )
+{
+  _Watchdog_Remove_ticks( &the_thread->Timer );
   _Thread_Unblock( the_thread );
 
 #if defined(RTEMS_MULTIPROCESSING)
-  if ( !_Objects_Is_local_id( the_thread->Object.id ) )
+  if ( !_Objects_Is_local_id( the_thread->Object.id ) ) {
     _Thread_MP_Free_proxy( the_thread );
+  }
 #endif
 }
 
@@ -83,13 +53,20 @@ void _Thread_queue_Enqueue_critical(
   ISR_lock_Context     *lock_context
 )
 {
-  Thread_blocking_operation_States sync_state;
+  const Thread_queue_Operations *operations;
+  Per_CPU_Control               *cpu_self;
+  bool                           success;
 
   _Thread_Lock_set( the_thread, &the_thread_queue->Lock );
 
-  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
+  operations = the_thread_queue->operations;
   _Thread_Wait_set_queue( the_thread, the_thread_queue );
+  _Thread_Wait_set_operations( the_thread, operations );
+
+  ( *operations->enqueue )( the_thread_queue, the_thread );
 
+  _Thread_Wait_flags_set( the_thread, THREAD_QUEUE_INTEND_TO_BLOCK );
+  cpu_self = _Thread_Dispatch_disable_critical();
   _Thread_queue_Release( the_thread_queue, lock_context );
 
 #if defined(RTEMS_MULTIPROCESSING)
@@ -105,121 +82,120 @@ void _Thread_queue_Enqueue_critical(
   /*
    *  If the thread wants to timeout, then schedule its timer.
    */
-  if ( timeout ) {
-    _Watchdog_Initialize(
-       &the_thread->Timer,
-       _Thread_queue_Timeout,
-       the_thread->Object.id,
-       NULL
+  if ( timeout != WATCHDOG_NO_TIMEOUT ) {
+    _Thread_Wait_set_timeout_code(
+      the_thread,
+      the_thread_queue->timeout_status
     );
-
+    _Watchdog_Initialize( &the_thread->Timer, _Thread_Timeout, 0, the_thread );
     _Watchdog_Insert_ticks( &the_thread->Timer, timeout );
   }
 
-  /*
-   * Now initiate the enqueuing and checking if the blocking operation
-   * should be completed or the thread has had its blocking condition
-   * satisfied before we got here.
-   */
-  _Thread_queue_Acquire( the_thread_queue, lock_context );
+  success = _Thread_Wait_flags_try_change(
+    the_thread,
+    THREAD_QUEUE_INTEND_TO_BLOCK,
+    THREAD_QUEUE_BLOCKED
+  );
+  if ( !success ) {
+    _Thread_queue_Unblock( the_thread );
+  }
 
-  sync_state = the_thread_queue->sync_state;
-  the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
+  _Thread_Dispatch_enable( cpu_self );
+}
 
-  if ( sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED ) {
-    const Thread_queue_Operations *operations;
+void _Thread_queue_Extract_locked(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread
+)
+{
+  ( *the_thread_queue->operations->extract )( the_thread_queue, the_thread );
 
-    the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
+  _Thread_Wait_set_queue( the_thread, NULL );
+  _Thread_Wait_restore_default_operations( the_thread );
+  _Thread_Lock_restore_default( the_thread );
+}
 
-    operations = the_thread_queue->operations;
-    _Thread_Wait_set_operations( the_thread, operations );
-    ( *operations->enqueue )( the_thread_queue, the_thread );
+void _Thread_queue_Unblock_critical(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread,
+  ISR_lock_Context     *lock_context
+)
+{
+  bool success;
+  bool unblock;
 
-    _Thread_queue_Release( the_thread_queue, lock_context );
+  success = _Thread_Wait_flags_try_change_critical(
+    the_thread,
+    THREAD_QUEUE_INTEND_TO_BLOCK,
+    THREAD_QUEUE_READY_AGAIN
+  );
+  if ( success ) {
+    unblock = false;
   } else {
-    /* Cancel a blocking operation due to ISR */
+    _Assert( _Thread_Wait_flags_get( the_thread ) == THREAD_QUEUE_BLOCKED );
+    _Thread_Wait_flags_set( the_thread, THREAD_QUEUE_READY_AGAIN );
+    unblock = true;
+  }
 
-    _Assert(
-      sync_state == THREAD_BLOCKING_OPERATION_TIMEOUT ||
-        sync_state == THREAD_BLOCKING_OPERATION_SATISFIED
-    );
+  if ( unblock ) {
+    Per_CPU_Control *cpu_self;
 
-    _Thread_blocking_operation_Finalize( the_thread_queue, the_thread, lock_context );
+    cpu_self = _Thread_Dispatch_disable_critical();
+    _Thread_queue_Release( the_thread_queue, lock_context );
+
+    _Thread_queue_Unblock( the_thread );
+
+    _Thread_Dispatch_enable( cpu_self );
+  } else {
+    _Thread_queue_Release( the_thread_queue, lock_context );
   }
 }
 
-void _Thread_queue_Extract_with_return_code(
-  Thread_Control *the_thread,
-  uint32_t        return_code
+void _Thread_queue_Extract_critical(
+  Thread_queue_Control *the_thread_queue,
+  Thread_Control       *the_thread,
+  ISR_lock_Context     *lock_context
 )
 {
-  Thread_queue_Control *the_thread_queue;
-  ISR_lock_Control     *lock;
+  _Thread_queue_Extract_locked( the_thread_queue, the_thread );
+  _Thread_queue_Unblock_critical( the_thread_queue, the_thread, lock_context );
+}
+
+void _Thread_queue_Extract( Thread_Control *the_thread )
+{
   ISR_lock_Context      lock_context;
+  ISR_lock_Control     *lock;
+  Thread_queue_Control *the_thread_queue;
 
   lock = _Thread_Lock_acquire( the_thread, &lock_context );
 
   the_thread_queue = the_thread->Wait.queue;
-  if ( the_thread_queue == NULL ) {
-    _Thread_Lock_release( lock, &lock_context );
-    return;
-  }
-
-  _SMP_Assert( lock == &the_thread_queue->Lock );
-
-  ( *the_thread_queue->operations->extract )( the_thread_queue, the_thread );
 
-  the_thread->Wait.return_code = return_code;
+  if ( the_thread_queue != NULL ) {
+    _SMP_Assert( lock == &the_thread_queue->Lock );
 
-  /*
-   * We found a thread to unblock.
-   *
-   * NOTE: This is invoked with interrupts still disabled.
-   */
-  _Thread_blocking_operation_Finalize( the_thread_queue, the_thread, &lock_context );
-}
-
-void _Thread_queue_Extract( Thread_Control *the_thread )
-{
-  _Thread_queue_Extract_with_return_code(
-    the_thread,
-    the_thread->Wait.return_code
-  );
+    _Thread_queue_Extract_critical( the_thread_queue, the_thread, &lock_context );
+  } else {
+    _Thread_Lock_release( lock, &lock_context );
+  }
 }
 
-Thread_Control *_Thread_queue_Dequeue(
-  Thread_queue_Control *the_thread_queue
-)
+Thread_Control *_Thread_queue_Dequeue( Thread_queue_Control *the_thread_queue )
 {
-  Thread_Control                   *the_thread;
-  ISR_lock_Context                  lock_context;
-  Thread_blocking_operation_States  sync_state;
+  ISR_lock_Context  lock_context;
+  Thread_Control   *the_thread;
 
   _Thread_queue_Acquire( the_thread_queue, &lock_context );
 
-  the_thread = ( *the_thread_queue->operations->dequeue )( the_thread_queue );
-  if ( the_thread == NULL ) {
-    /*
-     * We did not find a thread to unblock in the queue.  Maybe the executing
-     * thread is about to block on this thread queue.
-     */
-    sync_state = the_thread_queue->sync_state;
-    if ( (sync_state == THREAD_BLOCKING_OPERATION_TIMEOUT) ||
-         (sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) ) {
-      the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SATISFIED;
-      the_thread = _Thread_Executing;
-    } else {
-      _Thread_queue_Release( the_thread_queue, &lock_context );
-      return NULL;
-    }
-  }
+  the_thread = _Thread_queue_First_locked( the_thread_queue );
 
-  /*
-   * We found a thread to unblock.
-   *
-   * NOTE: This is invoked with interrupts still disabled.
-   */
-  _Thread_blocking_operation_Finalize( the_thread_queue, the_thread, &lock_context );
+  if ( the_thread != NULL ) {
+    _SMP_Assert( the_thread->Lock.current == &the_thread_queue->Lock );
+
+    _Thread_queue_Extract_critical( the_thread_queue, the_thread, &lock_context );
+  } else {
+    _Thread_queue_Release( the_thread_queue, &lock_context );
+  }
 
   return the_thread;
 }
diff --git a/cpukit/score/src/threadqfirst.c b/cpukit/score/src/threadqfirst.c
index 553b28bf62..c46f005c3e 100644
--- a/cpukit/score/src/threadqfirst.c
+++ b/cpukit/score/src/threadqfirst.c
@@ -20,13 +20,6 @@
 
 #include <rtems/score/threadqimpl.h>
 
-Thread_Control *_Thread_queue_First_locked(
-  Thread_queue_Control *the_thread_queue
-)
-{
-  return ( *the_thread_queue->operations->first )( the_thread_queue );
-}
-
 Thread_Control *_Thread_queue_First(
   Thread_queue_Control *the_thread_queue
 )
diff --git a/cpukit/score/src/threadqflush.c b/cpukit/score/src/threadqflush.c
index 1abe8aca3e..d37b9e8ae7 100644
--- a/cpukit/score/src/threadqflush.c
+++ b/cpukit/score/src/threadqflush.c
@@ -31,14 +31,30 @@ void _Thread_queue_Flush(
   uint32_t                    status
 )
 {
-  Thread_Control *the_thread;
+  ISR_lock_Context  lock_context;
+  Thread_Control   *the_thread;
+
+  _Thread_queue_Acquire( the_thread_queue, &lock_context );
+
+  while ( (the_thread = _Thread_queue_First_locked( the_thread_queue ) ) ) {
+#if defined(RTEMS_MULTIPROCESSING)
+    if ( _Objects_Is_local_id( the_thread->Object.id ) )
+#endif
+      the_thread->Wait.return_code = status;
+
+    _Thread_queue_Extract_critical(
+      the_thread_queue,
+      the_thread,
+      &lock_context
+    );
 
-  while ( (the_thread = _Thread_queue_Dequeue( the_thread_queue )) ) {
 #if defined(RTEMS_MULTIPROCESSING)
     if ( !_Objects_Is_local_id( the_thread->Object.id ) )
       ( *remote_extract_callout )( the_thread );
-    else
 #endif
-      the_thread->Wait.return_code = status;
+
+    _Thread_queue_Acquire( the_thread_queue, &lock_context );
   }
+
+  _Thread_queue_Release( the_thread_queue, &lock_context );
 }
diff --git a/cpukit/score/src/threadqops.c b/cpukit/score/src/threadqops.c
index 9958ed667c..2967a0efc3 100644
--- a/cpukit/score/src/threadqops.c
+++ b/cpukit/score/src/threadqops.c
@@ -55,16 +55,6 @@ static void _Thread_queue_FIFO_enqueue(
   );
 }
 
-static Thread_Control *_Thread_queue_FIFO_dequeue(
-  Thread_queue_Control *the_thread_queue
-)
-{
-  Chain_Control *fifo = &the_thread_queue->Queues.Fifo;
-
-  return _Chain_Is_empty( fifo ) ?
-    NULL : THREAD_CHAIN_NODE_TO_THREAD( _Chain_Get_first_unprotected( fifo ) );
-}
-
 static void _Thread_queue_FIFO_extract(
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread
@@ -121,17 +111,6 @@ static void _Thread_queue_Priority_enqueue(
   );
 }
 
-static Thread_Control *_Thread_queue_Priority_dequeue(
-  Thread_queue_Control *the_thread_queue
-)
-{
-  RBTree_Node *first;
-
-  first = _RBTree_Get( &the_thread_queue->Queues.Priority, RBT_LEFT );
-
-  return first != NULL ? THREAD_RBTREE_NODE_TO_THREAD( first ) : NULL;
-}
-
 static void _Thread_queue_Priority_extract(
   Thread_queue_Control *the_thread_queue,
   Thread_Control       *the_thread
@@ -168,7 +147,6 @@ const Thread_queue_Operations _Thread_queue_Operations_FIFO = {
   .priority_change = _Thread_queue_Do_nothing_priority_change,
   .initialize = _Thread_queue_FIFO_initialize,
   .enqueue = _Thread_queue_FIFO_enqueue,
-  .dequeue = _Thread_queue_FIFO_dequeue,
   .extract = _Thread_queue_FIFO_extract,
   .first = _Thread_queue_FIFO_first
 };
@@ -177,7 +155,6 @@ const Thread_queue_Operations _Thread_queue_Operations_priority = {
   .priority_change = _Thread_queue_Priority_priority_change,
   .initialize = _Thread_queue_Priority_initialize,
   .enqueue = _Thread_queue_Priority_enqueue,
-  .dequeue = _Thread_queue_Priority_dequeue,
   .extract = _Thread_queue_Priority_extract,
   .first = _Thread_queue_Priority_first
 };
diff --git a/cpukit/score/src/threadqprocesstimeout.c b/cpukit/score/src/threadqprocesstimeout.c
deleted file mode 100644
index dbb8f5ce45..0000000000
--- a/cpukit/score/src/threadqprocesstimeout.c
+++ /dev/null
@@ -1,80 +0,0 @@
-/**
- * @file
- * 
- * @brief Thread Queue Handler Process Timeout Handler
- * @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2008.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/threadimpl.h>
-
-void _Thread_queue_Process_timeout(
-  Thread_Control *the_thread
-)
-{
-  Thread_queue_Control *the_thread_queue;
-  ISR_Level             level;
-
-  /*
-   *  If the_thread_queue is not synchronized, then it is either
-   *  "nothing happened", "timeout", or "satisfied".   If the_thread
-   *  is the executing thread, then it is in the process of blocking
-   *  and it is the thread which is responsible for the synchronization
-   *  process.
-   *
-   *  If it is not satisfied, then it is "nothing happened" and
-   *  this is the "timeout" transition.  After a request is satisfied,
-   *  a timeout is not allowed to occur.
-   */
-
-  _ISR_Disable( level );
-  the_thread_queue = the_thread->Wait.queue;
-  if ( the_thread_queue != NULL ) {
-    if ( the_thread_queue->sync_state != THREAD_BLOCKING_OPERATION_SYNCHRONIZED &&
-         _Thread_Is_executing( the_thread ) ) {
-      if ( the_thread_queue->sync_state != THREAD_BLOCKING_OPERATION_SATISFIED ) {
-        the_thread->Wait.return_code = the_thread_queue->timeout_status;
-        the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_TIMEOUT;
-      }
-      _ISR_Enable( level );
-    } else {
-      _ISR_Enable( level );
-
-      /*
-       * After we enable interrupts here, a lot may happen in the meantime,
-       * e.g. nested interrupts may release the resource that times out here.
-       * So we enter _Thread_queue_Extract() speculatively.  Inside this
-       * function we check the actual status under ISR disable protection.
-       * This ensures that exactly one executing context performs the extract
-       * operation (other parties may call _Thread_queue_Dequeue()).  If this
-       * context won, then we have a timeout.
-       *
-       * We can use the_thread_queue pointer here even if
-       * the_thread->Wait.queue is already set to NULL since the extract
-       * operation will only use the thread queue discipline to select the
-       * right extract operation.  The timeout status is set during thread
-       * queue initialization.
-       */
-      _Thread_queue_Extract_with_return_code(
-        the_thread,
-        the_thread_queue->timeout_status
-      );
-    }
-  } else {
-    _ISR_Enable( level );
-  }
-}
-
diff --git a/cpukit/score/src/threadqtimeout.c b/cpukit/score/src/threadqtimeout.c
deleted file mode 100644
index fcacd1c781..0000000000
--- a/cpukit/score/src/threadqtimeout.c
+++ /dev/null
@@ -1,44 +0,0 @@
-/**
- *  @file
- *
- *  @brief Thread Queue Timeout
- *  @ingroup ScoreThreadQ
- */
-
-/*
- *  COPYRIGHT (c) 1989-2008.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/threadimpl.h>
-
-void _Thread_queue_Timeout(
-  Objects_Id  id,
-  void       *ignored __attribute__((unused))
-)
-{
-  Thread_Control       *the_thread;
-  Objects_Locations     location;
-
-  the_thread = _Thread_Get( id, &location );
-  switch ( location ) {
-    case OBJECTS_ERROR:
-#if defined(RTEMS_MULTIPROCESSING)
-    case OBJECTS_REMOTE:  /* impossible */
-#endif
-      break;
-    case OBJECTS_LOCAL:
-      _Thread_queue_Process_timeout( the_thread );
-      _Objects_Put_without_thread_dispatch( &the_thread->Object );
-      break;
-  }
-}
diff --git a/cpukit/score/src/threadtimeout.c b/cpukit/score/src/threadtimeout.c
index 300beb5219..f69bc35ea2 100644
--- a/cpukit/score/src/threadtimeout.c
+++ b/cpukit/score/src/threadtimeout.c
@@ -76,5 +76,11 @@ void _Thread_Timeout( Objects_Id id, void *arg )
 
   if ( unblock ) {
     _Thread_Unblock( the_thread );
+
+#if defined(RTEMS_MULTIPROCESSING)
+    if ( !_Objects_Is_local_id( the_thread->Object.id ) ) {
+      _Thread_MP_Free_proxy( the_thread );
+    }
+#endif
   }
 }