1 /*
2 * Copyright (c) 2018-2023, Arm Limited. All rights reserved.
3 * Copyright (c) 2023 Cypress Semiconductor Corporation (an Infineon
4 * company) or an affiliate of Cypress Semiconductor Corporation. All rights
5 * reserved.
6 *
7 * SPDX-License-Identifier: BSD-3-Clause
8 *
9 */
10
11 #include <stdint.h>
12 #include "thread.h"
13 #include "tfm_arch.h"
14 #include "utilities.h"
15 #include "critical_section.h"
16
17 /* Declaration of current thread pointer. */
18 struct thread_t *p_curr_thrd;
19
20 /* Force ZERO in case ZI(bss) clear is missing. */
21 static struct thread_t *p_thrd_head = NULL; /* Point to the first thread. */
22 static struct thread_t *p_rnbl_head = NULL; /* Point to the first runnable. */
23
24 /* Define Macro to fetch global to support future expansion (PERCPU e.g.) */
25 #define LIST_HEAD p_thrd_head
26 #define RNBL_HEAD p_rnbl_head
27
28 /* Callback function pointer for thread to query current state. */
29 static thrd_query_state_t query_state_cb = (thrd_query_state_t)NULL;
30
thrd_set_query_callback(thrd_query_state_t fn)31 void thrd_set_query_callback(thrd_query_state_t fn)
32 {
33 query_state_cb = fn;
34 }
35
thrd_next(void)36 struct thread_t *thrd_next(void)
37 {
38 struct thread_t *p_thrd = RNBL_HEAD;
39 uint32_t retval = 0;
40 struct critical_section_t cs_signal = CRITICAL_SECTION_STATIC_INIT;
41
42 CRITICAL_SECTION_ENTER(cs_signal);
43 /*
44 * First runnable thread has highest priority since threads are
45 * sorted by priority.
46 */
47 while (p_thrd) {
48 /* Change thread state if any signal changed */
49 p_thrd->state = query_state_cb(p_thrd, &retval);
50
51 if (p_thrd->state == THRD_STATE_RET_VAL_AVAIL) {
52 tfm_arch_set_context_ret_code(p_thrd->p_context_ctrl, retval);
53 p_thrd->state = THRD_STATE_RUNNABLE;
54 }
55
56 if (p_thrd->state == THRD_STATE_RUNNABLE) {
57 break;
58 }
59
60 p_thrd = p_thrd->next;
61 }
62 CRITICAL_SECTION_LEAVE(cs_signal);
63
64 return p_thrd;
65 }
66
insert_by_prior(struct thread_t ** head,struct thread_t * node)67 static void insert_by_prior(struct thread_t **head, struct thread_t *node)
68 {
69 if (*head == NULL || (node->priority <= (*head)->priority)) {
70 node->next = *head;
71 *head = node;
72 } else {
73 struct thread_t *iter = *head;
74
75 while (iter->next && (node->priority > iter->next->priority)) {
76 iter = iter->next;
77 }
78
79 node->next = iter->next;
80 iter->next = node;
81 }
82 }
83
thrd_start(struct thread_t * p_thrd,thrd_fn_t fn,thrd_fn_t exit_fn,void * param)84 void thrd_start(struct thread_t *p_thrd, thrd_fn_t fn, thrd_fn_t exit_fn, void *param)
85 {
86 SPM_ASSERT(p_thrd != NULL);
87
88 /* Insert a new thread with priority */
89 insert_by_prior(&LIST_HEAD, p_thrd);
90
91 tfm_arch_init_context(p_thrd->p_context_ctrl, (uintptr_t)fn, param,
92 (uintptr_t)exit_fn);
93
94 /* Mark it as RUNNABLE after insertion */
95 thrd_set_state(p_thrd, THRD_STATE_RUNNABLE);
96 }
97
thrd_set_state(struct thread_t * p_thrd,uint32_t new_state)98 void thrd_set_state(struct thread_t *p_thrd, uint32_t new_state)
99 {
100 SPM_ASSERT(p_thrd != NULL);
101
102 p_thrd->state = new_state;
103
104 /*
105 * Set first runnable thread as head to reduce enumerate
106 * depth while searching for a first runnable thread.
107 */
108 if ((p_thrd->state == THRD_STATE_RUNNABLE) &&
109 ((RNBL_HEAD == NULL) || (p_thrd->priority < RNBL_HEAD->priority))) {
110 RNBL_HEAD = p_thrd;
111 } else {
112 RNBL_HEAD = LIST_HEAD;
113 }
114 }
115
thrd_start_scheduler(struct thread_t ** ppth)116 uint32_t thrd_start_scheduler(struct thread_t **ppth)
117 {
118 struct thread_t *pth = thrd_next();
119
120 arch_attempt_schedule();
121
122 if (ppth) {
123 *ppth = pth;
124 }
125
126 return tfm_arch_refresh_hardware_context(pth->p_context_ctrl);
127 }
128
