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