diff --git a/subsys/bluetooth/controller/ll_sw/lll.h b/subsys/bluetooth/controller/ll_sw/lll.h index f5d33c90d70cd..243617c469548 100644 --- a/subsys/bluetooth/controller/ll_sw/lll.h +++ b/subsys/bluetooth/controller/ll_sw/lll.h @@ -611,7 +611,7 @@ struct lll_event *ull_prepare_enqueue(lll_is_abort_cb_t is_abort_cb, lll_prepare_cb_t prepare_cb, uint8_t is_resume); void *ull_prepare_dequeue_get(void); -void *ull_prepare_dequeue_iter(uint8_t *idx); +void *ull_prepare_dequeue_iter(void **idx); void ull_prepare_dequeue(uint8_t caller_id); void *ull_pdu_rx_alloc_peek(uint8_t count); void *ull_pdu_rx_alloc_peek_iter(uint8_t *idx); diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll.c index b44847c05850a..4c4cf7d1a7e75 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll.c @@ -70,7 +70,7 @@ static int init_reset(void); static inline void done_inc(void); #endif /* CONFIG_BT_CTLR_LOW_LAT_ULL_DONE */ static inline bool is_done_sync(void); -static inline struct lll_event *prepare_dequeue_iter_ready_get(uint8_t *idx); +static inline struct lll_event *prepare_dequeue_iter_ready_get(void **idx); static inline struct lll_event *resume_enqueue(lll_is_abort_cb_t is_abort_cb, lll_abort_cb_t abort_cb, lll_prepare_cb_t resume_cb, void *param); @@ -466,9 +466,9 @@ void lll_disable(void *param) } { struct lll_event *next; - uint8_t idx; + void *idx; - idx = UINT8_MAX; + idx = NULL; next = ull_prepare_dequeue_iter(&idx); while (next) { if (!next->is_aborted && @@ -482,8 +482,10 @@ void lll_disable(void *param) * the prepare pipeline hence re-iterate * through the prepare pipeline. */ - idx = UINT8_MAX; + idx = NULL; #endif /* CONFIG_BT_CTLR_LOW_LAT_ULL_DONE */ + } else if (!idx) { + break; } next = ull_prepare_dequeue_iter(&idx); @@ -835,18 +837,17 @@ int lll_prepare_resolve(lll_is_abort_cb_t is_abort_cb, lll_abort_cb_t abort_cb, struct lll_event *ready_short = NULL; struct lll_event *ready; struct lll_event *next; - uint8_t idx; + void *idx; int err; /* Find the ready prepare in the pipeline */ - idx = UINT8_MAX; + idx = NULL; ready = prepare_dequeue_iter_ready_get(&idx); /* Find any short prepare */ - if (ready) { + if (ready && (&ready->prepare_param != prepare_param)) { uint32_t ticks_at_preempt_min = prepare_param->ticks_at_expire; uint32_t ticks_at_preempt_next; - uint8_t idx_backup = idx; uint32_t diff; ticks_at_preempt_next = ready->prepare_param.ticks_at_expire; @@ -854,34 +855,12 @@ int lll_prepare_resolve(lll_is_abort_cb_t is_abort_cb, lll_abort_cb_t abort_cb, ticks_at_preempt_next); if (is_resume || ((diff & BIT(HAL_TICKER_CNTR_MSBIT)) == 0U)) { ticks_at_preempt_min = ticks_at_preempt_next; - if (&ready->prepare_param != prepare_param) { - ready_short = ready; - } + ready_short = ready; } else { ready = NULL; - idx_backup = UINT8_MAX; } - - do { - struct lll_event *ready_next; - - ready_next = prepare_dequeue_iter_ready_get(&idx); - if (!ready_next) { - break; - } - - ticks_at_preempt_next = ready_next->prepare_param.ticks_at_expire; - diff = ticker_ticks_diff_get(ticks_at_preempt_next, - ticks_at_preempt_min); - if ((diff & BIT(HAL_TICKER_CNTR_MSBIT)) == 0U) { - continue; - } - - ready_short = ready_next; - ticks_at_preempt_min = ticks_at_preempt_next; - } while (true); - - idx = idx_backup; + } else { + ready = NULL; } /* Current event active or another prepare is ready in the pipeline */ @@ -932,6 +911,8 @@ int lll_prepare_resolve(lll_is_abort_cb_t is_abort_cb, lll_abort_cb_t abort_cb, } else { next = ready; } + } else if (!idx) { + break; } ready = ull_prepare_dequeue_iter(&idx); @@ -989,6 +970,7 @@ int lll_prepare_resolve(lll_is_abort_cb_t is_abort_cb, lll_abort_cb_t abort_cb, */ /* Find next prepare needing preempt timeout to be setup */ + idx = NULL; next = prepare_dequeue_iter_ready_get(&idx); if (!next) { return err; @@ -1025,14 +1007,20 @@ static inline bool is_done_sync(void) #endif /* !CONFIG_BT_CTLR_LOW_LAT_ULL_DONE */ } -static inline struct lll_event *prepare_dequeue_iter_ready_get(uint8_t *idx) +static inline struct lll_event *prepare_dequeue_iter_ready_get(void **idx) { struct lll_event *ready; - do { + ready = ull_prepare_dequeue_iter(idx); + while ((ready != NULL) && ((ready->is_aborted != 0U) || (ready->is_resume != 0U) || + (ready->prepare_param.defer != 0U))) { + if (!*idx) { + ready = NULL; + break; + } + ready = ull_prepare_dequeue_iter(idx); - } while ((ready != NULL) && ((ready->is_aborted != 0U) || (ready->is_resume != 0U) || - (ready->prepare_param.defer != 0U))); + } return ready; } @@ -1223,7 +1211,7 @@ static void preempt(void *param) { lll_prepare_cb_t resume_cb; struct lll_event *ready; - uint8_t idx; + void *idx; int err; /* No event to abort */ @@ -1231,9 +1219,8 @@ static void preempt(void *param) return; } -preempt_find_preemptor: /* Find a prepare that is ready and not a resume */ - idx = UINT8_MAX; + idx = NULL; ready = prepare_dequeue_iter_ready_get(&idx); if (!ready) { /* No ready prepare */ @@ -1242,87 +1229,14 @@ static void preempt(void *param) /* Preemptor not in pipeline */ if (ready->prepare_param.param != param) { - uint32_t ticks_at_preempt_min = ready->prepare_param.ticks_at_expire; - struct lll_event *ready_short = NULL; - struct lll_event *ready_next = NULL; - struct lll_event *preemptor; - - /* Find if the short prepare request in the pipeline */ - do { - uint32_t ticks_at_preempt_next; - uint32_t diff; - - preemptor = prepare_dequeue_iter_ready_get(&idx); - if (!preemptor) { - break; - } - - if (!ready_next) { - ready_next = preemptor; - } - - if (preemptor->prepare_param.param == param) { - break; - } - - ticks_at_preempt_next = preemptor->prepare_param.ticks_at_expire; - diff = ticker_ticks_diff_get(ticks_at_preempt_next, - ticks_at_preempt_min); - if ((diff & BIT(HAL_TICKER_CNTR_MSBIT)) == 0U) { - continue; - } - - ready_short = preemptor; - ticks_at_preempt_min = ticks_at_preempt_next; - } while (true); - - /* "The" short prepare we were looking for is not in pipeline */ - if (!preemptor) { - uint32_t ret; - - /* Find any short prepare */ - if (ready_short) { - ready = ready_short; - } - - /* Start the preempt timeout for (short) ready event */ - ret = preempt_ticker_start(ready, NULL, ready); - LL_ASSERT((ret == TICKER_STATUS_SUCCESS) || - (ret == TICKER_STATUS_BUSY)); - - return; - } - - /* FIXME: Prepare pipeline is not a ordered list implementation, - * and for short prepare being enqueued, ideally the - * pipeline has to be implemented as ordered list. - * Until then a workaround to abort a prepare present - * before the short prepare being enqueued is implemented - * below. - * A proper solution will be to re-design the pipeline - * as a ordered list, instead of the current FIFO. - */ - - /* Abort the prepare that is present before the short prepare */ - ready->is_aborted = 1; - ready->abort_cb(&ready->prepare_param, ready->prepare_param.param); - - /* Abort all events in pipeline before the short prepare */ - if (preemptor != ready_next) { - goto preempt_find_preemptor; - } + uint32_t ret; - /* As the prepare queue has been refreshed due to the call of - * abort_cb which invokes the lll_done, find the latest prepare - */ - idx = UINT8_MAX; - ready = prepare_dequeue_iter_ready_get(&idx); - if (!ready) { - /* No ready prepare */ - return; - } + /* Start the preempt timeout for ready event */ + ret = preempt_ticker_start(ready, NULL, ready); + LL_ASSERT((ret == TICKER_STATUS_SUCCESS) || + (ret == TICKER_STATUS_BUSY)); - LL_ASSERT(ready->prepare_param.param == param); + return; } /* Check if current event want to continue */ @@ -1366,8 +1280,8 @@ static void preempt(void *param) lll_abort_cb_t abort_cb; uint8_t is_resume_abort; struct lll_event *iter; - uint8_t iter_idx; void *curr_param; + void *iter_idx; /* Remove parameter assignment from currently active radio event so that done event * is not generated. @@ -1385,8 +1299,8 @@ static void preempt(void *param) is_resume_abort = 0U; preempt_abort_resume: - /* Abort any duplicate non-resume, that they get dequeued */ - iter_idx = UINT8_MAX; + /* Abort any duplicates so that they get dequeued */ + iter_idx = NULL; iter = ull_prepare_dequeue_iter(&iter_idx); while (iter) { if (!iter->is_aborted && @@ -1401,8 +1315,10 @@ static void preempt(void *param) * the prepare pipeline hence re-iterate * through the prepare pipeline. */ - iter_idx = UINT8_MAX; + iter_idx = NULL; #endif /* CONFIG_BT_CTLR_LOW_LAT_ULL_DONE */ + } else if (!iter_idx) { + break; } iter = ull_prepare_dequeue_iter(&iter_idx); diff --git a/subsys/bluetooth/controller/ll_sw/ull.c b/subsys/bluetooth/controller/ll_sw/ull.c index bf15820ecd6db..caefb20490901 100644 --- a/subsys/bluetooth/controller/ll_sw/ull.c +++ b/subsys/bluetooth/controller/ll_sw/ull.c @@ -338,7 +338,16 @@ static struct k_sem *sem_recv; */ #define EVENT_PIPELINE_MAX (7U + (EVENT_DEFER_MAX)) -static MFIFO_DEFINE(prep, sizeof(struct lll_event), EVENT_PIPELINE_MAX); +#define EVENT_PIPELINE_EVENT_SIZE (sizeof(struct lll_event) + sizeof(void *)) + +static struct { + struct { + void *free; + uint8_t pool[EVENT_PIPELINE_MAX * EVENT_PIPELINE_EVENT_SIZE]; + } mem; + void **head; + void **tail; +} pipeline; /* Declare done-event RXFIFO. This is a composite pool-backed MFIFO for rx_nodes. * The declaration constructs the following data structures: @@ -877,9 +886,6 @@ void ll_reset(void) /* Re-initialize ULL internals */ - /* Re-initialize the prep mfifo */ - MFIFO_INIT(prep); - /* Re-initialize the free rx mfifo */ MFIFO_INIT(pdu_rx_free); @@ -2119,13 +2125,16 @@ struct lll_event *ull_prepare_enqueue(lll_is_abort_cb_t is_abort_cb, uint8_t is_resume) { struct lll_event *e; - uint8_t idx; + void **next; - idx = MFIFO_ENQUEUE_GET(prep, (void **)&e); - if (!e) { + /* Allocate lll_event */ + next = mem_acquire(&pipeline.mem.free); + if (!next) { return NULL; } + e = (void *)((uint8_t *)next + sizeof(void *)); + memcpy(&e->prepare_param, prepare_param, sizeof(e->prepare_param)); e->prepare_cb = prepare_cb; e->is_abort_cb = is_abort_cb; @@ -2133,30 +2142,124 @@ struct lll_event *ull_prepare_enqueue(lll_is_abort_cb_t is_abort_cb, e->is_resume = is_resume; e->is_aborted = 0U; - MFIFO_ENQUEUE(prep, idx); + /* Enqueue lll_event */ + *next = NULL; + if (pipeline.tail) { + struct lll_event *e_curr; + uint32_t diff; + + /* Should the prepare be placed as the tail? */ + e_curr = (void *)((uint8_t *)pipeline.tail + sizeof(void *)); + diff = ticker_ticks_diff_get(prepare_param->ticks_at_expire, + e_curr->prepare_param.ticks_at_expire); + if (is_resume || + (!e_curr->is_aborted && !e_curr->is_resume && + ((diff & BIT(HAL_TICKER_CNTR_MSBIT)) == 0U))) { + *pipeline.tail = next; + pipeline.tail = next; + } else { + /* Should the prepare be placed as the head? */ + e_curr = (void *)((uint8_t *)pipeline.head + sizeof(void *)); + diff = ticker_ticks_diff_get(e_curr->prepare_param.ticks_at_expire, + prepare_param->ticks_at_expire); + if (!e_curr->is_aborted && + (e_curr->is_resume || + (diff && ((diff & BIT(HAL_TICKER_CNTR_MSBIT)) == 0U)))) { + *next = pipeline.head; + pipeline.head = next; + } else { + void **prev; + void **curr; + + prev = NULL; + curr = pipeline.head; + e_curr = (void *)((uint8_t *)curr + sizeof(void *)); + do { + if (!e_curr->is_aborted && !e_curr->is_resume) { + prev = curr; + } + + curr = *curr; + if (!curr) { + break; + } + + e_curr = (void *)((uint8_t *)curr + sizeof(void *)); + diff = ticker_ticks_diff_get( + prepare_param->ticks_at_expire, + e_curr->prepare_param.ticks_at_expire); + } while (!e_curr->is_resume && + (e_curr->is_aborted || + ((diff & BIT(HAL_TICKER_CNTR_MSBIT)) == 0U))); + + if (!prev) { + *next = pipeline.head; + pipeline.head = next; + if (!*next) { + pipeline.tail = next; + } + } else { + *next = *prev; + *prev = next; + if (!*next) { + pipeline.tail = next; + } + } + } + } + } else { + pipeline.head = next; + pipeline.tail = next; + } return e; } void *ull_prepare_dequeue_get(void) { - return MFIFO_DEQUEUE_GET(prep); + void *e; + + /* peak lll_event */ + if (pipeline.head) { + e = (uint8_t *)pipeline.head + sizeof(void *); + } else { + e = NULL; + } + + return e; } -void *ull_prepare_dequeue_iter(uint8_t *idx) +void *ull_prepare_dequeue_iter(void **idx) { - return MFIFO_DEQUEUE_ITER_GET(prep, idx); + void *e; + + /* Start at the head */ + if (!*idx) { + *idx = pipeline.head; + } + + /* No more in the list */ + if (!*idx) { + return NULL; + } + + /* Peak lll_event */ + e = (uint8_t *)*idx + sizeof(void *); + + /* Proceed to next */ + *idx = *((void **)*idx); + + return e; } void ull_prepare_dequeue(uint8_t caller_id) { - uint32_t param_normal_head_ticks = 0U; - uint32_t param_normal_next_ticks = 0U; - void *param_normal_head = NULL; - void *param_normal_next = NULL; - void *param_resume_head = NULL; - void *param_resume_next = NULL; - struct lll_event *next; + static memq_link_t link; + static struct mayfly mfy = {0, 0, &link, NULL, lll_resume}; + uint8_t enqueued_loop = 2U; + uint8_t is_enqueued = 0U; + void **e_prev; + void **e_curr; uint8_t loop; /* Development assertion check to ensure the below loop processing @@ -2186,88 +2289,87 @@ void ull_prepare_dequeue(uint8_t caller_id) */ loop = (EVENT_PIPELINE_MAX + 3U); - next = ull_prepare_dequeue_get(); - while (next) { - uint32_t ticks = next->prepare_param.ticks_at_expire; - void *param = next->prepare_param.param; - uint8_t is_aborted = next->is_aborted; - uint8_t is_resume = next->is_resume; + e_prev = NULL; + e_curr = pipeline.head; + while (e_curr) { + struct lll_event *e; + uint8_t is_aborted; /* Assert if we exceed iterations processing the prepare queue */ LL_ASSERT(loop); loop--; - /* Let LLL invoke the `prepare` interface if radio not in active - * use. Otherwise, enqueue at end of the prepare pipeline queue. - */ - if (!is_aborted) { - static memq_link_t link; - static struct mayfly mfy = {0, 0, &link, NULL, - lll_resume}; - uint32_t ret; - - mfy.param = next; - ret = mayfly_enqueue(caller_id, TICKER_USER_ID_LLL, 0, - &mfy); - LL_ASSERT(!ret); - } + e = (void *)((uint8_t *)e_curr + sizeof(void *)); - MFIFO_DEQUEUE(prep); + is_aborted = !!e->is_aborted; - /* Check for anymore more prepare elements in queue */ - next = ull_prepare_dequeue_get(); - if (!next) { - break; - } + if (is_aborted || (!is_enqueued && !e->is_resume)) { + void **head = e_curr; - /* A valid prepare element has its `prepare` invoked or was - * enqueued back into prepare pipeline. - */ - if (!is_aborted) { - /* The prepare element was not a resume event, it would - * use the radio or was enqueued back into prepare - * pipeline with a preempt timeout being set. - * - * Remember the first encountered and the next element - * in the prepare pipeline so that we do not infinitely - * loop through the resume events in prepare pipeline. - */ - if (!is_resume) { - if (!param_normal_head) { - param_normal_head = param; - param_normal_head_ticks = ticks; - } else if (!param_normal_next) { - param_normal_next = param; - param_normal_next_ticks = ticks; + if (!e_prev) { + pipeline.head = *pipeline.head; + if (!pipeline.head) { + pipeline.tail = NULL; } + e_curr = pipeline.head; } else { - if (!param_resume_head) { - param_resume_head = param; - } else if (!param_resume_next) { - param_resume_next = param; + *e_prev = *e_curr; + if (!*e_prev) { + pipeline.tail = e_prev; } + e_curr = *e_prev; } - /* Stop traversing the prepare pipeline when we reach - * back to the first or next event where we - * initially started processing the prepare pipeline. - */ - if (!next->is_aborted && - ((!next->is_resume && - (((next->prepare_param.param == param_normal_head) && - (next->prepare_param.ticks_at_expire == - param_normal_head_ticks)) || - ((next->prepare_param.param == param_normal_next) && - (next->prepare_param.ticks_at_expire == - param_normal_next_ticks)))) || - (next->is_resume && !param_normal_next && - ((next->prepare_param.param == param_resume_head) || - (next->prepare_param.param == param_resume_next))))) { - break; + if (!is_aborted) { + uint32_t ret; + + if (enqueued_loop) { + enqueued_loop--; + } + + if (!enqueued_loop) { + is_enqueued = 1U; + } + + /* Trigger the LLL resume */ + mfy.param = e; + ret = mayfly_enqueue(caller_id, TICKER_USER_ID_LLL, 0, &mfy); + LL_ASSERT(!ret); + + e_prev = NULL; + e_curr = pipeline.head; } + + /* Free event memory */ + mem_release(head, &pipeline.mem.free); + + } else { + e_prev = e_curr; + e_curr = *e_curr; } } + + if (!is_enqueued && pipeline.head) { + void **head = pipeline.head; + struct lll_event *e; + uint32_t ret; + + pipeline.head = *pipeline.head; + if (!pipeline.head) { + pipeline.tail = NULL; + } + + e = (void *)((uint8_t *)head + sizeof(void *)); + LL_ASSERT(!e->is_aborted && e->is_resume); + + /* Trigger the LLL resume */ + mfy.param = e; + ret = mayfly_enqueue(caller_id, TICKER_USER_ID_LLL, 0, &mfy); + LL_ASSERT(!ret); + + mem_release(head, &pipeline.mem.free); + } } struct event_done_extra *ull_event_done_extra_get(void) @@ -2376,6 +2478,11 @@ static inline int init_reset(void) { memq_link_t *link; + /* Initialize prepare pipeline list */ + mem_init(pipeline.mem.pool, EVENT_PIPELINE_EVENT_SIZE, + sizeof(pipeline.mem.pool) / EVENT_PIPELINE_EVENT_SIZE, + &pipeline.mem.free); + /* Initialize and allocate done pool */ RXFIFO_INIT_ALLOC(done);