Bluetooth: controller: split: Added SyncInfo into Aux Packet
Added SyncInfo population in Aux packet. Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
This commit is contained in:
Vinayak Kariappa Chettimada
Carles Cufí
parent
a126850c15
commit
a43c3be2b9
2 changed files with 330 additions and 3 deletions
|
|
@ -210,7 +210,6 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
|
||||||
_ptr = ptr;
|
_ptr = ptr;
|
||||||
|
|
||||||
/* No ACAD and no AdvData */
|
/* No ACAD and no AdvData */
|
||||||
p->ext_hdr_len = 0;
|
|
||||||
p->adv_mode = evt_prop & 0x03;
|
p->adv_mode = evt_prop & 0x03;
|
||||||
|
|
||||||
/* Zero-init header flags */
|
/* Zero-init header flags */
|
||||||
|
|
@ -277,6 +276,7 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
|
||||||
ext_hdr_adi_adv_data);
|
ext_hdr_adi_adv_data);
|
||||||
pdu->len = len;
|
pdu->len = len;
|
||||||
} else {
|
} else {
|
||||||
|
p->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);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -47,10 +47,14 @@ 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 lll_adv_sync *lll;
|
struct pdu_adv_com_ext_adv *t;
|
||||||
struct ll_adv_sync_set *sync;
|
struct ll_adv_sync_set *sync;
|
||||||
|
struct ext_adv_hdr *ht, _ht;
|
||||||
|
struct lll_adv_sync *lll;
|
||||||
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;
|
||||||
|
|
||||||
adv = ull_adv_is_created_get(handle);
|
adv = ull_adv_is_created_get(handle);
|
||||||
if (!adv) {
|
if (!adv) {
|
||||||
|
|
@ -59,6 +63,14 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
|
||||||
|
|
||||||
lll = adv->lll.sync;
|
lll = adv->lll.sync;
|
||||||
if (!lll) {
|
if (!lll) {
|
||||||
|
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 ext_adv_hdr *hp, _hp, *hs, _hs;
|
||||||
|
struct ext_adv_sync_info *si;
|
||||||
|
uint8_t *_pp, *pp, *ps, *_ps;
|
||||||
|
uint8_t ip, is, ad_len;
|
||||||
|
|
||||||
sync = sync_acquire();
|
sync = sync_acquire();
|
||||||
if (!sync) {
|
if (!sync) {
|
||||||
return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||||||
|
|
@ -81,6 +93,283 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
|
||||||
lll->data_chan_id = 0;
|
lll->data_chan_id = 0;
|
||||||
|
|
||||||
lll->adv = &adv->lll;
|
lll->adv = &adv->lll;
|
||||||
|
|
||||||
|
/* Get reference to previous primary PDU data */
|
||||||
|
_pri = lll_adv_data_peek(&adv->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);
|
||||||
|
|
||||||
|
/* Get reference to new primary PDU data buffer */
|
||||||
|
pri = lll_adv_data_alloc(&adv->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;
|
||||||
|
|
||||||
|
/* Get reference to previous secondary PDU data */
|
||||||
|
_sec = lll_adv_aux_data_peek(&adv->lll);
|
||||||
|
_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);
|
||||||
|
|
||||||
|
/* Get reference to new secondary PDU data buffer */
|
||||||
|
sec = lll_adv_aux_data_alloc(&adv->lll, &is);
|
||||||
|
sec->type = pri->type;
|
||||||
|
sec->rfu = 0U;
|
||||||
|
|
||||||
|
if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2)) {
|
||||||
|
sec->chan_sel = _sec->chan_sel;
|
||||||
|
} else {
|
||||||
|
sec->chan_sel = 0U;
|
||||||
|
}
|
||||||
|
|
||||||
|
sec->tx_addr = _sec->tx_addr;
|
||||||
|
sec->rx_addr = _sec->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;
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
|
||||||
|
/* Prepare to add AdvA in secondary PDU */
|
||||||
|
hs->adv_addr = 1;
|
||||||
|
|
||||||
|
/* NOTE: AdvA is filled at enable */
|
||||||
|
sec->tx_addr = pri->tx_addr;
|
||||||
|
}
|
||||||
|
pri->tx_addr = 0U;
|
||||||
|
pri->rx_addr = 0U;
|
||||||
|
|
||||||
|
if (_hs.adv_addr) {
|
||||||
|
_ps += BDADDR_SIZE;
|
||||||
|
hs->adv_addr = 1;
|
||||||
|
}
|
||||||
|
if (hs->adv_addr) {
|
||||||
|
ps += 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 ext_adv_adi);
|
||||||
|
}
|
||||||
|
hp->adi = 1;
|
||||||
|
pp += sizeof(struct ext_adv_adi);
|
||||||
|
if (_hs.adi) {
|
||||||
|
_ps += sizeof(struct ext_adv_adi);
|
||||||
|
}
|
||||||
|
hs->adi = 1;
|
||||||
|
ps += sizeof(struct ext_adv_adi);
|
||||||
|
|
||||||
|
/* AuxPtr flag */
|
||||||
|
if (_hp.aux_ptr) {
|
||||||
|
_pp += sizeof(struct ext_adv_aux_ptr);
|
||||||
|
}
|
||||||
|
hp->aux_ptr = 1;
|
||||||
|
pp += sizeof(struct ext_adv_aux_ptr);
|
||||||
|
if (_hs.aux_ptr) {
|
||||||
|
_ps += sizeof(struct ext_adv_aux_ptr);
|
||||||
|
|
||||||
|
hs->aux_ptr = 1;
|
||||||
|
ps += sizeof(struct ext_adv_aux_ptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* No SyncInfo flag in primary channel PDU */
|
||||||
|
/* Add SyncInfo flag in secondary channel PDU */
|
||||||
|
hs->sync_info = 1;
|
||||||
|
ps += sizeof(*si);
|
||||||
|
|
||||||
|
/* Tx Power flag */
|
||||||
|
if (_hp.tx_pwr) {
|
||||||
|
_pp++;
|
||||||
|
|
||||||
|
/* 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 != BIT(2)) {
|
||||||
|
hp->tx_pwr = 1;
|
||||||
|
pp++;
|
||||||
|
} else {
|
||||||
|
hs->tx_pwr = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (_hs.tx_pwr) {
|
||||||
|
_ps++;
|
||||||
|
|
||||||
|
hs->tx_pwr = 1;
|
||||||
|
}
|
||||||
|
if (hs->tx_pwr) {
|
||||||
|
ps++;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 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,
|
||||||
|
ext_hdr_adi_adv_data);
|
||||||
|
|
||||||
|
/* set the primary PDU len */
|
||||||
|
pri->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,
|
||||||
|
ext_hdr_adi_adv_data);
|
||||||
|
|
||||||
|
/* TODO: Check AdvData overflow */
|
||||||
|
ad_len = _sec->len - _sec_len;
|
||||||
|
|
||||||
|
/* set the secondary PDU len */
|
||||||
|
sec->len = sec_len + ad_len;
|
||||||
|
|
||||||
|
/* Fill AdvData in secondary PDU */
|
||||||
|
memcpy(ps, _ps, ad_len);
|
||||||
|
|
||||||
|
/* Start filling primary PDU payload based on flags */
|
||||||
|
|
||||||
|
/* No AdvData in primary channel PDU */
|
||||||
|
|
||||||
|
/* No ACAD in primary channel PDU */
|
||||||
|
|
||||||
|
/* Tx Power */
|
||||||
|
if (hp->tx_pwr) {
|
||||||
|
*--pp = *--_pp;
|
||||||
|
} else if (hs->tx_pwr) {
|
||||||
|
*--ps = *--_ps;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* No SyncInfo in primary channel PDU */
|
||||||
|
/* Fill SyncInfo in secondary channel PDU */
|
||||||
|
ps -= sizeof(*si);
|
||||||
|
si = (void *)ps;
|
||||||
|
si->offs = 0; /* NOTE: Filled by secondary prepare */
|
||||||
|
si->offs_units = 0;
|
||||||
|
si->interval = interval;
|
||||||
|
memcpy(si->sca_chm, lll->data_chan_map, sizeof(si->sca_chm));
|
||||||
|
memcpy(&si->aa, lll->access_addr, sizeof(si->aa));
|
||||||
|
memcpy(si->crc_init, lll->crc_init, sizeof(si->crc_init));
|
||||||
|
|
||||||
|
si->evt_cntr = 0; /* TODO: Implementation defined */
|
||||||
|
|
||||||
|
/* AuxPtr */
|
||||||
|
if (_hp.aux_ptr) {
|
||||||
|
_pp -= sizeof(struct ext_adv_aux_ptr);
|
||||||
|
}
|
||||||
|
{
|
||||||
|
struct ext_adv_aux_ptr *aux;
|
||||||
|
|
||||||
|
pp -= sizeof(struct ext_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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* TODO: reduce duplicate code if below remains similar to
|
||||||
|
* primary PDU
|
||||||
|
*/
|
||||||
|
if (_hs.aux_ptr) {
|
||||||
|
struct ext_adv_aux_ptr *aux;
|
||||||
|
|
||||||
|
_ps -= sizeof(struct ext_adv_aux_ptr);
|
||||||
|
ps -= sizeof(struct ext_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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ADI */
|
||||||
|
{
|
||||||
|
struct ext_adv_adi *ap, *as;
|
||||||
|
uint16_t did = UINT16_MAX;
|
||||||
|
|
||||||
|
pp -= sizeof(struct ext_adv_adi);
|
||||||
|
ps -= sizeof(struct ext_adv_adi);
|
||||||
|
|
||||||
|
ap = (void *)pp;
|
||||||
|
as = (void *)ps;
|
||||||
|
|
||||||
|
if (_hp.adi) {
|
||||||
|
struct ext_adv_adi *_adi;
|
||||||
|
|
||||||
|
_pp -= sizeof(struct ext_adv_adi);
|
||||||
|
_ps -= sizeof(struct ext_adv_adi);
|
||||||
|
|
||||||
|
/* NOTE: memcpy shall handle overlapping buffers
|
||||||
|
*/
|
||||||
|
memcpy(pp, _pp, sizeof(struct ext_adv_adi));
|
||||||
|
memcpy(ps, _ps, sizeof(struct ext_adv_adi));
|
||||||
|
|
||||||
|
_adi = (void *)_pp;
|
||||||
|
did = _adi->did;
|
||||||
|
} else {
|
||||||
|
ap->sid = adv->sid;
|
||||||
|
as->sid = adv->sid;
|
||||||
|
}
|
||||||
|
|
||||||
|
did++;
|
||||||
|
|
||||||
|
ap->did = did;
|
||||||
|
as->did = did;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* No CTEInfo field in primary channel PDU */
|
||||||
|
|
||||||
|
/* No TargetA non-conn non-scan advertising */
|
||||||
|
|
||||||
|
/* 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) {
|
||||||
|
void *bdaddr;
|
||||||
|
|
||||||
|
if (_hs.adv_addr) {
|
||||||
|
_ps -= BDADDR_SIZE;
|
||||||
|
bdaddr = _ps;
|
||||||
|
} else {
|
||||||
|
_pp -= BDADDR_SIZE;
|
||||||
|
bdaddr = _pp;
|
||||||
|
}
|
||||||
|
|
||||||
|
ps -= BDADDR_SIZE;
|
||||||
|
|
||||||
|
memcpy(ps, bdaddr, BDADDR_SIZE);
|
||||||
|
}
|
||||||
|
|
||||||
|
lll_adv_aux_data_enqueue(&adv->lll, is);
|
||||||
|
lll_adv_data_enqueue(&adv->lll, ip);
|
||||||
} else {
|
} else {
|
||||||
sync = (void *)HDR_LLL2EVT(lll);
|
sync = (void *)HDR_LLL2EVT(lll);
|
||||||
}
|
}
|
||||||
|
|
@ -94,13 +383,51 @@ 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;
|
||||||
|
|
||||||
pdu->len = 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;
|
||||||
|
|
||||||
|
/* Non-connectable and Non-scannable adv mode */
|
||||||
|
t->adv_mode = 0;
|
||||||
|
|
||||||
|
/* No AdvA */
|
||||||
|
/* No TargetA */
|
||||||
|
|
||||||
|
/* TODO: CTEInfo */
|
||||||
|
|
||||||
|
/* No ADI */
|
||||||
|
|
||||||
|
/* TODO: AuxPtr */
|
||||||
|
|
||||||
|
/* No SyncInfo */
|
||||||
|
|
||||||
|
/* TODO: TxPower */
|
||||||
if (flags & BIT(6)) {
|
if (flags & BIT(6)) {
|
||||||
/* TODO: add/remove Tx Power in AUX_SYNC_IND PDU */
|
/* TODO: add/remove Tx Power in AUX_SYNC_IND PDU */
|
||||||
return BT_HCI_ERR_CMD_DISALLOWED;
|
return BT_HCI_ERR_CMD_DISALLOWED;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* TODO: ACAD */
|
||||||
|
|
||||||
|
/* TODO: AdvData */
|
||||||
|
|
||||||
|
/* Calc primary PDU len */
|
||||||
|
ter_len = pt - (uint8_t *)t;
|
||||||
|
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,
|
||||||
|
ext_hdr_adi_adv_data);
|
||||||
|
pdu->len = ter_len;
|
||||||
|
} else {
|
||||||
|
t->ext_hdr_len = 0;
|
||||||
|
pdu->len = offsetof(struct pdu_adv_com_ext_adv,
|
||||||
|
ext_hdr_adi_adv_data);
|
||||||
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue