1 /*
2 * Copyright (c) 2018-2019 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 struct { \
46 /* TODO: const, optimise RAM use */ \
47 /* TODO: Separate s,n,f,l out into common struct */ \
48 uint16_t const s; /* Stride between elements */ \
49 uint16_t const n; /* Number of buffers */ \
50 uint8_t f; /* First. Read index */ \
51 uint8_t l; /* Last. Write index */ \
52 uint8_t MALIGN(4) m[MROUND(sz) * ((cnt) + 1)]; \
53 } mfifo_##name = { \
54 .n = ((cnt) + 1), \
55 .s = MROUND(sz), \
56 .f = 0, \
57 .l = 0, \
58 }
59
60 /**
61 * @brief Initialize an MFIFO to be empty
62 * @details API 1 and 2. An MFIFO is empty if first == last
63 */
64 #define MFIFO_INIT(name) \
65 mfifo_##name.f = mfifo_##name.l = 0
66
67 /**
68 * @brief Non-destructive: Allocate buffer from the queue's tail, by index
69 * @details API 2. Used internally by API 1.
70 * Note that enqueue is split in 2 parts, allocation and commit:
71 * 1. Allocation: Buffer allocation from tail. May fail.
72 * 2. Commit: If allocation was successful, the enqueue can be committed.
73 * Committing can not fail, as only allocation can fail.
74 * Allocation is non-destructive as operations are performed on index copies,
75 * however the buffer remains in a state of limbo until committed.
76 * Note: The limbo state opens up a potential race where successive
77 * un-committed allocations returns the same buffer index.
78 *
79 * @param idx[out] Index of newly allocated buffer
80 * @return True if buffer could be allocated; otherwise false
81 */
mfifo_enqueue_idx_get(uint8_t count,uint8_t first,uint8_t last,uint8_t * idx)82 static inline bool mfifo_enqueue_idx_get(uint8_t count, uint8_t first, uint8_t last,
83 uint8_t *idx)
84 {
85 /* Non-destructive: Advance write-index modulo 'count' */
86 last = last + 1;
87 if (last == count) {
88 last = 0U;
89 }
90
91 /* Is queue full?
92 * We want to maintain the invariant of emptiness defined by
93 * first == last, but we just advanced a copy of the write-index before
94 * and may have wrapped. So if first == last the queue is full and we
95 * can not continue
96 */
97 if (last == first) {
98 return false; /* Queue is full */
99 }
100
101 *idx = last; /* Emit the allocated buffer's index */
102 return true; /* Successfully allocated new buffer */
103 }
104
105 /**
106 * @brief Non-destructive: Allocate buffer from named queue
107 * @details API 2.
108 * @param i[out] Index of newly allocated buffer
109 * @return True if buffer could be allocated; otherwise false
110 */
111 #define MFIFO_ENQUEUE_IDX_GET(name, i) \
112 mfifo_enqueue_idx_get(mfifo_##name.n, mfifo_##name.f, \
113 mfifo_##name.l, (i))
114
115 /**
116 * @brief Commit a previously allocated buffer (=void-ptr)
117 * @details API 2
118 */
mfifo_by_idx_enqueue(uint8_t * fifo,uint8_t size,uint8_t idx,void * mem,uint8_t * last)119 static inline void mfifo_by_idx_enqueue(uint8_t *fifo, uint8_t size, uint8_t idx,
120 void *mem, uint8_t *last)
121 {
122 /* API 2: fifo is array of void-ptrs */
123 void **p = (void **)(fifo + (*last) * size); /* buffer preceding idx */
124 *p = mem; /* store the payload which for API 2 is only a void-ptr */
125
126 cpu_dmb(); /* Ensure data accesses are synchronized */
127 *last = idx; /* Commit: Update write index */
128 }
129
130 /**
131 * @brief Commit a previously allocated buffer (=void-ptr)
132 * @details API 2
133 */
134 #define MFIFO_BY_IDX_ENQUEUE(name, i, mem) \
135 mfifo_by_idx_enqueue(mfifo_##name.m, mfifo_##name.s, (i), \
136 (mem), &mfifo_##name.l)
137
138 /**
139 * @brief Non-destructive: Allocate buffer from named queue
140 * @details API 1.
141 * The allocated buffer exists in limbo until committed.
142 * To commit the enqueue process, mfifo_enqueue() must be called afterwards
143 * @return Index of newly allocated buffer; only valid if mem != NULL
144 */
mfifo_enqueue_get(uint8_t * fifo,uint8_t size,uint8_t count,uint8_t first,uint8_t last,void ** mem)145 static inline uint8_t mfifo_enqueue_get(uint8_t *fifo, uint8_t size, uint8_t count,
146 uint8_t first, uint8_t last, void **mem)
147 {
148 uint8_t idx;
149
150 /* Attempt to allocate new buffer (idx) */
151 if (!mfifo_enqueue_idx_get(count, first, last, &idx)) {
152 /* Buffer could not be allocated */
153 *mem = NULL; /* Signal the failure */
154 return 0; /* DontCare */
155 }
156
157 /* We keep idx as the always-one-free, so we return preceding
158 * buffer (last). Recall that last has not been updated,
159 * so idx != last
160 */
161 *mem = (void *)(fifo + last * size); /* preceding buffer */
162
163 return idx;
164 }
165
166 /**
167 * @brief Non-destructive: Allocate buffer from named queue
168 * @details API 1.
169 * The allocated buffer exists in limbo until committed.
170 * To commit the enqueue process, MFIFO_ENQUEUE() must be called afterwards
171 * @param mem[out] Pointer to newly allocated buffer; NULL if allocation failed
172 * @return Index to the buffer one-ahead of allocated buffer
173 */
174 #define MFIFO_ENQUEUE_GET(name, mem) \
175 mfifo_enqueue_get(mfifo_##name.m, mfifo_##name.s, \
176 mfifo_##name.n, mfifo_##name.f, \
177 mfifo_##name.l, (mem))
178
179 /**
180 * @brief Atomically commit a previously allocated buffer
181 * @details API 1.
182 * Destructive update: Update the queue, bringing the allocated buffer out of
183 * limbo state -- thus completing its enqueue.
184 * Can not fail.
185 * The buffer must have been allocated using mfifo_enqueue_idx_get() or
186 * mfifo_enqueue_get().
187 *
188 * @param idx[in] Index one-ahead of previously allocated buffer
189 * @param last[out] Write-index
190 */
mfifo_enqueue(uint8_t idx,uint8_t * last)191 static inline void mfifo_enqueue(uint8_t idx, uint8_t *last)
192 {
193 cpu_dmb(); /* Ensure data accesses are synchronized */
194 *last = idx; /* Commit: Update write index */
195 }
196
197 /**
198 * @brief Atomically commit a previously allocated buffer
199 * @details API 1
200 * The buffer should have been allocated using MFIFO_ENQUEUE_GET
201 * @param idx[in] Index one-ahead of previously allocated buffer
202 */
203 #define MFIFO_ENQUEUE(name, idx) \
204 mfifo_enqueue((idx), &mfifo_##name.l)
205
206 /**
207 * @brief Number of available buffers
208 * @details API 1 and 2
209 * Empty if first == last
210 */
mfifo_avail_count_get(uint8_t count,uint8_t first,uint8_t last)211 static inline uint8_t mfifo_avail_count_get(uint8_t count, uint8_t first, 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_##name.f, \
226 mfifo_##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, uint8_t first,
233 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_##name.m, mfifo_##name.s, \
248 mfifo_##name.f, mfifo_##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, uint8_t first,
255 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_##name.m, mfifo_##name.s, \
271 mfifo_##name.f, mfifo_##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, uint8_t count,
274 uint8_t first, uint8_t last, uint8_t *idx)
275 {
276 void *p;
277 uint8_t i;
278
279 if (*idx >= count) {
280 *idx = first;
281 }
282
283 if (*idx == last) {
284 return NULL;
285 }
286
287 i = *idx + 1;
288 if (i == count) {
289 i = 0U;
290 }
291
292 p = (void *)(fifo + (*idx) * size);
293
294 *idx = i;
295
296 return p;
297 }
298
299 #define MFIFO_DEQUEUE_ITER_GET(name, idx) \
300 mfifo_dequeue_iter_get(mfifo_##name.m, mfifo_##name.s, \
301 mfifo_##name.n, mfifo_##name.f, \
302 mfifo_##name.l, (idx))
303
304 /**
305 * @brief Dequeue head-buffer from queue of buffers
306 *
307 * @param fifo[in] Contiguous memory holding the circular queue
308 * @param size[in] Size of each buffer in circular queue
309 * @param count[in] Number of buffers in circular queue
310 * @param last[in] Tail index, Span: [0 .. count-1]
311 * @param first[in,out] Head index, Span: [0 .. count-1]. Will be updated
312 * @return Head buffer; or NULL if queue was empty
313 */
mfifo_dequeue(uint8_t * fifo,uint8_t size,uint8_t count,uint8_t last,uint8_t * first)314 static inline void *mfifo_dequeue(uint8_t *fifo, uint8_t size, uint8_t count,
315 uint8_t last, uint8_t *first)
316 {
317 uint8_t _first = *first; /* Copy read-index */
318 void *mem;
319
320 /* Queue is empty if first == last */
321 if (_first == last) {
322 return NULL;
323 }
324
325 /* Obtain address of head buffer.
326 * API 2: fifo is array of void-ptrs
327 */
328 mem = *((void **)(fifo + _first * size));
329
330 /* Circular buffer increment read-index modulo 'count' */
331 _first += 1U;
332 if (_first == count) {
333 _first = 0U;
334 }
335
336 *first = _first; /* Write back read-index */
337
338 return mem;
339 }
340
341 /**
342 * @brief Dequeue head-buffer from named queue of buffers
343 *
344 * @param name[in] Name-fragment of circular queue
345 * @return Head buffer; or NULL if queue was empty
346 */
347 #define MFIFO_DEQUEUE(name) \
348 mfifo_dequeue(mfifo_##name.m, mfifo_##name.s, \
349 mfifo_##name.n, mfifo_##name.l, \
350 &mfifo_##name.f)
351
