From 0b590858ac014ba7cbedeae2b1914c57363dd6a7 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Wed, 20 Apr 2016 06:59:16 +0200
Subject: score: Use _Thread_queue_Flush_critical for futex

---
 cpukit/score/src/futex.c | 98 ++++++++++++++----------------------------------
 1 file changed, 28 insertions(+), 70 deletions(-)

(limited to 'cpukit/score/src/futex.c')

diff --git a/cpukit/score/src/futex.c b/cpukit/score/src/futex.c
index 73d48fd4df..59a625cb29 100644
--- a/cpukit/score/src/futex.c
+++ b/cpukit/score/src/futex.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2015, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -103,99 +103,57 @@ int _Futex_Wait( struct _Futex_Control *_futex, int *uaddr, int val )
   return eno;
 }
 
-/*
- * Use a noinline function to force the compiler to set up and tear down the
- * large stack frame only in the slow case.
- */
-static __attribute__((noinline)) int _Futex_Wake_slow(
-  Futex_Control      *futex,
-  int                 count,
-  Thread_queue_Heads *heads,
+typedef struct {
+  ISR_lock_Context Base;
+  int              count;
+} Futex_Lock_context;
+
+static Thread_Control *_Futex_Flush_filter(
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue,
   ISR_lock_Context   *lock_context
 )
 {
-  Chain_Control  unblock;
-  Chain_Node    *node;
-  Chain_Node    *tail;
-  int            woken;
-
-  woken = 0;
-  _Chain_Initialize_empty( &unblock );
-
-  while ( count > 0 && heads != NULL ) {
-    const Thread_queue_Operations *operations;
-    Thread_Control                *first;
-    bool                           do_unblock;
+  Futex_Lock_context *futex_lock_context;
 
-    operations = FUTEX_TQ_OPERATIONS;
-    first = ( *operations->first )( heads );
+  futex_lock_context = (Futex_Lock_context *) lock_context;
 
-    do_unblock = _Thread_queue_Extract_locked(
-      &futex->Queue.Queue,
-      operations,
-      first,
-      NULL,
-      0
-    );
-    if (do_unblock) {
-      _Chain_Append_unprotected( &unblock, &first->Wait.Node.Chain );
-    }
-
-    ++woken;
-    --count;
-    heads = futex->Queue.Queue.heads;
+  if ( futex_lock_context->count <= 0 ) {
+    return NULL;
   }
 
-  node = _Chain_First( &unblock );
-  tail = _Chain_Tail( &unblock );
-
-  if ( node != tail ) {
-    Per_CPU_Control *cpu_self;
-
-    cpu_self = _Thread_Dispatch_disable_critical( lock_context );
-    _Futex_Queue_release( futex, lock_context );
+  --futex_lock_context->count;
 
-    do {
-      Thread_Control *thread;
-      Chain_Node     *next;
-
-      next = _Chain_Next( node );
-      thread = THREAD_CHAIN_NODE_TO_THREAD( node );
-      _Thread_Unblock( thread );
-
-      node = next;
-    } while ( node != tail );
-
-    _Thread_Dispatch_enable( cpu_self );
-  } else {
-    _Futex_Queue_release( futex, lock_context );
-  }
-
-  return woken;
+  return the_thread;
 }
 
 int _Futex_Wake( struct _Futex_Control *_futex, int count )
 {
   Futex_Control      *futex;
-  ISR_lock_Context    lock_context;
-  Thread_queue_Heads *heads;
+  Futex_Lock_context  lock_context;
 
   futex = _Futex_Get( _futex );
-  _Futex_Queue_acquire( futex, &lock_context );
+  _Futex_Queue_acquire( futex, &lock_context.Base );
 
   /*
    * For some synchronization objects like barriers the _Futex_Wake() must be
    * called in the fast path.  Normally there are no threads on the queue, so
    * check this condition early.
    */
-  heads = futex->Queue.Queue.heads;
-  if ( __predict_true( heads == NULL ) ) {
-    _Futex_Queue_release( futex, &lock_context );
-
+  if ( __predict_true( _Thread_queue_Is_empty( &futex->Queue.Queue ) ) ) {
+    _Futex_Queue_release( futex, &lock_context.Base );
     return 0;
   }
 
-  return _Futex_Wake_slow( futex, count, heads, &lock_context );
+  lock_context.count = count;
+  return (int) _Thread_queue_Flush_critical(
+    &futex->Queue.Queue,
+    FUTEX_TQ_OPERATIONS,
+    _Futex_Flush_filter,
+    NULL,
+    0,
+    &lock_context.Base
+  );
 }
 
 #endif /* HAVE_STRUCT__THREAD_QUEUE_QUEUE */
-- 
cgit v1.2.3