1#include <compiler.h>
2#include <kassert.h>
3#include <smp/core.h>
4#include <smp/percpu.h>
5#include <sync/qspinlock.h>
6
7struct qnode {
8 _Atomic(struct qnode *) next;
9 _Atomic uint8_t locked;
10} __cache_aligned;
11
12PERCPU_DECLARE(qnodes, struct qnode[QSPINLOCK_LEVEL_MAX], NULL);
13
14/* The idea here: if we are running at DISPATCH, this lock
15 * is also a DISPATCH lock, otherwise, this is a HIGH lock
16 *
17 * If we acquire a HIGH lock, we can also have a DISPATCH
18 * lock sitting on a queue, so we use the separate qspinlock_level
19 * so we don't reuse the qnode */
20static enum qspinlock_level qspinlock_get_level() {
21 if (irql_get() == IRQL_DISPATCH_LEVEL)
22 return QSPINLOCK_LEVEL_NORMAL;
23
24 /* QSPINLOCK_LEVEL_NMI is unsupported, we don't yet support
25 * spinlocks in NMIs, and it's not planned */
26 return QSPINLOCK_LEVEL_IRQ;
27}
28
29static uint32_t qspinlock_exchange_tail(struct qspinlock *lock, uint32_t tail,
30 uint32_t val) {
31 uint32_t next;
32
33 do {
34 next = (val & ~Q_SPIN_TAIL_MASK) | tail;
35 } while (!atomic_compare_exchange_weak_explicit(
36 &lock->val, &val, next, memory_order_acq_rel, memory_order_relaxed));
37
38 return val;
39}
40
41void qspin_lock_slowpath(struct qspinlock *lock, uint32_t val) {
42
43 /* No tail? Check the pending bit */
44 if (!(val & Q_SPIN_TAIL_MASK)) {
45 while (!(val & Q_SPIN_PENDING_MASK)) {
46 uint32_t old = val;
47 if (atomic_compare_exchange_weak_explicit(
48 &lock->val, &old, val | Q_SPIN_PENDING_VAL,
49 memory_order_acquire, memory_order_relaxed)) {
50
51 /* We are the pender, now we wait for LOCK to clear */
52 while ((val = atomic_load_explicit(&lock->val,
53 memory_order_relaxed)) &
54 Q_SPIN_LOCKED_MASK)
55 cpu_relax();
56
57 /* pending -> locked: -0x100 + 1 */
58 atomic_fetch_add_explicit(
59 &lock->val, Q_SPIN_LOCKED_VAL - Q_SPIN_PENDING_VAL,
60 memory_order_acquire);
61 return;
62 }
63 val = old;
64 }
65 }
66
67 cpu_id_t cpu = smp_id(cond: TOPC_IRQL);
68
69 /* Fallback if not ready */
70 if (unlikely(!PERCPU_READY(qnodes))) {
71 while (!qspin_trylock_physical(lock))
72 cpu_relax();
73 return;
74 }
75
76 enum qspinlock_level lvl = qspinlock_get_level();
77 struct qnode *nodes = PERCPU_READ_FOR_CPU(qnodes, cpu);
78
79 struct qnode *node = &nodes[lvl];
80 atomic_store_explicit(&node->locked, 0, memory_order_relaxed);
81 atomic_store_explicit(&node->next, NULL, memory_order_relaxed);
82
83 /* Build the tail: We encode the level and the CPU */
84 uint32_t tail =
85 ((cpu + 1) << Q_SPIN_TAIL_CPU_OFFSET) | (lvl << Q_SPIN_TAIL_LVL_OFFSET);
86
87 /* Publish the tail without overwriting a concurrent locked/pending update.
88 */
89 uint32_t old_val = qspinlock_exchange_tail(lock, tail, val);
90
91 uint32_t old_tail = old_val & Q_SPIN_TAIL_MASK;
92 if (old_tail) {
93 cpu_id_t prev_cpu =
94 ((old_tail & Q_SPIN_TAIL_CPU_MASK) >> Q_SPIN_TAIL_CPU_OFFSET) - 1;
95 uint32_t prev_idx =
96 (old_tail & Q_SPIN_TAIL_LVL_MASK) >> Q_SPIN_TAIL_LVL_OFFSET;
97
98 struct qnode *prev_nodes = PERCPU_READ_FOR_CPU(qnodes, prev_cpu);
99 struct qnode *prev_node = &prev_nodes[prev_idx];
100
101 /* Chain us up */
102 atomic_store_explicit(&prev_node->next, node, memory_order_release);
103
104 /* The signal will propagate to us */
105 while (!atomic_load_explicit(&node->locked, memory_order_acquire))
106 cpu_relax();
107 }
108
109 /* We're at the head now, wait for pending, at this point no new CPU
110 * will be able to set PENDING as they're failing on Q_SPIN_TAIL_MASK */
111 while ((val = atomic_load_explicit(&lock->val, memory_order_relaxed)) &
112 Q_SPIN_LOCKED_PENDING_MASK)
113 cpu_relax();
114
115 /* If no one new joined, clear the tail and claim the lock, or claim
116 * lock + notify the successor to us */
117 while (true) {
118 if ((val & Q_SPIN_TAIL_MASK) == tail) {
119 if (atomic_compare_exchange_weak_explicit(
120 &lock->val, &val, Q_SPIN_LOCKED_VAL, memory_order_acquire,
121 memory_order_relaxed))
122 return; /* Got it */
123 } else {
124 atomic_fetch_or_explicit(&lock->val, Q_SPIN_LOCKED_VAL,
125 memory_order_acquire);
126 break;
127 }
128 cpu_relax();
129 }
130
131 /* successor links */
132 while (!atomic_load_explicit(&node->next, memory_order_acquire))
133 cpu_relax();
134
135 struct qnode *next_node =
136 atomic_load_explicit(&node->next, memory_order_relaxed);
137
138 /* "Level-triggered" "notification" */
139 atomic_store_explicit(&next_node->locked, 1, memory_order_release);
140}
141