1 files changed, 66 insertions, 7 deletions

diff --git a/util/usb_if.c b/util/usb_if.c
index 1f0adfecda..f8aa6bfd7e 100644
--- a/util/usb_if.c
+++ b/util/usb_if.c
@@ -66,26 +66,85 @@ out:
 	return iface_num;
 }
 
-int usb_findit(uint16_t vid, uint16_t pid, uint16_t subclass,
-	       uint16_t protocol, struct usb_endpoint *uep)
+static libusb_device_handle *check_device(libusb_device *dev,
+	uint16_t vid, uint16_t pid, const char *serial)
 {
-	int iface_num, r;
+	struct libusb_device_descriptor desc;
+	libusb_device_handle *handle = NULL;
+	char sn[256];
+	size_t sn_size = 0;
+
+	if (libusb_get_device_descriptor(dev, &desc) < 0)
+		return NULL;
+
+	if (libusb_open(dev, &handle) != LIBUSB_SUCCESS)
+		return NULL;
+
+	if (desc.iSerialNumber && serial) {
+		sn_size = libusb_get_string_descriptor_ascii(handle,
+				desc.iSerialNumber, (unsigned char *)sn,
+				sizeof(sn));
+	}
+	/*
+	 * If the VID, PID, and serial number don't match, then it's not the
+	 * correct device. Close the handle and return NULL.
+	 */
+	if ((vid && vid != desc.idVendor) ||
+	    (pid && pid != desc.idProduct) ||
+	    (serial && ((sn_size != strlen(serial)) ||
+	    memcmp(sn, serial, sn_size)))) {
+		libusb_close(handle);
+		return NULL;
+	}
+	return handle;
+}
+
+int usb_findit(const char *serial, uint16_t vid, uint16_t pid,
+	       uint16_t subclass, uint16_t protocol, struct usb_endpoint *uep)
+{
+	int iface_num, r, i;
+	libusb_device **devs;
+	libusb_device_handle *devh = NULL;
+	ssize_t count;
 
 	memset(uep, 0, sizeof(*uep));
 
+	/* Must supply either serial or vendor and product ids. */
+	if (!serial && !(vid && pid))
+		goto terminate_usb_findit;
+
 	r = libusb_init(NULL);
 	if (r < 0) {
 		USB_ERROR("libusb_init", r);
 		goto terminate_usb_findit;
 	}
 
-	printf("open_device %04x:%04x\n", vid, pid);
-	/* NOTE: This doesn't handle multiple matches! */
-	uep->devh = libusb_open_device_with_vid_pid(NULL, vid, pid);
-	if (!uep->devh) {
+	printf("finding_device ");
+	if (vid)
+		printf("%04x:%04x ", vid, pid);
+	if (serial)
+		printf("%s", serial);
+	printf("\n");
+
+	count = libusb_get_device_list(NULL, &devs);
+	if (count < 0)
+		goto terminate_usb_findit;
+
+	for (i = 0; i < count; i++) {
+		devh = check_device(devs[i], vid, pid, serial);
+		if (devh) {
+			printf("Found device.\n");
+			break;
+		}
+	}
+
+	libusb_free_device_list(devs, 1);
+
+	if (!devh) {
 		fprintf(stderr, "Can't find device\n");
 		goto terminate_usb_findit;
 	}
+	uep->devh = devh;
 
 	iface_num = find_interface(subclass, protocol, uep);
 	if (iface_num < 0) {