Bluetooth: Controller: Various fixes for CIS termination handling

Handling of CIS termination had several issues, most notably:

- it depended on allocating a termination node from the general rx
  node pool, causing asserts if the pool was exhausted
- CIS established events was not always generated when required,
  potentially causing CIS Centrals to get stuck without being able
  to create any new CISes
- Cancelling a CIS Create procedure only worked correctly if the
  CIS Create was currently active and happened to belong to the
  same CIS
- CIG state handling often (always?) assumed a CIG with only one CIS

ll_conn_iso_stream now has a dedicated termination node, same as
ll_conn and ll_sync_iso_set

LLCP statemachine for Cis Create procedure has been reworked to ensure
a notification node for CIS Established is available as early as possible.
In addition, it should now always be sent when needed

Introduced ull_central_iso_all_cises_terminated() to check if all CISes
in a CIG has been terminated (or not created yet) - which is now
used for updating the CIG state

ull_cp_cc_cancel() now takes the CIS to cancel as an argument so
it doesn't end up canceling an entirely different CIS Create procedure;
In addition it now works for queued procedures as well

Flushing a (central) CIS Create procedure in LLCP will now properly
generate a CIS Established event (with an error)

Signed-off-by: Troels Nilsson <trnn@demant.com>

Author: Tronil

subsys/bluetooth/controller/ll_sw/ull.c

@@ -459,6 +459,12 @@ static MFIFO_DEFINE(pdu_rx_free, sizeof(void *), PDU_RX_CNT);
 #define BT_CTLR_SCAN_SYNC_ISO_SET 0
 #endif
 
+#if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS)
+#define BT_CTLR_CONN_ISO_STREAMS CONFIG_BT_CTLR_CONN_ISO_STREAMS
+#else
+#define BT_CTLR_CONN_ISO_STREAMS 0
+#endif
+
 #define PDU_RX_POOL_SIZE (PDU_RX_NODE_POOL_ELEMENT_SIZE * \
 			  (RX_CNT + BT_CTLR_MAX_CONNECTABLE + \
 			   BT_CTLR_ADV_SET + BT_CTLR_SCAN_SYNC_SET))
@@ -508,7 +514,7 @@ static struct {
 	(sizeof(memq_link_t) *                                                 \
 	 (RX_CNT + 2 + BT_CTLR_MAX_CONN + BT_CTLR_ADV_SET +                    \
 	  (BT_CTLR_ADV_ISO_SET * 2) + (BT_CTLR_SCAN_SYNC_SET * 2) +            \
-	  (BT_CTLR_SCAN_SYNC_ISO_SET * 2) +                                    \
+	  (BT_CTLR_SCAN_SYNC_ISO_SET * 2) + BT_CTLR_CONN_ISO_STREAMS +         \
 	  (IQ_REPORT_CNT)))
 static struct {
 	uint16_t quota_pdu; /* Number of un-utilized buffers */
@@ -1726,8 +1732,8 @@ void ll_rx_mem_release(void **node_rx)
 
 				ll_conn_release(conn);
 			} else if (IS_CIS_HANDLE(rx_free->hdr.handle)) {
-				ll_rx_link_quota_inc();
-				ll_rx_release(rx_free);
+				/* Notify ull_conn_iso */
+				ull_conn_iso_cis_terminate_done(rx_free);
 			}
 		}
 		break;

subsys/bluetooth/controller/ll_sw/ull_central.c

@@ -559,7 +559,7 @@ uint8_t ll_enc_req_send(uint16_t handle, uint8_t const *const rand_num,
 #if defined(CONFIG_BT_CTLR_CENTRAL_ISO)
 	struct ll_conn_iso_stream *cis = ll_conn_iso_stream_get_by_acl(conn, NULL);
 
-	if (cis || ull_lp_cc_is_enqueued(conn)) {
+	if (cis || ull_lp_cc_is_enqueued(conn, NULL)) {
 		return BT_HCI_ERR_CMD_DISALLOWED;
 	}
 #endif /* CONFIG_BT_CTLR_CENTRAL_ISO */

subsys/bluetooth/controller/ll_sw/ull_central_iso.c

@@ -272,6 +272,7 @@ uint8_t ll_cig_parameters_commit(uint8_t cig_id, uint16_t *handles)
 
 	/* Create all configurable CISes */
 	for (uint8_t i = 0U; i < ll_iso_setup.cis_count; i++) {
+		memq_link_t *link_rx_terminate;
 		memq_link_t *link_tx_free;
 		memq_link_t link_tx;
 
@@ -305,9 +306,10 @@ uint8_t ll_cig_parameters_commit(uint8_t cig_id, uint16_t *handles)
 			cig->lll.num_cis++;
 		}
 
-		/* Store TX link and free link before transfer */
+		/* Store TX link, free link and terminate link before transfer */
 		link_tx_free = cis->lll.link_tx_free;
 		link_tx = cis->lll.link_tx;
+		link_rx_terminate = cis->node_rx_terminate.rx.hdr.link;
 
 		/* Transfer parameters from configuration cache */
 		memcpy(cis, &ll_iso_setup.stream[i], sizeof(struct ll_conn_iso_stream));
@@ -317,6 +319,7 @@ uint8_t ll_cig_parameters_commit(uint8_t cig_id, uint16_t *handles)
 
 		cis->lll.link_tx_free = link_tx_free;
 		cis->lll.link_tx = link_tx;
+		cis->node_rx_terminate.rx.hdr.link = link_rx_terminate;
 		cis->lll.handle = ll_conn_iso_stream_handle_get(cis);
 		handles[i] = cis->lll.handle;
 	}
@@ -744,6 +747,13 @@ void ll_cis_create(uint16_t cis_handle, uint16_t acl_handle)
 
 	(void)memset(&cis->hdr, 0U, sizeof(cis->hdr));
 
+	/* Allocate new terminate link if needed (only needed if CIS is re-used) */
+	if (cis->node_rx_terminate.rx.hdr.link == NULL) {
+		cis->node_rx_terminate.rx.hdr.link = ll_rx_link_alloc();
+		/* Failure should not be possible, but assert just in case */
+		LL_ASSERT_DBG(cis->node_rx_terminate.rx.hdr.link);
+	}
+
 	/* Initialize TX link */
 	if (!cis->lll.link_tx_free) {
 		cis->lll.link_tx_free = &cis->lll.link_tx;
@@ -1022,6 +1032,30 @@ int ull_central_iso_cis_offset_get(uint16_t cis_handle,
 #endif /* CONFIG_BT_CTLR_CENTRAL_SPACING != 0 */
 }
 
+bool ull_central_iso_all_cises_terminated(struct ll_conn_iso_group *cig)
+{
+	/* Check if all CISes associated with this CIG is terminated (or not created) */
+	for (uint16_t handle = LL_CIS_HANDLE_BASE; handle <= LL_CIS_HANDLE_LAST; handle++) {
+		struct ll_conn_iso_stream *cis;
+
+		cis = ll_conn_iso_stream_get(handle);
+		if (cis->group == cig) {
+			struct ll_conn *conn;
+
+			if (cis->established) {
+				return false;
+			}
+
+			/* Check if CIS is being created */
+			conn = ll_connected_get(cis->lll.acl_handle);
+			if (conn && ull_lp_cc_is_enqueued(conn, cis)) {
+				return false;
+			}
+		}
+	}
+	return true;
+}
+
 #if (CONFIG_BT_CTLR_CENTRAL_SPACING == 0)
 static void cig_offset_get(struct ll_conn_iso_stream *cis)
 {

subsys/bluetooth/controller/ll_sw/ull_central_iso_internal.h

@@ -19,3 +19,4 @@ uint8_t ull_central_iso_setup(uint16_t cis_handle,
 			      uint32_t *cis_offset_max,
 			      uint16_t *conn_event_count,
 			      uint8_t  *access_addr);
+bool ull_central_iso_all_cises_terminated(struct ll_conn_iso_group *cig);

subsys/bluetooth/controller/ll_sw/ull_conn.c

@@ -59,6 +59,7 @@
 
 #include "ull_iso_internal.h"
 #include "ull_conn_iso_internal.h"
+#include "ull_central_iso_internal.h"
 #include "ull_peripheral_iso_internal.h"
 #include "lll/lll_adv_types.h"
 #include "lll_adv.h"
@@ -438,43 +439,44 @@ uint8_t ll_terminate_ind_send(uint16_t handle, uint8_t reason)
 
 			/* Sanity-check instance to make sure it's created but not connected */
 			if (cis->group && cis->lll.handle == handle && !cis->established) {
-				if (cis->group->state == CIG_STATE_CONFIGURABLE) {
-					/* Disallow if CIG is still in configurable state */
-					return BT_HCI_ERR_CMD_DISALLOWED;
+				struct ll_conn_iso_group *cig = cis->group;
 
-				} else if (cis->group->state == CIG_STATE_INITIATING) {
-					conn = ll_connected_get(cis->lll.acl_handle);
-					LL_ASSERT_DBG(conn != NULL);
+				conn = ll_connected_get(cis->lll.acl_handle);
 
-					/* CIS is not yet established - try to cancel procedure */
-					if (ull_cp_cc_cancel(conn)) {
-						/* Successfully canceled - complete disconnect */
-						struct node_rx_pdu *node_terminate;
+				if (conn == NULL || !ull_lp_cc_is_enqueued(conn, cis)) {
+					/* Disallow if CIS is not created yet or terminated */
+					return BT_HCI_ERR_CMD_DISALLOWED;
+				}
 
-						node_terminate = ull_pdu_rx_alloc();
-						LL_ASSERT_ERR(node_terminate);
+				/* CIS is not yet established - try to cancel procedure */
+				if (ull_cp_cc_cancel(conn, cis)) {
+					/* Successfully canceled - complete disconnect */
+					struct node_rx_pdu *node_terminate;
 
-						node_terminate->hdr.handle = handle;
-						node_terminate->hdr.type = NODE_RX_TYPE_TERMINATE;
-						*((uint8_t *)node_terminate->pdu) =
-							BT_HCI_ERR_LOCALHOST_TERM_CONN;
+					node_terminate = &cis->node_rx_terminate.rx;
 
-						ll_rx_put_sched(node_terminate->hdr.link,
-							node_terminate);
+					node_terminate->hdr.handle = handle;
+					node_terminate->hdr.type = NODE_RX_TYPE_TERMINATE;
+					*((uint8_t *)node_terminate->pdu) =
+						BT_HCI_ERR_LOCALHOST_TERM_CONN;
 
-						/* We're no longer initiating a connection */
-						cis->group->state = CIG_STATE_CONFIGURABLE;
+					ll_rx_put_sched(node_terminate->hdr.link,
+							node_terminate);
 
-						/* This is now a successful disconnection */
-						return BT_HCI_ERR_SUCCESS;
+					/* We're no longer initiating a connection */
+					if (ull_central_iso_all_cises_terminated(cig)) {
+						cig->state = CIG_STATE_INACTIVE;
 					}
 
-					/* Procedure could not be canceled in the current
-					 * state - let it run its course and enqueue a
-					 * terminate procedure.
-					 */
-					return ull_cp_cis_terminate(conn, cis, reason);
+					/* This is now a successful disconnection */
+					return BT_HCI_ERR_SUCCESS;
 				}
+
+				/* Procedure could not be canceled in the current
+				 * state - let it run its course and enqueue a
+				 * terminate procedure.
+				 */
+				return ull_cp_cis_terminate(conn, cis, reason);
 			}
 #endif /* CONFIG_BT_CTLR_CENTRAL_ISO */
 			/* Disallow if CIS is not connected */
@@ -1897,8 +1899,54 @@ static void conn_cleanup_finalize(struct ll_conn *conn)
 	struct lll_conn *lll = &conn->lll;
 	uint32_t ticker_status;
 
+	if ((IS_ENABLED(CONFIG_BT_CTLR_PERIPHERAL_ISO) ||
+	     IS_ENABLED(CONFIG_BT_CTLR_CENTRAL_ISO)) &&
+	    ull_cp_cc_is_active(conn)) {
+		/* CIS creation procedure active, notify LLCP */
+		ull_cp_cc_acl_disconnect(conn);
+	}
+
+	/* Set LLCP state - flushes control procedures */
 	ull_cp_state_set(conn, ULL_CP_DISCONNECTED);
 
+#if defined(CONFIG_BT_CTLR_PERIPHERAL_ISO) || defined(CONFIG_BT_CTLR_CENTRAL_ISO)
+	/* Find and clean non-established, allocated CISes associated with this ACL conn */
+	for (uint8_t handle = LL_CIS_HANDLE_BASE; handle <= LL_CIS_HANDLE_LAST; handle++) {
+		struct ll_conn_iso_stream *cis;
+		struct ll_conn_iso_group *cig;
+
+		cis = ll_conn_iso_stream_get(handle);
+		cig = cis->group;
+		if (cig != NULL && cig->lll.num_cis != 0U &&
+		    cis->lll.acl_handle == conn->lll.handle) {
+			if (IS_PERIPHERAL(cig)) {
+				/* Remove data paths associated with this CIS for both directions.
+				 * Disable them one at a time to make sure both are removed, even if
+				 * only one is set.
+				 * Note: This only applies for peripherals, for centrals an explicit
+				 * command is required to clean up a CIG and associated CISes.
+				 * See e.g. BT Core v5.4, Vol 4, Part E, Section 7.8.100
+				 */
+				ll_remove_iso_path(handle, BIT(BT_HCI_DATAPATH_DIR_HOST_TO_CTLR));
+				ll_remove_iso_path(handle, BIT(BT_HCI_DATAPATH_DIR_CTLR_TO_HOST));
+
+				ll_conn_iso_stream_release(cis);
+				cig->lll.num_cis--;
+
+				if (cig->lll.num_cis == 0U) {
+					/* No more streams in the group - release group */
+					ll_conn_iso_group_release(cig);
+				}
+			} else if (IS_CENTRAL(cig) && cig->state == CIG_STATE_INITIATING &&
+				   ull_central_iso_all_cises_terminated(cig)) {
+
+				/* We're no longer initiating a connection */
+				cig->state = CIG_STATE_INACTIVE;
+			}
+		}
+	}
+#endif /* CONFIG_BT_CTLR_PERIPHERAL_ISO || CONFIG_BT_CTLR_CENTRAL_ISO */
+
 	/* Update tx buffer queue handling */
 #if defined(LLCP_TX_CTRL_BUF_QUEUE_ENABLE)
 	ull_cp_update_tx_buffer_queue(conn);

subsys/bluetooth/controller/ll_sw/ull_conn_iso.c

@@ -136,6 +136,10 @@ struct ll_conn_iso_stream *ll_conn_iso_stream_acquire(void)
 
 	if (cis) {
 		(void)memset(&cis->hdr, 0U, sizeof(cis->hdr));
+
+		cis->node_rx_terminate.rx.hdr.link = ll_rx_link_alloc();
+		/* Link allocation should never fail */
+		LL_ASSERT_DBG(cis->node_rx_terminate.rx.hdr.link != NULL);
 	}
 
 	return cis;
@@ -146,6 +150,13 @@ void ll_conn_iso_stream_release(struct ll_conn_iso_stream *cis)
 	cis->cis_id = 0;
 	cis->group = NULL;
 
+	if (cis->node_rx_terminate.rx.hdr.type == NODE_RX_TYPE_NONE &&
+	    cis->node_rx_terminate.rx.hdr.link != NULL) {
+		/* Free unused link */
+		ll_rx_link_release(cis->node_rx_terminate.rx.hdr.link);
+		cis->node_rx_terminate.rx.hdr.link = NULL;
+	}
+
 	mem_release(cis, &cis_free);
 }
 
@@ -1116,6 +1127,27 @@ void ull_conn_iso_start(struct ll_conn *conn, uint16_t cis_handle,
 	cis->lll.active = 1U;
 }
 
+void ull_conn_iso_cis_terminate_done(struct node_rx_pdu *rx)
+{
+	DECLARE_MAYFLY_ARRAY(mfys, cis_disabled_cb, CONFIG_BT_CTLR_CONN_ISO_GROUPS);
+	struct ll_conn_iso_stream *cis;
+
+	cis = ll_conn_iso_stream_get(rx->hdr.handle);
+	LL_ASSERT_DBG(rx == &cis->node_rx_terminate.rx);
+
+	rx->hdr.link = NULL;
+	rx->hdr.type = NODE_RX_TYPE_NONE;
+
+	if (cis->lll.flush != LLL_CIS_FLUSH_NONE) {
+		struct ll_conn_iso_group *cig = cis->group;
+
+		/* Complete teardown of CIS in ULL_HIGH context */
+		mfys[cig->lll.handle].param = &cig->lll;
+		(void)mayfly_enqueue(TICKER_USER_ID_THREAD,
+				     TICKER_USER_ID_ULL_HIGH, 1, &mfys[cig->lll.handle]);
+	}
+}
+
 #if !defined(CONFIG_BT_CTLR_JIT_SCHEDULING)
 static void cis_lazy_fill(struct ll_conn_iso_stream *cis)
 {
@@ -1267,6 +1299,14 @@ static void cis_disabled_cb(void *param)
 		} else if (cis->lll.flush == LLL_CIS_FLUSH_COMPLETE) {
 			ll_iso_stream_released_cb_t cis_released_cb;
 
+			if (cis->node_rx_terminate.rx.hdr.type == NODE_RX_TYPE_TERMINATE) {
+				/* Termination node type is still set, so it is waiting to
+				 * be processed. Wait for this to happen before finalizing
+				 * teardown
+				 */
+				continue;
+			}
+
 			conn = ll_conn_get(cis->lll.acl_handle);
 			LL_ASSERT_DBG(conn != NULL);
 
@@ -1323,8 +1363,7 @@ static void cis_disabled_cb(void *param)
 				/* Create and enqueue termination node. This shall prevent
 				 * further enqueuing of TX nodes for terminating CIS.
 				 */
-				node_terminate = ull_pdu_rx_alloc();
-				LL_ASSERT_ERR(node_terminate);
+				node_terminate = &cis->node_rx_terminate.rx;
 				node_terminate->hdr.handle = cis->lll.handle;
 				node_terminate->hdr.type = NODE_RX_TYPE_TERMINATE;
 				*((uint8_t *)node_terminate->pdu) = cis->terminate_reason;

subsys/bluetooth/controller/ll_sw/ull_conn_iso_internal.h

@@ -40,6 +40,7 @@ void ull_conn_iso_start(struct ll_conn *acl, uint16_t cis_handle,
 			uint32_t ticks_at_expire, uint32_t remainder,
 			uint16_t instant_latency);
 void ull_conn_iso_done(struct node_rx_event_done *done);
+void ull_conn_iso_cis_terminate_done(struct node_rx_pdu *rx);
 void ull_conn_iso_cis_stop(struct ll_conn_iso_stream *cis,
 			   ll_iso_stream_released_cb_t cis_released_cb,
 			   uint8_t reason);

subsys/bluetooth/controller/ll_sw/ull_conn_iso_types.h

@@ -51,6 +51,17 @@ struct ll_conn_iso_stream {
 #if defined(CONFIG_BT_CTLR_ISOAL_PSN_IGNORE)
 	uint64_t pkt_seq_num:39;
 #endif /* CONFIG_BT_CTLR_ISOAL_PSN_IGNORE */
+
+	/* node rx type with dummy uint8_t to ensure room for terminate
+	 * reason.
+	 * HCI will reference the value using the pdu member of
+	 * struct node_rx_pdu.
+	 */
+	struct {
+		struct node_rx_pdu rx;
+		/* Dummy declaration to ensure space allocated to hold one pdu byte */
+		uint8_t dummy_reason;
+	} node_rx_terminate;
 };
 
 struct ll_conn_iso_group {