Bluetooth: controller: LLL: enable TX of CTE with per. adv. PDU

Enable transmission of CTE with periodic advertising PDU.

Signed-off-by: Piotr Pryga <piotr.pryga@nordicsemi.no>

subsys/bluetooth/controller/Kconfig.df

@@ -110,7 +110,7 @@ config BT_CTLR_DF_ADV_CTE_TX
 
 config BT_CTLR_DF_MAX_ANT_SW_PATTERN_LEN
 	int "Maximum length of antenna switch pattern"
-	range 3 40 if SOC_COMPATIBLE_NRF
+	range 3 39 if SOC_COMPATIBLE_NRF
 	range 2 75 if !SOC_COMPATIBLE_NRF
 	default 12
 	help

subsys/bluetooth/controller/ll_sw/lll_df_internal.h

@@ -25,5 +25,14 @@ struct lll_df_adv_cfg {
 /* @brief Min supported length of antenna switching pattern */
 #define LLL_DF_MIN_ANT_PATTERN_LEN 3
 
+/* @brief Macro to convert length of CTE to [us] */
+#define CTE_LEN_US(n) ((n) * 8U)
+
 /* Provides number of available antennae for Direction Finding */
 uint8_t lll_df_ant_num_get(void);
+
+/* Enables CTE transmission according to provided configuration */
+void lll_df_conf_cte_tx_enable(uint8_t type, uint8_t length,
+			       uint8_t ant_num, uint8_t *ant_ids);
+/* Disables CTE transmission */
+void lll_df_conf_cte_tx_disable(void);

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

@@ -39,7 +39,9 @@
 #include "lll_tim_internal.h"
 #include "lll_adv_internal.h"
 #include "lll_prof_internal.h"
+#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
 #include "lll_df_internal.h"
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 
 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
 #define LOG_MODULE_NAME bt_ctlr_lll_adv
@@ -47,6 +49,14 @@
 #include "hal/debug.h"
 
 static int init_reset(void);
+
+static struct pdu_adv *adv_pdu_allocate(struct lll_adv_pdu *pdu, uint8_t last);
+#if IS_ENABLED(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);
+static int adv_extra_data_free(struct lll_adv_pdu *pdu, uint8_t last);
+#endif /* CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY */
+
 static int prepare_cb(struct lll_prepare_param *p);
 static int is_abort_cb(void *next, int prio, void *curr,
 		       lll_prepare_cb_t *resume_cb, int *resume_prio);
@@ -243,46 +253,6 @@ int lll_adv_data_release(struct lll_adv_pdu *pdu)
 	return 0;
 }
 
-static inline 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);
-
-	pdu->pdu[last] = (void *)p;
-
-	return p;
-}
-
 struct pdu_adv *lll_adv_pdu_alloc(struct lll_adv_pdu *pdu, uint8_t *idx)
 {
 	uint8_t first, last;
@@ -372,17 +342,6 @@ int lll_adv_and_extra_data_init(struct lll_adv_pdu *pdu)
 	return 0;
 }
 
-static inline void adv_extra_data_release(struct lll_adv_pdu *pdu, int idx)
-{
-	void *extra_data;
-
-	extra_data = pdu->extra_data[idx];
-	if (extra_data) {
-		pdu->extra_data[idx] = NULL;
-		mem_release(extra_data, &mem_extra_data.free);
-	}
-}
-
 int lll_adv_and_extra_data_release(struct lll_adv_pdu *pdu)
 {
 	uint8_t last;
@@ -410,46 +369,6 @@ int lll_adv_and_extra_data_release(struct lll_adv_pdu *pdu)
 	return 0;
 }
 
-static inline void *adv_extra_data_allocate(struct lll_adv_pdu *pdu,
-					    uint8_t last)
-{
-	void *extra_data;
-	int err;
-
-	extra_data = pdu->extra_data[last];
-	if (extra_data) {
-		return extra_data;
-	}
-
-	extra_data = MFIFO_DEQUEUE_PEEK(extra_data_free);
-	if (extra_data) {
-		err = k_sem_take(&sem_extra_data_free, K_NO_WAIT);
-		LL_ASSERT(!err);
-
-		MFIFO_DEQUEUE(extra_data_free);
-		pdu->extra_data[last] = extra_data;
-
-		return extra_data;
-	}
-
-	extra_data = mem_acquire(&mem_extra_data.free);
-	if (extra_data) {
-		pdu->extra_data[last] = extra_data;
-
-		return extra_data;
-	}
-
-	err = k_sem_take(&sem_extra_data_free, K_FOREVER);
-	LL_ASSERT(!err);
-
-	extra_data = MFIFO_DEQUEUE(extra_data_free);
-	LL_ASSERT(extra_data);
-
-	pdu->extra_data[last] = (void *)extra_data;
-
-	return extra_data;
-}
-
 struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu,
 						 void **extra_data,
 						 uint8_t *idx)
@@ -468,7 +387,7 @@ struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu,
 		uint8_t first_latest;
 
 		pdu->last = first;
-		cpu_dsb();
+		cpu_dmb();
 		first_latest = pdu->first;
 		if (first_latest != first) {
 			last++;
@@ -485,7 +404,18 @@ struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu,
 	if (extra_data) {
 		*extra_data = adv_extra_data_allocate(pdu, last);
 	} else {
-		pdu->extra_data[last] = NULL;
+		if (adv_extra_data_free(pdu, last)) {
+			LL_ASSERT(false);
+
+			/* 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
+			 * call to this function occures, no new memory will be
+			 * allocated. adv_pdu_allocate will return already
+			 * allocated memory.
+			 */
+			return NULL;
+		}
 	}
 
 	return p;
@@ -517,8 +447,9 @@ struct pdu_adv *lll_adv_pdu_and_extra_data_latest_get(struct lll_adv_pdu *pdu,
 		if (ed && (!MFIFO_ENQUEUE_IDX_GET(extra_data_free,
 						  &ed_free_idx))) {
 			LL_ASSERT(false);
-			/* ToDo what if enqueue fails and assert does not fire?
+			/* No pdu_free_idx clean up is required, sobsequent
-			 * pdu_free_idx should be released before return.
+			 * calls to MFIFO_ENQUEUE_IDX_GET return ther same
+			 * index to memory that is in limbo state.
 			 */
 			return NULL;
 		}
@@ -682,6 +613,121 @@ 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);
+
+	pdu->pdu[last] = (void *)p;
+
+	return p;
+}
+
+#if IS_ENABLED(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
+static void *adv_extra_data_allocate(struct lll_adv_pdu *pdu, uint8_t last)
+{
+	void *extra_data;
+	int err;
+
+	extra_data = pdu->extra_data[last];
+	if (extra_data) {
+		return extra_data;
+	}
+
+	extra_data = MFIFO_DEQUEUE_PEEK(extra_data_free);
+	if (extra_data) {
+		err = k_sem_take(&sem_extra_data_free, K_NO_WAIT);
+		LL_ASSERT(!err);
+
+		MFIFO_DEQUEUE(extra_data_free);
+		pdu->extra_data[last] = extra_data;
+
+		return extra_data;
+	}
+
+	extra_data = mem_acquire(&mem_extra_data.free);
+	if (extra_data) {
+		pdu->extra_data[last] = extra_data;
+
+		return extra_data;
+	}
+
+	err = k_sem_take(&sem_extra_data_free, K_FOREVER);
+	LL_ASSERT(!err);
+
+	extra_data = MFIFO_DEQUEUE(extra_data_free);
+	LL_ASSERT(extra_data);
+
+	pdu->extra_data[last] = (void *)extra_data;
+
+	return extra_data;
+}
+
+static int adv_extra_data_free(struct lll_adv_pdu *pdu, uint8_t last)
+{
+	uint8_t ed_free_idx;
+	void *ed;
+
+	ed = pdu->extra_data[last];
+
+	if (ed) {
+		if (!MFIFO_ENQUEUE_IDX_GET(extra_data_free, &ed_free_idx)) {
+			LL_ASSERT(false);
+			/* ToDo what if enqueue fails and assert does not fire?
+			 * pdu_free_idx should be released before return.
+			 */
+			return -ENOMEM;
+		}
+		pdu->extra_data[last] = NULL;
+
+		MFIFO_BY_IDX_ENQUEUE(extra_data_free, ed_free_idx, ed);
+		k_sem_give(&sem_extra_data_free);
+	}
+
+	return 0;
+}
+
+static inline void adv_extra_data_release(struct lll_adv_pdu *pdu, int idx)
+{
+	void *extra_data;
+
+	extra_data = pdu->extra_data[idx];
+	if (extra_data) {
+		pdu->extra_data[idx] = NULL;
+		mem_release(extra_data, &mem_extra_data.free);
+	}
+}
+#endif /* CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY */
+
 static int prepare_cb(struct lll_prepare_param *p)
 {
 	uint32_t ticks_at_event;

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

@@ -12,6 +12,7 @@
 #include "hal/ccm.h"
 #include "hal/radio.h"
 #include "hal/ticker.h"
+#include "hal/radio_df.h"
 
 #include "util/util.h"
 #include "util/memq.h"
@@ -28,6 +29,7 @@
 #include "lll_internal.h"
 #include "lll_adv_internal.h"
 #include "lll_tim_internal.h"
+#include "lll_df_internal.h"
 
 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
 #define LOG_MODULE_NAME bt_ctlr_lll_adv_sync
@@ -36,6 +38,7 @@
 
 static int init_reset(void);
 static int prepare_cb(struct lll_prepare_param *p);
+static void isr_done(void *param);
 
 int lll_adv_sync_init(void)
 {
@@ -93,6 +96,9 @@ static int init_reset(void)
 
 static int prepare_cb(struct lll_prepare_param *p)
 {
+#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+	struct lll_df_adv_cfg *df_cfg;
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 	struct lll_adv_sync *lll;
 	uint32_t ticks_at_event;
 	uint32_t ticks_at_start;
@@ -102,6 +108,7 @@ static int prepare_cb(struct lll_prepare_param *p)
 	struct evt_hdr *evt;
 	uint32_t remainder;
 	uint32_t start_us;
+	void *extra_data;
 	uint8_t phy_s;
 	uint8_t upd;
 
@@ -146,12 +153,33 @@ static int prepare_cb(struct lll_prepare_param *p)
 			     ((uint32_t)lll->crc_init[0])));
 	lll_chan_set(data_chan_use);
 
-	pdu = lll_adv_sync_data_latest_get(lll, NULL, &upd);
+	pdu = lll_adv_sync_data_latest_get(lll, &extra_data, &upd);
+#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+	if (extra_data) {
+		df_cfg = (struct lll_df_adv_cfg *)extra_data;
+		lll_df_conf_cte_tx_enable(df_cfg->cte_type, df_cfg->cte_length,
+					  df_cfg->ant_sw_len, df_cfg->ant_ids);
+		lll->cte_started = 1U;
+	} else {
+		df_cfg = NULL;
+		lll->cte_started = 0U;
+	}
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
+
 	radio_pkt_tx_set(pdu);
 
 	/* TODO: chaining */
 	radio_isr_set(lll_isr_done, lll);
-	radio_switch_complete_and_disable();
+	radio_isr_set(isr_done, lll);
+
+#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+	if (df_cfg) {
+		radio_switch_complete_and_phy_end_disable();
+	} else
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
+	{
+		radio_switch_complete_and_disable();
+	}
 
 	ticks_at_event = p->ticks_at_expire;
 	evt = HDR_LLL2EVT(lll);
@@ -193,3 +221,16 @@ static int prepare_cb(struct lll_prepare_param *p)
 
 	return 0;
 }
+
+static void isr_done(void *param)
+{
+	struct lll_adv_sync *lll;
+
+	lll = param;
+#if IS_ENABLED(CONFIG_BT_CTLR_DF_ADV_CTE_TX)
+	if (lll->cte_started) {
+		lll_df_conf_cte_tx_disable();
+	}
+#endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
+	lll_isr_done(lll);
+}

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

@@ -5,6 +5,8 @@
  */
 
 #include <stdint.h>
+#include <bluetooth/hci.h>
+
 #include "hal/radio_df.h"
 #include "lll_df.h"
 
@@ -47,6 +49,55 @@ uint8_t lll_df_ant_num_get(void)
 	return radio_df_ant_num_get();
 }
 
+/* @brief Function initializes transmission of Constant Tone Extension.
+ *
+ * @param[in] type      Type of CTE: AoA, AoD 1us, AoD 2us
+ * @param[in] length    Duration of CTE in 8us units
+ * @param[in] ant_num   Number of antennas in switch pattern
+ * @param[in] ant_ids   Antenna identifiers that create switch pattern.
+ *
+ * @Note Pay attention that first two antenna identifiers in a switch
+ * pattern has special purpose. First one is used in guard period, second
+ * in reference period. Actual switching is processed from third antenna.
+ *
+ * In case of AoA mode ant_num and ant_ids parameters are not used.
+ */
+void lll_df_conf_cte_tx_enable(uint8_t type, uint8_t length,
+			       uint8_t ant_num, uint8_t *ant_ids)
+{
+	if (type == BT_HCI_LE_AOA_CTE) {
+		radio_df_mode_set_aoa();
+	} else {
+		radio_df_mode_set_aod();
+
+		if (type == BT_HCI_LE_AOD_CTE_1US) {
+			radio_df_ant_switch_spacing_set_2us();
+		} else {
+			radio_df_ant_switch_spacing_set_4us();
+		}
+
+		radio_df_ant_switch_pattern_clear();
+		/* DFE extension in radio uses SWITCHPATTER[0] for guard period
+		 * SWITCHPATTER[1] for reference period. Bluetooth specification
+		 * does not include separate antenna patter for guard period.
+		 * To overcome that limitation ant_idx[0] is used twice:
+		 * for guard and reference period. On the other hand it limist
+		 * number of supported antenna switch patterns by one.
+		 */
+		radio_df_ant_switch_pattern_set(ant_ids[0]);
+		for (uint8_t idx = 0; idx < ant_num; ++idx) {
+			radio_df_ant_switch_pattern_set(ant_ids[idx]);
+		}
+	}
+
+	radio_df_cte_length_set(length);
+}
+
+void lll_df_conf_cte_tx_disable(void)
+{
+	radio_df_reset();
+}
+
 static int init_reset(void)
 {
 	return 0;