Bluetooth: controller: split: non-conn non-scan with aux packets

Added implementation to advertise non-connectable
non-scannable advertising with auxiliary packets.

Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c

@@ -28,6 +28,7 @@
 #include "lll_vendor.h"
 #include "lll_clock.h"
 #include "lll_adv.h"
+#include "lll_adv_aux.h"
 #include "lll_conn.h"
 #include "lll_chan.h"
 #include "lll_filter.h"
@@ -54,7 +55,12 @@ static void isr_done(void *param);
 static void isr_abort(void *param);
 static void isr_cleanup(void *param);
 static void isr_race(void *param);
-static void chan_prepare(struct lll_adv *lll);
+static struct pdu_adv *chan_prepare(struct lll_adv *lll);
+
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+static void aux_ptr_populate(struct pdu_adv *pdu, uint32_t start_us);
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
+
 static inline int isr_rx_pdu(struct lll_adv *lll,
 			     uint8_t devmatch_ok, uint8_t devmatch_id,
 			     uint8_t irkmatch_ok, uint8_t irkmatch_id,
@@ -64,10 +70,12 @@ static inline bool isr_rx_sr_check(struct lll_adv *lll, struct pdu_adv *adv,
 				   uint8_t *rl_idx);
 static inline bool isr_rx_sr_adva_check(struct pdu_adv *adv,
 					struct pdu_adv *sr);
+
 #if defined(CONFIG_BT_CTLR_SCAN_REQ_NOTIFY)
 static inline int isr_rx_sr_report(struct pdu_adv *pdu_adv_rx,
 				   uint8_t rssi_ready);
 #endif /* CONFIG_BT_CTLR_SCAN_REQ_NOTIFY */
+
 static inline bool isr_rx_ci_check(struct lll_adv *lll, struct pdu_adv *adv,
 				   struct pdu_adv *ci, uint8_t devmatch_ok,
 				   uint8_t *rl_idx);
@@ -123,9 +131,10 @@ static int prepare_cb(struct lll_prepare_param *prepare_param)
 	struct lll_adv *lll = prepare_param->param;
 	uint32_t aa = sys_cpu_to_le32(PDU_AC_ACCESS_ADDR);
 	uint32_t ticks_at_event, ticks_at_start;
+	struct pdu_adv *pdu;
 	struct evt_hdr *evt;
-	uint32_t remainder_us;
 	uint32_t remainder;
+	uint32_t start_us;
 
 	DEBUG_RADIO_START_A(1);
 
@@ -166,7 +175,7 @@ static int prepare_cb(struct lll_prepare_param *prepare_param)
 
 	lll->chan_map_curr = lll->chan_map;
 
-	chan_prepare(lll);
+	pdu = chan_prepare(lll);
 
 #if defined(CONFIG_BT_HCI_MESH_EXT)
 	_radio.mesh_adv_end_us = 0;
@@ -202,18 +211,25 @@ static int prepare_cb(struct lll_prepare_param *prepare_param)
 	ticks_at_start += HAL_TICKER_US_TO_TICKS(EVENT_OVERHEAD_START_US);
 
 	remainder = prepare_param->remainder;
-	remainder_us = radio_tmr_start(1, ticks_at_start, remainder);
+	start_us = radio_tmr_start(1, ticks_at_start, remainder);
 
 	/* capture end of Tx-ed PDU, used to calculate HCTO. */
 	radio_tmr_end_capture();
 
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+	if (pdu->type ==  PDU_ADV_TYPE_EXT_IND) {
+		aux_ptr_populate(pdu, start_us);
+	}
+#else /* !CONFIG_BT_CTLR_ADV_EXT */
+	ARG_UNUSED(pdu);
+#endif /* !CONFIG_BT_CTLR_ADV_EXT */
+
 #if defined(CONFIG_BT_CTLR_GPIO_PA_PIN)
 	radio_gpio_pa_setup();
-	radio_gpio_pa_lna_enable(remainder_us +
+	radio_gpio_pa_lna_enable(start_us + radio_tx_ready_delay_get(0, 0) -
-				 radio_tx_ready_delay_get(0, 0) -
 				 CONFIG_BT_CTLR_GPIO_PA_OFFSET);
 #else /* !CONFIG_BT_CTLR_GPIO_PA_PIN */
-	ARG_UNUSED(remainder_us);
+	ARG_UNUSED(start_us);
 #endif /* !CONFIG_BT_CTLR_GPIO_PA_PIN */
 
 #if defined(CONFIG_BT_CTLR_XTAL_ADVANCED) && \
@@ -492,27 +508,57 @@ static void isr_done(void *param)
 #endif /* CONFIG_BT_PERIPHERAL */
 
 	if (lll->chan_map_curr) {
+		struct pdu_adv *pdu;
 		uint32_t start_us;
 
-		chan_prepare(lll);
+		pdu = chan_prepare(lll);
 
-#if defined(CONFIG_BT_CTLR_GPIO_PA_PIN)
+#if defined(CONFIG_BT_CTLR_GPIO_PA_PIN) || defined(CONFIG_BT_CTLR_ADV_EXT)
 		start_us = radio_tmr_start_now(1);
 
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+		if (pdu->type ==  PDU_ADV_TYPE_EXT_IND) {
+			aux_ptr_populate(pdu, start_us);
+		}
+#else /* !CONFIG_BT_CTLR_ADV_EXT */
+		ARG_UNUSED(pdu);
+#endif /* !CONFIG_BT_CTLR_ADV_EXT */
+
+#if defined(CONFIG_BT_CTLR_GPIO_PA_PIN)
 		radio_gpio_pa_setup();
 		radio_gpio_pa_lna_enable(start_us +
 					 radio_tx_ready_delay_get(0, 0) -
 					 CONFIG_BT_CTLR_GPIO_PA_OFFSET);
-#else /* !CONFIG_BT_CTLR_GPIO_PA_PIN */
+#endif /* CONFIG_BT_CTLR_GPIO_PA_PIN */
+#else /* !(CONFIG_BT_CTLR_GPIO_PA_PIN || defined(CONFIG_BT_CTLR_ADV_EXT)) */
 		ARG_UNUSED(start_us);
 
 		radio_tx_enable();
-#endif /* !CONFIG_BT_CTLR_GPIO_PA_PIN */
+#endif /* !(CONFIG_BT_CTLR_GPIO_PA_PIN || defined(CONFIG_BT_CTLR_ADV_EXT)) */
 
 		/* capture end of Tx-ed PDU, used to calculate HCTO. */
 		radio_tmr_end_capture();
 
 		return;
+
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+	} else {
+		struct pdu_adv_com_ext_adv *p;
+		struct ext_adv_hdr *h;
+		struct pdu_adv *pdu;
+
+		pdu = lll_adv_data_curr_get(lll);
+		p = (void *)&pdu->adv_ext_ind;
+		h = (void *)p->ext_hdr_adi_adv_data;
+
+		if ((pdu->type == PDU_ADV_TYPE_EXT_IND) && h->aux_ptr) {
+			radio_filter_disable();
+
+			lll_adv_aux_prepare(lll);
+
+			return;
+		}
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
 	}
 
 	radio_filter_disable();
@@ -593,34 +639,44 @@ static void isr_race(void *param)
 	radio_status_reset();
 }
 
-static void chan_prepare(struct lll_adv *lll)
+static struct pdu_adv *chan_prepare(struct lll_adv *lll)
 {
 	struct pdu_adv *pdu;
-	struct pdu_adv *scan_pdu;
-	uint8_t chan;
 	uint8_t upd = 0U;
+	uint8_t chan;
 
+	chan = find_lsb_set(lll->chan_map_curr);
+	LL_ASSERT(chan);
+
+	lll->chan_map_curr &= (lll->chan_map_curr - 1);
+
+	lll_chan_set(36 + chan);
+
+	/* FIXME: get latest only when primary PDU without Aux PDUs */
 	pdu = lll_adv_data_latest_get(lll, &upd);
-	scan_pdu = lll_adv_scan_rsp_latest_get(lll, &upd);
-
-#if defined(CONFIG_BT_CTLR_PRIVACY)
-	if (upd) {
-		/* Copy the address from the adv packet we will send into the
-		 * scan response.
-		 */
-		memcpy(&scan_pdu->scan_rsp.addr[0],
-		       &pdu->adv_ind.addr[0], BDADDR_SIZE);
-	}
-#else
-	ARG_UNUSED(scan_pdu);
-	ARG_UNUSED(upd);
-#endif /* !CONFIG_BT_CTLR_PRIVACY */
 
 	radio_pkt_tx_set(pdu);
 
 	if ((pdu->type != PDU_ADV_TYPE_NONCONN_IND) &&
 	    (!IS_ENABLED(CONFIG_BT_CTLR_ADV_EXT) ||
 	     (pdu->type != PDU_ADV_TYPE_EXT_IND))) {
+		struct pdu_adv *scan_pdu;
+
+		scan_pdu = lll_adv_scan_rsp_latest_get(lll, &upd);
+
+#if defined(CONFIG_BT_CTLR_PRIVACY)
+		if (upd) {
+			/* Copy the address from the adv packet we will send
+			 * into the scan response.
+			 */
+			memcpy(&scan_pdu->scan_rsp.addr[0],
+			       &pdu->adv_ind.addr[0], BDADDR_SIZE);
+		}
+#else
+		ARG_UNUSED(scan_pdu);
+		ARG_UNUSED(upd);
+#endif /* !CONFIG_BT_CTLR_PRIVACY */
+
 		radio_isr_set(isr_tx, lll);
 		radio_tmr_tifs_set(EVENT_IFS_US);
 		radio_switch_complete_and_rx(0);
@@ -629,14 +685,42 @@ static void chan_prepare(struct lll_adv *lll)
 		radio_switch_complete_and_disable();
 	}
 
-	chan = find_lsb_set(lll->chan_map_curr);
+	return pdu;
-	LL_ASSERT(chan);
-
-	lll->chan_map_curr &= (lll->chan_map_curr - 1);
-
-	lll_chan_set(36 + chan);
 }
 
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+static void aux_ptr_populate(struct pdu_adv *pdu, uint32_t start_us)
+{
+	struct pdu_adv_com_ext_adv *p;
+	struct ext_adv_aux_ptr *aux;
+	struct ext_adv_hdr *h;
+	uint8_t *ptr;
+
+	p = (void *)&pdu->adv_ext_ind;
+	h = (void *)p->ext_hdr_adi_adv_data;
+
+	if (!h->aux_ptr) {
+		return;
+	}
+
+	ptr = (uint8_t *)h + sizeof(*h);
+
+	if (h->adv_addr) {
+		ptr += BDADDR_SIZE;
+	}
+
+	if (h->adi) {
+		ptr += sizeof(struct ext_adv_adi);
+	}
+
+	aux = (void *)ptr;
+	aux->offs = (1000 - start_us) / 30;
+	if (aux->offs_units) {
+		aux->offs /= 10;
+	}
+}
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
+
 static inline int isr_rx_pdu(struct lll_adv *lll,
 			     uint8_t devmatch_ok, uint8_t devmatch_id,
 			     uint8_t irkmatch_ok, uint8_t irkmatch_id,

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.h

@@ -28,7 +28,7 @@ struct lll_adv {
 
 #if defined(CONFIG_BT_CTLR_ADV_EXT)
 	uint8_t phy_p:3;
-#endif /* !CONFIG_BT_CTLR_ADV_EXT */
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
 
 #if defined(CONFIG_BT_HCI_MESH_EXT)
 	uint8_t is_mesh:1;
@@ -41,6 +41,10 @@ struct lll_adv {
 	struct lll_adv_pdu adv_data;
 	struct lll_adv_pdu scan_rsp;
 
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+	struct lll_adv_pdu aux_data;
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
+
 #if defined(CONFIG_BT_CTLR_TX_PWR_DYNAMIC_CONTROL)
 	int8_t tx_pwr_lvl;
 #endif /* CONFIG_BT_CTLR_TX_PWR_DYNAMIC_CONTROL */
@@ -106,4 +110,22 @@ static inline struct pdu_adv *lll_adv_scan_rsp_peek(struct lll_adv *lll)
 	return (void *)lll->scan_rsp.pdu[lll->scan_rsp.last];
 }
 
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+static inline struct pdu_adv *lll_adv_aux_data_alloc(struct lll_adv *lll,
+						     uint8_t *idx)
+{
+	return lll_adv_pdu_alloc(&lll->aux_data, idx);
+}
+
+static inline void lll_adv_aux_data_enqueue(struct lll_adv *lll, uint8_t idx)
+{
+	lll_adv_pdu_enqueue(&lll->aux_data, idx);
+}
+
+static inline struct pdu_adv *lll_adv_aux_data_peek(struct lll_adv *lll)
+{
+	return (void *)lll->aux_data.pdu[lll->aux_data.last];
+}
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
+
 extern uint16_t ull_adv_lll_handle_get(struct lll_adv *lll);

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c

@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2018-2020 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stddef.h>
+
+#include <toolchain.h>
+#include <zephyr/types.h>
+
+#include "hal/ccm.h"
+#include "hal/radio.h"
+
+#include "util/util.h"
+#include "util/memq.h"
+
+#include "pdu.h"
+
+#include "lll.h"
+#include "lll_clock.h"
+#include "lll_adv.h"
+#include "lll_chan.h"
+
+#include "lll_internal.h"
+#include "lll_adv_internal.h"
+
+#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
+#define LOG_MODULE_NAME bt_ctlr_lll_adv_aux
+#include "common/log.h"
+#include <soc.h>
+#include "hal/debug.h"
+
+static void isr_done(void *param);
+static void isr_cleanup(void *param);
+static void isr_race(void *param);
+
+void lll_adv_aux_prepare(struct lll_adv *lll)
+{
+	struct pdu_adv_com_ext_adv *p;
+	struct ext_adv_aux_ptr *aux;
+	struct pdu_adv *pri, *sec;
+	struct ext_adv_hdr *h;
+	uint32_t start_us;
+	uint8_t *ptr;
+	uint8_t upd = 0U;
+
+	/* AUX_ADV_IND PDU buffer get */
+	/* FIXME: get latest only when primary PDU without Aux PDUs */
+	sec = lll_adv_aux_data_latest_get(lll, &upd);
+
+	radio_pkt_tx_set(sec);
+
+	/* Get reference to primary PDU aux_ptr */
+	pri = lll_adv_data_curr_get(lll);
+	p = (void *)&pri->adv_ext_ind;
+	h = (void *)p->ext_hdr_adi_adv_data;
+	ptr = (uint8_t *)h + sizeof(*h);
+
+	/* traverse through adv_addr, if present */
+	if (h->adv_addr) {
+		ptr += BDADDR_SIZE;
+	}
+
+	/* traverse through adi, if present */
+	if (h->adi) {
+		ptr += sizeof(struct ext_adv_adi);
+	}
+	aux = (void *)ptr;
+
+	/* Use channel idx in aux_ptr */
+	lll_chan_set(aux->chan_idx);
+
+	/* TODO: Based on adv_mode switch to Rx, if needed */
+	radio_isr_set(isr_done, lll);
+	radio_switch_complete_and_disable();
+
+	start_us = 1000;
+	radio_tmr_start_us(1, start_us);
+
+	/* capture end of Tx-ed PDU, used to calculate HCTO. */
+	radio_tmr_end_capture();
+
+#if defined(CONFIG_BT_CTLR_GPIO_PA_PIN)
+	radio_gpio_pa_setup();
+	radio_gpio_pa_lna_enable(start_us +
+				 radio_tx_ready_delay_get(0, 0) -
+				 CONFIG_BT_CTLR_GPIO_PA_OFFSET);
+#endif /* !CONFIG_BT_CTLR_GPIO_PA_PIN */
+}
+
+static void isr_done(void *param)
+{
+	/* TODO: MOVE to a common interface, isr_lll_radio_status? */
+	/* Clear radio status and events */
+	radio_status_reset();
+	radio_tmr_status_reset();
+	radio_filter_status_reset();
+	radio_ar_status_reset();
+	radio_rssi_status_reset();
+
+	if (IS_ENABLED(CONFIG_BT_CTLR_GPIO_PA_PIN) ||
+	    IS_ENABLED(CONFIG_BT_CTLR_GPIO_LNA_PIN)) {
+		radio_gpio_pa_lna_disable();
+	}
+	/* TODO: MOVE ^^ */
+
+	isr_cleanup(param);
+}
+
+static void isr_cleanup(void *param)
+{
+	int err;
+
+	radio_isr_set(isr_race, param);
+	radio_tmr_stop();
+
+	err = lll_hfclock_off();
+	LL_ASSERT(!err || err == -EBUSY);
+
+	lll_done(NULL);
+}
+
+static void isr_race(void *param)
+{
+	/* NOTE: lll_disable could have a race with ... */
+	radio_status_reset();
+}

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.h

@@ -0,0 +1,7 @@
+/*
+ * Copyright (c) 2018-2020 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+void lll_adv_aux_prepare(struct lll_adv *lll);

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_internal.h

@@ -43,3 +43,16 @@ static inline struct pdu_adv *lll_adv_scan_rsp_curr_get(struct lll_adv *lll)
 {
 	return (void *)lll->scan_rsp.pdu[lll->scan_rsp.first];
 }
+
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+static inline struct pdu_adv *lll_adv_aux_data_latest_get(struct lll_adv *lll,
+							  uint8_t *is_modified)
+{
+	return lll_adv_pdu_latest_get(&lll->aux_data, is_modified);
+}
+
+static inline struct pdu_adv *lll_adv_aux_data_curr_get(struct lll_adv *lll)
+{
+	return (void *)lll->aux_data.pdu[lll->aux_data.first];
+}
+#endif /* CONFIG_BT_CTLR_ADV_EXT */

subsys/bluetooth/controller/ll_sw/nrf.cmake

@@ -9,6 +9,10 @@ if(CONFIG_BT_LL_SW_SPLIT)
     zephyr_library_sources(
       ll_sw/nordic/lll/lll_adv.c
       )
+    zephyr_library_sources_ifdef(
+      CONFIG_BT_CTLR_ADV_EXT
+      ll_sw/nordic/lll/lll_adv_aux.c
+      )
   endif()
   if(CONFIG_BT_OBSERVER)
     zephyr_library_sources(

subsys/bluetooth/controller/ll_sw/ull_adv.c

@@ -90,6 +90,7 @@ uint8_t ll_adv_params_set(uint8_t handle, uint16_t evt_prop, uint32_t interval,
 				     PDU_ADV_TYPE_NONCONN_IND,
 				     PDU_ADV_TYPE_DIRECT_IND,
 				     PDU_ADV_TYPE_EXT_IND};
+	uint8_t is_pdu_type_changed = 0;
 #else /* !CONFIG_BT_CTLR_ADV_EXT */
 uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 		       uint8_t own_addr_type, uint8_t direct_addr_type,
@@ -137,7 +138,7 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 			 */
 			if (((evt_prop & 0x03) == 0x03) ||
 			    ((evt_prop & 0x0C) == 0x0C)) {
-				return 0x12; /* invalid HCI cmd param */
+				return BT_HCI_ERR_INVALID_PARAM;
 			}
 
 			adv_type = 0x05; /* PDU_ADV_TYPE_EXT_IND */
@@ -162,7 +163,15 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 
 	/* update the "current" primary adv data */
 	pdu = lll_adv_data_peek(&adv->lll);
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+	if (pdu->type != pdu_adv_type[adv_type]) {
+		pdu->type = pdu_adv_type[adv_type];
+
+		is_pdu_type_changed = 1;
+	}
+#else /* !CONFIG_BT_CTLR_ADV_EXT */
 	pdu->type = pdu_adv_type[adv_type];
+#endif /* !CONFIG_BT_CTLR_ADV_EXT */
 	pdu->rfu = 0;
 
 	if (IS_ENABLED(CONFIG_BT_CTLR_CHAN_SEL_2) &&
@@ -181,9 +190,9 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 		memcpy(&adv->id_addr, direct_addr, BDADDR_SIZE);
 	}
 #endif /* CONFIG_BT_CTLR_PRIVACY */
-	pdu->tx_addr = own_addr_type & 0x1;
+
-	pdu->rx_addr = 0;
 	if (pdu->type == PDU_ADV_TYPE_DIRECT_IND) {
+		pdu->tx_addr = own_addr_type & 0x1;
 		pdu->rx_addr = direct_addr_type;
 		memcpy(&pdu->direct_ind.tgt_addr[0], direct_addr, BDADDR_SIZE);
 		pdu->len = sizeof(struct pdu_adv_direct_ind);
@@ -205,7 +214,11 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 		p->adv_mode = evt_prop & 0x03;
 
 		/* Zero-init header flags */
-		*(uint8_t *)&_h = *(uint8_t *)h;
+		if (is_pdu_type_changed) {
+			*(uint8_t *)&_h = 0;
+		} else {
+			*(uint8_t *)&_h = *(uint8_t *)h;
+		}
 		*(uint8_t *)h = 0;
 
 		/* AdvA flag */
@@ -219,8 +232,12 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 			h->adv_addr = 1;
 
 			/* NOTE: AdvA is filled at enable */
+			pdu->tx_addr = own_addr_type & 0x1;
 			ptr += BDADDR_SIZE;
+		} else {
+			pdu->tx_addr = 0;
 		}
+		pdu->rx_addr = 0;
 
 		/* TODO: TargetA flag in primary channel PDU only for directed
 		 */
@@ -283,8 +300,7 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 				}
 			}
 
-			ptr--;
+			*--ptr = _tx_pwr;
-			*ptr = _tx_pwr;
 		}
 
 		/* No SyncInfo in primary channel PDU */
@@ -299,7 +315,10 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 			 * will be set in Advertiser Event.
 			 */
 			aux = (void *)ptr;
-			aux->phy = find_lsb_set(phy_s);
+			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;
 		}
 		adv->phy_s = phy_s;
 
@@ -325,7 +344,12 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type,
 #endif /* CONFIG_BT_CTLR_ADV_EXT */
 
 	} else if (pdu->len == 0) {
+		pdu->tx_addr = own_addr_type & 0x1;
+		pdu->rx_addr = 0;
 		pdu->len = BDADDR_SIZE;
+	} else {
+		pdu->tx_addr = own_addr_type & 0x1;
+		pdu->rx_addr = 0;
 	}
 
 	/* update the current scan data */

subsys/bluetooth/controller/ll_sw/ull_adv_aux.c

@@ -37,14 +37,13 @@ uint8_t *ll_adv_aux_random_addr_get(uint8_t handle, uint8_t *addr)
 uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, uint8_t len,
 			    uint8_t *data)
 {
-	struct pdu_adv_com_ext_adv *p, *_p;
+	struct pdu_adv_com_ext_adv *p, *_p, *s, *_s;
-	struct ext_adv_hdr *h, _h;
+	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;
+	uint8_t *_pp, *pp, *ps, *_ps;
 	struct ll_adv_set *adv;
-	uint8_t pdu_len, _pdu_len;
+	uint8_t ip, is;
-	struct pdu_adv *prev;
-	struct pdu_adv *pdu;
-	uint8_t *_ptr, *ptr;
-	uint8_t idx;
 
 	/* TODO: */
 
@@ -70,74 +69,159 @@ uint8_t ll_adv_aux_ad_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, ui
 	}
 
 	/* Do not update data if not extended advertising. */
-	prev = lll_adv_data_peek(&adv->lll);
+	_pri = lll_adv_data_peek(&adv->lll);
-	if (prev->type != PDU_ADV_TYPE_EXT_IND) {
+	if (_pri->type != PDU_ADV_TYPE_EXT_IND) {
 		/* Advertising Handle has not been created using
 		 * Set Extended Advertising Parameter command
 		 */
 		return BT_HCI_ERR_CMD_DISALLOWED;
 	}
 
-	/* Get reference to previous PDU data */
+	/* Get reference to previous primary PDU data */
-	_p = (void *)&prev->adv_ext_ind;
+	_p = (void *)&_pri->adv_ext_ind;
-	h = (void *)_p->ext_hdr_adi_adv_data;
+	hp = (void *)_p->ext_hdr_adi_adv_data;
-	*(uint8_t *)&_h = *(uint8_t *)h;
+	*(uint8_t *)&_hp = *(uint8_t *)hp;
-	_ptr = (uint8_t *)h + sizeof(*h);
+	_pp = (uint8_t *)hp + sizeof(*hp);
 
-	/* Get reference new PDU data buffer */
+	/* Get reference to new primary PDU data buffer */
-	pdu = lll_adv_data_alloc(&adv->lll, &idx);
+	pri = lll_adv_data_alloc(&adv->lll, &ip);
-	p = (void *)&pdu->adv_ext_ind;
+	pri->type = _pri->type;
+	pri->rfu = 0U;
+	pri->chan_sel = 0U;
+	p = (void *)&pri->adv_ext_ind;
 	p->adv_mode = _p->adv_mode;
-	h = (void *)p->ext_hdr_adi_adv_data;
+	hp = (void *)p->ext_hdr_adi_adv_data;
-	ptr = (uint8_t *)h + sizeof(*h);
+	pp = (uint8_t *)hp + sizeof(*hp);
-	*(uint8_t *)h = 0;
+	*(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 */
-	if (_h.adv_addr) {
-		_ptr += BDADDR_SIZE;
-	}
 	/* NOTE: as we will use auxiliary packet, we remove AdvA in primary
-	 * channel. i.e. Do nothing to add AdvA in the new PDU.
+	 * channel. i.e. Do nothing to add AdvA in the primary PDU.
 	 */
+	if (_hp.adv_addr) {
+		_pp += BDADDR_SIZE;
 
-	/* No TargetA in primary channel for undirected */
+		/* Prepare to add AdvA in secondary PDU */
-	/* No CTEInfo flag in primary channel 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 (_h.adi) {
+	if (_hp.adi) {
-		_ptr += sizeof(struct ext_adv_adi);
+		_pp += sizeof(struct ext_adv_adi);
 	}
-	h->adi = 1;
+	hp->adi = 1;
-	ptr += sizeof(struct ext_adv_adi);
+	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 (_h.aux_ptr) {
+	if (_hp.aux_ptr) {
-		_ptr += sizeof(struct ext_adv_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);
 	}
-	h->aux_ptr = 1;
-	ptr += sizeof(struct ext_adv_aux_ptr);
 
 	/* No SyncInfo flag in primary channel PDU */
 
 	/* Tx Power flag */
-	if (_h.tx_pwr) {
+	if (_hp.tx_pwr) {
-		_ptr++;
+		_pp++;
 
 		/* C1, Tx Power is optional on the LE 1M PHY, and reserved for
 		 * for future use on the LE Coded PHY.
 		 */
 		if (adv->lll.phy_p != BIT(2)) {
-			h->tx_pwr = 1;
+			hp->tx_pwr = 1;
-			ptr++;
+			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 */
-	_pdu_len = _ptr - (uint8_t *)_p;
+	_pri_len = _pp - (uint8_t *)_p;
-	pdu_len = ptr - (uint8_t *)p;
+	pri_len = pp - (uint8_t *)p;
-	p->ext_hdr_len = pdu_len - offsetof(struct pdu_adv_com_ext_adv,
+	p->ext_hdr_len = pri_len - offsetof(struct pdu_adv_com_ext_adv,
 					    ext_hdr_adi_adv_data);
-	pdu->len = pdu_len;
+
+	/* 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 */
+
+	/* set the secondary PDU len */
+	sec->len = sec_len + len;
+
+	/* Fill AdvData in secondary PDU */
+	memcpy(ps, data, len);
 
 	/* Start filling primary PDU payload based on flags */
 
@@ -146,56 +230,81 @@ 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 (h->tx_pwr) {
+	if (hp->tx_pwr) {
-		*--ptr = *--_ptr;
+		*--pp = *--_pp;
+	} else if (hs->tx_pwr) {
+		*--ps = *--_ps;
 	}
 
 	/* No SyncInfo in primary channel PDU */
 
 	/* AuxPtr */
-	if (_h.aux_ptr) {
+	if (_hp.aux_ptr) {
-		_ptr -= sizeof(struct ext_adv_aux_ptr);
+		_pp -= sizeof(struct ext_adv_aux_ptr);
 	}
 	{
 		struct ext_adv_aux_ptr *aux;
 
-		ptr -= sizeof(struct ext_adv_aux_ptr);
+		pp -= sizeof(struct ext_adv_aux_ptr);
 
-		/* NOTE: Channel Index, CA, Offset Units and AUX Offset will be
+		/* NOTE: Aux Offset will be set in advertiser LLL event */
-		 * set in Advertiser Event.
+		aux = (void *)pp;
-		 */
+		aux->chan_idx = 0; /* FIXME: implementation defined */
-		aux = (void *)ptr;
+		aux->ca = 0; /* FIXME: implementation defined */
-		aux->phy = find_lsb_set(adv->phy_s);
+		aux->offs_units = 0; /* FIXME: implementation defined */
+		aux->phy = find_lsb_set(adv->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->phy_s) - 1;
 	}
 
 	/* ADI */
 	{
-		struct ext_adv_adi *adi;
+		struct ext_adv_adi *ap, *as;
 		uint16_t did = UINT16_MAX;
 
-		ptr -= sizeof(struct ext_adv_adi);
+		pp -= sizeof(struct ext_adv_adi);
+		ps -= sizeof(struct ext_adv_adi);
 
-		adi = (void *)ptr;
+		ap = (void *)pp;
+		as = (void *)ps;
 
-		if (_h.adi) {
+		if (_hp.adi) {
 			struct ext_adv_adi *_adi;
 
-			_ptr -= sizeof(struct ext_adv_adi);
+			_pp -= sizeof(struct ext_adv_adi);
+			_ps -= sizeof(struct ext_adv_adi);
 
 			/* NOTE: memcpy shall handle overlapping buffers */
-			memcpy(ptr, _ptr, sizeof(struct ext_adv_adi));
+			memcpy(pp, _pp, sizeof(struct ext_adv_adi));
+			memcpy(ps, _ps, sizeof(struct ext_adv_adi));
 
-			_adi = (void *)_ptr;
+			_adi = (void *)_pp;
 			did = _adi->did;
 		} else {
-			adi->sid = adv->sid;
+			ap->sid = adv->sid;
+			as->sid = adv->sid;
 		}
 
-		if ((op == 0x04) || len || (_pdu_len != pdu_len)) {
+		if ((op == 0x04) || len || (_pri_len != pri_len)) {
 			did++;
 		}
 
-		adi->did = did;
+		ap->did = did;
+		as->did = did;
 	}
 
 	/* No CTEInfo field in primary channel PDU */
@@ -204,7 +313,25 @@ 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 */
 
-	lll_adv_data_enqueue(&adv->lll, idx);
+	/* 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);
 
 	return 0;
 }