Bluetooth: Controller: Add target address into common extended header
Add and retain in subsequent updates the target address
in the auxiliary PDUs.
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 1399dd5..2d412e6 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c
@@ -428,6 +428,10 @@
pdu->tx_addr = 0;
}
+ /* TargetA flag */
+ if (pri_hdr_prev.tgt_addr) {
+ pri_dptr_prev += BDADDR_SIZE;
+ }
/* TargetA flag in primary channel PDU only for directed */
if (evt_prop & BT_HCI_LE_ADV_PROP_DIRECT) {
pri_hdr->tgt_addr = 1;
@@ -441,12 +445,17 @@
/* ADI flag */
if (pri_hdr_prev.adi) {
+ pri_dptr_prev += sizeof(struct pdu_adv_adi);
+
pri_hdr->adi = 1;
pri_dptr += sizeof(struct pdu_adv_adi);
}
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
/* AuxPtr flag */
+ if (pri_hdr_prev.aux_ptr) {
+ pri_dptr_prev += sizeof(struct pdu_adv_aux_ptr);
+ }
/* Need aux for connectable or scannable extended advertising */
if (pri_hdr_prev.aux_ptr ||
((evt_prop & (BT_HCI_LE_ADV_PROP_CONN |
@@ -459,6 +468,9 @@
/* No SyncInfo flag in primary channel PDU */
/* Tx Power flag */
+ 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.
*/
@@ -482,6 +494,9 @@
/* No ACAD in primary channel PDU */
/* Tx Power */
+ if (pri_hdr_prev.tx_pwr) {
+ pri_dptr_prev--;
+ }
if (pri_hdr->tx_pwr) {
uint8_t _tx_pwr;
@@ -501,6 +516,9 @@
#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
/* AuxPtr */
+ if (pri_hdr_prev.aux_ptr) {
+ pri_dptr_prev -= sizeof(struct pdu_adv_aux_ptr);
+ }
if (pri_hdr->aux_ptr) {
ull_adv_aux_ptr_fill(&pri_dptr, phy_s);
}
@@ -508,14 +526,17 @@
#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
/* ADI */
+ if (pri_hdr_prev.adi) {
+ pri_dptr_prev -= sizeof(struct pdu_adv_adi);
+ }
if (pri_hdr->adi) {
struct pdu_adv_adi *adi;
pri_dptr -= sizeof(struct pdu_adv_adi);
- /* NOTE: memcpy shall handle overlapping buffers */
- memcpy(pri_dptr, pri_dptr_prev,
- sizeof(struct pdu_adv_adi));
+ /* NOTE: memmove shall handle overlapping buffers */
+ memmove(pri_dptr, pri_dptr_prev,
+ sizeof(struct pdu_adv_adi));
adi = (void *)pri_dptr;
adi->sid = sid;
@@ -525,6 +546,9 @@
/* No CTEInfo field in primary channel PDU */
/* TargetA */
+ if (pri_hdr_prev.tgt_addr) {
+ pri_dptr_prev -= BDADDR_SIZE;
+ }
if (pri_hdr->tgt_addr) {
pri_dptr -= BDADDR_SIZE;
/* NOTE: RPA will be updated on enable, if needed */
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
index 017e6e9..ef031e2 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
@@ -583,7 +583,6 @@
return BT_HCI_ERR_UNSPECIFIED;
}
pri_pdu->tx_addr = 0U;
- pri_pdu->rx_addr = 0U;
if (pri_hdr_prev.adv_addr) {
pri_dptr_prev += BDADDR_SIZE;
@@ -593,7 +592,31 @@
}
sec_dptr += BDADDR_SIZE;
- /* No TargetA in primary and secondary channel for undirected */
+ /* No TargetA in primary and secondary channel for undirected.
+ * Move from primary to secondary PDU, if present in primary PDU.
+ */
+ if (pri_hdr_prev.tgt_addr) {
+ sec_hdr->tgt_addr = 1U;
+ sec_pdu->rx_addr = pri_pdu_prev->rx_addr;
+ sec_dptr += BDADDR_SIZE;
+
+ /* Retain the target address if present in the previous PDU */
+ } else if (!(sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_ADVA) &&
+ sec_hdr_prev.tgt_addr) {
+ sec_hdr->tgt_addr = 1U;
+ sec_pdu->rx_addr = sec_pdu_prev->rx_addr;
+ sec_dptr += BDADDR_SIZE;
+ }
+ pri_pdu->rx_addr = 0U;
+
+ if (pri_hdr_prev.tgt_addr) {
+ pri_dptr_prev += BDADDR_SIZE;
+ }
+
+ if (sec_hdr_prev.tgt_addr) {
+ sec_dptr_prev += BDADDR_SIZE;
+ }
+
/* No CTEInfo flag in primary and secondary channel PDU */
/* ADI flag */
@@ -815,7 +838,24 @@
/* No CTEInfo field in primary channel PDU */
- /* No TargetA non-conn non-scan advertising */
+ /* No TargetA non-conn non-scan advertising, but present in directed
+ * advertising.
+ */
+ if (sec_hdr->tgt_addr) {
+ void *bdaddr;
+
+ if (sec_hdr_prev.tgt_addr) {
+ sec_dptr_prev -= BDADDR_SIZE;
+ bdaddr = sec_dptr_prev;
+ } else {
+ pri_dptr_prev -= BDADDR_SIZE;
+ bdaddr = pri_dptr_prev;
+ }
+
+ sec_dptr -= BDADDR_SIZE;
+
+ memcpy(sec_dptr, bdaddr, BDADDR_SIZE);
+ }
/* No AdvA in primary channel due to AuxPtr being added */
