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