bartoszek/zephyr
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
a86ad87d84
zephyr/subsys/bluetooth/controller/util/mayfly.c
Piotr Pryga e84cabf0d4 Bluetooth: Controller: fix mayfly unwanted re-init every bt_enable call 
Mayfly by design uses a memq for storage of its jobs. The memq
requires head and tail to track the content. It is considered
empty is head equals tail.

When memq instance is initialized then there is a new link
instance stored in head and tail, nevertheless the memq is
still empty.

When new job is enqueued to a memq, the inilial link is used
to store the job. New link, provided by enqueue call, is stored
in the tail for future enqueue.

When enqueued job was served and is dequeued, the link it was
assigned to is returned and stored in the job object.
That link will be used in future for call to enqueue.

Now lets consider a situation when we are just after initalization.
Some default initial link is in empty memq. We enqueue and dequeue
a job. After dequeue, the job object stores the initial link object.
The one that was put into the memq during initialization.
Next Bluetooth stack is disabled and enabled again.

The job is enqueued again, but it still stores the initial link
address. After enqueue the memq head points to initial link object,
that stores new job. Tail points to link deliveded by enqueue call,
that is also the initial link object. The memq is considered to be
empty, nevertheless there was a successful enqueue operation.

The issue is casued by lack of re-initialization of a job object
on init. In most cases these objects are static members of some
functions, hence there is no re-initialization after bt_disable
and bt_enable calls.

The problem is fixed by re-initialization of mayfly only once
on bt_enable() call. Then it doesn't matter what links are stored
in dequeued objects and there is no need to re-initialized mayfly
job objects.

Signed-off-by: Piotr Pryga <piotr.pryga@nordicsemi.no>
2022-07-13 16:13:57 +02:00

353 lines
7.6 KiB
C
Raw Blame History

/*
* 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 */
Reference in New Issue View Git Blame Copy Permalink