1 /*
2 * Copyright (c) 2018 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <ksched.h>
9 #include <stdio.h>
10 #include <string.h>
11 #include <zephyr/sys/atomic.h>
12 #include <zephyr/debug/stack.h>
13 #include <zephyr/portability/cmsis_types.h>
14 #include "wrapper.h"
15
16 static const osThreadAttr_t init_thread_attrs = {
17 .name = "ZephyrThread",
18 .attr_bits = osThreadDetached,
19 .cb_mem = NULL,
20 .cb_size = 0,
21 .stack_mem = NULL,
22 .stack_size = 0,
23 .priority = osPriorityNormal,
24 .tz_module = 0,
25 .reserved = 0,
26 };
27
28 static sys_dlist_t thread_list;
29
30 static atomic_t num_dynamic_cb;
31 #if CONFIG_CMSIS_V2_THREAD_MAX_COUNT != 0
32 static struct cmsis_rtos_thread_cb cmsis_rtos_thread_cb_pool[CONFIG_CMSIS_V2_THREAD_MAX_COUNT];
33 #endif
34
35 static atomic_t num_dynamic_stack;
36 #if CONFIG_CMSIS_V2_THREAD_DYNAMIC_MAX_COUNT != 0
37 static K_THREAD_STACK_ARRAY_DEFINE(cmsis_rtos_thread_stack_pool,
38 CONFIG_CMSIS_V2_THREAD_DYNAMIC_MAX_COUNT,
39 CONFIG_CMSIS_V2_THREAD_DYNAMIC_STACK_SIZE);
40 #endif
41
_is_thread_cmsis_inactive(struct k_thread * thread)42 static inline int _is_thread_cmsis_inactive(struct k_thread *thread)
43 {
44 uint8_t state = thread->base.thread_state;
45
46 return state & _THREAD_DEAD;
47 }
48
zephyr_to_cmsis_priority(uint32_t z_prio)49 static inline uint32_t zephyr_to_cmsis_priority(uint32_t z_prio)
50 {
51 return (osPriorityISR - z_prio);
52 }
53
cmsis_to_zephyr_priority(uint32_t c_prio)54 static inline uint32_t cmsis_to_zephyr_priority(uint32_t c_prio)
55 {
56 return (osPriorityISR - c_prio);
57 }
58
zephyr_thread_wrapper(void * arg1,void * arg2,void * arg3)59 static void zephyr_thread_wrapper(void *arg1, void *arg2, void *arg3)
60 {
61 osThreadFunc_t fun_ptr = arg3;
62 fun_ptr(arg1);
63 }
64
is_cmsis_rtos_v2_thread(void * thread_id)65 void *is_cmsis_rtos_v2_thread(void *thread_id)
66 {
67 sys_dnode_t *pnode;
68 struct cmsis_rtos_thread_cb *itr;
69
70 SYS_DLIST_FOR_EACH_NODE(&thread_list, pnode) {
71 itr = CONTAINER_OF(pnode, struct cmsis_rtos_thread_cb, node);
72
73 if ((void *)itr == thread_id) {
74 return itr;
75 }
76 }
77
78 return NULL;
79 }
80
get_cmsis_thread_id(k_tid_t tid)81 osThreadId_t get_cmsis_thread_id(k_tid_t tid)
82 {
83 sys_dnode_t *pnode;
84 struct cmsis_rtos_thread_cb *itr;
85
86 if (tid != NULL) {
87 SYS_DLIST_FOR_EACH_NODE(&thread_list, pnode) {
88 itr = CONTAINER_OF(pnode, struct cmsis_rtos_thread_cb, node);
89
90 if (&itr->z_thread == tid) {
91 return (osThreadId_t)itr;
92 }
93 }
94 }
95
96 return NULL;
97 }
98
99 /**
100 * @brief Create a thread and add it to Active Threads.
101 */
osThreadNew(osThreadFunc_t threadfunc,void * arg,const osThreadAttr_t * attr)102 osThreadId_t osThreadNew(osThreadFunc_t threadfunc, void *arg, const osThreadAttr_t *attr)
103 {
104 int32_t prio;
105 osPriority_t cv2_prio;
106 struct cmsis_rtos_thread_cb *tid;
107 static uint32_t one_time;
108 void *stack;
109 size_t stack_size;
110
111 if (k_is_in_isr()) {
112 return NULL;
113 }
114
115 if (attr == NULL) {
116 attr = &init_thread_attrs;
117 }
118
119 if (attr->priority == osPriorityNone) {
120 cv2_prio = osPriorityNormal;
121 } else {
122 cv2_prio = attr->priority;
123 }
124
125 if (attr->cb_mem == NULL && num_dynamic_cb >= CONFIG_CMSIS_V2_THREAD_MAX_COUNT) {
126 return NULL;
127 }
128
129 if (attr->stack_mem == NULL &&
130 num_dynamic_stack >= CONFIG_CMSIS_V2_THREAD_DYNAMIC_MAX_COUNT) {
131 return NULL;
132 }
133
134 BUILD_ASSERT(osPriorityISR <= CONFIG_NUM_PREEMPT_PRIORITIES,
135 "Configure NUM_PREEMPT_PRIORITIES to at least osPriorityISR");
136
137 BUILD_ASSERT(CONFIG_CMSIS_V2_THREAD_DYNAMIC_STACK_SIZE <=
138 CONFIG_CMSIS_V2_THREAD_MAX_STACK_SIZE,
139 "Default dynamic thread stack size cannot exceed max stack size");
140
141 __ASSERT(attr->stack_size <= CONFIG_CMSIS_V2_THREAD_MAX_STACK_SIZE, "invalid stack size\n");
142
143 __ASSERT((cv2_prio >= osPriorityIdle) && (cv2_prio <= osPriorityISR), "invalid priority\n");
144
145 if (attr->stack_mem != NULL && attr->stack_size == 0) {
146 return NULL;
147 }
148
149 #if CONFIG_CMSIS_V2_THREAD_MAX_COUNT != 0
150 if (attr->cb_mem == NULL) {
151 uint32_t this_dynamic_cb;
152 this_dynamic_cb = atomic_inc(&num_dynamic_cb);
153 tid = &cmsis_rtos_thread_cb_pool[this_dynamic_cb];
154 } else
155 #endif
156 {
157 tid = (struct cmsis_rtos_thread_cb *)attr->cb_mem;
158 }
159
160 tid->attr_bits = attr->attr_bits;
161
162 #if CONFIG_CMSIS_V2_THREAD_DYNAMIC_MAX_COUNT != 0
163 if (attr->stack_mem == NULL) {
164 uint32_t this_dynamic_stack;
165 __ASSERT(CONFIG_CMSIS_V2_THREAD_DYNAMIC_STACK_SIZE > 0,
166 "dynamic stack size must be configured to be non-zero\n");
167 this_dynamic_stack = atomic_inc(&num_dynamic_stack);
168 stack_size = CONFIG_CMSIS_V2_THREAD_DYNAMIC_STACK_SIZE;
169 stack = cmsis_rtos_thread_stack_pool[this_dynamic_stack];
170 } else
171 #endif
172 {
173 stack_size = attr->stack_size;
174 stack = attr->stack_mem;
175 }
176
177 k_poll_signal_init(&tid->poll_signal);
178 k_poll_event_init(&tid->poll_event, K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY,
179 &tid->poll_signal);
180 tid->signal_results = 0U;
181
182 /* TODO: Do this somewhere only once */
183 if (one_time == 0U) {
184 sys_dlist_init(&thread_list);
185 one_time = 1U;
186 }
187
188 sys_dlist_append(&thread_list, &tid->node);
189
190 prio = cmsis_to_zephyr_priority(cv2_prio);
191
192 (void)k_thread_create(&tid->z_thread, stack, stack_size, zephyr_thread_wrapper, (void *)arg,
193 NULL, threadfunc, prio, 0, K_NO_WAIT);
194
195 if (attr->name == NULL) {
196 strncpy(tid->name, init_thread_attrs.name, sizeof(tid->name) - 1);
197 } else {
198 strncpy(tid->name, attr->name, sizeof(tid->name) - 1);
199 }
200
201 k_thread_name_set(&tid->z_thread, tid->name);
202
203 return (osThreadId_t)tid;
204 }
205
206 /**
207 * @brief Get name of a thread.
208 */
osThreadGetName(osThreadId_t thread_id)209 const char *osThreadGetName(osThreadId_t thread_id)
210 {
211 const char *name = NULL;
212
213 if (k_is_in_isr() || (thread_id == NULL)) {
214 name = NULL;
215 } else {
216 if (is_cmsis_rtos_v2_thread(thread_id) == NULL) {
217 name = NULL;
218 } else {
219 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
220
221 name = k_thread_name_get(&tid->z_thread);
222 }
223 }
224
225 return name;
226 }
227
228 /**
229 * @brief Return the thread ID of the current running thread.
230 */
osThreadGetId(void)231 osThreadId_t osThreadGetId(void)
232 {
233 k_tid_t tid = k_current_get();
234
235 return get_cmsis_thread_id(tid);
236 }
237
238 /**
239 * @brief Get current priority of an active thread.
240 */
osThreadGetPriority(osThreadId_t thread_id)241 osPriority_t osThreadGetPriority(osThreadId_t thread_id)
242 {
243 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
244 uint32_t priority;
245
246 if (k_is_in_isr() || (tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL) ||
247 (_is_thread_cmsis_inactive(&tid->z_thread))) {
248 return osPriorityError;
249 }
250
251 priority = k_thread_priority_get(&tid->z_thread);
252 return zephyr_to_cmsis_priority(priority);
253 }
254
255 /**
256 * @brief Change priority of an active thread.
257 */
osThreadSetPriority(osThreadId_t thread_id,osPriority_t priority)258 osStatus_t osThreadSetPriority(osThreadId_t thread_id, osPriority_t priority)
259 {
260 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
261
262 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL) ||
263 (priority <= osPriorityNone) || (priority > osPriorityISR)) {
264 return osErrorParameter;
265 }
266
267 if (k_is_in_isr()) {
268 return osErrorISR;
269 }
270
271 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
272 return osErrorResource;
273 }
274
275 k_thread_priority_set((k_tid_t)&tid->z_thread, cmsis_to_zephyr_priority(priority));
276
277 return osOK;
278 }
279
280 /**
281 * @brief Get current thread state of a thread.
282 */
osThreadGetState(osThreadId_t thread_id)283 osThreadState_t osThreadGetState(osThreadId_t thread_id)
284 {
285 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
286 osThreadState_t state;
287
288 if (k_is_in_isr() || (tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
289 return osThreadError;
290 }
291
292 switch (tid->z_thread.base.thread_state) {
293 case _THREAD_DUMMY:
294 state = osThreadError;
295 break;
296 case _THREAD_DEAD:
297 state = osThreadTerminated;
298 break;
299 case _THREAD_SUSPENDED:
300 case _THREAD_SLEEPING:
301 case _THREAD_PENDING:
302 state = osThreadBlocked;
303 break;
304 case _THREAD_QUEUED:
305 state = osThreadReady;
306 break;
307 default:
308 state = osThreadError;
309 break;
310 }
311
312 if (osThreadGetId() == thread_id) {
313 state = osThreadRunning;
314 }
315
316 return state;
317 }
318
319 /**
320 * @brief Pass control to next thread that is in READY state.
321 */
osThreadYield(void)322 osStatus_t osThreadYield(void)
323 {
324 if (k_is_in_isr()) {
325 return osErrorISR;
326 }
327
328 k_yield();
329 return osOK;
330 }
331
332 /**
333 * @brief Get stack size of a thread.
334 */
osThreadGetStackSize(osThreadId_t thread_id)335 uint32_t osThreadGetStackSize(osThreadId_t thread_id)
336 {
337 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
338
339 __ASSERT(tid, "");
340 __ASSERT(is_cmsis_rtos_v2_thread(tid), "");
341 __ASSERT(!k_is_in_isr(), "");
342
343 return tid->z_thread.stack_info.size;
344 }
345
346 /**
347 * @brief Get available stack space of a thread based on stack watermark
348 * recording during execution.
349 */
osThreadGetStackSpace(osThreadId_t thread_id)350 uint32_t osThreadGetStackSpace(osThreadId_t thread_id)
351 {
352 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
353 size_t unused;
354 int ret;
355
356 __ASSERT(tid, "");
357 __ASSERT(is_cmsis_rtos_v2_thread(tid), "");
358 __ASSERT(!k_is_in_isr(), "");
359
360 ret = k_thread_stack_space_get(&tid->z_thread, &unused);
361 if (ret != 0) {
362 unused = 0;
363 }
364
365 return (uint32_t)unused;
366 }
367
368 /**
369 * @brief Suspend execution of a thread.
370 */
osThreadSuspend(osThreadId_t thread_id)371 osStatus_t osThreadSuspend(osThreadId_t thread_id)
372 {
373 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
374
375 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
376 return osErrorParameter;
377 }
378
379 if (k_is_in_isr()) {
380 return osErrorISR;
381 }
382
383 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
384 return osErrorResource;
385 }
386
387 k_thread_suspend(&tid->z_thread);
388
389 return osOK;
390 }
391
392 /**
393 * @brief Resume execution of a thread.
394 */
osThreadResume(osThreadId_t thread_id)395 osStatus_t osThreadResume(osThreadId_t thread_id)
396 {
397 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
398
399 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
400 return osErrorParameter;
401 }
402
403 if (k_is_in_isr()) {
404 return osErrorISR;
405 }
406
407 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
408 return osErrorResource;
409 }
410
411 k_thread_resume(&tid->z_thread);
412
413 return osOK;
414 }
415
416 /**
417 * @brief Detach a thread (thread storage can be reclaimed when thread
418 * terminates).
419 */
osThreadDetach(osThreadId_t thread_id)420 osStatus_t osThreadDetach(osThreadId_t thread_id)
421 {
422 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
423
424 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
425 return osErrorParameter;
426 }
427
428 if (k_is_in_isr()) {
429 return osErrorISR;
430 }
431
432 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
433 return osErrorResource;
434 }
435
436 __ASSERT(tid->attr_bits != osThreadDetached,
437 "Thread already detached, behaviour undefined.");
438
439 tid->attr_bits = osThreadDetached;
440
441 return osOK;
442 }
443
444 /**
445 * @brief Wait for specified thread to terminate.
446 */
osThreadJoin(osThreadId_t thread_id)447 osStatus_t osThreadJoin(osThreadId_t thread_id)
448 {
449 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
450 int ret = 0;
451
452 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
453 return osErrorParameter;
454 }
455
456 if (k_is_in_isr()) {
457 return osErrorISR;
458 }
459
460 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
461 return osErrorResource;
462 }
463
464 if (tid->attr_bits != osThreadJoinable) {
465 return osErrorResource;
466 }
467
468 ret = k_thread_join(&tid->z_thread, K_FOREVER);
469 return (ret == 0) ? osOK : osErrorResource;
470 }
471
472 /**
473 * @brief Terminate execution of current running thread.
474 */
osThreadExit(void)475 __NO_RETURN void osThreadExit(void)
476 {
477 struct cmsis_rtos_thread_cb *tid;
478
479 __ASSERT(!k_is_in_isr(), "");
480 tid = osThreadGetId();
481
482 k_thread_abort((k_tid_t)&tid->z_thread);
483
484 CODE_UNREACHABLE;
485 }
486
487 /**
488 * @brief Terminate execution of a thread.
489 */
osThreadTerminate(osThreadId_t thread_id)490 osStatus_t osThreadTerminate(osThreadId_t thread_id)
491 {
492 struct cmsis_rtos_thread_cb *tid = (struct cmsis_rtos_thread_cb *)thread_id;
493
494 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
495 return osErrorParameter;
496 }
497
498 if (k_is_in_isr()) {
499 return osErrorISR;
500 }
501
502 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
503 return osErrorResource;
504 }
505
506 k_thread_abort((k_tid_t)&tid->z_thread);
507 return osOK;
508 }
509
510 /**
511 * @brief Get number of active threads.
512 */
osThreadGetCount(void)513 uint32_t osThreadGetCount(void)
514 {
515 struct k_thread *thread;
516 uint32_t count = 0U;
517
518 __ASSERT(!k_is_in_isr(), "");
519 for (thread = _kernel.threads; thread; thread = thread->next_thread) {
520 if (get_cmsis_thread_id(thread) && z_is_thread_queued(thread)) {
521 count++;
522 }
523 }
524
525 return count;
526 }
527
528 /**
529 * @brief Enumerate active threads.
530 */
osThreadEnumerate(osThreadId_t * thread_array,uint32_t array_items)531 uint32_t osThreadEnumerate(osThreadId_t *thread_array, uint32_t array_items)
532 {
533 struct k_thread *thread;
534 uint32_t count = 0U;
535 osThreadId_t tid;
536
537 __ASSERT(!k_is_in_isr(), "");
538 __ASSERT(thread_array != NULL, "");
539 __ASSERT(array_items, "");
540
541 for (thread = _kernel.threads; thread; thread = thread->next_thread) {
542 if (count == array_items) {
543 break;
544 }
545
546 tid = get_cmsis_thread_id(thread);
547 if (tid != NULL) {
548 thread_array[count] = tid;
549 count++;
550 }
551 }
552
553 return (count);
554 }
555
