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_PRESTART | _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_PRESTART:
312 state = osThreadInactive;
313 break;
314 case _THREAD_DEAD:
315 state = osThreadTerminated;
316 break;
317 case _THREAD_SUSPENDED:
318 case _THREAD_PENDING:
319 state = osThreadBlocked;
320 break;
321 case _THREAD_QUEUED:
322 state = osThreadReady;
323 break;
324 default:
325 state = osThreadError;
326 break;
327 }
328
329 if (osThreadGetId() == thread_id) {
330 state = osThreadRunning;
331 }
332
333 return state;
334 }
335
336 /**
337 * @brief Pass control to next thread that is in READY state.
338 */
osThreadYield(void)339 osStatus_t osThreadYield(void)
340 {
341 if (k_is_in_isr()) {
342 return osErrorISR;
343 }
344
345 k_yield();
346 return osOK;
347 }
348
349 /**
350 * @brief Get stack size of a thread.
351 */
osThreadGetStackSize(osThreadId_t thread_id)352 uint32_t osThreadGetStackSize(osThreadId_t thread_id)
353 {
354 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
355
356 __ASSERT(tid, "");
357 __ASSERT(is_cmsis_rtos_v2_thread(tid), "");
358 __ASSERT(!k_is_in_isr(), "");
359
360 return tid->z_thread.stack_info.size;
361 }
362
363 /**
364 * @brief Get available stack space of a thread based on stack watermark
365 * recording during execution.
366 */
osThreadGetStackSpace(osThreadId_t thread_id)367 uint32_t osThreadGetStackSpace(osThreadId_t thread_id)
368 {
369 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
370 size_t unused;
371 int ret;
372
373 __ASSERT(tid, "");
374 __ASSERT(is_cmsis_rtos_v2_thread(tid), "");
375 __ASSERT(!k_is_in_isr(), "");
376
377 ret = k_thread_stack_space_get(&tid->z_thread, &unused);
378 if (ret != 0) {
379 unused = 0;
380 }
381
382 return (uint32_t)unused;
383 }
384
385 /**
386 * @brief Suspend execution of a thread.
387 */
osThreadSuspend(osThreadId_t thread_id)388 osStatus_t osThreadSuspend(osThreadId_t thread_id)
389 {
390 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
391
392 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
393 return osErrorParameter;
394 }
395
396 if (k_is_in_isr()) {
397 return osErrorISR;
398 }
399
400 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
401 return osErrorResource;
402 }
403
404 k_thread_suspend(&tid->z_thread);
405
406 return osOK;
407 }
408
409 /**
410 * @brief Resume execution of a thread.
411 */
osThreadResume(osThreadId_t thread_id)412 osStatus_t osThreadResume(osThreadId_t thread_id)
413 {
414 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
415
416 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
417 return osErrorParameter;
418 }
419
420 if (k_is_in_isr()) {
421 return osErrorISR;
422 }
423
424 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
425 return osErrorResource;
426 }
427
428 k_thread_resume(&tid->z_thread);
429
430 return osOK;
431 }
432
433 /**
434 * @brief Detach a thread (thread storage can be reclaimed when thread
435 * terminates).
436 */
osThreadDetach(osThreadId_t thread_id)437 osStatus_t osThreadDetach(osThreadId_t thread_id)
438 {
439 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
440
441 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
442 return osErrorParameter;
443 }
444
445 if (k_is_in_isr()) {
446 return osErrorISR;
447 }
448
449 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
450 return osErrorResource;
451 }
452
453 __ASSERT(tid->attr_bits != osThreadDetached,
454 "Thread already detached, behaviour undefined.");
455
456 tid->attr_bits = osThreadDetached;
457
458 k_sem_give(&tid->join_guard);
459
460 return osOK;
461 }
462
463 /**
464 * @brief Wait for specified thread to terminate.
465 */
osThreadJoin(osThreadId_t thread_id)466 osStatus_t osThreadJoin(osThreadId_t thread_id)
467 {
468 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
469 osStatus_t status = osError;
470
471 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
472 return osErrorParameter;
473 }
474
475 if (k_is_in_isr()) {
476 return osErrorISR;
477 }
478
479 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
480 return osErrorResource;
481 }
482
483 if (tid->attr_bits != osThreadJoinable) {
484 return osErrorResource;
485 }
486
487 if (!tid->has_joined) {
488 if (k_sem_take(&tid->join_guard, K_FOREVER) != 0) {
489 __ASSERT(0, "Failed to take from join guard.");
490 }
491
492 k_sem_give(&tid->join_guard);
493 }
494
495 if (tid->has_joined && (tid->attr_bits == osThreadJoinable)) {
496 status = osOK;
497 } else {
498 status = osErrorResource;
499 }
500
501 return status;
502 }
503
504 /**
505 * @brief Terminate execution of current running thread.
506 */
osThreadExit(void)507 __NO_RETURN void osThreadExit(void)
508 {
509 struct cv2_thread *tid;
510
511 __ASSERT(!k_is_in_isr(), "");
512 tid = osThreadGetId();
513
514 k_sem_give(&tid->join_guard);
515
516 k_thread_abort((k_tid_t)&tid->z_thread);
517
518 CODE_UNREACHABLE;
519 }
520
521 /**
522 * @brief Terminate execution of a thread.
523 */
osThreadTerminate(osThreadId_t thread_id)524 osStatus_t osThreadTerminate(osThreadId_t thread_id)
525 {
526 struct cv2_thread *tid = (struct cv2_thread *)thread_id;
527
528 if ((tid == NULL) || (is_cmsis_rtos_v2_thread(tid) == NULL)) {
529 return osErrorParameter;
530 }
531
532 if (k_is_in_isr()) {
533 return osErrorISR;
534 }
535
536 if (_is_thread_cmsis_inactive(&tid->z_thread)) {
537 return osErrorResource;
538 }
539
540 k_sem_give(&tid->join_guard);
541
542 k_thread_abort((k_tid_t)&tid->z_thread);
543 return osOK;
544 }
545
546
547 /**
548 * @brief Get number of active threads.
549 */
osThreadGetCount(void)550 uint32_t osThreadGetCount(void)
551 {
552 struct k_thread *thread;
553 uint32_t count = 0U;
554
555 __ASSERT(!k_is_in_isr(), "");
556 for (thread = _kernel.threads; thread; thread = thread->next_thread) {
557 if (get_cmsis_thread_id(thread) && z_is_thread_queued(thread)) {
558 count++;
559 }
560 }
561
562 return count;
563 }
564
565 /**
566 * @brief Enumerate active threads.
567 */
osThreadEnumerate(osThreadId_t * thread_array,uint32_t array_items)568 uint32_t osThreadEnumerate(osThreadId_t *thread_array, uint32_t array_items)
569 {
570 struct k_thread *thread;
571 uint32_t count = 0U;
572 osThreadId_t tid;
573
574 __ASSERT(!k_is_in_isr(), "");
575 __ASSERT(thread_array != NULL, "");
576 __ASSERT(array_items, "");
577
578 for (thread = _kernel.threads; thread; thread = thread->next_thread) {
579 if (count == array_items) {
580 break;
581 }
582
583 tid = get_cmsis_thread_id(thread);
584 if (tid != NULL) {
585 thread_array[count] = tid;
586 count++;
587 }
588 }
589
590 return (count);
591 }
592
