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