1 /*
2 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <stdlib.h>
8 #include <string.h>
9 #include "pico/util/queue.h"
10
queue_init_with_spinlock(queue_t * q,uint element_size,uint element_count,uint spinlock_num)11 void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num) {
12 q->lock = spin_lock_instance(spinlock_num);
13 q->data = (uint8_t *)calloc(element_count + 1, element_size);
14 q->element_count = element_count;
15 q->element_size = element_size;
16 q->wptr = 0;
17 q->rptr = 0;
18 }
19
queue_free(queue_t * q)20 void queue_free(queue_t *q) {
21 free(q->data);
22 }
23
element_ptr(queue_t * q,uint index)24 static inline void *element_ptr(queue_t *q, uint index) {
25 assert(index <= q->element_count);
26 return q->data + index * q->element_size;
27 }
28
inc_index(queue_t * q,uint16_t index)29 static inline uint16_t inc_index(queue_t *q, uint16_t index) {
30 if (++index > q->element_count) { // > because we have element_count + 1 elements
31 index = 0;
32 }
33 return index;
34 }
35
queue_try_add(queue_t * q,void * data)36 bool queue_try_add(queue_t *q, void *data) {
37 bool success = false;
38 uint32_t flags = spin_lock_blocking(q->lock);
39 if (queue_get_level_unsafe(q) != q->element_count) {
40 memcpy(element_ptr(q, q->wptr), data, q->element_size);
41 q->wptr = inc_index(q, q->wptr);
42 success = true;
43 }
44 spin_unlock(q->lock, flags);
45 if (success) __sev();
46 return success;
47 }
48
queue_try_remove(queue_t * q,void * data)49 bool queue_try_remove(queue_t *q, void *data) {
50 bool success = false;
51 uint32_t flags = spin_lock_blocking(q->lock);
52 if (queue_get_level_unsafe(q) != 0) {
53 memcpy(data, element_ptr(q, q->rptr), q->element_size);
54 q->rptr = inc_index(q, q->rptr);
55 success = true;
56 }
57 spin_unlock(q->lock, flags);
58 if (success) __sev();
59 return success;
60 }
61
queue_try_peek(queue_t * q,void * data)62 bool queue_try_peek(queue_t *q, void *data) {
63 bool success = false;
64 uint32_t flags = spin_lock_blocking(q->lock);
65 if (queue_get_level_unsafe(q) != 0) {
66 memcpy(data, element_ptr(q, q->rptr), q->element_size);
67 success = true;
68 }
69 spin_unlock(q->lock, flags);
70 return success;
71 }
72
queue_add_blocking(queue_t * q,void * data)73 void queue_add_blocking(queue_t *q, void *data) {
74 bool done;
75 do {
76 done = queue_try_add(q, data);
77 if (done) break;
78 __wfe();
79 } while (true);
80 }
81
queue_remove_blocking(queue_t * q,void * data)82 void queue_remove_blocking(queue_t *q, void *data) {
83 bool done;
84 do {
85 done = queue_try_remove(q, data);
86 if (done) break;
87 __wfe();
88 } while (true);
89 }
90
queue_peek_blocking(queue_t * q,void * data)91 void queue_peek_blocking(queue_t *q, void *data) {
92 bool done;
93 do {
94 done = queue_try_peek(q, data);
95 if (done) break;
96 __wfe();
97 } while (true);
98 }
99