diff --git a/subsys/bluetooth/controller/ll_sw/lll_df.h b/subsys/bluetooth/controller/ll_sw/lll_df.h index 5e341b3da1a..4da241dc23e 100644 --- a/subsys/bluetooth/controller/ll_sw/lll_df.h +++ b/subsys/bluetooth/controller/ll_sw/lll_df.h @@ -9,3 +9,12 @@ int lll_df_reset(void); /* Provides number of available antennae for Direction Finding */ uint8_t lll_df_ant_num_get(void); + +/* Confirm if there is `count` of free IQ report node rx available and return + * pointer to first (oldest) one. + */ +void *ull_df_iq_report_alloc_peek(uint8_t count); +/* Dequeue first (oldest) free IQ report node rx. If the node was peeked before + * returned pointer equals to peeked one. + */ +void *ull_df_iq_report_alloc(void); diff --git a/subsys/bluetooth/controller/ll_sw/ull_df.c b/subsys/bluetooth/controller/ll_sw/ull_df.c index 27c45a7471b..b73ba8c4b1e 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_df.c +++ b/subsys/bluetooth/controller/ll_sw/ull_df.c @@ -162,6 +162,10 @@ static int init_reset(void) mem_init(mem_link_iq_report.pool, sizeof(memq_link_t), sizeof(mem_link_iq_report.pool) / sizeof(memq_link_t), &mem_link_iq_report.free); + + /* Allocate free IQ report node rx */ + mem_link_iq_report.quota_pdu = IQ_REPORT_CNT; + ull_df_rx_iq_report_alloc(UINT8_MAX); #endif /* CONFIG_BT_CTLR_DF_SCAN_CTE_RX */ return 0; } @@ -431,6 +435,74 @@ void ll_df_read_ant_inf(uint8_t *switch_sample_rates, *max_cte_len = LLL_DF_MAX_CTE_LEN; } +#if defined(CONFIG_BT_CTLR_DF_SCAN_CTE_RX) +void *ull_df_iq_report_alloc_peek(uint8_t count) +{ + if (count > MFIFO_AVAIL_COUNT_GET(iq_report_free)) { + return NULL; + } + + return MFIFO_DEQUEUE_PEEK(iq_report_free); +} + +void *ull_df_iq_report_alloc_peek_iter(uint8_t *idx) +{ + return *(void **)MFIFO_DEQUEUE_ITER_GET(iq_report_free, idx); +} + +void *ull_df_iq_report_alloc(void) +{ + return MFIFO_DEQUEUE(iq_report_free); +} + +void ull_df_iq_report_link_release(memq_link_t *link) +{ + mem_release(link, &mem_link_iq_report.free); +} + +void ull_df_iq_report_mem_release(struct node_rx_hdr *rx) +{ + mem_release(rx, &mem_iq_report.free); +} + +void ull_iq_report_link_inc_quota(int8_t delta) +{ + LL_ASSERT(delta <= 0 || mem_link_iq_report.quota_pdu < IQ_REPORT_CNT); + mem_link_iq_report.quota_pdu += delta; +} + +void ull_df_rx_iq_report_alloc(uint8_t max) +{ + uint8_t idx; + + if (max > mem_link_iq_report.quota_pdu) { + max = mem_link_iq_report.quota_pdu; + } + + while ((max--) && MFIFO_ENQUEUE_IDX_GET(iq_report_free, &idx)) { + memq_link_t *link; + struct node_rx_hdr *rx; + + link = mem_acquire(&mem_link_iq_report.free); + if (!link) { + return; + } + + rx = mem_acquire(&mem_iq_report.free); + if (!rx) { + mem_release(link, &mem_link_iq_report.free); + return; + } + + rx->link = link; + + MFIFO_BY_IDX_ENQUEUE(iq_report_free, idx, rx); + + ull_iq_report_link_inc_quota(-1); + } +} +#endif /* CONFIG_BT_CTLR_DF_SCAN_CTE_RX */ + #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) /* @brief Function releases unused memory for DF advertising configuration. * diff --git a/subsys/bluetooth/controller/ll_sw/ull_df.h b/subsys/bluetooth/controller/ll_sw/ull_df.h index 45843c8ed15..d2959c874e5 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_df.h +++ b/subsys/bluetooth/controller/ll_sw/ull_df.h @@ -23,3 +23,14 @@ enum df_switch_sample_support { int ull_df_init(void); int ull_df_reset(void); + +/* Release link to node_rx_iq_report memory. */ +void ull_df_iq_report_link_release(memq_link_t *link); +/* Release memory of node_rx_iq_report. */ +void ull_df_iq_report_mem_release(struct node_rx_hdr *rx); +/* Change quota of free node_iq_report links. Delta may be negative, + * then it will decrease number of free link elements. + */ +void ull_iq_report_link_inc_quota(int8_t delta); +/* Allocate node_rx_iq_report in free report PDUs list */ +void ull_df_rx_iq_report_alloc(uint8_t max);