1#include <math/bit_ops.h>
2#include <math/min_max.h>
3#include <mem/alloc.h>
4#include <string.h>
5#include <structures/spsc_fifo.h>
6
7void spsc_fifo_init_with(struct spsc_fifo *fifo, void *buffer, size_t size) {
8 fifo->size = size;
9 fifo->mask = size - 1;
10 fifo->data = (uint8_t *) buffer;
11 atomic_store_explicit(&fifo->head, 0, memory_order_relaxed);
12 atomic_store_explicit(&fifo->tail, 0, memory_order_relaxed);
13}
14
15bool spsc_fifo_init(struct spsc_fifo *fifo, size_t size) {
16 size_t cap = next_pow2(x: size);
17 if (cap < 2) {
18 cap = 2;
19 }
20
21 uint8_t *buf = kmalloc(cap, ALLOC_FLAGS_ZERO);
22 if (!buf) {
23 return false;
24 }
25
26 spsc_fifo_init_with(fifo, buffer: buf, size: cap);
27 return true;
28}
29
30void spsc_fifo_destroy(struct spsc_fifo *fifo) {
31 if (fifo->data) {
32 kfree(fifo->data);
33 fifo->data = NULL;
34 }
35 fifo->size = 0;
36 fifo->mask = 0;
37}
38
39size_t spsc_fifo_write(struct spsc_fifo *fifo, const void *src, size_t len) {
40 size_t head = atomic_load_explicit(&fifo->head, memory_order_relaxed);
41 size_t tail = atomic_load_explicit(&fifo->tail, memory_order_acquire);
42 size_t avail = fifo->size - (head - tail);
43
44 len = MIN(len, avail);
45 if (len == 0) {
46 return 0;
47 }
48
49 size_t off = head & fifo->mask;
50 size_t l = MIN(len, fifo->size - off);
51
52 memcpy(fifo->data + off, src, l);
53 if (len > l) {
54 memcpy(fifo->data, (const uint8_t *) src + l, len - l);
55 }
56
57 atomic_store_explicit(&fifo->head, head + len, memory_order_release);
58 return len;
59}
60
61size_t spsc_fifo_read(struct spsc_fifo *fifo, void *dst, size_t len) {
62 size_t head = atomic_load_explicit(&fifo->head, memory_order_acquire);
63 size_t tail = atomic_load_explicit(&fifo->tail, memory_order_relaxed);
64 size_t in_use = head - tail;
65
66 len = MIN(len, in_use);
67 if (len == 0) {
68 return 0;
69 }
70
71 size_t off = tail & fifo->mask;
72 size_t l = MIN(len, fifo->size - off);
73
74 memcpy(dst, fifo->data + off, l);
75 if (len > l) {
76 memcpy((uint8_t *) dst + l, fifo->data, len - l);
77 }
78
79 atomic_store_explicit(&fifo->tail, tail + len, memory_order_release);
80 return len;
81}
82
83size_t spsc_fifo_peek(const struct spsc_fifo *fifo, void *dst, size_t len) {
84 size_t head = atomic_load_explicit(&fifo->head, memory_order_acquire);
85 size_t tail = atomic_load_explicit(&fifo->tail, memory_order_relaxed);
86 size_t in_use = head - tail;
87
88 len = MIN(len, in_use);
89 if (len == 0) {
90 return 0;
91 }
92
93 size_t off = tail & fifo->mask;
94 size_t l = MIN(len, fifo->size - off);
95
96 memcpy(dst, fifo->data + off, l);
97 if (len > l) {
98 memcpy((uint8_t *) dst + l, fifo->data, len - l);
99 }
100
101 return len;
102}
103
104bool spsc_fifo_push_ptr(struct spsc_fifo *fifo, const void *ptr) {
105 return spsc_fifo_write(fifo, src: &ptr, len: sizeof(ptr)) == sizeof(ptr);
106}
107
108bool spsc_fifo_pop_ptr(struct spsc_fifo *fifo, void **out_ptr) {
109 return spsc_fifo_read(fifo, dst: out_ptr, len: sizeof(void *)) == sizeof(void *);
110}
111