Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 1 | /* |
| 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 Nashif | 397d29d | 2017-06-17 11:30:47 -0400 | [diff] [blame] | 18 | #include <linker/sections.h> |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 19 | #include <wait_q.h> |
| 20 | #include <ksched.h> |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 21 | #include <misc/sflist.h> |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 22 | #include <init.h> |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 23 | #include <syscall_handler.h> |
Andy Ross | 4f911e1 | 2018-09-05 10:13:38 -0700 | [diff] [blame] | 24 | #include <kernel_internal.h> |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 25 | |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 26 | struct alloc_node { |
| 27 | sys_sfnode_t node; |
| 28 | void *data; |
| 29 | }; |
| 30 | |
| 31 | void *z_queue_node_peek(sys_sfnode_t *node, bool needs_free) |
| 32 | { |
| 33 | void *ret; |
| 34 | |
Adithya Baglody | 2a78b8d | 2018-10-25 12:09:04 +0530 | [diff] [blame] | 35 | if ((node != NULL) && (sys_sfnode_flags_get(node) != (u8_t)0)) { |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 36 | /* If the flag is set, then the enqueue operation for this item |
| 37 | * did a behind-the scenes memory allocation of an alloc_node |
| 38 | * struct, which is what got put in the queue. Free it and pass |
| 39 | * back the data pointer. |
| 40 | */ |
| 41 | struct alloc_node *anode; |
| 42 | |
| 43 | anode = CONTAINER_OF(node, struct alloc_node, node); |
| 44 | ret = anode->data; |
| 45 | if (needs_free) { |
| 46 | k_free(anode); |
| 47 | } |
| 48 | } else { |
| 49 | /* Data was directly placed in the queue, the first 4 bytes |
| 50 | * reserved for the linked list. User mode isn't allowed to |
| 51 | * do this, although it can get data sent this way. |
| 52 | */ |
| 53 | ret = (void *)node; |
| 54 | } |
| 55 | |
| 56 | return ret; |
| 57 | } |
| 58 | |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 59 | #ifdef CONFIG_OBJECT_TRACING |
| 60 | |
Maciek Borzecki | 059544d | 2017-05-18 12:16:45 +0200 | [diff] [blame] | 61 | struct k_queue *_trace_list_k_queue; |
| 62 | |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 63 | /* |
| 64 | * Complete initialization of statically defined queues. |
| 65 | */ |
| 66 | static int init_queue_module(struct device *dev) |
| 67 | { |
| 68 | ARG_UNUSED(dev); |
| 69 | |
Nicolas Pitre | aa922885 | 2019-06-03 13:01:43 -0400 | [diff] [blame^] | 70 | Z_STRUCT_SECTION_FOREACH(k_queue, queue) { |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 71 | SYS_TRACING_OBJ_INIT(k_queue, queue); |
| 72 | } |
| 73 | return 0; |
| 74 | } |
| 75 | |
| 76 | SYS_INIT(init_queue_module, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_OBJECTS); |
| 77 | |
| 78 | #endif /* CONFIG_OBJECT_TRACING */ |
| 79 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 80 | void z_impl_k_queue_init(struct k_queue *queue) |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 81 | { |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 82 | sys_sflist_init(&queue->data_q); |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 83 | queue->lock = (struct k_spinlock) {}; |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 84 | z_waitq_init(&queue->wait_q); |
Luiz Augusto von Dentz | 7d01c5e | 2017-08-21 10:49:29 +0300 | [diff] [blame] | 85 | #if defined(CONFIG_POLL) |
| 86 | sys_dlist_init(&queue->poll_events); |
| 87 | #endif |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 88 | |
| 89 | SYS_TRACING_OBJ_INIT(k_queue, queue); |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 90 | z_object_init(queue); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 91 | } |
| 92 | |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 93 | #ifdef CONFIG_USERSPACE |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 94 | Z_SYSCALL_HANDLER(k_queue_init, queue_ptr) |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 95 | { |
| 96 | struct k_queue *queue = (struct k_queue *)queue_ptr; |
| 97 | |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 98 | Z_OOPS(Z_SYSCALL_OBJ_NEVER_INIT(queue, K_OBJ_QUEUE)); |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 99 | z_impl_k_queue_init(queue); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 100 | return 0; |
| 101 | } |
| 102 | #endif |
| 103 | |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 104 | #if !defined(CONFIG_POLL) |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 105 | static void prepare_thread_to_run(struct k_thread *thread, void *data) |
| 106 | { |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 107 | z_ready_thread(thread); |
| 108 | z_set_thread_return_value_with_data(thread, 0, data); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 109 | } |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 110 | #endif /* CONFIG_POLL */ |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 111 | |
Anas Nashif | 80e6a97 | 2018-06-23 08:20:34 -0500 | [diff] [blame] | 112 | #ifdef CONFIG_POLL |
Andy Ross | 8606fab | 2018-03-26 10:54:40 -0700 | [diff] [blame] | 113 | static inline void handle_poll_events(struct k_queue *queue, u32_t state) |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 114 | { |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 115 | z_handle_obj_poll_events(&queue->poll_events, state); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 116 | } |
Anas Nashif | 80e6a97 | 2018-06-23 08:20:34 -0500 | [diff] [blame] | 117 | #endif |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 118 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 119 | void z_impl_k_queue_cancel_wait(struct k_queue *queue) |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 120 | { |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 121 | k_spinlock_key_t key = k_spin_lock(&queue->lock); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 122 | #if !defined(CONFIG_POLL) |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 123 | struct k_thread *first_pending_thread; |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 124 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 125 | first_pending_thread = z_unpend_first_thread(&queue->wait_q); |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 126 | |
Flavio Ceolin | 4218d5f | 2018-09-17 09:39:51 -0700 | [diff] [blame] | 127 | if (first_pending_thread != NULL) { |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 128 | prepare_thread_to_run(first_pending_thread, NULL); |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 129 | } |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 130 | #else |
Paul Sokolovsky | 45c0b20 | 2018-08-21 23:29:11 +0300 | [diff] [blame] | 131 | handle_poll_events(queue, K_POLL_STATE_CANCELLED); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 132 | #endif /* !CONFIG_POLL */ |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 133 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 134 | z_reschedule(&queue->lock, key); |
Paul Sokolovsky | 3f50707 | 2017-04-25 17:54:31 +0300 | [diff] [blame] | 135 | } |
| 136 | |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 137 | #ifdef CONFIG_USERSPACE |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 138 | Z_SYSCALL_HANDLER1_SIMPLE_VOID(k_queue_cancel_wait, K_OBJ_QUEUE, |
| 139 | struct k_queue *); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 140 | #endif |
| 141 | |
Adithya Baglody | 2a78b8d | 2018-10-25 12:09:04 +0530 | [diff] [blame] | 142 | static s32_t queue_insert(struct k_queue *queue, void *prev, void *data, |
| 143 | bool alloc) |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 144 | { |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 145 | k_spinlock_key_t key = k_spin_lock(&queue->lock); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 146 | #if !defined(CONFIG_POLL) |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 147 | struct k_thread *first_pending_thread; |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 148 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 149 | first_pending_thread = z_unpend_first_thread(&queue->wait_q); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 150 | |
Flavio Ceolin | 4218d5f | 2018-09-17 09:39:51 -0700 | [diff] [blame] | 151 | if (first_pending_thread != NULL) { |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 152 | prepare_thread_to_run(first_pending_thread, data); |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 153 | z_reschedule(&queue->lock, key); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 154 | return 0; |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 155 | } |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 156 | #endif /* !CONFIG_POLL */ |
| 157 | |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 158 | /* Only need to actually allocate if no threads are pending */ |
| 159 | if (alloc) { |
| 160 | struct alloc_node *anode; |
| 161 | |
| 162 | anode = z_thread_malloc(sizeof(*anode)); |
Flavio Ceolin | 4218d5f | 2018-09-17 09:39:51 -0700 | [diff] [blame] | 163 | if (anode == NULL) { |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 164 | k_spin_unlock(&queue->lock, key); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 165 | return -ENOMEM; |
| 166 | } |
| 167 | anode->data = data; |
| 168 | sys_sfnode_init(&anode->node, 0x1); |
| 169 | data = anode; |
| 170 | } else { |
| 171 | sys_sfnode_init(data, 0x0); |
| 172 | } |
| 173 | sys_sflist_insert(&queue->data_q, prev, data); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 174 | |
| 175 | #if defined(CONFIG_POLL) |
Andy Ross | 8606fab | 2018-03-26 10:54:40 -0700 | [diff] [blame] | 176 | handle_poll_events(queue, K_POLL_STATE_DATA_AVAILABLE); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 177 | #endif /* CONFIG_POLL */ |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 178 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 179 | z_reschedule(&queue->lock, key); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 180 | return 0; |
| 181 | } |
| 182 | |
| 183 | void k_queue_insert(struct k_queue *queue, void *prev, void *data) |
| 184 | { |
Flavio Ceolin | cc74ad0 | 2018-08-13 14:34:11 -0700 | [diff] [blame] | 185 | (void)queue_insert(queue, prev, data, false); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 186 | } |
| 187 | |
| 188 | void k_queue_append(struct k_queue *queue, void *data) |
| 189 | { |
Flavio Ceolin | cc74ad0 | 2018-08-13 14:34:11 -0700 | [diff] [blame] | 190 | (void)queue_insert(queue, sys_sflist_peek_tail(&queue->data_q), |
| 191 | data, false); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 192 | } |
| 193 | |
| 194 | void k_queue_prepend(struct k_queue *queue, void *data) |
| 195 | { |
Flavio Ceolin | cc74ad0 | 2018-08-13 14:34:11 -0700 | [diff] [blame] | 196 | (void)queue_insert(queue, NULL, data, false); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 197 | } |
| 198 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 199 | s32_t z_impl_k_queue_alloc_append(struct k_queue *queue, void *data) |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 200 | { |
| 201 | return queue_insert(queue, sys_sflist_peek_tail(&queue->data_q), data, |
| 202 | true); |
| 203 | } |
| 204 | |
| 205 | #ifdef CONFIG_USERSPACE |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 206 | Z_SYSCALL_HANDLER(k_queue_alloc_append, queue, data) |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 207 | { |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 208 | Z_OOPS(Z_SYSCALL_OBJ(queue, K_OBJ_QUEUE)); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 209 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 210 | return z_impl_k_queue_alloc_append((struct k_queue *)queue, |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 211 | (void *)data); |
| 212 | } |
| 213 | #endif |
| 214 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 215 | s32_t z_impl_k_queue_alloc_prepend(struct k_queue *queue, void *data) |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 216 | { |
| 217 | return queue_insert(queue, NULL, data, true); |
| 218 | } |
| 219 | |
| 220 | #ifdef CONFIG_USERSPACE |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 221 | Z_SYSCALL_HANDLER(k_queue_alloc_prepend, queue, data) |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 222 | { |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 223 | Z_OOPS(Z_SYSCALL_OBJ(queue, K_OBJ_QUEUE)); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 224 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 225 | return z_impl_k_queue_alloc_prepend((struct k_queue *)queue, |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 226 | (void *)data); |
| 227 | } |
| 228 | #endif |
| 229 | |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 230 | void k_queue_append_list(struct k_queue *queue, void *head, void *tail) |
| 231 | { |
| 232 | __ASSERT(head && tail, "invalid head or tail"); |
| 233 | |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 234 | k_spinlock_key_t key = k_spin_lock(&queue->lock); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 235 | #if !defined(CONFIG_POLL) |
Flavio Ceolin | a42de64 | 2018-11-13 16:26:56 -0800 | [diff] [blame] | 236 | struct k_thread *thread = NULL; |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 237 | |
Flavio Ceolin | 76b3518 | 2018-12-16 12:48:29 -0800 | [diff] [blame] | 238 | if (head != NULL) { |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 239 | thread = z_unpend_first_thread(&queue->wait_q); |
Flavio Ceolin | a42de64 | 2018-11-13 16:26:56 -0800 | [diff] [blame] | 240 | } |
| 241 | |
| 242 | while ((head != NULL) && (thread != NULL)) { |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 243 | prepare_thread_to_run(thread, head); |
| 244 | head = *(void **)head; |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 245 | thread = z_unpend_first_thread(&queue->wait_q); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 246 | } |
| 247 | |
Flavio Ceolin | 4218d5f | 2018-09-17 09:39:51 -0700 | [diff] [blame] | 248 | if (head != NULL) { |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 249 | sys_sflist_append_list(&queue->data_q, head, tail); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 250 | } |
| 251 | |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 252 | #else |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 253 | sys_sflist_append_list(&queue->data_q, head, tail); |
Andy Ross | 8606fab | 2018-03-26 10:54:40 -0700 | [diff] [blame] | 254 | handle_poll_events(queue, K_POLL_STATE_DATA_AVAILABLE); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 255 | #endif /* !CONFIG_POLL */ |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 256 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 257 | z_reschedule(&queue->lock, key); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 258 | } |
| 259 | |
| 260 | void k_queue_merge_slist(struct k_queue *queue, sys_slist_t *list) |
| 261 | { |
| 262 | __ASSERT(!sys_slist_is_empty(list), "list must not be empty"); |
| 263 | |
| 264 | /* |
| 265 | * note: this works as long as: |
| 266 | * - the slist implementation keeps the next pointer as the first |
| 267 | * field of the node object type |
| 268 | * - list->tail->next = NULL. |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 269 | * - sflist implementation only differs from slist by stuffing |
| 270 | * flag bytes in the lower order bits of the data pointer |
| 271 | * - source list is really an slist and not an sflist with flags set |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 272 | */ |
| 273 | k_queue_append_list(queue, list->head, list->tail); |
| 274 | sys_slist_init(list); |
| 275 | } |
| 276 | |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 277 | #if defined(CONFIG_POLL) |
| 278 | static void *k_queue_poll(struct k_queue *queue, s32_t timeout) |
| 279 | { |
| 280 | struct k_poll_event event; |
Andy Ross | b173e43 | 2018-06-04 09:25:14 -0700 | [diff] [blame] | 281 | int err, elapsed = 0, done = 0; |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 282 | k_spinlock_key_t key; |
Paul Sokolovsky | 199d07e | 2017-10-16 13:36:37 +0300 | [diff] [blame] | 283 | void *val; |
Andy Ross | b173e43 | 2018-06-04 09:25:14 -0700 | [diff] [blame] | 284 | u32_t start; |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 285 | |
| 286 | k_poll_event_init(&event, K_POLL_TYPE_FIFO_DATA_AVAILABLE, |
| 287 | K_POLL_MODE_NOTIFY_ONLY, queue); |
| 288 | |
Andy Ross | b173e43 | 2018-06-04 09:25:14 -0700 | [diff] [blame] | 289 | if (timeout != K_FOREVER) { |
| 290 | start = k_uptime_get_32(); |
| 291 | } |
| 292 | |
Luiz Augusto von Dentz | f87c4c6 | 2017-10-17 11:34:21 +0300 | [diff] [blame] | 293 | do { |
| 294 | event.state = K_POLL_STATE_NOT_READY; |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 295 | |
Andy Ross | b173e43 | 2018-06-04 09:25:14 -0700 | [diff] [blame] | 296 | err = k_poll(&event, 1, timeout - elapsed); |
| 297 | |
| 298 | if (err && err != -EAGAIN) { |
Luiz Augusto von Dentz | f87c4c6 | 2017-10-17 11:34:21 +0300 | [diff] [blame] | 299 | return NULL; |
| 300 | } |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 301 | |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 302 | key = k_spin_lock(&queue->lock); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 303 | val = z_queue_node_peek(sys_sflist_get(&queue->data_q), true); |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 304 | k_spin_unlock(&queue->lock, key); |
Andy Ross | b173e43 | 2018-06-04 09:25:14 -0700 | [diff] [blame] | 305 | |
Flavio Ceolin | 4218d5f | 2018-09-17 09:39:51 -0700 | [diff] [blame] | 306 | if ((val == NULL) && (timeout != K_FOREVER)) { |
Andy Ross | b173e43 | 2018-06-04 09:25:14 -0700 | [diff] [blame] | 307 | elapsed = k_uptime_get_32() - start; |
| 308 | done = elapsed > timeout; |
| 309 | } |
| 310 | } while (!val && !done); |
Luiz Augusto von Dentz | f87c4c6 | 2017-10-17 11:34:21 +0300 | [diff] [blame] | 311 | |
Paul Sokolovsky | 199d07e | 2017-10-16 13:36:37 +0300 | [diff] [blame] | 312 | return val; |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 313 | } |
| 314 | #endif /* CONFIG_POLL */ |
| 315 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 316 | void *z_impl_k_queue_get(struct k_queue *queue, s32_t timeout) |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 317 | { |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 318 | k_spinlock_key_t key = k_spin_lock(&queue->lock); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 319 | void *data; |
| 320 | |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 321 | if (likely(!sys_sflist_is_empty(&queue->data_q))) { |
| 322 | sys_sfnode_t *node; |
| 323 | |
| 324 | node = sys_sflist_get_not_empty(&queue->data_q); |
| 325 | data = z_queue_node_peek(node, true); |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 326 | k_spin_unlock(&queue->lock, key); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 327 | return data; |
| 328 | } |
| 329 | |
| 330 | if (timeout == K_NO_WAIT) { |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 331 | k_spin_unlock(&queue->lock, key); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 332 | return NULL; |
| 333 | } |
| 334 | |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 335 | #if defined(CONFIG_POLL) |
Andy Ross | 603ea42 | 2018-07-25 13:01:54 -0700 | [diff] [blame] | 336 | k_spin_unlock(&queue->lock, key); |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 337 | |
| 338 | return k_queue_poll(queue, timeout); |
| 339 | |
| 340 | #else |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 341 | int ret = z_pend_curr(&queue->lock, key, &queue->wait_q, timeout); |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 342 | |
Adithya Baglody | 2a78b8d | 2018-10-25 12:09:04 +0530 | [diff] [blame] | 343 | return (ret != 0) ? NULL : _current->base.swap_data; |
Luiz Augusto von Dentz | 84db641 | 2017-07-13 12:43:59 +0300 | [diff] [blame] | 344 | #endif /* CONFIG_POLL */ |
Luiz Augusto von Dentz | a7ddb87 | 2017-02-21 14:50:42 +0200 | [diff] [blame] | 345 | } |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 346 | |
| 347 | #ifdef CONFIG_USERSPACE |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 348 | Z_SYSCALL_HANDLER(k_queue_get, queue, timeout_p) |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 349 | { |
| 350 | s32_t timeout = timeout_p; |
| 351 | |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 352 | Z_OOPS(Z_SYSCALL_OBJ(queue, K_OBJ_QUEUE)); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 353 | |
Patrik Flykt | 4344e27 | 2019-03-08 14:19:05 -0700 | [diff] [blame] | 354 | return (u32_t)z_impl_k_queue_get((struct k_queue *)queue, timeout); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 355 | } |
| 356 | |
Andrew Boie | 8345e5e | 2018-05-04 15:57:57 -0700 | [diff] [blame] | 357 | Z_SYSCALL_HANDLER1_SIMPLE(k_queue_is_empty, K_OBJ_QUEUE, struct k_queue *); |
| 358 | Z_SYSCALL_HANDLER1_SIMPLE(k_queue_peek_head, K_OBJ_QUEUE, struct k_queue *); |
| 359 | Z_SYSCALL_HANDLER1_SIMPLE(k_queue_peek_tail, K_OBJ_QUEUE, struct k_queue *); |
Andrew Boie | 2b9b4b2 | 2018-04-27 13:21:22 -0700 | [diff] [blame] | 360 | #endif /* CONFIG_USERSPACE */ |