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