zephyr/subsys/bluetooth/controller/util/mayfly.c

/*
 * Copyright (c) 2016-2017 Nordic Semiconductor ASA
 * Copyright (c) 2016 Vinayak Kariappa Chettimada
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <stddef.h>
#include <zephyr/types.h>
#include <zephyr/sys/printk.h>
#include "memq.h"
#include "mayfly.h"

static struct {
	memq_link_t *head;
	memq_link_t *tail;
	uint8_t        enable_req;
	uint8_t        enable_ack;
	uint8_t        disable_req;
	uint8_t        disable_ack;
} mft[MAYFLY_CALLEE_COUNT][MAYFLY_CALLER_COUNT];

static memq_link_t mfl[MAYFLY_CALLEE_COUNT][MAYFLY_CALLER_COUNT];
static uint8_t mfp[MAYFLY_CALLEE_COUNT];

#if defined(MAYFLY_UT)
static uint8_t _state;
#endif /* MAYFLY_UT */

void mayfly_init(void)
{
	uint8_t callee_id;

	callee_id = MAYFLY_CALLEE_COUNT;
	while (callee_id--) {
		uint8_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(uint8_t caller_id, uint8_t callee_id, uint8_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++;

			/* set mayfly callee pending */
			mfp[callee_id] = 1U;

			/* pend the callee for execution */
			mayfly_pend(caller_id, callee_id);
		}
	}
}

uint32_t mayfly_enqueue(uint8_t caller_id, uint8_t callee_id, uint8_t chain,
			struct mayfly *m)
{
	uint8_t state;
	uint8_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 != 0U) {
		if (chain) {
			if (state != 1U) {
				/* mark as ready in queue */
				m->_req = ack + 1;

				goto mayfly_enqueue_pend;
			}

			/* 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->_link, m, &mft[callee_id][caller_id].tail);

mayfly_enqueue_pend:
	/* set mayfly callee pending */
	mfp[callee_id] = 1U;

	/* pend the callee for execution */
	mayfly_pend(caller_id, callee_id);

	return 0;
}

static void dequeue(uint8_t callee_id, uint8_t caller_id, memq_link_t *link,
		    struct mayfly *m)
{
	uint8_t req;

	req = m->_req;
	if (((req - m->_ack) & 0x03) != 1U) {
		uint8_t ack;

#if defined(MAYFLY_UT)
		uint32_t mayfly_ut_run_test(void);
		void mayfly_ut_mfy(void *param);

		if (_state && m->fp == mayfly_ut_mfy) {
			static uint8_t single;

			if (!single) {
				single = 1U;
				mayfly_ut_run_test();
			}
		}
#endif /* MAYFLY_UT */

		/* dequeue mayfly struct */
		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 */
		ack = m->_ack;
		m->_ack = req;

		/* re-insert, if re-pended by interrupt */
		if (((m->_req - ack) & 0x03) == 1U) {
#if defined(MAYFLY_UT)
			printk("%s: RACE\n", __func__);
#endif /* MAYFLY_UT */

			m->_ack = ack;
			memq_enqueue(link, m, &mft[callee_id][callee_id].tail);
		}
	}
}

void mayfly_run(uint8_t callee_id)
{
	uint8_t disable = 0U;
	uint8_t enable = 0U;
	uint8_t caller_id;

	if (!mfp[callee_id]) {
		return;
	}
	mfp[callee_id] = 0U;

	/* iterate through each caller queue to this callee_id */
	caller_id = MAYFLY_CALLER_COUNT;
	while (caller_id--) {
		memq_link_t *link;
		struct mayfly *m = 0;

		/* fetch mayfly in callee queue, if any */
		link = memq_peek(mft[callee_id][caller_id].head,
				 mft[callee_id][caller_id].tail,
				 (void **)&m);
		while (link) {
			uint8_t state;
#if defined(MAYFLY_UT)
			_state = 0U;
#endif /* MAYFLY_UT */

			/* execute work if ready */
			state = (m->_req - m->_ack) & 0x03;
			if (state == 1U) {
#if defined(MAYFLY_UT)
				_state = 1U;
#endif /* MAYFLY_UT */

				/* mark mayfly as ran */
				m->_ack--;

				/* call the mayfly function */
				m->fp(m->param);
			}

			/* dequeue if not re-pended */
			dequeue(callee_id, caller_id, link, m);

			/* fetch next mayfly in callee queue, if any */
			link = memq_peek(mft[callee_id][caller_id].head,
					 mft[callee_id][caller_id].tail,
					 (void **)&m);

/**
 * When using cooperative thread implementation, an issue has been seen where
 * pended mayflies are never executed in certain scenarios.
 * This happens when mayflies with higher caller_id are constantly pended, in
 * which case lower value caller ids never get to be executed.
 * By allowing complete traversal of mayfly queues for all caller_ids, this
 * does not happen, however this means that more than one mayfly function is
 * potentially executed in a mayfly_run(), with added execution time as
 * consequence.
 */
#if defined(CONFIG_BT_MAYFLY_YIELD_AFTER_CALL)
			/* yield out of mayfly_run if a mayfly function was
			 * called.
			 */
			if (state == 1U) {
				/* pend callee (tailchain) if mayfly queue is
				 * not empty or all caller queues are not
				 * processed.
				 */
				if (caller_id || link) {
					/* set mayfly callee pending */
					mfp[callee_id] = 1U;

					/* pend the callee for execution */
					mayfly_pend(callee_id, callee_id);

					return;
				}
			}
#endif
		}

		if (mft[callee_id][caller_id].disable_req !=
		    mft[callee_id][caller_id].disable_ack) {
			disable = 1U;

			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 = 1U;

			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);
	}
}

#if defined(MAYFLY_UT)
#define MAYFLY_CALL_ID_CALLER MAYFLY_CALL_ID_0
#define MAYFLY_CALL_ID_CALLEE MAYFLY_CALL_ID_2

void mayfly_ut_mfy(void *param)
{
	printk("%s: ran.\n", __func__);

	(*((uint32_t *)param))++;
}

void mayfly_ut_test(void *param)
{
	static uint32_t *count;
	static memq_link_t link;
	static struct mayfly mfy = {0, 0, &link, NULL, mayfly_ut_mfy};
	uint32_t err;

	printk("%s: req= %u, ack= %u\n", __func__, mfy._req, mfy._ack);

	if (param) {
		count = param;
	}

	mfy.param = count;

	err = mayfly_enqueue(MAYFLY_CALL_ID_CALLER, MAYFLY_CALL_ID_CALLEE, 1,
			     &mfy);
	if (err) {
		printk("%s: FAILED (%u).\n", __func__, err);
	} else {
		printk("%s: SUCCESS.\n", __func__);
	}
}

uint32_t mayfly_ut_run_test(void)
{
	static memq_link_t link;
	static struct mayfly mfy = {0, 0, &link, NULL, mayfly_ut_test};
	uint32_t err;

	printk("%s: req= %u, ack= %u\n", __func__, mfy._req, mfy._ack);

	err = mayfly_enqueue(MAYFLY_CALL_ID_CALLEE, MAYFLY_CALL_ID_CALLER, 0,
			     &mfy);

	if (err) {
		printk("%s: FAILED.\n", __func__);
		return err;
	}

	printk("%s: SUCCESS.\n", __func__);

	return 0;
}

uint32_t mayfly_ut(void)
{
	static uint32_t count;
	static memq_link_t link;
	static struct mayfly mfy = {0, 0, &link, &count, mayfly_ut_test};
	uint32_t err;

	printk("%s: req= %u, ack= %u\n", __func__, mfy._req, mfy._ack);

	err = mayfly_enqueue(MAYFLY_CALL_ID_PROGRAM, MAYFLY_CALL_ID_CALLER, 0,
			     &mfy);

	if (err) {
		printk("%s: FAILED.\n", __func__);
		return err;
	}

	printk("%s: count = %u.\n", __func__, count);
	printk("%s: SUCCESS.\n", __func__);
	return 0;
}
#endif /* MAYFLY_UT */