1 /*
2 * Copyright (c) 2018 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/portability/cmsis_types.h>
9 #include <string.h>
10 #include "wrapper.h"
11
12 K_MEM_SLAB_DEFINE(cmsis_rtos_msgq_cb_slab, sizeof(struct cmsis_rtos_msgq_cb),
13 CONFIG_CMSIS_V2_MSGQ_MAX_COUNT, 4);
14
15 static const osMessageQueueAttr_t init_msgq_attrs = {
16 .name = "ZephyrMsgQ",
17 .attr_bits = 0,
18 .cb_mem = NULL,
19 .cb_size = 0,
20 .mq_mem = NULL,
21 .mq_size = 0,
22 };
23
24 /**
25 * @brief Create and Initialize Message queue.
26 */
osMessageQueueNew(uint32_t msg_count,uint32_t msg_size,const osMessageQueueAttr_t * attr)27 osMessageQueueId_t osMessageQueueNew(uint32_t msg_count, uint32_t msg_size,
28 const osMessageQueueAttr_t *attr)
29 {
30 struct cmsis_rtos_msgq_cb *msgq;
31
32 BUILD_ASSERT(K_HEAP_MEM_POOL_SIZE >= CONFIG_CMSIS_V2_MSGQ_MAX_DYNAMIC_SIZE,
33 "heap must be configured to be at least the max dynamic size");
34
35 if (k_is_in_isr()) {
36 return NULL;
37 }
38
39 if ((attr != NULL) && (attr->mq_size < msg_count * msg_size)) {
40 return NULL;
41 }
42
43 if (attr == NULL) {
44 attr = &init_msgq_attrs;
45 }
46
47 if (attr->cb_mem != NULL) {
48 __ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_msgq_cb), "Invalid cb_size\n");
49 msgq = (struct cmsis_rtos_msgq_cb *)attr->cb_mem;
50 } else if (k_mem_slab_alloc(&cmsis_rtos_msgq_cb_slab, (void **)&msgq, K_MSEC(100)) != 0) {
51 return NULL;
52 }
53 (void)memset(msgq, 0, sizeof(struct cmsis_rtos_msgq_cb));
54 msgq->is_cb_dynamic_allocation = attr->cb_mem == NULL;
55
56 if (attr->mq_mem == NULL) {
57 __ASSERT((msg_count * msg_size) <= CONFIG_CMSIS_V2_MSGQ_MAX_DYNAMIC_SIZE,
58 "message queue size exceeds dynamic maximum");
59
60 #if (K_HEAP_MEM_POOL_SIZE > 0)
61 msgq->pool = k_calloc(msg_count, msg_size);
62 if (msgq->pool == NULL) {
63 if (msgq->is_cb_dynamic_allocation) {
64 k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
65 }
66 return NULL;
67 }
68 msgq->is_dynamic_allocation = TRUE;
69 #else
70 if (msgq->is_cb_dynamic_allocation) {
71 k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
72 }
73 return NULL;
74 #endif
75 } else {
76 msgq->pool = attr->mq_mem;
77 msgq->is_dynamic_allocation = FALSE;
78 }
79
80 k_msgq_init(&msgq->z_msgq, msgq->pool, msg_size, msg_count);
81
82 msgq->name = (attr->name == NULL) ? init_msgq_attrs.name : attr->name;
83
84 return (osMessageQueueId_t)(msgq);
85 }
86
87 /**
88 * @brief Put a message to a Queue.
89 */
osMessageQueuePut(osMessageQueueId_t msgq_id,const void * msg_ptr,uint8_t msg_prio,uint32_t timeout)90 osStatus_t osMessageQueuePut(osMessageQueueId_t msgq_id, const void *msg_ptr, uint8_t msg_prio,
91 uint32_t timeout)
92 {
93 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
94 int retval;
95
96 ARG_UNUSED(msg_prio);
97
98 if (msgq == NULL) {
99 return osErrorParameter;
100 }
101
102 /* Can be called from ISRs only if timeout is set to 0 */
103 if (timeout > 0 && k_is_in_isr()) {
104 return osErrorParameter;
105 }
106
107 if (timeout == 0U) {
108 retval = k_msgq_put(&msgq->z_msgq, (void *)msg_ptr, K_NO_WAIT);
109 } else if (timeout == osWaitForever) {
110 retval = k_msgq_put(&msgq->z_msgq, (void *)msg_ptr, K_FOREVER);
111 } else {
112 retval = k_msgq_put(&msgq->z_msgq, (void *)msg_ptr, K_TICKS(timeout));
113 }
114
115 if (retval == 0) {
116 return osOK;
117 } else if (retval == -EAGAIN) {
118 return osErrorTimeout;
119 } else {
120 return osErrorResource;
121 }
122 }
123
124 /**
125 * @brief Get a message or Wait for a Message from a Queue.
126 */
osMessageQueueGet(osMessageQueueId_t msgq_id,void * msg_ptr,uint8_t * msg_prio,uint32_t timeout)127 osStatus_t osMessageQueueGet(osMessageQueueId_t msgq_id, void *msg_ptr, uint8_t *msg_prio,
128 uint32_t timeout)
129 {
130 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
131 int retval;
132
133 ARG_UNUSED(msg_prio);
134
135 if (msgq == NULL) {
136 return osErrorParameter;
137 }
138
139 /* Can be called from ISRs only if timeout is set to 0 */
140 if (timeout > 0 && k_is_in_isr()) {
141 return osErrorParameter;
142 }
143
144 if (timeout == 0U) {
145 retval = k_msgq_get(&msgq->z_msgq, msg_ptr, K_NO_WAIT);
146 } else if (timeout == osWaitForever) {
147 retval = k_msgq_get(&msgq->z_msgq, msg_ptr, K_FOREVER);
148 } else {
149 retval = k_msgq_get(&msgq->z_msgq, msg_ptr, K_TICKS(timeout));
150 }
151
152 if (retval == 0) {
153 return osOK;
154 } else if (retval == -EAGAIN) {
155 return osErrorTimeout;
156 } else if (retval == -ENOMSG) {
157 return osErrorResource;
158 }
159
160 return osOK;
161 }
162
163 /**
164 * @brief Get maximum number of messages in a Message Queue.
165 */
osMessageQueueGetCapacity(osMessageQueueId_t msgq_id)166 uint32_t osMessageQueueGetCapacity(osMessageQueueId_t msgq_id)
167 {
168 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
169
170 if (msgq == NULL) {
171 return 0;
172 } else {
173 return msgq->z_msgq.max_msgs;
174 }
175 }
176
177 /**
178 * @brief Get maximum message size in a Message Queue.
179 */
osMessageQueueGetMsgSize(osMessageQueueId_t msgq_id)180 uint32_t osMessageQueueGetMsgSize(osMessageQueueId_t msgq_id)
181 {
182 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
183
184 if (msgq == NULL) {
185 return 0;
186 } else {
187 return msgq->z_msgq.msg_size;
188 }
189 }
190
191 /**
192 * @brief Get number of queued messages in a Message Queue.
193 */
osMessageQueueGetCount(osMessageQueueId_t msgq_id)194 uint32_t osMessageQueueGetCount(osMessageQueueId_t msgq_id)
195 {
196 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
197
198 if (msgq == NULL) {
199 return 0;
200 } else {
201 return k_msgq_num_used_get(&msgq->z_msgq);
202 }
203 }
204
205 /**
206 * @brief Get number of available slots for messages in a Message Queue.
207 */
osMessageQueueGetSpace(osMessageQueueId_t msgq_id)208 uint32_t osMessageQueueGetSpace(osMessageQueueId_t msgq_id)
209 {
210 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
211
212 if (msgq == NULL) {
213 return 0;
214 } else {
215 return k_msgq_num_free_get(&msgq->z_msgq);
216 }
217 }
218
219 /**
220 * @brief Get name of a Message Queue object.
221 * This function may be called from Interrupt Service Routines.
222 */
osMessageQueueGetName(osMessageQueueId_t msgq_id)223 const char *osMessageQueueGetName(osMessageQueueId_t msgq_id)
224 {
225 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
226
227 if (msgq == NULL) {
228 return NULL;
229 }
230 return msgq->name;
231 }
232
233 /**
234 * @brief Reset a Message Queue to initial empty state.
235 */
osMessageQueueReset(osMessageQueueId_t msgq_id)236 osStatus_t osMessageQueueReset(osMessageQueueId_t msgq_id)
237 {
238 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
239
240 if (msgq == NULL) {
241 return osErrorParameter;
242 }
243
244 if (k_is_in_isr()) {
245 return osErrorISR;
246 }
247
248 /* The status code "osErrorResource" (the message queue specified by
249 * parameter msgq_id is in an invalid message queue state) is not
250 * supported in Zephyr.
251 */
252
253 k_msgq_purge(&msgq->z_msgq);
254
255 return osOK;
256 }
257
258 /**
259 * @brief Delete a Message Queue object.
260 */
osMessageQueueDelete(osMessageQueueId_t msgq_id)261 osStatus_t osMessageQueueDelete(osMessageQueueId_t msgq_id)
262 {
263 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
264
265 if (msgq == NULL) {
266 return osErrorParameter;
267 }
268
269 if (k_is_in_isr()) {
270 return osErrorISR;
271 }
272
273 /* The status code "osErrorResource" (the message queue specified by
274 * parameter msgq_id is in an invalid message queue state) is not
275 * supported in Zephyr.
276 */
277
278 if (msgq->is_dynamic_allocation) {
279 k_free(msgq->pool);
280 }
281 if (msgq->is_cb_dynamic_allocation) {
282 k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
283 }
284 return osOK;
285 }
286