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