1#include <math/bit_ops.h>
2#include <mem/alloc.h>
3#include <structures/mpmc_queue.h>
4
5void mpmc_queue_init_static(struct mpmc_queue *q, struct mpmc_slot *slots,
6 size_t capacity) {
7 q->capacity = capacity;
8 q->mask = capacity - 1;
9 q->slots = slots;
10
11 atomic_store_explicit(&q->head, 0, memory_order_relaxed);
12 atomic_store_explicit(&q->tail, 0, memory_order_relaxed);
13
14 for (size_t i = 0; i < capacity; i++) {
15 atomic_store_explicit(&q->slots[i].seq, i, memory_order_relaxed);
16 q->slots[i].data = 0;
17 }
18}
19
20bool mpmc_queue_init(struct mpmc_queue *q, size_t capacity) {
21 size_t cap = next_pow2(x: capacity);
22 if (cap < 2) {
23 cap = 2;
24 }
25
26 struct mpmc_slot *slots =
27 kmalloc(sizeof(struct mpmc_slot) * cap, ALLOC_FLAGS_ZERO);
28 if (!slots) {
29 return false;
30 }
31
32 mpmc_queue_init_static(q, slots, capacity: cap);
33 return true;
34}
35
36void mpmc_queue_destroy(struct mpmc_queue *q) {
37 if (q->slots) {
38 kfree(q->slots);
39 q->slots = NULL;
40 }
41 q->capacity = 0;
42 q->mask = 0;
43}
44
45bool mpmc_queue_enqueue_uintptr(struct mpmc_queue *q, uintptr_t val) {
46 uint64_t pos;
47 struct mpmc_slot *slot;
48 uint64_t seq;
49 int64_t diff;
50
51 while (true) {
52 pos = atomic_load_explicit(&q->head, memory_order_relaxed);
53 slot = &q->slots[pos & q->mask];
54 seq = atomic_load_explicit(&slot->seq, memory_order_acquire);
55 diff = (int64_t) seq - (int64_t) pos;
56
57 if (diff == 0) {
58 if (atomic_compare_exchange_weak_explicit(&q->head, &pos, pos + 1,
59 memory_order_acq_rel,
60 memory_order_relaxed)) {
61 slot->data = val;
62 atomic_store_explicit(&slot->seq, pos + 1,
63 memory_order_release);
64 return true;
65 }
66 } else if (diff < 0) {
67 return false; /* Queue is full */
68 }
69 }
70}
71
72bool mpmc_queue_enqueue(struct mpmc_queue *q, void *ptr) {
73 return mpmc_queue_enqueue_uintptr(q, val: (uintptr_t) ptr);
74}
75
76bool mpmc_queue_dequeue_uintptr(struct mpmc_queue *q, uintptr_t *out_val) {
77 uint64_t pos;
78 struct mpmc_slot *slot;
79 uint64_t seq;
80 int64_t diff;
81
82 while (true) {
83 pos = atomic_load_explicit(&q->tail, memory_order_relaxed);
84 slot = &q->slots[pos & q->mask];
85 seq = atomic_load_explicit(&slot->seq, memory_order_acquire);
86 diff = (int64_t) seq - (int64_t) (pos + 1);
87
88 if (diff == 0) {
89 if (atomic_compare_exchange_weak_explicit(&q->tail, &pos, pos + 1,
90 memory_order_acq_rel,
91 memory_order_relaxed)) {
92 if (out_val) {
93 *out_val = slot->data;
94 }
95 slot->data = 0;
96 atomic_store_explicit(&slot->seq, pos + q->capacity,
97 memory_order_release);
98 return true;
99 }
100 } else if (diff < 0) {
101 return false;
102 }
103 }
104}
105
106bool mpmc_queue_dequeue(struct mpmc_queue *q, void **out_ptr) {
107 uintptr_t val = 0;
108 if (mpmc_queue_dequeue_uintptr(q, out_val: &val)) {
109 if (out_ptr) {
110 *out_ptr = (void *) val;
111 }
112 return true;
113 }
114 return false;
115}
116