From c8475b9a3efa9a14bb94e13183c87393620a9bea Mon Sep 17 00:00:00 2001 From: Piotr Pryga Date: Fri, 22 Jan 2021 05:38:47 -0800 Subject: [PATCH] Bluetooth: controller: ULL: enable TX of CTE with per. adv. PDU Enable transmission of CTE with periodic advertising PDU. Signed-off-by: Piotr Pryga --- .../ll_sw/nordic/lll/lll_adv_sync.c | 2 + subsys/bluetooth/controller/ll_sw/ull_adv.c | 2 +- .../bluetooth/controller/ll_sw/ull_adv_aux.c | 14 + .../controller/ll_sw/ull_adv_internal.h | 32 +- .../bluetooth/controller/ll_sw/ull_adv_sync.c | 562 +++++++++++++----- subsys/bluetooth/controller/ll_sw/ull_df.c | 118 +++- 6 files changed, 572 insertions(+), 158 deletions(-) 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 33df37e7bea..84772ea38aa 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 @@ -29,7 +29,9 @@ #include "lll_internal.h" #include "lll_adv_internal.h" #include "lll_tim_internal.h" +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) #include "lll_df_internal.h" +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER) #define LOG_MODULE_NAME bt_ctlr_lll_adv_sync diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv.c b/subsys/bluetooth/controller/ll_sw/ull_adv.c index 781165e0c9d..1275415f0d5 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c @@ -1204,7 +1204,7 @@ uint8_t ll_adv_enable(uint8_t enable) ticks_slot_aux + HAL_TICKER_US_TO_TICKS(EVENT_MAFS_US); - ret = ull_adv_sync_start(sync, + ret = ull_adv_sync_start(adv, sync, ticks_anchor_sync); if (ret) { goto failure_cleanup; diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c index e3a7e7f525c..0aa5d1763ae 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c @@ -27,6 +27,9 @@ #include "lll_adv.h" #include "lll_adv_aux.h" #include "lll_adv_internal.h" +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) +#include "lll_df_internal.h" +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ #include "ull_adv_types.h" @@ -355,6 +358,17 @@ uint8_t ll_adv_aux_set_remove(uint8_t handle) } #endif /* CONFIG_BT_CTLR_ADV_PERIODIC */ +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + if (adv->df_cfg) { + if (adv->df_cfg->is_enabled) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + ull_df_adv_cfg_release(adv->df_cfg); + adv->df_cfg = NULL; + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + /* Release auxiliary channel set */ if (lll->aux) { struct ll_adv_aux_set *aux; diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h index fb4941b47b2..425ea95eeea 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h @@ -47,9 +47,19 @@ 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_CTE_INFO BIT(2) +#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_AD_DATA BIT(8) +/* Helper type to store data for extended advertising + * header fields and extra data. + */ +struct adv_pdu_field_data { + uint8_t *field_data; + void *extra_data; +}; + /* helper function to handle adv done events */ void ull_adv_done(struct node_rx_event_done *done); @@ -91,6 +101,15 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv, struct pdu_adv_adi *adi, uint8_t *pri_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, + uint16_t hdr_add_fields, + uint16_t hdr_rem_fields, + struct adv_pdu_field_data *data, + uint8_t *ter_idx); + /* helper function to calculate common ext adv payload header length and * adjust the data pointer. * NOTE: This function reverts the header data pointer if there is no @@ -129,12 +148,23 @@ int ull_adv_sync_init(void); int ull_adv_sync_reset(void); /* helper function to start periodic advertising */ -uint32_t ull_adv_sync_start(struct ll_adv_sync_set *sync, +uint32_t ull_adv_sync_start(struct ll_adv_set *adv, + struct ll_adv_sync_set *sync, uint32_t ticks_anchor); +/* helper function to update periodic advertising event length */ +void ull_adv_sync_update(struct ll_adv_sync_set *sync, uint32_t slot_plus_us, + uint32_t slot_minus_us); + /* helper function to schedule a mayfly to get sync offset */ void ull_adv_sync_offset_get(struct ll_adv_set *adv); int ull_adv_iso_init(void); int ull_adv_iso_reset(void); + +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) +/* helper function to release unused DF configuration memory */ +void ull_df_adv_cfg_release(struct lll_df_adv_cfg *df_adv_cfg); +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + #endif /* CONFIG_BT_CTLR_ADV_EXT */ diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c index e8c75fc1185..65d3f244a03 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c @@ -26,6 +26,9 @@ #include "lll_adv.h" #include "lll_adv_sync.h" #include "lll_adv_internal.h" +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) +#include "lll_df_internal.h" +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ #include "ull_adv_types.h" @@ -44,6 +47,22 @@ static inline void sync_release(struct ll_adv_sync_set *sync); 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 IS_ENABLED(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, @@ -56,6 +75,94 @@ 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; +/* @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) +{ + struct pdu_adv *pdu_prev, *pdu_new; + struct lll_adv_sync *lll_sync; + void *extra_data_prev; +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + void *extra_data; +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + int err; + + lll_sync = adv->lll.sync; + if (!lll_sync) { + return BT_HCI_ERR_UNKNOWN_ADV_IDENTIFIER; + } + + /* Get reference to previous periodic advertising PDU data */ + pdu_prev = lll_adv_sync_data_peek(lll_sync, &extra_data_prev); + +#if IS_ENABLED(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)) { + /* 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, ter_idx); + if (!pdu_new) { + return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; + } + } else { + extra_data = NULL; +#else + { +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + pdu_new = lll_adv_sync_data_alloc(lll_sync, NULL, ter_idx); + if (!pdu_new) { + return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; + } + } + + 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 IS_ENABLED(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)); + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + return 0; +} + uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) { struct pdu_adv_ext_hdr *ter_hdr, ter_hdr_prev; @@ -65,7 +172,7 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) struct ll_adv_sync_set *sync; struct pdu_adv *ter_pdu; struct ll_adv_set *adv; - uint8_t ter_len; + uint8_t err, ter_idx; adv = ull_adv_is_created_get(handle); if (!adv) { @@ -111,55 +218,39 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) sync->is_enabled = 0U; 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_pdu->len = offsetof(struct pdu_adv_com_ext_adv, + ext_hdr_adv_data); + + ter_com_hdr = (void *)&ter_pdu->adv_ext_ind; + ter_hdr = (void *)ter_com_hdr->ext_hdr_adv_data; + ter_dptr = ter_hdr->data; + ter_hdr_prev = *ter_hdr; + *(uint8_t *)ter_hdr = 0U; + ter_dptr_prev = ter_dptr; + + /* Non-connectable and Non-scannable adv mode */ + ter_com_hdr->adv_mode = 0U; } else { sync = (void *)HDR_LLL2EVT(lll_sync); } sync->interval = interval; - 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; - ter_hdr_prev = *ter_hdr; - *(uint8_t *)ter_hdr = 0U; - ter_dptr_prev = ter_dptr; - - /* Non-connectable and Non-scannable adv mode */ - ter_com_hdr->adv_mode = 0U; - - /* No AdvA */ - /* No TargetA */ - - /* TODO: CTEInfo */ - - /* No ADI */ - - /* TODO: AuxPtr */ - - /* No SyncInfo */ - - /* TODO: TxPower */ - if (flags & BT_HCI_LE_ADV_PROP_TX_POWER) { - /* TODO: add/remove Tx Power in AUX_SYNC_IND PDU */ - return BT_HCI_ERR_CMD_DISALLOWED; + err = ull_adv_sync_pdu_set_clear(adv, 0, 0, NULL, &ter_idx); + if (err) { + return err; } - /* TODO: ACAD */ - - /* TODO: AdvData */ - - /* 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; + lll_adv_sync_data_enqueue(lll_sync, ter_idx); return 0; } @@ -167,14 +258,19 @@ 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) { - struct pdu_adv_com_ext_adv *ter_com_hdr, *ter_com_hdr_prev; - struct pdu_adv_ext_hdr *ter_hdr, ter_hdr_prev; - struct pdu_adv *ter_pdu, *ter_pdu_prev; - uint8_t *ter_dptr, *ter_dptr_prev; - uint16_t ter_len, ter_len_prev; + struct adv_pdu_field_data pdu_data; struct lll_adv_sync *lll_sync; struct ll_adv_set *adv; + uint8_t value[5]; uint8_t ter_idx; + uint8_t err; + + /* TODO: handle other op values */ + if (op != BT_HCI_LE_EXT_ADV_OP_COMPLETE_DATA && + op != BT_HCI_LE_EXT_ADV_OP_UNCHANGED_DATA) { + /* FIXME: error code */ + return BT_HCI_ERR_CMD_DISALLOWED; + } adv = ull_adv_is_created_get(handle); if (!adv) { @@ -186,113 +282,18 @@ uint8_t ll_adv_sync_ad_data_set(uint8_t handle, uint8_t op, uint8_t len, return BT_HCI_ERR_UNKNOWN_ADV_IDENTIFIER; } - /* Get reference to previous tertiary PDU data */ - ter_pdu_prev = lll_adv_sync_data_peek(lll_sync, NULL); - ter_com_hdr_prev = (void *)&ter_pdu_prev->adv_ext_ind; - ter_hdr = (void *)ter_com_hdr_prev->ext_hdr_adv_data; - ter_hdr_prev = *ter_hdr; - ter_dptr_prev = ter_hdr->data; + pdu_data.field_data = value; + *pdu_data.field_data = len; + sys_put_le32((uint32_t)data, pdu_data.field_data + 1); - /* Get reference to new tertiary PDU data buffer */ - ter_pdu = lll_adv_sync_data_alloc(lll_sync, NULL, &ter_idx); - if (!ter_pdu) { - return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; + err = ull_adv_sync_pdu_set_clear(adv, ULL_ADV_PDU_HDR_FIELD_AD_DATA, + 0, &pdu_data, &ter_idx); + if (err) { + return err; } - ter_pdu->type = ter_pdu_prev->type; - ter_pdu->rfu = 0U; - ter_pdu->chan_sel = 0U; - ter_com_hdr = (void *)&ter_pdu->adv_ext_ind; - ter_com_hdr->adv_mode = ter_com_hdr_prev->adv_mode; - ter_hdr = (void *)ter_com_hdr->ext_hdr_adv_data; - ter_dptr = ter_hdr->data; - *(uint8_t *)ter_hdr = 0U; - - /* No AdvA */ - /* No TargetA */ - - /* TODO: CTEInfo */ - - /* No ADI */ - - /* AuxPtr */ - if (ter_hdr_prev.aux_ptr) { - ter_dptr_prev += sizeof(struct pdu_adv_aux_ptr); - - ter_hdr->aux_ptr = 1; - ter_dptr += sizeof(struct pdu_adv_aux_ptr); - } - - /* No SyncInfo */ - - /* Tx Power flag */ - if (ter_hdr_prev.tx_pwr) { - ter_dptr_prev++; - - ter_hdr->tx_pwr = 1; - ter_dptr++; - } - - /* TODO: ACAD */ - - /* AdvData */ - /* Calc previous tertiary PDU len */ - ter_len_prev = ull_adv_aux_hdr_len_calc(ter_com_hdr_prev, - &ter_dptr_prev); - - /* Did we parse beyond PDU length? */ - if (ter_len_prev > ter_pdu_prev->len) { - /* we should not encounter invalid length */ - return BT_HCI_ERR_UNSPECIFIED; - } - - /* Calc current 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); - - /* Add AD len to secondary PDU length */ - ter_len += len; - - /* Check AdvData overflow */ - if (ter_len > PDU_AC_PAYLOAD_SIZE_MAX) { - return BT_HCI_ERR_PACKET_TOO_LONG; - } - - /* set the secondary PDU len */ - ter_pdu->len = ter_len; - - /* Start filling tertiary PDU payload based on flags from here - * ============================================================== - */ - - /* Fill AdvData in tertiary PDU */ - memmove(ter_dptr, data, len); - - /* TODO: Fill ACAD in tertiary PDU */ - - /* Tx Power */ - if (ter_hdr->tx_pwr) { - *--ter_dptr = *--ter_dptr_prev; - } - - /* No SyncInfo in tertiary PDU */ - - /* AuxPtr */ - if (ter_hdr_prev.aux_ptr) { - ter_dptr_prev -= sizeof(struct pdu_adv_aux_ptr); - - ull_adv_aux_ptr_fill(&ter_dptr, lll_sync->adv->phy_s); - } - - /* No ADI */ - - /* TODO: CTEInfo */ - - /* No TargetA*/ - /* No AdvA */ lll_adv_sync_data_enqueue(lll_sync, ter_idx); - return 0; } @@ -382,7 +383,7 @@ uint8_t ll_adv_sync_enable(uint8_t handle, uint8_t enable) HAL_TICKER_US_TO_TICKS(EVENT_MAFS_US); } - ret = ull_adv_sync_start(sync, ticks_anchor_sync); + ret = ull_adv_sync_start(adv, sync, ticks_anchor_sync); if (ret) { sync_remove(sync, adv, 1U); @@ -450,9 +451,13 @@ uint16_t ull_adv_sync_lll_handle_get(struct lll_adv_sync *lll) return sync_handle_get((void *)lll->hdr.parent); } -uint32_t ull_adv_sync_start(struct ll_adv_sync_set *sync, +uint32_t ull_adv_sync_start(struct ll_adv_set *adv, + struct ll_adv_sync_set *sync, uint32_t ticks_anchor) { +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + struct lll_df_adv_cfg *df_cfg; +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ uint32_t ticks_slot_overhead; uint32_t volatile ret_cb; uint32_t interval_us; @@ -466,6 +471,13 @@ uint32_t ull_adv_sync_start(struct ll_adv_sync_set *sync, slot_us = EVENT_OVERHEAD_START_US + EVENT_OVERHEAD_END_US; slot_us += 1000; +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + df_cfg = adv->df_cfg; + if (df_cfg && df_cfg->is_enabled) { + slot_us += CTE_LEN_US(df_cfg->cte_length); + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + /* TODO: active_to_start feature port */ sync->evt.ticks_active_to_start = 0; sync->evt.ticks_xtal_to_start = @@ -511,6 +523,25 @@ void ull_adv_sync_offset_get(struct ll_adv_set *adv) LL_ASSERT(!ret); } +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) +void ull_adv_sync_update(struct ll_adv_sync_set *sync, uint32_t slot_plus_us, + uint32_t slot_minus_us) +{ + uint32_t ret; + + ret = ticker_update(TICKER_INSTANCE_ID_CTLR, + TICKER_USER_ID_THREAD, + (TICKER_ID_ADV_BASE + sync_handle_get(sync)), + 0, 0, + slot_plus_us, + slot_minus_us, + 0, 0, + NULL, NULL); + LL_ASSERT((ret == TICKER_STATUS_SUCCESS) || + (ret == TICKER_STATUS_BUSY)); +} +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + static int init_reset(void) { /* Initialize adv sync pool. */ @@ -589,6 +620,235 @@ static inline uint8_t sync_remove(struct ll_adv_sync_set *sync, return 0U; } +/* @brief Set or clear fields in extended advertising header. + * + * @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 + * @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. + * + * @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) +{ + struct pdu_adv_com_ext_adv *ter_com_hdr, *ter_com_hdr_prev; + struct pdu_adv_ext_hdr *ter_hdr, ter_hdr_prev; + uint8_t *ter_dptr, *ter_dptr_prev; + uint16_t ter_len, ter_len_prev; + uint8_t *ad_data; +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + uint8_t cte_info; +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + uint8_t ad_len; + + /* Get common pointers from reference to previous tertiary PDU data */ + ter_com_hdr_prev = (void *)&ter_pdu_prev->adv_ext_ind; + ter_hdr = (void *)ter_com_hdr_prev->ext_hdr_adv_data; + ter_hdr_prev = *ter_hdr; + ter_dptr_prev = ter_hdr->data; + + /* Set common fields in reference to new tertiary PDU data buffer */ + ter_pdu->type = ter_pdu_prev->type; + ter_pdu->rfu = 0U; + ter_pdu->chan_sel = 0U; + + ter_pdu->tx_addr = ter_pdu_prev->tx_addr; + ter_pdu->rx_addr = ter_pdu_prev->rx_addr; + + ter_com_hdr = (void *)&ter_pdu->adv_ext_ind; + ter_com_hdr->adv_mode = ter_com_hdr_prev->adv_mode; + ter_hdr = (void *)ter_com_hdr->ext_hdr_adv_data; + ter_dptr = ter_hdr->data; + *(uint8_t *)ter_hdr = 0U; + + /* No AdvA in AUX_SYNC_IND */ + /* No TargetA in AUX_SYNC_IND */ + +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + /* If requested add or update CTEInfo */ + if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) { + ter_hdr->cte_info = 1; + cte_info = *(uint8_t *)value; + value = (uint8_t *)value + 1; + ter_dptr += sizeof(struct pdu_cte_info); + /* If CTEInfo exists in prev and is not requested to be removed */ + } else if (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) && + ter_hdr_prev.cte_info) { + ter_hdr->cte_info = 1; + ter_dptr += sizeof(struct pdu_cte_info); + } + + /* If CTEInfo exists in prev PDU */ + if (ter_hdr_prev.cte_info) { + ter_dptr_prev += sizeof(struct pdu_cte_info); + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + + /* No ADI in AUX_SYNC_IND */ + + /* AuxPtr - will be added if AUX_CHAIN_IND is required */ + if ((hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_AUX_PTR) || + (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_AUX_PTR) && + ter_hdr_prev.aux_ptr)) { + ter_hdr->aux_ptr = 1; + } + if (ter_hdr->aux_ptr) { + ter_dptr += sizeof(struct pdu_adv_aux_ptr); + } + if (ter_hdr_prev.aux_ptr) { + ter_dptr_prev += sizeof(struct pdu_adv_aux_ptr); + } + + /* No SyncInfo in AUX_SYNC_IND */ + + /* Tx Power flag */ + if (ter_hdr_prev.tx_pwr) { + ter_dptr_prev++; + + ter_hdr->tx_pwr = 1; + ter_dptr++; + } + + /* TODO: ACAD */ + + /* AdvData */ + /* Calc previous tertiary PDU len */ + ter_len_prev = ull_adv_aux_hdr_len_calc(ter_com_hdr_prev, + &ter_dptr_prev); + + /* Did we parse beyond PDU length? */ + if (ter_len_prev > ter_pdu_prev->len) { + /* we should not encounter invalid length */ + return BT_HCI_ERR_UNSPECIFIED; + } + + /* Calc current 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); + + /* Get Adv data from function parameters */ + if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_AD_DATA) { + ad_data = value; + ad_len = *ad_data; + ++ad_data; + + ad_data = (void *)sys_get_le32(ad_data); + } else if (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_AD_DATA)) { + ad_len = ter_pdu_prev->len - ter_len_prev; + ad_data = ter_dptr_prev; + } else { + ad_len = 0; + ad_data = NULL; + } + + /* Add AD len to tertiary PDU length */ + ter_len += ad_len; + + /* Check AdvData overflow */ + if (ter_len > PDU_AC_PAYLOAD_SIZE_MAX) { + return BT_HCI_ERR_PACKET_TOO_LONG; + } + + /* set the secondary PDU len */ + ter_pdu->len = ter_len; + + /* Start filling tertiary PDU payload based on flags from here + * ============================================================== + */ + + /* Fill AdvData in tertiary PDU */ + memmove(ter_dptr, ad_data, ad_len); + + /* TODO: Fill ACAD in tertiary PDU */ + + /* Tx Power */ + if (ter_hdr->tx_pwr) { + *--ter_dptr = *--ter_dptr_prev; + } + + /* No SyncInfo in AUX_SYNC_IND */ + + /* AuxPtr */ + if (ter_hdr->aux_ptr) { + /* ToDo Update setup of aux_ptr - check documentation */ + if (ter_hdr_prev.aux_ptr) { + ter_dptr_prev -= sizeof(struct pdu_adv_aux_ptr); + ter_dptr -= sizeof(struct pdu_adv_aux_ptr); + memmove(ter_dptr, ter_dptr_prev, + sizeof(struct pdu_adv_aux_ptr)); + } else { + ull_adv_aux_ptr_fill(&ter_dptr, lll_sync->adv->phy_s); + } + } + + /* No ADI in AUX_SYNC_IND*/ + +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) + if (ter_hdr->cte_info) { + if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) { + *--ter_dptr = cte_info; + } else { + *--ter_dptr = *--ter_dptr_prev; + } + } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + + /* No TargetA in AUX_SYNC_IND */ + /* No AdvA in AUX_SYNC_IND */ + + return 0; +} + +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) +/* @brief Set or clear fields in extended advertising header and store + * extra_data if requested. + * + * @param[in] extra_data_prev Pointer to previous content of extra_data. + * @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 stored in extra_data. + * Content depends on the data depends on + * @p hdr_add_fields. + * + * @Note + * @p data 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. + * + * @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) +{ + /* 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) { + memmove(extra_data_new, extra_data_prev, + sizeof(struct lll_df_adv_cfg)); + } +} +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ + 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 e260927af53..2814c621fd5 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_df.c +++ b/subsys/bluetooth/controller/ll_sw/ull_df.c @@ -54,7 +54,7 @@ static int init_reset(void); * * @return Pointer to lll_df_adv_cfg or NULL if there is no more free memory. */ -static struct lll_df_adv_cfg *ull_df_adv_cfg_acquire(void); +static struct lll_df_adv_cfg *df_adv_cfg_acquire(void); /* @brief Function performs ULL Direction Finding initialization * @@ -98,6 +98,7 @@ static int init_reset(void) return 0; } +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) /* @brief Function sets CTE transmission parameters for periodic advertising. * * @param[in]adv_handle Handle of advertising set. @@ -118,8 +119,8 @@ uint8_t ll_df_set_cl_cte_tx_params(uint8_t adv_handle, uint8_t cte_len, uint8_t cte_type, uint8_t cte_count, uint8_t num_ant_ids, uint8_t *ant_ids) { - struct ll_adv_set *adv; struct lll_df_adv_cfg *cfg; + struct ll_adv_set *adv; /* Get the advertising set instance */ adv = ull_adv_is_created_get(adv_handle); @@ -158,7 +159,7 @@ uint8_t ll_df_set_cl_cte_tx_params(uint8_t adv_handle, uint8_t cte_len, } if (!adv->df_cfg) { - adv->df_cfg = ull_df_adv_cfg_acquire(); + adv->df_cfg = df_adv_cfg_acquire(); } cfg = adv->df_cfg; @@ -196,8 +197,102 @@ 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) { - return BT_HCI_ERR_CMD_DISALLOWED; + struct lll_adv_sync *lll_sync; + struct lll_df_adv_cfg *df_cfg; + struct ll_adv_sync_set *sync; + struct ll_adv_set *adv; + uint8_t err, ter_idx; + + /* Get the advertising set instance */ + adv = ull_adv_is_created_get(adv_handle); + if (!adv) { + return BT_HCI_ERR_UNKNOWN_ADV_IDENTIFIER; + } + + lll_sync = adv->lll.sync; + /* If there is no sync in advertising set, then the HCI_LE_Set_- + * Periodic_Advertising_Parameters command was not issued before. + */ + if (!lll_sync) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + sync = (void *)HDR_LLL2EVT(lll_sync); + + /* If df_cfg is NULL, then the HCI_LE_Set_Connectionless_CTE_Transmit_- + * Parameters command was not issued before. + */ + df_cfg = adv->df_cfg; + if (!df_cfg) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + if (adv->lll.phy_s == PHY_CODED) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + if (!cte_enable) { + if (!df_cfg->is_enabled) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + err = ull_adv_sync_pdu_set_clear(adv, 0, + ULL_ADV_PDU_HDR_FIELD_CTE_INFO, + NULL, &ter_idx); + if (err) { + return err; + } + + if (sync->is_started) { + /* If CTE is disabled when advertising is pending, + * decrease advertising event length + */ + ull_adv_sync_update(sync, 0, df_cfg->cte_length); + /* ToDo decrease number of chain PDUs in pending + * advertising if there are added empty chain PDUs + * to sent requested number of CTEs in a chain + */ + } + + df_cfg->is_enabled = 0U; + } else { + struct pdu_cte_info cte_info; + struct adv_pdu_field_data pdu_data; + + if (df_cfg->is_enabled) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + cte_info.type = df_cfg->cte_type; + 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, + ULL_ADV_PDU_HDR_FIELD_CTE_INFO, + 0, &pdu_data, &ter_idx); + if (err) { + return err; + } + + if (sync->is_started) { + /* If CTE is enabled when advertising is pending, + * increase advertising event length + */ + ull_adv_sync_update(sync, df_cfg->cte_length, 0); + /* ToDo increase number of chain PDUs in pending + * advertising if requested more CTEs than available + * PDU with advertising data. + */ + } + + df_cfg->is_enabled = 1U; + } + + lll_adv_sync_data_enqueue(adv->lll.sync, ter_idx); + + return BT_HCI_ERR_SUCCESS; } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ /* @brief Function sets CTE transmission parameters for a connection. * @@ -268,7 +363,19 @@ void ll_df_read_ant_inf(uint8_t *switch_sample_rates, *max_cte_len = LLL_DF_MAX_CTE_LEN; } -static struct lll_df_adv_cfg *ull_df_adv_cfg_acquire(void) +#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX) +/* @brief Function releases unused memory for DF advertising configuration. + * + * The memory is released to private @ref lll_df_adv_cfg_pool memory store. + * + * @param[in] df_adv_cfg Pointer to lll_df_adv_cfg memory to be released. + */ +void ull_df_adv_cfg_release(struct lll_df_adv_cfg *df_adv_cfg) +{ + mem_release(df_adv_cfg, &df_adv_cfg_free); +} + +static struct lll_df_adv_cfg *df_adv_cfg_acquire(void) { struct lll_df_adv_cfg *df_adv_cfg; @@ -281,3 +388,4 @@ static struct lll_df_adv_cfg *ull_df_adv_cfg_acquire(void) return df_adv_cfg; } +#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */