blob: cd403762a0536922e473435d19238f7262c12066 [file] [log] [blame]
/*
* Copyright (c) 2016 Nordic Semiconductor ASA
* Copyright (c) 2016 Vinayak Kariappa Chettimada
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/types.h>
#include "memq.h"
#include "mayfly.h"
static struct {
void *head;
void *tail;
u8_t enable_req;
u8_t enable_ack;
u8_t disable_req;
u8_t disable_ack;
} mft[MAYFLY_CALLEE_COUNT][MAYFLY_CALLER_COUNT];
static void *mfl[MAYFLY_CALLEE_COUNT][MAYFLY_CALLER_COUNT][2];
void mayfly_init(void)
{
u8_t callee_id;
callee_id = MAYFLY_CALLEE_COUNT;
while (callee_id--) {
u8_t caller_id;
caller_id = MAYFLY_CALLER_COUNT;
while (caller_id--) {
memq_init(mfl[callee_id][caller_id],
&mft[callee_id][caller_id].head,
&mft[callee_id][caller_id].tail);
}
}
}
void mayfly_enable(u8_t caller_id, u8_t callee_id, u8_t enable)
{
if (enable) {
if (mft[callee_id][caller_id].enable_req ==
mft[callee_id][caller_id].enable_ack) {
mft[callee_id][caller_id].enable_req++;
}
mayfly_enable_cb(caller_id, callee_id, enable);
} else {
if (mft[callee_id][caller_id].disable_req ==
mft[callee_id][caller_id].disable_ack) {
mft[callee_id][caller_id].disable_req++;
mayfly_pend(caller_id, callee_id);
}
}
}
u32_t mayfly_enqueue(u8_t caller_id, u8_t callee_id, u8_t chain,
struct mayfly *m)
{
u8_t state;
u8_t ack;
chain = chain || !mayfly_prio_is_equal(caller_id, callee_id) ||
!mayfly_is_enabled(caller_id, callee_id) ||
(mft[callee_id][caller_id].disable_req !=
mft[callee_id][caller_id].disable_ack);
/* shadow the ack */
ack = m->_ack;
/* already in queue */
state = (m->_req - ack) & 0x03;
if (state != 0) {
if (chain) {
if (state != 1) {
/* mark as ready in queue */
m->_req = ack + 1;
/* pend the callee for execution */
mayfly_pend(caller_id, callee_id);
return 0;
}
/* already ready */
return 1;
}
/* mark as done in queue, and fall thru */
m->_req = ack + 2;
}
/* handle mayfly(s) that can be inline */
if (!chain) {
/* call fp */
m->fp(m->param);
return 0;
}
/* new, add as ready in the queue */
m->_req = ack + 1;
memq_enqueue(m, m->_link, &mft[callee_id][caller_id].tail);
/* pend the callee for execution */
mayfly_pend(caller_id, callee_id);
return 0;
}
void mayfly_run(u8_t callee_id)
{
u8_t disable = 0;
u8_t enable = 0;
u8_t caller_id;
/* iterate through each caller queue to this callee_id */
caller_id = MAYFLY_CALLER_COUNT;
while (caller_id--) {
void *link;
struct mayfly *m = 0;
/* fetch mayfly in callee queue, if any */
link = memq_peek(mft[callee_id][caller_id].tail,
mft[callee_id][caller_id].head,
(void **)&m);
while (link) {
u8_t state;
u8_t req;
/* execute work if ready */
req = m->_req;
state = (req - m->_ack) & 0x03;
if (state == 1) {
/* mark mayfly as ran */
m->_ack--;
/* call the mayfly function */
m->fp(m->param);
}
/* dequeue if not re-pended */
req = m->_req;
if (((req - m->_ack) & 0x03) != 1) {
memq_dequeue(mft[callee_id][caller_id].tail,
&mft[callee_id][caller_id].head,
0);
/* release link into dequeued mayfly struct */
m->_link = link;
/* reset mayfly state to idle */
m->_ack = req;
}
/* fetch next mayfly in callee queue, if any */
link = memq_peek(mft[callee_id][caller_id].tail,
mft[callee_id][caller_id].head,
(void **)&m);
/* yield out of mayfly_run if a mayfly function was
* called.
*/
if (state == 1) {
/* pend callee (tailchain) if mayfly queue is
* not empty or all caller queues are not
* processed.
*/
if (caller_id || link) {
mayfly_pend(callee_id, callee_id);
return;
}
}
}
if (mft[callee_id][caller_id].disable_req !=
mft[callee_id][caller_id].disable_ack) {
disable = 1;
mft[callee_id][caller_id].disable_ack =
mft[callee_id][caller_id].disable_req;
}
if (mft[callee_id][caller_id].enable_req !=
mft[callee_id][caller_id].enable_ack) {
enable = 1;
mft[callee_id][caller_id].enable_ack =
mft[callee_id][caller_id].enable_req;
}
}
if (disable && !enable) {
mayfly_enable_cb(callee_id, callee_id, 0);
}
}