score: Introduce _Thread_queue_Flush_critical()

Replace _Thread_queue_Flush() with _Thread_queue_Flush_critical() and
add a filter function for customization of the thread queue flush
operation.

Update #2555.

6 files changed, 140 insertions, 35 deletions

diff --git a/cpukit/score/src/corebarrier.c b/cpukit/score/src/corebarrier.c
index 5313a0f6c8..3cb7906289 100644
--- a/cpukit/score/src/corebarrier.c
+++ b/cpukit/score/src/corebarrier.c
@@ -19,7 +19,6 @@
 #endif
 
 #include <rtems/score/corebarrierimpl.h>
-#include <rtems/score/threadqimpl.h>
 
 void _CORE_barrier_Initialize(
   CORE_barrier_Control       *the_barrier,
@@ -32,3 +31,14 @@ void _CORE_barrier_Initialize(
 
   _Thread_queue_Initialize( &the_barrier->Wait_queue );
 }
+
+Thread_Control *_CORE_barrier_Was_deleted(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  the_thread->Wait.return_code = CORE_BARRIER_WAS_DELETED;
+
+  return the_thread;
+}
diff --git a/cpukit/score/src/coremsgclose.c b/cpukit/score/src/coremsgclose.c
index 1511f83b0a..7184b11bfa 100644
--- a/cpukit/score/src/coremsgclose.c
+++ b/cpukit/score/src/coremsgclose.c
@@ -21,6 +21,17 @@
 #include <rtems/score/coremsgimpl.h>
 #include <rtems/score/wkspace.h>
 
+static Thread_Control *_CORE_message_queue_Was_deleted(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  the_thread->Wait.return_code = CORE_MESSAGE_QUEUE_STATUS_WAS_DELETED;
+
+  return the_thread;
+}
+
 void _CORE_message_queue_Do_close(
   CORE_message_queue_Control *the_message_queue
 #if defined(RTEMS_MULTIPROCESSING)
@@ -30,17 +41,21 @@ void _CORE_message_queue_Do_close(
 #endif
 )
 {
+  ISR_lock_Context lock_context;
+
   /*
    *  This will flush blocked threads whether they were blocked on
    *  a send or receive.
    */
 
-  _Thread_queue_Flush(
-    &the_message_queue->Wait_queue,
+  _CORE_message_queue_Acquire( the_message_queue, &lock_context );
+  _Thread_queue_Flush_critical(
+    &the_message_queue->Wait_queue.Queue,
     the_message_queue->operations,
-    CORE_MESSAGE_QUEUE_STATUS_WAS_DELETED,
+    _CORE_message_queue_Was_deleted,
     mp_callout,
-    mp_id
+    mp_id,
+    &lock_context
   );
 
   (void) _Workspace_Free( the_message_queue->message_buffers );
diff --git a/cpukit/score/src/coremsgflush.c b/cpukit/score/src/coremsgflush.c
index f67dcf28eb..38f26b7b87 100644
--- a/cpukit/score/src/coremsgflush.c
+++ b/cpukit/score/src/coremsgflush.c
@@ -41,7 +41,7 @@ uint32_t   _CORE_message_queue_Flush(
    *
    *  (1) The thread queue of pending senders is a logical extension
    *  of the pending message queue.  In this case, it should be
-   *  flushed using the _Thread_queue_Flush() service with a status
+   *  flushed using the _Thread_queue_Flush_critical() service with a status
    *  such as CORE_MESSAGE_QUEUE_SENDER_FLUSHED (which currently does
    *  not exist).  This can be implemented without changing the "big-O"
    *  of the message flushing part of the routine.
diff --git a/cpukit/score/src/coremutex.c b/cpukit/score/src/coremutex.c
index 88d487ca4b..75e0c49592 100644
--- a/cpukit/score/src/coremutex.c
+++ b/cpukit/score/src/coremutex.c
@@ -96,3 +96,25 @@ CORE_mutex_Status _CORE_mutex_Initialize(
 
   return CORE_MUTEX_STATUS_SUCCESSFUL;
 }
+
+Thread_Control *_CORE_mutex_Was_deleted(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  the_thread->Wait.return_code = CORE_MUTEX_WAS_DELETED;
+
+  return the_thread;
+}
+
+Thread_Control *_CORE_mutex_Unsatisfied_nowait(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  the_thread->Wait.return_code = CORE_MUTEX_STATUS_UNSATISFIED_NOWAIT;
+
+  return the_thread;
+}
diff --git a/cpukit/score/src/coresem.c b/cpukit/score/src/coresem.c
index 2bdd81c76a..02a3837410 100644
--- a/cpukit/score/src/coresem.c
+++ b/cpukit/score/src/coresem.c
@@ -36,3 +36,25 @@ void _CORE_semaphore_Initialize(
     the_semaphore->operations = &_Thread_queue_Operations_FIFO;
   }
 }
+
+Thread_Control *_CORE_semaphore_Was_deleted(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  the_thread->Wait.return_code = CORE_SEMAPHORE_WAS_DELETED;
+
+  return the_thread;
+}
+
+Thread_Control *_CORE_semaphore_Unsatisfied_nowait(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
+  ISR_lock_Context   *lock_context
+)
+{
+  the_thread->Wait.return_code = CORE_SEMAPHORE_STATUS_UNSATISFIED_NOWAIT;
+
+  return the_thread;
+}
diff --git a/cpukit/score/src/threadqflush.c b/cpukit/score/src/threadqflush.c
index c50831466d..0ce639eb68 100644
--- a/cpukit/score/src/threadqflush.c
+++ b/cpukit/score/src/threadqflush.c
@@ -18,46 +18,82 @@
 #include "config.h"
 #endif
 
-#include <rtems/score/threadqimpl.h>
-#include <rtems/score/objectimpl.h>
+#include <rtems/score/threadimpl.h>
 
-void _Thread_queue_Do_flush(
-  Thread_queue_Control          *the_thread_queue,
+size_t _Thread_queue_Do_flush_critical(
+  Thread_queue_Queue            *queue,
   const Thread_queue_Operations *operations,
-  uint32_t                       status
+  Thread_queue_Flush_filter      filter,
 #if defined(RTEMS_MULTIPROCESSING)
-  ,
   Thread_queue_MP_callout        mp_callout,
-  Objects_Id                     mp_id
+  Objects_Id                     mp_id,
 #endif
+  ISR_lock_Context              *lock_context
 )
 {
-  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,
-        operations
-      )
-    )
-  ) {
-    the_thread->Wait.return_code = status;
-
-    _Thread_queue_Extract_critical(
-      &the_thread_queue->Queue,
+  size_t         flushed;
+  Chain_Control  unblock;
+  Chain_Node    *node;
+  Chain_Node    *tail;
+
+  flushed = 0;
+  _Chain_Initialize_empty( &unblock );
+
+  while ( true ) {
+    Thread_queue_Heads *heads;
+    Thread_Control     *first;
+    bool                do_unblock;
+
+    heads = queue->heads;
+    if ( heads == NULL ) {
+      break;
+    }
+
+    first = ( *operations->first )( heads );
+    first = ( *filter )( first, queue, lock_context );
+    if ( first == NULL ) {
+      break;
+    }
+
+    do_unblock = _Thread_queue_Extract_locked(
+      queue,
       operations,
-      the_thread,
+      first,
       mp_callout,
-      mp_id,
-      &lock_context
+      mp_id
     );
+    if ( do_unblock ) {
+      _Chain_Append_unprotected( &unblock, &first->Wait.Node.Chain );
+    }
+
+    ++flushed;
+  }
+
+  node = _Chain_First( &unblock );
+  tail = _Chain_Tail( &unblock );
+
+  if ( node != tail ) {
+    Per_CPU_Control *cpu_self;
+
+    cpu_self = _Thread_Dispatch_disable_critical( lock_context );
+    _Thread_queue_Queue_release( queue, lock_context );
+
+    do {
+      Thread_Control *the_thread;
+      Chain_Node     *next;
+
+      next = _Chain_Next( node );
+      the_thread = THREAD_CHAIN_NODE_TO_THREAD( node );
+      _Thread_Timer_remove( the_thread );
+      _Thread_Unblock( the_thread );
+
+      node = next;
+    } while ( node != tail );
 
-    _Thread_queue_Acquire( the_thread_queue, &lock_context );
+    _Thread_Dispatch_enable( cpu_self );
+  } else {
+    _Thread_queue_Queue_release( queue, lock_context );
   }
 
-  _Thread_queue_Release( the_thread_queue, &lock_context );
+  return flushed;
 }