2 files changed, 48 insertions, 20 deletions

diff --git a/src/internal/pthread_impl.h b/src/internal/pthread_impl.h
index 304bf98d..049f5dfb 100644
--- a/src/internal/pthread_impl.h
+++ b/src/internal/pthread_impl.h
@@ -68,10 +68,10 @@ struct __timer {
 #define _rw_readers __u.__i[1]
 #define _rw_waiters __u.__i[2]
 #define _rw_owner __u.__i[3]
-#define _b_count __u.__i[0]
-#define _b_limit __u.__i[1]
-#define _b_left __u.__i[2]
-#define _b_waiters __u.__i[3]
+#define _b_inst __u.__p[0]
+#define _b_limit __u.__i[2]
+#define _b_lock __u.__i[3]
+#define _b_waiters __u.__i[4]
 
 #include "pthread_arch.h"
 
diff --git a/src/thread/pthread_barrier_wait.c b/src/thread/pthread_barrier_wait.c
index 7bfadb95..1e638d96 100644
--- a/src/thread/pthread_barrier_wait.c
+++ b/src/thread/pthread_barrier_wait.c
@@ -1,31 +1,59 @@
 #include "pthread_impl.h"
 
+struct instance
+{
+	int count;
+	int last;
+	int waiters;
+	int finished;
+};
+
 int pthread_barrier_wait(pthread_barrier_t *b)
 {
-	int cur;
+	int limit = b->_b_limit;
+	struct instance *inst;
 
 	/* Trivial case: count was set at 1 */
-	if (!b->_b_limit) return PTHREAD_BARRIER_SERIAL_THREAD;
+	if (!limit) return PTHREAD_BARRIER_SERIAL_THREAD;
 
-	/* Wait for anyone still suspended at previous use of barrier */
-	while ((cur=b->_b_left))
-		__wait(&b->_b_left, &b->_b_waiters, cur, 0);
+	/* Otherwise we need a lock on the barrier object */
+	while (a_swap(&b->_b_lock, 1))
+		__wait(&b->_b_lock, &b->_b_waiters, 1, 0);
+	inst = b->_b_inst;
 
-	/* If we are the last to reach barrier, reset it and wake others */
-	if (a_fetch_add(&b->_b_count, 1) == b->_b_limit) {
-		b->_b_left = b->_b_limit;
-		b->_b_count = 0;
-		__wake(&b->_b_count, -1, 0);
+	/* First thread to enter the barrier becomes the "instance owner" */
+	if (!inst) {
+		struct instance new_inst = { 0 };
+		int spins = 10000;
+		b->_b_inst = inst = &new_inst;
+		a_store(&b->_b_lock, 0);
+		if (b->_b_waiters) __wake(&b->_b_lock, 1, 0);
+		while (spins-- && !inst->finished)
+			a_spin();
+		a_inc(&inst->finished);
+		while (inst->finished == 1)
+			__syscall(SYS_futex, &inst->finished, FUTEX_WAIT,1,0);
 		return PTHREAD_BARRIER_SERIAL_THREAD;
 	}
 
-	/* Wait for our peers to reach the barrier */
-	while ((cur=b->_b_count))
-		__wait(&b->_b_count, 0, cur, 0);
+	/* Last thread to enter the barrier wakes all non-instance-owners */
+	if (++inst->count == limit) {
+a_spin(); a_spin();
+		b->_b_inst = 0;
+		a_store(&b->_b_lock, 0);
+		if (b->_b_waiters) __wake(&b->_b_lock, 1, 0);
+		a_store(&inst->last, 1);
+		if (inst->waiters)
+			__wake(&inst->last, -1, 0);
+	} else {
+		a_store(&b->_b_lock, 0);
+		if (b->_b_waiters) __wake(&b->_b_lock, 1, 0);
+		__wait(&inst->last, &inst->waiters, 0, 0);
+	}
 
-	/* If we're the last to wake up and barrier is awaiting reuse */
-	if (a_fetch_add(&b->_b_left, -1) == 1 && b->_b_waiters)
-		__wake(&b->_b_left, -1, 0);
+	/* Last thread to exit the barrier wakes the instance owner */
+	if (a_fetch_add(&inst->count,-1)==1 && a_fetch_add(&inst->finished,1))
+		__wake(&inst->finished, 1, 0);
 
 	return 0;
 }