From 0810824886abe0941738fd5b961ccbe6c7bde934 Mon Sep 17 00:00:00 2001
From: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
Date: Wed, 28 Jun 2023 09:57:57 +0530
Subject: [PATCH] Bluetooth: Controller: Fix Connected ISO for aborted prepare

Fix Connected ISO implementation to correctly handle SN,
NESN and payload_count when prepare callback is aborted due
to CPU overhead related latencies.

Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
---
 .../ll_sw/nordic/lll/lll_central_iso.c        | 26 +++++++++++++++++--
 .../ll_sw/nordic/lll/lll_peripheral_iso.c     | 20 ++++++++++++--
 2 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_central_iso.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_central_iso.c
index ff5542b365d8b09..6812a865306920b 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_central_iso.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_central_iso.c
@@ -155,6 +155,7 @@ static int prepare_cb(struct lll_prepare_param *p)
 	uint16_t lazy;
 	uint32_t ret;
 	uint8_t phy;
+	int err = 0;
 
 	DEBUG_RADIO_START_M(1);
 
@@ -384,10 +385,11 @@ static int prepare_cb(struct lll_prepare_param *p)
 	if (overhead) {
 		LL_ASSERT_MSG(false, "%s: Actual EVENT_OVERHEAD_START_US = %u",
 			      __func__, HAL_TICKER_TICKS_TO_US(overhead));
-		radio_isr_set(lll_isr_abort, cig_lll);
+
+		radio_isr_set(isr_done, cis_lll);
 		radio_disable();
 
-		return -ECANCELED;
+		err = -ECANCELED;
 	}
 #endif /* CONFIG_BT_CTLR_XTAL_ADVANCED */
 
@@ -408,10 +410,30 @@ static int prepare_cb(struct lll_prepare_param *p)
 				/* sn and nesn are 1-bit, only Least Significant bit is needed */
 				cis_lll->sn += cis_lll->tx.bn * cis_lazy;
 				cis_lll->nesn += cis_lll->rx.bn * cis_lazy;
+
+				/* Adjust sn and nesn for canceled events */
+				if (err) {
+					/* Adjust sn when flushing Tx */
+					/* FIXME: When Flush Timeout is implemented */
+					if (cis_lll->tx.bn_curr <= cis_lll->tx.bn) {
+						lll_flush_tx(cis_lll);
+					}
+
+					/* Adjust nesn when flushing Rx */
+					/* FIXME: When Flush Timeout is implemented */
+					if (cis_lll->rx.bn_curr <= cis_lll->rx.bn) {
+						lll_flush_rx(cis_lll);
+					}
+				}
 			}
 		}
 	} while (cis_lll);
 
+	/* Return if prepare callback cancelled */
+	if (err) {
+		return err;
+	}
+
 	/* Prepare is done */
 	ret = lll_prepare_done(cig_lll);
 	LL_ASSERT(!ret);
diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_peripheral_iso.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_peripheral_iso.c
index 12c294af0b9b5e3..a096febf061ed60 100644
--- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_peripheral_iso.c
+++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_peripheral_iso.c
@@ -173,6 +173,7 @@ static int prepare_cb(struct lll_prepare_param *p)
 	uint16_t lazy;
 	uint32_t ret;
 	uint8_t phy;
+	int err = 0;
 
 	DEBUG_RADIO_START_S(1);
 
@@ -361,10 +362,11 @@ static int prepare_cb(struct lll_prepare_param *p)
 	if (overhead) {
 		LL_ASSERT_MSG(false, "%s: Actual EVENT_OVERHEAD_START_US = %u",
 			      __func__, HAL_TICKER_TICKS_TO_US(overhead));
-		radio_isr_set(lll_isr_abort, cig_lll);
+
+		radio_isr_set(isr_done, cis_lll);
 		radio_disable();
 
-		return -ECANCELED;
+		err = -ECANCELED;
 	}
 #endif /* CONFIG_BT_CTLR_XTAL_ADVANCED */
 
@@ -400,9 +402,23 @@ static int prepare_cb(struct lll_prepare_param *p)
 			/* sn and nesn are 1-bit, only Least Significant bit is needed */
 			cis_lll->sn += cis_lll->tx.bn * lazy;
 			cis_lll->nesn += cis_lll->rx.bn * lazy;
+
+			/* Adjust sn and nesn for canceled events */
+			if (err) {
+				/* Adjust nesn when flushing Rx */
+				/* FIXME: When Flush Timeout is implemented */
+				if (cis_lll->rx.bn_curr <= cis_lll->rx.bn) {
+					lll_flush_rx(cis_lll);
+				}
+			}
 		}
 	};
 
+	/* Return if prepare callback cancelled */
+	if (err) {
+		return err;
+	}
+
 	/* Prepare is done */
 	ret = lll_prepare_done(cig_lll);
 	LL_ASSERT(!ret);