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 if (attr->name == NULL) {
83 strncpy(msgq->name, init_msgq_attrs.name, sizeof(msgq->name) - 1);
84 } else {
85 strncpy(msgq->name, attr->name, sizeof(msgq->name) - 1);
86 }
87
88 return (osMessageQueueId_t)(msgq);
89 }
90
91 /**
92 * @brief Put a message to a Queue.
93 */
osMessageQueuePut(osMessageQueueId_t msgq_id,const void * msg_ptr,uint8_t msg_prio,uint32_t timeout)94 osStatus_t osMessageQueuePut(osMessageQueueId_t msgq_id, const void *msg_ptr, uint8_t msg_prio,
95 uint32_t timeout)
96 {
97 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
98 int retval;
99
100 ARG_UNUSED(msg_prio);
101
102 if (msgq == NULL) {
103 return osErrorParameter;
104 }
105
106 /* Can be called from ISRs only if timeout is set to 0 */
107 if (timeout > 0 && k_is_in_isr()) {
108 return osErrorParameter;
109 }
110
111 if (timeout == 0U) {
112 retval = k_msgq_put(&msgq->z_msgq, (void *)msg_ptr, K_NO_WAIT);
113 } else if (timeout == osWaitForever) {
114 retval = k_msgq_put(&msgq->z_msgq, (void *)msg_ptr, K_FOREVER);
115 } else {
116 retval = k_msgq_put(&msgq->z_msgq, (void *)msg_ptr, K_TICKS(timeout));
117 }
118
119 if (retval == 0) {
120 return osOK;
121 } else if (retval == -EAGAIN) {
122 return osErrorTimeout;
123 } else {
124 return osErrorResource;
125 }
126 }
127
128 /**
129 * @brief Get a message or Wait for a Message from a Queue.
130 */
osMessageQueueGet(osMessageQueueId_t msgq_id,void * msg_ptr,uint8_t * msg_prio,uint32_t timeout)131 osStatus_t osMessageQueueGet(osMessageQueueId_t msgq_id, void *msg_ptr, uint8_t *msg_prio,
132 uint32_t timeout)
133 {
134 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
135 int retval;
136
137 ARG_UNUSED(msg_prio);
138
139 if (msgq == NULL) {
140 return osErrorParameter;
141 }
142
143 /* Can be called from ISRs only if timeout is set to 0 */
144 if (timeout > 0 && k_is_in_isr()) {
145 return osErrorParameter;
146 }
147
148 if (timeout == 0U) {
149 retval = k_msgq_get(&msgq->z_msgq, msg_ptr, K_NO_WAIT);
150 } else if (timeout == osWaitForever) {
151 retval = k_msgq_get(&msgq->z_msgq, msg_ptr, K_FOREVER);
152 } else {
153 retval = k_msgq_get(&msgq->z_msgq, msg_ptr, K_TICKS(timeout));
154 }
155
156 if (retval == 0) {
157 return osOK;
158 } else if (retval == -EAGAIN) {
159 return osErrorTimeout;
160 } else if (retval == -ENOMSG) {
161 return osErrorResource;
162 }
163
164 return osOK;
165 }
166
167 /**
168 * @brief Get maximum number of messages in a Message Queue.
169 */
osMessageQueueGetCapacity(osMessageQueueId_t msgq_id)170 uint32_t osMessageQueueGetCapacity(osMessageQueueId_t msgq_id)
171 {
172 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
173
174 if (msgq == NULL) {
175 return 0;
176 } else {
177 return msgq->z_msgq.max_msgs;
178 }
179 }
180
181 /**
182 * @brief Get maximum message size in a Message Queue.
183 */
osMessageQueueGetMsgSize(osMessageQueueId_t msgq_id)184 uint32_t osMessageQueueGetMsgSize(osMessageQueueId_t msgq_id)
185 {
186 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
187
188 if (msgq == NULL) {
189 return 0;
190 } else {
191 return msgq->z_msgq.msg_size;
192 }
193 }
194
195 /**
196 * @brief Get number of queued messages in a Message Queue.
197 */
osMessageQueueGetCount(osMessageQueueId_t msgq_id)198 uint32_t osMessageQueueGetCount(osMessageQueueId_t msgq_id)
199 {
200 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
201
202 if (msgq == NULL) {
203 return 0;
204 } else {
205 return k_msgq_num_used_get(&msgq->z_msgq);
206 }
207 }
208
209 /**
210 * @brief Get number of available slots for messages in a Message Queue.
211 */
osMessageQueueGetSpace(osMessageQueueId_t msgq_id)212 uint32_t osMessageQueueGetSpace(osMessageQueueId_t msgq_id)
213 {
214 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
215
216 if (msgq == NULL) {
217 return 0;
218 } else {
219 return k_msgq_num_free_get(&msgq->z_msgq);
220 }
221 }
222
223 /**
224 * @brief Get name of a Message Queue object.
225 */
osMessageQueueGetName(osMessageQueueId_t msgq_id)226 const char *osMessageQueueGetName(osMessageQueueId_t msgq_id)
227 {
228 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
229
230 if (!k_is_in_isr() && (msgq_id != NULL)) {
231 return msgq->name;
232 } else {
233 return NULL;
234 }
235 }
236
237 /**
238 * @brief Reset a Message Queue to initial empty state.
239 */
osMessageQueueReset(osMessageQueueId_t msgq_id)240 osStatus_t osMessageQueueReset(osMessageQueueId_t msgq_id)
241 {
242 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
243
244 if (msgq == NULL) {
245 return osErrorParameter;
246 }
247
248 if (k_is_in_isr()) {
249 return osErrorISR;
250 }
251
252 /* The status code "osErrorResource" (the message queue specified by
253 * parameter msgq_id is in an invalid message queue state) is not
254 * supported in Zephyr.
255 */
256
257 k_msgq_purge(&msgq->z_msgq);
258
259 return osOK;
260 }
261
262 /**
263 * @brief Delete a Message Queue object.
264 */
osMessageQueueDelete(osMessageQueueId_t msgq_id)265 osStatus_t osMessageQueueDelete(osMessageQueueId_t msgq_id)
266 {
267 struct cmsis_rtos_msgq_cb *msgq = (struct cmsis_rtos_msgq_cb *)msgq_id;
268
269 if (msgq == NULL) {
270 return osErrorParameter;
271 }
272
273 if (k_is_in_isr()) {
274 return osErrorISR;
275 }
276
277 /* The status code "osErrorResource" (the message queue specified by
278 * parameter msgq_id is in an invalid message queue state) is not
279 * supported in Zephyr.
280 */
281
282 if (msgq->is_dynamic_allocation) {
283 k_free(msgq->pool);
284 }
285 if (msgq->is_cb_dynamic_allocation) {
286 k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
287 }
288 return osOK;
289 }
290
