diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c index 1889ad8f9a6..5d1ebaa41ed 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c @@ -320,6 +320,8 @@ static int isr_rx_pdu(struct lll_scan_aux *lll, uint8_t rssi_ready) ftr->radio_end_us = radio_tmr_end_get() - radio_rx_chain_delay_get(lll->phy, 1); + ftr->rssi = (rssi_ready) ? (radio_rssi_get() & 0x7f) : 0x7f; + ull_rx_put(node_rx->hdr.link, node_rx); ull_rx_sched(); diff --git a/subsys/bluetooth/controller/ll_sw/ull.c b/subsys/bluetooth/controller/ll_sw/ull.c index 9470925f6bc..760e6e4c980 100644 --- a/subsys/bluetooth/controller/ll_sw/ull.c +++ b/subsys/bluetooth/controller/ll_sw/ull.c @@ -557,6 +557,32 @@ void ll_rx_dequeue(void) /* handle object specific clean up */ switch (rx->type) { +#if defined(CONFIG_BT_CTLR_ADV_EXT) + case NODE_RX_TYPE_EXT_1M_REPORT: + case NODE_RX_TYPE_EXT_2M_REPORT: + case NODE_RX_TYPE_EXT_CODED_REPORT: + { + struct node_rx_hdr *rx_curr; + struct pdu_adv *adv; + + adv = (void *)((struct node_rx_pdu *)rx)->pdu; + if (adv->type != PDU_ADV_TYPE_EXT_IND) { + break; + } + + rx_curr = rx->rx_ftr.extra; + while (rx_curr) { + memq_link_t *link_free; + + link_free = rx_curr->link; + rx_curr = rx_curr->rx_ftr.extra; + + mem_release(link_free, &mem_link_rx.free); + } + } + break; +#endif /* CONFIG_BT_CTLR_ADV_EXT */ + #if defined(CONFIG_BT_CONN) case NODE_RX_TYPE_CONNECTION: { @@ -636,13 +662,6 @@ void ll_rx_dequeue(void) case NODE_RX_TYPE_REPORT: #endif /* CONFIG_BT_OBSERVER */ -#if defined(CONFIG_BT_CTLR_ADV_EXT) - /* fallthrough */ - case NODE_RX_TYPE_EXT_1M_REPORT: - case NODE_RX_TYPE_EXT_2M_REPORT: - case NODE_RX_TYPE_EXT_CODED_REPORT: -#endif /* CONFIG_BT_CTLR_ADV_EXT */ - #if defined(CONFIG_BT_CTLR_SCAN_REQ_NOTIFY) case NODE_RX_TYPE_SCAN_REQ: #endif /* CONFIG_BT_CTLR_SCAN_REQ_NOTIFY */ @@ -1575,15 +1594,24 @@ static inline int rx_demux_rx(memq_link_t *link, struct node_rx_hdr *rx) } break; +#if defined(CONFIG_BT_OBSERVER) #if defined(CONFIG_BT_CTLR_ADV_EXT) case NODE_RX_TYPE_EXT_1M_REPORT: case NODE_RX_TYPE_EXT_2M_REPORT: case NODE_RX_TYPE_EXT_CODED_REPORT: { + struct pdu_adv *adv; uint8_t phy = 0U; memq_dequeue(memq_ull_rx.tail, &memq_ull_rx.head, NULL); + adv = (void *)((struct node_rx_pdu *)rx)->pdu; + if (adv->type != PDU_ADV_TYPE_EXT_IND) { + ll_rx_put(link, rx); + ll_rx_sched(); + break; + } + switch (rx->type) { case NODE_RX_TYPE_EXT_1M_REPORT: phy = BIT(0); @@ -1604,6 +1632,7 @@ static inline int rx_demux_rx(memq_link_t *link, struct node_rx_hdr *rx) } break; #endif /* CONFIG_BT_CTLR_ADV_EXT */ +#endif /* CONFIG_BT_OBSERVER */ #if defined(CONFIG_BT_CONN) case NODE_RX_TYPE_CONNECTION: diff --git a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c index 768b6d7d06c..9f89d0ce653 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c +++ b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c @@ -37,7 +37,7 @@ static int init_reset(void); static inline struct ll_scan_aux_set *aux_acquire(void); static inline void aux_release(struct ll_scan_aux_set *aux); static inline uint8_t aux_handle_get(struct ll_scan_aux_set *aux); -static void rx_flush_release(struct ll_scan_aux_set *aux); +static void flush(struct ll_scan_aux_set *aux, struct node_rx_hdr *rx); static void ticker_cb(uint32_t ticks_at_expire, uint32_t remainder, uint16_t lazy, void *param); static void ticker_op_cb(uint32_t status, void *param); @@ -98,12 +98,10 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx, uint8_t phy) aux = NULL; } - pdu = (void *)((struct node_rx_pdu *)rx)->pdu; - if (pdu->type != PDU_ADV_TYPE_EXT_IND) { - aux = NULL; - goto ull_scan_aux_rx_flush; - } + rx->link = link; + ftr->extra = NULL; + pdu = (void *)((struct node_rx_pdu *)rx)->pdu; p = (void *)&pdu->adv_ext_ind; if (!p->ext_hdr_len) { goto ull_scan_aux_rx_flush; @@ -114,21 +112,6 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx, uint8_t phy) goto ull_scan_aux_rx_flush; } - if (!aux) { - aux = aux_acquire(); - if (!aux) { - goto ull_scan_aux_rx_flush; - } - - aux->rx_last = NULL; - lll = &aux->lll; - - ull_hdr_init((void *)lll); - lll_hdr_init(lll, aux); - } else { - LL_ASSERT(0); - } - ptr = (uint8_t *)h + sizeof(*h); if (h->adv_addr) { @@ -151,15 +134,29 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx, uint8_t phy) goto ull_scan_aux_rx_flush; } - /* Enqueue the rx in aux context */ - link->next = NULL; - link->mem = rx; - if (aux->rx_last) { - aux->rx_last->next = link; + if (!aux) { + aux = aux_acquire(); + if (!aux) { + goto ull_scan_aux_rx_flush; + } + + aux->rx_last = NULL; + lll = &aux->lll; + + ull_hdr_init(&aux->ull); + lll_hdr_init(lll, aux); } else { - aux->rx_head = link; + LL_ASSERT(0); } - aux->rx_last = link; + + /* Enqueue the rx in aux context */ + if (aux->rx_last) { + ftr = &aux->rx_last->rx_ftr; + ftr->extra = rx; + } else { + aux->rx_head = rx; + } + aux->rx_last = rx; lll->chan = aux_ptr->chan_idx; lll->phy = BIT(aux_ptr->phy); @@ -233,7 +230,9 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx, uint8_t phy) ull_scan_aux_rx_flush: if (aux) { - rx_flush_release(aux); + flush(aux, rx); + + return; } ll_rx_put(link, rx); @@ -245,7 +244,7 @@ void ull_scan_aux_done(struct node_rx_event_done *done) struct lll_scan_aux *lll = (void *)HDR_ULL2LLL(done->param); struct ll_scan_aux_set *aux = (void *)HDR_LLL2EVT(lll); - rx_flush_release(aux); + flush(aux, NULL); } uint8_t ull_scan_aux_lll_handle_get(struct lll_scan_aux *lll) @@ -279,20 +278,22 @@ static inline uint8_t aux_handle_get(struct ll_scan_aux_set *aux) sizeof(struct ll_scan_aux_set)); } -static void rx_flush_release(struct ll_scan_aux_set *aux) +static void flush(struct ll_scan_aux_set *aux, struct node_rx_hdr *rx) { if (aux->rx_last) { - memq_link_t *head = aux->rx_head; + if (rx) { + struct node_rx_ftr *ftr; - do { - struct node_rx_hdr *rx = head->mem; - memq_link_t *link = head; + ftr = &aux->rx_last->rx_ftr; + ftr->extra = rx; + } - head = link->next; - - ll_rx_put(link, rx); - } while (head); + rx = aux->rx_head; + ll_rx_put(rx->link, rx); + ll_rx_sched(); + } else if (rx) { + ll_rx_put(rx->link, rx); ll_rx_sched(); } @@ -349,5 +350,5 @@ static void ticker_op_cb(uint32_t status, void *param) static void ticker_op_aux_failure(void *param) { - rx_flush_release(param); + flush(param, NULL); } diff --git a/subsys/bluetooth/controller/ll_sw/ull_scan_types.h b/subsys/bluetooth/controller/ll_sw/ull_scan_types.h index 45e410bb4ac..d70f9c256fd 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_scan_types.h +++ b/subsys/bluetooth/controller/ll_sw/ull_scan_types.h @@ -18,6 +18,6 @@ struct ll_scan_aux_set { struct ull_hdr ull; struct lll_scan_aux lll; - memq_link_t *rx_head; - memq_link_t *rx_last; + struct node_rx_hdr *rx_head; + struct node_rx_hdr *rx_last; };