michaelh/zephyr
1
0
Fork 0
Code Releases Activity

Bluetooth: controller: split: Refactor auto variable names

Browse source 
Review rework updates, refactor auto variable names.

Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
This commit is contained in:
Vinayak Kariappa Chettimada 2020-05-18 19:16:05 +05:30 • committed by Carles Cufí
commit 205fe1825e
4 changed files with 418 additions and 386 deletions
Download patch file Download diff file Expand all files Collapse all files

38
subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c
View file

@ -88,48 +88,48 @@ static int prepare_cb(struct lll_prepare_param *prepare_param)
{ {
struct lll_adv_aux *lll = prepare_param->param; struct lll_adv_aux *lll = prepare_param->param;
uint32_t aa = sys_cpu_to_le32(PDU_AC_ACCESS_ADDR); uint32_t aa = sys_cpu_to_le32(PDU_AC_ACCESS_ADDR);
struct pdu_adv_com_ext_adv *pri_com_hdr;
uint32_t ticks_at_event, ticks_at_start; uint32_t ticks_at_event, ticks_at_start;
struct pdu_adv_com_ext_adv *p; struct pdu_adv *pri_pdu, *sec_pdu;
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux_ptr;
struct pdu_adv *pri, *sec; struct pdu_adv_hdr *pri_hdr;
struct lll_adv *lll_adv; struct lll_adv *lll_adv;
struct pdu_adv_hdr *h;
struct evt_hdr *evt; struct evt_hdr *evt;
uint32_t remainder; uint32_t remainder;
uint32_t start_us; uint32_t start_us;
uint8_t *pri_dptr;
uint8_t phy_s; uint8_t phy_s;
uint8_t *ptr;
uint8_t upd; uint8_t upd;
DEBUG_RADIO_START_A(1); DEBUG_RADIO_START_A(1);
/* FIXME: get latest only when primary PDU without Aux PDUs */ /* FIXME: get latest only when primary PDU without Aux PDUs */
sec = lll_adv_aux_data_latest_get(lll, &upd); sec_pdu = lll_adv_aux_data_latest_get(lll, &upd);
/* Get reference to primary PDU */ /* Get reference to primary PDU */
lll_adv = lll->adv; lll_adv = lll->adv;
pri = lll_adv_data_curr_get(lll_adv); pri_pdu = lll_adv_data_curr_get(lll_adv);
LL_ASSERT(pri->type == PDU_ADV_TYPE_EXT_IND); LL_ASSERT(pri_pdu->type == PDU_ADV_TYPE_EXT_IND);
/* Get reference to extended header */ /* Get reference to extended header */
p = (void *)&pri->adv_ext_ind; pri_com_hdr = (void *)&pri_pdu->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h); pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* traverse through adv_addr, if present */ /* traverse through adv_addr, if present */
if (h->adv_addr) { if (pri_hdr->adv_addr) {
ptr += BDADDR_SIZE; pri_dptr += BDADDR_SIZE;
} }
/* traverse through adi, if present */ /* traverse through adi, if present */
if (h->adi) { if (pri_hdr->adi) {
ptr += sizeof(struct pdu_adv_adi); pri_dptr += sizeof(struct pdu_adv_adi);
} }
aux = (void *)ptr; aux_ptr = (void *)pri_dptr;
/* Abort if no aux_ptr filled */ /* Abort if no aux_ptr filled */
if (!h->aux_ptr || !aux->offs) { if (!pri_hdr->aux_ptr || !aux_ptr->offs) {
radio_isr_set(lll_isr_abort, lll); radio_isr_set(lll_isr_abort, lll);
radio_disable(); radio_disable();
} }
@ -159,10 +159,10 @@ static int prepare_cb(struct lll_prepare_param *prepare_param)
#endif /* !BT_CTLR_ADV_EXT_PBACK */ #endif /* !BT_CTLR_ADV_EXT_PBACK */
/* Use channel idx in aux_ptr */ /* Use channel idx in aux_ptr */
lll_chan_set(aux->chan_idx); lll_chan_set(aux_ptr->chan_idx);
/* Set the Radio Tx Packet */ /* Set the Radio Tx Packet */
radio_pkt_tx_set(sec); radio_pkt_tx_set(sec_pdu);
/* TODO: Based on adv_mode switch to Rx, if needed */ /* TODO: Based on adv_mode switch to Rx, if needed */
radio_isr_set(lll_isr_done, lll); radio_isr_set(lll_isr_done, lll);

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

@ -257,41 +257,41 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
#if defined(CONFIG_BT_CTLR_ADV_EXT) #if defined(CONFIG_BT_CTLR_ADV_EXT)
} else if (pdu->type == PDU_ADV_TYPE_EXT_IND) { } else if (pdu->type == PDU_ADV_TYPE_EXT_IND) {
struct pdu_adv_com_ext_adv *p; struct pdu_adv_hdr *pri_hdr, pri_hdr_prev;
struct pdu_adv_hdr *h, _h; struct pdu_adv_com_ext_adv *pri_com_hdr;
uint8_t *_ptr, *ptr; uint8_t *pri_dptr_prev, *pri_dptr;
uint8_t len; uint8_t len;
p = (void *)&pdu->adv_ext_ind; pri_com_hdr = (void *)&pdu->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h); pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
_ptr = ptr; pri_dptr_prev = pri_dptr;
/* No ACAD and no AdvData */ /* No ACAD and no AdvData */
p->adv_mode = evt_prop & 0x03; pri_com_hdr->adv_mode = evt_prop & 0x03;
/* Zero-init header flags */ /* Zero-init header flags */
if (is_pdu_type_changed) { if (is_pdu_type_changed) {
*(uint8_t *)&_h = 0; *(uint8_t *)&pri_hdr_prev = 0;
} else { } else {
*(uint8_t *)&_h = *(uint8_t *)h; *(uint8_t *)&pri_hdr_prev = *(uint8_t *)pri_hdr;
} }
*(uint8_t *)h = 0; *(uint8_t *)pri_hdr = 0;
/* AdvA flag */ /* AdvA flag */
if (_h.adv_addr) { if (pri_hdr_prev.adv_addr) {
_ptr += BDADDR_SIZE; pri_dptr_prev += BDADDR_SIZE;
} }
if (!p->adv_mode && if (!pri_com_hdr->adv_mode &&
(!_h.aux_ptr || (!pri_hdr_prev.aux_ptr ||
(!(evt_prop & BT_HCI_LE_ADV_PROP_ANON) && (!(evt_prop & BT_HCI_LE_ADV_PROP_ANON) &&
(phy_p != PHY_CODED)))) { (phy_p != PHY_CODED)))) {
/* TODO: optional on 1M with Aux Ptr */ /* TODO: optional on 1M with Aux Ptr */
h->adv_addr = 1; pri_hdr->adv_addr = 1;
/* NOTE: AdvA is filled at enable */ /* NOTE: AdvA is filled at enable */
pdu->tx_addr = own_addr_type & 0x1; pdu->tx_addr = own_addr_type & 0x1;
ptr += BDADDR_SIZE; pri_dptr += BDADDR_SIZE;
} else { } else {
pdu->tx_addr = 0; pdu->tx_addr = 0;
} }
@ -303,16 +303,16 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
/* No CTEInfo flag in primary channel PDU */ /* No CTEInfo flag in primary channel PDU */
/* ADI flag */ /* ADI flag */
if (_h.adi) { if (pri_hdr_prev.adi) {
h->adi = 1; pri_hdr->adi = 1;
ptr += sizeof(struct pdu_adv_adi); pri_dptr += sizeof(struct pdu_adv_adi);
} }
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0) #if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
/* AuxPtr flag */ /* AuxPtr flag */
if (_h.aux_ptr) { if (pri_hdr_prev.aux_ptr) {
h->aux_ptr = 1; pri_hdr->aux_ptr = 1;
ptr += sizeof(struct pdu_adv_aux_ptr); pri_dptr += sizeof(struct pdu_adv_aux_ptr);
} }
#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */ #endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
@ -323,21 +323,21 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
* for future use on the LE Coded PHY. * for future use on the LE Coded PHY.
*/ */
if ((evt_prop & BT_HCI_LE_ADV_PROP_TX_POWER) && if ((evt_prop & BT_HCI_LE_ADV_PROP_TX_POWER) &&
(!_h.aux_ptr || (phy_p != PHY_CODED))) { (!pri_hdr_prev.aux_ptr || (phy_p != PHY_CODED))) {
h->tx_pwr = 1; pri_hdr->tx_pwr = 1;
ptr++; pri_dptr++;
} }
/* Calc primary PDU len */ /* Calc primary PDU len */
len = ptr - (uint8_t *)p; len = pri_dptr - (uint8_t *)pri_com_hdr;
if (len > (offsetof(struct pdu_adv_com_ext_adv, if (len > (offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data) + sizeof(*h))) { ext_hdr_adi_adv_data) + sizeof(*pri_hdr))) {
p->ext_hdr_len = len - pri_com_hdr->ext_hdr_len = len -
offsetof(struct pdu_adv_com_ext_adv, offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
pdu->len = len; pdu->len = len;
} else { } else {
p->ext_hdr_len = 0; pri_com_hdr->ext_hdr_len = 0;
pdu->len = offsetof(struct pdu_adv_com_ext_adv, pdu->len = offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
} }
@ -349,7 +349,7 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
/* No ACAD in primary channel PDU */ /* No ACAD in primary channel PDU */
/* Tx Power */ /* Tx Power */
if (h->tx_pwr) { if (pri_hdr->tx_pwr) {
uint8_t _tx_pwr; uint8_t _tx_pwr;
_tx_pwr = 0; _tx_pwr = 0;
@ -361,40 +361,43 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
} }
} }
*--ptr = _tx_pwr; *--pri_dptr = _tx_pwr;
} }
/* No SyncInfo in primary channel PDU */ /* No SyncInfo in primary channel PDU */
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0) #if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
/* AuxPtr */ /* AuxPtr */
if (h->aux_ptr) { if (pri_hdr->aux_ptr) {
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux_ptr;
ptr -= sizeof(struct pdu_adv_aux_ptr); pri_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Channel Index, CA, Offset Units and AUX Offset /* NOTE: Aux Offset will be set in advertiser LLL event
* will be set in Advertiser Event.
*/ */
aux = (void *)ptr; aux_ptr = (void *)pri_dptr;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */ /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */ aux_ptr->chan_idx = 0;
aux->phy = find_lsb_set(phy_s) - 1; aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(phy_s) - 1;
} }
adv->lll.phy_s = phy_s; adv->lll.phy_s = phy_s;
#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */ #endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
/* ADI */ /* ADI */
if (h->adi) { if (pri_hdr->adi) {
struct pdu_adv_adi *adi; struct pdu_adv_adi *adi;
ptr -= sizeof(struct pdu_adv_adi); pri_dptr -= sizeof(struct pdu_adv_adi);
/* NOTE: memcpy shall handle overlapping buffers */ /* NOTE: memcpy shall handle overlapping buffers */
memcpy(ptr, _ptr, sizeof(struct pdu_adv_adi)); memcpy(pri_dptr, pri_dptr_prev,
sizeof(struct pdu_adv_adi));
adi = (void *)ptr; adi = (void *)pri_dptr;
adi->sid = sid; adi->sid = sid;
} }
adv->sid = sid; adv->sid = sid;
@ -519,16 +522,16 @@ uint8_t ll_adv_enable(uint8_t enable)
#if defined(CONFIG_BT_CTLR_ADV_EXT) #if defined(CONFIG_BT_CTLR_ADV_EXT)
} else if (pdu_adv->type == PDU_ADV_TYPE_EXT_IND) { } else if (pdu_adv->type == PDU_ADV_TYPE_EXT_IND) {
struct pdu_adv_com_ext_adv *p; struct pdu_adv_com_ext_adv *pri_com_hdr;
struct pdu_adv_hdr *h; struct pdu_adv_hdr *pri_hdr;
uint8_t *ptr; uint8_t *pri_dptr;
p = (void *)&pdu_adv->adv_ext_ind; pri_com_hdr = (void *)&pdu_adv->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h); pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* AdvA, fill here at enable */ /* AdvA, fill here at enable */
if (h->adv_addr) { if (pri_hdr->adv_addr) {
uint8_t const *tx_addr = uint8_t const *tx_addr =
ll_adv_aux_random_addr_get(adv, NULL); ll_adv_aux_random_addr_get(adv, NULL);
@ -539,35 +542,35 @@ uint8_t ll_adv_enable(uint8_t enable)
return BT_HCI_ERR_INVALID_PARAM; return BT_HCI_ERR_INVALID_PARAM;
} }
memcpy(ptr, tx_addr, BDADDR_SIZE); memcpy(pri_dptr, tx_addr, BDADDR_SIZE);
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0) #if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
} else if (h->aux_ptr) { } else if (pri_hdr->aux_ptr) {
struct pdu_adv_com_ext_adv *s; struct pdu_adv_com_ext_adv *sec_com_hdr;
struct pdu_adv *pdu_aux; struct pdu_adv_hdr *sec_hdr;
struct pdu_adv_hdr *hs; struct pdu_adv *sec_pdu;
uint8_t *ps; uint8_t *sec_dptr;
pdu_aux = lll_adv_aux_data_peek(lll->aux); sec_pdu = lll_adv_aux_data_peek(lll->aux);
s = (void *)&pdu_aux->adv_ext_ind; sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
hs = (void *)s->ext_hdr_adi_adv_data; sec_hdr = (void *)sec_com_hdr->ext_hdr_adi_adv_data;
ps = (uint8_t *)hs + sizeof(*hs); sec_dptr = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
if (hs->adv_addr) { if (sec_hdr->adv_addr) {
uint8_t const *tx_addr = uint8_t const *tx_addr =
ll_adv_aux_random_addr_get(adv, NULL); ll_adv_aux_random_addr_get(adv, NULL);
/* TODO: Privacy */ /* TODO: Privacy */
if (pdu_aux->tx_addr && if (sec_pdu->tx_addr &&
!mem_nz((void *)tx_addr, BDADDR_SIZE)) { !mem_nz((void *)tx_addr, BDADDR_SIZE)) {
return BT_HCI_ERR_INVALID_PARAM; return BT_HCI_ERR_INVALID_PARAM;
} }
memcpy(ps, tx_addr, BDADDR_SIZE); memcpy(sec_dptr, tx_addr, BDADDR_SIZE);
} }
if (h->sync_info) { if (pri_hdr->sync_info) {
/* TODO: allocate periodic advertising context /* TODO: allocate periodic advertising context
*/ */
} }

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

@ -83,16 +83,19 @@ uint8_t const *ll_adv_aux_random_addr_get(struct ll_adv_set const *const adv,
uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, uint8_t len, uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, uint8_t len,
uint8_t const *const data) uint8_t const *const data)
{ {
struct pdu_adv_com_ext_adv *p, *_p, *s, *_s; struct pdu_adv *pri_pdu, *pri_pdu_prev, *sec_pdu, *sec_pdu_prev;
uint8_t pri_len, _pri_len, sec_len, _sec_len; struct pdu_adv_com_ext_adv *pri_com_hdr, *pri_com_hdr_prev;
struct pdu_adv *_pri, *pri, *_sec, *sec; struct pdu_adv_com_ext_adv *sec_com_hdr, *sec_com_hdr_prev;
struct pdu_adv_hdr *hp, _hp, *hs, _hs; uint8_t pri_len, pri_len_prev, sec_len, sec_len_prev;
struct pdu_adv_hdr *pri_hdr, pri_hdr_prev;
struct pdu_adv_hdr *sec_hdr, sec_hdr_prev;
uint8_t *pri_dptr, *pri_dptr_prev;
uint8_t *sec_dptr, *sec_dptr_prev;
struct lll_adv_aux *lll_aux; struct lll_adv_aux *lll_aux;
struct ll_adv_aux_set *aux; struct ll_adv_aux_set *aux;
uint8_t *_pp, *pp, *ps, *_ps;
struct ll_adv_set *adv; struct ll_adv_set *adv;
struct lll_adv *lll; struct lll_adv *lll;
uint8_t ip, is; uint8_t pri_idx, sec_idx;
/* op param definitions: /* op param definitions:
* 0x00 - Intermediate fragment of fragmented extended advertising data * 0x00 - Intermediate fragment of fragmented extended advertising data
@ -121,8 +124,8 @@ uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
/* Do not use Common Extended Advertising Header Format if not extended /* Do not use Common Extended Advertising Header Format if not extended
* advertising. * advertising.
*/ */
_pri = lll_adv_data_peek(lll); pri_pdu_prev = lll_adv_data_peek(lll);
if (_pri->type != PDU_ADV_TYPE_EXT_IND) { if (pri_pdu_prev->type != PDU_ADV_TYPE_EXT_IND) {
return ull_adv_data_set(adv, len, data); return ull_adv_data_set(adv, len, data);
} }
@ -147,160 +150,162 @@ uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
} }
/* Get reference to previous primary PDU data */ /* Get reference to previous primary PDU data */
_p = (void *)&_pri->adv_ext_ind; pri_com_hdr_prev = (void *)&pri_pdu_prev->adv_ext_ind;
hp = (void *)_p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr_prev->ext_hdr_adi_adv_data;
*(uint8_t *)&_hp = *(uint8_t *)hp; pri_hdr_prev = *pri_hdr;
_pp = (uint8_t *)hp + sizeof(*hp); pri_dptr_prev = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* Get reference to new primary PDU data buffer */ /* Get reference to new primary PDU data buffer */
pri = lll_adv_data_alloc(lll, &ip); pri_pdu = lll_adv_data_alloc(lll, &pri_idx);
pri->type = _pri->type; pri_pdu->type = pri_pdu_prev->type;
pri->rfu = 0U; pri_pdu->rfu = 0U;
pri->chan_sel = 0U; pri_pdu->chan_sel = 0U;
p = (void *)&pri->adv_ext_ind; pri_com_hdr = (void *)&pri_pdu->adv_ext_ind;
p->adv_mode = _p->adv_mode; pri_com_hdr->adv_mode = pri_com_hdr_prev->adv_mode;
hp = (void *)p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pp = (uint8_t *)hp + sizeof(*hp); pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
*(uint8_t *)hp = 0U; *(uint8_t *)pri_hdr = 0U;
/* Get reference to previous secondary PDU data */ /* Get reference to previous secondary PDU data */
_sec = lll_adv_aux_data_peek(lll_aux); sec_pdu_prev = lll_adv_aux_data_peek(lll_aux);
_s = (void *)&_sec->adv_ext_ind; sec_com_hdr_prev = (void *)&sec_pdu_prev->adv_ext_ind;
hs = (void *)_s->ext_hdr_adi_adv_data; sec_hdr = (void *)sec_com_hdr_prev->ext_hdr_adi_adv_data;
*(uint8_t *)&_hs = *(uint8_t *)hs; sec_hdr_prev = *sec_hdr;
_ps = (uint8_t *)hs + sizeof(*hs); sec_dptr_prev = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
/* Get reference to new secondary PDU data buffer */ /* Get reference to new secondary PDU data buffer */
sec = lll_adv_aux_data_alloc(lll_aux, &is); sec_pdu = lll_adv_aux_data_alloc(lll_aux, &sec_idx);
sec->type = pri->type; sec_pdu->type = pri_pdu->type;
sec->rfu = 0U; sec_pdu->rfu = 0U;
if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2)) { if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2)) {
sec->chan_sel = _sec->chan_sel; sec_pdu->chan_sel = sec_pdu_prev->chan_sel;
} else { } else {
sec->chan_sel = 0U; sec_pdu->chan_sel = 0U;
} }
sec->tx_addr = _sec->tx_addr; sec_pdu->tx_addr = sec_pdu_prev->tx_addr;
sec->rx_addr = _sec->rx_addr; sec_pdu->rx_addr = sec_pdu_prev->rx_addr;
s = (void *)&sec->adv_ext_ind; sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
s->adv_mode = p->adv_mode; sec_com_hdr->adv_mode = pri_com_hdr->adv_mode;
hs = (void *)s->ext_hdr_adi_adv_data; sec_hdr = (void *)sec_com_hdr->ext_hdr_adi_adv_data;
ps = (uint8_t *)hs + sizeof(*hs); sec_dptr = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
*(uint8_t *)hs = 0U; *(uint8_t *)sec_hdr = 0U;
/* AdvA flag */ /* AdvA flag */
/* NOTE: as we will use auxiliary packet, we remove AdvA in primary /* NOTE: as we will use auxiliary packet, we remove AdvA in primary
* channel. i.e. Do nothing to add AdvA in the primary PDU. * channel. i.e. Do nothing to add AdvA in the primary PDU.
*/ */
if (_hp.adv_addr) { if (pri_hdr_prev.adv_addr) {
_pp += BDADDR_SIZE; pri_dptr_prev += BDADDR_SIZE;
/* Prepare to add AdvA in secondary PDU */ /* Prepare to add AdvA in secondary PDU */
hs->adv_addr = 1; sec_hdr->adv_addr = 1;
/* NOTE: AdvA is filled at enable */ /* NOTE: AdvA is filled at enable */
sec->tx_addr = _pri->tx_addr; sec_pdu->tx_addr = pri_pdu_prev->tx_addr;
} }
pri->tx_addr = 0U; pri_pdu->tx_addr = 0U;
pri->rx_addr = 0U; pri_pdu->rx_addr = 0U;
if (_hs.adv_addr) { if (sec_hdr_prev.adv_addr) {
_ps += BDADDR_SIZE; sec_dptr_prev += BDADDR_SIZE;
/* Prepare to copy AdvA from previous double buffered PDU */ /* Prepare to copy AdvA from previous double buffered PDU */
hs->adv_addr = 1; sec_hdr->adv_addr = 1;
} }
if (hs->adv_addr) { if (sec_hdr->adv_addr) {
ps += BDADDR_SIZE; sec_dptr += BDADDR_SIZE;
} }
/* No TargetA in primary and secondary channel for undirected */ /* No TargetA in primary and secondary channel for undirected */
/* No CTEInfo flag in primary and secondary channel PDU */ /* No CTEInfo flag in primary and secondary channel PDU */
/* ADI flag */ /* ADI flag */
if (_hp.adi) { if (pri_hdr_prev.adi) {
_pp += sizeof(struct pdu_adv_adi); pri_dptr_prev += sizeof(struct pdu_adv_adi);
} }
hp->adi = 1; pri_hdr->adi = 1;
pp += sizeof(struct pdu_adv_adi); pri_dptr += sizeof(struct pdu_adv_adi);
if (_hs.adi) { if (sec_hdr_prev.adi) {
_ps += sizeof(struct pdu_adv_adi); sec_dptr_prev += sizeof(struct pdu_adv_adi);
} }
hs->adi = 1; sec_hdr->adi = 1;
ps += sizeof(struct pdu_adv_adi); sec_dptr += sizeof(struct pdu_adv_adi);
/* AuxPtr flag */ /* AuxPtr flag */
if (_hp.aux_ptr) { if (pri_hdr_prev.aux_ptr) {
_pp += sizeof(struct pdu_adv_aux_ptr); pri_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
} }
hp->aux_ptr = 1; pri_hdr->aux_ptr = 1;
pp += sizeof(struct pdu_adv_aux_ptr); pri_dptr += sizeof(struct pdu_adv_aux_ptr);
if (_hs.aux_ptr) { if (sec_hdr_prev.aux_ptr) {
_ps += sizeof(struct pdu_adv_aux_ptr); sec_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
hs->aux_ptr = 1; sec_hdr->aux_ptr = 1;
ps += sizeof(struct pdu_adv_aux_ptr); sec_dptr += sizeof(struct pdu_adv_aux_ptr);
} }
/* No SyncInfo flag in primary channel PDU */ /* No SyncInfo flag in primary channel PDU */
/* SyncInfo flag in secondary channel PDU */ /* SyncInfo flag in secondary channel PDU */
if (_hs.sync_info) { if (sec_hdr_prev.sync_info) {
_ps += sizeof(struct pdu_adv_sync_info); sec_dptr_prev += sizeof(struct pdu_adv_sync_info);
hs->sync_info = 1; sec_hdr->sync_info = 1;
ps += sizeof(struct pdu_adv_sync_info); sec_dptr += sizeof(struct pdu_adv_sync_info);
} }
/* Tx Power flag */ /* Tx Power flag */
if (_hp.tx_pwr) { if (pri_hdr_prev.tx_pwr) {
_pp++; pri_dptr_prev++;
/* C1, Tx Power is optional on the LE 1M PHY, and reserved for /* C1, Tx Power is optional on the LE 1M PHY, and reserved for
* for future use on the LE Coded PHY. * for future use on the LE Coded PHY.
*/ */
if (lll->phy_p != PHY_CODED) { if (lll->phy_p != PHY_CODED) {
hp->tx_pwr = 1; pri_hdr->tx_pwr = 1;
pp++; pri_dptr++;
} else { } else {
hs->tx_pwr = 1; sec_hdr->tx_pwr = 1;
} }
} }
if (_hs.tx_pwr) { if (sec_hdr_prev.tx_pwr) {
_ps++; sec_dptr_prev++;
hs->tx_pwr = 1; sec_hdr->tx_pwr = 1;
} }
if (hs->tx_pwr) { if (sec_hdr->tx_pwr) {
ps++; sec_dptr++;
} }
/* No ACAD in Primary channel PDU */ /* No ACAD in Primary channel PDU */
/* TODO: ACAD in Secondary channel PDU */ /* TODO: ACAD in Secondary channel PDU */
/* Calc primary PDU len */ /* Calc primary PDU len */
_pri_len = _pp - (uint8_t *)_p; pri_len_prev = pri_dptr_prev - (uint8_t *)pri_com_hdr_prev;
pri_len = pp - (uint8_t *)p; pri_len = pri_dptr - (uint8_t *)pri_com_hdr;
p->ext_hdr_len = pri_len - offsetof(struct pdu_adv_com_ext_adv, pri_com_hdr->ext_hdr_len = pri_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
/* set the primary PDU len */ /* set the primary PDU len */
pri->len = pri_len; pri_pdu->len = pri_len;
/* Calc secondary PDU len */ /* Calc secondary PDU len */
_sec_len = _ps - (uint8_t *)_s; sec_len_prev = sec_dptr_prev - (uint8_t *)sec_com_hdr_prev;
sec_len = ps - (uint8_t *)s; sec_len = sec_dptr - (uint8_t *)sec_com_hdr;
s->ext_hdr_len = sec_len - offsetof(struct pdu_adv_com_ext_adv, sec_com_hdr->ext_hdr_len = sec_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
/* TODO: Check AdvData overflow */ /* TODO: Check AdvData overflow */
/* Fill AdvData in secondary PDU */ /* Fill AdvData in secondary PDU */
memcpy(ps, data, len); memcpy(sec_dptr, data, len);
/* set the secondary PDU len */ /* set the secondary PDU len */
sec->len = sec_len + len; sec_pdu->len = sec_len + len;
/* Start filling primary PDU extended header based on flags */ /* Start filling primary PDU extended header based on flags */
@ -309,86 +314,95 @@ uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
/* No ACAD in primary channel PDU */ /* No ACAD in primary channel PDU */
/* Tx Power */ /* Tx Power */
if (hp->tx_pwr) { if (pri_hdr->tx_pwr) {
*--pp = *--_pp; *--pri_dptr = *--pri_dptr_prev;
} else if (hs->tx_pwr) { } else if (sec_hdr->tx_pwr) {
*--ps = *--_ps; *--sec_dptr = *--sec_dptr_prev;
} }
/* No SyncInfo in primary channel PDU */ /* No SyncInfo in primary channel PDU */
/* SyncInfo in secondary channel PDU */ /* SyncInfo in secondary channel PDU */
if (hs->sync_info) { if (sec_hdr->sync_info) {
_ps -= sizeof(struct pdu_adv_sync_info); sec_dptr_prev -= sizeof(struct pdu_adv_sync_info);
ps -= sizeof(struct pdu_adv_sync_info); sec_dptr -= sizeof(struct pdu_adv_sync_info);
memcpy(ps, _ps, sizeof(struct pdu_adv_sync_info)); memcpy(sec_dptr, sec_dptr_prev,
sizeof(struct pdu_adv_sync_info));
} }
/* AuxPtr */ /* AuxPtr */
if (_hp.aux_ptr) { if (pri_hdr_prev.aux_ptr) {
_pp -= sizeof(struct pdu_adv_aux_ptr); pri_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
} }
{ {
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux_ptr;
pp -= sizeof(struct pdu_adv_aux_ptr); pri_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Aux Offset will be set in advertiser LLL event */ /* NOTE: Aux Offset will be set in advertiser LLL event */
aux = (void *)pp; aux_ptr = (void *)pri_dptr;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */ /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */ aux_ptr->chan_idx = 0;
aux->phy = find_lsb_set(lll->phy_s) - 1; aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(lll->phy_s) - 1;
} }
if (_hs.aux_ptr) { if (sec_hdr_prev.aux_ptr) {
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux_ptr;
_ps -= sizeof(struct pdu_adv_aux_ptr); sec_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
ps -= sizeof(struct pdu_adv_aux_ptr); sec_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Aux Offset will be set in advertiser LLL event */ /* NOTE: Aux Offset will be set in advertiser LLL event */
aux = (void *)ps; aux_ptr = (void *)sec_dptr;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */ /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */ aux_ptr->chan_idx = 0;
aux->phy = find_lsb_set(lll->phy_s) - 1; aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(lll->phy_s) - 1;
} }
/* ADI */ /* ADI */
{ {
struct pdu_adv_adi *ap, *as; struct pdu_adv_adi *pri_adi, *sec_adi;
uint16_t did = UINT16_MAX; uint16_t did = UINT16_MAX;
pp -= sizeof(struct pdu_adv_adi); pri_dptr -= sizeof(struct pdu_adv_adi);
ps -= sizeof(struct pdu_adv_adi); sec_dptr -= sizeof(struct pdu_adv_adi);
ap = (void *)pp; pri_adi = (void *)pri_dptr;
as = (void *)ps; sec_adi = (void *)sec_dptr;
if (_hp.adi) { if (pri_hdr_prev.adi) {
struct pdu_adv_adi *_adi; struct pdu_adv_adi *pri_adi_prev;
_pp -= sizeof(struct pdu_adv_adi); pri_dptr_prev -= sizeof(struct pdu_adv_adi);
_ps -= sizeof(struct pdu_adv_adi); sec_dptr_prev -= sizeof(struct pdu_adv_adi);
/* NOTE: memcpy shall handle overlapping buffers */ /* NOTE: memcpy shall handle overlapping buffers */
memcpy(pp, _pp, sizeof(struct pdu_adv_adi)); memcpy(pri_dptr, pri_dptr_prev,
memcpy(ps, _ps, sizeof(struct pdu_adv_adi)); sizeof(struct pdu_adv_adi));
memcpy(sec_dptr, sec_dptr_prev,
sizeof(struct pdu_adv_adi));
_adi = (void *)_pp; pri_adi_prev = (void *)pri_dptr_prev;
did = sys_le16_to_cpu(_adi->did); did = sys_le16_to_cpu(pri_adi_prev->did);
} else { } else {
ap->sid = adv->sid; pri_adi->sid = adv->sid;
as->sid = adv->sid; sec_adi->sid = adv->sid;
} }
if ((op == 0x04) || len || (_pri_len != pri_len) || if ((op == 0x04) || len || (pri_len_prev != pri_len) ||
(_sec->len != sec->len)) { (sec_pdu_prev->len != sec_pdu->len)) {
did++; did++;
} }
ap->did = sys_cpu_to_le16(did); pri_adi->did = sys_cpu_to_le16(did);
as->did = sys_cpu_to_le16(did); sec_adi->did = sys_cpu_to_le16(did);
} }
/* No CTEInfo field in primary channel PDU */ /* No CTEInfo field in primary channel PDU */
@ -398,24 +412,24 @@ uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
/* No AdvA in primary channel due to AuxPtr being added */ /* No AdvA in primary channel due to AuxPtr being added */
/* NOTE: AdvA in aux channel is also filled at enable and RPA timeout */ /* NOTE: AdvA in aux channel is also filled at enable and RPA timeout */
if (hs->adv_addr) { if (sec_hdr->adv_addr) {
void *bdaddr; void *bdaddr;
if (_hs.adv_addr) { if (sec_hdr_prev.adv_addr) {
_ps -= BDADDR_SIZE; sec_dptr_prev -= BDADDR_SIZE;
bdaddr = _ps; bdaddr = sec_dptr_prev;
} else { } else {
_pp -= BDADDR_SIZE; pri_dptr_prev -= BDADDR_SIZE;
bdaddr = _pp; bdaddr = pri_dptr_prev;
} }
ps -= BDADDR_SIZE; sec_dptr -= BDADDR_SIZE;
memcpy(ps, bdaddr, BDADDR_SIZE); memcpy(sec_dptr, bdaddr, BDADDR_SIZE);
} }
lll_adv_aux_data_enqueue(lll_aux, is); lll_adv_aux_data_enqueue(lll_aux, sec_idx);
lll_adv_data_enqueue(lll, ip); lll_adv_data_enqueue(lll, pri_idx);
if (adv->is_enabled && !aux->is_started) { if (adv->is_enabled && !aux->is_started) {
volatile uint32_t ret_cb; volatile uint32_t ret_cb;
@ -448,7 +462,7 @@ uint8_t ll_adv_aux_sr_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
uint8_t const *const data) uint8_t const *const data)
{ {
struct ll_adv_set *adv; struct ll_adv_set *adv;
struct pdu_adv *_pri; struct pdu_adv *pri_pdu_prev;
struct lll_adv *lll; struct lll_adv *lll;
/* op param definitions: /* op param definitions:
@ -477,8 +491,8 @@ uint8_t ll_adv_aux_sr_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
/* Do not use Common Extended Advertising Header Format if not extended /* Do not use Common Extended Advertising Header Format if not extended
* advertising. * advertising.
*/ */
_pri = lll_adv_data_peek(lll); pri_pdu_prev = lll_adv_data_peek(lll);
if (_pri->type != PDU_ADV_TYPE_EXT_IND) { if (pri_pdu_prev->type != PDU_ADV_TYPE_EXT_IND) {
return ull_scan_rsp_set(adv, len, data); return ull_scan_rsp_set(adv, len, data);
} }
@ -644,13 +658,13 @@ struct pdu_adv_aux_ptr *ull_adv_aux_lll_offset_fill(uint32_t ticks_offset,
uint32_t start_us, uint32_t start_us,
struct pdu_adv *pdu) struct pdu_adv *pdu)
{ {
struct pdu_adv_com_ext_adv *p; struct pdu_adv_com_ext_adv *pri_com_hdr;
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux;
struct pdu_adv_hdr *h; struct pdu_adv_hdr *h;
uint8_t *ptr; uint8_t *ptr;
p = (void *)&pdu->adv_ext_ind; pri_com_hdr = (void *)&pdu->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data; h = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h); ptr = (uint8_t *)h + sizeof(*h);
if (h->adv_addr) { if (h->adv_addr) {

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

@ -52,13 +52,13 @@ static void *adv_sync_free;
uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
{ {
struct pdu_adv_hdr *ter_hdr, ter_hdr_prev;
struct pdu_adv_com_ext_adv *ter_com_hdr;
uint8_t *ter_dptr_prev, *ter_dptr;
struct lll_adv_sync *lll_sync; struct lll_adv_sync *lll_sync;
struct pdu_adv_com_ext_adv *t;
struct ll_adv_sync_set *sync; struct ll_adv_sync_set *sync;
struct pdu_adv_hdr *ht, _ht;
struct ll_adv_set *adv; struct ll_adv_set *adv;
struct pdu_adv *pdu; struct pdu_adv *pdu;
uint8_t *_pt, *pt;
uint8_t ter_len; uint8_t ter_len;
adv = ull_adv_is_created_get(handle); adv = ull_adv_is_created_get(handle);
@ -68,14 +68,18 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
lll_sync = adv->lll.sync; lll_sync = adv->lll.sync;
if (!lll_sync) { if (!lll_sync) {
struct pdu_adv_com_ext_adv *p, *_p, *s, *_s; struct pdu_adv_com_ext_adv *pri_com_hdr, *pri_com_hdr_prev;
uint8_t pri_len, _pri_len, sec_len, _sec_len; struct pdu_adv_com_ext_adv *sec_com_hdr, *sec_com_hdr_prev;
struct pdu_adv *_pri, *pri, *_sec, *sec; uint8_t pri_len, pri_len_prev, sec_len, sec_len_prev;
struct pdu_adv_hdr *hp, _hp, *hs, _hs; struct pdu_adv_hdr *pri_hdr, pri_hdr_prev;
struct pdu_adv_hdr *sec_hdr, sec_hdr_prev;
struct pdu_adv *pri_pdu, *pri_pdu_prev;
struct pdu_adv *sec_pdu_prev, *sec_pdu;
uint8_t *pri_dptr, *pri_dptr_prev;
uint8_t *sec_dptr, *sec_dptr_prev;
struct pdu_adv_sync_info *si; struct pdu_adv_sync_info *si;
struct lll_adv_aux *lll_aux; struct lll_adv_aux *lll_aux;
uint8_t *_pp, *pp, *ps, *_ps; uint8_t pri_idx, sec_idx, ad_len;
uint8_t ip, is, ad_len;
struct lll_adv *lll; struct lll_adv *lll;
int err; int err;
@ -110,156 +114,158 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
lll_sync->data_chan_id = 0; lll_sync->data_chan_id = 0;
/* Get reference to previous primary PDU data */ /* Get reference to previous primary PDU data */
_pri = lll_adv_data_peek(lll); pri_pdu_prev = lll_adv_data_peek(lll);
_p = (void *)&_pri->adv_ext_ind; pri_com_hdr_prev = (void *)&pri_pdu_prev->adv_ext_ind;
hp = (void *)_p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr_prev->ext_hdr_adi_adv_data;
*(uint8_t *)&_hp = *(uint8_t *)hp; pri_hdr_prev = *pri_hdr;
_pp = (uint8_t *)hp + sizeof(*hp); pri_dptr_prev = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* Get reference to new primary PDU data buffer */ /* Get reference to new primary PDU data buffer */
pri = lll_adv_data_alloc(lll, &ip); pri_pdu = lll_adv_data_alloc(lll, &pri_idx);
pri->type = _pri->type; pri_pdu->type = pri_pdu_prev->type;
pri->rfu = 0U; pri_pdu->rfu = 0U;
pri->chan_sel = 0U; pri_pdu->chan_sel = 0U;
p = (void *)&pri->adv_ext_ind; pri_com_hdr = (void *)&pri_pdu->adv_ext_ind;
p->adv_mode = _p->adv_mode; pri_com_hdr->adv_mode = pri_com_hdr_prev->adv_mode;
hp = (void *)p->ext_hdr_adi_adv_data; pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pp = (uint8_t *)hp + sizeof(*hp); pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
*(uint8_t *)hp = 0U; *(uint8_t *)pri_hdr = 0U;
/* Get reference to previous secondary PDU data */ /* Get reference to previous secondary PDU data */
_sec = lll_adv_aux_data_peek(lll_aux); sec_pdu_prev = lll_adv_aux_data_peek(lll_aux);
_s = (void *)&_sec->adv_ext_ind; sec_com_hdr_prev = (void *)&sec_pdu_prev->adv_ext_ind;
hs = (void *)_s->ext_hdr_adi_adv_data; sec_hdr = (void *)sec_com_hdr_prev->ext_hdr_adi_adv_data;
*(uint8_t *)&_hs = *(uint8_t *)hs; sec_hdr_prev = *sec_hdr;
_ps = (uint8_t *)hs + sizeof(*hs); sec_dptr_prev = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
/* Get reference to new secondary PDU data buffer */ /* Get reference to new secondary PDU data buffer */
sec = lll_adv_aux_data_alloc(lll_aux, &is); sec_pdu = lll_adv_aux_data_alloc(lll_aux, &sec_idx);
sec->type = pri->type; sec_pdu->type = pri_pdu->type;
sec->rfu = 0U; sec_pdu->rfu = 0U;
if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2)) { if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2)) {
sec->chan_sel = _sec->chan_sel; sec_pdu->chan_sel = sec_pdu_prev->chan_sel;
} else { } else {
sec->chan_sel = 0U; sec_pdu->chan_sel = 0U;
} }
sec->tx_addr = _sec->tx_addr; sec_pdu->tx_addr = sec_pdu_prev->tx_addr;
sec->rx_addr = _sec->rx_addr; sec_pdu->rx_addr = sec_pdu_prev->rx_addr;
s = (void *)&sec->adv_ext_ind; sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
s->adv_mode = p->adv_mode; sec_com_hdr->adv_mode = pri_com_hdr->adv_mode;
hs = (void *)s->ext_hdr_adi_adv_data; sec_hdr = (void *)sec_com_hdr->ext_hdr_adi_adv_data;
ps = (uint8_t *)hs + sizeof(*hs); sec_dptr = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
*(uint8_t *)hs = 0U; *(uint8_t *)sec_hdr = 0U;
/* AdvA flag */ /* AdvA flag */
/* NOTE: as we will use auxiliary packet, we remove AdvA in /* NOTE: as we will use auxiliary packet, we remove AdvA in
* primary channel. i.e. Do nothing to add AdvA in the primary * primary channel. i.e. Do nothing to add AdvA in the primary
* PDU. * PDU.
*/ */
if (_hp.adv_addr) { if (pri_hdr_prev.adv_addr) {
_pp += BDADDR_SIZE; pri_dptr_prev += BDADDR_SIZE;
/* Prepare to add AdvA in secondary PDU */ /* Prepare to add AdvA in secondary PDU */
hs->adv_addr = 1; sec_hdr->adv_addr = 1;
/* NOTE: AdvA is filled at enable */ /* NOTE: AdvA is filled at enable */
sec->tx_addr = pri->tx_addr; sec_pdu->tx_addr = pri_pdu->tx_addr;
} }
pri->tx_addr = 0U; pri_pdu->tx_addr = 0U;
pri->rx_addr = 0U; pri_pdu->rx_addr = 0U;
if (_hs.adv_addr) { if (sec_hdr_prev.adv_addr) {
_ps += BDADDR_SIZE; sec_dptr_prev += BDADDR_SIZE;
hs->adv_addr = 1; sec_hdr->adv_addr = 1;
} }
if (hs->adv_addr) { if (sec_hdr->adv_addr) {
ps += BDADDR_SIZE; sec_dptr += BDADDR_SIZE;
} }
/* No TargetA in primary and secondary channel for undirected */ /* No TargetA in primary and secondary channel for undirected */
/* No CTEInfo flag in primary and secondary channel PDU */ /* No CTEInfo flag in primary and secondary channel PDU */
/* ADI flag */ /* ADI flag */
if (_hp.adi) { if (pri_hdr_prev.adi) {
_pp += sizeof(struct pdu_adv_adi); pri_dptr_prev += sizeof(struct pdu_adv_adi);
} }
hp->adi = 1; pri_hdr->adi = 1;
pp += sizeof(struct pdu_adv_adi); pri_dptr += sizeof(struct pdu_adv_adi);
if (_hs.adi) { if (sec_hdr_prev.adi) {
_ps += sizeof(struct pdu_adv_adi); sec_dptr_prev += sizeof(struct pdu_adv_adi);
} }
hs->adi = 1; sec_hdr->adi = 1;
ps += sizeof(struct pdu_adv_adi); sec_dptr += sizeof(struct pdu_adv_adi);
/* AuxPtr flag */ /* AuxPtr flag */
if (_hp.aux_ptr) { if (pri_hdr_prev.aux_ptr) {
_pp += sizeof(struct pdu_adv_aux_ptr); pri_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
} }
hp->aux_ptr = 1; pri_hdr->aux_ptr = 1;
pp += sizeof(struct pdu_adv_aux_ptr); pri_dptr += sizeof(struct pdu_adv_aux_ptr);
if (_hs.aux_ptr) { if (sec_hdr_prev.aux_ptr) {
_ps += sizeof(struct pdu_adv_aux_ptr); sec_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
hs->aux_ptr = 1; sec_hdr->aux_ptr = 1;
ps += sizeof(struct pdu_adv_aux_ptr); sec_dptr += sizeof(struct pdu_adv_aux_ptr);
} }
/* No SyncInfo flag in primary channel PDU */ /* No SyncInfo flag in primary channel PDU */
/* Add SyncInfo flag in secondary channel PDU */ /* Add SyncInfo flag in secondary channel PDU */
hs->sync_info = 1; sec_hdr->sync_info = 1;
ps += sizeof(*si); sec_dptr += sizeof(*si);
/* Tx Power flag */ /* Tx Power flag */
if (_hp.tx_pwr) { if (pri_hdr_prev.tx_pwr) {
_pp++; pri_dptr_prev++;
/* C1, Tx Power is optional on the LE 1M PHY, and /* C1, Tx Power is optional on the LE 1M PHY, and
* reserved for future use on the LE Coded PHY. * reserved for future use on the LE Coded PHY.
*/ */
if (adv->lll.phy_p != PHY_CODED) { if (adv->lll.phy_p != PHY_CODED) {
hp->tx_pwr = 1; pri_hdr->tx_pwr = 1;
pp++; pri_dptr++;
} else { } else {
hs->tx_pwr = 1; sec_hdr->tx_pwr = 1;
} }
} }
if (_hs.tx_pwr) { if (sec_hdr_prev.tx_pwr) {
_ps++; sec_dptr_prev++;
hs->tx_pwr = 1; sec_hdr->tx_pwr = 1;
} }
if (hs->tx_pwr) { if (sec_hdr->tx_pwr) {
ps++; sec_dptr++;
} }
/* TODO: ACAD place holder */ /* TODO: ACAD place holder */
/* Calc primary PDU len */ /* Calc primary PDU len */
_pri_len = _pp - (uint8_t *)_p; pri_len_prev = pri_dptr_prev - (uint8_t *)pri_com_hdr_prev;
pri_len = pp - (uint8_t *)p; pri_len = pri_dptr - (uint8_t *)pri_com_hdr;
p->ext_hdr_len = pri_len - offsetof(struct pdu_adv_com_ext_adv, pri_com_hdr->ext_hdr_len = pri_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
/* set the primary PDU len */ /* set the primary PDU len */
pri->len = pri_len; pri_pdu->len = pri_len;
/* Calc secondary PDU len */ /* Calc secondary PDU len */
_sec_len = _ps - (uint8_t *)_s; sec_len_prev = sec_dptr_prev - (uint8_t *)sec_com_hdr_prev;
sec_len = ps - (uint8_t *)s; sec_len = sec_dptr - (uint8_t *)sec_com_hdr;
s->ext_hdr_len = sec_len - offsetof(struct pdu_adv_com_ext_adv, sec_com_hdr->ext_hdr_len = sec_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
/* TODO: Check AdvData overflow */ /* TODO: Check AdvData overflow */
ad_len = _sec->len - _sec_len; ad_len = sec_pdu_prev->len - sec_len_prev;
/* set the secondary PDU len */ /* set the secondary PDU len */
sec->len = sec_len + ad_len; sec_pdu->len = sec_len + ad_len;
/* Fill AdvData in secondary PDU */ /* Fill AdvData in secondary PDU */
memcpy(ps, _ps, ad_len); memcpy(sec_dptr, sec_dptr_prev, ad_len);
/* Start filling primary PDU payload based on flags */ /* Start filling primary PDU payload based on flags */
@ -268,16 +274,16 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
/* No ACAD in primary channel PDU */ /* No ACAD in primary channel PDU */
/* Tx Power */ /* Tx Power */
if (hp->tx_pwr) { if (pri_hdr->tx_pwr) {
*--pp = *--_pp; *--pri_dptr = *--pri_dptr_prev;
} else if (hs->tx_pwr) { } else if (sec_hdr->tx_pwr) {
*--ps = *--_ps; *--sec_dptr = *--sec_dptr_prev;
} }
/* No SyncInfo in primary channel PDU */ /* No SyncInfo in primary channel PDU */
/* Fill SyncInfo in secondary channel PDU */ /* Fill SyncInfo in secondary channel PDU */
ps -= sizeof(*si); sec_dptr -= sizeof(*si);
si = (void *)ps; si = (void *)sec_dptr;
si->offs = 0; /* NOTE: Filled by secondary prepare */ si->offs = 0; /* NOTE: Filled by secondary prepare */
si->offs_units = 0; si->offs_units = 0;
si->interval = sys_cpu_to_le16(interval); si->interval = sys_cpu_to_le16(interval);
@ -289,74 +295,82 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
si->evt_cntr = 0; /* TODO: Implementation defined */ si->evt_cntr = 0; /* TODO: Implementation defined */
/* AuxPtr */ /* AuxPtr */
if (_hp.aux_ptr) { if (pri_hdr_prev.aux_ptr) {
_pp -= sizeof(struct pdu_adv_aux_ptr); pri_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
} }
{ {
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux_ptr;
pp -= sizeof(struct pdu_adv_aux_ptr); pri_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Aux Offset will be set in advertiser LLL event /* NOTE: Aux Offset will be set in advertiser LLL event
*/ */
aux = (void *)pp; aux_ptr = (void *)pri_dptr;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */ /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */ aux_ptr->chan_idx = 0;
aux->phy = find_lsb_set(adv->lll.phy_s) - 1; aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(adv->lll.phy_s) - 1;
} }
/* TODO: reduce duplicate code if below remains similar to /* TODO: reduce duplicate code if below remains similar to
* primary PDU * primary PDU
*/ */
if (_hs.aux_ptr) { if (sec_hdr_prev.aux_ptr) {
struct pdu_adv_aux_ptr *aux; struct pdu_adv_aux_ptr *aux_ptr;
_ps -= sizeof(struct pdu_adv_aux_ptr); sec_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
ps -= sizeof(struct pdu_adv_aux_ptr); sec_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Aux Offset will be set in advertiser LLL event /* NOTE: Aux Offset will be set in advertiser LLL event
*/ */
aux = (void *)ps; aux_ptr = (void *)sec_dptr;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */ /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */ aux_ptr->chan_idx = 0;
aux->phy = find_lsb_set(adv->lll.phy_s) - 1; aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(adv->lll.phy_s) - 1;
} }
/* ADI */ /* ADI */
{ {
struct pdu_adv_adi *ap, *as; struct pdu_adv_adi *pri_adi, *sec_adi;
uint16_t did = UINT16_MAX; uint16_t did = UINT16_MAX;
pp -= sizeof(struct pdu_adv_adi); pri_dptr -= sizeof(struct pdu_adv_adi);
ps -= sizeof(struct pdu_adv_adi); sec_dptr -= sizeof(struct pdu_adv_adi);
ap = (void *)pp; pri_adi = (void *)pri_dptr;
as = (void *)ps; sec_adi = (void *)sec_dptr;
if (_hp.adi) { if (pri_hdr_prev.adi) {
struct pdu_adv_adi *_adi; struct pdu_adv_adi *pri_adi_prev;
_pp -= sizeof(struct pdu_adv_adi); pri_dptr_prev -= sizeof(struct pdu_adv_adi);
_ps -= sizeof(struct pdu_adv_adi); sec_dptr_prev -= sizeof(struct pdu_adv_adi);
/* NOTE: memcpy shall handle overlapping buffers /* NOTE: memcpy shall handle overlapping buffers
*/ */
memcpy(pp, _pp, sizeof(struct pdu_adv_adi)); memcpy(pri_dptr, pri_dptr_prev,
memcpy(ps, _ps, sizeof(struct pdu_adv_adi)); sizeof(struct pdu_adv_adi));
memcpy(sec_dptr, sec_dptr_prev,
sizeof(struct pdu_adv_adi));
_adi = (void *)_pp; pri_adi_prev = (void *)pri_dptr_prev;
did = sys_le16_to_cpu(_adi->did); did = sys_le16_to_cpu(pri_adi_prev->did);
} else { } else {
ap->sid = adv->sid; pri_adi->sid = adv->sid;
as->sid = adv->sid; sec_adi->sid = adv->sid;
} }
did++; did++;
ap->did = sys_cpu_to_le16(did); pri_adi->did = sys_cpu_to_le16(did);
as->did = sys_cpu_to_le16(did); sec_adi->did = sys_cpu_to_le16(did);
} }
/* No CTEInfo field in primary channel PDU */ /* No CTEInfo field in primary channel PDU */
@ -368,24 +382,24 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
/* NOTE: AdvA in aux channel is also filled at enable and RPA /* NOTE: AdvA in aux channel is also filled at enable and RPA
* timeout * timeout
*/ */
if (hs->adv_addr) { if (sec_hdr->adv_addr) {
void *bdaddr; void *bdaddr;
if (_hs.adv_addr) { if (sec_hdr_prev.adv_addr) {
_ps -= BDADDR_SIZE; sec_dptr_prev -= BDADDR_SIZE;
bdaddr = _ps; bdaddr = sec_dptr_prev;
} else { } else {
_pp -= BDADDR_SIZE; pri_dptr_prev -= BDADDR_SIZE;
bdaddr = _pp; bdaddr = pri_dptr_prev;
} }
ps -= BDADDR_SIZE; sec_dptr -= BDADDR_SIZE;
memcpy(ps, bdaddr, BDADDR_SIZE); memcpy(sec_dptr, bdaddr, BDADDR_SIZE);
} }
lll_adv_aux_data_enqueue(lll_aux, is); lll_adv_aux_data_enqueue(lll_aux, sec_idx);
lll_adv_data_enqueue(lll, ip); lll_adv_data_enqueue(lll, pri_idx);
} else { } else {
sync = (void *)HDR_LLL2EVT(lll_sync); sync = (void *)HDR_LLL2EVT(lll_sync);
} }
@ -399,15 +413,15 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
pdu->tx_addr = 0U; pdu->tx_addr = 0U;
pdu->rx_addr = 0U; pdu->rx_addr = 0U;
t = (void *)&pdu->adv_ext_ind; ter_com_hdr = (void *)&pdu->adv_ext_ind;
ht = (void *)t->ext_hdr_adi_adv_data; ter_hdr = (void *)ter_com_hdr->ext_hdr_adi_adv_data;
pt = (uint8_t *)ht + sizeof(*ht); ter_dptr = (uint8_t *)ter_hdr + sizeof(*ter_hdr);
*(uint8_t *)&_ht = *(uint8_t *)ht; ter_hdr_prev = *ter_hdr;
*(uint8_t *)ht = 0; *(uint8_t *)ter_hdr = 0;
_pt = pt; ter_dptr_prev = ter_dptr;
/* Non-connectable and Non-scannable adv mode */ /* Non-connectable and Non-scannable adv mode */
t->adv_mode = 0; ter_com_hdr->adv_mode = 0;
/* No AdvA */ /* No AdvA */
/* No TargetA */ /* No TargetA */
@ -431,15 +445,16 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
/* TODO: AdvData */ /* TODO: AdvData */
/* Calc tertiary PDU len */ /* Calc tertiary PDU len */
ter_len = pt - (uint8_t *)t; ter_len = ter_dptr - (uint8_t *)ter_com_hdr;
if (ter_len > if (ter_len >
(offsetof(struct pdu_adv_com_ext_adv, ext_hdr_adi_adv_data) + (offsetof(struct pdu_adv_com_ext_adv, ext_hdr_adi_adv_data) +
sizeof(*ht))) { sizeof(*ter_hdr))) {
t->ext_hdr_len = ter_len - offsetof(struct pdu_adv_com_ext_adv, ter_com_hdr->ext_hdr_len = ter_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
pdu->len = ter_len; pdu->len = ter_len;
} else { } else {
t->ext_hdr_len = 0; ter_com_hdr->ext_hdr_len = 0;
pdu->len = offsetof(struct pdu_adv_com_ext_adv, pdu->len = offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data); ext_hdr_adi_adv_data);
} }