Bluetooth: controller: Fix populate offset in latest advertising PDU

Fix implementation to populate the aux, and sync offset
in the latest PDU. If both the current and latest of the
double buffer has been filled and LLL did not pick the
latest PDU, then the offset should be filled into the latest
PDU (and not into the first/current PDU).

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

Author: cvinayak

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h

@@ -65,6 +65,28 @@ static inline struct pdu_adv *lll_adv_scan_rsp_peek(struct lll_adv *lll)
 	return (void *)lll->scan_rsp.pdu[lll->scan_rsp.last];
 }
 
+static inline struct pdu_adv *
+lll_adv_pdu_latest_peek(const struct lll_adv_pdu *const pdu)
+{
+	uint8_t first;
+
+	first = pdu->first;
+	if (first != pdu->last) {
+		first += 1U;
+		if (first == DOUBLE_BUFFER_SIZE) {
+			first = 0U;
+		}
+	}
+
+	return (void *)pdu->pdu[first];
+}
+
+static inline struct pdu_adv *
+lll_adv_data_latest_peek(const struct lll_adv *const lll)
+{
+	return lll_adv_pdu_latest_peek(&lll->adv_data);
+}
+
 #if defined(CONFIG_BT_CTLR_ADV_EXT)
 static inline struct pdu_adv *lll_adv_aux_data_alloc(struct lll_adv_aux *lll,
 						     uint8_t *idx)
@@ -83,6 +105,12 @@ static inline struct pdu_adv *lll_adv_aux_data_peek(struct lll_adv_aux *lll)
 	return (void *)lll->data.pdu[lll->data.last];
 }
 
+static inline struct pdu_adv *
+lll_adv_aux_data_latest_peek(const struct lll_adv_aux *const lll)
+{
+	return lll_adv_pdu_latest_peek(&lll->data);
+}
+
 static inline struct pdu_adv *lll_adv_aux_data_curr_get(struct lll_adv_aux *lll)
 {
 	return (void *)lll->data.pdu[lll->data.first];
@@ -139,6 +167,12 @@ static inline struct pdu_adv *lll_adv_sync_data_peek(struct lll_adv_sync *lll,
 	return (void *)lll->data.pdu[last];
 }
 
+static inline struct pdu_adv *
+lll_adv_sync_data_latest_peek(const struct lll_adv_sync *const lll)
+{
+	return lll_adv_pdu_latest_peek(&lll->data);
+}
+
 #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
 static inline void *lll_adv_sync_extra_data_peek(struct lll_adv_sync *lll)
 {

subsys/bluetooth/controller/ll_sw/ull_adv_sync.c

@@ -1545,7 +1545,7 @@ static void mfy_sync_offset_get(void *param)
 	 */
 	lll_sync->ticks_offset = ticks_to_expire + 1;
 
-	pdu = lll_adv_aux_data_curr_get(adv->lll.aux);
+	pdu = lll_adv_aux_data_latest_peek(adv->lll.aux);
 	si = sync_info_get(pdu);
 	sync_info_offset_fill(si, ticks_to_expire, 0);
 	si->evt_cntr = lll_sync->event_counter + lll_sync->latency_prepare +