Bluetooth: Controller: Fix ISO Data timestamp when FT > 1

Fix ISO data timestamp to reflect the SDU reference point
and not the ISO event anchor point when PDUs received after
retransmissions.

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

Author: cvinayak

subsys/bluetooth/controller/ll_sw/lll_conn_iso.h

@@ -77,6 +77,9 @@ struct lll_conn_iso_group {
 	uint8_t  role:1;      /* 0: CENTRAL, 1: PERIPHERAL*/
 	uint8_t  paused:1;    /* 1: CIG is paused */
 
+	/* ISO interval to calculate timestamp under FT > 1 */
+	uint32_t iso_interval_us;
+
 	/* Accumulates LLL prepare callback latencies */
 	uint16_t latency_prepare;
 	uint16_t latency_event;

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

@@ -716,6 +716,7 @@ static void isr_rx(void *param)
 		    (cis_lll->rx.bn_curr <= cis_lll->rx.bn) &&
 		    (pdu_rx->sn == cis_lll->nesn) &&
 		    ull_iso_pdu_rx_alloc_peek(2U)) {
+			struct lll_conn_iso_group *cig_lll;
 			struct node_rx_iso_meta *iso_meta;
 
 			cis_lll->nesn++;
@@ -753,6 +754,10 @@ static void isr_rx(void *param)
 			iso_meta->timestamp =
 				HAL_TICKER_TICKS_TO_US(radio_tmr_start_get()) +
 				radio_tmr_ready_restore();
+			cig_lll = ull_conn_iso_lll_group_get_by_stream(cis_lll);
+			iso_meta->timestamp -= (cis_lll->event_count -
+						(cis_lll->rx.payload_count / cis_lll->rx.bn)) *
+					       cig_lll->iso_interval_us;
 			iso_meta->timestamp %=
 				HAL_TICKER_TICKS_TO_US(BIT(HAL_TICKER_CNTR_MSBIT + 1U));
 			iso_meta->status = 0U;
@@ -788,8 +793,8 @@ static void isr_rx(void *param)
 
 isr_rx_next_subevent:
 	if (cie || (se_curr == cis_lll->nse)) {
-		struct lll_conn_iso_stream *old_cis_lll;
 		struct lll_conn_iso_stream *next_cis_lll;
+		struct lll_conn_iso_stream *old_cis_lll;
 		struct lll_conn_iso_group *cig_lll;
 		struct lll_conn *next_conn_lll;
 		uint8_t phy;

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

@@ -545,6 +545,7 @@ static void isr_rx(void *param)
 		    (cis_lll->rx.bn_curr <= cis_lll->rx.bn) &&
 		    (pdu_rx->sn == cis_lll->nesn) &&
 		    ull_iso_pdu_rx_alloc_peek(2U)) {
+			struct lll_conn_iso_group *cig_lll;
 			struct node_rx_iso_meta *iso_meta;
 
 			cis_lll->nesn++;
@@ -584,6 +585,10 @@ static void isr_rx(void *param)
 				HAL_TICKER_TICKS_TO_US(radio_tmr_start_get()) +
 				radio_tmr_aa_restore() - cis_offset_first -
 				addr_us_get(cis_lll->rx.phy);
+			cig_lll = ull_conn_iso_lll_group_get_by_stream(cis_lll);
+			iso_meta->timestamp -= (cis_lll->event_count -
+						(cis_lll->rx.payload_count / cis_lll->rx.bn)) *
+					       cig_lll->iso_interval_us;
 			iso_meta->timestamp %=
 				HAL_TICKER_TICKS_TO_US(BIT(HAL_TICKER_CNTR_MSBIT + 1U));
 			iso_meta->status = 0U;

subsys/bluetooth/controller/ll_sw/ull_central_iso.c

@@ -254,6 +254,7 @@ uint8_t ll_cig_parameters_commit(uint8_t cig_id, uint16_t *handles)
 	}
 
 	iso_interval_us = cig->iso_interval * ISO_INT_UNIT_US;
+	cig->lll.iso_interval_us = iso_interval_us;
 
 	lll_hdr_init(&cig->lll, cig);
 	max_se_length = 0U;

subsys/bluetooth/controller/ll_sw/ull_peripheral_iso.c

@@ -191,6 +191,7 @@ uint8_t ull_peripheral_iso_acquire(struct ll_conn *acl,
 
 		cig->iso_interval = sys_le16_to_cpu(req->iso_interval);
 		iso_interval_us = cig->iso_interval * CONN_INT_UNIT_US;
+		cig->lll.iso_interval_us = iso_interval_us;
 
 		cig->cig_id = req->cig_id;
 		cig->lll.handle = LLL_HANDLE_INVALID;