Branch data Line data Source code
1 : : /*
2 : : * Copyright (c) 2021 Nordic Semiconductor
3 : : *
4 : : * SPDX-License-Identifier: Apache-2.0
5 : : */
6 : : #include <sys/mpsc_pbuf.h>
7 : :
8 : : #define MPSC_PBUF_DEBUG 0
9 : :
10 : : #define MPSC_PBUF_DBG(buffer, ...) do { \
11 : : if (MPSC_PBUF_DEBUG) { \
12 : : printk(__VA_ARGS__); \
13 : : if (buffer) { \
14 : : mpsc_state_print(buffer); \
15 : : } \
16 : : } \
17 : : } while (0)
18 : :
19 : : static inline void mpsc_state_print(struct mpsc_pbuf_buffer *buffer)
20 : : {
21 : : if (MPSC_PBUF_DEBUG) {
22 : : printk("wr:%d/%d, rd:%d/%d\n",
23 : : buffer->wr_idx, buffer->tmp_wr_idx,
24 : : buffer->rd_idx, buffer->tmp_rd_idx);
25 : : }
26 : : }
27 : :
28 : 0 : void mpsc_pbuf_init(struct mpsc_pbuf_buffer *buffer,
29 : : const struct mpsc_pbuf_buffer_config *cfg)
30 : : {
31 : : int err;
32 : :
33 : 0 : memset(buffer, 0, offsetof(struct mpsc_pbuf_buffer, buf));
34 : 0 : buffer->get_wlen = cfg->get_wlen;
35 : 0 : buffer->notify_drop = cfg->notify_drop;
36 : 0 : buffer->buf = cfg->buf;
37 : 0 : buffer->size = cfg->size;
38 : 0 : buffer->max_usage = 0;
39 : 0 : buffer->flags = cfg->flags;
40 : :
41 [ # # ]: 0 : if (is_power_of_two(buffer->size)) {
42 : 0 : buffer->flags |= MPSC_PBUF_SIZE_POW2;
43 : : }
44 : :
45 : 0 : err = k_sem_init(&buffer->sem, 0, 1);
46 [ # # ]: 0 : __ASSERT_NO_MSG(err == 0);
47 : 0 : }
48 : :
49 : 0 : static inline bool free_space(struct mpsc_pbuf_buffer *buffer, uint32_t *res)
50 : : {
51 [ # # ]: 0 : if (buffer->rd_idx > buffer->tmp_wr_idx) {
52 : 0 : *res = buffer->rd_idx - buffer->tmp_wr_idx - 1;
53 : :
54 : 0 : return false;
55 [ # # ]: 0 : } else if (!buffer->rd_idx) {
56 : 0 : *res = buffer->size - buffer->tmp_wr_idx - 1;
57 : 0 : return false;
58 : : }
59 : :
60 : 0 : *res = buffer->size - buffer->tmp_wr_idx;
61 : :
62 : 0 : return true;
63 : : }
64 : :
65 : 0 : static inline bool available(struct mpsc_pbuf_buffer *buffer, uint32_t *res)
66 : : {
67 [ # # ]: 0 : if (buffer->tmp_rd_idx <= buffer->wr_idx) {
68 : 0 : *res = (buffer->wr_idx - buffer->tmp_rd_idx);
69 : :
70 : 0 : return false;
71 : : }
72 : :
73 : 0 : *res = buffer->size - buffer->tmp_rd_idx;
74 : :
75 : 0 : return true;
76 : : }
77 : :
78 : 0 : static inline uint32_t get_usage(struct mpsc_pbuf_buffer *buffer)
79 : : {
80 : : uint32_t f;
81 : :
82 [ # # ]: 0 : if (free_space(buffer, &f)) {
83 : 0 : f += (buffer->rd_idx - 1);
84 : : }
85 : :
86 : 0 : return buffer->size - 1 - f;
87 : : }
88 : :
89 : 0 : static inline void max_utilization_update(struct mpsc_pbuf_buffer *buffer)
90 : : {
91 [ # # ]: 0 : if (!(buffer->flags & MPSC_PBUF_MAX_UTILIZATION)) {
92 : 0 : return;
93 : : }
94 : :
95 [ # # ]: 0 : buffer->max_usage = MAX(buffer->max_usage, get_usage(buffer));
96 : : }
97 : :
98 : 0 : static inline bool is_valid(union mpsc_pbuf_generic *item)
99 : : {
100 : 0 : return item->hdr.valid;
101 : : }
102 : :
103 : 0 : static inline bool is_invalid(union mpsc_pbuf_generic *item)
104 : : {
105 [ # # # # ]: 0 : return !item->hdr.valid && !item->hdr.busy;
106 : : }
107 : :
108 : 0 : static inline uint32_t idx_inc(struct mpsc_pbuf_buffer *buffer,
109 : : uint32_t idx, uint32_t val)
110 : : {
111 : 0 : uint32_t i = idx + val;
112 : :
113 [ # # ]: 0 : if (buffer->flags & MPSC_PBUF_SIZE_POW2) {
114 : 0 : return i & (buffer->size - 1);
115 : : }
116 : :
117 [ # # ]: 0 : return (i >= buffer->size) ? i - buffer->size : i;
118 : : }
119 : :
120 : 0 : static inline uint32_t get_skip(union mpsc_pbuf_generic *item)
121 : : {
122 [ # # # # ]: 0 : if (item->hdr.busy && !item->hdr.valid) {
123 : 0 : return item->skip.len;
124 : : }
125 : :
126 : 0 : return 0;
127 : : }
128 : :
129 : 0 : static void add_skip_item(struct mpsc_pbuf_buffer *buffer, uint32_t wlen)
130 : : {
131 : 0 : union mpsc_pbuf_generic skip = {
132 : : .skip = { .valid = 0, .busy = 1, .len = wlen }
133 : : };
134 : :
135 : 0 : buffer->buf[buffer->tmp_wr_idx] = skip.raw;
136 : 0 : buffer->tmp_wr_idx = idx_inc(buffer, buffer->tmp_wr_idx, wlen);
137 : 0 : buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, wlen);
138 : 0 : }
139 : :
140 : : /* Attempts to drop a packet. If user packets dropping is allowed then any
141 : : * type of packet is dropped. Otherwise only skip packets (internal padding).
142 : : *
143 : : * If user packet was dropped @p user_packet is set to true. Function returns
144 : : * a pointer to a dropped packet or null if nothing was dropped. It may point
145 : : * to user packet (@p user_packet set to true) or internal, skip packet.
146 : : */
147 : 0 : static union mpsc_pbuf_generic *drop_item_locked(struct mpsc_pbuf_buffer *buffer,
148 : : uint32_t free_wlen,
149 : : bool allow_drop,
150 : : bool *user_packet)
151 : : {
152 : : union mpsc_pbuf_generic *item;
153 : : uint32_t rd_wlen;
154 : : uint32_t skip_wlen;
155 : :
156 : 0 : *user_packet = false;
157 : 0 : item = (union mpsc_pbuf_generic *)&buffer->buf[buffer->rd_idx];
158 : 0 : skip_wlen = get_skip(item);
159 : :
160 [ # # ]: 0 : rd_wlen = skip_wlen ? skip_wlen : buffer->get_wlen(item);
161 [ # # ]: 0 : if (skip_wlen) {
162 : 0 : allow_drop = true;
163 [ # # ]: 0 : } else if (allow_drop) {
164 [ # # ]: 0 : if (item->hdr.busy) {
165 : : /* item is currently processed and cannot be overwritten. */
166 : 0 : add_skip_item(buffer, free_wlen + 1);
167 : 0 : buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, rd_wlen);
168 : 0 : buffer->tmp_wr_idx = idx_inc(buffer, buffer->tmp_wr_idx, rd_wlen);
169 : :
170 : : /* Get next itme followed the busy one. */
171 : 0 : uint32_t next_rd_idx = idx_inc(buffer, buffer->rd_idx, rd_wlen);
172 : :
173 : 0 : item = (union mpsc_pbuf_generic *)&buffer->buf[next_rd_idx];
174 : 0 : skip_wlen = get_skip(item);
175 [ # # ]: 0 : if (skip_wlen) {
176 : 0 : rd_wlen += skip_wlen;
177 : : } else {
178 : 0 : rd_wlen += buffer->get_wlen(item);
179 : 0 : *user_packet = true;
180 : : }
181 : : } else {
182 : 0 : *user_packet = true;
183 : : }
184 : : } else {
185 : 0 : item = NULL;
186 : : }
187 : :
188 [ # # ]: 0 : if (allow_drop) {
189 : 0 : buffer->rd_idx = idx_inc(buffer, buffer->rd_idx, rd_wlen);
190 : 0 : buffer->tmp_rd_idx = buffer->rd_idx;
191 : : }
192 : :
193 : 0 : return item;
194 : : }
195 : :
196 : 0 : void mpsc_pbuf_put_word(struct mpsc_pbuf_buffer *buffer,
197 : : const union mpsc_pbuf_generic item)
198 : : {
199 : : bool cont;
200 : : uint32_t free_wlen;
201 : : k_spinlock_key_t key;
202 : 0 : union mpsc_pbuf_generic *dropped_item = NULL;
203 : : bool valid_drop;
204 : :
205 : : do {
206 : 0 : cont = false;
207 : 0 : key = k_spin_lock(&buffer->lock);
208 : 0 : (void)free_space(buffer, &free_wlen);
209 [ # # ]: 0 : if (free_wlen) {
210 : 0 : buffer->buf[buffer->tmp_wr_idx] = item.raw;
211 : 0 : buffer->tmp_wr_idx = idx_inc(buffer,
212 : : buffer->tmp_wr_idx, 1);
213 : 0 : buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, 1);
214 : 0 : max_utilization_update(buffer);
215 : : } else {
216 : 0 : bool user_drop = buffer->flags & MPSC_PBUF_MODE_OVERWRITE;
217 : :
218 : 0 : dropped_item = drop_item_locked(buffer, free_wlen,
219 : : user_drop, &valid_drop);
220 : 0 : cont = dropped_item != NULL;
221 : : }
222 : :
223 : 0 : k_spin_unlock(&buffer->lock, key);
224 : :
225 [ # # # # ]: 0 : if (cont && valid_drop) {
226 : : /* Notify about item being dropped. */
227 : 0 : buffer->notify_drop(buffer, dropped_item);
228 : : }
229 [ # # ]: 0 : } while (cont);
230 : :
231 : 0 : }
232 : :
233 : 0 : union mpsc_pbuf_generic *mpsc_pbuf_alloc(struct mpsc_pbuf_buffer *buffer,
234 : : size_t wlen, k_timeout_t timeout)
235 : : {
236 : 0 : union mpsc_pbuf_generic *item = NULL;
237 : 0 : union mpsc_pbuf_generic *dropped_item = NULL;
238 : : bool cont;
239 : : uint32_t free_wlen;
240 : : bool valid_drop;
241 : :
242 : : MPSC_PBUF_DBG(buffer, "alloc %d words, ", (int)wlen);
243 : :
244 [ # # ]: 0 : if (wlen > (buffer->size - 1)) {
245 : : MPSC_PBUF_DBG(buffer, "Failed to alloc, ");
246 : 0 : return NULL;
247 : : }
248 : :
249 : : do {
250 : : k_spinlock_key_t key;
251 : : bool wrap;
252 : :
253 : 0 : cont = false;
254 : 0 : key = k_spin_lock(&buffer->lock);
255 : 0 : wrap = free_space(buffer, &free_wlen);
256 : :
257 [ # # ]: 0 : if (free_wlen >= wlen) {
258 : 0 : item =
259 : 0 : (union mpsc_pbuf_generic *)&buffer->buf[buffer->tmp_wr_idx];
260 : 0 : item->hdr.valid = 0;
261 : 0 : item->hdr.busy = 0;
262 : 0 : buffer->tmp_wr_idx = idx_inc(buffer,
263 : : buffer->tmp_wr_idx, wlen);
264 [ # # ]: 0 : } else if (wrap) {
265 : 0 : add_skip_item(buffer, free_wlen);
266 : 0 : cont = true;
267 [ # # ]: 0 : } else if (!K_TIMEOUT_EQ(timeout, K_NO_WAIT) &&
268 [ # # ]: 0 : !k_is_in_isr()) {
269 : : int err;
270 : :
271 : 0 : k_spin_unlock(&buffer->lock, key);
272 : 0 : err = k_sem_take(&buffer->sem, timeout);
273 : 0 : key = k_spin_lock(&buffer->lock);
274 [ # # ]: 0 : if (err == 0) {
275 : 0 : cont = true;
276 : : }
277 : : } else {
278 : 0 : bool user_drop = buffer->flags & MPSC_PBUF_MODE_OVERWRITE;
279 : :
280 : 0 : dropped_item = drop_item_locked(buffer, free_wlen,
281 : : user_drop, &valid_drop);
282 : 0 : cont = dropped_item != NULL;
283 : : }
284 : :
285 : 0 : k_spin_unlock(&buffer->lock, key);
286 : :
287 [ # # # # : 0 : if (cont && dropped_item && valid_drop) {
# # ]
288 : : /* Notify about item being dropped. */
289 : 0 : buffer->notify_drop(buffer, dropped_item);
290 : 0 : dropped_item = NULL;
291 : : }
292 [ # # ]: 0 : } while (cont);
293 : :
294 : : MPSC_PBUF_DBG(buffer, "allocated %p ", item);
295 : :
296 : : if (IS_ENABLED(CONFIG_MPSC_CLEAR_ALLOCATED) && item) {
297 : : /* During test fill with 0's to simplify message comparison */
298 : : memset(item, 0, sizeof(int) * wlen);
299 : : }
300 : :
301 : 0 : return item;
302 : : }
303 : :
304 : 0 : void mpsc_pbuf_commit(struct mpsc_pbuf_buffer *buffer,
305 : : union mpsc_pbuf_generic *item)
306 : : {
307 : 0 : uint32_t wlen = buffer->get_wlen(item);
308 : :
309 : 0 : k_spinlock_key_t key = k_spin_lock(&buffer->lock);
310 : :
311 : 0 : item->hdr.valid = 1;
312 : 0 : buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, wlen);
313 : 0 : max_utilization_update(buffer);
314 : 0 : k_spin_unlock(&buffer->lock, key);
315 : : MPSC_PBUF_DBG(buffer, "committed %p ", item);
316 : 0 : }
317 : :
318 : 0 : void mpsc_pbuf_put_word_ext(struct mpsc_pbuf_buffer *buffer,
319 : : const union mpsc_pbuf_generic item,
320 : : const void *data)
321 : : {
322 : : static const size_t l =
323 : : (sizeof(item) + sizeof(data)) / sizeof(uint32_t);
324 : 0 : union mpsc_pbuf_generic *dropped_item = NULL;
325 : : bool cont;
326 : : bool valid_drop;
327 : :
328 : : do {
329 : : k_spinlock_key_t key;
330 : : uint32_t free_wlen;
331 : : bool wrap;
332 : :
333 : 0 : cont = false;
334 : 0 : key = k_spin_lock(&buffer->lock);
335 : 0 : wrap = free_space(buffer, &free_wlen);
336 : :
337 [ # # ]: 0 : if (free_wlen >= l) {
338 : 0 : buffer->buf[buffer->tmp_wr_idx] = item.raw;
339 : 0 : void **p =
340 : 0 : (void **)&buffer->buf[buffer->tmp_wr_idx + 1];
341 : :
342 : 0 : *p = (void *)data;
343 : 0 : buffer->tmp_wr_idx =
344 : 0 : idx_inc(buffer, buffer->tmp_wr_idx, l);
345 : 0 : buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, l);
346 : 0 : max_utilization_update(buffer);
347 [ # # ]: 0 : } else if (wrap) {
348 : 0 : add_skip_item(buffer, free_wlen);
349 : 0 : cont = true;
350 : : } else {
351 : 0 : bool user_drop = buffer->flags & MPSC_PBUF_MODE_OVERWRITE;
352 : :
353 : 0 : dropped_item = drop_item_locked(buffer, free_wlen,
354 : : user_drop, &valid_drop);
355 : 0 : cont = dropped_item != NULL;
356 : : }
357 : :
358 : 0 : k_spin_unlock(&buffer->lock, key);
359 : :
360 [ # # # # : 0 : if (cont && dropped_item && valid_drop) {
# # ]
361 : : /* Notify about item being dropped. */
362 : 0 : buffer->notify_drop(buffer, dropped_item);
363 : 0 : dropped_item = NULL;
364 : : }
365 [ # # ]: 0 : } while (cont);
366 : 0 : }
367 : :
368 : 0 : void mpsc_pbuf_put_data(struct mpsc_pbuf_buffer *buffer, const uint32_t *data,
369 : : size_t wlen)
370 : : {
371 : : bool cont;
372 : 0 : union mpsc_pbuf_generic *dropped_item = NULL;
373 : : bool valid_drop;
374 : :
375 : : do {
376 : : uint32_t free_wlen;
377 : : k_spinlock_key_t key;
378 : : bool wrap;
379 : :
380 : 0 : cont = false;
381 : 0 : key = k_spin_lock(&buffer->lock);
382 : 0 : wrap = free_space(buffer, &free_wlen);
383 : :
384 [ # # ]: 0 : if (free_wlen >= wlen) {
385 : 0 : memcpy(&buffer->buf[buffer->tmp_wr_idx], data,
386 : : wlen * sizeof(uint32_t));
387 : 0 : buffer->tmp_wr_idx =
388 : 0 : idx_inc(buffer, buffer->tmp_wr_idx, wlen);
389 : 0 : buffer->wr_idx = idx_inc(buffer, buffer->wr_idx, wlen);
390 : 0 : max_utilization_update(buffer);
391 [ # # ]: 0 : } else if (wrap) {
392 : 0 : add_skip_item(buffer, free_wlen);
393 : 0 : cont = true;
394 : : } else {
395 : 0 : bool user_drop = buffer->flags & MPSC_PBUF_MODE_OVERWRITE;
396 : :
397 : 0 : dropped_item = drop_item_locked(buffer, free_wlen,
398 : : user_drop, &valid_drop);
399 : 0 : cont = dropped_item != NULL;
400 : : }
401 : :
402 : 0 : k_spin_unlock(&buffer->lock, key);
403 : :
404 [ # # # # : 0 : if (cont && dropped_item && valid_drop) {
# # ]
405 : : /* Notify about item being dropped. */
406 : 0 : buffer->notify_drop(buffer, dropped_item);
407 : 0 : dropped_item = NULL;
408 : : }
409 [ # # ]: 0 : } while (cont);
410 : 0 : }
411 : :
412 : 0 : const union mpsc_pbuf_generic *mpsc_pbuf_claim(struct mpsc_pbuf_buffer *buffer)
413 : : {
414 : : union mpsc_pbuf_generic *item;
415 : : bool cont;
416 : :
417 : : do {
418 : : uint32_t a;
419 : : k_spinlock_key_t key;
420 : : bool wrap;
421 : :
422 : 0 : cont = false;
423 : 0 : key = k_spin_lock(&buffer->lock);
424 : 0 : wrap = available(buffer, &a);
425 : 0 : item = (union mpsc_pbuf_generic *)
426 : 0 : &buffer->buf[buffer->tmp_rd_idx];
427 : :
428 [ # # # # ]: 0 : if (!a || is_invalid(item)) {
429 : 0 : item = NULL;
430 : : } else {
431 : 0 : uint32_t skip = get_skip(item);
432 : :
433 [ # # # # ]: 0 : if (skip || !is_valid(item)) {
434 : 0 : uint32_t inc =
435 [ # # ]: 0 : skip ? skip : buffer->get_wlen(item);
436 : :
437 : 0 : buffer->tmp_rd_idx =
438 : 0 : idx_inc(buffer, buffer->tmp_rd_idx, inc);
439 : 0 : buffer->rd_idx =
440 : 0 : idx_inc(buffer, buffer->rd_idx, inc);
441 : 0 : cont = true;
442 : : } else {
443 : 0 : item->hdr.busy = 1;
444 : 0 : buffer->tmp_rd_idx =
445 : 0 : idx_inc(buffer, buffer->tmp_rd_idx,
446 : 0 : buffer->get_wlen(item));
447 : : }
448 : : }
449 : :
450 : 0 : if (!cont) {
451 : : MPSC_PBUF_DBG(buffer, "claimed: %p ", item);
452 : : }
453 : 0 : k_spin_unlock(&buffer->lock, key);
454 [ # # ]: 0 : } while (cont);
455 : :
456 : 0 : return item;
457 : : }
458 : :
459 : 0 : void mpsc_pbuf_free(struct mpsc_pbuf_buffer *buffer,
460 : : const union mpsc_pbuf_generic *item)
461 : : {
462 : 0 : uint32_t wlen = buffer->get_wlen(item);
463 : 0 : k_spinlock_key_t key = k_spin_lock(&buffer->lock);
464 : 0 : union mpsc_pbuf_generic *witem = (union mpsc_pbuf_generic *)item;
465 : :
466 : 0 : witem->hdr.valid = 0;
467 [ # # ]: 0 : if (!(buffer->flags & MPSC_PBUF_MODE_OVERWRITE) ||
468 [ # # ]: 0 : ((uint32_t *)item == &buffer->buf[buffer->rd_idx])) {
469 : 0 : witem->hdr.busy = 0;
470 : 0 : buffer->rd_idx = idx_inc(buffer, buffer->rd_idx, wlen);
471 : : } else {
472 : 0 : witem->skip.len = wlen;
473 : : }
474 : : MPSC_PBUF_DBG(buffer, "freed: %p ", item);
475 : :
476 : 0 : k_spin_unlock(&buffer->lock, key);
477 : 0 : k_sem_give(&buffer->sem);
478 : 0 : }
479 : :
480 : 0 : bool mpsc_pbuf_is_pending(struct mpsc_pbuf_buffer *buffer)
481 : : {
482 : : uint32_t a;
483 : :
484 : 0 : (void)available(buffer, &a);
485 : :
486 : 0 : return a ? true : false;
487 : : }
488 : :
489 : 0 : void mpsc_pbuf_get_utilization(struct mpsc_pbuf_buffer *buffer,
490 : : uint32_t *size, uint32_t *now)
491 : : {
492 : : /* One byte is left for full/empty distinction. */
493 : 0 : *size = (buffer->size - 1) * sizeof(int);
494 : 0 : *now = get_usage(buffer) * sizeof(int);
495 : 0 : }
496 : :
497 : 0 : int mpsc_pbuf_get_max_utilization(struct mpsc_pbuf_buffer *buffer, uint32_t *max)
498 : : {
499 : :
500 [ # # ]: 0 : if (!(buffer->flags & MPSC_PBUF_MAX_UTILIZATION)) {
501 : 0 : return -ENOTSUP;
502 : : }
503 : :
504 : 0 : *max = buffer->max_usage * sizeof(int);
505 : 0 : return 0;
506 : : }
|