Bluetooth: ISO: Add CIG reconfigure function

Add function to reconfigure and even add additional
CIS to a CIG.

Signed-off-by: Emil Gydesen <emil.gydesen@nordicsemi.no>

include/bluetooth/iso.h

@@ -518,6 +518,28 @@ int bt_iso_server_register(struct bt_iso_server *server);
 int bt_iso_cig_create(const struct bt_iso_cig_param *param,
 		      struct bt_iso_cig **out_cig);
 
+/** @brief Reconfigure a CIG as a central
+ *
+ *  This function can be used to update a CIG. It will update the group specific
+ *  parameters, and, if supplied, change the QoS parameters of the individual
+ *  CIS. If the cis_channels in @p param contains CIS that was not originally
+ *  in the call to bt_iso_cig_create(), these will be added to the group.
+ *  It is not possible to remove any CIS from the group after creation.
+ *
+ *  This can be called at any time before connecting an ISO to a remote device.
+ *  Once any CIS in the group has connected, the group cannot be changed.
+ *
+ *  Once a CIG is created, the channels supplied in the @p param can be
+ *  connected using bt_iso_chan_connect.
+ *
+ *  @param cig       Connected Isochronous Group object.
+ *  @param param     The parameters used to reconfigure the CIG.
+ *
+ *  @return 0 in case of success or negative value in case of error.
+ */
+int bt_iso_cig_reconfigure(struct bt_iso_cig *cig,
+			   const struct bt_iso_cig_param *param);
+
 /** @brief Terminates a CIG as a central
  *
  *  All the CIS in the CIG shall be disconnected first.

subsys/bluetooth/host/iso.c

@@ -1088,25 +1088,33 @@ static struct bt_iso_cig *get_free_cig(void)
 	return NULL;
 }
 
+static bool cis_is_in_cig(const struct bt_iso_cig *cig,
+			  const struct bt_iso_chan *cis)
+{
+	return cig->id == cis->iso->iso.cig_id;
+}
+
 static int cig_init_cis(struct bt_iso_cig *cig,
 			const struct bt_iso_cig_param *param)
 {
 	for (uint8_t i = 0; i < param->num_cis; i++) {
 		struct bt_iso_chan *cis = param->cis_channels[i];
 
-		cis->iso = iso_new();
 		if (cis->iso == NULL) {
-			BT_ERR("Unable to allocate CIS connection");
+			cis->iso = iso_new();
-			return -ENOMEM;
+			if (cis->iso == NULL) {
-		}
+				BT_ERR("Unable to allocate CIS connection");
+				return -ENOMEM;
+			}
 
-		cis->iso->iso.cig_id = cig->id;
+			cis->iso->iso.cig_id = cig->id;
-		cis->iso->iso.is_bis = false;
+			cis->iso->iso.is_bis = false;
-		cis->iso->iso.cis_id = i;
+			cis->iso->iso.cis_id = cig->num_cis++;
 
-		bt_iso_chan_add(cis->iso, cis);
+			bt_iso_chan_add(cis->iso, cis);
 
-		sys_slist_append(&cig->cis_channels, &cis->node);
+			sys_slist_append(&cig->cis_channels, &cis->node);
+		} /* else already initialized */
 	}
 
 	return 0;
@@ -1146,11 +1154,6 @@ static bool valid_cig_param(const struct bt_iso_cig_param *param)
 			BT_DBG("cis_channels[%d]: Invalid QOS", i);
 			return false;
 		}
-
-		if (cis->iso != NULL) {
-			BT_DBG("cis_channels[%d]: already allocated", i);
-			return false;
-		}
 	}
 
 	if (param->framing != BT_ISO_FRAMING_UNFRAMED &&
@@ -1209,6 +1212,7 @@ int bt_iso_cig_create(const struct bt_iso_cig_param *param,
 		return -ENOTSUP;
 	}
 
+	/* TBD: Should we allow creating empty CIGs? */
 	CHECKIF(param->cis_channels == NULL) {
 		BT_DBG("NULL CIS channels");
 		return -EINVAL;
@@ -1224,6 +1228,15 @@ int bt_iso_cig_create(const struct bt_iso_cig_param *param,
 		return -EINVAL;
 	}
 
+	for (uint8_t i = 0; i < param->num_cis; i++) {
+		struct bt_iso_chan *cis = param->cis_channels[i];
+
+		if (cis->iso != NULL) {
+			BT_DBG("cis_channels[%d]: already allocated", i);
+			return false;
+		}
+	}
+
 	cig = get_free_cig();
 
 	if (!cig) {
@@ -1236,7 +1249,6 @@ int bt_iso_cig_create(const struct bt_iso_cig_param *param,
 		cleanup_cig(cig);
 		return err;
 	}
-	cig->num_cis = param->num_cis;
 
 	rsp = hci_le_set_cig_params(cig, param);
 	if (rsp == NULL) {
@@ -1270,6 +1282,98 @@ int bt_iso_cig_create(const struct bt_iso_cig_param *param,
 	return err;
 }
 
+static void restore_cig(struct bt_iso_cig *cig, uint8_t existing_num_cis)
+{
+	struct bt_iso_chan *cis, *tmp;
+
+	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&cig->cis_channels, cis, tmp, node) {
+		/* Remove all newly added by comparing the cis_id to the number
+		 * of CIS that was previously added before
+		 * bt_iso_cig_reconfigure was called
+		 */
+		if (cis->iso != NULL &&
+		    cis->iso->iso.cis_id >= existing_num_cis) {
+			bt_conn_unref(cis->iso);
+			cis->iso = NULL;
+
+			sys_slist_remove(&cig->cis_channels, NULL, &cis->node);
+			cig->num_cis--;
+		}
+	}
+}
+
+
+int bt_iso_cig_reconfigure(struct bt_iso_cig *cig,
+			   const struct bt_iso_cig_param *param)
+{
+	struct bt_hci_rp_le_set_cig_params *cig_rsp;
+	uint8_t existing_num_cis;
+	struct bt_iso_chan *cis;
+	struct net_buf *rsp;
+	int err;
+	int i;
+
+	CHECKIF(cig == NULL) {
+		BT_DBG("cig is NULL");
+		return -EINVAL;
+	}
+
+	CHECKIF(!valid_cig_param(param)) {
+		BT_DBG("Invalid CIG params");
+		return -EINVAL;
+	}
+
+	for (uint8_t i = 0; i < param->num_cis; i++) {
+		struct bt_iso_chan *cis = param->cis_channels[i];
+
+		if (cis->iso != NULL && !cis_is_in_cig(cig, cis)) {
+			BT_DBG("Cannot reconfigure other CIG's (id 0x%02X) CIS "
+			       "with this CIG (id 0x%02X)",
+			       cis->iso->iso.cig_id, cig->id);
+			return -EINVAL;
+		}
+	}
+
+	/* Used to restore CIG in case of error */
+	existing_num_cis = cig->num_cis;
+
+	err = cig_init_cis(cig, param);
+	if (err != 0) {
+		BT_DBG("Could not init CIS %d", err);
+		restore_cig(cig, existing_num_cis);
+		return err;
+	}
+
+	rsp = hci_le_set_cig_params(cig, param);
+	if (rsp == NULL) {
+		BT_WARN("Unexpected response to hci_le_set_cig_params");
+		err = -EIO;
+		restore_cig(cig, existing_num_cis);
+		return err;
+	}
+
+	cig_rsp = (void *)rsp->data;
+
+	if (rsp->len < sizeof(cig_rsp) ||
+	    cig_rsp->num_handles != param->num_cis) {
+		BT_WARN("Unexpected response to hci_le_set_cig_params");
+		err = -EIO;
+		net_buf_unref(rsp);
+		restore_cig(cig, existing_num_cis);
+		return err;
+	}
+
+	i = 0;
+	SYS_SLIST_FOR_EACH_CONTAINER(&cig->cis_channels, cis, node) {
+		/* Assign the connection handle */
+		cis->iso->handle = sys_le16_to_cpu(cig_rsp->handle[i++]);
+	}
+
+	net_buf_unref(rsp);
+
+	return err;
+}
+
 int bt_iso_cig_terminate(struct bt_iso_cig *cig)
 {
 	struct bt_iso_chan *cis;