1 /*
2 * Copyright (c) 2018-2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /**
8 * Memory FIFO permitting enqueue at tail (last) and dequeue from head (first).
9 *
10 * Implemented as a circular queue over buffers. Buffers lie contiguously in
11 * the backing storage.
12 *
13 * Enqueuing is a 2 step procedure: Alloc and commit. We say an allocated
14 * buffer yet to be committed, exists in a limbo state - until committed.
15 * It is only safe to write to a buffer while it is in limbo.
16 *
17 * Invariant: last-index refers to the buffer that is safe to write while in
18 * limbo-state. Outside limbo state, last-index refers one buffer ahead of what
19 * has been enqueued.
20 *
21 * There are essentially two APIs, distinguished by the buffer-type:
22 * API 1 Value-type : MFIFO_DEFINE(name1, sizeof(struct foo), cnt1);
23 * API 2 Pointer-type : MFIFO_DEFINE(name2, sizeof(void *), cnt2);
24 *
25 * Enqueuing is performed differently between APIs:
26 * | Allocate | Commit
27 * ------+------------------------+----------------------
28 * API 1 | MFIFO_ENQUEUE_GET | MFIFO_ENQUEUE
29 * API 2 | MFIFO_ENQUEUE_IDX_GET | MFIFO_BY_IDX_ENQUEUE
30 *
31 * TODO: Reduce size: All functions except mfifo_enqueue should not be inline
32 */
33
34 /**
35 * @brief Define a Memory FIFO implemented as a circular queue.
36 * @details API 1 and 2.
37 * Contains one-more buffer than needed.
38 *
39 * TODO: We can avoid string-concat macros below by setting type in
40 * MFIFO_DEFINE struct or use a typedef. Yes the size of field 'm' may be
41 * different, but it is trailing and sizeof is not applied here, so it can
42 * be a flexible array member.
43 */
44 #define MFIFO_DEFINE(name, sz, cnt) \
45 const struct { \
46 uint16_t s; /* Stride between elements */ \
47 uint16_t n; /* Number of buffers */ \
48 } mfifo_##name = { \
49 .s = MROUND(sz), \
50 .n = ((cnt) + 1), \
51 }; \
52 static struct { \
53 uint8_t MALIGN(4) m[MROUND(sz) * ((cnt) + 1)]; \
54 uint8_t f; /* First. Read index */ \
55 uint8_t l; /* Last. Write index */ \
56 } mfifo_fifo_##name;
57
58 /**
59 * @brief Initialize an MFIFO to be empty
60 * @details API 1 and 2. An MFIFO is empty if first == last
61 */
62 #define MFIFO_INIT(name) \
63 mfifo_fifo_##name.f = mfifo_fifo_##name.l = 0
64
65 /**
66 * @brief Non-destructive: Allocate buffer from the queue's tail, by index
67 * @details API 2. Used internally by API 1.
68 * Note that enqueue is split in 2 parts, allocation and commit:
69 * 1. Allocation: Buffer allocation from tail. May fail.
70 * 2. Commit: If allocation was successful, the enqueue can be committed.
71 * Committing can not fail, as only allocation can fail.
72 * Allocation is non-destructive as operations are performed on index copies,
73 * however the buffer remains in a state of limbo until committed.
74 * Note: The limbo state opens up a potential race where successive
75 * un-committed allocations returns the same buffer index.
76 *
77 * @param idx[out] Index of newly allocated buffer
78 * @return True if buffer could be allocated; otherwise false
79 */
mfifo_enqueue_idx_get(uint8_t count,uint8_t first,uint8_t last,uint8_t * idx)80 static inline bool mfifo_enqueue_idx_get(uint8_t count, uint8_t first,
81 uint8_t last, uint8_t *idx)
82 {
83 /* Non-destructive: Advance write-index modulo 'count' */
84 last = last + 1;
85 if (last == count) {
86 last = 0U;
87 }
88
89 /* Is queue full?
90 * We want to maintain the invariant of emptiness defined by
91 * first == last, but we just advanced a copy of the write-index before
92 * and may have wrapped. So if first == last the queue is full and we
93 * can not continue
94 */
95 if (last == first) {
96 return false; /* Queue is full */
97 }
98
99 *idx = last; /* Emit the allocated buffer's index */
100 return true; /* Successfully allocated new buffer */
101 }
102
103 /**
104 * @brief Non-destructive: Allocate buffer from named queue
105 * @details API 2.
106 * @param i[out] Index of newly allocated buffer
107 * @return True if buffer could be allocated; otherwise false
108 */
109 #define MFIFO_ENQUEUE_IDX_GET(name, i) \
110 mfifo_enqueue_idx_get(mfifo_##name.n, mfifo_fifo_##name.f, \
111 mfifo_fifo_##name.l, (i))
112
113 /**
114 * @brief Commit a previously allocated buffer (=void-ptr)
115 * @details API 2
116 */
mfifo_by_idx_enqueue(uint8_t * fifo,uint8_t size,uint8_t idx,void * mem,uint8_t * last)117 static inline void mfifo_by_idx_enqueue(uint8_t *fifo, uint8_t size,
118 uint8_t idx, void *mem, uint8_t *last)
119 {
120 /* API 2: fifo is array of void-ptrs */
121 void **p = (void **)(fifo + (*last) * size); /* buffer preceding idx */
122 *p = mem; /* store the payload which for API 2 is only a void-ptr */
123
124 cpu_dmb(); /* Ensure data accesses are synchronized */
125 *last = idx; /* Commit: Update write index */
126 }
127
128 /**
129 * @brief Commit a previously allocated buffer (=void-ptr)
130 * @details API 2
131 */
132 #define MFIFO_BY_IDX_ENQUEUE(name, i, mem) \
133 mfifo_by_idx_enqueue(mfifo_fifo_##name.m, mfifo_##name.s, (i), \
134 (mem), &mfifo_fifo_##name.l)
135
136 /**
137 * @brief Non-destructive: Allocate buffer from named queue
138 * @details API 1.
139 * The allocated buffer exists in limbo until committed.
140 * To commit the enqueue process, mfifo_enqueue() must be called afterwards
141 * @return Index of newly allocated buffer; only valid if mem != NULL
142 */
mfifo_enqueue_get(uint8_t * fifo,uint8_t size,uint8_t count,uint8_t first,uint8_t last,void ** mem)143 static inline uint8_t mfifo_enqueue_get(uint8_t *fifo, uint8_t size,
144 uint8_t count, uint8_t first,
145 uint8_t last, void **mem)
146 {
147 uint8_t idx;
148
149 /* Attempt to allocate new buffer (idx) */
150 if (!mfifo_enqueue_idx_get(count, first, last, &idx)) {
151 /* Buffer could not be allocated */
152 *mem = NULL; /* Signal the failure */
153 return 0; /* DontCare */
154 }
155
156 /* We keep idx as the always-one-free, so we return preceding
157 * buffer (last). Recall that last has not been updated,
158 * so idx != last
159 */
160 *mem = (void *)(fifo + last * size); /* preceding buffer */
161
162 return idx;
163 }
164
165 /**
166 * @brief Non-destructive: Allocate buffer from named queue
167 * @details API 1.
168 * The allocated buffer exists in limbo until committed.
169 * To commit the enqueue process, MFIFO_ENQUEUE() must be called afterwards
170 * @param mem[out] Pointer to newly allocated buffer; NULL if allocation failed
171 * @return Index to the buffer one-ahead of allocated buffer
172 */
173 #define MFIFO_ENQUEUE_GET(name, mem) \
174 mfifo_enqueue_get(mfifo_fifo_##name.m, mfifo_##name.s, \
175 mfifo_##name.n, mfifo_fifo_##name.f, \
176 mfifo_fifo_##name.l, (mem))
177
178 /**
179 * @brief Atomically commit a previously allocated buffer
180 * @details API 1.
181 * Destructive update: Update the queue, bringing the allocated buffer out of
182 * limbo state -- thus completing its enqueue.
183 * Can not fail.
184 * The buffer must have been allocated using mfifo_enqueue_idx_get() or
185 * mfifo_enqueue_get().
186 *
187 * @param idx[in] Index one-ahead of previously allocated buffer
188 * @param last[out] Write-index
189 */
mfifo_enqueue(uint8_t idx,uint8_t * last)190 static inline void mfifo_enqueue(uint8_t idx, uint8_t *last)
191 {
192 cpu_dmb(); /* Ensure data accesses are synchronized */
193 *last = idx; /* Commit: Update write index */
194 }
195
196 /**
197 * @brief Atomically commit a previously allocated buffer
198 * @details API 1
199 * The buffer should have been allocated using MFIFO_ENQUEUE_GET
200 * @param idx[in] Index one-ahead of previously allocated buffer
201 */
202 #define MFIFO_ENQUEUE(name, idx) \
203 mfifo_enqueue((idx), &mfifo_fifo_##name.l)
204
205 /**
206 * @brief Number of available buffers
207 * @details API 1 and 2
208 * Empty if first == last
209 */
mfifo_avail_count_get(uint8_t count,uint8_t first,uint8_t last)210 static inline uint8_t mfifo_avail_count_get(uint8_t count, uint8_t first,
211 uint8_t last)
212 {
213 if (last >= first) {
214 return last - first;
215 } else {
216 return count - first + last;
217 }
218 }
219
220 /**
221 * @brief Number of available buffers
222 * @details API 1 and 2
223 */
224 #define MFIFO_AVAIL_COUNT_GET(name) \
225 mfifo_avail_count_get(mfifo_##name.n, mfifo_fifo_##name.f, \
226 mfifo_fifo_##name.l)
227
228 /**
229 * @brief Non-destructive peek
230 * @details API 1
231 */
mfifo_dequeue_get(uint8_t * fifo,uint8_t size,uint8_t first,uint8_t last)232 static inline void *mfifo_dequeue_get(uint8_t *fifo, uint8_t size,
233 uint8_t first, uint8_t last)
234 {
235 if (first == last) {
236 return NULL;
237 }
238
239 /* API 1: fifo is array of some value type */
240 return (void *)(fifo + first * size);
241 }
242
243 /**
244 * @details API 1
245 */
246 #define MFIFO_DEQUEUE_GET(name) \
247 mfifo_dequeue_get(mfifo_fifo_##name.m, mfifo_##name.s, \
248 mfifo_fifo_##name.f, mfifo_fifo_##name.l)
249
250 /**
251 * @brief Non-destructive: Peek at head (oldest) buffer
252 * @details API 2
253 */
mfifo_dequeue_peek(uint8_t * fifo,uint8_t size,uint8_t first,uint8_t last)254 static inline void *mfifo_dequeue_peek(uint8_t *fifo, uint8_t size,
255 uint8_t first, uint8_t last)
256 {
257 if (first == last) {
258 return NULL; /* Queue is empty */
259 }
260
261 /* API 2: fifo is array of void-ptrs */
262 return *((void **)(fifo + first * size));
263 }
264
265 /**
266 * @brief Non-destructive: Peek at head (oldest) buffer
267 * @details API 2
268 */
269 #define MFIFO_DEQUEUE_PEEK(name) \
270 mfifo_dequeue_peek(mfifo_fifo_##name.m, mfifo_##name.s, \
271 mfifo_fifo_##name.f, mfifo_fifo_##name.l)
272
mfifo_dequeue_iter_get(uint8_t * fifo,uint8_t size,uint8_t count,uint8_t first,uint8_t last,uint8_t * idx)273 static inline void *mfifo_dequeue_iter_get(uint8_t *fifo, uint8_t size,
274 uint8_t count, uint8_t first,
275 uint8_t last, uint8_t *idx)
276 {
277 void *p;
278 uint8_t i;
279
280 if (*idx >= count) {
281 *idx = first;
282 }
283
284 if (*idx == last) {
285 return NULL;
286 }
287
288 i = *idx + 1;
289 if (i == count) {
290 i = 0U;
291 }
292
293 p = (void *)(fifo + (*idx) * size);
294
295 *idx = i;
296
297 return p;
298 }
299
300 #define MFIFO_DEQUEUE_ITER_GET(name, idx) \
301 mfifo_dequeue_iter_get(mfifo_fifo_##name.m, mfifo_##name.s, \
302 mfifo_##name.n, mfifo_fifo_##name.f, \
303 mfifo_fifo_##name.l, (idx))
304
305 /**
306 * @brief Dequeue head-buffer from queue of buffers
307 *
308 * @param fifo[in] Contiguous memory holding the circular queue
309 * @param size[in] Size of each buffer in circular queue
310 * @param count[in] Number of buffers in circular queue
311 * @param last[in] Tail index, Span: [0 .. count-1]
312 * @param first[in,out] Head index, Span: [0 .. count-1]. Will be updated
313 * @return Head buffer; or NULL if queue was empty
314 */
mfifo_dequeue(uint8_t * fifo,uint8_t size,uint8_t count,uint8_t last,uint8_t * first)315 static inline void *mfifo_dequeue(uint8_t *fifo, uint8_t size, uint8_t count,
316 uint8_t last, uint8_t *first)
317 {
318 uint8_t _first = *first; /* Copy read-index */
319 void *mem;
320
321 /* Queue is empty if first == last */
322 if (_first == last) {
323 return NULL;
324 }
325
326 /* Obtain address of head buffer.
327 * API 2: fifo is array of void-ptrs
328 */
329 mem = *((void **)(fifo + _first * size));
330
331 /* Circular buffer increment read-index modulo 'count' */
332 _first += 1U;
333 if (_first == count) {
334 _first = 0U;
335 }
336
337 *first = _first; /* Write back read-index */
338
339 return mem;
340 }
341
342 /**
343 * @brief Dequeue head-buffer from named queue of buffers
344 *
345 * @param name[in] Name-fragment of circular queue
346 * @return Head buffer; or NULL if queue was empty
347 */
348 #define MFIFO_DEQUEUE(name) \
349 mfifo_dequeue(mfifo_fifo_##name.m, mfifo_##name.s, \
350 mfifo_##name.n, mfifo_fifo_##name.l, \
351 &mfifo_fifo_##name.f)
352
