Bluetooth: controller: split: Refactor auto variable names
Review rework updates, refactor auto variable names. Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
This commit is contained in:
parent
372a2e2fd3
commit
205fe1825e
4 changed files with 418 additions and 386 deletions
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
}
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue