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;
|
||||
|
||||
/* No ACAD and no AdvData */
|
||||
p->ext_hdr_len = 0;
|
||||
p->adv_mode = evt_prop & 0x03;
|
||||
|
||||
/* 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);
|
||||
pdu->len = len;
|
||||
} else {
|
||||
p->ext_hdr_len = 0;
|
||||
pdu->len = offsetof(struct pdu_adv_com_ext_adv,
|
||||
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)
|
||||
{
|
||||
struct lll_adv_sync *lll;
|
||||
struct pdu_adv_com_ext_adv *t;
|
||||
struct ll_adv_sync_set *sync;
|
||||
struct ext_adv_hdr *ht, _ht;
|
||||
struct lll_adv_sync *lll;
|
||||
struct ll_adv_set *adv;
|
||||
struct pdu_adv *pdu;
|
||||
uint8_t *_pt, *pt;
|
||||
uint8_t ter_len;
|
||||
|
||||
adv = ull_adv_is_created_get(handle);
|
||||
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;
|
||||
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();
|
||||
if (!sync) {
|
||||
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->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 {
|
||||
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->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)) {
|
||||
/* TODO: add/remove Tx Power in AUX_SYNC_IND PDU */
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue