[BlueZ,1/1] isotest: Add support for creating/synchronizing to multiple BISes

diff --git a/tools/isotest.c b/tools/isotest.c
index caa711b2e..0eae3cdc1 100644
--- a/tools/isotest.c
+++ b/tools/isotest.c
@@ -32,6 +32,7 @@ 
 #include <linux/sockios.h>
 #include <time.h>
 #include <inttypes.h>
+#include <sys/wait.h>
 
 #include "lib/bluetooth.h"
 #include "lib/hci.h"
@@ -45,6 +46,9 @@ 
 #define SEC_USEC(_t)  (_t  * 1000000L)
 #define TS_USEC(_ts)  (SEC_USEC((_ts)->tv_sec) + NSEC_USEC((_ts)->tv_nsec))
 
+#define DEFAULT_BIG_ID 0x01
+#define DEFAULT_BIS_ID 0x01
+
 /* Test modes */
 enum {
 	SEND,
@@ -72,6 +76,8 @@  static bool quiet;
 struct bt_iso_qos *iso_qos;
 static bool inout;
 
+static uint8_t num_bis = 1;
+
 struct lookup_table {
 	const char *name;
 	int flag;
@@ -316,8 +322,6 @@  static int do_connect(char *peer)
 	struct sockaddr_iso addr;
 	int sk;
 
-	mgmt_set_experimental();
-
 	/* Create socket */
 	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_ISO);
 	if (sk < 0) {
@@ -390,6 +394,45 @@  error:
 	return -1;
 }
 
+static int *bcast_do_connect_mbis(uint8_t count, char *peer)
+{
+	int *sk;
+	uint8_t sk_cnt = 0;
+
+	sk = malloc(count * sizeof(*sk));
+	if (!sk) {
+		syslog(LOG_ERR, "Can't allocate socket array");
+		return NULL;
+	}
+
+	defer_setup = 1;
+
+	for (int i = 0; i < count; i++) {
+		if (i == count - 1)
+			defer_setup = 0;
+
+		sk[i] = do_connect(peer);
+		if (sk[i] < 0) {
+			syslog(LOG_ERR, "Can't create socket: %s (%d)",
+					strerror(errno), errno);
+
+			goto error;
+		}
+
+		sk_cnt++;
+	}
+
+	return sk;
+
+error:
+	for (int i = 0; i < sk_cnt; i++)
+		close(sk[i]);
+
+	free(sk);
+	return NULL;
+
+}
+
 static void do_listen(char *filename, void (*handler)(int fd, int sk),
 							char *peer)
 {
@@ -431,8 +474,11 @@  static void do_listen(char *filename, void (*handler)(int fd, int sk),
 	if (peer) {
 		str2ba(peer, &addr->iso_bc->bc_bdaddr);
 		addr->iso_bc->bc_bdaddr_type = bdaddr_type;
-		addr->iso_bc->bc_num_bis = 1;
-		addr->iso_bc->bc_bis[0] = 1;
+		addr->iso_bc->bc_num_bis = num_bis;
+
+		for (int i = 0; i < num_bis; i++)
+			addr->iso_bc->bc_bis[i] = i + 1;
+
 		optlen += sizeof(*addr->iso_bc);
 	}
 
@@ -584,6 +630,7 @@  static void recv_mode(int fd, int sk)
 							strerror(errno), errno);
 				if (errno != ENOTCONN)
 					return;
+
 				r = 0;
 			}
 
@@ -704,12 +751,66 @@  static int read_file(int fd, ssize_t count, bool rewind)
 	return len;
 }
 
-static void do_send(int sk, int fd, struct bt_iso_io_qos *out, uint32_t num,
-		    bool repeat)
+static void do_send(int sk, int fd, char *peer, bool repeat)
 {
 	uint32_t seq;
 	struct timespec t_start;
-	int len, used;
+	int send_len, used;
+	socklen_t len;
+	struct bt_iso_qos qos;
+	uint32_t num;
+	struct bt_iso_io_qos *out;
+
+	syslog(LOG_INFO, "Sending ...");
+
+	/* Read QoS */
+	if (!strcmp(peer, "00:00:00:00:00:00"))
+		out = &qos.bcast.out;
+	else
+		out = &qos.ucast.out;
+
+	memset(&qos, 0, sizeof(qos));
+	len = sizeof(qos);
+	if (getsockopt(sk, SOL_BLUETOOTH, BT_ISO_QOS, &qos, &len) < 0) {
+		syslog(LOG_ERR, "Can't get Output QoS socket option: %s (%d)",
+				strerror(errno), errno);
+		out->sdu = ISO_DEFAULT_MTU;
+	}
+
+	/* num of packets = latency (ms) / interval (us) */
+	num = (out->latency * 1000 / out->interval);
+
+	syslog(LOG_INFO, "Number of packets: %d", num);
+
+	if (!sndbuf)
+		/* Use socket buffer as a jitter buffer for the entire buffer
+		 * latency:
+		 * jitter buffer = 2 * (SDU * subevents)
+		 */
+		sndbuf = 2 * ((out->latency * 1000 / out->interval) *
+							out->sdu);
+
+	len = sizeof(sndbuf);
+	if (setsockopt(sk, SOL_SOCKET, SO_SNDBUF, &sndbuf, len) < 0) {
+		syslog(LOG_ERR, "Can't set socket SO_SNDBUF option: %s (%d)",
+				strerror(errno), errno);
+	}
+
+	syslog(LOG_INFO, "Socket jitter buffer: %d buffer", sndbuf);
+
+	if (sndto.tv_usec) {
+		len = sizeof(sndto);
+		if (setsockopt(sk, SOL_SOCKET, SO_SNDTIMEO, &sndto, len) < 0) {
+			syslog(LOG_ERR, "Can't set socket SO_SNDTIMEO option: "
+				"%s (%d)", strerror(errno), errno);
+		} else {
+			syslog(LOG_INFO, "Socket send timeout: %ld usec",
+							sndto.tv_usec);
+		}
+	}
+
+	for (int i = 6; i < out->sdu; i++)
+		buf[i] = 0x7f;
 
 	if (clock_gettime(CLOCK_MONOTONIC, &t_start) < 0) {
 		perror("clock_gettime");
@@ -718,17 +819,17 @@  static void do_send(int sk, int fd, struct bt_iso_io_qos *out, uint32_t num,
 
 	for (seq = 0; ; seq++) {
 		if (fd >= 0) {
-			len = read_file(fd, out->sdu, repeat);
-			if (len < 0) {
+			send_len = read_file(fd, out->sdu, repeat);
+			if (send_len < 0) {
 				syslog(LOG_ERR, "read failed: %s (%d)",
-						strerror(-len), -len);
+						strerror(-send_len), -send_len);
 				exit(1);
 			}
 		} else
-			len = out->sdu;
+			send_len = out->sdu;
 
-		len = send(sk, buf, len, 0);
-		if (len <= 0) {
+		send_len = send(sk, buf, send_len, 0);
+		if (send_len <= 0) {
 			syslog(LOG_ERR, "send failed: %s (%d)",
 						strerror(errno), errno);
 			exit(1);
@@ -739,7 +840,7 @@  static void do_send(int sk, int fd, struct bt_iso_io_qos *out, uint32_t num,
 		if (!quiet)
 			syslog(LOG_INFO,
 				"[seq %d] %d bytes buffered %d (%d bytes)",
-				seq, len, used / len, used);
+				seq, send_len, used / send_len, used);
 
 		if (seq && !((seq + 1) % num))
 			send_wait(&t_start, num * out->interval);
@@ -748,11 +849,11 @@  static void do_send(int sk, int fd, struct bt_iso_io_qos *out, uint32_t num,
 
 static void send_mode(char *filename, char *peer, int i, bool repeat)
 {
-	struct bt_iso_qos qos;
-	socklen_t len;
 	int sk, fd = -1;
-	uint32_t num;
-	struct bt_iso_io_qos *out;
+	int *sk_arr;
+	uint8_t nconn = strcmp(peer, "00:00:00:00:00:00") ? 1 : num_bis;
+
+	mgmt_set_experimental();
 
 	if (filename) {
 		char altername[PATH_MAX];
@@ -769,6 +870,33 @@  static void send_mode(char *filename, char *peer, int i, bool repeat)
 			fd = open_file(filename);
 	}
 
+	if (nconn > 1) {
+		sk_arr = bcast_do_connect_mbis(nconn, peer);
+		if (!sk_arr)
+			exit(1);
+
+		for (int i = 0; i < nconn; i++) {
+			if (fork()) {
+				/* Parent */
+				continue;
+			}
+
+			/* Child */
+			do_send(sk_arr[i], fd, peer, repeat);
+			exit(0);
+		}
+
+		/* Wait for children to exit */
+		while (wait(NULL) > 0)
+			;
+
+		for (int i = 0; i < nconn; i++)
+			close(sk_arr[i]);
+
+		free(sk_arr);
+		return;
+	}
+
 	sk = do_connect(peer);
 	if (sk < 0) {
 		syslog(LOG_ERR, "Can't connect to the server: %s (%d)",
@@ -782,62 +910,13 @@  static void send_mode(char *filename, char *peer, int i, bool repeat)
 		sleep(abs(defer_setup) - 1);
 	}
 
-	syslog(LOG_INFO, "Sending ...");
-
-	/* Read QoS */
-	if (!strcmp(peer, "00:00:00:00:00:00"))
-		out = &qos.bcast.out;
-	else
-		out = &qos.ucast.out;
-
-	memset(&qos, 0, sizeof(qos));
-	len = sizeof(qos);
-	if (getsockopt(sk, SOL_BLUETOOTH, BT_ISO_QOS, &qos, &len) < 0) {
-		syslog(LOG_ERR, "Can't get Output QoS socket option: %s (%d)",
-				strerror(errno), errno);
-		out->sdu = ISO_DEFAULT_MTU;
-	}
-
-	/* num of packets = latency (ms) / interval (us) */
-	num = (out->latency * 1000 / out->interval);
-
-	syslog(LOG_INFO, "Number of packets: %d", num);
-
-	if (!sndbuf)
-		/* Use socket buffer as a jitter buffer for the entire buffer
-		 * latency:
-		 * jitter buffer = 2 * (SDU * subevents)
-		 */
-		sndbuf = 2 * ((out->latency * 1000 / out->interval) *
-							out->sdu);
-
-	len = sizeof(sndbuf);
-	if (setsockopt(sk, SOL_SOCKET, SO_SNDBUF, &sndbuf, len) < 0) {
-		syslog(LOG_ERR, "Can't set socket SO_SNDBUF option: %s (%d)",
-				strerror(errno), errno);
-	}
-
-	syslog(LOG_INFO, "Socket jitter buffer: %d buffer", sndbuf);
-
-	if (sndto.tv_usec) {
-		len = sizeof(sndto);
-		if (setsockopt(sk, SOL_SOCKET, SO_SNDTIMEO, &sndto, len) < 0) {
-			syslog(LOG_ERR, "Can't set socket SO_SNDTIMEO option: "
-				"%s (%d)", strerror(errno), errno);
-		} else {
-			syslog(LOG_INFO, "Socket send timeout: %ld usec",
-							sndto.tv_usec);
-		}
-	}
-
-	for (i = 6; i < out->sdu; i++)
-		buf[i] = 0x7f;
-
-	do_send(sk, fd, out, num, repeat);
+	do_send(sk, fd, peer, repeat);
 }
 
 static void reconnect_mode(char *peer)
 {
+	mgmt_set_experimental();
+
 	while (1) {
 		int sk;
 
@@ -856,6 +935,8 @@  static void reconnect_mode(char *peer)
 
 static void multy_connect_mode(char *peer)
 {
+	mgmt_set_experimental();
+
 	while (1) {
 		int i, sk;
 
@@ -989,7 +1070,8 @@  static void usage(void)
 		"\t[-B, --preset <value>]\n"
 		"\t[-G, --CIG/BIG <value>]\n"
 		"\t[-T, --CIS/BIS <value>]\n"
-		"\t[-V, --type <value>] address type (help for list)\n");
+		"\t[-V, --type <value>] address type (help for list)\n"
+		"\t[-N, --nbis <value>] Number of BISes to create/synchronize to\n");
 }
 
 static const struct option main_options[] = {
@@ -1019,6 +1101,7 @@  static const struct option main_options[] = {
 	{ "CIG/BIG",   required_argument, NULL, 'G'},
 	{ "CIS/BIS",   required_argument, NULL, 'T'},
 	{ "type",      required_argument, NULL, 'V'},
+	{ "nbis",      required_argument, NULL, 'N'},
 	{}
 };
 
@@ -1048,6 +1131,8 @@  int main(int argc, char *argv[])
 	char *filename = NULL;
 	bool repeat = false;
 	unsigned int i;
+	uint8_t nconn = 1;
+	char *peer;
 
 	iso_qos = malloc(sizeof(*iso_qos));
 	/* Default to 16_2_1 */
@@ -1058,7 +1143,7 @@  int main(int argc, char *argv[])
 		int opt;
 
 		opt = getopt_long(argc, argv,
-			"d::cmr::s::nb:i:j:hqt:CV:W:M:S:P:F:I:L:Y:R:B:G:T:e:k:",
+			"d::cmr::s::nb:i:j:hqt:CV:W:M:S:P:F:I:L:Y:R:B:G:T:e:k:N:",
 			main_options, NULL);
 		if (opt < 0)
 			break;
@@ -1224,6 +1309,23 @@  int main(int argc, char *argv[])
 					exit(1);
 			break;
 
+		case 'N':
+			if (optarg)
+				num_bis = atoi(optarg);
+
+			if (num_bis > 1) {
+				/* If the user requested multiple BISes,
+				 * make sure that all BISes are bound
+				 * for the same BIG and advertising set
+				 */
+				if (iso_qos->bcast.big == BT_ISO_QOS_BIG_UNSET)
+					iso_qos->bcast.big = DEFAULT_BIG_ID;
+
+				if (iso_qos->bcast.bis == BT_ISO_QOS_BIS_UNSET)
+					iso_qos->bcast.bis = DEFAULT_BIS_ID;
+			}
+			break;
+
 		/* fall through */
 		default:
 			usage();
@@ -1297,10 +1399,46 @@  int main(int argc, char *argv[])
 			break;
 
 		case CONNECT:
-			sk = do_connect(argv[optind + i]);
-			if (sk < 0)
-				exit(1);
-			dump_mode(-1, sk);
+			peer = argv[optind + i];
+
+			mgmt_set_experimental();
+
+			if (!strcmp(peer, "00:00:00:00:00:00"))
+				nconn = num_bis;
+
+			if (nconn > 1) {
+				int *sk_arr =  bcast_do_connect_mbis(nconn,
+								peer);
+
+				if (!sk_arr)
+					exit(1);
+
+				for (int i = 0; i < nconn; i++) {
+					if (fork()) {
+						/* Parent */
+						continue;
+					}
+
+					/* Child */
+					dump_mode(-1, sk_arr[i]);
+					exit(0);
+				}
+
+				/* Wait for children to exit */
+				while (wait(NULL) > 0)
+					;
+
+				for (int i = 0; i < nconn; i++)
+					close(sk_arr[i]);
+
+				free(sk_arr);
+			} else {
+				sk = do_connect(argv[optind + i]);
+				if (sk < 0)
+					exit(1);
+				dump_mode(-1, sk);
+			}
+
 			break;
 
 		case RECV:
diff --git a/tools/isotest.rst b/tools/isotest.rst
index 124dc71ab..fc5b3c56f 100644
--- a/tools/isotest.rst
+++ b/tools/isotest.rst
@@ -172,6 +172,10 @@  OPTIONS
 
 -k, --bcode=<BCODE>  Socket QoS Broadcast Code
 
+-N, --nbis=<NBIS>  Number of BISes to create as part of a
+                   BIG (BIS broadcaster) or to synchronize
+                   to (BIS broadcast receiver)
+
 EXAMPLES
 ========