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