From 822d66ef69adf795491a21b08395935b6035b977 Mon Sep 17 00:00:00 2001 From: Harald Welte Date: Mon, 6 Mar 2017 20:58:03 +0100 Subject: [PATCH] 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 | 9 +- host/libusb_util.c | 245 ++++++++++++++++++++++++++++++++++++++++ host/libusb_util.h | 48 ++++++++ host/simtrace2-remsim.c | 53 ++++++++- host/simtrace2_usb.c | 77 +++++++++++++ 5 files changed, 423 insertions(+), 9 deletions(-) create mode 100644 host/libusb_util.c create mode 100644 host/libusb_util.h create mode 100644 host/simtrace2_usb.c diff --git a/host/Makefile b/host/Makefile index e3b07317..47ccfec2 100644 --- a/host/Makefile +++ b/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 diff --git a/host/libusb_util.c b/host/libusb_util.c new file mode 100644 index 00000000..81486606 --- /dev/null +++ b/host/libusb_util.c @@ -0,0 +1,245 @@ +#include +#include +#include +#include +#include + +#include + +#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; +} diff --git a/host/libusb_util.h b/host/libusb_util.h new file mode 100644 index 00000000..fc780b15 --- /dev/null +++ b/host/libusb_util.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +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); diff --git a/host/simtrace2-remsim.c b/host/simtrace2-remsim.c index ff5c234a..6b35e835 100644 --- a/host/simtrace2-remsim.c +++ b/host/simtrace2-remsim.c @@ -35,6 +35,7 @@ #include +#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; diff --git a/host/simtrace2_usb.c b/host/simtrace2_usb.c new file mode 100644 index 00000000..e4c58184 --- /dev/null +++ b/host/simtrace2_usb.c @@ -0,0 +1,77 @@ +#include +#include +#include +#include + +#include + +#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(); +}