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