Bluetooth: Controller: Fix private address update in auxiliary PDUs

Fix missing implementation to update private resolvable
address in auxiliary PDUs on RPA timeout.

Without the implementation the controller asserted on RPA
timeout when AdvA is not present in the primary channel PDU
of Extended Advertising set.

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

Author: cvinayak

subsys/bluetooth/controller/ll_sw/ull_filter.c

@@ -777,12 +777,15 @@ void ull_filter_rpa_update(bool timeout)
 
 	if (timeout) {
 #if defined(CONFIG_BT_BROADCASTER)
-		struct ll_adv_set *adv;
+		uint8_t handle;
 
-		/* TODO: foreach adv set */
-		adv = ull_adv_is_enabled_get(0);
-		if (adv) {
-			rpa_adv_refresh(adv);
+		for (handle = 0U; handle < BT_CTLR_ADV_SET; handle++) {
+			struct ll_adv_set *adv;
+
+			adv = ull_adv_is_enabled_get(handle);
+			if (adv) {
+				rpa_adv_refresh(adv);
+			}
 		}
 #endif
 	}
@@ -1131,26 +1134,101 @@ static void rl_update(void)
 #if defined(CONFIG_BT_BROADCASTER)
 static void rpa_adv_refresh(struct ll_adv_set *adv)
 {
+	struct lll_adv_aux *lll_aux;
 	struct pdu_adv *prev;
+	struct lll_adv *lll;
 	struct pdu_adv *pdu;
-	uint8_t idx;
+	uint8_t pri_idx;
+
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+	uint8_t sec_idx;
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
 
 	if (adv->own_addr_type != BT_ADDR_LE_PUBLIC_ID &&
 	    adv->own_addr_type != BT_ADDR_LE_RANDOM_ID) {
 		return;
 	}
 
-	if (adv->lll.rl_idx >= ARRAY_SIZE(rl)) {
+	lll = &adv->lll;
+	if (lll->rl_idx >= ARRAY_SIZE(rl)) {
 		return;
 	}
 
-	prev = lll_adv_data_peek(&adv->lll);
-	pdu = lll_adv_data_alloc(&adv->lll, &idx);
 
-	(void)memcpy(pdu, prev, PDU_AC_LL_HEADER_SIZE + prev->len);
-	ull_adv_pdu_update_addrs(adv, pdu);
+	pri_idx = UINT8_MAX;
+	lll_aux = NULL;
+	pdu = NULL;
+	prev = lll_adv_data_peek(lll);
+
+	if (false) {
+
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+	} else if (prev->type == PDU_ADV_TYPE_EXT_IND) {
+		struct pdu_adv_com_ext_adv *pri_com_hdr;
+		struct pdu_adv_ext_hdr pri_hdr_flags;
+		struct pdu_adv_ext_hdr *pri_hdr;
+
+		/* Pick the primary PDU header flags */
+		pri_com_hdr = (void *)&prev->adv_ext_ind;
+		pri_hdr = (void *)pri_com_hdr->ext_hdr_adv_data;
+		if (pri_com_hdr->ext_hdr_len) {
+			pri_hdr_flags = *pri_hdr;
+		} else {
+			*(uint8_t *)&pri_hdr_flags = 0U;
+		}
+
+		/* AdvA, in primary or auxiliary PDU */
+		if (pri_hdr_flags.adv_addr) {
+			pdu = lll_adv_data_alloc(lll, &pri_idx);
+			(void)memcpy(pdu, prev, (PDU_AC_LL_HEADER_SIZE +
+						 prev->len));
+
+#if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
+		} else if (pri_hdr_flags.aux_ptr) {
+			struct pdu_adv_com_ext_adv *sec_com_hdr;
+			struct pdu_adv_ext_hdr sec_hdr_flags;
+			struct pdu_adv_ext_hdr *sec_hdr;
+			struct pdu_adv *sec_pdu;
+
+			lll_aux = lll->aux;
+			sec_pdu = lll_adv_aux_data_peek(lll_aux);
+
+			sec_com_hdr = (void *)&sec_pdu->adv_ext_ind;
+			sec_hdr = (void *)sec_com_hdr->ext_hdr_adv_data;
+			if (sec_com_hdr->ext_hdr_len) {
+				sec_hdr_flags = *sec_hdr;
+			} else {
+				*(uint8_t *)&sec_hdr_flags = 0U;
+			}
+
+			if (sec_hdr_flags.adv_addr) {
+				pdu = lll_adv_aux_data_alloc(lll_aux, &sec_idx);
+				(void)memcpy(pdu, sec_pdu,
+					     (PDU_AC_LL_HEADER_SIZE +
+					      sec_pdu->len));
+			}
+#endif /* (CONFIG_BT_CTLR_ADV_AUX_SET > 0) */
+		}
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
+
+	} else {
+		pdu = lll_adv_data_alloc(lll, &pri_idx);
+		(void)memcpy(pdu, prev, (PDU_AC_LL_HEADER_SIZE + prev->len));
+	}
+
+	if (pdu) {
+		ull_adv_pdu_update_addrs(adv, pdu);
+
+		if (pri_idx != UINT8_MAX) {
+			lll_adv_data_enqueue(lll, pri_idx);
+
+#if defined(CONFIG_BT_CTLR_ADV_EXT)
+		} else {
+			lll_adv_aux_data_enqueue(lll_aux, sec_idx);
+#endif /* CONFIG_BT_CTLR_ADV_EXT */
 
-	lll_adv_data_enqueue(&adv->lll, idx);
+		}
+	}
 }
 #endif /* CONFIG_BT_BROADCASTER */