1 /*
2 * Copyright (c) 2024, Texas Instruments Incorporated
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8
9 #include <ti/drivers/dpl/HwiP.h>
10 #include <ti/drivers/dpl/SwiP.h>
11
12 #include "QueueP.h"
13
14 /* Lowest priority interrupt for CC23X0 */
15 #define INT_PRI_LEVEL7 0x000000C0
16
17 #define NUMPRI 4
18
19 typedef enum
20 {
21 SwiP_State_Idle = 0,
22 SwiP_State_Posted,
23 SwiP_State_Running,
24 SwiP_State_Interrupted
25 } SwiP_State;
26
27 typedef uint32_t SwiP_LockState;
28
29 enum
30 {
31 SwiP_LockState_Locked,
32 SwiP_LockState_Unlocked
33 };
34
35 typedef struct SwiP_Obj
36 {
37 QueueP_Elem elem;
38 QueueP_Handle readyList;
39 SwiP_Params params;
40 SwiP_Fxn fxn;
41 uint32_t trigger;
42 uint32_t state;
43 } SwiP_Obj;
44
45 extern bool HwiP_inSwi(void);
46
47 static void SwiP_handleHwi();
48
49 static const SwiP_Params SwiP_defaultParams = {
50 .arg0 = (uintptr_t)NULL,
51 .arg1 = (uintptr_t)NULL,
52 .priority = ~0, /* max priority */
53 .trigger = 0,
54 };
55
56 static volatile int SwiP_readyMask;
57 static volatile SwiP_LockState SwiP_lockState;
58 static volatile uint32_t SwiP_currentTrigger;
59 static volatile bool SwiP_schedulerRunning;
60 static volatile bool SwiP_initialized = false;
61 static HwiP_Struct SwiP_hwiStruct;
62 static QueueP_Obj SwiP_readyList[NUMPRI];
63
64 /* don't call with n == 0 */
maxbit(int n)65 static int maxbit(int n)
66 {
67 int mask = 1 << (NUMPRI - 1);
68 int max = NUMPRI - 1;
69
70 while (mask)
71 {
72 if (n & mask)
73 {
74 return max;
75 }
76 max--;
77 mask >>= 1;
78 }
79
80 return 0;
81 }
82
83 /*
84 * ======== SwiP_Params_init ========
85 */
SwiP_Params_init(SwiP_Params * params)86 void SwiP_Params_init(SwiP_Params *params)
87 {
88 /* structure copy */
89 *params = SwiP_defaultParams;
90 }
91
92 /*
93 * ======== SwiP_construct ========
94 */
SwiP_construct(SwiP_Struct * handle,SwiP_Fxn swiFxn,SwiP_Params * params)95 SwiP_Handle SwiP_construct(SwiP_Struct *handle, SwiP_Fxn swiFxn, SwiP_Params *params)
96 {
97 SwiP_Obj *swi = (SwiP_Obj *)handle;
98 HwiP_Params hwiParams;
99 uintptr_t hwiKey;
100 uint32_t priority;
101 int i;
102
103 if (handle != NULL)
104 {
105 hwiKey = HwiP_disable();
106
107 if (SwiP_initialized == false)
108 {
109 for (i = 0; i < NUMPRI; i++)
110 {
111 QueueP_init(&SwiP_readyList[i]);
112 }
113 SwiP_readyMask = 0;
114 SwiP_currentTrigger = 0;
115 SwiP_lockState = SwiP_LockState_Unlocked;
116 SwiP_schedulerRunning = false;
117
118 HwiP_Params_init(&hwiParams);
119 hwiParams.priority = INT_PRI_LEVEL7; // use the lowest priority
120 HwiP_construct(&SwiP_hwiStruct, HwiP_swiPIntNum, SwiP_handleHwi, &hwiParams);
121
122 SwiP_initialized = true;
123 }
124
125 HwiP_restore(hwiKey);
126
127 if (params == NULL)
128 {
129 params = (SwiP_Params *)&SwiP_defaultParams;
130 }
131
132 if (params->priority == (~0))
133 {
134 priority = NUMPRI - 1;
135 }
136 else
137 {
138 priority = params->priority;
139 }
140
141 if (priority >= NUMPRI)
142 {
143 return NULL;
144 }
145 else
146 {
147 QueueP_init((QueueP_Obj *)&swi->elem);
148 swi->params = *params;
149 swi->params.priority = priority;
150 swi->fxn = swiFxn;
151 swi->trigger = swi->params.trigger;
152 swi->state = SwiP_State_Idle;
153 swi->readyList = &SwiP_readyList[priority];
154 }
155 }
156
157 return ((SwiP_Handle)swi);
158 }
159
160 /*
161 * ======== SwiP_destruct ========
162 */
SwiP_destruct(SwiP_Struct * handle)163 void SwiP_destruct(SwiP_Struct *handle)
164 {
165 SwiP_Obj *swi = (SwiP_Obj *)handle;
166 uintptr_t hwiKey = HwiP_disable();
167
168 /* if on SwiP_readyList, remove it */
169 QueueP_remove(&swi->elem);
170 if (QueueP_empty(swi->readyList))
171 {
172 SwiP_readyMask &= ~(1 << swi->params.priority);
173 }
174
175 HwiP_restore(hwiKey);
176 }
177
178 /*
179 * ======== SwiP_disable ========
180 */
SwiP_disable(void)181 uintptr_t SwiP_disable(void)
182 {
183 uintptr_t previousHwiState = HwiP_disable();
184
185 SwiP_LockState previousLockState = SwiP_lockState;
186 SwiP_lockState = SwiP_LockState_Locked;
187
188 HwiP_restore(previousHwiState);
189
190 return previousLockState;
191 }
192
193 /*
194 * This is a non-preemptive fixed-priority scheduler implementation.
195 * It runs with interrupts disabled, but enables them for each swi.
196 */
SwiP_dispatch(uintptr_t hwiKey)197 void SwiP_dispatch(uintptr_t hwiKey)
198 {
199 SwiP_Obj *swi;
200 int maxpri;
201
202 while (SwiP_readyMask && (SwiP_lockState == SwiP_LockState_Unlocked))
203 {
204 maxpri = maxbit(SwiP_readyMask);
205 swi = (SwiP_Obj *)QueueP_get(&SwiP_readyList[maxpri]);
206
207 if (QueueP_empty(&SwiP_readyList[maxpri]))
208 {
209 SwiP_readyMask &= ~(1 << maxpri);
210 }
211
212 /*
213 * Prepare the swi for execution. The trigger has to be saved
214 * because the swi might re-post itself with another trigger value.
215 */
216 swi->state = SwiP_State_Running;
217 SwiP_currentTrigger = swi->trigger;
218 swi->trigger = swi->params.trigger;
219
220 /* run the swi with interrupts enabled */
221 HwiP_restore(hwiKey);
222 swi->fxn(swi->params.arg0, swi->params.arg1);
223 hwiKey = HwiP_disable();
224
225 /* If the swi didn't get re-posted, set it to idle now */
226 if (swi->state == SwiP_State_Running)
227 {
228 swi->state = SwiP_State_Idle;
229 }
230 }
231
232 /* Scheduler was set to running immediately after posting the interrupt */
233 SwiP_schedulerRunning = false;
234 }
235
SwiP_handleHwi()236 static void SwiP_handleHwi()
237 {
238 uintptr_t hwiKey = HwiP_disable();
239
240 SwiP_dispatch(hwiKey);
241
242 HwiP_restore(hwiKey);
243 }
244
245 /*
246 * ======== SwiP_getTrigger ========
247 */
SwiP_getTrigger()248 uint32_t SwiP_getTrigger()
249 {
250 return (SwiP_currentTrigger);
251 }
252
253 /*
254 * ======== SwiP_or ========
255 */
SwiP_or(SwiP_Handle handle,uint32_t mask)256 void SwiP_or(SwiP_Handle handle, uint32_t mask)
257 {
258 SwiP_Obj *swi = (SwiP_Obj *)handle;
259 uintptr_t hwiKey = HwiP_disable();
260
261 swi->trigger |= mask;
262
263 HwiP_restore(hwiKey);
264 SwiP_post(swi);
265 }
266
267 /*
268 * ======== SwiP_post ========
269 */
SwiP_post(SwiP_Handle handle)270 void SwiP_post(SwiP_Handle handle)
271 {
272 SwiP_Obj *swi = (SwiP_Obj *)handle;
273 uintptr_t hwiKey = HwiP_disable();
274
275 /* (Re-)post an swi only once */
276 if (swi->state != SwiP_State_Posted)
277 {
278 swi->state = SwiP_State_Posted;
279
280 QueueP_put(&SwiP_readyList[swi->params.priority], (QueueP_Elem *)&swi->elem);
281 SwiP_readyMask |= 1 << swi->params.priority;
282 }
283
284 /* Activate the scheduler when not already running */
285 if ((SwiP_schedulerRunning == false) && (SwiP_lockState == SwiP_LockState_Unlocked))
286 {
287 HwiP_post(HwiP_swiPIntNum);
288 /* Set the scheduler into running state to avoid double posts */
289 SwiP_schedulerRunning = true;
290 }
291
292 HwiP_restore(hwiKey);
293 }
294
295 /*
296 * ======== SwiP_restore ========
297 */
SwiP_restore(uintptr_t key)298 void SwiP_restore(uintptr_t key)
299 {
300 uintptr_t hwiKey = HwiP_disable();
301
302 SwiP_lockState = key;
303
304 /* Determine whether the scheduler needs to run */
305 if (SwiP_readyMask && (key == SwiP_LockState_Unlocked) && (SwiP_schedulerRunning == false))
306 {
307 HwiP_post(HwiP_swiPIntNum);
308 /* Set the scheduler into running state to avoid double posts */
309 SwiP_schedulerRunning = true;
310 }
311
312 HwiP_restore(hwiKey);
313 }
314
315 /*
316 * ======== SwiP_inISR ========
317 */
SwiP_inISR(void)318 bool SwiP_inISR(void)
319 {
320 return (HwiP_inSwi());
321 }
