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