From 21892eecf102528fd938c9828e6f233a257ede3a 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] 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 e493b1c873e..7f67727df21 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 b435158214d..afd882d88f4 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 b2273e0a48d..9fc10982f2c 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 46f22c66222..3d764c139ef 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 0bdc7d5d1af..ea960b5a6c5 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 07ad2128d52..e298fdb38f8 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 ef89d658655..8b1e20f7e61 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 0813dd7043c..777bff0a29d 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 54e7c00b3c1..705e19f2a12 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 f3353ed8efa..bf30cbe0f5b 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"