1 /*
2 * Copyright (c) 2016-2017 Nordic Semiconductor ASA
3 * Copyright (c) 2016 Vinayak Kariappa Chettimada
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <stddef.h>
9
10 #include <soc.h>
11 #include <zephyr/types.h>
12 #include <zephyr/sys/printk.h>
13
14 #include "hal/cpu.h"
15
16 #include "memq.h"
17 #include "mayfly.h"
18
19 static struct {
20 memq_link_t *head;
21 memq_link_t *tail;
22 uint8_t enable_req;
23 uint8_t enable_ack;
24 uint8_t disable_req;
25 uint8_t disable_ack;
26 } mft[MAYFLY_CALLEE_COUNT][MAYFLY_CALLER_COUNT];
27
28 static memq_link_t mfl[MAYFLY_CALLEE_COUNT][MAYFLY_CALLER_COUNT];
29 static uint8_t mfp[MAYFLY_CALLEE_COUNT];
30
31 #if defined(MAYFLY_UT)
32 static uint8_t _state;
33 #endif /* MAYFLY_UT */
34
mayfly_init(void)35 void mayfly_init(void)
36 {
37 uint8_t callee_id;
38
39 callee_id = MAYFLY_CALLEE_COUNT;
40 while (callee_id--) {
41 uint8_t caller_id;
42
43 caller_id = MAYFLY_CALLER_COUNT;
44 while (caller_id--) {
45 memq_init(&mfl[callee_id][caller_id],
46 &mft[callee_id][caller_id].head,
47 &mft[callee_id][caller_id].tail);
48 }
49 }
50 }
51
mayfly_enable(uint8_t caller_id,uint8_t callee_id,uint8_t enable)52 void mayfly_enable(uint8_t caller_id, uint8_t callee_id, uint8_t enable)
53 {
54 if (enable) {
55 if (mft[callee_id][caller_id].enable_req ==
56 mft[callee_id][caller_id].enable_ack) {
57 mft[callee_id][caller_id].enable_req++;
58 }
59
60 mayfly_enable_cb(caller_id, callee_id, enable);
61 } else {
62 if (mft[callee_id][caller_id].disable_req ==
63 mft[callee_id][caller_id].disable_ack) {
64 mft[callee_id][caller_id].disable_req++;
65
66 /* set mayfly callee pending */
67 mfp[callee_id] = 1U;
68
69 /* pend the callee for execution */
70 mayfly_pend(caller_id, callee_id);
71 }
72 }
73 }
74
mayfly_enqueue(uint8_t caller_id,uint8_t callee_id,uint8_t chain,struct mayfly * m)75 uint32_t mayfly_enqueue(uint8_t caller_id, uint8_t callee_id, uint8_t chain,
76 struct mayfly *m)
77 {
78 uint8_t state;
79 uint8_t ack;
80
81 chain = chain || !mayfly_prio_is_equal(caller_id, callee_id) ||
82 !mayfly_is_enabled(caller_id, callee_id) ||
83 (mft[callee_id][caller_id].disable_req !=
84 mft[callee_id][caller_id].disable_ack);
85
86 /* shadow the ack */
87 ack = m->_ack;
88
89 /* already in queue */
90 state = (m->_req - ack) & 0x03;
91 if (state != 0U) {
92 if (chain) {
93 if (state != 1U) {
94 /* mark as ready in queue */
95 m->_req = ack + 1;
96
97 goto mayfly_enqueue_pend;
98 }
99
100 /* already ready */
101 return 1;
102 }
103
104 /* mark as done in queue, and fall thru */
105 m->_req = ack + 2;
106 }
107
108 /* handle mayfly(s) that can be inline */
109 if (!chain) {
110 /* call fp */
111 m->fp(m->param);
112
113 return 0;
114 }
115
116 /* new, add as ready in the queue */
117 m->_req = ack + 1;
118 memq_enqueue(m->_link, m, &mft[callee_id][caller_id].tail);
119
120 mayfly_enqueue_pend:
121 /* set mayfly callee pending */
122 mfp[callee_id] = 1U;
123
124 /* pend the callee for execution */
125 mayfly_pend(caller_id, callee_id);
126
127 return 0;
128 }
129
dequeue(uint8_t callee_id,uint8_t caller_id,memq_link_t * link,struct mayfly * m)130 static void dequeue(uint8_t callee_id, uint8_t caller_id, memq_link_t *link,
131 struct mayfly *m)
132 {
133 uint8_t req;
134
135 req = m->_req;
136 if (((req - m->_ack) & 0x03) != 1U) {
137 uint8_t ack;
138
139 #if defined(MAYFLY_UT)
140 uint32_t mayfly_ut_run_test(void);
141 void mayfly_ut_mfy(void *param);
142
143 if (_state && m->fp == mayfly_ut_mfy) {
144 static uint8_t single;
145
146 if (!single) {
147 single = 1U;
148 mayfly_ut_run_test();
149 }
150 }
151 #endif /* MAYFLY_UT */
152
153 /* dequeue mayfly struct */
154 memq_dequeue(mft[callee_id][caller_id].tail,
155 &mft[callee_id][caller_id].head,
156 0);
157
158 /* release link into dequeued mayfly struct */
159 m->_link = link;
160
161 /* reset mayfly state to idle */
162 cpu_dmb();
163 ack = m->_ack;
164 m->_ack = req;
165
166 /* re-insert, if re-pended by interrupt */
167 cpu_dmb();
168 if (((m->_req - ack) & 0x03) == 1U) {
169 #if defined(MAYFLY_UT)
170 printk("%s: RACE\n", __func__);
171 #endif /* MAYFLY_UT */
172
173 m->_ack = ack;
174 memq_enqueue(link, m, &mft[callee_id][callee_id].tail);
175 }
176 }
177 }
178
mayfly_run(uint8_t callee_id)179 void mayfly_run(uint8_t callee_id)
180 {
181 uint8_t disable = 0U;
182 uint8_t enable = 0U;
183 uint8_t caller_id;
184
185 if (!mfp[callee_id]) {
186 return;
187 }
188 mfp[callee_id] = 0U;
189
190 /* iterate through each caller queue to this callee_id */
191 caller_id = MAYFLY_CALLER_COUNT;
192 while (caller_id--) {
193 memq_link_t *link;
194 struct mayfly *m = 0;
195
196 /* fetch mayfly in callee queue, if any */
197 link = memq_peek(mft[callee_id][caller_id].head,
198 mft[callee_id][caller_id].tail,
199 (void **)&m);
200 while (link) {
201 uint8_t state;
202 #if defined(MAYFLY_UT)
203 _state = 0U;
204 #endif /* MAYFLY_UT */
205
206 /* execute work if ready */
207 state = (m->_req - m->_ack) & 0x03;
208 if (state == 1U) {
209 #if defined(MAYFLY_UT)
210 _state = 1U;
211 #endif /* MAYFLY_UT */
212
213 /* mark mayfly as ran */
214 m->_ack--;
215
216 /* call the mayfly function */
217 m->fp(m->param);
218 }
219
220 /* dequeue if not re-pended */
221 dequeue(callee_id, caller_id, link, m);
222
223 /* fetch next mayfly in callee queue, if any */
224 link = memq_peek(mft[callee_id][caller_id].head,
225 mft[callee_id][caller_id].tail,
226 (void **)&m);
227
228 /**
229 * When using cooperative thread implementation, an issue has been seen where
230 * pended mayflies are never executed in certain scenarios.
231 * This happens when mayflies with higher caller_id are constantly pended, in
232 * which case lower value caller ids never get to be executed.
233 * By allowing complete traversal of mayfly queues for all caller_ids, this
234 * does not happen, however this means that more than one mayfly function is
235 * potentially executed in a mayfly_run(), with added execution time as
236 * consequence.
237 */
238 #if defined(CONFIG_BT_MAYFLY_YIELD_AFTER_CALL)
239 /* yield out of mayfly_run if a mayfly function was
240 * called.
241 */
242 if (state == 1U) {
243 /* pend callee (tailchain) if mayfly queue is
244 * not empty or all caller queues are not
245 * processed.
246 */
247 if (caller_id || link) {
248 /* set mayfly callee pending */
249 mfp[callee_id] = 1U;
250
251 /* pend the callee for execution */
252 mayfly_pend(callee_id, callee_id);
253
254 return;
255 }
256 }
257 #endif
258 }
259
260 if (mft[callee_id][caller_id].disable_req !=
261 mft[callee_id][caller_id].disable_ack) {
262 disable = 1U;
263
264 mft[callee_id][caller_id].disable_ack =
265 mft[callee_id][caller_id].disable_req;
266 }
267
268 if (mft[callee_id][caller_id].enable_req !=
269 mft[callee_id][caller_id].enable_ack) {
270 enable = 1U;
271
272 mft[callee_id][caller_id].enable_ack =
273 mft[callee_id][caller_id].enable_req;
274 }
275 }
276
277 if (disable && !enable) {
278 mayfly_enable_cb(callee_id, callee_id, 0);
279 }
280 }
281
282 #if defined(MAYFLY_UT)
283 #define MAYFLY_CALL_ID_CALLER MAYFLY_CALL_ID_0
284 #define MAYFLY_CALL_ID_CALLEE MAYFLY_CALL_ID_2
285
mayfly_ut_mfy(void * param)286 void mayfly_ut_mfy(void *param)
287 {
288 printk("%s: ran.\n", __func__);
289
290 (*((uint32_t *)param))++;
291 }
292
mayfly_ut_test(void * param)293 void mayfly_ut_test(void *param)
294 {
295 static uint32_t *count;
296 static memq_link_t link;
297 static struct mayfly mfy = {0, 0, &link, NULL, mayfly_ut_mfy};
298 uint32_t err;
299
300 printk("%s: req= %u, ack= %u\n", __func__, mfy._req, mfy._ack);
301
302 if (param) {
303 count = param;
304 }
305
306 mfy.param = count;
307
308 err = mayfly_enqueue(MAYFLY_CALL_ID_CALLER, MAYFLY_CALL_ID_CALLEE, 1,
309 &mfy);
310 if (err) {
311 printk("%s: FAILED (%u).\n", __func__, err);
312 } else {
313 printk("%s: SUCCESS.\n", __func__);
314 }
315 }
316
mayfly_ut_run_test(void)317 uint32_t mayfly_ut_run_test(void)
318 {
319 static memq_link_t link;
320 static struct mayfly mfy = {0, 0, &link, NULL, mayfly_ut_test};
321 uint32_t err;
322
323 printk("%s: req= %u, ack= %u\n", __func__, mfy._req, mfy._ack);
324
325 err = mayfly_enqueue(MAYFLY_CALL_ID_CALLEE, MAYFLY_CALL_ID_CALLER, 0,
326 &mfy);
327
328 if (err) {
329 printk("%s: FAILED.\n", __func__);
330 return err;
331 }
332
333 printk("%s: SUCCESS.\n", __func__);
334
335 return 0;
336 }
337
mayfly_ut(void)338 uint32_t mayfly_ut(void)
339 {
340 static uint32_t count;
341 static memq_link_t link;
342 static struct mayfly mfy = {0, 0, &link, &count, mayfly_ut_test};
343 uint32_t err;
344
345 printk("%s: req= %u, ack= %u\n", __func__, mfy._req, mfy._ack);
346
347 err = mayfly_enqueue(MAYFLY_CALL_ID_PROGRAM, MAYFLY_CALL_ID_CALLER, 0,
348 &mfy);
349
350 if (err) {
351 printk("%s: FAILED.\n", __func__);
352 return err;
353 }
354
355 printk("%s: count = %u.\n", __func__, count);
356 printk("%s: SUCCESS.\n", __func__);
357 return 0;
358 }
359 #endif /* MAYFLY_UT */
360
