From 92dc4f289551b0e61b019a74c288484a41dc715d Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Tue, 2 Feb 2021 13:41:43 +0100
Subject: [PATCH 1/9] Bluetooth: controller: Fix PKT_AC_US macro

Advertising channel packets do not have MIC, there's no need to have
extra parameter which always has to be set to 0 anyway.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 subsys/bluetooth/controller/ll_sw/pdu.h          | 2 +-
 subsys/bluetooth/controller/ll_sw/ull_scan_aux.c | 4 ++--
 subsys/bluetooth/controller/ll_sw/ull_sync.c     | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/pdu.h b/subsys/bluetooth/controller/ll_sw/pdu.h
index 21359d5939a3d..35b013b751b31 100644
--- a/subsys/bluetooth/controller/ll_sw/pdu.h
+++ b/subsys/bluetooth/controller/ll_sw/pdu.h
@@ -160,7 +160,7 @@
 
 #define PKT_US(octets, phy) PKT_DC_US((octets), (PDU_MIC_SIZE), (phy))
 
-#define PKT_AC_US(octets, mic, phy) PKT_DC_US((octets), (mic), (phy))
+#define PKT_AC_US(octets, phy) PKT_DC_US((octets), 0, (phy))
 
 #define PKT_BIS_US(octets, mic, phy) PKT_DC_US((octets), (mic), (phy))
 
diff --git a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c
index 3a59e48ba15ea..da9234c5f03bf 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c
@@ -269,7 +269,7 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx)
 	lll->chan = aux_ptr->chan_idx;
 	lll->phy = BIT(aux_ptr->phy);
 
-	aux_offset_us = ftr->radio_end_us - PKT_AC_US(pdu->len, 0, phy);
+	aux_offset_us = ftr->radio_end_us - PKT_AC_US(pdu->len, phy);
 	if (aux_ptr->offs_units) {
 		lll->window_size_us = 300U;
 	} else {
@@ -302,7 +302,7 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx)
 		HAL_TICKER_US_TO_TICKS(EVENT_OVERHEAD_START_US +
 				       ready_delay_us +
 				       PKT_AC_US(PDU_AC_EXT_PAYLOAD_SIZE_MAX,
-						 0, lll->phy) +
+						 lll->phy) +
 				       EVENT_OVERHEAD_END_US);
 
 	ticks_slot_offset = MAX(aux->ull.ticks_active_to_start,
diff --git a/subsys/bluetooth/controller/ll_sw/ull_sync.c b/subsys/bluetooth/controller/ll_sw/ull_sync.c
index 99d82371a8a7c..1355be891a455 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_sync.c
@@ -438,7 +438,7 @@ void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux,
 	sync_offset_us += (uint32_t)si->offs * lll->window_size_event_us;
 	/* offs_adjust may be 1 only if sync setup by LL_PERIODIC_SYNC_IND */
 	sync_offset_us += (si->offs_adjust ? OFFS_ADJUST_US : 0U);
-	sync_offset_us -= PKT_AC_US(pdu->len, 0, lll->phy);
+	sync_offset_us -= PKT_AC_US(pdu->len, lll->phy);
 	sync_offset_us -= EVENT_TICKER_RES_MARGIN_US;
 	sync_offset_us -= EVENT_JITTER_US;
 	sync_offset_us -= ready_delay_us;
@@ -455,7 +455,7 @@ void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux,
 		HAL_TICKER_US_TO_TICKS(EVENT_OVERHEAD_START_US +
 				       ready_delay_us +
 				       PKT_AC_US(PDU_AC_EXT_PAYLOAD_SIZE_MAX,
-						 0, lll->phy) +
+						 lll->phy) +
 				       EVENT_OVERHEAD_END_US);
 
 	ticks_slot_offset = MAX(sync->ull.ticks_active_to_start,

From 3b63792d622c472e4b6212169f7b1613248c4246 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Mon, 1 Feb 2021 16:03:06 +0100
Subject: [PATCH 2/9] Bluetooth: controller: Add helper to allocate new pdu_adv

This adds helper to always allocate advertising PDU either from memory
pool or pdu_free queue and does not reuse existing PDU in adv_pdu.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../controller/ll_sw/nordic/lll/lll_adv.c     | 85 +++++++++----------
 .../controller/ll_sw/nordic/lll/lll_adv_pdu.h |  1 +
 2 files changed, 41 insertions(+), 45 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
index e76f0cab9fdc8..dac02462812b7 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
@@ -51,7 +51,6 @@
 
 static int init_reset(void);
 
-static struct pdu_adv *adv_pdu_allocate(struct lll_adv_pdu *pdu, uint8_t last);
 #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
 static inline void adv_extra_data_release(struct lll_adv_pdu *pdu, int idx);
 static void *adv_extra_data_allocate(struct lll_adv_pdu *pdu, uint8_t last);
@@ -257,6 +256,7 @@ int lll_adv_data_release(struct lll_adv_pdu *pdu)
 struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx)
 {
 	uint8_t first, last;
+	void *p;
 
 	first = pdu->first;
 	last = pdu->last;
@@ -281,7 +281,45 @@ struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx)
 
 	*idx = last;
 
-	return adv_pdu_allocate(pdu, last);
+	p = (void *)pdu->pdu[last];
+	if (p) {
+		return p;
+	}
+
+	p = lll_adv_pdu_alloc_pdu_adv();
+
+	pdu->pdu[last] = (void *)p;
+
+	return p;
+}
+
+struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void)
+{
+	struct pdu_adv *p;
+	int err;
+
+	p = MFIFO_DEQUEUE_PEEK(pdu_free);
+	if (p) {
+		err = k_sem_take(&sem_pdu_free, K_NO_WAIT);
+		LL_ASSERT(!err);
+
+		MFIFO_DEQUEUE(pdu_free);
+
+		return p;
+	}
+
+	p = mem_acquire(&mem_pdu.free);
+	if (p) {
+		return p;
+	}
+
+	err = k_sem_take(&sem_pdu_free, K_FOREVER);
+	LL_ASSERT(!err);
+
+	p = MFIFO_DEQUEUE(pdu_free);
+	LL_ASSERT(p);
+
+	return p;
 }
 
 struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu,
@@ -607,49 +645,6 @@ static int init_reset(void)
 	return 0;
 }
 
-static struct pdu_adv *adv_pdu_allocate(struct lll_adv_pdu *pdu, uint8_t last)
-{
-	void *p;
-	int err;
-
-	p = (void *)pdu->pdu[last];
-	if (p) {
-		return p;
-	}
-
-	p = MFIFO_DEQUEUE_PEEK(pdu_free);
-	if (p) {
-		err = k_sem_take(&sem_pdu_free, K_NO_WAIT);
-		LL_ASSERT(!err);
-
-		MFIFO_DEQUEUE(pdu_free);
-		pdu->pdu[last] = (void *)p;
-
-		return p;
-	}
-
-	p = mem_acquire(&mem_pdu.free);
-	if (p) {
-		pdu->pdu[last] = (void *)p;
-
-		return p;
-	}
-
-	err = k_sem_take(&sem_pdu_free, K_FOREVER);
-	LL_ASSERT(!err);
-
-	p = MFIFO_DEQUEUE(pdu_free);
-	LL_ASSERT(p);
-	/* If !p then check initial value of sem_pdu_free. It must be the same
-	 * as number of elements in pdu_free store. This may not happen in
-	 * runtime.
-	 */
-
-	pdu->pdu[last] = (void *)p;
-
-	return p;
-}
-
 #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
 static void *adv_extra_data_allocate(struct lll_adv_pdu *pdu, uint8_t last)
 {
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
index 9926b21b80dfb..ef89d65865524 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
@@ -14,6 +14,7 @@ static inline void lll_adv_pdu_enqueue(struct lll_adv_pdu *pdu, uint8_t idx)
 }
 
 struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx);
+struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void);
 
 static inline struct pdu_adv *lll_adv_data_alloc(struct lll_adv *lll,
 						 uint8_t *idx)

From 5d4ed8e0d6738aaa0c069e469fe43c89060db1e2 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Thu, 18 Feb 2021 15:49:15 +0100
Subject: [PATCH 3/9] Bluetooth: controller: Optimize
 lll_adv_pdu_and_extra_data_alloc

This function is the same as lll_adv_pdu_alloc except it also allocates
extra data at the end - it can just use lll_adv_pdu_alloc call to avoid
extensive c&p.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../controller/ll_sw/nordic/lll/lll_adv.c     | 31 ++-----------------
 1 file changed, 3 insertions(+), 28 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
index dac02462812b7..0bdc7d5d1af14 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
@@ -410,38 +410,13 @@ struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu,
 						 void **extra_data,
 						 uint8_t *idx)
 {
-	uint8_t first, last;
 	struct pdu_adv *p;
-
-	first = pdu->first;
-	last = pdu->last;
-	if (first == last) {
-		last++;
-		if (last == DOUBLE_BUFFER_SIZE) {
-			last = 0U;
-		}
-	} else {
-		uint8_t first_latest;
-
-		pdu->last = first;
-		cpu_dmb();
-		first_latest = pdu->first;
-		if (first_latest != first) {
-			last++;
-			if (last == DOUBLE_BUFFER_SIZE) {
-				last = 0U;
-			}
-		}
-	}
-
-	*idx = last;
-
-	p = adv_pdu_allocate(pdu, last);
+	p = lll_adv_pdu_alloc(pdu, idx);
 
 	if (extra_data) {
-		*extra_data = adv_extra_data_allocate(pdu, last);
+		*extra_data = adv_extra_data_allocate(pdu, *idx);
 	} else {
-		if (adv_extra_data_free(pdu, last)) {
+		if (adv_extra_data_free(pdu, *idx)) {
 			/* There is no release of memory allocated by
 			 * adv_pdu_allocate because there is no memory leak.
 			 * If caller can recover from this error and subsequent

From 47dd05fb0107d8464dd5f09cbde6352d0c6a7174 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Wed, 24 Feb 2021 16:43:58 +0100
Subject: [PATCH 4/9] Bluetooth: controller: Add support for linked adv PDUs

This adds support to allow advertising PDUs to be linked which is
required to send advertising trains, i.e. AUX_CHAIN_IND.

PDUs are linked with a simple single-linked list, the pointer to next
PDU is stored at the very end of PDU buffer. This prevents it from
being overwritten if PDU is modified and allows for build time offset
calculation.

There are few helpers added to make handling easier, e.g.:
- get next linked PDU
- get last linked PDU
- link one PDU to another
- link PDU at the end
- release all linked PDUs (except for 1st)

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../bluetooth/controller/Kconfig.ll_sw_split  |  7 ++
 subsys/bluetooth/controller/hci/hci.c         |  1 +
 subsys/bluetooth/controller/ll_sw/ll_addr.c   |  1 +
 subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c |  1 +
 .../controller/ll_sw/nordic/lll/lll_adv.c     | 68 ++++++++++++++++---
 .../controller/ll_sw/nordic/lll/lll_adv_aux.c |  1 +
 .../controller/ll_sw/nordic/lll/lll_adv_pdu.h | 47 +++++++++++++
 .../ll_sw/nordic/lll/lll_adv_sync.c           |  1 +
 .../bluetooth/controller/ll_sw/ull_master.c   |  1 +
 subsys/bluetooth/controller/ll_sw/ull_slave.c |  1 +
 10 files changed, 119 insertions(+), 10 deletions(-)

diff --git a/subsys/bluetooth/controller/Kconfig.ll_sw_split b/subsys/bluetooth/controller/Kconfig.ll_sw_split
index e493b1c873e26..7f67727df2115 100644
--- a/subsys/bluetooth/controller/Kconfig.ll_sw_split
+++ b/subsys/bluetooth/controller/Kconfig.ll_sw_split
@@ -165,6 +165,13 @@ config BT_CTLR_ADV_SYNC_SET
 	help
 	  Maximum supported periodic advertising sets.
 
+config BT_CTLR_ADV_PDU_LINK
+	bool "Enable linking of advertising PDU trains"
+	help
+	  Enables extra space in each advertising PDU to allow linking PDUs. This
+	  is required to enable advertising data trains (i.e. transmission of
+	  AUX_CHAIN_IND).
+
 config BT_CTLR_ADV_DATA_BUF_MAX
 	int "Advertising Data Maximum Buffers"
 	depends on BT_BROADCASTER
diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c
index b435158214d37..afd882d88f447 100644
--- a/subsys/bluetooth/controller/hci/hci.c
+++ b/subsys/bluetooth/controller/hci/hci.c
@@ -26,6 +26,7 @@
 
 #include "util/util.h"
 #include "util/memq.h"
+#include "util/mem.h"
 
 #include "hal/ecb.h"
 #include "hal/ccm.h"
diff --git a/subsys/bluetooth/controller/ll_sw/ll_addr.c b/subsys/bluetooth/controller/ll_sw/ll_addr.c
index b2273e0a48d39..9fc10982f2c8a 100644
--- a/subsys/bluetooth/controller/ll_sw/ll_addr.c
+++ b/subsys/bluetooth/controller/ll_sw/ll_addr.c
@@ -14,6 +14,7 @@
 
 #include "util/util.h"
 #include "util/memq.h"
+#include "util/mem.h"
 
 #include "pdu.h"
 
diff --git a/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c b/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c
index 46f22c66222b0..3d764c139ef1f 100644
--- a/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c
+++ b/subsys/bluetooth/controller/ll_sw/ll_tx_pwr.c
@@ -16,6 +16,7 @@
 
 #include "util/util.h"
 #include "util/memq.h"
+#include "util/mem.h"
 
 #include "pdu.h"
 
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
index 0bdc7d5d1af14..ea960b5a6c519 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c
@@ -97,8 +97,7 @@ static inline bool isr_rx_ci_adva_check(uint8_t tx_addr, uint8_t *addr,
 #define BT_CTLR_ADV_SYNC_SET 0
 #endif
 
-#define PDU_MEM_SIZE       MROUND(PDU_AC_LL_HEADER_SIZE + \
-				  PDU_AC_PAYLOAD_SIZE_MAX)
+#define PDU_MEM_SIZE       PDU_ADV_MEM_SIZE
 #define PDU_MEM_COUNT_MIN  (BT_CTLR_ADV_SET + \
 			    (BT_CTLR_ADV_SET * PAYLOAD_FRAG_COUNT) + \
 			    (BT_CTLR_ADV_AUX_SET * PAYLOAD_FRAG_COUNT) + \
@@ -305,11 +304,17 @@ struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void)
 
 		MFIFO_DEQUEUE(pdu_free);
 
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+		PDU_ADV_NEXT_PTR(p) = NULL;
+#endif
 		return p;
 	}
 
 	p = mem_acquire(&mem_pdu.free);
 	if (p) {
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+		PDU_ADV_NEXT_PTR(p) = NULL;
+#endif
 		return p;
 	}
 
@@ -319,9 +324,33 @@ struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void)
 	p = MFIFO_DEQUEUE(pdu_free);
 	LL_ASSERT(p);
 
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+	PDU_ADV_NEXT_PTR(p) = NULL;
+#endif
 	return p;
 }
 
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+void lll_adv_pdu_release(struct pdu_adv *pdu)
+{
+	mem_release(pdu, &mem_pdu.free);
+}
+
+void lll_adv_pdu_linked_release_all(struct pdu_adv *pdu_first)
+{
+	struct pdu_adv *pdu = pdu_first;
+
+	while (pdu) {
+		struct pdu_adv *pdu_next;
+
+		pdu_next = PDU_ADV_NEXT_PTR(pdu);
+		PDU_ADV_NEXT_PTR(pdu) = NULL;
+		lll_adv_pdu_release(pdu);
+		pdu = pdu_next;
+	}
+}
+#endif
+
 struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu,
 				       uint8_t *is_modified)
 {
@@ -333,11 +362,34 @@ struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu,
 		uint8_t pdu_idx;
 		void *p;
 
-		if (!MFIFO_ENQUEUE_IDX_GET(pdu_free, &free_idx)) {
-			return NULL;
-		}
-
 		pdu_idx = first;
+		p = pdu->pdu[pdu_idx];
+
+		do {
+			void *next;
+
+			/* Store partial list in current data index if there is
+			 * no free slot in mfifo. It can be released on next
+			 * switch attempt (on next event).
+			 */
+			if (!MFIFO_ENQUEUE_IDX_GET(pdu_free, &free_idx)) {
+				pdu->pdu[pdu_idx] = p;
+				return NULL;
+			}
+
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+			next = lll_adv_pdu_linked_next_get(p);
+#else
+			next = NULL;
+#endif
+
+			MFIFO_BY_IDX_ENQUEUE(pdu_free, free_idx, p);
+			k_sem_give(&sem_pdu_free);
+
+			p = next;
+		} while (p);
+
+		pdu->pdu[pdu_idx] = NULL;
 
 		first += 1U;
 		if (first == DOUBLE_BUFFER_SIZE) {
@@ -346,11 +398,7 @@ struct pdu_adv *lll_adv_pdu_latest_get(struct lll_adv_pdu *pdu,
 		pdu->first = first;
 		*is_modified = 1U;
 
-		p = pdu->pdu[pdu_idx];
 		pdu->pdu[pdu_idx] = NULL;
-
-		MFIFO_BY_IDX_ENQUEUE(pdu_free, free_idx, p);
-		k_sem_give(&sem_pdu_free);
 	}
 
 	return (void *)pdu->pdu[first];
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c
index 07ad2128d52ac..e298fdb38f80a 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_aux.c
@@ -17,6 +17,7 @@
 #include "hal/ticker.h"
 
 #include "util/util.h"
+#include "util/mem.h"
 #include "util/memq.h"
 
 #include "pdu.h"
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
index ef89d65865524..8b1e20f7e6196 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
@@ -4,6 +4,18 @@
  * SPDX-License-Identifier: Apache-2.0
  */
 
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+#define PDU_ADV_MEM_SIZE       MROUND(PDU_AC_LL_HEADER_SIZE + \
+				      PDU_AC_PAYLOAD_SIZE_MAX + \
+				      sizeof(uintptr_t))
+#define PDU_ADV_NEXT_PTR(p)    *(struct pdu_adv **)((uint8_t *)(p) + \
+						    PDU_ADV_MEM_SIZE - \
+						    sizeof(uintptr_t))
+#else
+#define PDU_ADV_MEM_SIZE       MROUND(PDU_AC_LL_HEADER_SIZE + \
+				      PDU_AC_PAYLOAD_SIZE_MAX)
+#endif
+
 int lll_adv_data_init(struct lll_adv_pdu *pdu);
 int lll_adv_data_reset(struct lll_adv_pdu *pdu);
 int lll_adv_data_release(struct lll_adv_pdu *pdu);
@@ -124,3 +136,38 @@ static inline struct pdu_adv *lll_adv_sync_data_peek(struct lll_adv_sync *lll,
 }
 #endif /* CONFIG_BT_CTLR_ADV_PERIODIC */
 #endif /* CONFIG_BT_CTLR_ADV_EXT */
+
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+/* Release single PDU, shall only be called from ULL */
+void lll_adv_pdu_release(struct pdu_adv *pdu);
+/* Release PDU and all linked PDUs, shall only be called from ULL */
+void lll_adv_pdu_linked_release_all(struct pdu_adv *pdu_first);
+
+static inline struct pdu_adv *lll_adv_pdu_linked_next_get(struct pdu_adv *pdu)
+{
+	return PDU_ADV_NEXT_PTR(pdu);
+}
+
+static inline struct pdu_adv *lll_adv_pdu_linked_last_get(struct pdu_adv *pdu)
+{
+	while (PDU_ADV_NEXT_PTR(pdu)) {
+		pdu = PDU_ADV_NEXT_PTR(pdu);
+	}
+	return pdu;
+}
+
+static inline void lll_adv_pdu_linked_append(struct pdu_adv *pdu,
+					     struct pdu_adv *prev)
+{
+	PDU_ADV_NEXT_PTR(prev) = pdu;
+}
+
+static inline void lll_adv_pdu_linked_append_end(struct pdu_adv *pdu,
+						 struct pdu_adv *first)
+{
+	struct pdu_adv *last;
+
+	last = lll_adv_pdu_linked_last_get(first);
+	lll_adv_pdu_linked_append(pdu, last);
+}
+#endif
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
index 0813dd7043ca5..777bff0a29dce 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
@@ -15,6 +15,7 @@
 #include "hal/radio_df.h"
 
 #include "util/util.h"
+#include "util/mem.h"
 #include "util/memq.h"
 
 #include "pdu.h"
diff --git a/subsys/bluetooth/controller/ll_sw/ull_master.c b/subsys/bluetooth/controller/ll_sw/ull_master.c
index 54e7c00b3c10f..705e19f2a1209 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_master.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_master.c
@@ -11,6 +11,7 @@
 
 #include "util/util.h"
 #include "util/memq.h"
+#include "util/mem.h"
 #include "util/mayfly.h"
 
 #include "hal/cpu.h"
diff --git a/subsys/bluetooth/controller/ll_sw/ull_slave.c b/subsys/bluetooth/controller/ll_sw/ull_slave.c
index f3353ed8efaee..bf30cbe0f5b14 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_slave.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_slave.c
@@ -11,6 +11,7 @@
 
 #include "util/util.h"
 #include "util/memq.h"
+#include "util/mem.h"
 #include "util/mayfly.h"
 
 #include "hal/cpu.h"

From c033f1ae7a6310b4e3445eba59a48535986a4865 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Tue, 2 Feb 2021 15:37:05 +0100
Subject: [PATCH 5/9] Bluetooth: controller: Enable back-to-back chaining for
 periodic adv

This enables chaining ota for periodic advertising. AUX_CHAIN_IND PDUs
will be sent automatically if AuxPtr is detected in preceding PDU.
AuxPtr offset is always set to achieve minimal required frame spacing,
i.e. 300us (T_mafs). AuxPtr in all PDUs in advertising train are
updated on enqueue since PDU spacing is already known at that time so
we do not need to waste time in LLL.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../bluetooth/controller/Kconfig.ll_sw_split  |  20 ++
 subsys/bluetooth/controller/ll_sw/lll_adv.h   |   3 +
 .../controller/ll_sw/nordic/lll/lll_adv_pdu.h |   4 +
 .../ll_sw/nordic/lll/lll_adv_sync.c           | 174 +++++++++++++++++-
 .../controller/ll_sw/nordic/lll/lll_scan.c    |   2 +-
 .../ll_sw/nordic/lll/lll_scan_aux.c           |   6 +-
 6 files changed, 197 insertions(+), 12 deletions(-)

diff --git a/subsys/bluetooth/controller/Kconfig.ll_sw_split b/subsys/bluetooth/controller/Kconfig.ll_sw_split
index 7f67727df2115..a2f53f1a6947b 100644
--- a/subsys/bluetooth/controller/Kconfig.ll_sw_split
+++ b/subsys/bluetooth/controller/Kconfig.ll_sw_split
@@ -172,6 +172,26 @@ config BT_CTLR_ADV_PDU_LINK
 	  is required to enable advertising data trains (i.e. transmission of
 	  AUX_CHAIN_IND).
 
+config BT_CTLR_ADV_SYNC_PDU_BACK2BACK
+	bool "Enable back-to-back transmission of periodic advertising trains"
+	depends on BT_CTLR_ADV_PERIODIC
+	select BT_CTLR_ADV_PDU_LINK
+	help
+	  Enables transmission of AUX_CHAIN_IND in periodic advertising train by
+	  sending each AUX_CHAIN_IND one after another back-to-back.
+	  Note, consecutive AUX_CHAIN_IND packets are not scheduled but sent at
+	  a constant offset on a best effort basis. This means advertising train can
+	  be preempted by other event at any time.
+
+config BT_CTLR_ADV_SYNC_PDU_BACK2BACK_AFS
+	int "AUX Frame Space for back-to-back transmission of periodic advertising trains"
+	depends on BT_CTLR_ADV_SYNC_PDU_BACK2BACK
+	default 300
+	range 300 1000
+	help
+	  Specific AUX Frame Space to be used for back-to-back transmission of
+	  periodic advertising trains. Time specified in microseconds.
+
 config BT_CTLR_ADV_DATA_BUF_MAX
 	int "Advertising Data Maximum Buffers"
 	depends on BT_BROADCASTER
diff --git a/subsys/bluetooth/controller/ll_sw/lll_adv.h b/subsys/bluetooth/controller/ll_sw/lll_adv.h
index 5baaec685616a..fc01cffaf25af 100644
--- a/subsys/bluetooth/controller/ll_sw/lll_adv.h
+++ b/subsys/bluetooth/controller/ll_sw/lll_adv.h
@@ -27,6 +27,9 @@ struct lll_adv_sync {
 	uint32_t ticks_offset;
 
 	struct lll_adv_pdu data;
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+	struct pdu_adv *last_pdu;
+#endif /* CONFIG_BT_CTLR_ADV_PDU_LINK */
 
 #if defined(CONFIG_BT_CTLR_ADV_ISO)
 	struct lll_adv_iso *iso;
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
index 8b1e20f7e6196..3d314a12668b7 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h
@@ -91,6 +91,10 @@ static inline struct pdu_adv *lll_adv_aux_data_curr_get(struct lll_adv_aux *lll)
 #if defined(CONFIG_BT_CTLR_ADV_PERIODIC)
 int lll_adv_and_extra_data_release(struct lll_adv_pdu *pdu);
 
+#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK)
+void lll_adv_sync_pdu_b2b_update(struct lll_adv_sync *lll, uint8_t idx);
+#endif
+
 struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu,
 						 void **extra_data,
 						 uint8_t *idx);
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
index 777bff0a29dce..59f85e389aef2 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_sync.c
@@ -33,6 +33,7 @@
 #include "lll_internal.h"
 #include "lll_adv_internal.h"
 #include "lll_tim_internal.h"
+#include "lll_prof_internal.h"
 #include "lll_df_internal.h"
 
 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
@@ -40,10 +41,21 @@
 #include "common/log.h"
 #include "hal/debug.h"
 
+#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK)
+#define ADV_SYNC_PDU_B2B_AFS  (CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK_AFS)
+#endif
+
 static int init_reset(void);
 static int prepare_cb(struct lll_prepare_param *p);
 static void abort_cb(struct lll_prepare_param *prepare_param, void *param);
 static void isr_done(void *param);
+#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK)
+static void isr_tx(void *param);
+static void pdu_b2b_update(struct lll_adv_sync *lll, struct pdu_adv *pdu);
+static void pdu_b2b_aux_ptr_update(struct pdu_adv *pdu, uint8_t phy,
+				   uint8_t flags, uint8_t chan_idx,
+				   uint32_t tifs);
+#endif /* CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK */
 
 int lll_adv_sync_init(void)
 {
@@ -148,6 +160,15 @@ static int prepare_cb(struct lll_prepare_param *p)
 	pdu = lll_adv_sync_data_latest_get(lll, &extra_data, &upd);
 	LL_ASSERT(pdu);
 
+#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK)
+	if (upd) {
+		/* AuxPtr offsets for b2b TX are fixed for given chain so we can
+		 * calculate them here in advance.
+		 */
+		pdu_b2b_update(lll, pdu);
+	}
+#endif
+
 #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
 	if (extra_data) {
 		df_cfg = (struct lll_df_adv_cfg *)extra_data;
@@ -162,16 +183,26 @@ static int prepare_cb(struct lll_prepare_param *p)
 
 	radio_pkt_tx_set(pdu);
 
-	/* TODO: chaining */
-	radio_isr_set(isr_done, lll);
+#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK)
+	if (pdu->adv_ext_ind.ext_hdr_len && pdu->adv_ext_ind.ext_hdr.aux_ptr) {
+		lll->last_pdu = pdu;
 
+		radio_isr_set(isr_tx, lll);
+		radio_tmr_tifs_set(ADV_SYNC_PDU_B2B_AFS);
+		radio_switch_complete_and_b2b_tx(phy_s, 0, phy_s, 0);
+#else
+	if (0) {
+#endif /* CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK */
+	} else {
+		radio_isr_set(isr_done, lll);
 #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
-	if (df_cfg) {
-		radio_switch_complete_and_phy_end_disable();
-	} else
+		if (df_cfg) {
+			radio_switch_complete_and_phy_end_disable();
+		} else
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
-	{
-		radio_switch_complete_and_disable();
+		{
+			radio_switch_complete_and_disable();
+		}
 	}
 
 	ticks_at_event = p->ticks_at_expire;
@@ -256,3 +287,132 @@ static void isr_done(void *param)
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 	lll_isr_done(lll);
 }
+
+#if defined(CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK)
+static void isr_tx(void *param)
+{
+	struct lll_adv_sync *lll_sync;
+	struct pdu_adv *pdu;
+	struct lll_adv *lll;
+
+	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
+		lll_prof_latency_capture();
+	}
+
+	/* Clear radio tx status and events */
+	lll_isr_tx_status_reset();
+
+	lll_sync = param;
+	lll = lll_sync->adv;
+
+	/* TODO: do not hardcode to single value */
+	lll_chan_set(0);
+
+	pdu = lll_adv_pdu_linked_next_get(lll_sync->last_pdu);
+	LL_ASSERT(pdu);
+	lll_sync->last_pdu = pdu;
+
+	/* setup tIFS switching */
+	if (pdu->adv_ext_ind.ext_hdr_len && pdu->adv_ext_ind.ext_hdr.aux_ptr) {
+		radio_tmr_tifs_set(ADV_SYNC_PDU_B2B_AFS);
+		radio_isr_set(isr_tx, lll_sync);
+		radio_switch_complete_and_b2b_tx(lll->phy_s, 0, lll->phy_s, 0);
+	} else {
+		radio_isr_set(lll_isr_done, lll);
+#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+		if (lll_sync->cte_started) {
+			radio_switch_complete_and_phy_end_disable();
+		} else
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
+		{
+			radio_switch_complete_and_disable();
+		}
+	}
+
+	radio_pkt_tx_set(pdu);
+
+	/* assert if radio packet ptr is not set and radio started rx */
+	LL_ASSERT(!radio_is_ready());
+
+	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
+		lll_prof_cputime_capture();
+	}
+
+	/* capture end of AUX_SYNC_IND/AUX_CHAIN_IND PDU, used for calculating
+	 * next PDU timestamp.
+	 */
+	radio_tmr_end_capture();
+
+#if defined(CONFIG_BT_CTLR_GPIO_LNA_PIN)
+	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
+		/* PA/LNA enable is overwriting packet end used in ISR
+		 * profiling, hence back it up for later use.
+		 */
+		lll_prof_radio_end_backup();
+	}
+
+	radio_gpio_lna_setup();
+	radio_gpio_pa_lna_enable(radio_tmr_tifs_base_get() +
+				 ADV_SYNC_PDU_B2B_AFS - 4 -
+				 radio_tx_chain_delay_get(lll->phy_s, 0) -
+				 CONFIG_BT_CTLR_GPIO_LNA_OFFSET);
+#endif /* CONFIG_BT_CTLR_GPIO_LNA_PIN */
+
+	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
+		lll_prof_send();
+	}
+}
+
+static void pdu_b2b_update(struct lll_adv_sync *lll, struct pdu_adv *pdu)
+{
+	while (pdu) {
+		pdu_b2b_aux_ptr_update(pdu, lll->adv->phy_s, 0, 0,
+				       ADV_SYNC_PDU_B2B_AFS);
+		pdu = lll_adv_pdu_linked_next_get(pdu);
+	}
+}
+
+static void pdu_b2b_aux_ptr_update(struct pdu_adv *pdu, uint8_t phy,
+				   uint8_t flags, uint8_t chan_idx,
+				   uint32_t tifs)
+{
+	struct pdu_adv_com_ext_adv *com_hdr;
+	struct pdu_adv_ext_hdr *hdr;
+	struct pdu_adv_aux_ptr *aux;
+	uint32_t offs;
+	uint8_t *dptr;
+
+	com_hdr = &pdu->adv_ext_ind;
+	hdr = &com_hdr->ext_hdr;
+	/* Skip flags */
+	dptr = hdr->data;
+
+	if (!com_hdr->ext_hdr_len || !hdr->aux_ptr) {
+		return;
+	}
+
+	LL_ASSERT(!hdr->adv_addr);
+	LL_ASSERT(!hdr->tgt_addr);
+
+	if (hdr->cte_info) {
+		dptr++;
+	}
+
+	LL_ASSERT(!hdr->adi);
+
+	/* Update AuxPtr */
+	aux = (void *)dptr;
+	offs = PKT_AC_US(pdu->len, phy) + tifs;
+	offs = offs / OFFS_UNIT_30_US;
+	if ((offs >> 13) != 0) {
+		aux->offs = offs / (OFFS_UNIT_300_US / OFFS_UNIT_30_US);
+		aux->offs_units = 1U;
+	} else {
+		aux->offs = offs;
+		aux->offs_units = 0U;
+	}
+	aux->chan_idx = chan_idx;
+	aux->ca = 0;
+	aux->phy = find_lsb_set(phy) - 1;
+}
+#endif /* CONFIG_BT_CTLR_ADV_SYNC_PDU_BACK2BACK */
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c
index 0747008022e4b..0a94f09aa79f4 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c
@@ -146,7 +146,7 @@ void lll_scan_prepare_connect_req(struct lll_scan *lll, struct pdu_adv *pdu_tx,
 
 	conn_interval_us = (uint32_t)lll_conn->interval * CONN_INT_UNIT_US;
 	conn_offset_us = radio_tmr_end_get() + EVENT_IFS_US +
-			 PKT_AC_US(sizeof(struct pdu_adv_connect_ind), 0,
+			 PKT_AC_US(sizeof(struct pdu_adv_connect_ind),
 				   phy == PHY_LEGACY ? PHY_1M : phy);
 
 	/* Add transmitWindowDelay to default calculated connection offset:
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c
index fa843c3fbde05..f55eed1cc9b67 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c
@@ -434,11 +434,9 @@ static int isr_rx_pdu(struct lll_scan_aux *lll, uint8_t devmatch_ok,
 		ull = HDR_LLL2ULL(lll_scan);
 		if (pdu_end_us > (HAL_TICKER_TICKS_TO_US(ull->ticks_slot) -
 				  EVENT_IFS_US -
-				  PKT_AC_US(aux_connect_req_len, 0,
-					    lll->phy) -
+				  PKT_AC_US(aux_connect_req_len, lll->phy) -
 				  EVENT_IFS_US -
-				  PKT_AC_US(aux_connect_rsp_len, 0,
-					    lll->phy) -
+				  PKT_AC_US(aux_connect_rsp_len, lll->phy) -
 				  EVENT_OVERHEAD_START_US -
 				  EVENT_TICKER_RES_MARGIN_US)) {
 			return -ETIME;

From a5efa4abd7658ed62db44628be10d4043f1d2989 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Mon, 1 Mar 2021 15:29:21 +0100
Subject: [PATCH 6/9] Bluetooth: controller: Add helper to initialize adv PDU
 from scratch

This adds adv_sync_pdu_init helper which initializes pdu_adv buffer
with contents of AUX_SYNC_IND/AUX_CHAIN_IND PDU. Extended header flags
can be specified to reserve required space for corresponding fields if
necessary.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../controller/ll_sw/ull_adv_internal.h       |  3 +
 .../bluetooth/controller/ll_sw/ull_adv_sync.c | 68 ++++++++++++-------
 2 files changed, 47 insertions(+), 24 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
index bcefaa5c357c5..88642b0dd9800 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
@@ -47,9 +47,12 @@ const uint8_t *ull_adv_pdu_update_addrs(struct ll_adv_set *adv,
 #if defined(CONFIG_BT_CTLR_ADV_EXT)
 
 #define ULL_ADV_PDU_HDR_FIELD_ADVA      BIT(0)
+#define ULL_ADV_PDU_HDR_FIELD_TARGETA   BIT(1)
 #define ULL_ADV_PDU_HDR_FIELD_CTE_INFO  BIT(2)
+#define ULL_ADV_PDU_HDR_FIELD_ADI       BIT(3)
 #define ULL_ADV_PDU_HDR_FIELD_AUX_PTR   BIT(4)
 #define ULL_ADV_PDU_HDR_FIELD_SYNC_INFO BIT(5)
+#define ULL_ADV_PDU_HDR_FIELD_TX_POWER  BIT(7)
 #define ULL_ADV_PDU_HDR_FIELD_AD_DATA   BIT(8)
 
 /* Helper type to store data for extended advertising
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
index 8f3acd51a42b3..6aa32b2e78682 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
@@ -77,6 +77,49 @@ static void ticker_op_cb(uint32_t status, void *param);
 static struct ll_adv_sync_set ll_adv_sync_pool[CONFIG_BT_CTLR_ADV_SYNC_SET];
 static void *adv_sync_free;
 
+static void adv_sync_pdu_init(struct pdu_adv *pdu, uint8_t ext_hdr_flags)
+{
+	struct pdu_adv_com_ext_adv *com_hdr;
+	struct pdu_adv_ext_hdr *ext_hdr;
+	uint8_t *dptr;
+	uint8_t len;
+
+	pdu->type = PDU_ADV_TYPE_AUX_SYNC_IND;
+	pdu->rfu = 0U;
+	pdu->chan_sel = 0U;
+
+	pdu->tx_addr = 0U;
+	pdu->rx_addr = 0U;
+
+	com_hdr = &pdu->adv_ext_ind;
+	/* Non-connectable and Non-scannable adv mode */
+	com_hdr->adv_mode = 0U;
+
+	ext_hdr = &com_hdr->ext_hdr;
+	*(uint8_t *)ext_hdr = ext_hdr_flags;
+	dptr = ext_hdr->data;
+
+	LL_ASSERT(!(ext_hdr_flags & (ULL_ADV_PDU_HDR_FIELD_ADVA |
+				     ULL_ADV_PDU_HDR_FIELD_TARGETA |
+				     ULL_ADV_PDU_HDR_FIELD_ADI |
+				     ULL_ADV_PDU_HDR_FIELD_SYNC_INFO)));
+
+	if (ext_hdr_flags & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) {
+		dptr += sizeof(struct pdu_cte_info);
+	}
+	if (ext_hdr_flags & ULL_ADV_PDU_HDR_FIELD_AUX_PTR) {
+		dptr += sizeof(struct pdu_adv_aux_ptr);
+	}
+	if (ext_hdr_flags & ULL_ADV_PDU_HDR_FIELD_TX_POWER) {
+		dptr += sizeof(uint8_t);
+	}
+
+	/* Calc tertiary PDU len */
+	len = ull_adv_aux_hdr_len_calc(com_hdr, &dptr);
+	ull_adv_aux_hdr_len_fill(com_hdr, len);
+
+	pdu->len = len;
+}
 uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
 {
 	struct lll_adv_sync *lll_sync;
@@ -91,12 +134,8 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
 
 	lll_sync = adv->lll.sync;
 	if (!lll_sync) {
-		struct pdu_adv_com_ext_adv *ter_com_hdr;
-		struct pdu_adv_ext_hdr *ter_hdr;
 		struct pdu_adv *ter_pdu;
 		struct lll_adv *lll;
-		uint8_t *ter_dptr;
-		uint8_t ter_len;
 		int err;
 
 		sync = sync_acquire();
@@ -135,26 +174,7 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
 		sync->is_started = 0U;
 
 		ter_pdu = lll_adv_sync_data_peek(lll_sync, NULL);
-		ter_pdu->type = PDU_ADV_TYPE_AUX_SYNC_IND;
-		ter_pdu->rfu = 0U;
-		ter_pdu->chan_sel = 0U;
-
-		ter_pdu->tx_addr = 0U;
-		ter_pdu->rx_addr = 0U;
-
-		ter_com_hdr = (void *)&ter_pdu->adv_ext_ind;
-		ter_hdr = (void *)ter_com_hdr->ext_hdr_adv_data;
-		ter_dptr = ter_hdr->data;
-		*(uint8_t *)ter_hdr = 0U;
-
-		/* Non-connectable and Non-scannable adv mode */
-		ter_com_hdr->adv_mode = 0U;
-
-		/* Calc tertiary PDU len */
-		ter_len = ull_adv_aux_hdr_len_calc(ter_com_hdr, &ter_dptr);
-		ull_adv_aux_hdr_len_fill(ter_com_hdr, ter_len);
-
-		ter_pdu->len = ter_len;
+		adv_sync_pdu_init(ter_pdu, 0);
 	} else {
 		sync = HDR_LLL2ULL(lll_sync);
 	}

From 0859ee8d669df2e9840378c63f61db3f17d38464 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Mon, 1 Mar 2021 16:00:53 +0100
Subject: [PATCH 7/9] Bluetooth: controller: Add initial support to update AD
 in chain

This adds some initial support to update AD in chain. We still only
support placing AD in 1st PDU, but this will properly copy any linked
PDUs that may be added due to e.g. CTEInfo present.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../controller/ll_sw/ull_adv_internal.h       |  27 +-
 .../bluetooth/controller/ll_sw/ull_adv_sync.c | 445 ++++++++++++++----
 subsys/bluetooth/controller/ll_sw/ull_df.c    |  43 +-
 3 files changed, 407 insertions(+), 108 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
index 88642b0dd9800..8b589a6dcd700 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
@@ -110,14 +110,35 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv,
 /* helper function to release periodic advertising instance */
 void ull_adv_sync_release(struct ll_adv_sync_set *sync);
 
+/* helper function to allocate new PDU data for AUX_SYNC_IND and return
+ * previous and new PDU for further processing.
+ */
+uint8_t ull_adv_sync_pdu_alloc(struct ll_adv_set *adv,
+			       uint16_t hdr_add_fields,
+			       uint16_t hdr_rem_fields,
+			       struct adv_pdu_field_data *data,
+			       struct pdu_adv **ter_pdu_prev,
+			       struct pdu_adv **ter_pdu_new,
+			       void **extra_data_prev,
+			       void **extra_data_new,
+			       uint8_t *ter_idx);
+
 /* helper function to set/clear common extended header format fields
  * for AUX_SYNC_IND PDU.
  */
-uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv,
+uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync,
+				   struct pdu_adv *ter_pdu_prev,
+				   struct pdu_adv *ter_pdu,
 				   uint16_t hdr_add_fields,
 				   uint16_t hdr_rem_fields,
-				   struct adv_pdu_field_data *data,
-				   uint8_t *ter_idx);
+				   struct adv_pdu_field_data *data);
+
+/* helper function to update extra_data field */
+void ull_adv_sync_extra_data_set_clear(void *extra_data_prev,
+				       void *extra_data_new,
+				       uint16_t hdr_add_fields,
+				       uint16_t hdr_rem_fields,
+				       void *data);
 
 /* helper function to calculate common ext adv payload header length and
  * adjust the data pointer.
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
index 6aa32b2e78682..69ef3a76d3520 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
@@ -50,14 +50,6 @@ static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync);
 static inline uint8_t sync_remove(struct ll_adv_sync_set *sync,
 				  struct ll_adv_set *adv, uint8_t enable);
 
-#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
-static inline void adv_sync_extra_data_set_clear(void *extra_data_prev,
-						 void *extra_data_new,
-						 uint16_t hdr_add_fields,
-						 uint16_t hdr_rem_fields,
-						 void *data);
-#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
-
 static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync,
 				      struct pdu_adv *ter_pdu_prev,
 				      struct pdu_adv *ter_pdu,
@@ -120,8 +112,194 @@ static void adv_sync_pdu_init(struct pdu_adv *pdu, uint8_t ext_hdr_flags)
 
 	pdu->len = len;
 }
+
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+static uint8_t adv_sync_pdu_init_from_prev_pdu(struct pdu_adv *pdu,
+					       struct pdu_adv *pdu_prev,
+					       uint16_t ext_hdr_flags_add,
+					       uint16_t ext_hdr_flags_rem)
+{
+	struct pdu_adv_com_ext_adv *com_hdr_prev;
+	struct pdu_adv_ext_hdr *ext_hdr_prev;
+	struct pdu_adv_com_ext_adv *com_hdr;
+	struct pdu_adv_ext_hdr *ext_hdr;
+	uint8_t ext_hdr_flags_prev;
+	uint8_t ext_hdr_flags;
+	uint8_t *dptr_prev;
+	uint8_t len_prev;
+	uint8_t *dptr;
+	uint8_t len;
+
+	/* Copy complete header, assume it was set properly in old PDU */
+	*(uint8_t *)pdu = *(uint8_t *)pdu_prev;
+
+	com_hdr_prev = &pdu_prev->adv_ext_ind;
+	com_hdr = &pdu->adv_ext_ind;
+
+	com_hdr->adv_mode = 0U;
+
+	ext_hdr_prev = &com_hdr_prev->ext_hdr;
+	ext_hdr = &com_hdr->ext_hdr;
+
+	if (com_hdr_prev->ext_hdr_len) {
+		ext_hdr_flags_prev = *(uint8_t *) ext_hdr_prev;
+	} else {
+		ext_hdr_flags_prev = 0;
+	}
+	ext_hdr_flags = ext_hdr_flags_prev |
+			(ext_hdr_flags_add & (~ext_hdr_flags_rem));
+
+	*(uint8_t *)ext_hdr = ext_hdr_flags;
+
+	LL_ASSERT(!ext_hdr->adv_addr);
+	LL_ASSERT(!ext_hdr->tgt_addr);
+	LL_ASSERT(!ext_hdr->adi);
+	LL_ASSERT(!ext_hdr->sync_info);
+
+	dptr = ext_hdr->data;
+	dptr_prev = ext_hdr_prev->data;
+
+	/* Note: skip length verification of ext header writes as we assume that
+	 *       all PDUs are large enough to store at least complete ext header.
+	 */
+
+	/* Copy CTEInfo, if applicable */
+	if (ext_hdr->cte_info) {
+		if (ext_hdr_prev->cte_info) {
+			memcpy(dptr, dptr_prev, sizeof(struct pdu_cte_info));
+		}
+		dptr += sizeof(struct pdu_cte_info);
+	}
+	if (ext_hdr_prev->cte_info) {
+		dptr_prev += sizeof(struct pdu_cte_info);
+	}
+
+	/* Add AuxPtr, if applicable. Do not copy since it will be updated later
+	 * anyway.
+	 */
+	if (ext_hdr->aux_ptr) {
+		dptr += sizeof(struct pdu_adv_aux_ptr);
+	}
+	if (ext_hdr_prev->aux_ptr) {
+		dptr_prev += sizeof(struct pdu_adv_aux_ptr);
+	}
+
+	/* Copy TxPower, if applicable */
+	if (ext_hdr->tx_pwr) {
+		if (ext_hdr_prev->tx_pwr) {
+			memcpy(dptr, dptr_prev, sizeof(uint8_t));
+		}
+		dptr += sizeof(uint8_t);
+	}
+	if (ext_hdr_prev->tx_pwr) {
+		dptr_prev += sizeof(uint8_t);
+	}
+
+	LL_ASSERT(ext_hdr_prev  >= 0);
+
+	/* Copy ACAD */
+	len = com_hdr_prev->ext_hdr_len - (dptr_prev - (uint8_t *)ext_hdr_prev);
+	memcpy(dptr, dptr_prev, len);
+	dptr += len;
+
+	/* Check populated ext header length excluding length itself. If 0, then
+	 * there was neither field nor ACAD populated and we skip ext header
+	 * entirely.
+	 */
+	len = dptr - ext_hdr->data;
+	if (len == 0) {
+		com_hdr->ext_hdr_len = 0;
+	} else {
+		com_hdr->ext_hdr_len = len +
+				       offsetof(struct pdu_adv_ext_hdr, data);
+	}
+
+	/* Both PDUs have now ext header length calculated properly, reset
+	 * pointers to start of AD.
+	 */
+	dptr = &com_hdr->ext_hdr_adv_data[com_hdr->ext_hdr_len];
+	dptr_prev = &com_hdr_prev->ext_hdr_adv_data[com_hdr_prev->ext_hdr_len];
+
+	/* Calculate length of AD to copy and AD length available in new PDU */
+	len_prev = pdu_prev->len - (dptr_prev - pdu_prev->payload);
+	len = PDU_AC_PAYLOAD_SIZE_MAX - (dptr - pdu->payload);
+
+	/* TODO: we should allow partial copy and let caller refragment data */
+	if (len < len_prev) {
+		return BT_HCI_ERR_PACKET_TOO_LONG;
+	}
+
+	/* Copy AD */
+	if (!(ext_hdr_flags_rem & ULL_ADV_PDU_HDR_FIELD_AD_DATA)) {
+		len = MIN(len, len_prev);
+		memcpy(dptr, dptr_prev, len);
+		dptr += len;
+	}
+
+	/* Finalize PDU */
+	pdu->len = dptr - pdu->payload;
+
+	return 0;
+}
+
+static uint8_t adv_sync_pdu_ad_data_set(struct pdu_adv *pdu,
+					const uint8_t *data, uint8_t len)
+{
+	struct pdu_adv_com_ext_adv *com_hdr;
+	uint8_t len_max;
+	uint8_t *dptr;
+
+	com_hdr = &pdu->adv_ext_ind;
+
+	dptr = &com_hdr->ext_hdr_adv_data[com_hdr->ext_hdr_len];
+
+	len_max = PDU_AC_PAYLOAD_SIZE_MAX - (dptr - pdu->payload);
+	/* TODO: we should allow partial copy and let caller refragment data */
+	if (len > len_max) {
+		return BT_HCI_ERR_PACKET_TOO_LONG;
+	}
+
+	memcpy(dptr, data, len);
+	dptr += len;
+
+	pdu->len = dptr - pdu->payload;
+
+	return 0;
+}
+
+static struct pdu_adv *adv_sync_pdu_duplicate_chain(struct pdu_adv *pdu)
+{
+	struct pdu_adv *pdu_dup = NULL;
+	uint8_t err;
+
+	while (pdu) {
+		struct pdu_adv *pdu_new;
+
+		pdu_new = lll_adv_pdu_alloc_pdu_adv();
+
+		/* We make exact copy of old PDU, there's really nothing that
+		 * can go wrong there assuming original PDU was created properly
+		 */
+		err = adv_sync_pdu_init_from_prev_pdu(pdu_new, pdu, 0, 0);
+		LL_ASSERT(err == 0);
+
+		if (pdu_dup) {
+			lll_adv_pdu_linked_append_end(pdu_new, pdu_dup);
+		} else {
+			pdu_dup = pdu_new;
+		}
+
+		pdu = lll_adv_pdu_linked_next_get(pdu);
+	}
+
+	return pdu_dup;
+}
+#endif /* CONFIG_BT_CTLR_ADV_PDU_LINK */
+
 uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
 {
+	void *extra_data_prev, *extra_data;
+	struct pdu_adv *pdu_prev, *pdu;
 	struct lll_adv_sync *lll_sync;
 	struct ll_adv_sync_set *sync;
 	struct ll_adv_set *adv;
@@ -181,7 +359,20 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
 
 	sync->interval = interval;
 
-	err = ull_adv_sync_pdu_set_clear(adv, 0, 0, NULL, &ter_idx);
+	err = ull_adv_sync_pdu_alloc(adv, 0, 0, NULL, &pdu_prev, &pdu,
+				     &extra_data_prev, &extra_data, &ter_idx);
+	if (err) {
+		return err;
+	}
+
+#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+	if (extra_data) {
+		ull_adv_sync_extra_data_set_clear(extra_data_prev, extra_data,
+						  0, 0, NULL);
+	}
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
+
+	err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, 0, 0, NULL);
 	if (err) {
 		return err;
 	}
@@ -194,7 +385,9 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags)
 uint8_t ll_adv_sync_ad_data_set(uint8_t handle, uint8_t op, uint8_t len,
 				uint8_t const *const data)
 {
+	void *extra_data_prev, *extra_data;
 	struct adv_pdu_field_data pdu_data;
+	struct pdu_adv *pdu_prev, *pdu;
 	struct lll_adv_sync *lll_sync;
 	struct ll_adv_set *adv;
 	uint8_t value[5];
@@ -222,15 +415,49 @@ uint8_t ll_adv_sync_ad_data_set(uint8_t handle, uint8_t op, uint8_t len,
 	*pdu_data.field_data = len;
 	sys_put_le32((uint32_t)data, pdu_data.field_data + 1);
 
-	err = ull_adv_sync_pdu_set_clear(adv, ULL_ADV_PDU_HDR_FIELD_AD_DATA,
-					 0, &pdu_data, &ter_idx);
+	err = ull_adv_sync_pdu_alloc(adv, ULL_ADV_PDU_HDR_FIELD_AD_DATA, 0,
+				     &pdu_data, &pdu_prev, &pdu,
+				     &extra_data_prev, &extra_data, &ter_idx);
 	if (err) {
 		return err;
 	}
 
-	lll_adv_sync_data_enqueue(lll_sync, ter_idx);
+#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+	if (extra_data) {
+		ull_adv_sync_extra_data_set_clear(extra_data_prev, extra_data,
+						  ULL_ADV_PDU_HDR_FIELD_AD_DATA,
+						  0, &pdu_data);
+	}
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 
-	return 0;
+	err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu,
+					 ULL_ADV_PDU_HDR_FIELD_AD_DATA,
+					 0, &pdu_data);
+
+
+#if defined(CONFIG_BT_CTLR_ADV_PDU_LINK)
+	/* alloc() will return the same PDU as peek() in case there was PDU
+	 * queued but not switched to current before alloc() - no need to deal
+	 * with chain as it's already there. In other case we need to duplicate
+	 * chain from current PDU and append it to new PDU.
+	 */
+	if (pdu != pdu_prev) {
+		struct pdu_adv *next, *next_dup;
+
+		LL_ASSERT(lll_adv_pdu_linked_next_get(pdu) == NULL);
+
+		next = lll_adv_pdu_linked_next_get(pdu_prev);
+		next_dup = adv_sync_pdu_duplicate_chain(next);
+
+		lll_adv_pdu_linked_append(next_dup, pdu);
+	}
+#endif /* CONFIG_BT_CTLR_ADV_PDU_LINK */
+
+	if (!err) {
+		lll_adv_sync_data_enqueue(lll_sync, ter_idx);
+	}
+
+	return err;
 }
 
 uint8_t ll_adv_sync_enable(uint8_t handle, uint8_t enable)
@@ -484,45 +711,22 @@ void ull_adv_sync_update(struct ll_adv_sync_set *sync, uint32_t slot_plus_us,
 }
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 
-/* @brief Set or clear fields in extended advertising header and store
- *        extra_data if requested.
- *
- * @param[in]  adv              Advertising set.
- * @param[in]  hdr_add_fields   Flag with information which fields add.
- * @param[in]  hdr_rem_fields   Flag with information which fields remove.
- * @param[in]  data             Pointer to data to be added to header and
- *                              extra_data. Content depends on the value of
- *                              @p hdr_add_fields.
- * @param[out] ter_idx          Index of new PDU.
- *
- * @Note
- * @p data content depends on the flag provided by @p hdr_add_fields:
- * - ULL_ADV_PDU_HDR_FIELD_CTE_INFO:
- *   # @p data->field_data points to single byte with CTEInfo field
- *   # @p data->extra_data points to memory where is struct lll_df_adv_cfg
- *     for LLL.
- * - ULL_ADV_PDU_HDR_FIELD_AD_DATA:
- *   # @p data->field_data points to memory where first byte
- *     is size of advertising data, following byte is a pointer to actual
- *     advertising data.
- *   # @p data->extra_data is NULL
- * - ULL_ADV_PDU_HDR_FIELD_AUX_PTR: # @p data parameter is not used
- *
- * @return Zero in case of success, other value in case of failure.
- */
-uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv,
-				   uint16_t hdr_add_fields,
-				   uint16_t hdr_rem_fields,
-				   struct adv_pdu_field_data *data,
-				   uint8_t *ter_idx)
+uint8_t ull_adv_sync_pdu_alloc(struct ll_adv_set *adv,
+			       uint16_t hdr_add_fields,
+			       uint16_t hdr_rem_fields,
+			       struct adv_pdu_field_data *data,
+			       struct pdu_adv **ter_pdu_prev,
+			       struct pdu_adv **ter_pdu_new,
+			       void **extra_data_prev,
+			       void **extra_data_new,
+			       uint8_t *ter_idx)
 {
 	struct pdu_adv *pdu_prev, *pdu_new;
 	struct lll_adv_sync *lll_sync;
-	void *extra_data_prev;
-#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
-	void *extra_data;
-#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
-	int err;
+	void *ed_prev;
+#if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
+	void *ed_new;
+#endif
 
 	lll_sync = adv->lll.sync;
 	if (!lll_sync) {
@@ -530,23 +734,23 @@ uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv,
 	}
 
 	/* Get reference to previous periodic advertising PDU data */
-	pdu_prev = lll_adv_sync_data_peek(lll_sync, &extra_data_prev);
+	pdu_prev = lll_adv_sync_data_peek(lll_sync, &ed_prev);
 
 #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
 	/* Get reference to new periodic advertising PDU data buffer */
 	if ((hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) ||
 	    (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) &&
-	    extra_data_prev)) {
+	     ed_prev)) {
 		/* If there was an extra data in past PDU data or it is required
 		 * by the hdr_add_fields then allocate memmory for it.
 		 */
-		pdu_new = lll_adv_sync_data_alloc(lll_sync, &extra_data,
+		pdu_new = lll_adv_sync_data_alloc(lll_sync, &ed_new,
 						  ter_idx);
 		if (!pdu_new) {
 			return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
 		}
 	} else {
-		extra_data = NULL;
+		ed_new = NULL;
 #else
 	{
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
@@ -556,23 +760,99 @@ uint8_t ull_adv_sync_pdu_set_clear(struct ll_adv_set *adv,
 		}
 	}
 
-	err = adv_sync_hdr_set_clear(lll_sync, pdu_prev, pdu_new,
+#if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
+	if (extra_data_prev) {
+		*extra_data_prev = ed_prev;
+	}
+	if (extra_data_new) {
+		*extra_data_new = ed_new;
+	}
+#endif /* CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY */
+
+	*ter_pdu_prev = pdu_prev;
+	*ter_pdu_new = pdu_new;
+
+	return 0;
+}
+
+/* @brief Set or clear fields in extended advertising header and store
+ *        extra_data if requested.
+ *
+ * @param[in]  adv              Advertising set.
+ * @param[in]  hdr_add_fields   Flag with information which fields add.
+ * @param[in]  hdr_rem_fields   Flag with information which fields remove.
+ * @param[in]  data             Pointer to data to be added to header and
+ *                              extra_data. Content depends on the value of
+ *                              @p hdr_add_fields.
+ * @param[out] ter_idx          Index of new PDU.
+ *
+ * @Note
+ * @p data content depends on the flag provided by @p hdr_add_fields:
+ * - ULL_ADV_PDU_HDR_FIELD_CTE_INFO:
+ *   # @p data->field_data points to single byte with CTEInfo field
+ *   # @p data->extra_data points to memory where is struct lll_df_adv_cfg
+ *     for LLL.
+ * - ULL_ADV_PDU_HDR_FIELD_AD_DATA:
+ *   # @p data->field_data points to memory where first byte
+ *     is size of advertising data, following byte is a pointer to actual
+ *     advertising data.
+ *   # @p data->extra_data is NULL
+ * - ULL_ADV_PDU_HDR_FIELD_AUX_PTR: # @p data parameter is not used
+ *
+ * @return Zero in case of success, other value in case of failure.
+ */
+uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync,
+				   struct pdu_adv *ter_pdu_prev,
+				   struct pdu_adv *ter_pdu,
+				   uint16_t hdr_add_fields,
+				   uint16_t hdr_rem_fields,
+				   struct adv_pdu_field_data *data)
+{
+	int err;
+
+	err = adv_sync_hdr_set_clear(lll_sync, ter_pdu_prev, ter_pdu,
 				     hdr_add_fields, hdr_rem_fields,
 				     (data ? data->field_data : NULL));
-	if (err) {
-		return err;
-	}
+
+	return err;
+}
 
 #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
-	if (extra_data) {
-		adv_sync_extra_data_set_clear(extra_data_prev, extra_data,
-					      hdr_add_fields, hdr_rem_fields,
-					      (data ? data->extra_data : NULL));
+/* @brief Set or clear fields in extended advertising header and store
+ *        extra_data if requested.
+ *
+ * @param[in]  extra_data_prev  Pointer to previous content of extra_data.
+ * @param[in]  hdr_add_fields   Flag with information which fields add.
+ * @param[in]  hdr_rem_fields   Flag with information which fields remove.
+ * @param[in]  data             Pointer to data to be stored in extra_data.
+ *                              Content depends on the data depends on
+ *                              @p hdr_add_fields.
+ *
+ * @Note
+ * @p data depends on the flag provided by @p hdr_add_fields.
+ * Information about content of value may be found in description of
+ * @ref ull_adv_sync_pdu_set_clear.
+ *
+ * @return Zero in case of success, other value in case of failure.
+ */
+void ull_adv_sync_extra_data_set_clear(void *extra_data_prev,
+				       void *extra_data_new,
+				       uint16_t hdr_add_fields,
+				       uint16_t hdr_rem_fields,
+				       void *data)
+{
+	/* Currently only CTE enable requires extra_data. Due to that fact
+	 * CTE additional data are just copied to extra_data memory.
+	 */
+	if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) {
+		memcpy(extra_data_new, data, sizeof(struct lll_df_adv_cfg));
+	} else if (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) ||
+		   extra_data_prev) {
+		memmove(extra_data_new, extra_data_prev,
+			sizeof(struct lll_df_adv_cfg));
 	}
-#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
-
-	return 0;
 }
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 
 static int init_reset(void)
 {
@@ -873,43 +1153,6 @@ static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync,
 	return 0;
 }
 
-#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
-/* @brief Set or clear fields in extended advertising header and store
- *        extra_data if requested.
- *
- * @param[in]  extra_data_prev  Pointer to previous content of extra_data.
- * @param[in]  hdr_add_fields   Flag with information which fields add.
- * @param[in]  hdr_rem_fields   Flag with information which fields remove.
- * @param[in]  data             Pointer to data to be stored in extra_data.
- *                              Content depends on the data depends on
- *                              @p hdr_add_fields.
- *
- * @Note
- * @p data depends on the flag provided by @p hdr_add_fields.
- * Information about content of value may be found in description of
- * @ref ull_adv_sync_pdu_set_clear.
- *
- * @return Zero in case of success, other value in case of failure.
- */
-static inline void adv_sync_extra_data_set_clear(void *extra_data_prev,
-						 void *extra_data_new,
-						 uint16_t hdr_add_fields,
-						 uint16_t hdr_rem_fields,
-						 void *data)
-{
-	/* Currently only CTE enable requires extra_data. Due to that fact
-	 * CTE additional data are just copied to extra_data memory.
-	 */
-	if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) {
-		memcpy(extra_data_new, data, sizeof(struct lll_df_adv_cfg));
-	} else if ((hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) ||
-		    extra_data_prev) {
-		memmove(extra_data_new, extra_data_prev,
-			sizeof(struct lll_df_adv_cfg));
-	}
-}
-#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
-
 static void mfy_sync_offset_get(void *param)
 {
 	struct ll_adv_set *adv = param;
diff --git a/subsys/bluetooth/controller/ll_sw/ull_df.c b/subsys/bluetooth/controller/ll_sw/ull_df.c
index 86fcb23545d79..7d0d448d7b764 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_df.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_df.c
@@ -256,6 +256,8 @@ uint8_t ll_df_set_cl_cte_tx_params(uint8_t adv_handle, uint8_t cte_len,
  */
 uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable)
 {
+	void *extra_data_prev, *extra_data;
+	struct pdu_adv *pdu_prev, *pdu;
 	struct lll_adv_sync *lll_sync;
 	struct lll_df_adv_cfg *df_cfg;
 	struct ll_adv_sync_set *sync;
@@ -295,9 +297,25 @@ uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable)
 			return BT_HCI_ERR_CMD_DISALLOWED;
 		}
 
-		err = ull_adv_sync_pdu_set_clear(adv, 0,
+		err = ull_adv_sync_pdu_alloc(adv, 0,
+					     ULL_ADV_PDU_HDR_FIELD_CTE_INFO,
+					     NULL, &pdu_prev, &pdu,
+					     &extra_data_prev, &extra_data,
+					     &ter_idx);
+		if (err) {
+			return err;
+		}
+
+		if (extra_data) {
+			ull_adv_sync_extra_data_set_clear(extra_data_prev,
+							  extra_data, 0,
+							  ULL_ADV_PDU_HDR_FIELD_CTE_INFO,
+							  NULL);
+		}
+
+		err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, 0,
 						 ULL_ADV_PDU_HDR_FIELD_CTE_INFO,
-						 NULL, &ter_idx);
+						 NULL);
 		if (err) {
 			return err;
 		}
@@ -326,9 +344,26 @@ uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable)
 		cte_info.time = df_cfg->cte_length;
 		pdu_data.field_data = (uint8_t *)&cte_info;
 		pdu_data.extra_data = df_cfg;
-		err = ull_adv_sync_pdu_set_clear(adv,
+
+		err = ull_adv_sync_pdu_alloc(adv, 0,
+					     ULL_ADV_PDU_HDR_FIELD_CTE_INFO,
+					     NULL, &pdu_prev, &pdu,
+					     &extra_data_prev, &extra_data,
+					     &ter_idx);
+		if (err) {
+			return err;
+		}
+
+		if (extra_data) {
+			ull_adv_sync_extra_data_set_clear(extra_data_prev,
+							  extra_data,
+							  ULL_ADV_PDU_HDR_FIELD_CTE_INFO,
+							  0, &pdu_data);
+		}
+
+		err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu,
 						 ULL_ADV_PDU_HDR_FIELD_CTE_INFO,
-						 0, &pdu_data, &ter_idx);
+						 0, &pdu_data);
 		if (err) {
 			return err;
 		}

From 541c79bd3b43863fe47a873e00ea5f200583ab0b Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Mon, 1 Mar 2021 16:27:07 +0100
Subject: [PATCH 8/9] Bluetooth: controller: Add helper to update CTEInfo

This adds simple helper to update CTEInfo. It assumes proper periodic
adv PDU as input to simplify code.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../bluetooth/controller/ll_sw/ull_adv_sync.c | 22 +++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
index 69ef3a76d3520..b89aa4c2f9b7d 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
@@ -267,6 +267,28 @@ static uint8_t adv_sync_pdu_ad_data_set(struct pdu_adv *pdu,
 	return 0;
 }
 
+static uint8_t adv_sync_pdu_cte_info_set(struct pdu_adv *pdu,
+					 const struct pdu_cte_info *cte_info)
+{
+	struct pdu_adv_com_ext_adv *com_hdr;
+	struct pdu_adv_ext_hdr *ext_hdr;
+	uint8_t *dptr;
+
+	com_hdr = &pdu->adv_ext_ind;
+	ext_hdr = &com_hdr->ext_hdr;
+	dptr = ext_hdr->data;
+
+	/* Periodic adv PDUs do not have AdvA/TargetA */
+	LL_ASSERT(!ext_hdr->adv_addr);
+	LL_ASSERT(!ext_hdr->tgt_addr);
+
+	if (ext_hdr->cte_info) {
+		memcpy(dptr, cte_info, sizeof(*cte_info));
+	}
+
+	return 0;
+}
+
 static struct pdu_adv *adv_sync_pdu_duplicate_chain(struct pdu_adv *pdu)
 {
 	struct pdu_adv *pdu_dup = NULL;

From 3b84e030e7ac376931f3323f9e64418e44fe7501 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Fri, 25 Jun 2021 12:57:32 +0200
Subject: [PATCH 9/9] Bluetooth: controller: Remove adv_sync_hdr_set_clear

adv_sync_hdr_set_clear was just wrapped by ull_adv_sync_pdu_set_clear
so we can merge both into single function.

Signed-off-by: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
---
 .../bluetooth/controller/ll_sw/ull_adv_sync.c | 273 ++++++++----------
 1 file changed, 118 insertions(+), 155 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
index b89aa4c2f9b7d..6b3fbd4411264 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
@@ -50,13 +50,6 @@ static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync);
 static inline uint8_t sync_remove(struct ll_adv_sync_set *sync,
 				  struct ll_adv_set *adv, uint8_t enable);
 
-static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync,
-				      struct pdu_adv *ter_pdu_prev,
-				      struct pdu_adv *ter_pdu,
-				      uint16_t hdr_add_fields,
-				      uint16_t hdr_rem_fields,
-				      void *data);
-
 static void mfy_sync_offset_get(void *param);
 static inline struct pdu_adv_sync_info *sync_info_get(struct pdu_adv *pdu);
 static inline void sync_info_offset_fill(struct pdu_adv_sync_info *si,
@@ -829,154 +822,6 @@ uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync,
 				   uint16_t hdr_add_fields,
 				   uint16_t hdr_rem_fields,
 				   struct adv_pdu_field_data *data)
-{
-	int err;
-
-	err = adv_sync_hdr_set_clear(lll_sync, ter_pdu_prev, ter_pdu,
-				     hdr_add_fields, hdr_rem_fields,
-				     (data ? data->field_data : NULL));
-
-	return err;
-}
-
-#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
-/* @brief Set or clear fields in extended advertising header and store
- *        extra_data if requested.
- *
- * @param[in]  extra_data_prev  Pointer to previous content of extra_data.
- * @param[in]  hdr_add_fields   Flag with information which fields add.
- * @param[in]  hdr_rem_fields   Flag with information which fields remove.
- * @param[in]  data             Pointer to data to be stored in extra_data.
- *                              Content depends on the data depends on
- *                              @p hdr_add_fields.
- *
- * @Note
- * @p data depends on the flag provided by @p hdr_add_fields.
- * Information about content of value may be found in description of
- * @ref ull_adv_sync_pdu_set_clear.
- *
- * @return Zero in case of success, other value in case of failure.
- */
-void ull_adv_sync_extra_data_set_clear(void *extra_data_prev,
-				       void *extra_data_new,
-				       uint16_t hdr_add_fields,
-				       uint16_t hdr_rem_fields,
-				       void *data)
-{
-	/* Currently only CTE enable requires extra_data. Due to that fact
-	 * CTE additional data are just copied to extra_data memory.
-	 */
-	if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) {
-		memcpy(extra_data_new, data, sizeof(struct lll_df_adv_cfg));
-	} else if (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) ||
-		   extra_data_prev) {
-		memmove(extra_data_new, extra_data_prev,
-			sizeof(struct lll_df_adv_cfg));
-	}
-}
-#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
-
-static int init_reset(void)
-{
-	/* Initialize adv sync pool. */
-	mem_init(ll_adv_sync_pool, sizeof(struct ll_adv_sync_set),
-		 sizeof(ll_adv_sync_pool) / sizeof(struct ll_adv_sync_set),
-		 &adv_sync_free);
-
-	return 0;
-}
-
-static inline struct ll_adv_sync_set *sync_acquire(void)
-{
-	return mem_acquire(&adv_sync_free);
-}
-
-static inline void sync_release(struct ll_adv_sync_set *sync)
-{
-	mem_release(sync, &adv_sync_free);
-}
-
-static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync)
-{
-	return mem_index_get(sync, ll_adv_sync_pool,
-			     sizeof(struct ll_adv_sync_set));
-}
-
-static uint8_t sync_stop(struct ll_adv_sync_set *sync)
-{
-	uint8_t sync_handle;
-	int err;
-
-	sync_handle = sync_handle_get(sync);
-
-	err = ull_ticker_stop_with_mark(TICKER_ID_ADV_SYNC_BASE + sync_handle,
-					sync, &sync->lll);
-	LL_ASSERT(err == 0 || err == -EALREADY);
-	if (err) {
-		return BT_HCI_ERR_CMD_DISALLOWED;
-	}
-
-	return 0;
-}
-
-static inline uint8_t sync_remove(struct ll_adv_sync_set *sync,
-				  struct ll_adv_set *adv, uint8_t enable)
-{
-	uint8_t pri_idx;
-	uint8_t err;
-
-	/* Remove sync_info from auxiliary PDU */
-	err = ull_adv_aux_hdr_set_clear(adv, 0,
-					ULL_ADV_PDU_HDR_FIELD_SYNC_INFO,
-					NULL, NULL, &pri_idx);
-	if (err) {
-		return err;
-	}
-
-	lll_adv_data_enqueue(&adv->lll, pri_idx);
-
-	if (sync->is_started) {
-		/* TODO: we removed sync info, but if sync_stop() fails, what do
-		 * we do?
-		 */
-		err = sync_stop(sync);
-		if (err) {
-			return err;
-		}
-
-		sync->is_started = 0U;
-	}
-
-	if (!enable) {
-		sync->is_enabled = 0U;
-	}
-
-	return 0U;
-}
-
-/* @brief Set or clear fields in extended advertising header.
- *
- * @param[in]  adv              Advertising set.
- * @param[in]  hdr_add_fields   Flag with information which fields add.
- * @param[in]  hdr_rem_fields   Flag with information which fields remove.
- * @param[in]  value            Pointer to data to be added to header.
- *                              Content depends on the value of
- *                              @p hdr_add_fields.
- * @param[out] ter_idx          Index of new PDU.
- *
- * @Note
- * @p value depends on the flag provided by @p hdr_add_fields.
- * Information about content of value may be found in description of
- * @ref ull_adv_sync_pdu_set_clear.
- *
- * @return Zero in case of success, other value in case of failure.
- */
-static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync,
-				      struct pdu_adv *ter_pdu_prev,
-				      struct pdu_adv *ter_pdu,
-				      uint16_t hdr_add_fields,
-				      uint16_t hdr_rem_fields,
-				      void *value)
 {
 	struct pdu_adv_com_ext_adv *ter_com_hdr, *ter_com_hdr_prev;
 	struct pdu_adv_ext_hdr *ter_hdr, ter_hdr_prev;
@@ -990,6 +835,9 @@ static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync,
 	uint8_t cte_info;
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 	uint8_t ad_len;
+	void *value;
+
+	value = data ? data->field_data : NULL;
 
 	/* Get common pointers from reference to previous tertiary PDU data */
 	ter_com_hdr_prev = (void *)&ter_pdu_prev->adv_ext_ind;
@@ -1175,6 +1023,121 @@ static uint8_t adv_sync_hdr_set_clear(struct lll_adv_sync *lll_sync,
 	return 0;
 }
 
+#if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+/* @brief Set or clear fields in extended advertising header and store
+ *        extra_data if requested.
+ *
+ * @param[in]  extra_data_prev  Pointer to previous content of extra_data.
+ * @param[in]  hdr_add_fields   Flag with information which fields add.
+ * @param[in]  hdr_rem_fields   Flag with information which fields remove.
+ * @param[in]  data             Pointer to data to be stored in extra_data.
+ *                              Content depends on the data depends on
+ *                              @p hdr_add_fields.
+ *
+ * @Note
+ * @p data depends on the flag provided by @p hdr_add_fields.
+ * Information about content of value may be found in description of
+ * @ref ull_adv_sync_pdu_set_clear.
+ *
+ * @return Zero in case of success, other value in case of failure.
+ */
+void ull_adv_sync_extra_data_set_clear(void *extra_data_prev,
+				       void *extra_data_new,
+				       uint16_t hdr_add_fields,
+				       uint16_t hdr_rem_fields,
+				       void *data)
+{
+	/* Currently only CTE enable requires extra_data. Due to that fact
+	 * CTE additional data are just copied to extra_data memory.
+	 */
+	if (hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) {
+		memcpy(extra_data_new, data, sizeof(struct lll_df_adv_cfg));
+	} else if (!(hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_CTE_INFO) ||
+		   extra_data_prev) {
+		memmove(extra_data_new, extra_data_prev,
+			sizeof(struct lll_df_adv_cfg));
+	}
+}
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
+
+static int init_reset(void)
+{
+	/* Initialize adv sync pool. */
+	mem_init(ll_adv_sync_pool, sizeof(struct ll_adv_sync_set),
+		 sizeof(ll_adv_sync_pool) / sizeof(struct ll_adv_sync_set),
+		 &adv_sync_free);
+
+	return 0;
+}
+
+static inline struct ll_adv_sync_set *sync_acquire(void)
+{
+	return mem_acquire(&adv_sync_free);
+}
+
+static inline void sync_release(struct ll_adv_sync_set *sync)
+{
+	mem_release(sync, &adv_sync_free);
+}
+
+static inline uint16_t sync_handle_get(struct ll_adv_sync_set *sync)
+{
+	return mem_index_get(sync, ll_adv_sync_pool,
+			     sizeof(struct ll_adv_sync_set));
+}
+
+static uint8_t sync_stop(struct ll_adv_sync_set *sync)
+{
+	uint8_t sync_handle;
+	int err;
+
+	sync_handle = sync_handle_get(sync);
+
+	err = ull_ticker_stop_with_mark(TICKER_ID_ADV_SYNC_BASE + sync_handle,
+					sync, &sync->lll);
+	LL_ASSERT(err == 0 || err == -EALREADY);
+	if (err) {
+		return BT_HCI_ERR_CMD_DISALLOWED;
+	}
+
+	return 0;
+}
+
+static inline uint8_t sync_remove(struct ll_adv_sync_set *sync,
+				  struct ll_adv_set *adv, uint8_t enable)
+{
+	uint8_t pri_idx;
+	uint8_t err;
+
+	/* Remove sync_info from auxiliary PDU */
+	err = ull_adv_aux_hdr_set_clear(adv, 0,
+					ULL_ADV_PDU_HDR_FIELD_SYNC_INFO,
+					NULL, NULL, &pri_idx);
+	if (err) {
+		return err;
+	}
+
+	lll_adv_data_enqueue(&adv->lll, pri_idx);
+
+	if (sync->is_started) {
+		/* TODO: we removed sync info, but if sync_stop() fails, what do
+		 * we do?
+		 */
+		err = sync_stop(sync);
+		if (err) {
+			return err;
+		}
+
+		sync->is_started = 0U;
+	}
+
+	if (!enable) {
+		sync->is_enabled = 0U;
+	}
+
+	return 0U;
+}
+
 static void mfy_sync_offset_get(void *param)
 {
 	struct ll_adv_set *adv = param;