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