Bluetooth: Controller: Update PDU header fields population
Updated the implementation to allow population of auxiliary
pointer field as necessary to append PDU chains.
Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv.c b/subsys/bluetooth/controller/ll_sw/ull_adv.c
index 7a7c2d7..fea6955 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c
@@ -560,7 +560,8 @@
pri_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
}
if (pri_hdr->aux_ptr) {
- ull_adv_aux_ptr_fill(&pri_dptr, phy_s);
+ pri_dptr -= sizeof(struct pdu_adv_aux_ptr);
+ ull_adv_aux_ptr_fill((void *)pri_dptr, 0U, phy_s);
}
adv->lll.phy_s = phy_s;
#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
index 46ceabc..eb3c720 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
@@ -503,9 +503,9 @@
struct pdu_adv *pri_pdu, *pri_pdu_prev;
struct pdu_adv *sec_pdu_prev, *sec_pdu;
struct pdu_adv_adi *pri_adi, *sec_adi;
- struct pdu_adv_sync_info *sync_info;
uint8_t *pri_dptr, *pri_dptr_prev;
uint8_t *sec_dptr, *sec_dptr_prev;
+ struct pdu_adv_aux_ptr *aux_ptr;
uint8_t pri_len, sec_len_prev;
struct lll_adv_aux *lll_aux;
struct ll_adv_aux_set *aux;
@@ -527,12 +527,12 @@
pri_pdu_prev = lll_adv_data_peek(lll);
if (pri_pdu_prev->type != PDU_ADV_TYPE_EXT_IND) {
if (sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_AD_DATA) {
- uint8_t *val_ptr = value;
+ /* pick the data length */
+ ad_len = *((uint8_t *)value);
+ value = (uint8_t *)value + sizeof(uint8_t);
- ad_len = *val_ptr;
- val_ptr++;
-
- ad_data = (void *)sys_get_le32(val_ptr);
+ /* pick the reference to data */
+ (void)memcpy(&ad_data, value, sizeof(ad_data));
return ull_adv_data_set(adv, ad_len, ad_data);
}
@@ -625,6 +625,9 @@
if (sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_ADVA) {
uint8_t own_addr_type = *(uint8_t *)value;
+ /* Move to next value */
+ value = (uint8_t *)value + sizeof(own_addr_type);
+
sec_pdu->tx_addr = own_addr_type & 0x1;
} else if (pri_hdr_prev.adv_addr) {
sec_pdu->tx_addr = pri_pdu_prev->tx_addr;
@@ -689,13 +692,37 @@
}
pri_hdr->aux_ptr = 1;
pri_dptr += sizeof(struct pdu_adv_aux_ptr);
+
+ if (sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_AUX_PTR) {
+ sec_hdr->aux_ptr = 1;
+ aux_ptr = NULL;
+
+ /* return the size of aux pointer structure */
+ *(uint8_t *)value = sizeof(struct pdu_adv_aux_ptr);
+ value = (uint8_t *)value + sizeof(uint8_t);
+
+ /* return the pointer to aux pointer struct inside the PDU
+ * buffer
+ */
+ memcpy(value, &sec_dptr, sizeof(sec_dptr));
+ value = (uint8_t *)value + sizeof(sec_dptr);
+ } else if (!(sec_hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_AUX_PTR) &&
+ sec_hdr_prev.aux_ptr) {
+ sec_hdr->aux_ptr = 1;
+ aux_ptr = (void *)sec_dptr_prev;
+ } else {
+ aux_ptr = NULL;
+ }
if (sec_hdr_prev.aux_ptr) {
sec_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
-
- sec_hdr->aux_ptr = 1;
+ }
+ if (sec_hdr->aux_ptr) {
sec_dptr += sizeof(struct pdu_adv_aux_ptr);
}
+#if defined(CONFIG_BT_CTLR_ADV_PERIODIC)
+ struct pdu_adv_sync_info *sync_info;
+
/* No SyncInfo flag in primary channel PDU */
/* Add/Remove SyncInfo flag in secondary channel PDU */
if (sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_SYNC_INFO) {
@@ -704,25 +731,27 @@
/* return the size of sync info structure */
*(uint8_t *)value = sizeof(*sync_info);
- value = (uint8_t *)value + 1;
+ value = (uint8_t *)value + sizeof(uint8_t);
/* return the pointer to sync info struct inside the PDU
* buffer
*/
(void)memcpy(value, &sec_dptr, sizeof(sec_dptr));
value = (uint8_t *)value + sizeof(sec_dptr);
-
- sec_dptr += sizeof(*sync_info);
} else if (!(sec_hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_SYNC_INFO) &&
sec_hdr_prev.sync_info) {
sec_hdr->sync_info = 1;
sync_info = (void *)sec_dptr_prev;
-
- sec_dptr_prev += sizeof(*sync_info);
- sec_dptr += sizeof(*sync_info);
} else {
sync_info = NULL;
}
+ if (sec_hdr_prev.sync_info) {
+ sec_dptr_prev += sizeof(*sync_info);
+ }
+ if (sec_hdr->sync_info) {
+ sec_dptr += sizeof(*sync_info);
+ }
+#endif /* CONFIG_BT_CTLR_ADV_PERIODIC */
/* Tx Power flag */
if (pri_hdr_prev.tx_pwr) {
@@ -774,12 +803,12 @@
/* AD Data, add or remove */
if (sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_AD_DATA) {
- uint8_t *val_ptr = value;
+ /* pick the data length */
+ ad_len = *((uint8_t *)value);
+ value = (uint8_t *)value + sizeof(uint8_t);
- ad_len = *val_ptr;
- val_ptr++;
-
- ad_data = (void *)sys_get_le32(val_ptr);
+ /* pick the reference to data */
+ (void)memcpy(&ad_data, value, sizeof(ad_data));
} else {
/* Calc the previous AD data length in auxiliary PDU */
ad_len = sec_pdu_prev->len - sec_len_prev;
@@ -842,12 +871,18 @@
if (pri_hdr_prev.aux_ptr) {
pri_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
}
- ull_adv_aux_ptr_fill(&pri_dptr, lll->phy_s);
+ pri_dptr -= sizeof(struct pdu_adv_aux_ptr);
+ ull_adv_aux_ptr_fill((void *)pri_dptr, 0U, lll->phy_s);
if (sec_hdr_prev.aux_ptr) {
sec_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
+ }
+ if (sec_hdr->aux_ptr) {
+ sec_dptr -= sizeof(struct pdu_adv_aux_ptr);
+ }
- ull_adv_aux_ptr_fill(&sec_dptr, lll->phy_s);
+ if (aux_ptr) {
+ memmove(sec_dptr, aux_ptr, sizeof(*aux_ptr));
}
/* ADI */
@@ -939,24 +974,28 @@
return 0;
}
-void ull_adv_aux_ptr_fill(uint8_t **dptr, uint8_t phy_s)
+void ull_adv_aux_ptr_fill(struct pdu_adv_aux_ptr *aux_ptr, uint32_t offs_us,
+ uint8_t phy_s)
{
- struct pdu_adv_aux_ptr *aux_ptr;
+ uint32_t offs;
/* NOTE: Channel Index and Aux Offset will be set on every advertiser's
* event prepare when finding the auxiliary event's ticker offset.
+ * Here we fill initial values.
*/
-
- *dptr -= sizeof(struct pdu_adv_aux_ptr);
- aux_ptr = (void *)*dptr;
-
aux_ptr->chan_idx = 0U;
aux_ptr->ca = (lll_clock_ppm_local_get() <= SCA_50_PPM) ?
SCA_VALUE_50_PPM : SCA_VALUE_500_PPM;
- aux_ptr->offs_units = OFFS_UNIT_VALUE_30_US;
- aux_ptr->offs = 0U;
+ offs = offs_us / OFFS_UNIT_30_US;
+ if (!!(offs >> 13)) {
+ aux_ptr->offs = offs / (OFFS_UNIT_300_US / OFFS_UNIT_30_US);
+ aux_ptr->offs_units = OFFS_UNIT_VALUE_300_US;
+ } else {
+ aux_ptr->offs = offs;
+ aux_ptr->offs_units = OFFS_UNIT_VALUE_30_US;
+ }
aux_ptr->phy = find_lsb_set(phy_s) - 1;
}
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
index dbd8ed7..3bd2d58 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
@@ -159,7 +159,8 @@
}
/* helper function to fill the aux ptr structure in common ext adv payload */
-void ull_adv_aux_ptr_fill(uint8_t **dptr, uint8_t phy_s);
+void ull_adv_aux_ptr_fill(struct pdu_adv_aux_ptr *aux_ptr, uint32_t offs_us,
+ uint8_t phy_s);
/* helper function to handle adv aux done events */
void ull_adv_aux_done(struct node_rx_event_done *done);
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
index 187eca8..8d4c984 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
@@ -1227,7 +1227,9 @@
memmove(ter_dptr, ter_dptr_prev,
sizeof(struct pdu_adv_aux_ptr));
} else {
- ull_adv_aux_ptr_fill(&ter_dptr, lll_sync->adv->phy_s);
+ ter_dptr -= sizeof(struct pdu_adv_aux_ptr);
+ ull_adv_aux_ptr_fill((void *)ter_dptr, 0U,
+ lll_sync->adv->phy_s);
}
}
