[BlueZ,3/5] btdev: support multiple CIG

diff --git a/emulator/btdev.c b/emulator/btdev.c
index 7980a5280..0eec1d9f2 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -42,6 +42,7 @@ 
 #define RL_SIZE			16
 #define CIS_SIZE		3
 #define BIS_SIZE		3
+#define CIG_SIZE		3
 
 #define has_bredr(btdev)	(!((btdev)->features[4] & 0x20))
 #define has_le(btdev)		(!!((btdev)->features[4] & 0x40))
@@ -101,6 +102,11 @@  struct le_ext_adv {
 	unsigned int id;
 };
 
+struct le_cig {
+	struct bt_hci_cmd_le_set_cig_params params;
+	struct bt_hci_cis_params cis[CIS_SIZE];
+} __attribute__ ((packed));
+
 struct btdev {
 	enum btdev_type type;
 	uint16_t id;
@@ -204,10 +210,7 @@  struct btdev {
 	uint16_t le_pa_sync_handle;
 	uint8_t  big_handle;
 	uint8_t  le_ltk[16];
-	struct {
-		struct bt_hci_cmd_le_set_cig_params params;
-		struct bt_hci_cis_params cis[CIS_SIZE];
-	} __attribute__ ((packed)) le_cig;
+	struct le_cig le_cig[CIG_SIZE];
 	uint8_t  le_iso_path[2];
 
 	/* Real time length of AL array */
@@ -5757,6 +5760,36 @@  static int cmd_read_iso_tx_sync(struct btdev *dev, const void *data,
 	return -ENOTSUP;
 }
 
+static int find_cig(struct btdev *dev, uint8_t cig_id)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(dev->le_cig); ++i)
+		if (dev->le_cig[i].params.cig_id == cig_id)
+			return i;
+	return -1;
+}
+
+static uint16_t make_cis_handle(uint8_t cig_idx, uint8_t cis_idx)
+{
+	/* Put CIG+CIS idxs to handle so don't need to track separately */
+	return ISO_HANDLE + cig_idx*CIS_SIZE + cis_idx;
+}
+
+static int parse_cis_handle(uint16_t handle, int *cis_idx)
+{
+	int cig_idx;
+
+	if (handle < ISO_HANDLE || handle >= ISO_HANDLE + CIS_SIZE*CIG_SIZE)
+		return -1;
+
+	cig_idx = (handle - ISO_HANDLE) / CIS_SIZE;
+	if (cis_idx)
+		*cis_idx = (handle - ISO_HANDLE) % CIS_SIZE;
+
+	return cig_idx;
+}
+
 static int cmd_set_cig_params(struct btdev *dev, const void *data,
 							uint8_t len)
 {
@@ -5766,12 +5799,13 @@  static int cmd_set_cig_params(struct btdev *dev, const void *data,
 		uint16_t handle[CIS_SIZE];
 	} __attribute__ ((packed)) rsp;
 	int i = 0;
+	int cig_idx;
 	uint32_t interval;
 	uint16_t latency;
 
 	memset(&rsp, 0, sizeof(rsp));
 
-	if (cmd->num_cis > ARRAY_SIZE(dev->le_cig.cis)) {
+	if (cmd->num_cis > ARRAY_SIZE(dev->le_cig[0].cis)) {
 		rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
 		goto done;
 	}
@@ -5820,14 +5854,14 @@  static int cmd_set_cig_params(struct btdev *dev, const void *data,
 		goto done;
 	}
 
-	if (dev->le_cig.params.cig_id != 0xff &&
-				dev->le_cig.params.cig_id != cmd->cig_id) {
-		rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
+	cig_idx = find_cig(dev, cmd->cig_id);
+	if (cig_idx < 0)
+		cig_idx = find_cig(dev, 0xff);
+	if (cig_idx < 0) {
+		rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
 		goto done;
 	}
 
-	memcpy(&dev->le_cig, data, len);
-
 	rsp.params.status = BT_HCI_ERR_SUCCESS;
 	rsp.params.cig_id = cmd->cig_id;
 
@@ -5835,7 +5869,7 @@  static int cmd_set_cig_params(struct btdev *dev, const void *data,
 		struct btdev_conn *iso;
 
 		rsp.params.num_handles++;
-		rsp.handle[i] = cpu_to_le16(ISO_HANDLE + i);
+		rsp.handle[i] = cpu_to_le16(make_cis_handle(cig_idx, i));
 
 		/* BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 4, Part E
 		 * page 2553
@@ -5845,14 +5879,16 @@  static int cmd_set_cig_params(struct btdev *dev, const void *data,
 		 * code Command Disallowed (0x0C).
 		 */
 		iso = queue_find(dev->conns, match_handle,
-				UINT_TO_PTR(cpu_to_le16(rsp.handle[i])));
+				UINT_TO_PTR(le16_to_cpu(rsp.handle[i])));
 		if (iso) {
-			rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
+			rsp.params.status = BT_HCI_ERR_COMMAND_DISALLOWED;
 			i = 0;
 			goto done;
 		}
 	}
 
+	memcpy(&dev->le_cig[cig_idx], data, len);
+
 done:
 	cmd_complete(dev, BT_HCI_CMD_LE_SET_CIG_PARAMS, &rsp,
 				sizeof(rsp.params) + (i * sizeof(uint16_t)));
@@ -5878,35 +5914,40 @@  static void le_cis_estabilished(struct btdev *dev, struct btdev_conn *conn,
 						uint8_t status)
 {
 	struct bt_hci_evt_le_cis_established evt;
+	int cig_idx, cis_idx;
 
 	memset(&evt, 0, sizeof(evt));
 
 	evt.status = status;
 	evt.conn_handle = conn ? cpu_to_le16(conn->handle) : 0x0000;
 
+	cig_idx = conn ? parse_cis_handle(conn->link->handle, &cis_idx) : -1;
+	if (cig_idx < 0 && !evt.status)
+		evt.status = BT_HCI_ERR_UNSPECIFIED_ERROR;
+
 	if (!evt.status) {
 		struct btdev *remote = conn->link->dev;
-		int i = conn->handle - ISO_HANDLE;
+		struct le_cig *le_cig = &remote->le_cig[cig_idx];
 
 		/* TODO: Figure out if these values makes sense */
-		memcpy(evt.cig_sync_delay, remote->le_cig.params.c_interval,
-				sizeof(remote->le_cig.params.c_interval));
-		memcpy(evt.cis_sync_delay, remote->le_cig.params.p_interval,
-				sizeof(remote->le_cig.params.p_interval));
-		memcpy(evt.c_latency, &remote->le_cig.params.c_interval,
-				sizeof(remote->le_cig.params.c_interval));
-		memcpy(evt.p_latency, &remote->le_cig.params.p_interval,
-				sizeof(remote->le_cig.params.p_interval));
-		evt.c_phy = remote->le_cig.cis[i].c_phy;
-		evt.p_phy = remote->le_cig.cis[i].p_phy;
+		memcpy(evt.cig_sync_delay, le_cig->params.c_interval,
+				sizeof(le_cig->params.c_interval));
+		memcpy(evt.cis_sync_delay, le_cig->params.p_interval,
+				sizeof(le_cig->params.p_interval));
+		memcpy(evt.c_latency, &le_cig->params.c_interval,
+				sizeof(le_cig->params.c_interval));
+		memcpy(evt.p_latency, &le_cig->params.p_interval,
+				sizeof(le_cig->params.p_interval));
+		evt.c_phy = le_cig->cis[cis_idx].c_phy;
+		evt.p_phy = le_cig->cis[cis_idx].p_phy;
 		evt.nse = 0x01;
 		evt.c_bn = 0x01;
 		evt.p_bn = 0x01;
 		evt.c_ft = 0x01;
 		evt.p_ft = 0x01;
-		evt.c_mtu = remote->le_cig.cis[i].c_sdu;
-		evt.p_mtu = remote->le_cig.cis[i].p_sdu;
-		evt.interval = remote->le_cig.params.c_latency;
+		evt.c_mtu = le_cig->cis[cis_idx].c_sdu;
+		evt.p_mtu = le_cig->cis[cis_idx].p_sdu;
+		evt.interval = le_cig->params.c_latency;
 	}
 
 	le_meta_event(dev, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt, sizeof(evt));
@@ -5927,9 +5968,20 @@  static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 		struct btdev_conn *acl;
 		struct btdev_conn *iso;
 		struct bt_hci_evt_le_cis_req evt;
+		struct le_cig *le_cig;
+		int cig_idx, cis_idx;
+
+		cig_idx = parse_cis_handle(le16_to_cpu(cis->cis_handle),
+								&cis_idx);
+		if (cig_idx < 0) {
+			le_cis_estabilished(dev, NULL,
+						BT_HCI_ERR_UNKNOWN_CONN_ID);
+			break;
+		}
+		le_cig = &dev->le_cig[cig_idx];
 
 		acl = queue_find(dev->conns, match_handle,
-				UINT_TO_PTR(cpu_to_le16(cis->acl_handle)));
+				UINT_TO_PTR(le16_to_cpu(cis->acl_handle)));
 		if (!acl) {
 			le_cis_estabilished(dev, NULL,
 						BT_HCI_ERR_UNKNOWN_CONN_ID);
@@ -5937,9 +5989,9 @@  static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 		}
 
 		iso = queue_find(dev->conns, match_handle,
-				UINT_TO_PTR(cpu_to_le16(cis->cis_handle)));
+				UINT_TO_PTR(le16_to_cpu(cis->cis_handle)));
 		if (!iso) {
-			iso = conn_add_cis(acl, cpu_to_le16(cis->cis_handle));
+			iso = conn_add_cis(acl, le16_to_cpu(cis->cis_handle));
 			if (!iso) {
 				le_cis_estabilished(dev, NULL,
 						BT_HCI_ERR_UNKNOWN_CONN_ID);
@@ -5949,8 +6001,8 @@  static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 
 		evt.acl_handle = cpu_to_le16(acl->handle);
 		evt.cis_handle = cpu_to_le16(iso->handle);
-		evt.cig_id = iso->dev->le_cig.params.cig_id;
-		evt.cis_id = iso->dev->le_cig.cis[i].cis_id;
+		evt.cig_id = le_cig->params.cig_id;
+		evt.cis_id = le_cig->cis[cis_idx].cis_id;
 
 		le_meta_event(iso->link->dev, BT_HCI_EVT_LE_CIS_REQ, &evt,
 					sizeof(evt));
@@ -5959,20 +6011,37 @@  static int cmd_create_cis_complete(struct btdev *dev, const void *data,
 	return 0;
 }
 
+static bool match_handle_cig_idx(const void *data, const void *match_data)
+{
+	const struct btdev_conn *conn = data;
+	int cig_idx = PTR_TO_INT(match_data);
+
+	return cig_idx >= 0 && parse_cis_handle(conn->handle, NULL) == cig_idx;
+}
+
 static int cmd_remove_cig(struct btdev *dev, const void *data, uint8_t len)
 {
 	const struct bt_hci_cmd_le_remove_cig *cmd = data;
 	struct bt_hci_rsp_le_remove_cig rsp;
+	struct btdev_conn *conn = NULL;
+	int idx;
 
-	memset(&dev->le_cig, 0, sizeof(dev->le_cig));
 	memset(&rsp, 0, sizeof(rsp));
 
 	rsp.cig_id = cmd->cig_id;
 
-	if (dev->le_cig.params.cig_id == cmd->cig_id)
+	idx = find_cig(dev, cmd->cig_id);
+	conn = queue_find(dev->conns, match_handle_cig_idx, INT_TO_PTR(idx));
+
+	if (idx >= 0 && !conn) {
+		memset(&dev->le_cig[idx], 0, sizeof(struct le_cig));
+		dev->le_cig[idx].params.cig_id = 0xff;
 		rsp.status = BT_HCI_ERR_SUCCESS;
-	else
+	} else if (conn) {
+		rsp.status = BT_HCI_ERR_COMMAND_DISALLOWED;
+	} else {
 		rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+	}
 
 	cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_CIG, &rsp, sizeof(rsp));
 
@@ -6842,6 +6911,7 @@  struct btdev *btdev_create(enum btdev_type type, uint16_t id)
 {
 	struct btdev *btdev;
 	int index;
+	unsigned int i;
 
 	btdev = malloc(sizeof(*btdev));
 	if (!btdev)
@@ -6909,9 +6979,11 @@  struct btdev *btdev_create(enum btdev_type type, uint16_t id)
 
 	btdev->iso_mtu = 251;
 	btdev->iso_max_pkt = 1;
-	btdev->le_cig.params.cig_id = 0xff;
 	btdev->big_handle = 0xff;
 
+	for (i = 0; i < ARRAY_SIZE(btdev->le_cig); ++i)
+		btdev->le_cig[i].params.cig_id = 0xff;
+
 	btdev->country_code = 0x00;
 
 	index = add_btdev(btdev);