simtrace2-remsim: Improve support for many interfaces + devices

Rather than using the first available interface on the first available
device, we now have a "simtrace2-list" program that lists all compatible
interfaces on all configurations of all devices on the system

host/Makefile

@@ -1,16 +1,19 @@
 LDFLAGS=`pkg-config --libs libusb-1.0 libosmocore` -losmocore
 CFLAGS=-Wall -g
 
-all: simtrace2-remsim simtrace2-remsim-usb2udp
+all: simtrace2-remsim simtrace2-remsim-usb2udp simtrace2-list
 
-simtrace2-remsim: simtrace2-remsim.o apdu_dispatch.o simtrace2-discovery.o
+simtrace2-remsim: simtrace2-remsim.o apdu_dispatch.o simtrace2-discovery.o libusb_util.o
 	$(CC) -o $@ $^ $(LDFLAGS) -losmosim
 
 simtrace2-remsim-usb2udp: usb2udp.o simtrace2-discovery.o
 	$(CC) -o $@ $^ $(LDFLAGS)
 
+simtrace2-list: simtrace2_usb.o libusb_util.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
 %.o: %.c
 	$(CC) $(CFLAGS) `pkg-config --cflags libusb-1.0 libosmocore` -o $@ -c $^
 
 clean:
-	@rm -f simtrace2-remsim simtrace2-remsim-usb2udp *.o
+	@rm -f simtrace2-remsim simtrace2-remsim-usb2udp simtrace2-list *.o

host/libusb_util.c

@@ -0,0 +1,245 @@
+#include <errno.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include <libusb.h>
+
+#include "libusb_util.h"
+
+static int match_dev_id(const struct libusb_device_descriptor *desc, const struct dev_id *id)
+{
+	if ((desc->idVendor == id->vendor_id) && (desc->idProduct == id->product_id))
+		return 1;
+	return 0;
+}
+
+
+static int match_dev_ids(const struct libusb_device_descriptor *desc, const struct dev_id *ids)
+{
+	const struct dev_id *id;
+
+	for (id = ids; id->vendor_id || id->product_id; id++) {
+		if (match_dev_id(desc, id))
+			return 1;
+	}
+	return 0;
+}
+
+libusb_device **find_matching_usb_devs(const struct dev_id *dev_ids)
+{
+	libusb_device **list;
+	libusb_device **out = calloc(256, sizeof(libusb_device *));
+	libusb_device **cur = out;
+	unsigned int i;
+	int rc;
+
+	if (!out)
+		return NULL;
+
+	rc = libusb_get_device_list(NULL, &list);
+	if (rc <= 0) {
+		perror("No USB devices found");
+		free(out);
+		return NULL;
+	}
+
+	for (i = 0; list[i] != NULL; i++) {
+		struct libusb_device_descriptor dev_desc;
+		libusb_device *dev = list[i];
+
+		rc = libusb_get_device_descriptor(dev, &dev_desc);
+		if (rc < 0) {
+			perror("Couldn't get device descriptor\n");
+			libusb_unref_device(dev);
+			continue;
+		}
+
+		if (match_dev_ids(&dev_desc, dev_ids)) {
+			*cur = dev;
+			cur++;
+			/* FIXME: overflow check */
+		} else
+			libusb_unref_device(dev);
+	}
+	if (cur == out) {
+		libusb_free_device_list(list, 1);
+		free(out);
+		return NULL;
+	}
+
+	libusb_free_device_list(list, 0);
+	return out;
+}
+
+int dev_find_matching_interfaces(libusb_device *dev, int class, int sub_class, int protocol,
+				 struct usb_interface_match *out, unsigned int out_len)
+{
+	struct libusb_device_descriptor dev_desc;
+	int rc, i, out_idx = 0;
+	uint8_t addr;
+
+	rc = libusb_get_device_descriptor(dev, &dev_desc);
+	if (rc < 0) {
+		perror("Couldn't get device descriptor\n");
+		return -EIO;
+	}
+
+	addr = libusb_get_device_address(dev);
+
+	/* iterate over all configurations */
+	for (i = 0; i < dev_desc.bNumConfigurations; i++) {
+		struct libusb_config_descriptor *conf_desc;
+		int j;
+
+		rc = libusb_get_config_descriptor(dev, i, &conf_desc);
+		if (rc < 0) {
+			fprintf(stderr, "Couldn't get config descriptor %u\n", i);
+			continue;
+		}
+		/* iterate over all interfaces */
+		for (j = 0; j < conf_desc->bNumInterfaces; j++) {
+			const struct libusb_interface *intf = &conf_desc->interface[j];
+			int k;
+			/* iterate over all alternate settings */
+			for (k = 0; k < intf->num_altsetting; k++) {
+				const struct libusb_interface_descriptor *if_desc;
+				if_desc = &intf->altsetting[k];
+				if (class > 0 && if_desc->bInterfaceClass != class)
+					continue;
+				if (sub_class > 0 && if_desc->bInterfaceSubClass != sub_class)
+					continue;
+				if (protocol > 0 && if_desc->bInterfaceProtocol != protocol)
+					continue;
+				/* MATCH! */
+				out[out_idx].usb_dev = dev;
+				out[out_idx].vendor = dev_desc.idVendor;
+				out[out_idx].product = dev_desc.idProduct;
+				out[out_idx].addr = addr;
+				out[out_idx].configuration = conf_desc->bConfigurationValue;
+				out[out_idx].interface = if_desc->bInterfaceNumber;
+				out[out_idx].altsetting = if_desc->bAlternateSetting;
+				out[out_idx].class = if_desc->bInterfaceClass;
+				out[out_idx].sub_class = if_desc->bInterfaceSubClass;
+				out[out_idx].protocol = if_desc->bInterfaceProtocol;
+				out[out_idx].string_idx = if_desc->iInterface;
+				out_idx++;
+				if (out_idx >= out_len)
+					return out_idx;
+			}
+		}
+	}
+	return out_idx;
+}
+
+int usb_match_interfaces(libusb_context *ctx, const struct dev_id *dev_ids,
+			 int class, int sub_class, int protocol,
+			 struct usb_interface_match *out, unsigned int out_len)
+{
+	struct usb_interface_match *out_cur = out;
+	unsigned int out_len_remain = out_len;
+	libusb_device **list;
+	libusb_device **dev;
+
+	list = find_matching_usb_devs(dev_ids);
+	if (!list)
+		return 0;
+
+	for (dev = list; *dev; dev++) {
+		int rc;
+
+#if 0
+		struct libusb_device_descriptor dev_desc;
+		uint8_t ports[8];
+		uint8_t addr;
+		rc = libusb_get_device_descriptor(*dev, &dev_desc);
+		if (rc < 0) {
+			perror("Cannot get device descriptor");
+			continue;
+		}
+
+		addr = libusb_get_device_address(*dev);
+
+		rc = libusb_get_port_numbers(*dev, ports, sizeof(ports));
+		if (rc < 0) {
+			perror("Cannot get device path");
+			continue;
+		}
+
+		printf("Found USB Device %04x:%04x at address %d\n",
+			dev_desc.idVendor, dev_desc.idProduct, addr);
+#endif
+
+		rc = dev_find_matching_interfaces(*dev, 255, 2, -1, out_cur, out_len_remain);
+		if (rc < 0)
+			continue;
+		out_cur += rc;
+		out_len_remain -= rc;
+
+	}
+	return out_len - out_len_remain;
+}
+
+libusb_device_handle *usb_open_claim_interface(libusb_context *ctx,
+					       const struct usb_interface_match *ifm)
+{
+	int rc, config;
+	struct dev_id dev_ids[] = { { ifm->vendor, ifm->product }, { 0, 0 } };
+	libusb_device **list;
+	libusb_device **dev;
+	libusb_device_handle *usb_devh = NULL;
+
+	list = find_matching_usb_devs(dev_ids);
+	if (!list) {
+		perror("No USB device with matching VID/PID");
+		return NULL;
+	}
+
+	for (dev = list; *dev; dev++) {
+		int addr;
+
+		addr = libusb_get_device_address(*dev);
+		if (addr == ifm->addr) {
+			rc = libusb_open(*dev, &usb_devh);
+			if (rc < 0) {
+				perror("Cannot open device");
+				break;
+			}
+			rc = libusb_get_configuration(usb_devh, &config);
+			if (rc < 0) {
+				perror("Cannot get current configuration");
+				libusb_close(usb_devh);
+				break;
+			}
+			if (config != ifm->configuration) {
+				rc = libusb_set_configuration(usb_devh, ifm->configuration);
+				if (rc < 0) {
+					perror("Cannot set configuration");
+					libusb_close(usb_devh);
+					break;
+				}
+			}
+			rc = libusb_claim_interface(usb_devh, ifm->interface);
+			if (rc < 0) {
+				perror("Cannot claim interface");
+				libusb_close(usb_devh);
+				break;
+			}
+			rc = libusb_set_interface_alt_setting(usb_devh, ifm->interface, ifm->altsetting);
+			if (rc < 0) {
+				perror("Cannot set interface altsetting");
+				libusb_release_interface(usb_devh, ifm->interface);
+				libusb_close(usb_devh);
+				break;
+			}
+		}
+	}
+
+	/* unref / free list */
+	for (dev = list; *dev; dev++)
+		libusb_unref_device(*dev);
+	free(list);
+
+	return usb_devh;
+}

host/libusb_util.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <libusb.h>
+
+struct dev_id {
+	uint16_t vendor_id;
+	uint16_t product_id;
+};
+
+/* Find any USB devices in the system matching the given Vendor and
+ * Product ID */
+libusb_device **find_matching_usb_devs(const struct dev_id *dev_ids);
+
+/* structure describing a single matching interface found */
+struct usb_interface_match {
+	/* libusb device E*/
+	libusb_device *usb_dev;
+	/* Vendor ID of the device running matching interface */
+	uint16_t vendor;
+	/* Product ID of the device running matching interface */
+	uint16_t product;
+	/* USB Bus Address */
+	uint8_t addr;
+	/* configuration of matching interface */
+	uint8_t configuration;
+	/* interface number of matching interface */
+	uint8_t interface;
+	/* altsetting of matching interface */
+	uint8_t altsetting;
+	/* bInterfaceClass of matching interface */
+	uint8_t class;
+	/* bInterfaceSubClass of matching interface */
+	uint8_t sub_class;
+	/* bInterfaceProtocol of matching interface */
+	uint8_t protocol;
+	/* index of string descriptor of matching interface */
+	uint8_t string_idx;
+};
+
+int dev_find_matching_interfaces(libusb_device *dev, int class, int sub_class, int protocol,
+				 struct usb_interface_match *out, unsigned int out_len);
+
+int usb_match_interfaces(libusb_context *ctx, const struct dev_id *dev_ids,
+			 int class, int sub_class, int protocol,
+			 struct usb_interface_match *out, unsigned int out_len);
+
+libusb_device_handle *usb_open_claim_interface(libusb_context *ctx,
+					       const struct usb_interface_match *ifm);

host/simtrace2-remsim.c

@@ -35,6 +35,7 @@
 
 #include <libusb.h>
 
+#include "libusb_util.h"
 #include "simtrace.h"
 #include "cardemu_prot.h"
 #include "apdu_dispatch.h"
@@ -343,6 +344,12 @@ static void print_help(void)
 		"\t-i\t--gsmtap-ip\tA.B.C.D\n"
 		"\t-a\t--skip-atr\n"
 		"\t-k\t--keep-running\n"
+		"\t-V\t--usb-vendor\tVENDOR_ID\n"
+		"\t-P\t--usb-product\tPRODUCT_ID\n"
+		"\t-C\t--usb-config\tCONFIG_ID\n"
+		"\t-I\t--usb-interface\tINTERFACE_ID\n"
+		"\t-S\t--usb-altsetting ALTSETTING_ID\n"
+		"\t-A\t--usb-address\tADDRESS\n"
 		"\n"
 		);
 }
@@ -354,6 +361,12 @@ static const struct option opts[] = {
 	{ "skip-atr", 0, 0, 'a' },
 	{ "help", 0, 0, 'h' },
 	{ "keep-running", 0, 0, 'k' },
+	{ "usb-vendor", 1, 0, 'V' },
+	{ "usb-product", 1, 0, 'P' },
+	{ "usb-config", 1, 0, 'C' },
+	{ "usb-interface", 1, 0, 'I' },
+	{ "usb-altsetting", 1, 0, 'S' },
+	{ "usb-address", 1, 0, 'A' },
 	{ NULL, 0, 0, 0 }
 };
 
@@ -415,7 +428,8 @@ int main(int argc, char **argv)
 	int skip_atr = 0;
 	int keep_running = 0;
 	int remote_udp_port = 52342;
-	int if_num = 0;
+	int if_num = 0, vendor_id = -1, product_id = -1;
+	int config_id = -1, altsetting = 0, addr = -1;
 	char *remote_udp_host = NULL;
 	struct osim_reader_hdl *reader;
 	struct osim_card_hdl *card;
@@ -425,7 +439,7 @@ int main(int argc, char **argv)
 	while (1) {
 		int option_index = 0;
 
-		c = getopt_long(argc, argv, "r:p:hi:I:ak", opts, &option_index);
+		c = getopt_long(argc, argv, "r:p:hi:V:P:C:I:S:A:ak", opts, &option_index);
 		if (c == -1)
 			break;
 		switch (c) {
@@ -442,18 +456,38 @@ int main(int argc, char **argv)
 		case 'i':
 			gsmtap_host = optarg;
 			break;
-		case 'I':
-			if_num = atoi(optarg);
-			break;
 		case 'a':
 			skip_atr = 1;
 			break;
 		case 'k':
 			keep_running = 1;
 			break;
+		case 'V':
+			vendor_id = strtol(optarg, NULL, 16);
+			break;
+		case 'P':
+			product_id = strtol(optarg, NULL, 16);
+			break;
+		case 'C':
+			config_id = atoi(optarg);
+			break;
+		case 'I':
+			if_num = atoi(optarg);
+			break;
+		case 'S':
+			altsetting = atoi(optarg);
+			break;
+		case 'A':
+			addr = atoi(optarg);
+			break;
 		}
 	}
 
+	if (!remote_udp_host && (vendor_id < 0 || product_id < 0)) {
+		fprintf(stderr, "You have to specify the vendor and product ID\n");
+		goto do_exit;
+	}
+
 	memset(ci, 0, sizeof(*ci));
 	ci->udp_fd = -1;
 
@@ -503,7 +537,14 @@ int main(int argc, char **argv)
 
 	do {
 		if (ci->udp_fd < 0) {
-			ci->usb_devh = libusb_open_device_with_vid_pid(NULL, SIMTRACE_USB_VENDOR, 0x4004);
+			struct usb_interface_match _ifm, *ifm = &_ifm;
+			ifm->vendor = vendor_id;
+			ifm->product = product_id;
+			ifm->configuration = config_id;
+			ifm->interface = if_num;
+			ifm->altsetting = altsetting;
+			ifm->addr = addr;
+			ci->usb_devh = usb_open_claim_interface(NULL, ifm);
 			if (!ci->usb_devh) {
 				fprintf(stderr, "can't open USB device\n");
 				goto close_exit;

host/simtrace2_usb.c

@@ -0,0 +1,77 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include <osmocom/core/utils.h>
+
+#include "libusb_util.h"
+
+#define USB_VENDOR_OPENMOKO		0x1d50
+#define USB_PRODUCT_OWHW_SAM3_DFU	0x4000
+#define USB_PRODUCT_OWHW_SAM3		0x4001
+#define USB_PRODUCT_QMOD_HUB		0x4002
+#define USB_PRODUCT_QMOD_SAM3_DFU	0x4003
+#define USB_PRODUCT_QMOD_SAM3		0x4004
+#define USB_PRODUCT_SIMTRACE2_DFU	0x60e2
+#define USB_PRODUCT_SIMTRACE2		0x60e3
+
+static const struct dev_id compatible_dev_ids[] = {
+	{ USB_VENDOR_OPENMOKO, USB_PRODUCT_OWHW_SAM3 },
+	{ USB_VENDOR_OPENMOKO, USB_PRODUCT_QMOD_SAM3 },
+	{ USB_VENDOR_OPENMOKO, USB_PRODUCT_SIMTRACE2 },
+	{ 0, 0 }
+};
+
+//libusb_get_string_descriptor_ascii(hdl, idx, *data, len)
+
+static int find_devices(void)
+{
+	struct usb_interface_match ifm[16];
+	int rc, i, num_interfaces;
+
+	rc = usb_match_interfaces(NULL, compatible_dev_ids,
+				  255, 2, -1, ifm, ARRAY_SIZE(ifm));
+	if (rc < 0)
+		return rc;
+	num_interfaces = rc;
+
+	for (i = 0; i < num_interfaces; i++) {
+		struct usb_interface_match *m = &ifm[i];
+		libusb_device_handle *dev_handle;
+		char strbuf[256];
+
+		printf("\t%04x:%04x Addr=%u, Cfg=%u, Intf=%u, Alt=%u: %d/%d/%d ",
+			m->vendor, m->product, m->addr,
+			m->configuration, m->interface, m->altsetting,
+			m->class, m->sub_class, m->protocol);
+
+		rc = libusb_open(m->usb_dev, &dev_handle);
+		if (rc < 0) {
+			printf("\n");
+			perror("Cannot open device");
+			continue;
+		}
+		rc = libusb_get_string_descriptor_ascii(dev_handle, m->string_idx, strbuf, sizeof(strbuf));
+		libusb_close(dev_handle);
+		if (rc < 0) {
+			printf("\n");
+			perror("Cannot read string");
+			continue;
+		}
+		printf("(%s)\n", strbuf);
+#if 0
+		dev_handle = usb_open_claim_interface(NULL, m);
+		printf("dev_handle=%p\n", dev_handle);
+		libusb_close(dev_handle);
+#endif
+	}
+
+	return num_interfaces;
+}
+
+int main(int argc, char **argv)
+{
+	libusb_init(NULL);
+	find_devices();
+}