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