From 885f4388144840e411512543cd92fb9335ce806a Mon Sep 17 00:00:00 2001
From: Spencer Oliver <spen@spen-soft.co.uk>
Date: Thu, 2 Oct 2014 21:54:23 +0100
Subject: [PATCH] cmsis-dap: add serial number support

Change-Id: I66926d1013e2b3a43ce0d18d3599771428706b6a
Signed-off-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-on: http://openocd.zylin.com/2329
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 doc/openocd.texi                 |  5 ++++
 src/jtag/drivers/cmsis_dap_usb.c | 41 +++++++++++++++++++++++++++++++-
 tcl/interface/cmsis-dap.cfg      |  3 +++
 3 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 925eebed2..88d89284c 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -2585,6 +2585,11 @@ cmsis_dap_vid_pid 0xc251 0xf001 0x0d28 0x0204
 @end example
 @end deffn
 
+@deffn {Config Command} {cmsis_dap_serial} [serial]
+Specifies the @var{serial} of the CMSIS-DAP device to use.
+If not specified, serial numbers are not considered.
+@end deffn
+
 @deffn {Command} {cmsis-dap info}
 Display various device information, like hardware version, firmware version, current bus status.
 @end deffn
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index fd8c82e01..95f553b62 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -60,6 +60,7 @@
 /* vid = pid = 0 marks the end of the list */
 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
+static wchar_t *cmsis_dap_serial;
 static bool swd_mode;
 
 #define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
@@ -201,7 +202,12 @@ static int cmsis_dap_usb_open(void)
 
 		if (found) {
 			/* we have found an adapter, so exit further checks */
-			break;
+			/* check serial number matches if given */
+			if (cmsis_dap_serial != NULL) {
+				if (wcscmp(cmsis_dap_serial, cur_dev->serial_number) == 0)
+					break;
+			} else
+				break;
 		}
 
 		cur_dev = cur_dev->next;
@@ -278,6 +284,11 @@ static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 		cmsis_dap_handle = NULL;
 	}
 
+	if (cmsis_dap_serial) {
+		free(cmsis_dap_serial);
+		cmsis_dap_serial = NULL;
+	}
+
 	return;
 }
 
@@ -1006,6 +1017,27 @@ COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_serial_command)
+{
+	if (CMD_ARGC == 1) {
+		size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
+		cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
+		if (cmsis_dap_serial == NULL) {
+			LOG_ERROR("unable to allocate memory");
+			return ERROR_OK;
+		}
+		if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
+			free(cmsis_dap_serial);
+			cmsis_dap_serial = NULL;
+			LOG_ERROR("unable to convert serial");
+		}
+	} else {
+		LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
+	}
+
+	return ERROR_OK;
+}
+
 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	{
 		.name = "info",
@@ -1032,6 +1064,13 @@ static const struct command_registration cmsis_dap_command_handlers[] = {
 		.help = "the vendor ID and product ID of the CMSIS-DAP device",
 		.usage = "(vid pid)* ",
 	},
+	{
+		.name = "cmsis_dap_serial",
+		.handler = &cmsis_dap_handle_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the serial number of the adapter",
+		.usage = "serial_string",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/tcl/interface/cmsis-dap.cfg b/tcl/interface/cmsis-dap.cfg
index a8ea92ec5..ab5c187eb 100644
--- a/tcl/interface/cmsis-dap.cfg
+++ b/tcl/interface/cmsis-dap.cfg
@@ -5,3 +5,6 @@
 #
 
 interface cmsis-dap
+
+# Optionally specify the serial number of CMSIS-DAP usb device.
+#cmsis_dap_serial 02200201E6661E601B98E3B9