kernel: improve contended push lock wakeups

kernel-src/lib/pushlock.c

@@ -8,22 +8,38 @@
 #define CAS_RELEASE(ptr, saved, new) \
 	__atomic_compare_exchange_n(ptr, &saved, new, false, __ATOMIC_RELEASE, __ATOMIC_RELAXED)
 
-typedef struct pushlock_wait_block_t {
-	struct pushlock_wait_block_t *next;
-	size_t shared_count;
-	semaphore_t semaphore;
-} __attribute__((aligned(8))) pushlock_wait_block_t;
-
 #define PUSHLOCK_FLAGS_ACQUIRED 1
 #define PUSHLOCK_FLAGS_EXCLUSIVE 2
 #define PUSHLOCK_FLAGS_CONTENDED 4
-#define PUSHLOCK_MASK_FLAGS 0x7
-#define PUSHLOCK_MASK_POINTER_SHARED_COUNT (0xfffffffffffffff8lu)
 
-#define PUSHLOCK_GET_POINTER(x) ((pushlock_wait_block_t *)((x) & PUSHLOCK_MASK_POINTER_SHARED_COUNT))
+#define PUSHLOCK_MASK_FLAGS \
-#define PUSHLOCK_GET_SHARED_COUNT(x) ((uintptr_t)PUSHLOCK_GET_POINTER(x) >> 3)
+	(PUSHLOCK_FLAGS_ACQUIRED | PUSHLOCK_FLAGS_EXCLUSIVE | PUSHLOCK_FLAGS_CONTENDED)
-#define PUSHLOCK_INCREMENT_SHARED_COUNT(x) ((x) + 0x8)
+
-#define PUSHLOCK_DECREMENT_SHARED_COUNT(x) ((x) - 0x8)
+#define PUSHLOCK_VALUE_ALIGNMENT (PUSHLOCK_MASK_FLAGS + 1)
+#define PUSHLOCK_MASK_POINTER_SHARED_COUNT (~(pushlock_t)PUSHLOCK_MASK_FLAGS)
+
+_Static_assert(
+	(PUSHLOCK_VALUE_ALIGNMENT & (PUSHLOCK_VALUE_ALIGNMENT - 1)) == 0,
+	"push lock flags must occupy contiguous low bits");
+
+typedef struct pushlock_wait_block_t {
+	struct pushlock_wait_block_t *next;
+	size_t shared_count;
+	bool exclusive;
+	semaphore_t semaphore;
+} __attribute__((aligned(PUSHLOCK_VALUE_ALIGNMENT))) pushlock_wait_block_t;
+
+#define PUSHLOCK_GET_POINTER(x) \
+	((pushlock_wait_block_t *)((x) & PUSHLOCK_MASK_POINTER_SHARED_COUNT))
+
+#define PUSHLOCK_GET_SHARED_COUNT(x) \
+	(((pushlock_t)(x) & PUSHLOCK_MASK_POINTER_SHARED_COUNT) / PUSHLOCK_VALUE_ALIGNMENT)
+
+#define PUSHLOCK_INCREMENT_SHARED_COUNT(x) \
+	((x) + PUSHLOCK_VALUE_ALIGNMENT)
+
+#define PUSHLOCK_DECREMENT_SHARED_COUNT(x) \
+	((x) - PUSHLOCK_VALUE_ALIGNMENT)
 
 bool pushlock_try_acquire_exclusive(pushlock_t *pushlock) {
 retry_lock:
@@ -47,7 +63,7 @@ retry_lock:
 		return;
 
 	if ((saved_value & PUSHLOCK_FLAGS_ACQUIRED) == 0) {
- 		if (!CAS_ACQUIRE(pushlock, saved_value, saved_value | PUSHLOCK_FLAGS_ACQUIRED | PUSHLOCK_FLAGS_EXCLUSIVE))
+		if (!CAS_ACQUIRE(pushlock, saved_value, saved_value | PUSHLOCK_FLAGS_ACQUIRED | PUSHLOCK_FLAGS_EXCLUSIVE))
 			goto retry_lock;
 
 		return;
@@ -55,6 +71,7 @@ retry_lock:
 
 	pushlock_wait_block_t wait_block;
 	SEMAPHORE_INIT(&wait_block.semaphore, 0);
+	wait_block.exclusive = true;
 
 	if (saved_value & PUSHLOCK_FLAGS_CONTENDED) {
 		wait_block.next = PUSHLOCK_GET_POINTER(saved_value);
@@ -89,7 +106,7 @@ retry_lock:
 	if (saved_value & PUSHLOCK_FLAGS_ACQUIRED)
 		return false;
 
-	// try to acquire as exclusive
+	// The waiter pointer occupies the shared count field, so acquire as a single owner while contended.
 	if (!CAS_ACQUIRE(pushlock, saved_value, saved_value | PUSHLOCK_FLAGS_ACQUIRED | PUSHLOCK_FLAGS_EXCLUSIVE))
 		goto retry_lock;
 
@@ -105,6 +122,7 @@ retry_lock:
 	if ((saved_value & PUSHLOCK_FLAGS_EXCLUSIVE) || ((saved_value & PUSHLOCK_FLAGS_ACQUIRED) && (saved_value & PUSHLOCK_FLAGS_CONTENDED))) {
 		pushlock_wait_block_t wait_block;
 		SEMAPHORE_INIT(&wait_block.semaphore, 0);
+		wait_block.exclusive = false;
 
 		wait_block.next = PUSHLOCK_GET_POINTER(saved_value);
 		__atomic_store_n(&wait_block.shared_count, 0, __ATOMIC_RELAXED);
@@ -126,9 +144,94 @@ retry_lock:
 		goto retry_lock;
 }
 
-// TODO: if we are releasing to a shared waiter, we should wake up every shared waiter
+static void pushlock_signal_waiters(pushlock_wait_block_t *wait_block) {
+	pushlock_wait_block_t *next_wait_block;
+	pushlock_wait_block_t *previous_wait_block = NULL;
+
+	// Reverse the detached chain to signal older waiters first.
+	while (wait_block) {
+		next_wait_block = wait_block->next;
+		wait_block->next = previous_wait_block;
+		previous_wait_block = wait_block;
+		wait_block = next_wait_block;
+	}
+
+	wait_block = previous_wait_block;
+
+	while (wait_block) {
+		// The wait block is stack allocated, so save next before waking.
+		next_wait_block = wait_block->next;
+		semaphore_signal(&wait_block->semaphore);
+		wait_block = next_wait_block;
+	}
+}
+
+static void pushlock_wake_waiters(pushlock_t *pushlock) {
+	pushlock_t saved_value;
+	pushlock_t new_value;
+	pushlock_wait_block_t *head_wait_block;
+	pushlock_wait_block_t *penultimate_wait_block;
+	pushlock_wait_block_t *oldest_wait_block;
+
+	for (;;) {
+		// Wait blocks are published with release semantics. Acquire
+		// the current head before traversing the chain.
+		saved_value = __atomic_load_n(pushlock, __ATOMIC_ACQUIRE);
+
+#ifdef DEBUG_LOCKS
+		__assert(saved_value & PUSHLOCK_FLAGS_ACQUIRED);
+		__assert(saved_value & PUSHLOCK_FLAGS_CONTENDED);
+#endif
+
+		head_wait_block = PUSHLOCK_GET_POINTER(saved_value);
+		penultimate_wait_block = NULL;
+		oldest_wait_block = head_wait_block;
+
+		while (oldest_wait_block->next) {
+			penultimate_wait_block = oldest_wait_block;
+			oldest_wait_block = oldest_wait_block->next;
+		}
+
+		// Wake an exclusive oldest waiter alone, leaving newer waiters queued.
+		if (oldest_wait_block->exclusive && penultimate_wait_block != NULL) {
+			// New waiters can only be inserted at the head, so they
+			// can't modify this tail link.
+			penultimate_wait_block->next = NULL;
+
+			for (;;) {
+				new_value =
+					(saved_value & PUSHLOCK_MASK_POINTER_SHARED_COUNT) |
+					PUSHLOCK_FLAGS_CONTENDED;
+
+				if (CAS_RELEASE(pushlock, saved_value, new_value)) {
+					break;
+				}
+
+				// A waiter was inserted at the head. The failed CAS
+				// updated saved_value, while the detached tail stays
+				// unchanged.
+			}
+
+			semaphore_signal(&oldest_wait_block->semaphore);
+			return;
+		}
+
+		// A single waiter, or a chain whose oldest waiter is shared, can be
+		// detached completely. Woken waiters retry acquisition without handoff.
+		if (CAS_RELEASE(pushlock, saved_value, 0)) {
+			pushlock_signal_waiters(head_wait_block);
+			return;
+		}
+
+		// The chain changed before it could be detached. Restart with
+		// an acquire load before traversing it again.
+	}
+}
+
 void pushlock_release_exclusive(pushlock_t *pushlock) {
-	pushlock_t saved_value = PUSHLOCK_FLAGS_ACQUIRED | PUSHLOCK_FLAGS_EXCLUSIVE;
+	pushlock_t saved_value;
+
+	saved_value = PUSHLOCK_FLAGS_ACQUIRED | PUSHLOCK_FLAGS_EXCLUSIVE;
 	if (CAS_RELEASE(pushlock, saved_value, 0))
 		return;
 
@@ -137,63 +240,23 @@ void pushlock_release_exclusive(pushlock_t *pushlock) {
 	__assert(saved_value & PUSHLOCK_FLAGS_EXCLUSIVE);
 #endif
 
-	__atomic_thread_fence(__ATOMIC_ACQUIRE);
+	pushlock_wake_waiters(pushlock);
-	pushlock_wait_block_t *wait_block = PUSHLOCK_GET_POINTER(saved_value);
-	if (wait_block->next == NULL) {
-		if (CAS_RELEASE(pushlock, saved_value, 0)) {
-			semaphore_signal(&wait_block->semaphore);
-			return;
-		}
-
-		// there are multiple waiters now, so we have to iterate through the list
-		__atomic_thread_fence(__ATOMIC_ACQUIRE);
-		wait_block = PUSHLOCK_GET_POINTER(saved_value);
-	}
-
-	while (wait_block->next->next)
-		wait_block = wait_block->next;
-
-	pushlock_wait_block_t *to_wake = wait_block->next;
-	wait_block->next = NULL;
-
-	while (!CAS_RELEASE(pushlock, saved_value, (saved_value & PUSHLOCK_MASK_POINTER_SHARED_COUNT) | PUSHLOCK_FLAGS_CONTENDED));
-
-	semaphore_signal(&to_wake->semaphore);
 }
 
 static void pushlock_release_shared_contended(pushlock_t *pushlock, pushlock_t saved_value) {
-	pushlock_wait_block_t *last_wait_block = PUSHLOCK_GET_POINTER(saved_value);
+	pushlock_wait_block_t *oldest_wait_block;
-	pushlock_wait_block_t *penultimate_wait_block = NULL;
-	if (last_wait_block->next) {
-		penultimate_wait_block = last_wait_block;
-		while (penultimate_wait_block->next->next)
-			penultimate_wait_block = penultimate_wait_block->next;
 
-		last_wait_block = penultimate_wait_block->next;
+	oldest_wait_block = PUSHLOCK_GET_POINTER(saved_value);
-	}
 
-	if (__atomic_sub_fetch(&last_wait_block->shared_count, 1, __ATOMIC_RELEASE))
+	while (oldest_wait_block->next)
+		oldest_wait_block = oldest_wait_block->next;
+
+	if (__atomic_sub_fetch(&oldest_wait_block->shared_count, 1, __ATOMIC_RELEASE))
 		return;
+
 	__atomic_thread_fence(__ATOMIC_ACQUIRE);
 
-	if (penultimate_wait_block == NULL) {
+	pushlock_wake_waiters(pushlock);
-		if (CAS_RELEASE(pushlock, saved_value, 0)) {
-			semaphore_signal(&last_wait_block->semaphore);
-			return;
-		}
-
-		__atomic_thread_fence(__ATOMIC_ACQUIRE);
-		penultimate_wait_block = PUSHLOCK_GET_POINTER(saved_value);
-		while (penultimate_wait_block->next->next)
-			penultimate_wait_block = penultimate_wait_block->next;
-		last_wait_block = penultimate_wait_block->next;
-	}
-
-	penultimate_wait_block->next = NULL;
-
-	while (!CAS_RELEASE(pushlock, saved_value, (saved_value & PUSHLOCK_MASK_POINTER_SHARED_COUNT) | PUSHLOCK_FLAGS_CONTENDED));
-
-	semaphore_signal(&last_wait_block->semaphore);
 }
 
 void pushlock_release_shared(pushlock_t *pushlock) {