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;
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;
struct pdu_adv_com_ext_adv *p;
struct pdu_adv_aux_ptr *aux;
struct pdu_adv *pri, *sec;
struct pdu_adv *pri_pdu, *sec_pdu;
struct pdu_adv_aux_ptr *aux_ptr;
struct pdu_adv_hdr *pri_hdr;
struct lll_adv *lll_adv;
struct pdu_adv_hdr *h;
struct evt_hdr *evt;
uint32_t remainder;
uint32_t start_us;
uint8_t *pri_dptr;
uint8_t phy_s;
uint8_t *ptr;
uint8_t upd;
DEBUG_RADIO_START_A(1);
/* 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 */
lll_adv = lll->adv;
pri = lll_adv_data_curr_get(lll_adv);
LL_ASSERT(pri->type == PDU_ADV_TYPE_EXT_IND);
pri_pdu = lll_adv_data_curr_get(lll_adv);
LL_ASSERT(pri_pdu->type == PDU_ADV_TYPE_EXT_IND);
/* Get reference to extended header */
p = (void *)&pri->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h);
pri_com_hdr = (void *)&pri_pdu->adv_ext_ind;
pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* traverse through adv_addr, if present */
if (h->adv_addr) {
ptr += BDADDR_SIZE;
if (pri_hdr->adv_addr) {
pri_dptr += BDADDR_SIZE;
}
/* traverse through adi, if present */
if (h->adi) {
ptr += sizeof(struct pdu_adv_adi);
if (pri_hdr->adi) {
pri_dptr += sizeof(struct pdu_adv_adi);
}
aux = (void *)ptr;
aux_ptr = (void *)pri_dptr;
/* 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_disable();
}
@ -159,10 +159,10 @@ static int prepare_cb(struct lll_prepare_param *prepare_param)
#endif /* !BT_CTLR_ADV_EXT_PBACK */
/* Use channel idx in aux_ptr */
lll_chan_set(aux->chan_idx);
lll_chan_set(aux_ptr->chan_idx);
/* 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 */
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)
} else if (pdu->type == PDU_ADV_TYPE_EXT_IND) {
struct pdu_adv_com_ext_adv *p;
struct pdu_adv_hdr *h, _h;
uint8_t *_ptr, *ptr;
struct pdu_adv_hdr *pri_hdr, pri_hdr_prev;
struct pdu_adv_com_ext_adv *pri_com_hdr;
uint8_t *pri_dptr_prev, *pri_dptr;
uint8_t len;
p = (void *)&pdu->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h);
_ptr = ptr;
pri_com_hdr = (void *)&pdu->adv_ext_ind;
pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
pri_dptr_prev = pri_dptr;
/* No ACAD and no AdvData */
p->adv_mode = evt_prop & 0x03;
pri_com_hdr->adv_mode = evt_prop & 0x03;
/* Zero-init header flags */
if (is_pdu_type_changed) {
*(uint8_t *)&_h = 0;
*(uint8_t *)&pri_hdr_prev = 0;
} 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 */
if (_h.adv_addr) {
_ptr += BDADDR_SIZE;
if (pri_hdr_prev.adv_addr) {
pri_dptr_prev += BDADDR_SIZE;
}
if (!p->adv_mode &&
(!_h.aux_ptr ||
if (!pri_com_hdr->adv_mode &&
(!pri_hdr_prev.aux_ptr ||
(!(evt_prop & BT_HCI_LE_ADV_PROP_ANON) &&
(phy_p != PHY_CODED)))) {
/* TODO: optional on 1M with Aux Ptr */
h->adv_addr = 1;
pri_hdr->adv_addr = 1;
/* NOTE: AdvA is filled at enable */
pdu->tx_addr = own_addr_type & 0x1;
ptr += BDADDR_SIZE;
pri_dptr += BDADDR_SIZE;
} else {
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 */
/* ADI flag */
if (_h.adi) {
h->adi = 1;
ptr += sizeof(struct pdu_adv_adi);
if (pri_hdr_prev.adi) {
pri_hdr->adi = 1;
pri_dptr += sizeof(struct pdu_adv_adi);
}
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
/* AuxPtr flag */
if (_h.aux_ptr) {
h->aux_ptr = 1;
ptr += sizeof(struct pdu_adv_aux_ptr);
if (pri_hdr_prev.aux_ptr) {
pri_hdr->aux_ptr = 1;
pri_dptr += sizeof(struct pdu_adv_aux_ptr);
}
#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.
*/
if ((evt_prop & BT_HCI_LE_ADV_PROP_TX_POWER) &&
(!_h.aux_ptr || (phy_p != PHY_CODED))) {
h->tx_pwr = 1;
ptr++;
(!pri_hdr_prev.aux_ptr || (phy_p != PHY_CODED))) {
pri_hdr->tx_pwr = 1;
pri_dptr++;
}
/* 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,
ext_hdr_adi_adv_data) + sizeof(*h))) {
p->ext_hdr_len = len -
ext_hdr_adi_adv_data) + sizeof(*pri_hdr))) {
pri_com_hdr->ext_hdr_len = len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
pdu->len = len;
} else {
p->ext_hdr_len = 0;
pri_com_hdr->ext_hdr_len = 0;
pdu->len = offsetof(struct pdu_adv_com_ext_adv,
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 */
/* Tx Power */
if (h->tx_pwr) {
if (pri_hdr->tx_pwr) {
uint8_t _tx_pwr;
_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 */
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
/* AuxPtr */
if (h->aux_ptr) {
struct pdu_adv_aux_ptr *aux;
if (pri_hdr->aux_ptr) {
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
* will be set in Advertiser Event.
/* NOTE: Aux Offset will be set in advertiser LLL event
*/
aux = (void *)ptr;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */
aux->phy = find_lsb_set(phy_s) - 1;
aux_ptr = (void *)pri_dptr;
/* FIXME: implementation defined */
aux_ptr->chan_idx = 0;
aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(phy_s) - 1;
}
adv->lll.phy_s = phy_s;
#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
/* ADI */
if (h->adi) {
if (pri_hdr->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 */
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;
}
adv->sid = sid;
@ -519,16 +522,16 @@ uint8_t ll_adv_enable(uint8_t enable)
#if defined(CONFIG_BT_CTLR_ADV_EXT)
} else if (pdu_adv->type == PDU_ADV_TYPE_EXT_IND) {
struct pdu_adv_com_ext_adv *p;
struct pdu_adv_hdr *h;
uint8_t *ptr;
struct pdu_adv_com_ext_adv *pri_com_hdr;
struct pdu_adv_hdr *pri_hdr;
uint8_t *pri_dptr;
p = (void *)&pdu_adv->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h);
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);
/* AdvA, fill here at enable */
if (h->adv_addr) {
if (pri_hdr->adv_addr) {
uint8_t const *tx_addr =
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;
}
memcpy(ptr, tx_addr, BDADDR_SIZE);
memcpy(pri_dptr, tx_addr, BDADDR_SIZE);
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
} else if (h->aux_ptr) {
struct pdu_adv_com_ext_adv *s;
struct pdu_adv *pdu_aux;
struct pdu_adv_hdr *hs;
uint8_t *ps;
} 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;
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;
hs = (void *)s->ext_hdr_adi_adv_data;
ps = (uint8_t *)hs + sizeof(*hs);
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 (hs->adv_addr) {
if (sec_hdr->adv_addr) {
uint8_t const *tx_addr =
ll_adv_aux_random_addr_get(adv, NULL);
/* TODO: Privacy */
if (pdu_aux->tx_addr &&
if (sec_pdu->tx_addr &&
!mem_nz((void *)tx_addr, BDADDR_SIZE)) {
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
*/
}

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 const *const data)
{
struct pdu_adv_com_ext_adv *p, *_p, *s, *_s;
uint8_t pri_len, _pri_len, sec_len, _sec_len;
struct pdu_adv *_pri, *pri, *_sec, *sec;
struct pdu_adv_hdr *hp, _hp, *hs, _hs;
struct pdu_adv *pri_pdu, *pri_pdu_prev, *sec_pdu, *sec_pdu_prev;
struct pdu_adv_com_ext_adv *pri_com_hdr, *pri_com_hdr_prev;
struct pdu_adv_com_ext_adv *sec_com_hdr, *sec_com_hdr_prev;
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 ll_adv_aux_set *aux;
uint8_t *_pp, *pp, *ps, *_ps;
struct ll_adv_set *adv;
struct lll_adv *lll;
uint8_t ip, is;
uint8_t pri_idx, sec_idx;
/* op param definitions:
* 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
* advertising.
*/
_pri = lll_adv_data_peek(lll);
if (_pri->type != PDU_ADV_TYPE_EXT_IND) {
pri_pdu_prev = lll_adv_data_peek(lll);
if (pri_pdu_prev->type != PDU_ADV_TYPE_EXT_IND) {
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 */
_p = (void *)&_pri->adv_ext_ind;
hp = (void *)_p->ext_hdr_adi_adv_data;
*(uint8_t *)&_hp = *(uint8_t *)hp;
_pp = (uint8_t *)hp + sizeof(*hp);
pri_com_hdr_prev = (void *)&pri_pdu_prev->adv_ext_ind;
pri_hdr = (void *)pri_com_hdr_prev->ext_hdr_adi_adv_data;
pri_hdr_prev = *pri_hdr;
pri_dptr_prev = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* Get reference to new primary PDU data buffer */
pri = lll_adv_data_alloc(lll, &ip);
pri->type = _pri->type;
pri->rfu = 0U;
pri->chan_sel = 0U;
p = (void *)&pri->adv_ext_ind;
p->adv_mode = _p->adv_mode;
hp = (void *)p->ext_hdr_adi_adv_data;
pp = (uint8_t *)hp + sizeof(*hp);
*(uint8_t *)hp = 0U;
pri_pdu = lll_adv_data_alloc(lll, &pri_idx);
pri_pdu->type = pri_pdu_prev->type;
pri_pdu->rfu = 0U;
pri_pdu->chan_sel = 0U;
pri_com_hdr = (void *)&pri_pdu->adv_ext_ind;
pri_com_hdr->adv_mode = pri_com_hdr_prev->adv_mode;
pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
*(uint8_t *)pri_hdr = 0U;
/* Get reference to previous secondary PDU data */
_sec = lll_adv_aux_data_peek(lll_aux);
_s = (void *)&_sec->adv_ext_ind;
hs = (void *)_s->ext_hdr_adi_adv_data;
*(uint8_t *)&_hs = *(uint8_t *)hs;
_ps = (uint8_t *)hs + sizeof(*hs);
sec_pdu_prev = lll_adv_aux_data_peek(lll_aux);
sec_com_hdr_prev = (void *)&sec_pdu_prev->adv_ext_ind;
sec_hdr = (void *)sec_com_hdr_prev->ext_hdr_adi_adv_data;
sec_hdr_prev = *sec_hdr;
sec_dptr_prev = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
/* Get reference to new secondary PDU data buffer */
sec = lll_adv_aux_data_alloc(lll_aux, &is);
sec->type = pri->type;
sec->rfu = 0U;
sec_pdu = lll_adv_aux_data_alloc(lll_aux, &sec_idx);
sec_pdu->type = pri_pdu->type;
sec_pdu->rfu = 0U;
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 {
sec->chan_sel = 0U;
sec_pdu->chan_sel = 0U;
}
sec->tx_addr = _sec->tx_addr;
sec->rx_addr = _sec->rx_addr;
sec_pdu->tx_addr = sec_pdu_prev->tx_addr;
sec_pdu->rx_addr = sec_pdu_prev->rx_addr;
s = (void *)&sec->adv_ext_ind;
s->adv_mode = p->adv_mode;
hs = (void *)s->ext_hdr_adi_adv_data;
ps = (uint8_t *)hs + sizeof(*hs);
*(uint8_t *)hs = 0U;
sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
sec_com_hdr->adv_mode = pri_com_hdr->adv_mode;
sec_hdr = (void *)sec_com_hdr->ext_hdr_adi_adv_data;
sec_dptr = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
*(uint8_t *)sec_hdr = 0U;
/* AdvA flag */
/* NOTE: as we will use auxiliary packet, we remove AdvA in primary
* channel. i.e. Do nothing to add AdvA in the primary PDU.
*/
if (_hp.adv_addr) {
_pp += BDADDR_SIZE;
if (pri_hdr_prev.adv_addr) {
pri_dptr_prev += BDADDR_SIZE;
/* Prepare to add AdvA in secondary PDU */
hs->adv_addr = 1;
sec_hdr->adv_addr = 1;
/* 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->rx_addr = 0U;
pri_pdu->tx_addr = 0U;
pri_pdu->rx_addr = 0U;
if (_hs.adv_addr) {
_ps += BDADDR_SIZE;
if (sec_hdr_prev.adv_addr) {
sec_dptr_prev += BDADDR_SIZE;
/* Prepare to copy AdvA from previous double buffered PDU */
hs->adv_addr = 1;
sec_hdr->adv_addr = 1;
}
if (hs->adv_addr) {
ps += BDADDR_SIZE;
if (sec_hdr->adv_addr) {
sec_dptr += BDADDR_SIZE;
}
/* No TargetA in primary and secondary channel for undirected */
/* No CTEInfo flag in primary and secondary channel PDU */
/* ADI flag */
if (_hp.adi) {
_pp += sizeof(struct pdu_adv_adi);
if (pri_hdr_prev.adi) {
pri_dptr_prev += sizeof(struct pdu_adv_adi);
}
hp->adi = 1;
pp += sizeof(struct pdu_adv_adi);
if (_hs.adi) {
_ps += sizeof(struct pdu_adv_adi);
pri_hdr->adi = 1;
pri_dptr += sizeof(struct pdu_adv_adi);
if (sec_hdr_prev.adi) {
sec_dptr_prev += sizeof(struct pdu_adv_adi);
}
hs->adi = 1;
ps += sizeof(struct pdu_adv_adi);
sec_hdr->adi = 1;
sec_dptr += sizeof(struct pdu_adv_adi);
/* AuxPtr flag */
if (_hp.aux_ptr) {
_pp += sizeof(struct pdu_adv_aux_ptr);
if (pri_hdr_prev.aux_ptr) {
pri_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
}
hp->aux_ptr = 1;
pp += sizeof(struct pdu_adv_aux_ptr);
if (_hs.aux_ptr) {
_ps += sizeof(struct pdu_adv_aux_ptr);
pri_hdr->aux_ptr = 1;
pri_dptr += sizeof(struct pdu_adv_aux_ptr);
if (sec_hdr_prev.aux_ptr) {
sec_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
hs->aux_ptr = 1;
ps += sizeof(struct pdu_adv_aux_ptr);
sec_hdr->aux_ptr = 1;
sec_dptr += sizeof(struct pdu_adv_aux_ptr);
}
/* No SyncInfo flag in primary channel PDU */
/* SyncInfo flag in secondary channel PDU */
if (_hs.sync_info) {
_ps += sizeof(struct pdu_adv_sync_info);
if (sec_hdr_prev.sync_info) {
sec_dptr_prev += sizeof(struct pdu_adv_sync_info);
hs->sync_info = 1;
ps += sizeof(struct pdu_adv_sync_info);
sec_hdr->sync_info = 1;
sec_dptr += sizeof(struct pdu_adv_sync_info);
}
/* Tx Power flag */
if (_hp.tx_pwr) {
_pp++;
if (pri_hdr_prev.tx_pwr) {
pri_dptr_prev++;
/* C1, Tx Power is optional on the LE 1M PHY, and reserved for
* for future use on the LE Coded PHY.
*/
if (lll->phy_p != PHY_CODED) {
hp->tx_pwr = 1;
pp++;
pri_hdr->tx_pwr = 1;
pri_dptr++;
} else {
hs->tx_pwr = 1;
sec_hdr->tx_pwr = 1;
}
}
if (_hs.tx_pwr) {
_ps++;
if (sec_hdr_prev.tx_pwr) {
sec_dptr_prev++;
hs->tx_pwr = 1;
sec_hdr->tx_pwr = 1;
}
if (hs->tx_pwr) {
ps++;
if (sec_hdr->tx_pwr) {
sec_dptr++;
}
/* No ACAD in Primary channel PDU */
/* TODO: ACAD in Secondary channel PDU */
/* Calc primary PDU len */
_pri_len = _pp - (uint8_t *)_p;
pri_len = pp - (uint8_t *)p;
p->ext_hdr_len = pri_len - offsetof(struct pdu_adv_com_ext_adv,
pri_len_prev = pri_dptr_prev - (uint8_t *)pri_com_hdr_prev;
pri_len = pri_dptr - (uint8_t *)pri_com_hdr;
pri_com_hdr->ext_hdr_len = pri_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
/* set the primary PDU len */
pri->len = pri_len;
pri_pdu->len = pri_len;
/* Calc secondary PDU len */
_sec_len = _ps - (uint8_t *)_s;
sec_len = ps - (uint8_t *)s;
s->ext_hdr_len = sec_len - offsetof(struct pdu_adv_com_ext_adv,
sec_len_prev = sec_dptr_prev - (uint8_t *)sec_com_hdr_prev;
sec_len = sec_dptr - (uint8_t *)sec_com_hdr;
sec_com_hdr->ext_hdr_len = sec_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
/* TODO: Check AdvData overflow */
/* Fill AdvData in secondary PDU */
memcpy(ps, data, len);
memcpy(sec_dptr, data, 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 */
@ -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 */
/* Tx Power */
if (hp->tx_pwr) {
*--pp = *--_pp;
} else if (hs->tx_pwr) {
*--ps = *--_ps;
if (pri_hdr->tx_pwr) {
*--pri_dptr = *--pri_dptr_prev;
} else if (sec_hdr->tx_pwr) {
*--sec_dptr = *--sec_dptr_prev;
}
/* No SyncInfo in primary channel PDU */
/* SyncInfo in secondary channel PDU */
if (hs->sync_info) {
_ps -= sizeof(struct pdu_adv_sync_info);
ps -= sizeof(struct pdu_adv_sync_info);
if (sec_hdr->sync_info) {
sec_dptr_prev -= 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 */
if (_hp.aux_ptr) {
_pp -= sizeof(struct pdu_adv_aux_ptr);
if (pri_hdr_prev.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 */
aux = (void *)pp;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */
aux->phy = find_lsb_set(lll->phy_s) - 1;
aux_ptr = (void *)pri_dptr;
/* FIXME: implementation defined */
aux_ptr->chan_idx = 0;
aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(lll->phy_s) - 1;
}
if (_hs.aux_ptr) {
struct pdu_adv_aux_ptr *aux;
if (sec_hdr_prev.aux_ptr) {
struct pdu_adv_aux_ptr *aux_ptr;
_ps -= sizeof(struct pdu_adv_aux_ptr);
ps -= sizeof(struct pdu_adv_aux_ptr);
sec_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
sec_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Aux Offset will be set in advertiser LLL event */
aux = (void *)ps;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */
aux->phy = find_lsb_set(lll->phy_s) - 1;
aux_ptr = (void *)sec_dptr;
/* FIXME: implementation defined */
aux_ptr->chan_idx = 0;
aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(lll->phy_s) - 1;
}
/* ADI */
{
struct pdu_adv_adi *ap, *as;
struct pdu_adv_adi *pri_adi, *sec_adi;
uint16_t did = UINT16_MAX;
pp -= sizeof(struct pdu_adv_adi);
ps -= sizeof(struct pdu_adv_adi);
pri_dptr -= sizeof(struct pdu_adv_adi);
sec_dptr -= sizeof(struct pdu_adv_adi);
ap = (void *)pp;
as = (void *)ps;
pri_adi = (void *)pri_dptr;
sec_adi = (void *)sec_dptr;
if (_hp.adi) {
struct pdu_adv_adi *_adi;
if (pri_hdr_prev.adi) {
struct pdu_adv_adi *pri_adi_prev;
_pp -= sizeof(struct pdu_adv_adi);
_ps -= sizeof(struct pdu_adv_adi);
pri_dptr_prev -= sizeof(struct pdu_adv_adi);
sec_dptr_prev -= sizeof(struct pdu_adv_adi);
/* NOTE: memcpy shall handle overlapping buffers */
memcpy(pp, _pp, sizeof(struct pdu_adv_adi));
memcpy(ps, _ps, sizeof(struct pdu_adv_adi));
memcpy(pri_dptr, pri_dptr_prev,
sizeof(struct pdu_adv_adi));
memcpy(sec_dptr, sec_dptr_prev,
sizeof(struct pdu_adv_adi));
_adi = (void *)_pp;
did = sys_le16_to_cpu(_adi->did);
pri_adi_prev = (void *)pri_dptr_prev;
did = sys_le16_to_cpu(pri_adi_prev->did);
} else {
ap->sid = adv->sid;
as->sid = adv->sid;
pri_adi->sid = adv->sid;
sec_adi->sid = adv->sid;
}
if ((op == 0x04) || len || (_pri_len != pri_len) ||
(_sec->len != sec->len)) {
if ((op == 0x04) || len || (pri_len_prev != pri_len) ||
(sec_pdu_prev->len != sec_pdu->len)) {
did++;
}
ap->did = sys_cpu_to_le16(did);
as->did = sys_cpu_to_le16(did);
pri_adi->did = sys_cpu_to_le16(did);
sec_adi->did = sys_cpu_to_le16(did);
}
/* 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 */
/* NOTE: AdvA in aux channel is also filled at enable and RPA timeout */
if (hs->adv_addr) {
if (sec_hdr->adv_addr) {
void *bdaddr;
if (_hs.adv_addr) {
_ps -= BDADDR_SIZE;
bdaddr = _ps;
if (sec_hdr_prev.adv_addr) {
sec_dptr_prev -= BDADDR_SIZE;
bdaddr = sec_dptr_prev;
} else {
_pp -= BDADDR_SIZE;
bdaddr = _pp;
pri_dptr_prev -= BDADDR_SIZE;
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_data_enqueue(lll, ip);
lll_adv_aux_data_enqueue(lll_aux, sec_idx);
lll_adv_data_enqueue(lll, pri_idx);
if (adv->is_enabled && !aux->is_started) {
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)
{
struct ll_adv_set *adv;
struct pdu_adv *_pri;
struct pdu_adv *pri_pdu_prev;
struct lll_adv *lll;
/* 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
* advertising.
*/
_pri = lll_adv_data_peek(lll);
if (_pri->type != PDU_ADV_TYPE_EXT_IND) {
pri_pdu_prev = lll_adv_data_peek(lll);
if (pri_pdu_prev->type != PDU_ADV_TYPE_EXT_IND) {
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,
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_hdr *h;
uint8_t *ptr;
p = (void *)&pdu->adv_ext_ind;
h = (void *)p->ext_hdr_adi_adv_data;
pri_com_hdr = (void *)&pdu->adv_ext_ind;
h = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
ptr = (uint8_t *)h + sizeof(*h);
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)
{
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 pdu_adv_com_ext_adv *t;
struct ll_adv_sync_set *sync;
struct pdu_adv_hdr *ht, _ht;
struct ll_adv_set *adv;
struct pdu_adv *pdu;
uint8_t *_pt, *pt;
uint8_t ter_len;
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;
if (!lll_sync) {
struct pdu_adv_com_ext_adv *p, *_p, *s, *_s;
uint8_t pri_len, _pri_len, sec_len, _sec_len;
struct pdu_adv *_pri, *pri, *_sec, *sec;
struct pdu_adv_hdr *hp, _hp, *hs, _hs;
struct pdu_adv_com_ext_adv *pri_com_hdr, *pri_com_hdr_prev;
struct pdu_adv_com_ext_adv *sec_com_hdr, *sec_com_hdr_prev;
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;
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 lll_adv_aux *lll_aux;
uint8_t *_pp, *pp, *ps, *_ps;
uint8_t ip, is, ad_len;
uint8_t pri_idx, sec_idx, ad_len;
struct lll_adv *lll;
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;
/* Get reference to previous primary PDU data */
_pri = lll_adv_data_peek(lll);
_p = (void *)&_pri->adv_ext_ind;
hp = (void *)_p->ext_hdr_adi_adv_data;
*(uint8_t *)&_hp = *(uint8_t *)hp;
_pp = (uint8_t *)hp + sizeof(*hp);
pri_pdu_prev = lll_adv_data_peek(lll);
pri_com_hdr_prev = (void *)&pri_pdu_prev->adv_ext_ind;
pri_hdr = (void *)pri_com_hdr_prev->ext_hdr_adi_adv_data;
pri_hdr_prev = *pri_hdr;
pri_dptr_prev = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
/* Get reference to new primary PDU data buffer */
pri = lll_adv_data_alloc(lll, &ip);
pri->type = _pri->type;
pri->rfu = 0U;
pri->chan_sel = 0U;
p = (void *)&pri->adv_ext_ind;
p->adv_mode = _p->adv_mode;
hp = (void *)p->ext_hdr_adi_adv_data;
pp = (uint8_t *)hp + sizeof(*hp);
*(uint8_t *)hp = 0U;
pri_pdu = lll_adv_data_alloc(lll, &pri_idx);
pri_pdu->type = pri_pdu_prev->type;
pri_pdu->rfu = 0U;
pri_pdu->chan_sel = 0U;
pri_com_hdr = (void *)&pri_pdu->adv_ext_ind;
pri_com_hdr->adv_mode = pri_com_hdr_prev->adv_mode;
pri_hdr = (void *)pri_com_hdr->ext_hdr_adi_adv_data;
pri_dptr = (uint8_t *)pri_hdr + sizeof(*pri_hdr);
*(uint8_t *)pri_hdr = 0U;
/* Get reference to previous secondary PDU data */
_sec = lll_adv_aux_data_peek(lll_aux);
_s = (void *)&_sec->adv_ext_ind;
hs = (void *)_s->ext_hdr_adi_adv_data;
*(uint8_t *)&_hs = *(uint8_t *)hs;
_ps = (uint8_t *)hs + sizeof(*hs);
sec_pdu_prev = lll_adv_aux_data_peek(lll_aux);
sec_com_hdr_prev = (void *)&sec_pdu_prev->adv_ext_ind;
sec_hdr = (void *)sec_com_hdr_prev->ext_hdr_adi_adv_data;
sec_hdr_prev = *sec_hdr;
sec_dptr_prev = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
/* Get reference to new secondary PDU data buffer */
sec = lll_adv_aux_data_alloc(lll_aux, &is);
sec->type = pri->type;
sec->rfu = 0U;
sec_pdu = lll_adv_aux_data_alloc(lll_aux, &sec_idx);
sec_pdu->type = pri_pdu->type;
sec_pdu->rfu = 0U;
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 {
sec->chan_sel = 0U;
sec_pdu->chan_sel = 0U;
}
sec->tx_addr = _sec->tx_addr;
sec->rx_addr = _sec->rx_addr;
sec_pdu->tx_addr = sec_pdu_prev->tx_addr;
sec_pdu->rx_addr = sec_pdu_prev->rx_addr;
s = (void *)&sec->adv_ext_ind;
s->adv_mode = p->adv_mode;
hs = (void *)s->ext_hdr_adi_adv_data;
ps = (uint8_t *)hs + sizeof(*hs);
*(uint8_t *)hs = 0U;
sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
sec_com_hdr->adv_mode = pri_com_hdr->adv_mode;
sec_hdr = (void *)sec_com_hdr->ext_hdr_adi_adv_data;
sec_dptr = (uint8_t *)sec_hdr + sizeof(*sec_hdr);
*(uint8_t *)sec_hdr = 0U;
/* AdvA flag */
/* NOTE: as we will use auxiliary packet, we remove AdvA in
* primary channel. i.e. Do nothing to add AdvA in the primary
* PDU.
*/
if (_hp.adv_addr) {
_pp += BDADDR_SIZE;
if (pri_hdr_prev.adv_addr) {
pri_dptr_prev += BDADDR_SIZE;
/* Prepare to add AdvA in secondary PDU */
hs->adv_addr = 1;
sec_hdr->adv_addr = 1;
/* 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->rx_addr = 0U;
pri_pdu->tx_addr = 0U;
pri_pdu->rx_addr = 0U;
if (_hs.adv_addr) {
_ps += BDADDR_SIZE;
hs->adv_addr = 1;
if (sec_hdr_prev.adv_addr) {
sec_dptr_prev += BDADDR_SIZE;
sec_hdr->adv_addr = 1;
}
if (hs->adv_addr) {
ps += BDADDR_SIZE;
if (sec_hdr->adv_addr) {
sec_dptr += BDADDR_SIZE;
}
/* No TargetA in primary and secondary channel for undirected */
/* No CTEInfo flag in primary and secondary channel PDU */
/* ADI flag */
if (_hp.adi) {
_pp += sizeof(struct pdu_adv_adi);
if (pri_hdr_prev.adi) {
pri_dptr_prev += sizeof(struct pdu_adv_adi);
}
hp->adi = 1;
pp += sizeof(struct pdu_adv_adi);
if (_hs.adi) {
_ps += sizeof(struct pdu_adv_adi);
pri_hdr->adi = 1;
pri_dptr += sizeof(struct pdu_adv_adi);
if (sec_hdr_prev.adi) {
sec_dptr_prev += sizeof(struct pdu_adv_adi);
}
hs->adi = 1;
ps += sizeof(struct pdu_adv_adi);
sec_hdr->adi = 1;
sec_dptr += sizeof(struct pdu_adv_adi);
/* AuxPtr flag */
if (_hp.aux_ptr) {
_pp += sizeof(struct pdu_adv_aux_ptr);
if (pri_hdr_prev.aux_ptr) {
pri_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
}
hp->aux_ptr = 1;
pp += sizeof(struct pdu_adv_aux_ptr);
if (_hs.aux_ptr) {
_ps += sizeof(struct pdu_adv_aux_ptr);
pri_hdr->aux_ptr = 1;
pri_dptr += sizeof(struct pdu_adv_aux_ptr);
if (sec_hdr_prev.aux_ptr) {
sec_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
hs->aux_ptr = 1;
ps += sizeof(struct pdu_adv_aux_ptr);
sec_hdr->aux_ptr = 1;
sec_dptr += sizeof(struct pdu_adv_aux_ptr);
}
/* No SyncInfo flag in primary channel PDU */
/* Add SyncInfo flag in secondary channel PDU */
hs->sync_info = 1;
ps += sizeof(*si);
sec_hdr->sync_info = 1;
sec_dptr += sizeof(*si);
/* Tx Power flag */
if (_hp.tx_pwr) {
_pp++;
if (pri_hdr_prev.tx_pwr) {
pri_dptr_prev++;
/* C1, Tx Power is optional on the LE 1M PHY, and
* reserved for future use on the LE Coded PHY.
*/
if (adv->lll.phy_p != PHY_CODED) {
hp->tx_pwr = 1;
pp++;
pri_hdr->tx_pwr = 1;
pri_dptr++;
} else {
hs->tx_pwr = 1;
sec_hdr->tx_pwr = 1;
}
}
if (_hs.tx_pwr) {
_ps++;
if (sec_hdr_prev.tx_pwr) {
sec_dptr_prev++;
hs->tx_pwr = 1;
sec_hdr->tx_pwr = 1;
}
if (hs->tx_pwr) {
ps++;
if (sec_hdr->tx_pwr) {
sec_dptr++;
}
/* TODO: ACAD place holder */
/* Calc primary PDU len */
_pri_len = _pp - (uint8_t *)_p;
pri_len = pp - (uint8_t *)p;
p->ext_hdr_len = pri_len - offsetof(struct pdu_adv_com_ext_adv,
pri_len_prev = pri_dptr_prev - (uint8_t *)pri_com_hdr_prev;
pri_len = pri_dptr - (uint8_t *)pri_com_hdr;
pri_com_hdr->ext_hdr_len = pri_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
/* set the primary PDU len */
pri->len = pri_len;
pri_pdu->len = pri_len;
/* Calc secondary PDU len */
_sec_len = _ps - (uint8_t *)_s;
sec_len = ps - (uint8_t *)s;
s->ext_hdr_len = sec_len - offsetof(struct pdu_adv_com_ext_adv,
sec_len_prev = sec_dptr_prev - (uint8_t *)sec_com_hdr_prev;
sec_len = sec_dptr - (uint8_t *)sec_com_hdr;
sec_com_hdr->ext_hdr_len = sec_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
/* TODO: Check AdvData overflow */
ad_len = _sec->len - _sec_len;
ad_len = sec_pdu_prev->len - sec_len_prev;
/* set the secondary PDU len */
sec->len = sec_len + ad_len;
sec_pdu->len = sec_len + ad_len;
/* 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 */
@ -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 */
/* Tx Power */
if (hp->tx_pwr) {
*--pp = *--_pp;
} else if (hs->tx_pwr) {
*--ps = *--_ps;
if (pri_hdr->tx_pwr) {
*--pri_dptr = *--pri_dptr_prev;
} else if (sec_hdr->tx_pwr) {
*--sec_dptr = *--sec_dptr_prev;
}
/* No SyncInfo in primary channel PDU */
/* Fill SyncInfo in secondary channel PDU */
ps -= sizeof(*si);
si = (void *)ps;
sec_dptr -= sizeof(*si);
si = (void *)sec_dptr;
si->offs = 0; /* NOTE: Filled by secondary prepare */
si->offs_units = 0;
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 */
/* AuxPtr */
if (_hp.aux_ptr) {
_pp -= sizeof(struct pdu_adv_aux_ptr);
if (pri_hdr_prev.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
*/
aux = (void *)pp;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */
aux->phy = find_lsb_set(adv->lll.phy_s) - 1;
aux_ptr = (void *)pri_dptr;
/* FIXME: implementation defined */
aux_ptr->chan_idx = 0;
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
* primary PDU
*/
if (_hs.aux_ptr) {
struct pdu_adv_aux_ptr *aux;
if (sec_hdr_prev.aux_ptr) {
struct pdu_adv_aux_ptr *aux_ptr;
_ps -= sizeof(struct pdu_adv_aux_ptr);
ps -= sizeof(struct pdu_adv_aux_ptr);
sec_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
sec_dptr -= sizeof(struct pdu_adv_aux_ptr);
/* NOTE: Aux Offset will be set in advertiser LLL event
*/
aux = (void *)ps;
aux->chan_idx = 0; /* FIXME: implementation defined */
aux->ca = 0; /* FIXME: implementation defined */
aux->offs_units = 0; /* FIXME: implementation defined */
aux->phy = find_lsb_set(adv->lll.phy_s) - 1;
aux_ptr = (void *)sec_dptr;
/* FIXME: implementation defined */
aux_ptr->chan_idx = 0;
aux_ptr->ca = 0;
aux_ptr->offs_units = 0;
aux_ptr->phy = find_lsb_set(adv->lll.phy_s) - 1;
}
/* ADI */
{
struct pdu_adv_adi *ap, *as;
struct pdu_adv_adi *pri_adi, *sec_adi;
uint16_t did = UINT16_MAX;
pp -= sizeof(struct pdu_adv_adi);
ps -= sizeof(struct pdu_adv_adi);
pri_dptr -= sizeof(struct pdu_adv_adi);
sec_dptr -= sizeof(struct pdu_adv_adi);
ap = (void *)pp;
as = (void *)ps;
pri_adi = (void *)pri_dptr;
sec_adi = (void *)sec_dptr;
if (_hp.adi) {
struct pdu_adv_adi *_adi;
if (pri_hdr_prev.adi) {
struct pdu_adv_adi *pri_adi_prev;
_pp -= sizeof(struct pdu_adv_adi);
_ps -= sizeof(struct pdu_adv_adi);
pri_dptr_prev -= sizeof(struct pdu_adv_adi);
sec_dptr_prev -= sizeof(struct pdu_adv_adi);
/* NOTE: memcpy shall handle overlapping buffers
*/
memcpy(pp, _pp, sizeof(struct pdu_adv_adi));
memcpy(ps, _ps, sizeof(struct pdu_adv_adi));
memcpy(pri_dptr, pri_dptr_prev,
sizeof(struct pdu_adv_adi));
memcpy(sec_dptr, sec_dptr_prev,
sizeof(struct pdu_adv_adi));
_adi = (void *)_pp;
did = sys_le16_to_cpu(_adi->did);
pri_adi_prev = (void *)pri_dptr_prev;
did = sys_le16_to_cpu(pri_adi_prev->did);
} else {
ap->sid = adv->sid;
as->sid = adv->sid;
pri_adi->sid = adv->sid;
sec_adi->sid = adv->sid;
}
did++;
ap->did = sys_cpu_to_le16(did);
as->did = sys_cpu_to_le16(did);
pri_adi->did = sys_cpu_to_le16(did);
sec_adi->did = sys_cpu_to_le16(did);
}
/* 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
* timeout
*/
if (hs->adv_addr) {
if (sec_hdr->adv_addr) {
void *bdaddr;
if (_hs.adv_addr) {
_ps -= BDADDR_SIZE;
bdaddr = _ps;
if (sec_hdr_prev.adv_addr) {
sec_dptr_prev -= BDADDR_SIZE;
bdaddr = sec_dptr_prev;
} else {
_pp -= BDADDR_SIZE;
bdaddr = _pp;
pri_dptr_prev -= BDADDR_SIZE;
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_data_enqueue(lll, ip);
lll_adv_aux_data_enqueue(lll_aux, sec_idx);
lll_adv_data_enqueue(lll, pri_idx);
} else {
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->rx_addr = 0U;
t = (void *)&pdu->adv_ext_ind;
ht = (void *)t->ext_hdr_adi_adv_data;
pt = (uint8_t *)ht + sizeof(*ht);
*(uint8_t *)&_ht = *(uint8_t *)ht;
*(uint8_t *)ht = 0;
_pt = pt;
ter_com_hdr = (void *)&pdu->adv_ext_ind;
ter_hdr = (void *)ter_com_hdr->ext_hdr_adi_adv_data;
ter_dptr = (uint8_t *)ter_hdr + sizeof(*ter_hdr);
ter_hdr_prev = *ter_hdr;
*(uint8_t *)ter_hdr = 0;
ter_dptr_prev = ter_dptr;
/* Non-connectable and Non-scannable adv mode */
t->adv_mode = 0;
ter_com_hdr->adv_mode = 0;
/* No AdvA */
/* No TargetA */
@ -431,15 +445,16 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
/* TODO: AdvData */
/* Calc tertiary PDU len */
ter_len = pt - (uint8_t *)t;
ter_len = ter_dptr - (uint8_t *)ter_com_hdr;
if (ter_len >
(offsetof(struct pdu_adv_com_ext_adv, ext_hdr_adi_adv_data) +
sizeof(*ht))) {
t->ext_hdr_len = ter_len - offsetof(struct pdu_adv_com_ext_adv,
sizeof(*ter_hdr))) {
ter_com_hdr->ext_hdr_len = ter_len -
offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
pdu->len = ter_len;
} else {
t->ext_hdr_len = 0;
ter_com_hdr->ext_hdr_len = 0;
pdu->len = offsetof(struct pdu_adv_com_ext_adv,
ext_hdr_adi_adv_data);
}