blob: a493a6bd2e2030774e20dd9b4bab78eae4b3d50c [file] [log] [blame]
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +02001/*
2 * Copyright (c) 2010-2016 Wind River Systems, Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7/**
8 * @file
9 *
10 * @brief dynamic-size QUEUE object.
11 */
12
13
14#include <kernel.h>
15#include <kernel_structs.h>
16#include <debug/object_tracing_common.h>
17#include <toolchain.h>
Anas Nashif397d29d2017-06-17 11:30:47 -040018#include <linker/sections.h>
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020019#include <wait_q.h>
20#include <ksched.h>
Andrew Boie2b9b4b22018-04-27 13:21:22 -070021#include <misc/sflist.h>
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020022#include <init.h>
Andrew Boie2b9b4b22018-04-27 13:21:22 -070023#include <syscall_handler.h>
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020024
25extern struct k_queue _k_queue_list_start[];
26extern struct k_queue _k_queue_list_end[];
27
Andrew Boie2b9b4b22018-04-27 13:21:22 -070028struct alloc_node {
29 sys_sfnode_t node;
30 void *data;
31};
32
33void *z_queue_node_peek(sys_sfnode_t *node, bool needs_free)
34{
35 void *ret;
36
37 if (node && sys_sfnode_flags_get(node)) {
38 /* If the flag is set, then the enqueue operation for this item
39 * did a behind-the scenes memory allocation of an alloc_node
40 * struct, which is what got put in the queue. Free it and pass
41 * back the data pointer.
42 */
43 struct alloc_node *anode;
44
45 anode = CONTAINER_OF(node, struct alloc_node, node);
46 ret = anode->data;
47 if (needs_free) {
48 k_free(anode);
49 }
50 } else {
51 /* Data was directly placed in the queue, the first 4 bytes
52 * reserved for the linked list. User mode isn't allowed to
53 * do this, although it can get data sent this way.
54 */
55 ret = (void *)node;
56 }
57
58 return ret;
59}
60
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020061#ifdef CONFIG_OBJECT_TRACING
62
Maciek Borzecki059544d2017-05-18 12:16:45 +020063struct k_queue *_trace_list_k_queue;
64
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020065/*
66 * Complete initialization of statically defined queues.
67 */
68static int init_queue_module(struct device *dev)
69{
70 ARG_UNUSED(dev);
71
72 struct k_queue *queue;
73
74 for (queue = _k_queue_list_start; queue < _k_queue_list_end; queue++) {
75 SYS_TRACING_OBJ_INIT(k_queue, queue);
76 }
77 return 0;
78}
79
80SYS_INIT(init_queue_module, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_OBJECTS);
81
82#endif /* CONFIG_OBJECT_TRACING */
83
Andrew Boie2b9b4b22018-04-27 13:21:22 -070084void _impl_k_queue_init(struct k_queue *queue)
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020085{
Andrew Boie2b9b4b22018-04-27 13:21:22 -070086 sys_sflist_init(&queue->data_q);
Andy Rossccf3bf72018-05-10 11:10:34 -070087 _waitq_init(&queue->wait_q);
Luiz Augusto von Dentz7d01c5e2017-08-21 10:49:29 +030088#if defined(CONFIG_POLL)
89 sys_dlist_init(&queue->poll_events);
90#endif
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020091
92 SYS_TRACING_OBJ_INIT(k_queue, queue);
Andrew Boie2b9b4b22018-04-27 13:21:22 -070093 _k_object_init(queue);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +020094}
95
Andrew Boie2b9b4b22018-04-27 13:21:22 -070096#ifdef CONFIG_USERSPACE
Andrew Boie8345e5e2018-05-04 15:57:57 -070097Z_SYSCALL_HANDLER(k_queue_init, queue_ptr)
Andrew Boie2b9b4b22018-04-27 13:21:22 -070098{
99 struct k_queue *queue = (struct k_queue *)queue_ptr;
100
Andrew Boie8345e5e2018-05-04 15:57:57 -0700101 Z_OOPS(Z_SYSCALL_OBJ_NEVER_INIT(queue, K_OBJ_QUEUE));
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700102 _impl_k_queue_init(queue);
103 return 0;
104}
105#endif
106
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300107#if !defined(CONFIG_POLL)
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200108static void prepare_thread_to_run(struct k_thread *thread, void *data)
109{
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200110 _ready_thread(thread);
111 _set_thread_return_value_with_data(thread, 0, data);
112}
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300113#endif /* CONFIG_POLL */
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200114
Anas Nashif80e6a972018-06-23 08:20:34 -0500115#ifdef CONFIG_POLL
Andy Ross8606fab2018-03-26 10:54:40 -0700116static inline void handle_poll_events(struct k_queue *queue, u32_t state)
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200117{
Andy Ross8606fab2018-03-26 10:54:40 -0700118 _handle_obj_poll_events(&queue->poll_events, state);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200119}
Anas Nashif80e6a972018-06-23 08:20:34 -0500120#endif
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200121
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700122void _impl_k_queue_cancel_wait(struct k_queue *queue)
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300123{
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300124 unsigned int key = irq_lock();
125#if !defined(CONFIG_POLL)
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300126 struct k_thread *first_pending_thread;
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300127
128 first_pending_thread = _unpend_first_thread(&queue->wait_q);
129
Flavio Ceolin4218d5f2018-09-17 09:39:51 -0700130 if (first_pending_thread != NULL) {
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300131 prepare_thread_to_run(first_pending_thread, NULL);
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300132 }
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300133#else
Paul Sokolovsky45c0b202018-08-21 23:29:11 +0300134 handle_poll_events(queue, K_POLL_STATE_CANCELLED);
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300135#endif /* !CONFIG_POLL */
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300136
Andy Ross15cb5d72018-04-02 18:40:10 -0700137 _reschedule(key);
Paul Sokolovsky3f507072017-04-25 17:54:31 +0300138}
139
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700140#ifdef CONFIG_USERSPACE
Andrew Boie8345e5e2018-05-04 15:57:57 -0700141Z_SYSCALL_HANDLER1_SIMPLE_VOID(k_queue_cancel_wait, K_OBJ_QUEUE,
142 struct k_queue *);
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700143#endif
144
145static int queue_insert(struct k_queue *queue, void *prev, void *data,
146 bool alloc)
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200147{
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300148 unsigned int key = irq_lock();
149#if !defined(CONFIG_POLL)
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200150 struct k_thread *first_pending_thread;
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200151
152 first_pending_thread = _unpend_first_thread(&queue->wait_q);
153
Flavio Ceolin4218d5f2018-09-17 09:39:51 -0700154 if (first_pending_thread != NULL) {
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200155 prepare_thread_to_run(first_pending_thread, data);
Andy Ross15cb5d72018-04-02 18:40:10 -0700156 _reschedule(key);
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700157 return 0;
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200158 }
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300159#endif /* !CONFIG_POLL */
160
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700161 /* Only need to actually allocate if no threads are pending */
162 if (alloc) {
163 struct alloc_node *anode;
164
165 anode = z_thread_malloc(sizeof(*anode));
Flavio Ceolin4218d5f2018-09-17 09:39:51 -0700166 if (anode == NULL) {
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700167 return -ENOMEM;
168 }
169 anode->data = data;
170 sys_sfnode_init(&anode->node, 0x1);
171 data = anode;
172 } else {
173 sys_sfnode_init(data, 0x0);
174 }
175 sys_sflist_insert(&queue->data_q, prev, data);
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300176
177#if defined(CONFIG_POLL)
Andy Ross8606fab2018-03-26 10:54:40 -0700178 handle_poll_events(queue, K_POLL_STATE_DATA_AVAILABLE);
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300179#endif /* CONFIG_POLL */
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200180
Andy Ross15cb5d72018-04-02 18:40:10 -0700181 _reschedule(key);
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700182 return 0;
183}
184
185void k_queue_insert(struct k_queue *queue, void *prev, void *data)
186{
Flavio Ceolincc74ad02018-08-13 14:34:11 -0700187 (void)queue_insert(queue, prev, data, false);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200188}
189
190void k_queue_append(struct k_queue *queue, void *data)
191{
Flavio Ceolincc74ad02018-08-13 14:34:11 -0700192 (void)queue_insert(queue, sys_sflist_peek_tail(&queue->data_q),
193 data, false);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200194}
195
196void k_queue_prepend(struct k_queue *queue, void *data)
197{
Flavio Ceolincc74ad02018-08-13 14:34:11 -0700198 (void)queue_insert(queue, NULL, data, false);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200199}
200
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700201int _impl_k_queue_alloc_append(struct k_queue *queue, void *data)
202{
203 return queue_insert(queue, sys_sflist_peek_tail(&queue->data_q), data,
204 true);
205}
206
207#ifdef CONFIG_USERSPACE
Andrew Boie8345e5e2018-05-04 15:57:57 -0700208Z_SYSCALL_HANDLER(k_queue_alloc_append, queue, data)
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700209{
Andrew Boie8345e5e2018-05-04 15:57:57 -0700210 Z_OOPS(Z_SYSCALL_OBJ(queue, K_OBJ_QUEUE));
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700211
212 return _impl_k_queue_alloc_append((struct k_queue *)queue,
213 (void *)data);
214}
215#endif
216
217int _impl_k_queue_alloc_prepend(struct k_queue *queue, void *data)
218{
219 return queue_insert(queue, NULL, data, true);
220}
221
222#ifdef CONFIG_USERSPACE
Andrew Boie8345e5e2018-05-04 15:57:57 -0700223Z_SYSCALL_HANDLER(k_queue_alloc_prepend, queue, data)
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700224{
Andrew Boie8345e5e2018-05-04 15:57:57 -0700225 Z_OOPS(Z_SYSCALL_OBJ(queue, K_OBJ_QUEUE));
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700226
227 return _impl_k_queue_alloc_prepend((struct k_queue *)queue,
228 (void *)data);
229}
230#endif
231
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200232void k_queue_append_list(struct k_queue *queue, void *head, void *tail)
233{
234 __ASSERT(head && tail, "invalid head or tail");
235
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300236 unsigned int key = irq_lock();
237#if !defined(CONFIG_POLL)
Andy Ross345553b2018-03-09 13:05:15 -0800238 struct k_thread *thread;
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200239
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200240 while (head && ((thread = _unpend_first_thread(&queue->wait_q)))) {
241 prepare_thread_to_run(thread, head);
242 head = *(void **)head;
243 }
244
Flavio Ceolin4218d5f2018-09-17 09:39:51 -0700245 if (head != NULL) {
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700246 sys_sflist_append_list(&queue->data_q, head, tail);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200247 }
248
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300249#else
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700250 sys_sflist_append_list(&queue->data_q, head, tail);
Andy Ross8606fab2018-03-26 10:54:40 -0700251 handle_poll_events(queue, K_POLL_STATE_DATA_AVAILABLE);
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300252#endif /* !CONFIG_POLL */
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200253
Andy Ross15cb5d72018-04-02 18:40:10 -0700254 _reschedule(key);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200255}
256
257void k_queue_merge_slist(struct k_queue *queue, sys_slist_t *list)
258{
259 __ASSERT(!sys_slist_is_empty(list), "list must not be empty");
260
261 /*
262 * note: this works as long as:
263 * - the slist implementation keeps the next pointer as the first
264 * field of the node object type
265 * - list->tail->next = NULL.
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700266 * - sflist implementation only differs from slist by stuffing
267 * flag bytes in the lower order bits of the data pointer
268 * - source list is really an slist and not an sflist with flags set
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200269 */
270 k_queue_append_list(queue, list->head, list->tail);
271 sys_slist_init(list);
272}
273
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300274#if defined(CONFIG_POLL)
275static void *k_queue_poll(struct k_queue *queue, s32_t timeout)
276{
277 struct k_poll_event event;
Andy Rossb173e432018-06-04 09:25:14 -0700278 int err, elapsed = 0, done = 0;
Paul Sokolovsky199d07e2017-10-16 13:36:37 +0300279 unsigned int key;
280 void *val;
Andy Rossb173e432018-06-04 09:25:14 -0700281 u32_t start;
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300282
283 k_poll_event_init(&event, K_POLL_TYPE_FIFO_DATA_AVAILABLE,
284 K_POLL_MODE_NOTIFY_ONLY, queue);
285
Andy Rossb173e432018-06-04 09:25:14 -0700286 if (timeout != K_FOREVER) {
287 start = k_uptime_get_32();
288 }
289
Luiz Augusto von Dentzf87c4c62017-10-17 11:34:21 +0300290 do {
291 event.state = K_POLL_STATE_NOT_READY;
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300292
Andy Rossb173e432018-06-04 09:25:14 -0700293 err = k_poll(&event, 1, timeout - elapsed);
294
295 if (err && err != -EAGAIN) {
Luiz Augusto von Dentzf87c4c62017-10-17 11:34:21 +0300296 return NULL;
297 }
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300298
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700299 /* sys_sflist_* aren't threadsafe, so must be always protected
300 * by irq_lock.
Luiz Augusto von Dentzf87c4c62017-10-17 11:34:21 +0300301 */
302 key = irq_lock();
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700303 val = z_queue_node_peek(sys_sflist_get(&queue->data_q), true);
Luiz Augusto von Dentzf87c4c62017-10-17 11:34:21 +0300304 irq_unlock(key);
Andy Rossb173e432018-06-04 09:25:14 -0700305
Flavio Ceolin4218d5f2018-09-17 09:39:51 -0700306 if ((val == NULL) && (timeout != K_FOREVER)) {
Andy Rossb173e432018-06-04 09:25:14 -0700307 elapsed = k_uptime_get_32() - start;
308 done = elapsed > timeout;
309 }
310 } while (!val && !done);
Luiz Augusto von Dentzf87c4c62017-10-17 11:34:21 +0300311
Paul Sokolovsky199d07e2017-10-16 13:36:37 +0300312 return val;
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300313}
314#endif /* CONFIG_POLL */
315
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700316void *_impl_k_queue_get(struct k_queue *queue, s32_t timeout)
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200317{
318 unsigned int key;
319 void *data;
320
321 key = irq_lock();
322
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700323 if (likely(!sys_sflist_is_empty(&queue->data_q))) {
324 sys_sfnode_t *node;
325
326 node = sys_sflist_get_not_empty(&queue->data_q);
327 data = z_queue_node_peek(node, true);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200328 irq_unlock(key);
329 return data;
330 }
331
332 if (timeout == K_NO_WAIT) {
333 irq_unlock(key);
334 return NULL;
335 }
336
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300337#if defined(CONFIG_POLL)
338 irq_unlock(key);
339
340 return k_queue_poll(queue, timeout);
341
342#else
Andy Rosse0a572b2018-03-26 11:58:10 -0700343 int ret = _pend_current_thread(key, &queue->wait_q, timeout);
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200344
Andy Rosse0a572b2018-03-26 11:58:10 -0700345 return ret ? NULL : _current->base.swap_data;
Luiz Augusto von Dentz84db6412017-07-13 12:43:59 +0300346#endif /* CONFIG_POLL */
Luiz Augusto von Dentza7ddb872017-02-21 14:50:42 +0200347}
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700348
349#ifdef CONFIG_USERSPACE
Andrew Boie8345e5e2018-05-04 15:57:57 -0700350Z_SYSCALL_HANDLER(k_queue_get, queue, timeout_p)
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700351{
352 s32_t timeout = timeout_p;
353
Andrew Boie8345e5e2018-05-04 15:57:57 -0700354 Z_OOPS(Z_SYSCALL_OBJ(queue, K_OBJ_QUEUE));
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700355
356 return (u32_t)_impl_k_queue_get((struct k_queue *)queue, timeout);
357}
358
Andrew Boie8345e5e2018-05-04 15:57:57 -0700359Z_SYSCALL_HANDLER1_SIMPLE(k_queue_is_empty, K_OBJ_QUEUE, struct k_queue *);
360Z_SYSCALL_HANDLER1_SIMPLE(k_queue_peek_head, K_OBJ_QUEUE, struct k_queue *);
361Z_SYSCALL_HANDLER1_SIMPLE(k_queue_peek_tail, K_OBJ_QUEUE, struct k_queue *);
Andrew Boie2b9b4b22018-04-27 13:21:22 -0700362#endif /* CONFIG_USERSPACE */