Support hla_serial command for ST-LINK adapters.

The hla_serial command allows for a programming device serial number to be
specified in addition to USB VID/PID.  This allows for multiple ST-LINK/V2
programmers to be attached to a single machine and operated using openocd.

Change-Id: I350654bf676eb26ba3a90450acfa55d2a5d2d791
Signed-off-by: Austin Phillips <austin_phillips@hotmail.com>
Reviewed-on: http://openocd.zylin.com/2198
Tested-by: jenkins
Reviewed-by: Martin Glunz <mg@wunderkis.de>
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

10 files changed, 120 insertions, 31 deletions

diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index 917d795..d933845 100644
--- a/src/jtag/aice/aice_usb.c
+++ b/src/jtag/aice/aice_usb.c
@@ -2099,7 +2099,7 @@ static int aice_usb_open(struct aice_port_param_s *param)
 	const uint16_t pids[] = { param->pid, 0 };
 	struct jtag_libusb_device_handle *devh;
 
-	if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
+	if (jtag_libusb_open(vids, pids, NULL, &devh) != ERROR_OK)
 		return ERROR_FAIL;
 
 	/* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
@@ -2123,7 +2123,7 @@ static int aice_usb_open(struct aice_port_param_s *param)
 	/* reopen jlink after usb_reset
 	 * on win32 this may take a second or two to re-enumerate */
 	int retval;
-	while ((retval = jtag_libusb_open(vids, pids, &devh)) != ERROR_OK) {
+	while ((retval = jtag_libusb_open(vids, pids, NULL, &devh)) != ERROR_OK) {
 		usleep(1000);
 		timeout--;
 		if (!timeout)
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index 39be941..55b1e45 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -1699,7 +1699,7 @@ static void jlink_swd_queue_cmd(struct adiv5_dap *dap, uint8_t cmd, uint32_t *ds
 static struct jlink *jlink_usb_open()
 {
 	struct jtag_libusb_device_handle *devh;
-	if (jtag_libusb_open(vids, pids, &devh) != ERROR_OK)
+	if (jtag_libusb_open(vids, pids, NULL, &devh) != ERROR_OK)
 		return NULL;
 
 	/* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
@@ -1723,7 +1723,7 @@ static struct jlink *jlink_usb_open()
 	/* reopen jlink after usb_reset
 	 * on win32 this may take a second or two to re-enumerate */
 	int retval;
-	while ((retval = jtag_libusb_open(vids, pids, &devh)) != ERROR_OK) {
+	while ((retval = jtag_libusb_open(vids, pids, NULL, &devh)) != ERROR_OK) {
 		usleep(1000);
 		timeout--;
 		if (!timeout)
diff --git a/src/jtag/drivers/libusb0_common.c b/src/jtag/drivers/libusb0_common.c
index 16dd4ec..b44b786 100644
--- a/src/jtag/drivers/libusb0_common.c
+++ b/src/jtag/drivers/libusb0_common.c
@@ -37,9 +37,40 @@ static bool jtag_libusb_match(struct jtag_libusb_device *dev,
 	return false;
 }
 
+/* Returns true if the string descriptor indexed by str_index in device matches string */
+static bool string_descriptor_equal(usb_dev_handle *device, uint8_t str_index,
+									const char *string)
+{
+	int retval;
+	bool matched;
+	char desc_string[256+1]; /* Max size of string descriptor */
+
+	if (str_index == 0)
+		return false;
+
+	retval = usb_get_string_simple(device, str_index,
+			desc_string, sizeof(desc_string)-1);
+	if (retval < 0) {
+		LOG_ERROR("usb_get_string_simple() failed with %d", retval);
+		return false;
+	}
+
+	/* Null terminate descriptor string in case it needs to be logged. */
+	desc_string[sizeof(desc_string)-1] = '\0';
+
+	matched = strncmp(string, desc_string, sizeof(desc_string)) == 0;
+	if (!matched)
+		LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
+			desc_string, string);
+	return matched;
+}
+
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out)
 {
+	int retval = -ENODEV;
+	struct jtag_libusb_device_handle *libusb_handle;
 	usb_init();
 
 	usb_find_busses();
@@ -52,13 +83,24 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 			if (!jtag_libusb_match(dev, vids, pids))
 				continue;
 
-			*out = usb_open(dev);
-			if (NULL == *out)
-				return -errno;
-			return 0;
+			libusb_handle = usb_open(dev);
+			if (NULL == libusb_handle) {
+				retval = -errno;
+				continue;
+			}
+
+			/* Device must be open to use libusb_get_string_descriptor_ascii. */
+			if (serial != NULL &&
+					!string_descriptor_equal(libusb_handle, dev->descriptor.iSerialNumber, serial)) {
+				usb_close(libusb_handle);
+				continue;
+			}
+			*out = libusb_handle;
+			retval = 0;
+			break;
 		}
 	}
-	return -ENODEV;
+	return retval;
 }
 
 void jtag_libusb_close(jtag_libusb_device_handle *dev)
diff --git a/src/jtag/drivers/libusb0_common.h b/src/jtag/drivers/libusb0_common.h
index db247ad..370954f 100644
--- a/src/jtag/drivers/libusb0_common.h
+++ b/src/jtag/drivers/libusb0_common.h
@@ -54,6 +54,7 @@ static inline int jtag_libusb_release_interface(jtag_libusb_device_handle *devh,
 }
 
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out);
 void jtag_libusb_close(jtag_libusb_device_handle *dev);
 int jtag_libusb_control_transfer(jtag_libusb_device_handle *dev,
diff --git a/src/jtag/drivers/libusb1_common.c b/src/jtag/drivers/libusb1_common.c
index 99e0d48..a29b2e9 100644
--- a/src/jtag/drivers/libusb1_common.c
+++ b/src/jtag/drivers/libusb1_common.c
@@ -28,25 +28,53 @@
 static struct libusb_context *jtag_libusb_context; /**< Libusb context **/
 static libusb_device **devs; /**< The usb device list **/
 
-static bool jtag_libusb_match(struct jtag_libusb_device *dev,
+static bool jtag_libusb_match(struct libusb_device_descriptor *dev_desc,
 		const uint16_t vids[], const uint16_t pids[])
 {
-	struct libusb_device_descriptor dev_desc;
-
 	for (unsigned i = 0; vids[i]; i++) {
-		if (libusb_get_device_descriptor(dev, &dev_desc) == 0) {
-			if (dev_desc.idVendor == vids[i] &&
-				dev_desc.idProduct == pids[i])
-				return true;
+		if (dev_desc->idVendor == vids[i] &&
+			dev_desc->idProduct == pids[i]) {
+			return true;
 		}
 	}
 	return false;
 }
 
+/* Returns true if the string descriptor indexed by str_index in device matches string */
+static bool string_descriptor_equal(libusb_device_handle *device, uint8_t str_index,
+									const char *string)
+{
+	int retval;
+	bool matched;
+	char desc_string[256+1]; /* Max size of string descriptor */
+
+	if (str_index == 0)
+		return false;
+
+	retval = libusb_get_string_descriptor_ascii(device, str_index,
+			(unsigned char *)desc_string, sizeof(desc_string)-1);
+	if (retval < 0) {
+		LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %d", retval);
+		return false;
+	}
+
+	/* Null terminate descriptor string in case it needs to be logged. */
+	desc_string[sizeof(desc_string)-1] = '\0';
+
+	matched = strncmp(string, desc_string, sizeof(desc_string)) == 0;
+	if (!matched)
+		LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
+			desc_string, string);
+	return matched;
+}
+
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out)
 {
 	int cnt, idx, errCode;
+	int retval = -ENODEV;
+	struct jtag_libusb_device_handle *libusb_handle = NULL;
 
 	if (libusb_init(&jtag_libusb_context) < 0)
 		return -ENODEV;
@@ -54,22 +82,37 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 	cnt = libusb_get_device_list(jtag_libusb_context, &devs);
 
 	for (idx = 0; idx < cnt; idx++) {
-		if (!jtag_libusb_match(devs[idx], vids, pids))
+		struct libusb_device_descriptor dev_desc;
+
+		if (libusb_get_device_descriptor(devs[idx], &dev_desc) != 0)
 			continue;
 
-		errCode = libusb_open(devs[idx], out);
+		if (!jtag_libusb_match(&dev_desc, vids, pids))
+			continue;
 
-		/** Free the device list **/
-		libusb_free_device_list(devs, 1);
+		errCode = libusb_open(devs[idx], &libusb_handle);
 
 		if (errCode) {
 			LOG_ERROR("libusb_open() failed with %s",
 				  libusb_error_name(errCode));
-			return errCode;
+			continue;
+		}
+
+		/* Device must be open to use libusb_get_string_descriptor_ascii. */
+		if (serial != NULL &&
+				!string_descriptor_equal(libusb_handle, dev_desc.iSerialNumber, serial)) {
+			libusb_close(libusb_handle);
+			continue;
 		}
-		return 0;
+
+		/* Success. */
+		*out = libusb_handle;
+		retval = 0;
+		break;
 	}
-	return -ENODEV;
+	if (cnt >= 0)
+		libusb_free_device_list(devs, 1);
+	return retval;
 }
 
 void jtag_libusb_close(jtag_libusb_device_handle *dev)
diff --git a/src/jtag/drivers/libusb1_common.h b/src/jtag/drivers/libusb1_common.h
index e33b83a..a4447ca 100644
--- a/src/jtag/drivers/libusb1_common.h
+++ b/src/jtag/drivers/libusb1_common.h
@@ -48,6 +48,7 @@ static inline int jtag_libusb_release_interface(jtag_libusb_device_handle *devh,
 }
 
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
+		const char *serial,
 		struct jtag_libusb_device_handle **out);
 void jtag_libusb_close(jtag_libusb_device_handle *dev);
 int jtag_libusb_control_transfer(jtag_libusb_device_handle *dev,
diff --git a/src/jtag/drivers/opendous.c b/src/jtag/drivers/opendous.c
index 8bfaf0d..6af3b28 100644
--- a/src/jtag/drivers/opendous.c
+++ b/src/jtag/drivers/opendous.c
@@ -709,7 +709,7 @@ struct opendous_jtag *opendous_usb_open(void)
 	struct opendous_jtag *result;
 
 	struct jtag_libusb_device_handle *devh;
-	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, &devh) != ERROR_OK)
+	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, NULL, &devh) != ERROR_OK)
 		return NULL;
 
 	jtag_libusb_set_configuration(devh, 0);
diff --git a/src/jtag/drivers/osbdm.c b/src/jtag/drivers/osbdm.c
index db9c29f..d1eeedb 100644
--- a/src/jtag/drivers/osbdm.c
+++ b/src/jtag/drivers/osbdm.c
@@ -375,7 +375,7 @@ static int osbdm_flush(struct osbdm *osbdm, struct queue* queue)
 static int osbdm_open(struct osbdm *osbdm)
 {
 	(void)memset(osbdm, 0, sizeof(*osbdm));
-	if (jtag_libusb_open(osbdm_vid, osbdm_pid, &osbdm->devh) != ERROR_OK)
+	if (jtag_libusb_open(osbdm_vid, osbdm_pid, NULL, &osbdm->devh) != ERROR_OK)
 		return ERROR_FAIL;
 
 	if (jtag_libusb_claim_interface(osbdm->devh, 0) != ERROR_OK)
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index bd970ab..58af0dd 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1600,9 +1600,11 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 
 	const uint16_t vids[] = { param->vid, 0 };
 	const uint16_t pids[] = { param->pid, 0 };
+	const char *serial = param->serial;
 
-	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
-		param->vid, param->pid);
+	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
+			param->transport, param->vid, param->pid,
+			param->serial ? param->serial : "");
 
 	/*
 	  On certain host USB configurations(e.g. MacBook Air)
@@ -1614,7 +1616,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	  in order to become operational.
 	 */
 	do {
-		if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
+		if (jtag_libusb_open(vids, pids, serial, &h->fd) != ERROR_OK) {
 			LOG_ERROR("open failed");
 			goto error_open;
 		}
diff --git a/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c b/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
index 437150d..70dab20 100644
--- a/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
+++ b/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
@@ -186,7 +186,7 @@ static int ublast2_libusb_init(struct ublast_lowlevel *low)
 	bool renumeration = false;
 	int ret;
 
-	if (jtag_libusb_open(vids, pids, &temp) == ERROR_OK) {
+	if (jtag_libusb_open(vids, pids, NULL, &temp) == ERROR_OK) {
 		LOG_INFO("Altera USB-Blaster II (uninitialized) found");
 		LOG_INFO("Loading firmware...");
 		ret = load_usb_blaster_firmware(temp, low);
@@ -200,13 +200,13 @@ static int ublast2_libusb_init(struct ublast_lowlevel *low)
 	const uint16_t pids_renum[] = { low->ublast_pid, 0 };
 
 	if (renumeration == false) {
-		if (jtag_libusb_open(vids_renum, pids_renum, &low->libusb_dev) != ERROR_OK) {
+		if (jtag_libusb_open(vids_renum, pids_renum, NULL, &low->libusb_dev) != ERROR_OK) {
 			LOG_ERROR("Altera USB-Blaster II not found");
 			return ERROR_FAIL;
 		}
 	} else {
 		int retry = 10;
-		while (jtag_libusb_open(vids_renum, pids_renum, &low->libusb_dev) != ERROR_OK && retry--) {
+		while (jtag_libusb_open(vids_renum, pids_renum, NULL, &low->libusb_dev) != ERROR_OK && retry--) {
 			usleep(1000000);
 			LOG_INFO("Waiting for renumerate...");
 		}