diff --git a/subsys/bluetooth/controller/Kconfig.ll_sw_split b/subsys/bluetooth/controller/Kconfig.ll_sw_split index e493b1c873e26..a2f53f1a6947b 100644 --- a/subsys/bluetooth/controller/Kconfig.ll_sw_split +++ b/subsys/bluetooth/controller/Kconfig.ll_sw_split @@ -165,6 +165,33 @@ config BT_CTLR_ADV_SYNC_SET help Maximum supported periodic advertising sets. +config BT_CTLR_ADV_PDU_LINK + bool "Enable linking of advertising PDU trains" + help + Enables extra space in each advertising PDU to allow linking PDUs. This + is required to enable advertising data trains (i.e. transmission of + AUX_CHAIN_IND). + +config BT_CTLR_ADV_SYNC_PDU_BACK2BACK + bool "Enable back-to-back transmission of periodic advertising trains" + depends on BT_CTLR_ADV_PERIODIC + select BT_CTLR_ADV_PDU_LINK + help + Enables transmission of AUX_CHAIN_IND in periodic advertising train by + sending each AUX_CHAIN_IND one after another back-to-back. + Note, consecutive AUX_CHAIN_IND packets are not scheduled but sent at + a constant offset on a best effort basis. This means advertising train can + be preempted by other event at any time. + +config BT_CTLR_ADV_SYNC_PDU_BACK2BACK_AFS + int "AUX Frame Space for back-to-back transmission of periodic advertising trains" + depends on BT_CTLR_ADV_SYNC_PDU_BACK2BACK + default 300 + range 300 1000 + help + Specific AUX Frame Space to be used for back-to-back transmission of + periodic advertising trains. Time specified in microseconds. + config BT_CTLR_ADV_DATA_BUF_MAX int "Advertising Data Maximum Buffers" depends on BT_BROADCASTER diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c index b435158214d37..afd882d88f447 100644 --- a/subsys/bluetooth/controller/hci/hci.c +++ b/subsys/bluetooth/controller/hci/hci.c @@ -26,6 +26,7 @@ #include "util/util.h" #include "util/memq.h" +#include "util/mem.h" #include "hal/ecb.h" #include "hal/ccm.h" diff --git a/subsys/bluetooth/controller/ll_sw/ll_addr.c b/subsys/bluetooth/controller/ll_sw/ll_addr.c index b2273e0a48d39..9fc10982f2c8a 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_addr.c +++ b/subsys/bluetooth/controller/ll_sw/ll_addr.c @@ -14,6 +14,7 @@ #include "util/util.h" #include "util/memq.h" +#include "util/mem.h" #include "pdu.h" diff --git a/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c b/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c index 46f22c66222b0..3d764c139ef1f 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c +++ b/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c @@ -16,6 +16,7 @@ #include "util/util.h" #include "util/memq.h" +#include "util/mem.h" #include "pdu.h" diff --git a/subsys/bluetooth/controller/ll_sw/lll_adv.h b/subsys/bluetooth/controller/ll_sw/lll_adv.h index 5baaec685616a..fc01cffaf25af 100644 --- a/subsys/bluetooth/controller/ll_sw/lll_adv.h +++ b/subsys/bluetooth/controller/ll_sw/lll_adv.h @@ -27,6 +27,9 @@ struct lll_adv_sync { uint32_t ticks_offset; struct lll_adv_pdu data; +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) + struct pdu_adv *last_pdu; +#endif /* CONFIG_BT_CTLR_ADV_PDU_LINK */ #if defined(CONFIG_BT_CTLR_ADV_ISO) struct lll_adv_iso *iso; diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c index e76f0cab9fdc8..ea960b5a6c519 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c @@ -51,7 +51,6 @@ static int init_reset(void); -static struct pdu_adv *adv_pdu_allocate(struct lll_adv_pdu *pdu, uint8_t last); #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY) static inline void adv_extra_data_release(struct lll_adv_pdu *pdu, int idx); static void *adv_extra_data_allocate(struct lll_adv_pdu *pdu, uint8_t last); @@ -98,8 +97,7 @@ static inline bool isr_rx_ci_adva_check(uint8_t tx_addr, uint8_t *addr, #define BT_CTLR_ADV_SYNC_SET 0 #endif -#define PDU_MEM_SIZE MROUND(PDU_AC_LL_HEADER_SIZE + \ - PDU_AC_PAYLOAD_SIZE_MAX) +#define PDU_MEM_SIZE PDU_ADV_MEM_SIZE #define PDU_MEM_COUNT_MIN (BT_CTLR_ADV_SET + \ (BT_CTLR_ADV_SET * PAYLOAD_FRAG_COUNT) + \ (BT_CTLR_ADV_AUX_SET * PAYLOAD_FRAG_COUNT) + \ @@ -257,6 +255,7 @@ int lll_adv_data_release(struct lll_adv_pdu *pdu) struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx) { uint8_t first, last; + void *p; first = pdu->first; last = pdu->last; @@ -281,9 +280,77 @@ struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx) *idx = last; - return adv_pdu_allocate(pdu, last); + p = (void *)pdu->pdu[last]; + if (p) { + return p; + } + + p = lll_adv_pdu_alloc_pdu_adv(); + + pdu->pdu[last] = (void *)p; + + return p; +} + +struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void) +{ + struct pdu_adv *p; + int err; + + p = MFIFO_DEQUEUE_PEEK(pdu_free); + if (p) { + err = k_sem_take(&sem_pdu_free, K_NO_WAIT); + LL_ASSERT(!err); + + MFIFO_DEQUEUE(pdu_free); + +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) + PDU_ADV_NEXT_PTR(p) = NULL; +#endif + return p; + } + + p = mem_acquire(&mem_pdu.free); + if (p) { +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) + PDU_ADV_NEXT_PTR(p) = NULL; +#endif + return p; + } + + err = k_sem_take(&sem_pdu_free, K_FOREVER); + LL_ASSERT(!err); + + p = MFIFO_DEQUEUE(pdu_free); + LL_ASSERT(p); + +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) + PDU_ADV_NEXT_PTR(p) = NULL; +#endif + return p; } +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) +void lll_adv_pdu_release(struct pdu_adv *pdu) +{ + mem_release(pdu, &mem_pdu.free); +} + +void lll_adv_pdu_linked_release_all(struct pdu_adv *pdu_first) +{ + struct pdu_adv *pdu = pdu_first; + + while (pdu) { + struct pdu_adv *pdu_next; + + pdu_next = PDU_ADV_NEXT_PTR(pdu); + PDU_ADV_NEXT_PTR(pdu) = NULL; + lll_adv_pdu_release(pdu); + pdu = pdu_next; + } +} +#endif + struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu, uint8_t *is_modified) { @@ -295,11 +362,34 @@ struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu, uint8_t pdu_idx; void *p; - if (!MFIFO_ENQUEUE_IDX_GET(pdu_free, &free_idx)) { - return NULL; - } - pdu_idx = first; + p = pdu->pdu[pdu_idx]; + + do { + void *next; + + /* Store partial list in current data index if there is + * no free slot in mfifo. It can be released on next + * switch attempt (on next event). + */ + if (!MFIFO_ENQUEUE_IDX_GET(pdu_free, &free_idx)) { + pdu->pdu[pdu_idx] = p; + return NULL; + } + +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) + next = lll_adv_pdu_linked_next_get(p); +#else + next = NULL; +#endif + + MFIFO_BY_IDX_ENQUEUE(pdu_free, free_idx, p); + k_sem_give(&sem_pdu_free); + + p = next; + } while (p); + + pdu->pdu[pdu_idx] = NULL; first += 1U; if (first == DOUBLE_BUFFER_SIZE) { @@ -308,11 +398,7 @@ struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu, pdu->first = first; *is_modified = 1U; - p = pdu->pdu[pdu_idx]; pdu->pdu[pdu_idx] = NULL; - - MFIFO_BY_IDX_ENQUEUE(pdu_free, free_idx, p); - k_sem_give(&sem_pdu_free); } return (void *)pdu->pdu[first]; @@ -372,38 +458,13 @@ struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu, void **extra_data, uint8_t *idx) { - uint8_t first, last; struct pdu_adv *p; - - first = pdu->first; - last = pdu->last; - if (first == last) { - last++; - if (last == DOUBLE_BUFFER_SIZE) { - last = 0U; - } - } else { - uint8_t first_latest; - - pdu->last = first; - cpu_dmb(); - first_latest = pdu->first; - if (first_latest != first) { - last++; - if (last == DOUBLE_BUFFER_SIZE) { - last = 0U; - } - } - } - - *idx = last; - - p = adv_pdu_allocate(pdu, last); + p = lll_adv_pdu_alloc(pdu, idx); if (extra_data) { - *extra_data = adv_extra_data_allocate(pdu, last); + *extra_data = adv_extra_data_allocate(pdu, *idx); } else { - if (adv_extra_data_free(pdu, last)) { + if (adv_extra_data_free(pdu, *idx)) { /* There is no release of memory allocated by * adv_pdu_allocate because there is no memory leak. * If caller can recover from this error and subsequent @@ -607,49 +668,6 @@ static int init_reset(void) return 0; } -static struct pdu_adv *adv_pdu_allocate(struct lll_adv_pdu *pdu, uint8_t last) -{ - void *p; - int err; - - p = (void *)pdu->pdu[last]; - if (p) { - return p; - } - - p = MFIFO_DEQUEUE_PEEK(pdu_free); - if (p) { - err = k_sem_take(&sem_pdu_free, K_NO_WAIT); - LL_ASSERT(!err); - - MFIFO_DEQUEUE(pdu_free); - pdu->pdu[last] = (void *)p; - - return p; - } - - p = mem_acquire(&mem_pdu.free); - if (p) { - pdu->pdu[last] = (void *)p; - - return p; - } - - err = k_sem_take(&sem_pdu_free, K_FOREVER); - LL_ASSERT(!err); - - p = MFIFO_DEQUEUE(pdu_free); - LL_ASSERT(p); - /* If !p then check initial value of sem_pdu_free. It must be the same - * as number of elements in pdu_free store. This may not happen in - * runtime. - */ - - pdu->pdu[last] = (void *)p; - - return p; -} - #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY) static void *adv_extra_data_allocate(struct lll_adv_pdu *pdu, uint8_t last) { diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c index 07ad2128d52ac..e298fdb38f80a 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c @@ -17,6 +17,7 @@ #include "hal/ticker.h" #include "util/util.h" +#include "util/mem.h" #include "util/memq.h" #include "pdu.h" diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h index 9926b21b80dfb..3d314a12668b7 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h @@ -4,6 +4,18 @@ * SPDX-License-Identifier: Apache-2.0 */ +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) +#define PDU_ADV_MEM_SIZE MROUND(PDU_AC_LL_HEADER_SIZE + \ + PDU_AC_PAYLOAD_SIZE_MAX + \ + sizeof(uintptr_t)) +#define PDU_ADV_NEXT_PTR(p) *(struct pdu_adv **)((uint8_t *)(p) + \ + PDU_ADV_MEM_SIZE - \ + sizeof(uintptr_t)) +#else +#define PDU_ADV_MEM_SIZE MROUND(PDU_AC_LL_HEADER_SIZE + \ + PDU_AC_PAYLOAD_SIZE_MAX) +#endif + int lll_adv_data_init(struct lll_adv_pdu *pdu); int lll_adv_data_reset(struct lll_adv_pdu *pdu); int lll_adv_data_release(struct lll_adv_pdu *pdu); @@ -14,6 +26,7 @@ static inline void lll_adv_pdu_enqueue(struct lll_adv_pdu *pdu, uint8_t idx) } struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx); +struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void); static inline struct pdu_adv *lll_adv_data_alloc(struct lll_adv *lll, uint8_t *idx) @@ -78,6 +91,10 @@ static inline struct pdu_adv *lll_adv_aux_data_curr_get(struct lll_adv_aux *lll) #if defined(CONFIG_BT_CTLR_ADV_PERIODIC) int lll_adv_and_extra_data_release(struct lll_adv_pdu *pdu); +#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK) +void lll_adv_sync_pdu_b2b_update(struct lll_adv_sync *lll, uint8_t idx); +#endif + struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu, void **extra_data, uint8_t *idx); @@ -123,3 +140,38 @@ static inline struct pdu_adv *lll_adv_sync_data_peek(struct lll_adv_sync *lll, } #endif /* CONFIG_BT_CTLR_ADV_PERIODIC */ #endif /* CONFIG_BT_CTLR_ADV_EXT */ + +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) +/* Release single PDU, shall only be called from ULL */ +void lll_adv_pdu_release(struct pdu_adv *pdu); +/* Release PDU and all linked PDUs, shall only be called from ULL */ +void lll_adv_pdu_linked_release_all(struct pdu_adv *pdu_first); + +static inline struct pdu_adv *lll_adv_pdu_linked_next_get(struct pdu_adv *pdu) +{ + return PDU_ADV_NEXT_PTR(pdu); +} + +static inline struct pdu_adv *lll_adv_pdu_linked_last_get(struct pdu_adv *pdu) +{ + while (PDU_ADV_NEXT_PTR(pdu)) { + pdu = PDU_ADV_NEXT_PTR(pdu); + } + return pdu; +} + +static inline void lll_adv_pdu_linked_append(struct pdu_adv *pdu, + struct pdu_adv *prev) +{ + PDU_ADV_NEXT_PTR(prev) = pdu; +} + +static inline void lll_adv_pdu_linked_append_end(struct pdu_adv *pdu, + struct pdu_adv *first) +{ + struct pdu_adv *last; + + last = lll_adv_pdu_linked_last_get(first); + lll_adv_pdu_linked_append(pdu, last); +} +#endif diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c index 0813dd7043ca5..59f85e389aef2 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c @@ -15,6 +15,7 @@ #include "hal/radio_df.h" #include "util/util.h" +#include "util/mem.h" #include "util/memq.h" #include "pdu.h" @@ -32,6 +33,7 @@ #include "lll_internal.h" #include "lll_adv_internal.h" #include "lll_tim_internal.h" +#include "lll_prof_internal.h" #include "lll_df_internal.h" #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER) @@ -39,10 +41,21 @@ #include "common/log.h" #include "hal/debug.h" +#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK) +#define ADV_SYNC_PDU_B2B_AFS (CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK_AFS) +#endif + static int init_reset(void); static int prepare_cb(struct lll_prepare_param *p); static void abort_cb(struct lll_prepare_param *prepare_param, void *param); static void isr_done(void *param); +#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK) +static void isr_tx(void *param); +static void pdu_b2b_update(struct lll_adv_sync *lll, struct pdu_adv *pdu); +static void pdu_b2b_aux_ptr_update(struct pdu_adv *pdu, uint8_t phy, + uint8_t flags, uint8_t chan_idx, + uint32_t tifs); +#endif /* CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK */ int lll_adv_sync_init(void) { @@ -147,6 +160,15 @@ static int prepare_cb(struct lll_prepare_param *p) pdu = lll_adv_sync_data_latest_get(lll, &extra_data, &upd); LL_ASSERT(pdu); +#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK) + if (upd) { + /* AuxPtr offsets for b2b TX are fixed for given chain so we can + * calculate them here in advance. + */ + pdu_b2b_update(lll, pdu); + } +#endif + #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) if (extra_data) { df_cfg = (struct lll_df_adv_cfg *)extra_data; @@ -161,16 +183,26 @@ static int prepare_cb(struct lll_prepare_param *p) radio_pkt_tx_set(pdu); - /* TODO: chaining */ - radio_isr_set(isr_done, lll); +#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK) + if (pdu->adv_ext_ind.ext_hdr_len && pdu->adv_ext_ind.ext_hdr.aux_ptr) { + lll->last_pdu = pdu; + radio_isr_set(isr_tx, lll); + radio_tmr_tifs_set(ADV_SYNC_PDU_B2B_AFS); + radio_switch_complete_and_b2b_tx(phy_s, 0, phy_s, 0); +#else + if (0) { +#endif /* CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK */ + } else { + radio_isr_set(isr_done, lll); #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) - if (df_cfg) { - radio_switch_complete_and_phy_end_disable(); - } else + if (df_cfg) { + radio_switch_complete_and_phy_end_disable(); + } else #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ - { - radio_switch_complete_and_disable(); + { + radio_switch_complete_and_disable(); + } } ticks_at_event = p->ticks_at_expire; @@ -255,3 +287,132 @@ static void isr_done(void *param) #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ lll_isr_done(lll); } + +#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK) +static void isr_tx(void *param) +{ + struct lll_adv_sync *lll_sync; + struct pdu_adv *pdu; + struct lll_adv *lll; + + if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) { + lll_prof_latency_capture(); + } + + /* Clear radio tx status and events */ + lll_isr_tx_status_reset(); + + lll_sync = param; + lll = lll_sync->adv; + + /* TODO: do not hardcode to single value */ + lll_chan_set(0); + + pdu = lll_adv_pdu_linked_next_get(lll_sync->last_pdu); + LL_ASSERT(pdu); + lll_sync->last_pdu = pdu; + + /* setup tIFS switching */ + if (pdu->adv_ext_ind.ext_hdr_len && pdu->adv_ext_ind.ext_hdr.aux_ptr) { + radio_tmr_tifs_set(ADV_SYNC_PDU_B2B_AFS); + radio_isr_set(isr_tx, lll_sync); + radio_switch_complete_and_b2b_tx(lll->phy_s, 0, lll->phy_s, 0); + } else { + radio_isr_set(lll_isr_done, lll); +#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + if (lll_sync->cte_started) { + radio_switch_complete_and_phy_end_disable(); + } else +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + { + radio_switch_complete_and_disable(); + } + } + + radio_pkt_tx_set(pdu); + + /* assert if radio packet ptr is not set and radio started rx */ + LL_ASSERT(!radio_is_ready()); + + if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) { + lll_prof_cputime_capture(); + } + + /* capture end of AUX_SYNC_IND/AUX_CHAIN_IND PDU, used for calculating + * next PDU timestamp. + */ + radio_tmr_end_capture(); + +#if defined(CONFIG_BT_CTLR_GPIO_LNA_PIN) + if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) { + /* PA/LNA enable is overwriting packet end used in ISR + * profiling, hence back it up for later use. + */ + lll_prof_radio_end_backup(); + } + + radio_gpio_lna_setup(); + radio_gpio_pa_lna_enable(radio_tmr_tifs_base_get() + + ADV_SYNC_PDU_B2B_AFS - 4 - + radio_tx_chain_delay_get(lll->phy_s, 0) - + CONFIG_BT_CTLR_GPIO_LNA_OFFSET); +#endif /* CONFIG_BT_CTLR_GPIO_LNA_PIN */ + + if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) { + lll_prof_send(); + } +} + +static void pdu_b2b_update(struct lll_adv_sync *lll, struct pdu_adv *pdu) +{ + while (pdu) { + pdu_b2b_aux_ptr_update(pdu, lll->adv->phy_s, 0, 0, + ADV_SYNC_PDU_B2B_AFS); + pdu = lll_adv_pdu_linked_next_get(pdu); + } +} + +static void pdu_b2b_aux_ptr_update(struct pdu_adv *pdu, uint8_t phy, + uint8_t flags, uint8_t chan_idx, + uint32_t tifs) +{ + struct pdu_adv_com_ext_adv *com_hdr; + struct pdu_adv_ext_hdr *hdr; + struct pdu_adv_aux_ptr *aux; + uint32_t offs; + uint8_t *dptr; + + com_hdr = &pdu->adv_ext_ind; + hdr = &com_hdr->ext_hdr; + /* Skip flags */ + dptr = hdr->data; + + if (!com_hdr->ext_hdr_len || !hdr->aux_ptr) { + return; + } + + LL_ASSERT(!hdr->adv_addr); + LL_ASSERT(!hdr->tgt_addr); + + if (hdr->cte_info) { + dptr++; + } + + LL_ASSERT(!hdr->adi); + + /* Update AuxPtr */ + aux = (void *)dptr; + offs = PKT_AC_US(pdu->len, phy) + tifs; + offs = offs / OFFS_UNIT_30_US; + if ((offs >> 13) != 0) { + aux->offs = offs / (OFFS_UNIT_300_US / OFFS_UNIT_30_US); + aux->offs_units = 1U; + } else { + aux->offs = offs; + aux->offs_units = 0U; + } + aux->chan_idx = chan_idx; + aux->ca = 0; + aux->phy = find_lsb_set(phy) - 1; +} +#endif /* CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK */ diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c index 0747008022e4b..0a94f09aa79f4 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c @@ -146,7 +146,7 @@ void lll_scan_prepare_connect_req(struct lll_scan *lll, struct pdu_adv *pdu_tx, conn_interval_us = (uint32_t)lll_conn->interval * CONN_INT_UNIT_US; conn_offset_us = radio_tmr_end_get() + EVENT_IFS_US + - PKT_AC_US(sizeof(struct pdu_adv_connect_ind), 0, + PKT_AC_US(sizeof(struct pdu_adv_connect_ind), phy == PHY_LEGACY ? PHY_1M : phy); /* Add transmitWindowDelay to default calculated connection offset: 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 fa843c3fbde05..f55eed1cc9b67 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 @@ -434,11 +434,9 @@ static int isr_rx_pdu(struct lll_scan_aux *lll, uint8_t devmatch_ok, ull = HDR_LLL2ULL(lll_scan); if (pdu_end_us > (HAL_TICKER_TICKS_TO_US(ull->ticks_slot) - EVENT_IFS_US - - PKT_AC_US(aux_connect_req_len, 0, - lll->phy) - + PKT_AC_US(aux_connect_req_len, lll->phy) - EVENT_IFS_US - - PKT_AC_US(aux_connect_rsp_len, 0, - lll->phy) - + PKT_AC_US(aux_connect_rsp_len, lll->phy) - EVENT_OVERHEAD_START_US - EVENT_TICKER_RES_MARGIN_US)) { return -ETIME; diff --git a/subsys/bluetooth/controller/ll_sw/pdu.h b/subsys/bluetooth/controller/ll_sw/pdu.h index 21359d5939a3d..35b013b751b31 100644 --- a/subsys/bluetooth/controller/ll_sw/pdu.h +++ b/subsys/bluetooth/controller/ll_sw/pdu.h @@ -160,7 +160,7 @@ #define PKT_US(octets, phy) PKT_DC_US((octets), (PDU_MIC_SIZE), (phy)) -#define PKT_AC_US(octets, mic, phy) PKT_DC_US((octets), (mic), (phy)) +#define PKT_AC_US(octets, phy) PKT_DC_US((octets), 0, (phy)) #define PKT_BIS_US(octets, mic, phy) PKT_DC_US((octets), (mic), (phy)) diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h index bcefaa5c357c5..8b589a6dcd700 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h @@ -47,9 +47,12 @@ const uint8_t *ull_adv_pdu_update_addrs(struct ll_adv_set *adv, #if defined(CONFIG_BT_CTLR_ADV_EXT) #define ULL_ADV_PDU_HDR_FIELD_ADVA BIT(0) +#define ULL_ADV_PDU_HDR_FIELD_TARGETA BIT(1) #define ULL_ADV_PDU_HDR_FIELD_CTE_INFO BIT(2) +#define ULL_ADV_PDU_HDR_FIELD_ADI BIT(3) #define ULL_ADV_PDU_HDR_FIELD_AUX_PTR BIT(4) #define ULL_ADV_PDU_HDR_FIELD_SYNC_INFO BIT(5) +#define ULL_ADV_PDU_HDR_FIELD_TX_POWER BIT(7) #define ULL_ADV_PDU_HDR_FIELD_AD_DATA BIT(8) /* Helper type to store data for extended advertising @@ -107,14 +110,35 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv, /* helper function to release periodic advertising instance */ void ull_adv_sync_release(struct ll_adv_sync_set *sync); +/* helper function to allocate new PDU data for AUX_SYNC_IND and return + * previous and new PDU for further processing. + */ +uint8_t ull_adv_sync_pdu_alloc(struct ll_adv_set *adv, + uint16_t hdr_add_fields, + uint16_t hdr_rem_fields, + struct adv_pdu_field_data *data, + struct pdu_adv **ter_pdu_prev, + struct pdu_adv **ter_pdu_new, + void **extra_data_prev, + void **extra_data_new, + uint8_t *ter_idx); + /* helper function to set/clear common extended header format fields * for AUX_SYNC_IND PDU. */ -uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv, +uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync, + struct pdu_adv *ter_pdu_prev, + struct pdu_adv *ter_pdu, uint16_t hdr_add_fields, uint16_t hdr_rem_fields, - struct adv_pdu_field_data *data, - uint8_t *ter_idx); + struct adv_pdu_field_data *data); + +/* helper function to update extra_data field */ +void ull_adv_sync_extra_data_set_clear(void *extra_data_prev, + void *extra_data_new, + uint16_t hdr_add_fields, + uint16_t hdr_rem_fields, + void *data); /* helper function to calculate common ext adv payload header length and * adjust the data pointer. diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c index 8f3acd51a42b3..6b3fbd4411264 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c @@ -50,21 +50,6 @@ static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync); static inline uint8_t sync_remove(struct ll_adv_sync_set *sync, struct ll_adv_set *adv, uint8_t enable); -#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) -static inline void adv_sync_extra_data_set_clear(void *extra_data_prev, - void *extra_data_new, - uint16_t hdr_add_fields, - uint16_t hdr_rem_fields, - void *data); -#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ - -static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync, - struct pdu_adv *ter_pdu_prev, - struct pdu_adv *ter_pdu, - uint16_t hdr_add_fields, - uint16_t hdr_rem_fields, - void *data); - static void mfy_sync_offset_get(void *param); static inline struct pdu_adv_sync_info *sync_info_get(struct pdu_adv *pdu); static inline void sync_info_offset_fill(struct pdu_adv_sync_info *si, @@ -77,8 +62,259 @@ static void ticker_op_cb(uint32_t status, void *param); static struct ll_adv_sync_set ll_adv_sync_pool[CONFIG_BT_CTLR_ADV_SYNC_SET]; static void *adv_sync_free; +static void adv_sync_pdu_init(struct pdu_adv *pdu, uint8_t ext_hdr_flags) +{ + struct pdu_adv_com_ext_adv *com_hdr; + struct pdu_adv_ext_hdr *ext_hdr; + uint8_t *dptr; + uint8_t len; + + pdu->type = PDU_ADV_TYPE_AUX_SYNC_IND; + pdu->rfu = 0U; + pdu->chan_sel = 0U; + + pdu->tx_addr = 0U; + pdu->rx_addr = 0U; + + com_hdr = &pdu->adv_ext_ind; + /* Non-connectable and Non-scannable adv mode */ + com_hdr->adv_mode = 0U; + + ext_hdr = &com_hdr->ext_hdr; + *(uint8_t *)ext_hdr = ext_hdr_flags; + dptr = ext_hdr->data; + + LL_ASSERT(!(ext_hdr_flags & (ULL_ADV_PDU_HDR_FIELD_ADVA | + ULL_ADV_PDU_HDR_FIELD_TARGETA | + ULL_ADV_PDU_HDR_FIELD_ADI | + ULL_ADV_PDU_HDR_FIELD_SYNC_INFO))); + + if (ext_hdr_flags & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) { + dptr += sizeof(struct pdu_cte_info); + } + if (ext_hdr_flags & ULL_ADV_PDU_HDR_FIELD_AUX_PTR) { + dptr += sizeof(struct pdu_adv_aux_ptr); + } + if (ext_hdr_flags & ULL_ADV_PDU_HDR_FIELD_TX_POWER) { + dptr += sizeof(uint8_t); + } + + /* Calc tertiary PDU len */ + len = ull_adv_aux_hdr_len_calc(com_hdr, &dptr); + ull_adv_aux_hdr_len_fill(com_hdr, len); + + pdu->len = len; +} + +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) +static uint8_t adv_sync_pdu_init_from_prev_pdu(struct pdu_adv *pdu, + struct pdu_adv *pdu_prev, + uint16_t ext_hdr_flags_add, + uint16_t ext_hdr_flags_rem) +{ + struct pdu_adv_com_ext_adv *com_hdr_prev; + struct pdu_adv_ext_hdr *ext_hdr_prev; + struct pdu_adv_com_ext_adv *com_hdr; + struct pdu_adv_ext_hdr *ext_hdr; + uint8_t ext_hdr_flags_prev; + uint8_t ext_hdr_flags; + uint8_t *dptr_prev; + uint8_t len_prev; + uint8_t *dptr; + uint8_t len; + + /* Copy complete header, assume it was set properly in old PDU */ + *(uint8_t *)pdu = *(uint8_t *)pdu_prev; + + com_hdr_prev = &pdu_prev->adv_ext_ind; + com_hdr = &pdu->adv_ext_ind; + + com_hdr->adv_mode = 0U; + + ext_hdr_prev = &com_hdr_prev->ext_hdr; + ext_hdr = &com_hdr->ext_hdr; + + if (com_hdr_prev->ext_hdr_len) { + ext_hdr_flags_prev = *(uint8_t *) ext_hdr_prev; + } else { + ext_hdr_flags_prev = 0; + } + ext_hdr_flags = ext_hdr_flags_prev | + (ext_hdr_flags_add & (~ext_hdr_flags_rem)); + + *(uint8_t *)ext_hdr = ext_hdr_flags; + + LL_ASSERT(!ext_hdr->adv_addr); + LL_ASSERT(!ext_hdr->tgt_addr); + LL_ASSERT(!ext_hdr->adi); + LL_ASSERT(!ext_hdr->sync_info); + + dptr = ext_hdr->data; + dptr_prev = ext_hdr_prev->data; + + /* Note: skip length verification of ext header writes as we assume that + * all PDUs are large enough to store at least complete ext header. + */ + + /* Copy CTEInfo, if applicable */ + if (ext_hdr->cte_info) { + if (ext_hdr_prev->cte_info) { + memcpy(dptr, dptr_prev, sizeof(struct pdu_cte_info)); + } + dptr += sizeof(struct pdu_cte_info); + } + if (ext_hdr_prev->cte_info) { + dptr_prev += sizeof(struct pdu_cte_info); + } + + /* Add AuxPtr, if applicable. Do not copy since it will be updated later + * anyway. + */ + if (ext_hdr->aux_ptr) { + dptr += sizeof(struct pdu_adv_aux_ptr); + } + if (ext_hdr_prev->aux_ptr) { + dptr_prev += sizeof(struct pdu_adv_aux_ptr); + } + + /* Copy TxPower, if applicable */ + if (ext_hdr->tx_pwr) { + if (ext_hdr_prev->tx_pwr) { + memcpy(dptr, dptr_prev, sizeof(uint8_t)); + } + dptr += sizeof(uint8_t); + } + if (ext_hdr_prev->tx_pwr) { + dptr_prev += sizeof(uint8_t); + } + + LL_ASSERT(ext_hdr_prev >= 0); + + /* Copy ACAD */ + len = com_hdr_prev->ext_hdr_len - (dptr_prev - (uint8_t *)ext_hdr_prev); + memcpy(dptr, dptr_prev, len); + dptr += len; + + /* Check populated ext header length excluding length itself. If 0, then + * there was neither field nor ACAD populated and we skip ext header + * entirely. + */ + len = dptr - ext_hdr->data; + if (len == 0) { + com_hdr->ext_hdr_len = 0; + } else { + com_hdr->ext_hdr_len = len + + offsetof(struct pdu_adv_ext_hdr, data); + } + + /* Both PDUs have now ext header length calculated properly, reset + * pointers to start of AD. + */ + dptr = &com_hdr->ext_hdr_adv_data[com_hdr->ext_hdr_len]; + dptr_prev = &com_hdr_prev->ext_hdr_adv_data[com_hdr_prev->ext_hdr_len]; + + /* Calculate length of AD to copy and AD length available in new PDU */ + len_prev = pdu_prev->len - (dptr_prev - pdu_prev->payload); + len = PDU_AC_PAYLOAD_SIZE_MAX - (dptr - pdu->payload); + + /* TODO: we should allow partial copy and let caller refragment data */ + if (len < len_prev) { + return BT_HCI_ERR_PACKET_TOO_LONG; + } + + /* Copy AD */ + if (!(ext_hdr_flags_rem & ULL_ADV_PDU_HDR_FIELD_AD_DATA)) { + len = MIN(len, len_prev); + memcpy(dptr, dptr_prev, len); + dptr += len; + } + + /* Finalize PDU */ + pdu->len = dptr - pdu->payload; + + return 0; +} + +static uint8_t adv_sync_pdu_ad_data_set(struct pdu_adv *pdu, + const uint8_t *data, uint8_t len) +{ + struct pdu_adv_com_ext_adv *com_hdr; + uint8_t len_max; + uint8_t *dptr; + + com_hdr = &pdu->adv_ext_ind; + + dptr = &com_hdr->ext_hdr_adv_data[com_hdr->ext_hdr_len]; + + len_max = PDU_AC_PAYLOAD_SIZE_MAX - (dptr - pdu->payload); + /* TODO: we should allow partial copy and let caller refragment data */ + if (len > len_max) { + return BT_HCI_ERR_PACKET_TOO_LONG; + } + + memcpy(dptr, data, len); + dptr += len; + + pdu->len = dptr - pdu->payload; + + return 0; +} + +static uint8_t adv_sync_pdu_cte_info_set(struct pdu_adv *pdu, + const struct pdu_cte_info *cte_info) +{ + struct pdu_adv_com_ext_adv *com_hdr; + struct pdu_adv_ext_hdr *ext_hdr; + uint8_t *dptr; + + com_hdr = &pdu->adv_ext_ind; + ext_hdr = &com_hdr->ext_hdr; + dptr = ext_hdr->data; + + /* Periodic adv PDUs do not have AdvA/TargetA */ + LL_ASSERT(!ext_hdr->adv_addr); + LL_ASSERT(!ext_hdr->tgt_addr); + + if (ext_hdr->cte_info) { + memcpy(dptr, cte_info, sizeof(*cte_info)); + } + + return 0; +} + +static struct pdu_adv *adv_sync_pdu_duplicate_chain(struct pdu_adv *pdu) +{ + struct pdu_adv *pdu_dup = NULL; + uint8_t err; + + while (pdu) { + struct pdu_adv *pdu_new; + + pdu_new = lll_adv_pdu_alloc_pdu_adv(); + + /* We make exact copy of old PDU, there's really nothing that + * can go wrong there assuming original PDU was created properly + */ + err = adv_sync_pdu_init_from_prev_pdu(pdu_new, pdu, 0, 0); + LL_ASSERT(err == 0); + + if (pdu_dup) { + lll_adv_pdu_linked_append_end(pdu_new, pdu_dup); + } else { + pdu_dup = pdu_new; + } + + pdu = lll_adv_pdu_linked_next_get(pdu); + } + + return pdu_dup; +} +#endif /* CONFIG_BT_CTLR_ADV_PDU_LINK */ + uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) { + void *extra_data_prev, *extra_data; + struct pdu_adv *pdu_prev, *pdu; struct lll_adv_sync *lll_sync; struct ll_adv_sync_set *sync; struct ll_adv_set *adv; @@ -91,12 +327,8 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) lll_sync = adv->lll.sync; if (!lll_sync) { - struct pdu_adv_com_ext_adv *ter_com_hdr; - struct pdu_adv_ext_hdr *ter_hdr; struct pdu_adv *ter_pdu; struct lll_adv *lll; - uint8_t *ter_dptr; - uint8_t ter_len; int err; sync = sync_acquire(); @@ -135,33 +367,27 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) sync->is_started = 0U; ter_pdu = lll_adv_sync_data_peek(lll_sync, NULL); - ter_pdu->type = PDU_ADV_TYPE_AUX_SYNC_IND; - ter_pdu->rfu = 0U; - ter_pdu->chan_sel = 0U; - - ter_pdu->tx_addr = 0U; - ter_pdu->rx_addr = 0U; - - ter_com_hdr = (void *)&ter_pdu->adv_ext_ind; - ter_hdr = (void *)ter_com_hdr->ext_hdr_adv_data; - ter_dptr = ter_hdr->data; - *(uint8_t *)ter_hdr = 0U; - - /* Non-connectable and Non-scannable adv mode */ - ter_com_hdr->adv_mode = 0U; - - /* Calc tertiary PDU len */ - ter_len = ull_adv_aux_hdr_len_calc(ter_com_hdr, &ter_dptr); - ull_adv_aux_hdr_len_fill(ter_com_hdr, ter_len); - - ter_pdu->len = ter_len; + adv_sync_pdu_init(ter_pdu, 0); } else { sync = HDR_LLL2ULL(lll_sync); } sync->interval = interval; - err = ull_adv_sync_pdu_set_clear(adv, 0, 0, NULL, &ter_idx); + err = ull_adv_sync_pdu_alloc(adv, 0, 0, NULL, &pdu_prev, &pdu, + &extra_data_prev, &extra_data, &ter_idx); + if (err) { + return err; + } + +#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + if (extra_data) { + ull_adv_sync_extra_data_set_clear(extra_data_prev, extra_data, + 0, 0, NULL); + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + + err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, 0, 0, NULL); if (err) { return err; } @@ -174,7 +400,9 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) uint8_t ll_adv_sync_ad_data_set(uint8_t handle, uint8_t op, uint8_t len, uint8_t const *const data) { + void *extra_data_prev, *extra_data; struct adv_pdu_field_data pdu_data; + struct pdu_adv *pdu_prev, *pdu; struct lll_adv_sync *lll_sync; struct ll_adv_set *adv; uint8_t value[5]; @@ -202,15 +430,49 @@ uint8_t ll_adv_sync_ad_data_set(uint8_t handle, uint8_t op, uint8_t len, *pdu_data.field_data = len; sys_put_le32((uint32_t)data, pdu_data.field_data + 1); - err = ull_adv_sync_pdu_set_clear(adv, ULL_ADV_PDU_HDR_FIELD_AD_DATA, - 0, &pdu_data, &ter_idx); + err = ull_adv_sync_pdu_alloc(adv, ULL_ADV_PDU_HDR_FIELD_AD_DATA, 0, + &pdu_data, &pdu_prev, &pdu, + &extra_data_prev, &extra_data, &ter_idx); if (err) { return err; } - lll_adv_sync_data_enqueue(lll_sync, ter_idx); +#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + if (extra_data) { + ull_adv_sync_extra_data_set_clear(extra_data_prev, extra_data, + ULL_ADV_PDU_HDR_FIELD_AD_DATA, + 0, &pdu_data); + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ - return 0; + err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, + ULL_ADV_PDU_HDR_FIELD_AD_DATA, + 0, &pdu_data); + + +#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK) + /* alloc() will return the same PDU as peek() in case there was PDU + * queued but not switched to current before alloc() - no need to deal + * with chain as it's already there. In other case we need to duplicate + * chain from current PDU and append it to new PDU. + */ + if (pdu != pdu_prev) { + struct pdu_adv *next, *next_dup; + + LL_ASSERT(lll_adv_pdu_linked_next_get(pdu) == NULL); + + next = lll_adv_pdu_linked_next_get(pdu_prev); + next_dup = adv_sync_pdu_duplicate_chain(next); + + lll_adv_pdu_linked_append(next_dup, pdu); + } +#endif /* CONFIG_BT_CTLR_ADV_PDU_LINK */ + + if (!err) { + lll_adv_sync_data_enqueue(lll_sync, ter_idx); + } + + return err; } uint8_t ll_adv_sync_enable(uint8_t handle, uint8_t enable) @@ -464,45 +726,22 @@ void ull_adv_sync_update(struct ll_adv_sync_set *sync, uint32_t slot_plus_us, } #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ -/* @brief Set or clear fields in extended advertising header and store - * extra_data if requested. - * - * @param[in] adv Advertising set. - * @param[in] hdr_add_fields Flag with information which fields add. - * @param[in] hdr_rem_fields Flag with information which fields remove. - * @param[in] data Pointer to data to be added to header and - * extra_data. Content depends on the value of - * @p hdr_add_fields. - * @param[out] ter_idx Index of new PDU. - * - * @Note - * @p data content depends on the flag provided by @p hdr_add_fields: - * - ULL_ADV_PDU_HDR_FIELD_CTE_INFO: - * # @p data->field_data points to single byte with CTEInfo field - * # @p data->extra_data points to memory where is struct lll_df_adv_cfg - * for LLL. - * - ULL_ADV_PDU_HDR_FIELD_AD_DATA: - * # @p data->field_data points to memory where first byte - * is size of advertising data, following byte is a pointer to actual - * advertising data. - * # @p data->extra_data is NULL - * - ULL_ADV_PDU_HDR_FIELD_AUX_PTR: # @p data parameter is not used - * - * @return Zero in case of success, other value in case of failure. - */ -uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv, - uint16_t hdr_add_fields, - uint16_t hdr_rem_fields, - struct adv_pdu_field_data *data, - uint8_t *ter_idx) +uint8_t ull_adv_sync_pdu_alloc(struct ll_adv_set *adv, + uint16_t hdr_add_fields, + uint16_t hdr_rem_fields, + struct adv_pdu_field_data *data, + struct pdu_adv **ter_pdu_prev, + struct pdu_adv **ter_pdu_new, + void **extra_data_prev, + void **extra_data_new, + uint8_t *ter_idx) { struct pdu_adv *pdu_prev, *pdu_new; struct lll_adv_sync *lll_sync; - void *extra_data_prev; -#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) - void *extra_data; -#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ - int err; + void *ed_prev; +#if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY) + void *ed_new; +#endif lll_sync = adv->lll.sync; if (!lll_sync) { @@ -510,23 +749,23 @@ uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv, } /* Get reference to previous periodic advertising PDU data */ - pdu_prev = lll_adv_sync_data_peek(lll_sync, &extra_data_prev); + pdu_prev = lll_adv_sync_data_peek(lll_sync, &ed_prev); #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) /* Get reference to new periodic advertising PDU data buffer */ if ((hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) || (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) && - extra_data_prev)) { + ed_prev)) { /* If there was an extra data in past PDU data or it is required * by the hdr_add_fields then allocate memmory for it. */ - pdu_new = lll_adv_sync_data_alloc(lll_sync, &extra_data, + pdu_new = lll_adv_sync_data_alloc(lll_sync, &ed_new, ter_idx); if (!pdu_new) { return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; } } else { - extra_data = NULL; + ed_new = NULL; #else { #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ @@ -536,125 +775,53 @@ uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv, } } - err = adv_sync_hdr_set_clear(lll_sync, pdu_prev, pdu_new, - hdr_add_fields, hdr_rem_fields, - (data ? data->field_data : NULL)); - if (err) { - return err; +#if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY) + if (extra_data_prev) { + *extra_data_prev = ed_prev; } - -#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) - if (extra_data) { - adv_sync_extra_data_set_clear(extra_data_prev, extra_data, - hdr_add_fields, hdr_rem_fields, - (data ? data->extra_data : NULL)); + if (extra_data_new) { + *extra_data_new = ed_new; } -#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ - - return 0; -} - -static int init_reset(void) -{ - /* Initialize adv sync pool. */ - mem_init(ll_adv_sync_pool, sizeof(struct ll_adv_sync_set), - sizeof(ll_adv_sync_pool) / sizeof(struct ll_adv_sync_set), - &adv_sync_free); - - return 0; -} - -static inline struct ll_adv_sync_set *sync_acquire(void) -{ - return mem_acquire(&adv_sync_free); -} - -static inline void sync_release(struct ll_adv_sync_set *sync) -{ - mem_release(sync, &adv_sync_free); -} - -static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync) -{ - return mem_index_get(sync, ll_adv_sync_pool, - sizeof(struct ll_adv_sync_set)); -} +#endif /* CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY */ -static uint8_t sync_stop(struct ll_adv_sync_set *sync) -{ - uint8_t sync_handle; - int err; - - sync_handle = sync_handle_get(sync); - - err = ull_ticker_stop_with_mark(TICKER_ID_ADV_SYNC_BASE + sync_handle, - sync, &sync->lll); - LL_ASSERT(err == 0 || err == -EALREADY); - if (err) { - return BT_HCI_ERR_CMD_DISALLOWED; - } + *ter_pdu_prev = pdu_prev; + *ter_pdu_new = pdu_new; return 0; } -static inline uint8_t sync_remove(struct ll_adv_sync_set *sync, - struct ll_adv_set *adv, uint8_t enable) -{ - uint8_t pri_idx; - uint8_t err; - - /* Remove sync_info from auxiliary PDU */ - err = ull_adv_aux_hdr_set_clear(adv, 0, - ULL_ADV_PDU_HDR_FIELD_SYNC_INFO, - NULL, NULL, &pri_idx); - if (err) { - return err; - } - - lll_adv_data_enqueue(&adv->lll, pri_idx); - - if (sync->is_started) { - /* TODO: we removed sync info, but if sync_stop() fails, what do - * we do? - */ - err = sync_stop(sync); - if (err) { - return err; - } - - sync->is_started = 0U; - } - - if (!enable) { - sync->is_enabled = 0U; - } - - return 0U; -} - -/* @brief Set or clear fields in extended advertising header. +/* @brief Set or clear fields in extended advertising header and store + * extra_data if requested. * * @param[in] adv Advertising set. * @param[in] hdr_add_fields Flag with information which fields add. * @param[in] hdr_rem_fields Flag with information which fields remove. - * @param[in] value Pointer to data to be added to header. - * Content depends on the value of + * @param[in] data Pointer to data to be added to header and + * extra_data. Content depends on the value of * @p hdr_add_fields. * @param[out] ter_idx Index of new PDU. * * @Note - * @p value depends on the flag provided by @p hdr_add_fields. - * Information about content of value may be found in description of - * @ref ull_adv_sync_pdu_set_clear. + * @p data content depends on the flag provided by @p hdr_add_fields: + * - ULL_ADV_PDU_HDR_FIELD_CTE_INFO: + * # @p data->field_data points to single byte with CTEInfo field + * # @p data->extra_data points to memory where is struct lll_df_adv_cfg + * for LLL. + * - ULL_ADV_PDU_HDR_FIELD_AD_DATA: + * # @p data->field_data points to memory where first byte + * is size of advertising data, following byte is a pointer to actual + * advertising data. + * # @p data->extra_data is NULL + * - ULL_ADV_PDU_HDR_FIELD_AUX_PTR: # @p data parameter is not used * * @return Zero in case of success, other value in case of failure. */ -static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync, - struct pdu_adv *ter_pdu_prev, - struct pdu_adv *ter_pdu, - uint16_t hdr_add_fields, - uint16_t hdr_rem_fields, - void *value) +uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync, + struct pdu_adv *ter_pdu_prev, + struct pdu_adv *ter_pdu, + uint16_t hdr_add_fields, + uint16_t hdr_rem_fields, + struct adv_pdu_field_data *data) { struct pdu_adv_com_ext_adv *ter_com_hdr, *ter_com_hdr_prev; struct pdu_adv_ext_hdr *ter_hdr, ter_hdr_prev; @@ -668,6 +835,9 @@ static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync, uint8_t cte_info; #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ uint8_t ad_len; + void *value; + + value = data ? data->field_data : NULL; /* Get common pointers from reference to previous tertiary PDU data */ ter_com_hdr_prev = (void *)&ter_pdu_prev->adv_ext_ind; @@ -871,25 +1041,103 @@ static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync, * * @return Zero in case of success, other value in case of failure. */ -static inline void adv_sync_extra_data_set_clear(void *extra_data_prev, - void *extra_data_new, - uint16_t hdr_add_fields, - uint16_t hdr_rem_fields, - void *data) +void ull_adv_sync_extra_data_set_clear(void *extra_data_prev, + void *extra_data_new, + uint16_t hdr_add_fields, + uint16_t hdr_rem_fields, + void *data) { /* Currently only CTE enable requires extra_data. Due to that fact * CTE additional data are just copied to extra_data memory. */ if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) { memcpy(extra_data_new, data, sizeof(struct lll_df_adv_cfg)); - } else if ((hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) || - extra_data_prev) { + } else if (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) || + extra_data_prev) { memmove(extra_data_new, extra_data_prev, sizeof(struct lll_df_adv_cfg)); } } #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ +static int init_reset(void) +{ + /* Initialize adv sync pool. */ + mem_init(ll_adv_sync_pool, sizeof(struct ll_adv_sync_set), + sizeof(ll_adv_sync_pool) / sizeof(struct ll_adv_sync_set), + &adv_sync_free); + + return 0; +} + +static inline struct ll_adv_sync_set *sync_acquire(void) +{ + return mem_acquire(&adv_sync_free); +} + +static inline void sync_release(struct ll_adv_sync_set *sync) +{ + mem_release(sync, &adv_sync_free); +} + +static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync) +{ + return mem_index_get(sync, ll_adv_sync_pool, + sizeof(struct ll_adv_sync_set)); +} + +static uint8_t sync_stop(struct ll_adv_sync_set *sync) +{ + uint8_t sync_handle; + int err; + + sync_handle = sync_handle_get(sync); + + err = ull_ticker_stop_with_mark(TICKER_ID_ADV_SYNC_BASE + sync_handle, + sync, &sync->lll); + LL_ASSERT(err == 0 || err == -EALREADY); + if (err) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + return 0; +} + +static inline uint8_t sync_remove(struct ll_adv_sync_set *sync, + struct ll_adv_set *adv, uint8_t enable) +{ + uint8_t pri_idx; + uint8_t err; + + /* Remove sync_info from auxiliary PDU */ + err = ull_adv_aux_hdr_set_clear(adv, 0, + ULL_ADV_PDU_HDR_FIELD_SYNC_INFO, + NULL, NULL, &pri_idx); + if (err) { + return err; + } + + lll_adv_data_enqueue(&adv->lll, pri_idx); + + if (sync->is_started) { + /* TODO: we removed sync info, but if sync_stop() fails, what do + * we do? + */ + err = sync_stop(sync); + if (err) { + return err; + } + + sync->is_started = 0U; + } + + if (!enable) { + sync->is_enabled = 0U; + } + + return 0U; +} + static void mfy_sync_offset_get(void *param) { struct ll_adv_set *adv = param; diff --git a/subsys/bluetooth/controller/ll_sw/ull_df.c b/subsys/bluetooth/controller/ll_sw/ull_df.c index 86fcb23545d79..7d0d448d7b764 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_df.c +++ b/subsys/bluetooth/controller/ll_sw/ull_df.c @@ -256,6 +256,8 @@ uint8_t ll_df_set_cl_cte_tx_params(uint8_t adv_handle, uint8_t cte_len, */ uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable) { + void *extra_data_prev, *extra_data; + struct pdu_adv *pdu_prev, *pdu; struct lll_adv_sync *lll_sync; struct lll_df_adv_cfg *df_cfg; struct ll_adv_sync_set *sync; @@ -295,9 +297,25 @@ uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable) return BT_HCI_ERR_CMD_DISALLOWED; } - err = ull_adv_sync_pdu_set_clear(adv, 0, + err = ull_adv_sync_pdu_alloc(adv, 0, + ULL_ADV_PDU_HDR_FIELD_CTE_INFO, + NULL, &pdu_prev, &pdu, + &extra_data_prev, &extra_data, + &ter_idx); + if (err) { + return err; + } + + if (extra_data) { + ull_adv_sync_extra_data_set_clear(extra_data_prev, + extra_data, 0, + ULL_ADV_PDU_HDR_FIELD_CTE_INFO, + NULL); + } + + err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, 0, ULL_ADV_PDU_HDR_FIELD_CTE_INFO, - NULL, &ter_idx); + NULL); if (err) { return err; } @@ -326,9 +344,26 @@ uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable) cte_info.time = df_cfg->cte_length; pdu_data.field_data = (uint8_t *)&cte_info; pdu_data.extra_data = df_cfg; - err = ull_adv_sync_pdu_set_clear(adv, + + err = ull_adv_sync_pdu_alloc(adv, 0, + ULL_ADV_PDU_HDR_FIELD_CTE_INFO, + NULL, &pdu_prev, &pdu, + &extra_data_prev, &extra_data, + &ter_idx); + if (err) { + return err; + } + + if (extra_data) { + ull_adv_sync_extra_data_set_clear(extra_data_prev, + extra_data, + ULL_ADV_PDU_HDR_FIELD_CTE_INFO, + 0, &pdu_data); + } + + err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, ULL_ADV_PDU_HDR_FIELD_CTE_INFO, - 0, &pdu_data, &ter_idx); + 0, &pdu_data); if (err) { return err; } diff --git a/subsys/bluetooth/controller/ll_sw/ull_master.c b/subsys/bluetooth/controller/ll_sw/ull_master.c index 54e7c00b3c10f..705e19f2a1209 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_master.c +++ b/subsys/bluetooth/controller/ll_sw/ull_master.c @@ -11,6 +11,7 @@ #include "util/util.h" #include "util/memq.h" +#include "util/mem.h" #include "util/mayfly.h" #include "hal/cpu.h" diff --git a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c index 3a59e48ba15ea..da9234c5f03bf 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c +++ b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c @@ -269,7 +269,7 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx) lll->chan = aux_ptr->chan_idx; lll->phy = BIT(aux_ptr->phy); - aux_offset_us = ftr->radio_end_us - PKT_AC_US(pdu->len, 0, phy); + aux_offset_us = ftr->radio_end_us - PKT_AC_US(pdu->len, phy); if (aux_ptr->offs_units) { lll->window_size_us = 300U; } else { @@ -302,7 +302,7 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx) HAL_TICKER_US_TO_TICKS(EVENT_OVERHEAD_START_US + ready_delay_us + PKT_AC_US(PDU_AC_EXT_PAYLOAD_SIZE_MAX, - 0, lll->phy) + + lll->phy) + EVENT_OVERHEAD_END_US); ticks_slot_offset = MAX(aux->ull.ticks_active_to_start, diff --git a/subsys/bluetooth/controller/ll_sw/ull_slave.c b/subsys/bluetooth/controller/ll_sw/ull_slave.c index f3353ed8efaee..bf30cbe0f5b14 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_slave.c +++ b/subsys/bluetooth/controller/ll_sw/ull_slave.c @@ -11,6 +11,7 @@ #include "util/util.h" #include "util/memq.h" +#include "util/mem.h" #include "util/mayfly.h" #include "hal/cpu.h" diff --git a/subsys/bluetooth/controller/ll_sw/ull_sync.c b/subsys/bluetooth/controller/ll_sw/ull_sync.c index 99d82371a8a7c..1355be891a455 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_sync.c +++ b/subsys/bluetooth/controller/ll_sw/ull_sync.c @@ -438,7 +438,7 @@ void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux, sync_offset_us += (uint32_t)si->offs * lll->window_size_event_us; /* offs_adjust may be 1 only if sync setup by LL_PERIODIC_SYNC_IND */ sync_offset_us += (si->offs_adjust ? OFFS_ADJUST_US : 0U); - sync_offset_us -= PKT_AC_US(pdu->len, 0, lll->phy); + sync_offset_us -= PKT_AC_US(pdu->len, lll->phy); sync_offset_us -= EVENT_TICKER_RES_MARGIN_US; sync_offset_us -= EVENT_JITTER_US; sync_offset_us -= ready_delay_us; @@ -455,7 +455,7 @@ void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux, HAL_TICKER_US_TO_TICKS(EVENT_OVERHEAD_START_US + ready_delay_us + PKT_AC_US(PDU_AC_EXT_PAYLOAD_SIZE_MAX, - 0, lll->phy) + + lll->phy) + EVENT_OVERHEAD_END_US); ticks_slot_offset = MAX(sync->ull.ticks_active_to_start,