michaelh/zephyr
1
0
Fork 0
Code Releases Activity

Bluetooth: controller: ULL: enable TX of CTE with per. adv. PDU

Browse source 
Enable transmission of CTE with periodic advertising PDU.

Signed-off-by: Piotr Pryga <piotr.pryga@nordicsemi.no>
This commit is contained in:
Piotr Pryga 2021-01-22 05:38:47 -08:00 • committed by Anas Nashif
commit c8475b9a3e
6 changed files with 572 additions and 158 deletions
Download patch file Download diff file Expand all files Collapse all files

2
subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
View file

@ -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

2
subsys/bluetooth/controller/ll_sw/ull_adv.c
View file

@ -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;

14
subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
View file

@ -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;

32
subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
View file

@ -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 */

562
subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
View file

@ -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;

118
subsys/bluetooth/controller/ll_sw/ull_df.c
View file

@ -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 */