drivers/cmsis_dap_usb: restructure init sequence a bit

This fixes the issue of improper initialisation sequence and in
particular makes "cmsis_dap_vid_pid" config specification functional.

Not really elegant but it's in line with the ftdi driver and so can be
reworked in a uniform way later when the internal API is changed.

Change-Id: Ief9fc64ad0ac24e1c66727153f383e4f30a830c7
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/2192
Reviewed-by: Andrey Yurovsky <yurovsky@gmail.com>
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

1 files changed, 41 insertions, 28 deletions

diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 7d4c579a..4073d215 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 bool swd_mode;
 
 #define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
 #define USB_TIMEOUT       1000
@@ -720,11 +721,50 @@ static int cmsis_dap_reset_link(void)
 	return retval;
 }
 
+static int cmsis_dap_swd_open(void)
+{
+	int retval;
+
+	DEBUG_IO("CMSIS-DAP: cmsis_dap_swd_open");
+
+	if (cmsis_dap_handle == NULL) {
+
+		/* SWD init */
+		retval = cmsis_dap_usb_open();
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = cmsis_dap_get_caps_info();
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
+		LOG_ERROR("CMSIS-DAP: SWD not supported");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Add more setup here.??... */
+
+	LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
+	return ERROR_OK;
+}
+
 static int cmsis_dap_init(void)
 {
 	int retval;
 	uint8_t *data;
 
+	if (swd_mode) {
+		retval = cmsis_dap_swd_open();
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
 	if (cmsis_dap_handle == NULL) {
 
 		/* JTAG init */
@@ -832,34 +872,7 @@ static int cmsis_dap_init(void)
 
 static int cmsis_dap_swd_init(void)
 {
-	int retval;
-
-	DEBUG_IO("CMSIS-DAP: cmsis_dap_swd_init");
-
-	if (cmsis_dap_handle == NULL) {
-
-		/* SWD init */
-		retval = cmsis_dap_usb_open();
-		if (retval != ERROR_OK)
-			return retval;
-
-		retval = cmsis_dap_get_caps_info();
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
-		LOG_ERROR("CMSIS-DAP: SWD not supported");
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* Add more setup here.??... */
-
-	LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
+	swd_mode = true;
 	return ERROR_OK;
 }