Bluetooth: Controller: Fix Extended Adv Chain PDU channel indices

Fix missing population of channel indices for Extended
Advertising Chain PDUs.

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

Author: cvinayak

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

@@ -49,12 +49,20 @@
 
 static int init_reset(void);
 static int prepare_cb(struct lll_prepare_param *p);
+#if !defined(CONFIG_BT_TICKER_EXT_EXPIRE_INFO) || \
+	defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
+static int aux_ptr_get(struct pdu_adv *pdu, struct pdu_adv_aux_ptr **aux_ptr);
+#endif /* !CONFIG_BT_TICKER_EXT_EXPIRE_INFO ||
+	* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK
+	*/
 #if !defined(CONFIG_BT_TICKER_EXT_EXPIRE_INFO)
 static void isr_early_abort(void *param);
 #endif /* !CONFIG_BT_TICKER_EXT_EXPIRE_INFO */
 static void isr_done(void *param);
 #if defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
 static void isr_tx_chain(void *param);
+static void chain_pdu_aux_ptr_chan_idx_set(struct lll_adv_aux *lll);
+static void aux_ptr_chan_idx_set(struct lll_adv_aux *lll, struct pdu_adv *pdu);
 #endif /* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
 static void isr_tx_rx(void *param);
 static void isr_rx(void *param);
@@ -150,47 +158,22 @@ static int prepare_cb(struct lll_prepare_param *p)
 
 #else /* !CONFIG_BT_TICKER_EXT_EXPIRE_INFO */
 	struct pdu_adv_aux_ptr *aux_ptr;
-	struct pdu_adv_ext_hdr *pri_hdr;
 	struct pdu_adv *pri_pdu;
-	uint8_t *pri_dptr;
+	int err;
 
 	/* Get reference to primary PDU */
 	pri_pdu = lll_adv_data_curr_get(lll_adv);
 	LL_ASSERT(pri_pdu->type == PDU_ADV_TYPE_EXT_IND);
 
-	/* Get reference to extended header */
+	/* Get reference to common extended header */
 	com_hdr = (void *)&pri_pdu->adv_ext_ind;
-	pri_hdr = (void *)com_hdr->ext_hdr_adv_data;
-	pri_dptr = pri_hdr->data;
-
-	/* NOTE: We shall be here in auxiliary PDU prepare due to
-	 * aux_ptr flag being set in the extended common header
-	 * flags. Hence, ext_hdr_len is non-zero, an explicit check
-	 * is not needed.
-	 */
-	LL_ASSERT(com_hdr->ext_hdr_len);
-
-	/* traverse through adv_addr, if present */
-	if (pri_hdr->adv_addr) {
-		pri_dptr += BDADDR_SIZE;
-	}
-
-	/* traverse through tgt_addr, if present */
-	if (pri_hdr->tgt_addr) {
-		pri_dptr += BDADDR_SIZE;
-	}
-
-	/* No CTEInfo flag in primary and secondary channel PDU */
 
-	/* traverse through adi, if present */
-	if (pri_hdr->adi) {
-		pri_dptr += sizeof(struct pdu_adv_adi);
-	}
-
-	aux_ptr = (void *)pri_dptr;
+	/* Get reference to aux pointer structure */
+	err = aux_ptr_get(pri_pdu, &aux_ptr);
+	LL_ASSERT(!err);
 
 	/* Abort if no aux_ptr filled */
-	if (unlikely(!pri_hdr->aux_ptr || !PDU_ADV_AUX_PTR_OFFSET_GET(aux_ptr))) {
+	if (unlikely(!aux_ptr || !PDU_ADV_AUX_PTR_OFFSET_GET(aux_ptr))) {
 		radio_isr_set(isr_early_abort, lll);
 		radio_disable();
 
@@ -200,7 +183,7 @@ static int prepare_cb(struct lll_prepare_param *p)
 	chan_idx = aux_ptr->chan_idx;
 #endif /* !CONFIG_BT_TICKER_EXT_EXPIRE_INFO */
 
-	/* Increment counter used in ULL for channel index calculation */
+	/* Increment counter, for next channel index calculation */
 	lll->data_chan_counter++;
 
 	/* Set up Radio H/W */
@@ -228,9 +211,6 @@ static int prepare_cb(struct lll_prepare_param *p)
 	/* Use channel idx calculated or that was in aux_ptr */
 	lll_chan_set(chan_idx);
 
-	/* Set the Radio Tx Packet */
-	radio_pkt_tx_set(sec_pdu);
-
 	/* Switch to Rx if connectable or scannable */
 	if (com_hdr->adv_mode & (BT_HCI_LE_ADV_PROP_CONN |
 				     BT_HCI_LE_ADV_PROP_SCAN)) {
@@ -282,19 +262,38 @@ static int prepare_cb(struct lll_prepare_param *p)
 #if defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
 	} else if (sec_pdu->adv_ext_ind.ext_hdr_len &&
 		   sec_pdu->adv_ext_ind.ext_hdr.aux_ptr) {
+		/* Set the last used auxiliary PDU for transmission */
 		lll->last_pdu = sec_pdu;
 
+		/* Populate chan idx for AUX_ADV_IND PDU */
+		aux_ptr_chan_idx_set(lll, sec_pdu);
+
 		radio_isr_set(isr_tx_chain, lll);
 		radio_tmr_tifs_set(EVENT_B2B_MAFS_US);
 		radio_switch_complete_and_b2b_tx(phy_s, lll_adv->phy_flags,
 						 phy_s, lll_adv->phy_flags);
-#endif /* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
+	} else {
+		/* No chain PDU */
+		lll->last_pdu = NULL;
 
+#else /* !CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
 	} else {
+#endif /* !CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
+
 		radio_isr_set(isr_done, lll);
 		radio_switch_complete_and_disable();
 	}
 
+	if (IS_ENABLED(CONFIG_BT_CTLR_ADV_PERIODIC) &&
+	    IS_ENABLED(CONFIG_BT_TICKER_EXT_EXPIRE_INFO) &&
+	    sec_pdu->adv_ext_ind.ext_hdr_len &&
+	    sec_pdu->adv_ext_ind.ext_hdr.sync_info) {
+		ull_adv_sync_lll_syncinfo_fill(sec_pdu, lll);
+	}
+
+	/* Set the Radio Tx Packet */
+	radio_pkt_tx_set(sec_pdu);
+
 	ticks_at_event = p->ticks_at_expire;
 	ull = HDR_LLL2ULL(lll);
 	ticks_at_event += lll_event_offset_get(ull);
@@ -335,12 +334,10 @@ static int prepare_cb(struct lll_prepare_param *p)
 	}
 #endif /* CONFIG_BT_CTLR_XTAL_ADVANCED */
 
-	if (IS_ENABLED(CONFIG_BT_CTLR_ADV_PERIODIC) &&
-	    IS_ENABLED(CONFIG_BT_TICKER_EXT_EXPIRE_INFO) &&
-	    sec_pdu->adv_ext_ind.ext_hdr_len &&
-	    sec_pdu->adv_ext_ind.ext_hdr.sync_info) {
-		ull_adv_sync_lll_syncinfo_fill(sec_pdu, lll);
-	}
+#if defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
+	/* Populate chan idx for AUX_CHAIN_IND PDU */
+	chain_pdu_aux_ptr_chan_idx_set(lll);
+#endif /* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
 
 	ret = lll_prepare_done(lll);
 	LL_ASSERT(!ret);
@@ -350,6 +347,57 @@ static int prepare_cb(struct lll_prepare_param *p)
 	return 0;
 }
 
+#if !defined(CONFIG_BT_TICKER_EXT_EXPIRE_INFO) || \
+	defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
+static int aux_ptr_get(struct pdu_adv *pdu, struct pdu_adv_aux_ptr **aux_ptr)
+{
+	struct pdu_adv_com_ext_adv *com_hdr;
+	struct pdu_adv_ext_hdr *hdr;
+	uint8_t *dptr;
+
+	/* Get reference to common extended header */
+	com_hdr = (void *)&pdu->adv_ext_ind;
+	if (com_hdr->ext_hdr_len == 0U) {
+		*aux_ptr = NULL;
+
+		return -EINVAL;
+	}
+
+	/* Get reference to extended header flags and header fields */
+	hdr = (void *)com_hdr->ext_hdr_adv_data;
+	dptr = hdr->data;
+
+	/* traverse through adv_addr, if present */
+	if (hdr->adv_addr) {
+		dptr += BDADDR_SIZE;
+	}
+
+	/* traverse through tgt_addr, if present */
+	if (hdr->tgt_addr) {
+		dptr += BDADDR_SIZE;
+	}
+
+	/* No CTEInfo flag in primary and secondary channel PDU */
+
+	/* traverse through adi, if present */
+	if (hdr->adi) {
+		dptr += sizeof(struct pdu_adv_adi);
+	}
+
+	/* check for aux_ptr flag */
+	if (hdr->aux_ptr) {
+		/* Return reference to aux pointer structure */
+		*aux_ptr = (void *)dptr;
+	} else {
+		*aux_ptr = NULL;
+	}
+
+	return 0;
+}
+#endif /* !CONFIG_BT_TICKER_EXT_EXPIRE_INFO ||
+	* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK
+	*/
+
 #if !defined(CONFIG_BT_TICKER_EXT_EXPIRE_INFO)
 static void isr_race(void *param)
 {
@@ -395,9 +443,11 @@ static void isr_done(void *param)
 #if defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
 static void isr_tx_chain(void *param)
 {
+	struct pdu_adv_aux_ptr *aux_ptr;
 	struct lll_adv_aux *lll_aux;
 	struct lll_adv *lll;
 	struct pdu_adv *pdu;
+	int err;
 
 	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
 		lll_prof_latency_capture();
@@ -406,14 +456,22 @@ static void isr_tx_chain(void *param)
 	/* Clear radio tx status and events */
 	lll_isr_tx_status_reset();
 
+	/* Get reference to auxiliary and primary advertising LLL contexts */
 	lll_aux = param;
 	lll = lll_aux->adv;
 
-	/* FIXME: Use implementation defined channel index */
-	lll_chan_set(0);
+	/* Get reference to aux pointer structure */
+	err = aux_ptr_get(lll_aux->last_pdu, &aux_ptr);
+	LL_ASSERT(!err && aux_ptr);
 
+	/* Use channel idx that was in aux_ptr */
+	lll_chan_set(aux_ptr->chan_idx);
+
+	/* Get reference to the auxiliary chain PDU */
 	pdu = lll_adv_pdu_linked_next_get(lll_aux->last_pdu);
 	LL_ASSERT(pdu);
+
+	/* Set the last used auxiliary PDU for transmission */
 	lll_aux->last_pdu = pdu;
 
 	/* setup tIFS switching */
@@ -458,10 +516,56 @@ static void isr_tx_chain(void *param)
 				 HAL_RADIO_GPIO_PA_OFFSET);
 #endif /* HAL_RADIO_GPIO_HAVE_PA_PIN */
 
+	/* Populate chan idx for AUX_CHAIN_IND PDU */
+	chain_pdu_aux_ptr_chan_idx_set(lll_aux);
+
 	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
 		lll_prof_send();
 	}
 }
+
+static void chain_pdu_aux_ptr_chan_idx_set(struct lll_adv_aux *lll)
+{
+	struct pdu_adv *chain_pdu;
+
+	/* No chain PDU */
+	if (!lll->last_pdu) {
+		return;
+	}
+
+	/* Get reference to the auxiliary chain PDU */
+	chain_pdu = lll_adv_pdu_linked_next_get(lll->last_pdu);
+
+	/* Check if there is further chain PDU */
+	if (chain_pdu && chain_pdu->adv_ext_ind.ext_hdr_len &&
+	    chain_pdu->adv_ext_ind.ext_hdr.aux_ptr) {
+		aux_ptr_chan_idx_set(lll, chain_pdu);
+	}
+}
+
+static void aux_ptr_chan_idx_set(struct lll_adv_aux *lll, struct pdu_adv *pdu)
+{
+	struct pdu_adv_aux_ptr *aux_ptr;
+	struct ll_adv_aux_set *aux;
+	uint8_t chan_idx;
+	int err;
+
+	/* Get reference to aux pointer structure */
+	err = aux_ptr_get(pdu, &aux_ptr);
+	LL_ASSERT(!err && aux_ptr);
+
+	/* Calculate a new channel index */
+	aux = HDR_LLL2ULL(lll);
+	chan_idx = lll_chan_sel_2(lll->data_chan_counter, aux->data_chan_id,
+				  aux->chm[aux->chm_first].data_chan_map,
+				  aux->chm[aux->chm_first].data_chan_count);
+
+	/* Increment counter, for next channel index calculation */
+	lll->data_chan_counter++;
+
+	/* Set the channel index for the auxiliary chain PDU */
+	aux_ptr->chan_idx = chan_idx;
+}
 #endif /* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
 
 static void isr_tx_rx(void *param)
@@ -682,6 +786,7 @@ static inline int isr_rx_pdu(struct lll_adv_aux *lll_aux, uint8_t phy_flags_rx,
 #if defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
 		} else if (sr_pdu->adv_ext_ind.ext_hdr_len &&
 			   sr_pdu->adv_ext_ind.ext_hdr.aux_ptr) {
+			/* Set the last used auxiliary PDU for transmission */
 			lll_aux->last_pdu = sr_pdu;
 
 			radio_isr_set(isr_tx_chain, lll_aux);
@@ -710,20 +815,6 @@ static inline int isr_rx_pdu(struct lll_adv_aux *lll_aux, uint8_t phy_flags_rx,
 			lll_prof_cputime_capture();
 		}
 
-#if defined(CONFIG_BT_CTLR_SCAN_REQ_NOTIFY)
-		if (lll->scan_req_notify) {
-			uint32_t err;
-
-			/* Generate the scan request event */
-			err = lll_adv_scan_req_report(lll, pdu_rx, rl_idx,
-						      rssi_ready);
-			if (err) {
-				/* Scan Response will not be transmitted */
-				return err;
-			}
-		}
-#endif /* CONFIG_BT_CTLR_SCAN_REQ_NOTIFY */
-
 #if defined(HAL_RADIO_GPIO_HAVE_PA_PIN)
 		if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
 			/* PA/LNA enable is overwriting packet end used in ISR
@@ -739,6 +830,26 @@ static inline int isr_rx_pdu(struct lll_adv_aux *lll_aux, uint8_t phy_flags_rx,
 								  phy_flags_rx) -
 					 HAL_RADIO_GPIO_PA_OFFSET);
 #endif /* HAL_RADIO_GPIO_HAVE_PA_PIN */
+
+#if defined(CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK)
+		/* Populate chan idx for AUX_CHAIN_IND PDU */
+		chain_pdu_aux_ptr_chan_idx_set(lll_aux);
+#endif /* CONFIG_BT_CTLR_ADV_AUX_PDU_BACK2BACK */
+
+#if defined(CONFIG_BT_CTLR_SCAN_REQ_NOTIFY)
+		if (lll->scan_req_notify) {
+			uint32_t err;
+
+			/* Generate the scan request event */
+			err = lll_adv_scan_req_report(lll, pdu_rx, rl_idx,
+						      rssi_ready);
+			if (err) {
+				/* Scan Response will not be transmitted */
+				return err;
+			}
+		}
+#endif /* CONFIG_BT_CTLR_SCAN_REQ_NOTIFY */
+
 		return 0;
 
 #if defined(CONFIG_BT_PERIPHERAL)