michaelh/zephyr
1
0
Fork 0
Code Releases Activity

Bluetooth: controller: Fix AdvA/TgtA for extended adv PDUs

Browse source 
AdvA in extended advertising PDUs was only set if random address was
used. This patch enables proper support for AdvA/TgtA in those PDUs
also with LL Privacy enabled.

On enable, we always update advertising PDU (i.e. ADV_IND, ADV_EXT_IND
or ADV_AUX_IND, depending on advertising set parameters) as well as
scan response PDU.

On RPA timeout, we simply copy old PDU as-is and update AdvA in new
PDU, since both PDUs are exactly the same (except AdvA) so no need to
recreate it step-by-step.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
This commit is contained in:
Andrzej Kaczmarek 2020-11-02 16:24:58 +01:00 • committed by Carles Cufí
commit e4c39d6e19
4 changed files with 160 additions and 108 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -75,6 +75,9 @@ static void ext_disabled_cb(void *param);
static inline uint8_t disable(uint8_t handle);
static const uint8_t *adva_update(struct ll_adv_set *adv, struct pdu_adv *pdu);
static void tgta_update(struct ll_adv_set *adv, struct pdu_adv *pdu);
static struct ll_adv_set ll_adv[BT_CTLR_ADV_SET];
#if defined(CONFIG_BT_TICKER_EXT)
@ -381,10 +384,15 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
} else {
pdu->tx_addr = 0;
}
pdu->rx_addr = 0;
/* TODO: TargetA flag in primary channel PDU only for directed
*/
/* TargetA flag in primary channel PDU only for directed */
if (evt_prop & BT_HCI_LE_ADV_PROP_DIRECT) {
pri_hdr->tgt_addr = 1;
pdu->rx_addr = direct_addr_type;
pri_dptr += BDADDR_SIZE;
} else {
pdu->rx_addr = 0;
}
/* No CTEInfo flag in primary channel PDU */
@ -473,7 +481,12 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
/* No CTEInfo field in primary channel PDU */
/* NOTE: TargetA, filled at enable and RPA timeout */
/* TargetA */
if (pri_hdr->tgt_addr) {
pri_dptr -= BDADDR_SIZE;
/* NOTE: RPA will be updated on enable, if needed */
memcpy(pri_dptr, direct_addr, BDADDR_SIZE);
}
/* NOTE: AdvA, filled at enable and RPA timeout */
@ -582,6 +595,7 @@ uint8_t ll_adv_enable(uint8_t enable)
uint8_t const handle = 0;
uint32_t ticks_anchor;
#endif /* !CONFIG_BT_CTLR_ADV_EXT || !CONFIG_BT_HCI_MESH_EXT */
struct pdu_adv *pdu_adv_to_update;
uint32_t ticks_slot_overhead;
uint32_t ticks_slot_offset;
uint32_t volatile ret_cb;
@ -604,18 +618,16 @@ uint8_t ll_adv_enable(uint8_t enable)
pdu_adv = lll_adv_data_peek(lll);
pdu_scan = lll_adv_scan_rsp_peek(lll);
pdu_adv_to_update = NULL;
if (0) {
#if defined(CONFIG_BT_CTLR_ADV_EXT)
} else if (pdu_adv->type == PDU_ADV_TYPE_EXT_IND) {
struct pdu_adv_com_ext_adv *pri_com_hdr;
struct pdu_adv_hdr *pri_hdr;
uint8_t *pri_dptr;
pri_com_hdr = (void *)&pdu_adv->adv_ext_ind;
pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
if (pri_com_hdr->adv_mode & BT_HCI_LE_ADV_PROP_SCAN) {
struct pdu_adv *sr = lll_adv_scan_rsp_peek(lll);
@ -627,100 +639,73 @@ uint8_t ll_adv_enable(uint8_t enable)
/* AdvA, fill here at enable */
if (pri_hdr->adv_addr) {
uint8_t const *tx_addr =
ll_adv_aux_random_addr_get(adv, NULL);
/* TODO: Privacy */
if (pdu_adv->tx_addr &&
!mem_nz((void *)tx_addr, BDADDR_SIZE)) {
return BT_HCI_ERR_INVALID_PARAM;
}
memcpy(pri_dptr, tx_addr, BDADDR_SIZE);
pdu_adv_to_update = pdu_adv;
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
} else if (pri_hdr->aux_ptr) {
struct pdu_adv_com_ext_adv *sec_com_hdr;
struct pdu_adv_hdr *sec_hdr;
struct pdu_adv *sec_pdu;
uint8_t *sec_dptr;
sec_pdu = lll_adv_aux_data_peek(lll->aux);
sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
sec_hdr = (void *)sec_com_hdr->ext_hdr_adi_adv_data;
sec_dptr = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
if (sec_hdr->adv_addr) {
uint8_t const *tx_addr =
ll_adv_aux_random_addr_get(adv, NULL);
/* TODO: Privacy */
if (sec_pdu->tx_addr &&
!mem_nz((void *)tx_addr, BDADDR_SIZE)) {
return BT_HCI_ERR_INVALID_PARAM;
}
memcpy(sec_dptr, tx_addr, BDADDR_SIZE);
pdu_adv_to_update = sec_pdu;
}
#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
}
/* TODO: TargetA, fill here at enable */
#endif /* CONFIG_BT_CTLR_ADV_EXT */
} else {
bool priv = false;
pdu_adv_to_update = pdu_adv;
}
#if defined(CONFIG_BT_CTLR_PRIVACY)
lll->rl_idx = FILTER_IDX_NONE;
lll->rl_idx = FILTER_IDX_NONE;
/* Prepare whitelist and optionally resolving list */
ull_filter_adv_update(lll->filter_policy);
/* Prepare whitelist and optionally resolving list */
ull_filter_adv_update(lll->filter_policy);
if (adv->own_addr_type == BT_ADDR_LE_PUBLIC_ID ||
adv->own_addr_type == BT_ADDR_LE_RANDOM_ID) {
/* Look up the resolving list */
lll->rl_idx = ull_filter_rl_find(adv->id_addr_type,
adv->id_addr, NULL);
if (adv->own_addr_type == BT_ADDR_LE_PUBLIC_ID ||
adv->own_addr_type == BT_ADDR_LE_RANDOM_ID) {
/* Look up the resolving list */
lll->rl_idx = ull_filter_rl_find(adv->id_addr_type,
adv->id_addr, NULL);
if (lll->rl_idx != FILTER_IDX_NONE) {
/* Generate RPAs if required */
ull_filter_rpa_update(false);
}
ull_filter_adv_pdu_update(adv, pdu_adv);
ull_filter_adv_pdu_update(adv, pdu_scan);
priv = true;
if (lll->rl_idx != FILTER_IDX_NONE) {
/* Generate RPAs if required */
ull_filter_rpa_update(false);
}
}
#endif /* !CONFIG_BT_CTLR_PRIVACY */
if (!priv) {
uint8_t const *tx_addr;
#if defined(CONFIG_BT_CTLR_ADV_EXT)
if (ll_adv_cmds_is_ext() && pdu_adv->tx_addr) {
tx_addr = ll_adv_aux_random_addr_get(adv, NULL);
} else
#endif /* CONFIG_BT_CTLR_ADV_EXT */
{
tx_addr = ll_addr_get(pdu_adv->tx_addr, NULL);
}
if (pdu_adv_to_update) {
const uint8_t *adv_addr;
memcpy(&pdu_adv->adv_ind.addr[0], tx_addr,
BDADDR_SIZE);
memcpy(&pdu_scan->scan_rsp.addr[0], tx_addr,
BDADDR_SIZE);
}
adv_addr = ull_adv_pdu_update_addrs(adv, pdu_adv_to_update);
/* In case the local IRK was not set or no match was
* found the fallback address was used instead, check
* that a valid address has been set.
*/
if (pdu_adv->tx_addr &&
!mem_nz(pdu_adv->adv_ind.addr, BDADDR_SIZE)) {
if (pdu_adv_to_update->tx_addr &&
!mem_nz((void *)adv_addr, BDADDR_SIZE)) {
return BT_HCI_ERR_INVALID_PARAM;
}
#if defined(CONFIG_BT_CTLR_ADV_EXT)
/* Do not update scan response for extended non-scannable since
* there may be no scan response set.
*/
if ((pdu_adv->type != PDU_ADV_TYPE_EXT_IND) ||
(pdu_adv->adv_ext_ind.adv_mode & BT_HCI_LE_ADV_PROP_SCAN)) {
#else
if (1) {
#endif
ull_adv_pdu_update_addrs(adv, pdu_scan);
}
}
#if defined(CONFIG_BT_HCI_MESH_EXT)
@ -1555,6 +1540,31 @@ void ull_adv_done(struct node_rx_event_done *done)
}
#endif /* CONFIG_BT_CTLR_ADV_EXT */
const uint8_t *ull_adv_pdu_update_addrs(struct ll_adv_set *adv,
struct pdu_adv *pdu)
{
#if defined(CONFIG_BT_CTLR_ADV_EXT)
struct pdu_adv_hdr *hdr = (void *)pdu->adv_ext_ind.ext_hdr_adi_adv_data;
#endif
const uint8_t *adv_addr;
adv_addr = adva_update(adv, pdu);
/* Update TargetA only if directed advertising PDU is supplied. Note
* that AUX_SCAN_REQ does not have TargetA flag set so it will be
* ignored here as expected.
*/
if ((pdu->type == PDU_ADV_TYPE_DIRECT_IND) ||
#if defined(CONFIG_BT_CTLR_ADV_EXT)
((pdu->type == PDU_ADV_TYPE_EXT_IND) && hdr->tgt_addr) ||
#endif
0) {
tgta_update(adv, pdu);
}
return adv_addr;
}
static int init_reset(void)
{
uint8_t handle;
@ -1940,3 +1950,65 @@ static inline uint8_t disable(uint8_t handle)
return 0;
}
static inline uint8_t *adv_pdu_adva_get(struct pdu_adv *pdu)
{
#if defined(CONFIG_BT_CTLR_ADV_EXT)
struct pdu_adv_hdr *hdr = (void *)pdu->adv_ext_ind.ext_hdr_adi_adv_data;
/* All extended PDUs have AdvA at the same offset in common header */
if (pdu->type == PDU_ADV_TYPE_EXT_IND) {
LL_ASSERT(hdr->adv_addr);
return &pdu->adv_ext_ind.ext_hdr_adi_adv_data[1];
}
#endif
/* All legacy PDUs have AdvA at the same offset */
return pdu->adv_ind.addr;
}
static const uint8_t *adva_update(struct ll_adv_set *adv, struct pdu_adv *pdu)
{
#if defined(CONFIG_BT_CTLR_PRIVACY)
const uint8_t *tx_addr = ull_filter_adva_get(adv);
#else
const uint8_t *tx_addr = NULL;
#endif
uint8_t *adv_addr;
if (tx_addr) {
pdu->tx_addr = 1;
#if defined(CONFIG_BT_CTLR_ADV_EXT)
} else if (ll_adv_cmds_is_ext() && pdu->tx_addr) {
tx_addr = ll_adv_aux_random_addr_get(adv, NULL);
#endif
} else {
tx_addr = ll_addr_get(pdu->tx_addr, NULL);
}
adv_addr = adv_pdu_adva_get(pdu);
memcpy(adv_addr, tx_addr, BDADDR_SIZE);
return adv_addr;
}
static void tgta_update(struct ll_adv_set *adv, struct pdu_adv *pdu)
{
#if defined(CONFIG_BT_CTLR_PRIVACY)
const uint8_t *rx_addr = NULL;
uint8_t *tgt_addr;
rx_addr = ull_filter_tgta_get(adv);
if (rx_addr) {
pdu->rx_addr = 1;
/* TargetA always follows AdvA in all PDUs */
tgt_addr = adv_pdu_adva_get(pdu) + BDADDR_SIZE;
memcpy(tgt_addr, rx_addr, BDADDR_SIZE);
}
#endif
/* NOTE: identity TargetA is set when configuring advertising set, no
* need to update if LL Privacy is not supported.
*/
}

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

@ -40,6 +40,9 @@ uint8_t ull_adv_data_set(struct ll_adv_set *adv, uint8_t len,
uint8_t ull_scan_rsp_set(struct ll_adv_set *adv, uint8_t len,
uint8_t const *const data);
/* Update AdvA and TgtA (if application) in advertising PDU */
const uint8_t *ull_adv_pdu_update_addrs(struct ll_adv_set *adv,
struct pdu_adv *pdu);
#if defined(CONFIG_BT_CTLR_ADV_EXT)

54
subsys/bluetooth/controller/ll_sw/ull_filter.c
View file

@ -576,42 +576,29 @@ void ull_filter_rpa_update(bool timeout)
}
#if defined(CONFIG_BT_BROADCASTER)
void ull_filter_adv_pdu_update(struct ll_adv_set *adv, struct pdu_adv *pdu)
const uint8_t *ull_filter_adva_get(struct ll_adv_set *adv)
{
uint8_t *adva = pdu->type == PDU_ADV_TYPE_SCAN_RSP ?
&pdu->scan_rsp.addr[0] :
&pdu->adv_ind.addr[0];
uint8_t idx = adv->lll.rl_idx;
/* AdvA */
if (idx < ARRAY_SIZE(rl) && rl[idx].lirk) {
LL_ASSERT(rl[idx].rpas_ready);
pdu->tx_addr = 1;
memcpy(adva, rl[idx].local_rpa->val, BDADDR_SIZE);
} else {
pdu->tx_addr = adv->own_addr_type & 0x1;
#if defined(CONFIG_BT_CTLR_ADV_EXT)
if (ll_adv_cmds_is_ext() && pdu->tx_addr) {
ll_adv_aux_random_addr_get(adv, adva);
} else
#endif /* CONFIG_BT_CTLR_ADV_EXT */
{
ll_addr_get(pdu->tx_addr, adva);
}
return rl[idx].local_rpa->val;
}
return NULL;
}
const uint8_t *ull_filter_tgta_get(struct ll_adv_set *adv)
{
uint8_t idx = adv->lll.rl_idx;
/* TargetA */
if (pdu->type == PDU_ADV_TYPE_DIRECT_IND) {
if (idx < ARRAY_SIZE(rl) && rl[idx].pirk) {
pdu->rx_addr = 1;
memcpy(&pdu->direct_ind.tgt_addr[0],
rl[idx].peer_rpa.val, BDADDR_SIZE);
} else {
pdu->rx_addr = adv->id_addr_type;
memcpy(&pdu->direct_ind.tgt_addr[0],
adv->id_addr, BDADDR_SIZE);
}
if (idx < ARRAY_SIZE(rl) && rl[idx].pirk) {
return rl[idx].peer_rpa.val;
}
return NULL;
}
#endif /* CONFIG_BT_BROADCASTER */
@ -986,20 +973,9 @@ static void rpa_adv_refresh(struct ll_adv_set *adv)
prev = lll_adv_data_peek(&adv->lll);
pdu = lll_adv_data_alloc(&adv->lll, &idx);
pdu->type = prev->type;
pdu->rfu = 0;
if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2)) {
pdu->chan_sel = prev->chan_sel;
} else {
pdu->chan_sel = 0;
}
ull_filter_adv_pdu_update(adv, pdu);
memcpy(&pdu->adv_ind.data[0], &prev->adv_ind.data[0],
prev->len - BDADDR_SIZE);
pdu->len = prev->len;
memcpy(pdu, prev, PDU_AC_LL_HEADER_SIZE + pdu->len);
ull_adv_pdu_update_addrs(adv, pdu);
lll_adv_data_enqueue(&adv->lll, idx);
}

3
subsys/bluetooth/controller/ll_sw/ull_filter.h
View file

@ -8,7 +8,8 @@ void ull_filter_adv_scan_state_cb(uint8_t bm);
void ull_filter_adv_update(uint8_t adv_fp);
void ull_filter_scan_update(uint8_t scan_fp);
void ull_filter_rpa_update(bool timeout);
void ull_filter_adv_pdu_update(struct ll_adv_set *adv, struct pdu_adv *pdu);
uint8_t ull_filter_rl_find(uint8_t id_addr_type, uint8_t const *const id_addr,
uint8_t *const free);
void ull_filter_reset(bool init);
const uint8_t *ull_filter_adva_get(struct ll_adv_set *adv);
const uint8_t *ull_filter_tgta_get(struct ll_adv_set *adv);