Bluetooth: Controller: Fix Extended Advertising channel use

Add implementation defined channel index in the auxiliary
pointer of the common extended payload format in the primary
channel PDUs and the same be used in the transmission of
auxiliary PDUs.

Fixes #35668.

Signed-off-by: Vinayak Kariappa Chettimada <[email protected]>

Author: cvinayak

subsys/bluetooth/controller/CMakeLists.txt

@@ -112,6 +112,8 @@ if(CONFIG_BT_LL_SW_SPLIT)
       )
   endif()
   if(CONFIG_BT_CONN OR
+     (CONFIG_BT_BROADCASTER AND
+      CONFIG_BT_CTLR_ADV_EXT) OR
      CONFIG_BT_CTLR_ADV_PERIODIC OR
      CONFIG_BT_CTLR_SYNC_PERIODIC)
     zephyr_library_sources(

subsys/bluetooth/controller/Kconfig

@@ -430,7 +430,7 @@ config BT_CTLR_PHY_CODED
 
 config BT_CTLR_CHAN_SEL_2
 	bool "Channel Selection Algorithm #2"
-	depends on (BT_CONN || BT_CTLR_ADV_PERIODIC || BT_CTLR_SYNC_PERIODIC) && BT_CTLR_CHAN_SEL_2_SUPPORT
+	depends on (BT_CONN || (BT_BROADCASTER && BT_CTLR_ADV_EXT) || BT_CTLR_ADV_PERIODIC || BT_CTLR_SYNC_PERIODIC) && BT_CTLR_CHAN_SEL_2_SUPPORT
 	default y
 	help
 	  Enable support for Bluetooth 5.0 LE Channel Selection Algorithm #2 in

subsys/bluetooth/controller/ll_sw/lll_adv.h

@@ -56,6 +56,14 @@ struct lll_adv_aux {
 	struct lll_hdr hdr;
 	struct lll_adv *adv;
 
+	/* Implementation defined radio event counter to calculate auxiliary
+	 * PDU channel index.
+	 */
+	uint16_t data_chan_counter;
+
+	/* Temporary stored use by primary channel PDU event to fill the
+	 * auxiliary offset to this auxiliary PDU event.
+	 */
 	uint32_t ticks_offset;
 
 	struct lll_adv_pdu data;

subsys/bluetooth/controller/ll_sw/lll_adv_aux.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020 Nordic Semiconductor ASA
+ * Copyright (c) 2018-2021 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -10,5 +10,6 @@ void lll_adv_aux_prepare(void *param);
 
 extern uint8_t ull_adv_aux_lll_handle_get(struct lll_adv_aux *lll);
 extern struct pdu_adv_aux_ptr *
-ull_adv_aux_lll_offset_fill(uint32_t ticks_offset, uint32_t start_us,
-			    struct pdu_adv *pdu);
+	ull_adv_aux_lll_offset_fill(struct pdu_adv *pdu,
+				    uint32_t ticks_offset,
+				    uint32_t start_us);

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

@@ -1217,9 +1217,13 @@ static void isr_done(void *param)
 		start_us = radio_tmr_start_now(1);
 
 #if defined(CONFIG_BT_CTLR_ADV_EXT)
-		if (lll->aux) {
-			ull_adv_aux_lll_offset_fill(lll->aux->ticks_offset,
-						    start_us, pdu);
+		struct lll_adv_aux *lll_aux;
+
+		lll_aux = lll->aux;
+		if (lll_aux) {
+			(void)ull_adv_aux_lll_offset_fill(pdu,
+							  lll_aux->ticks_offset,
+							  start_us);
 		}
 #else /* !CONFIG_BT_CTLR_ADV_EXT */
 		ARG_UNUSED(pdu);

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

@@ -164,6 +164,9 @@ static int prepare_cb(struct lll_prepare_param *p)
 		return 0;
 	}
 
+	/* Increment counter used in ULL for channel index calculation */
+	lll->data_chan_counter++;
+
 	/* Set up Radio H/W */
 	radio_reset();
 

subsys/bluetooth/controller/ll_sw/ull_adv_aux.c

@@ -24,6 +24,7 @@
 #include "lll.h"
 #include "lll_clock.h"
 #include "lll/lll_vendor.h"
+#include "lll_chan.h"
 #include "lll/lll_adv_types.h"
 #include "lll_adv.h"
 #include "lll/lll_adv_pdu.h"
@@ -33,6 +34,7 @@
 #include "ull_adv_types.h"
 
 #include "ull_internal.h"
+#include "ull_chan_internal.h"
 #include "ull_adv_internal.h"
 
 #include "ll.h"
@@ -449,6 +451,43 @@ uint8_t const *ull_adv_aux_random_addr_get(struct ll_adv_set const *const adv,
 	return adv->rnd_addr;
 }
 
+uint8_t ull_adv_aux_chm_update(void)
+{
+	/* For each created extended advertising set */
+	for (uint8_t handle = 0; handle < BT_CTLR_ADV_SET; ++handle) {
+		struct ll_adv_aux_set *aux;
+		struct ll_adv_set *adv;
+		uint8_t chm_last;
+
+		adv = ull_adv_is_created_get(handle);
+		if (!adv || !adv->lll.aux) {
+			continue;
+		}
+
+		aux = HDR_LLL2ULL(adv->lll.aux);
+		if (aux->chm_last != aux->chm_first) {
+			/* TODO: Handle previous Channel Map Update being in
+			 * progress
+			 */
+			continue;
+		}
+
+		/* Append the channelMapNew that will be picked up by ULL */
+		chm_last = aux->chm_last + 1;
+		if (chm_last == DOUBLE_BUFFER_SIZE) {
+			chm_last = 0U;
+		}
+		aux->chm[chm_last].data_chan_count =
+			ull_chan_map_get(aux->chm[chm_last].data_chan_map);
+		aux->chm_last = chm_last;
+	}
+
+	/* TODO: Should failure due to Channel Map Update being already in
+	 *       progress be returned to caller?
+	 */
+	return 0;
+}
+
 uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv,
 				  uint16_t sec_hdr_add_fields,
 				  uint16_t sec_hdr_rem_fields,
@@ -903,17 +942,18 @@ void ull_adv_aux_ptr_fill(uint8_t **dptr, uint8_t phy_s)
 {
 	struct pdu_adv_aux_ptr *aux_ptr;
 
+	/* NOTE: Channel Index and Aux Offset will be set on every advertiser's
+	 * event prepare when finding the auxiliary event's ticker offset.
+	 */
+
 	*dptr -= sizeof(struct pdu_adv_aux_ptr);
 	aux_ptr = (void *)*dptr;
 
-	/* FIXME: implementation defined */
 	aux_ptr->chan_idx = 0U;
 
 	aux_ptr->ca = (lll_clock_ppm_local_get() <= SCA_50_PPM) ?
 		      SCA_VALUE_50_PPM : SCA_VALUE_500_PPM;
 
-	/* NOTE: Aux Offset will be set in advertiser LLL event
-	 */
 	aux_ptr->offs_units = 0U;
 	aux_ptr->offs = 0U;
 
@@ -1015,6 +1055,7 @@ struct ll_adv_aux_set *ull_adv_aux_acquire(struct lll_adv *lll)
 {
 	struct lll_adv_aux *lll_aux;
 	struct ll_adv_aux_set *aux;
+	uint8_t chm_last;
 	int err;
 
 	aux = aux_acquire();
@@ -1032,6 +1073,18 @@ struct ll_adv_aux_set *ull_adv_aux_acquire(struct lll_adv *lll)
 		return NULL;
 	}
 
+	/* Initialize data channel calculation counter, data channel identifier,
+	 * and channel map to use.
+	 */
+	lll_csrand_get(&lll_aux->data_chan_counter,
+		       sizeof(lll_aux->data_chan_counter));
+	lll_csrand_get(&aux->data_chan_id, sizeof(aux->data_chan_id));
+	chm_last = aux->chm_first;
+	aux->chm_last = chm_last;
+	aux->chm[chm_last].data_chan_count =
+		ull_chan_map_get(aux->chm[chm_last].data_chan_map);
+
+
 	/* NOTE: ull_hdr_init(&aux->ull); is done on start */
 	lll_hdr_init(lll_aux, aux);
 
@@ -1063,12 +1116,12 @@ void ull_adv_aux_offset_get(struct ll_adv_set *adv)
 	LL_ASSERT(!ret);
 }
 
-struct pdu_adv_aux_ptr *ull_adv_aux_lll_offset_fill(uint32_t ticks_offset,
-						    uint32_t start_us,
-						    struct pdu_adv *pdu)
+struct pdu_adv_aux_ptr *ull_adv_aux_lll_offset_fill(struct pdu_adv *pdu,
+						    uint32_t ticks_offset,
+						    uint32_t start_us)
 {
 	struct pdu_adv_com_ext_adv *pri_com_hdr;
-	struct pdu_adv_aux_ptr *aux;
+	struct pdu_adv_aux_ptr *aux_ptr;
 	struct pdu_adv_ext_hdr *h;
 	uint32_t offs;
 	uint8_t *ptr;
@@ -1085,18 +1138,18 @@ struct pdu_adv_aux_ptr *ull_adv_aux_lll_offset_fill(uint32_t ticks_offset,
 		ptr += sizeof(struct pdu_adv_adi);
 	}
 
-	aux = (void *)ptr;
+	aux_ptr = (void *)ptr;
 	offs = HAL_TICKER_TICKS_TO_US(ticks_offset) - start_us;
 	offs = offs / OFFS_UNIT_30_US;
 	if (!!(offs >> 13)) {
-		aux->offs = offs / (OFFS_UNIT_300_US / OFFS_UNIT_30_US);
-		aux->offs_units = 1U;
+		aux_ptr->offs = offs / (OFFS_UNIT_300_US / OFFS_UNIT_30_US);
+		aux_ptr->offs_units = 1U;
 	} else {
-		aux->offs = offs;
-		aux->offs_units = 0U;
+		aux_ptr->offs = offs;
+		aux_ptr->offs_units = 0U;
 	}
 
-	return aux;
+	return aux_ptr;
 }
 
 void ull_adv_aux_done(struct node_rx_event_done *done)
@@ -1211,16 +1264,22 @@ static uint8_t aux_time_update(struct ll_adv_aux_set *aux, struct pdu_adv *pdu,
 
 static void mfy_aux_offset_get(void *param)
 {
-	struct ll_adv_set *adv = param;
+	struct pdu_adv_aux_ptr *aux_ptr;
+	struct lll_adv_aux *lll_aux;
 	struct ll_adv_aux_set *aux;
 	uint32_t ticks_to_expire;
+	uint8_t data_chan_count;
+	uint8_t *data_chan_map;
 	uint32_t ticks_current;
+	struct ll_adv_set *adv;
 	struct pdu_adv *pdu;
 	uint8_t ticker_id;
 	uint8_t retry;
 	uint8_t id;
 
-	aux = HDR_LLL2ULL(adv->lll.aux);
+	adv = param;
+	lll_aux = adv->lll.aux;
+	aux = HDR_LLL2ULL(lll_aux);
 	ticker_id = TICKER_ID_ADV_AUX_BASE + ull_adv_aux_handle_get(aux);
 
 	id = TICKER_NULL;
@@ -1259,19 +1318,32 @@ static void mfy_aux_offset_get(void *param)
 	/* Store the ticks offset for population in other advertising primary
 	 * channel PDUs.
 	 */
-	aux->lll.ticks_offset = ticks_to_expire;
+	lll_aux->ticks_offset = ticks_to_expire;
 
 	/* NOTE: as remainder used in scheduling primary PDU not available,
 	 * compensate with a probable jitter of one ticker resolution unit that
 	 * would be included in the packet timer capture when scheduling next
 	 * advertising primary channel PDU.
 	 */
-	aux->lll.ticks_offset +=
+	lll_aux->ticks_offset +=
 		HAL_TICKER_US_TO_TICKS(EVENT_TICKER_RES_MARGIN_US);
 
-	/* FIXME: we are in ULL_LOW context, fill offset in LLL context */
-	pdu = lll_adv_data_curr_get(&adv->lll);
-	ull_adv_aux_lll_offset_fill(ticks_to_expire, 0, pdu);
+	/* FIXME: we are in ULL_LOW context, fill offset in LLL context? */
+	pdu = lll_adv_data_latest_peek(&adv->lll);
+	aux_ptr = ull_adv_aux_lll_offset_fill(pdu, ticks_to_expire, 0);
+
+	/* Process channel map update, if any */
+	if (aux->chm_first != aux->chm_last) {
+		/* Use channelMapNew */
+		aux->chm_first = aux->chm_last;
+	}
+
+	/* Calculate the radio channel to use */
+	data_chan_map = aux->chm[aux->chm_first].data_chan_map;
+	data_chan_count = aux->chm[aux->chm_first].data_chan_count;
+	aux_ptr->chan_idx = lll_chan_sel_2(lll_aux->data_chan_counter,
+					   aux->data_chan_id,
+					   data_chan_map, data_chan_count);
 }
 
 static void ticker_cb(uint32_t ticks_at_expire, uint32_t ticks_drift,

subsys/bluetooth/controller/ll_sw/ull_adv_internal.h

@@ -76,6 +76,9 @@ uint8_t ull_adv_aux_handle_get(struct ll_adv_aux_set *aux);
 uint8_t const *ull_adv_aux_random_addr_get(struct ll_adv_set const *const adv,
 					   uint8_t *const addr);
 
+/* Helper function to apply Channel Map Update for auxiliary PDUs */
+uint8_t ull_adv_aux_chm_update(void);
+
 /* helper function to initialize event timings */
 uint32_t ull_adv_aux_evt_init(struct ll_adv_aux_set *aux);
 

subsys/bluetooth/controller/ll_sw/ull_adv_types.h

@@ -71,6 +71,14 @@ struct ll_adv_aux_set {
 
 	uint16_t interval;
 
+	uint16_t data_chan_id;
+	struct {
+		uint8_t data_chan_map[PDU_CHANNEL_MAP_SIZE];
+		uint8_t data_chan_count:6;
+	} chm[DOUBLE_BUFFER_SIZE];
+	uint8_t  chm_first;
+	uint8_t  chm_last;
+
 	uint8_t is_started:1;
 };
 

subsys/bluetooth/controller/ll_sw/ull_chan.c

@@ -52,6 +52,10 @@ uint8_t ll_chm_update(uint8_t const *const chm)
 	(void)ull_adv_sync_chm_update();
 #endif /* CONFIG_BT_CTLR_ADV_PERIODIC */
 
+#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
+	(void)ull_adv_aux_chm_update();
+#endif /*(CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
+
 	/* TODO: Should failure due to Channel Map Update being already in
 	 *       progress be returned to caller?
 	 */