1#include <compiler/core.h>
2#include <enforce.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} cc_cache_aligned;
11
12PERCPU_DEFINE(struct qnode[QSPINLOCK_LEVEL_MAX], qnodes);
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(void) {
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_cas_weak(&lock->val, &val, next, mo_acq_rel, mo_rlx));
36
37 return val;
38}
39
40void qspin_lock_slowpath(struct qspinlock *lock, uint32_t val) {
41
42 /* No tail? Check the pending bit */
43 if (!(val & Q_SPIN_TAIL_MASK)) {
44 while (!(val & Q_SPIN_PENDING_MASK)) {
45 uint32_t old = val;
46 if (atomic_cas_weak(&lock->val, &old, val | Q_SPIN_PENDING_VAL,
47 mo_acquire, mo_rlx)) {
48
49 /* We are the pender, now we wait for LOCK to clear */
50 while ((val = atomic_load_rlx(&lock->val)) & Q_SPIN_LOCKED_MASK)
51 cpu_pause();
52
53 /* pending -> locked: -0x100 + 1 */
54 atomic_fetch_add_acq(&lock->val,
55 Q_SPIN_LOCKED_VAL - Q_SPIN_PENDING_VAL);
56 return;
57 }
58 val = old;
59 }
60 }
61
62 cpu_id_t cpu = smp_id(cond: TOPC_IRQL);
63
64 /* Fallback if not ready */
65 if (cc_unlikely(!PERCPU_READY(qnodes))) {
66 while (!qspin_trylock_physical(lock))
67 cpu_pause();
68 return;
69 }
70
71 enum qspinlock_level lvl = qspinlock_get_level();
72 struct qnode *nodes = PERCPU_READ_FOR(qnodes, cpu);
73
74 struct qnode *node = &nodes[lvl];
75 atomic_store_rlx(&node->locked, 0);
76 atomic_store_rlx(&node->next, NULL);
77
78 /* Build the tail: We encode the level and the CPU */
79 uint32_t tail =
80 ((cpu + 1) << Q_SPIN_TAIL_CPU_OFFSET) | (lvl << Q_SPIN_TAIL_LVL_OFFSET);
81
82 /* Publish the tail without overwriting a concurrent locked/pending update.
83 */
84 uint32_t old_val = qspinlock_exchange_tail(lock, tail, val);
85
86 uint32_t old_tail = old_val & Q_SPIN_TAIL_MASK;
87 if (old_tail) {
88 cpu_id_t prev_cpu =
89 ((old_tail & Q_SPIN_TAIL_CPU_MASK) >> Q_SPIN_TAIL_CPU_OFFSET) - 1;
90 uint32_t prev_idx =
91 (old_tail & Q_SPIN_TAIL_LVL_MASK) >> Q_SPIN_TAIL_LVL_OFFSET;
92
93 struct qnode *prev_nodes = PERCPU_READ_FOR(qnodes, prev_cpu);
94 struct qnode *prev_node = &prev_nodes[prev_idx];
95
96 /* Chain us up */
97 atomic_store_release(&prev_node->next, node);
98
99 /* The signal will propagate to us */
100 while (!atomic_load_acq(&node->locked))
101 cpu_pause();
102 }
103
104 /* We're at the head now, wait for pending, at this point no new CPU
105 * will be able to set PENDING as they're failing on Q_SPIN_TAIL_MASK */
106 while ((val = atomic_load_rlx(&lock->val)) & Q_SPIN_LOCKED_PENDING_MASK)
107 cpu_pause();
108
109 /* If no one new joined, clear the tail and claim the lock, or claim
110 * lock + notify the successor to us */
111 while (true) {
112 if ((val & Q_SPIN_TAIL_MASK) == tail) {
113 if (atomic_cas_weak(&lock->val, &val, Q_SPIN_LOCKED_VAL, mo_acquire,
114 mo_rlx))
115 return; /* Got it */
116 } else {
117 atomic_fetch_or_acq(&lock->val, Q_SPIN_LOCKED_VAL);
118 break;
119 }
120 cpu_pause();
121 }
122
123 /* successor links */
124 while (!atomic_load_acq(&node->next))
125 cpu_pause();
126
127 struct qnode *next_node = atomic_load_rlx(&node->next);
128
129 /* "Level-triggered" "notification" */
130 atomic_store_release(&next_node->locked, 1);
131}
132