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>

13 files changed, 139 insertions, 32 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index e51de4de..15965dab 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -3079,13 +3079,17 @@ This type of adapter does not expose some of the lower level api's
 that OpenOCD would normally use to access the target.
 
 Currently supported adapters include the ST STLINK and TI ICDI.
+STLINK firmware version >= V2.J21.S4 recommended due to issues with earlier
+versions of firmware where serial number is reset after first use.  Suggest
+using ST firmware update utility to upgrade STLINK firmware even if current
+version reported is V2.J21.S4.
 
 @deffn {Config Command} {hla_device_desc} description
 Currently Not Supported.
 @end deffn
 
 @deffn {Config Command} {hla_serial} serial
-Currently Not Supported.
+Specifies the serial number of the adapter.
 @end deffn
 
 @deffn {Config Command} {hla_layout} (@option{stlink}|@option{icdi})
diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index 917d795b..d933845c 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 39be9412..55b1e45f 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 16dd4ecb..b44b7861 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 db247ad8..370954fd 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 99e0d482..a29b2e91 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 e33b83ad..a4447ca9 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 8bfaf0d9..6af3b28f 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 db9c29f2..d1eeedb3 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 bd970ab4..58af0dd5 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 437150d7..70dab20d 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...");
 		}
diff --git a/tcl/interface/stlink-v2-1.cfg b/tcl/interface/stlink-v2-1.cfg
index e1bccbc2..093e8017 100644
--- a/tcl/interface/stlink-v2-1.cfg
+++ b/tcl/interface/stlink-v2-1.cfg
@@ -7,3 +7,10 @@ hla_layout stlink
 hla_device_desc "ST-LINK/V2-1"
 hla_vid_pid 0x0483 0x374b
 
+# Optionally specify the serial number of ST-LINK/V2 usb device.  ST-LINK/V2
+# devices seem to have serial numbers with unreadable characters.  ST-LINK/V2
+# firmware version >= V2.J21.S4 recommended to avoid issues with adapter serial
+# number reset issues.
+# eg.
+#hla_serial "\xaa\xbc\x6e\x06\x50\x75\xff\x55\x17\x42\x19\x3f"
+
diff --git a/tcl/interface/stlink-v2.cfg b/tcl/interface/stlink-v2.cfg
index e145d635..ae545a11 100644
--- a/tcl/interface/stlink-v2.cfg
+++ b/tcl/interface/stlink-v2.cfg
@@ -7,3 +7,10 @@ hla_layout stlink
 hla_device_desc "ST-LINK/V2"
 hla_vid_pid 0x0483 0x3748
 
+# Optionally specify the serial number of ST-LINK/V2 usb device.  ST-LINK/V2
+# devices seem to have serial numbers with unreadable characters.  ST-LINK/V2
+# firmware version >= V2.J21.S4 recommended to avoid issues with adapter serial
+# number reset issues.
+# eg.
+#hla_serial "\xaa\xbc\x6e\x06\x50\x75\xff\x55\x17\x42\x19\x3f"
+