1 /*
2 * Copyright (c) 2016 Wind River Systems, Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /**
8 * @file
9 * @brief Message queues.
10 */
11
12
13 #include <zephyr/kernel.h>
14 #include <zephyr/kernel_structs.h>
15
16 #include <zephyr/toolchain.h>
17 #include <zephyr/linker/sections.h>
18 #include <string.h>
19 #include <ksched.h>
20 #include <wait_q.h>
21 #include <zephyr/sys/dlist.h>
22 #include <zephyr/sys/math_extras.h>
23 #include <zephyr/init.h>
24 #include <zephyr/internal/syscall_handler.h>
25 #include <kernel_internal.h>
26 #include <zephyr/sys/check.h>
27
28 #ifdef CONFIG_OBJ_CORE_MSGQ
29 static struct k_obj_type obj_type_msgq;
30 #endif /* CONFIG_OBJ_CORE_MSGQ */
31
handle_poll_events(struct k_msgq * msgq)32 static inline bool handle_poll_events(struct k_msgq *msgq)
33 {
34 #ifdef CONFIG_POLL
35 return z_handle_obj_poll_events(&msgq->poll_events,
36 K_POLL_STATE_MSGQ_DATA_AVAILABLE);
37 #else
38 ARG_UNUSED(msgq);
39 return false;
40 #endif /* CONFIG_POLL */
41 }
42
k_msgq_init(struct k_msgq * msgq,char * buffer,size_t msg_size,uint32_t max_msgs)43 void k_msgq_init(struct k_msgq *msgq, char *buffer, size_t msg_size,
44 uint32_t max_msgs)
45 {
46 msgq->msg_size = msg_size;
47 msgq->max_msgs = max_msgs;
48 msgq->buffer_start = buffer;
49 msgq->buffer_end = buffer + (max_msgs * msg_size);
50 msgq->read_ptr = buffer;
51 msgq->write_ptr = buffer;
52 msgq->used_msgs = 0;
53 msgq->flags = 0;
54 z_waitq_init(&msgq->wait_q);
55 msgq->lock = (struct k_spinlock) {};
56 #ifdef CONFIG_POLL
57 sys_dlist_init(&msgq->poll_events);
58 #endif /* CONFIG_POLL */
59
60 #ifdef CONFIG_OBJ_CORE_MSGQ
61 k_obj_core_init_and_link(K_OBJ_CORE(msgq), &obj_type_msgq);
62 #endif /* CONFIG_OBJ_CORE_MSGQ */
63
64 SYS_PORT_TRACING_OBJ_INIT(k_msgq, msgq);
65
66 k_object_init(msgq);
67 }
68
z_impl_k_msgq_alloc_init(struct k_msgq * msgq,size_t msg_size,uint32_t max_msgs)69 int z_impl_k_msgq_alloc_init(struct k_msgq *msgq, size_t msg_size,
70 uint32_t max_msgs)
71 {
72 void *buffer;
73 int ret;
74 size_t total_size;
75
76 SYS_PORT_TRACING_OBJ_FUNC_ENTER(k_msgq, alloc_init, msgq);
77
78 if (size_mul_overflow(msg_size, max_msgs, &total_size)) {
79 ret = -EINVAL;
80 } else {
81 buffer = z_thread_malloc(total_size);
82 if (buffer != NULL) {
83 k_msgq_init(msgq, buffer, msg_size, max_msgs);
84 msgq->flags = K_MSGQ_FLAG_ALLOC;
85 ret = 0;
86 } else {
87 ret = -ENOMEM;
88 }
89 }
90
91 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, alloc_init, msgq, ret);
92
93 return ret;
94 }
95
96 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_alloc_init(struct k_msgq * msgq,size_t msg_size,uint32_t max_msgs)97 int z_vrfy_k_msgq_alloc_init(struct k_msgq *msgq, size_t msg_size,
98 uint32_t max_msgs)
99 {
100 K_OOPS(K_SYSCALL_OBJ_NEVER_INIT(msgq, K_OBJ_MSGQ));
101
102 return z_impl_k_msgq_alloc_init(msgq, msg_size, max_msgs);
103 }
104 #include <zephyr/syscalls/k_msgq_alloc_init_mrsh.c>
105 #endif /* CONFIG_USERSPACE */
106
k_msgq_cleanup(struct k_msgq * msgq)107 int k_msgq_cleanup(struct k_msgq *msgq)
108 {
109 SYS_PORT_TRACING_OBJ_FUNC_ENTER(k_msgq, cleanup, msgq);
110
111 CHECKIF(z_waitq_head(&msgq->wait_q) != NULL) {
112 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, cleanup, msgq, -EBUSY);
113
114 return -EBUSY;
115 }
116
117 if ((msgq->flags & K_MSGQ_FLAG_ALLOC) != 0U) {
118 k_free(msgq->buffer_start);
119 msgq->flags &= ~K_MSGQ_FLAG_ALLOC;
120 }
121
122 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, cleanup, msgq, 0);
123
124 return 0;
125 }
126
127
z_impl_k_msgq_put(struct k_msgq * msgq,const void * data,k_timeout_t timeout)128 int z_impl_k_msgq_put(struct k_msgq *msgq, const void *data, k_timeout_t timeout)
129 {
130 __ASSERT(!arch_is_in_isr() || K_TIMEOUT_EQ(timeout, K_NO_WAIT), "");
131
132 struct k_thread *pending_thread;
133 k_spinlock_key_t key;
134 int result;
135 bool resched = false;
136
137 key = k_spin_lock(&msgq->lock);
138
139 SYS_PORT_TRACING_OBJ_FUNC_ENTER(k_msgq, put, msgq, timeout);
140
141 if (msgq->used_msgs < msgq->max_msgs) {
142 /* message queue isn't full */
143 pending_thread = z_unpend_first_thread(&msgq->wait_q);
144 if (unlikely(pending_thread != NULL)) {
145 resched = true;
146
147 /* give message to waiting thread */
148 (void)memcpy(pending_thread->base.swap_data, data, msgq->msg_size);
149 /* wake up waiting thread */
150 arch_thread_return_value_set(pending_thread, 0);
151 z_ready_thread(pending_thread);
152 } else {
153 /* put message in queue */
154 __ASSERT_NO_MSG(msgq->write_ptr >= msgq->buffer_start &&
155 msgq->write_ptr < msgq->buffer_end);
156 (void)memcpy(msgq->write_ptr, (char *)data, msgq->msg_size);
157 msgq->write_ptr += msgq->msg_size;
158 if (msgq->write_ptr == msgq->buffer_end) {
159 msgq->write_ptr = msgq->buffer_start;
160 }
161 msgq->used_msgs++;
162 resched = handle_poll_events(msgq);
163 }
164 result = 0;
165 } else if (K_TIMEOUT_EQ(timeout, K_NO_WAIT)) {
166 /* don't wait for message space to become available */
167 result = -ENOMSG;
168 } else {
169 SYS_PORT_TRACING_OBJ_FUNC_BLOCKING(k_msgq, put, msgq, timeout);
170
171 /* wait for put message success, failure, or timeout */
172 _current->base.swap_data = (void *) data;
173
174 result = z_pend_curr(&msgq->lock, key, &msgq->wait_q, timeout);
175 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, put, msgq, timeout, result);
176 return result;
177 }
178
179 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, put, msgq, timeout, result);
180
181 if (resched) {
182 z_reschedule(&msgq->lock, key);
183 } else {
184 k_spin_unlock(&msgq->lock, key);
185 }
186
187 return result;
188 }
189
190 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_put(struct k_msgq * msgq,const void * data,k_timeout_t timeout)191 static inline int z_vrfy_k_msgq_put(struct k_msgq *msgq, const void *data,
192 k_timeout_t timeout)
193 {
194 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
195 K_OOPS(K_SYSCALL_MEMORY_READ(data, msgq->msg_size));
196
197 return z_impl_k_msgq_put(msgq, data, timeout);
198 }
199 #include <zephyr/syscalls/k_msgq_put_mrsh.c>
200 #endif /* CONFIG_USERSPACE */
201
z_impl_k_msgq_get_attrs(struct k_msgq * msgq,struct k_msgq_attrs * attrs)202 void z_impl_k_msgq_get_attrs(struct k_msgq *msgq, struct k_msgq_attrs *attrs)
203 {
204 attrs->msg_size = msgq->msg_size;
205 attrs->max_msgs = msgq->max_msgs;
206 attrs->used_msgs = msgq->used_msgs;
207 }
208
209 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_get_attrs(struct k_msgq * msgq,struct k_msgq_attrs * attrs)210 static inline void z_vrfy_k_msgq_get_attrs(struct k_msgq *msgq,
211 struct k_msgq_attrs *attrs)
212 {
213 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
214 K_OOPS(K_SYSCALL_MEMORY_WRITE(attrs, sizeof(struct k_msgq_attrs)));
215 z_impl_k_msgq_get_attrs(msgq, attrs);
216 }
217 #include <zephyr/syscalls/k_msgq_get_attrs_mrsh.c>
218 #endif /* CONFIG_USERSPACE */
219
z_impl_k_msgq_get(struct k_msgq * msgq,void * data,k_timeout_t timeout)220 int z_impl_k_msgq_get(struct k_msgq *msgq, void *data, k_timeout_t timeout)
221 {
222 __ASSERT(!arch_is_in_isr() || K_TIMEOUT_EQ(timeout, K_NO_WAIT), "");
223
224 k_spinlock_key_t key;
225 struct k_thread *pending_thread;
226 int result;
227 bool resched = false;
228
229 key = k_spin_lock(&msgq->lock);
230
231 SYS_PORT_TRACING_OBJ_FUNC_ENTER(k_msgq, get, msgq, timeout);
232
233 if (msgq->used_msgs > 0U) {
234 /* take first available message from queue */
235 (void)memcpy((char *)data, msgq->read_ptr, msgq->msg_size);
236 msgq->read_ptr += msgq->msg_size;
237 if (msgq->read_ptr == msgq->buffer_end) {
238 msgq->read_ptr = msgq->buffer_start;
239 }
240 msgq->used_msgs--;
241
242 /* handle first thread waiting to write (if any) */
243 pending_thread = z_unpend_first_thread(&msgq->wait_q);
244 if (unlikely(pending_thread != NULL)) {
245 SYS_PORT_TRACING_OBJ_FUNC_BLOCKING(k_msgq, get, msgq, timeout);
246
247 /* add thread's message to queue */
248 __ASSERT_NO_MSG(msgq->write_ptr >= msgq->buffer_start &&
249 msgq->write_ptr < msgq->buffer_end);
250 (void)memcpy(msgq->write_ptr, (char *)pending_thread->base.swap_data,
251 msgq->msg_size);
252 msgq->write_ptr += msgq->msg_size;
253 if (msgq->write_ptr == msgq->buffer_end) {
254 msgq->write_ptr = msgq->buffer_start;
255 }
256 msgq->used_msgs++;
257
258 /* wake up waiting thread */
259 arch_thread_return_value_set(pending_thread, 0);
260 z_ready_thread(pending_thread);
261 resched = true;
262 }
263 result = 0;
264 } else if (K_TIMEOUT_EQ(timeout, K_NO_WAIT)) {
265 /* don't wait for a message to become available */
266 result = -ENOMSG;
267 } else {
268 SYS_PORT_TRACING_OBJ_FUNC_BLOCKING(k_msgq, get, msgq, timeout);
269
270 /* wait for get message success or timeout */
271 _current->base.swap_data = data;
272
273 result = z_pend_curr(&msgq->lock, key, &msgq->wait_q, timeout);
274 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, get, msgq, timeout, result);
275 return result;
276 }
277
278 SYS_PORT_TRACING_OBJ_FUNC_EXIT(k_msgq, get, msgq, timeout, result);
279
280 if (resched) {
281 z_reschedule(&msgq->lock, key);
282 } else {
283 k_spin_unlock(&msgq->lock, key);
284 }
285
286 return result;
287 }
288
289 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_get(struct k_msgq * msgq,void * data,k_timeout_t timeout)290 static inline int z_vrfy_k_msgq_get(struct k_msgq *msgq, void *data,
291 k_timeout_t timeout)
292 {
293 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
294 K_OOPS(K_SYSCALL_MEMORY_WRITE(data, msgq->msg_size));
295
296 return z_impl_k_msgq_get(msgq, data, timeout);
297 }
298 #include <zephyr/syscalls/k_msgq_get_mrsh.c>
299 #endif /* CONFIG_USERSPACE */
300
z_impl_k_msgq_peek(struct k_msgq * msgq,void * data)301 int z_impl_k_msgq_peek(struct k_msgq *msgq, void *data)
302 {
303 k_spinlock_key_t key;
304 int result;
305
306 key = k_spin_lock(&msgq->lock);
307
308 if (msgq->used_msgs > 0U) {
309 /* take first available message from queue */
310 (void)memcpy((char *)data, msgq->read_ptr, msgq->msg_size);
311 result = 0;
312 } else {
313 /* don't wait for a message to become available */
314 result = -ENOMSG;
315 }
316
317 SYS_PORT_TRACING_OBJ_FUNC(k_msgq, peek, msgq, result);
318
319 k_spin_unlock(&msgq->lock, key);
320
321 return result;
322 }
323
324 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_peek(struct k_msgq * msgq,void * data)325 static inline int z_vrfy_k_msgq_peek(struct k_msgq *msgq, void *data)
326 {
327 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
328 K_OOPS(K_SYSCALL_MEMORY_WRITE(data, msgq->msg_size));
329
330 return z_impl_k_msgq_peek(msgq, data);
331 }
332 #include <zephyr/syscalls/k_msgq_peek_mrsh.c>
333 #endif /* CONFIG_USERSPACE */
334
z_impl_k_msgq_peek_at(struct k_msgq * msgq,void * data,uint32_t idx)335 int z_impl_k_msgq_peek_at(struct k_msgq *msgq, void *data, uint32_t idx)
336 {
337 k_spinlock_key_t key;
338 int result;
339 uint32_t bytes_to_end;
340 uint32_t byte_offset;
341 char *start_addr;
342
343 key = k_spin_lock(&msgq->lock);
344
345 if (msgq->used_msgs > idx) {
346 bytes_to_end = (msgq->buffer_end - msgq->read_ptr);
347 byte_offset = idx * msgq->msg_size;
348 start_addr = msgq->read_ptr;
349 /* check item available in start/end of ring buffer */
350 if (bytes_to_end <= byte_offset) {
351 /* Tweak the values in case */
352 byte_offset -= bytes_to_end;
353 /* wrap-around is required */
354 start_addr = msgq->buffer_start;
355 }
356 (void)memcpy(data, start_addr + byte_offset, msgq->msg_size);
357 result = 0;
358 } else {
359 /* don't wait for a message to become available */
360 result = -ENOMSG;
361 }
362
363 SYS_PORT_TRACING_OBJ_FUNC(k_msgq, peek, msgq, result);
364
365 k_spin_unlock(&msgq->lock, key);
366
367 return result;
368 }
369
370 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_peek_at(struct k_msgq * msgq,void * data,uint32_t idx)371 static inline int z_vrfy_k_msgq_peek_at(struct k_msgq *msgq, void *data, uint32_t idx)
372 {
373 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
374 K_OOPS(K_SYSCALL_MEMORY_WRITE(data, msgq->msg_size));
375
376 return z_impl_k_msgq_peek_at(msgq, data, idx);
377 }
378 #include <zephyr/syscalls/k_msgq_peek_at_mrsh.c>
379 #endif /* CONFIG_USERSPACE */
380
z_impl_k_msgq_purge(struct k_msgq * msgq)381 void z_impl_k_msgq_purge(struct k_msgq *msgq)
382 {
383 k_spinlock_key_t key;
384 struct k_thread *pending_thread;
385 bool resched = false;
386
387 key = k_spin_lock(&msgq->lock);
388
389 SYS_PORT_TRACING_OBJ_FUNC(k_msgq, purge, msgq);
390
391 /* wake up any threads that are waiting to write */
392 for (pending_thread = z_unpend_first_thread(&msgq->wait_q);
393 pending_thread != NULL;
394 pending_thread = z_unpend_first_thread(&msgq->wait_q)) {
395 arch_thread_return_value_set(pending_thread, -ENOMSG);
396 z_ready_thread(pending_thread);
397 resched = true;
398 }
399
400 msgq->used_msgs = 0;
401 msgq->read_ptr = msgq->write_ptr;
402
403 if (resched) {
404 z_reschedule(&msgq->lock, key);
405 } else {
406 k_spin_unlock(&msgq->lock, key);
407 }
408 }
409
410 #ifdef CONFIG_USERSPACE
z_vrfy_k_msgq_purge(struct k_msgq * msgq)411 static inline void z_vrfy_k_msgq_purge(struct k_msgq *msgq)
412 {
413 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
414 z_impl_k_msgq_purge(msgq);
415 }
416 #include <zephyr/syscalls/k_msgq_purge_mrsh.c>
417
z_vrfy_k_msgq_num_free_get(struct k_msgq * msgq)418 static inline uint32_t z_vrfy_k_msgq_num_free_get(struct k_msgq *msgq)
419 {
420 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
421 return z_impl_k_msgq_num_free_get(msgq);
422 }
423 #include <zephyr/syscalls/k_msgq_num_free_get_mrsh.c>
424
z_vrfy_k_msgq_num_used_get(struct k_msgq * msgq)425 static inline uint32_t z_vrfy_k_msgq_num_used_get(struct k_msgq *msgq)
426 {
427 K_OOPS(K_SYSCALL_OBJ(msgq, K_OBJ_MSGQ));
428 return z_impl_k_msgq_num_used_get(msgq);
429 }
430 #include <zephyr/syscalls/k_msgq_num_used_get_mrsh.c>
431
432 #endif /* CONFIG_USERSPACE */
433
434 #ifdef CONFIG_OBJ_CORE_MSGQ
init_msgq_obj_core_list(void)435 static int init_msgq_obj_core_list(void)
436 {
437 /* Initialize msgq object type */
438
439 z_obj_type_init(&obj_type_msgq, K_OBJ_TYPE_MSGQ_ID,
440 offsetof(struct k_msgq, obj_core));
441
442 /* Initialize and link statically defined message queues */
443
444 STRUCT_SECTION_FOREACH(k_msgq, msgq) {
445 k_obj_core_init_and_link(K_OBJ_CORE(msgq), &obj_type_msgq);
446 }
447
448 return 0;
449 };
450
451 SYS_INIT(init_msgq_obj_core_list, PRE_KERNEL_1,
452 CONFIG_KERNEL_INIT_PRIORITY_OBJECTS);
453
454 #endif /* CONFIG_OBJ_CORE_MSGQ */
455
