From b2189fa93657a8288d2a90c92814d64bf25425b3 Mon Sep 17 00:00:00 2001
From: Spencer Oliver <spen@spen-soft.co.uk>
Date: Thu, 11 Apr 2013 11:15:32 +0100
Subject: [PATCH] stlink: fix connect under reset issues

We need to make sure that srst is asserted before we attempt to switch into
jtag or swd mode otherwise we receive a error (-9) - invalid device id.

Change-Id: I625166c751cfba8e8a5290f40122bb9afc9dbb39
Signed-off-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-on: http://openocd.zylin.com/1315
Tested-by: jenkins
---
 src/jtag/drivers/stlink_usb.c | 12 ++++++++++--
 src/jtag/hla/hla_interface.c  | 24 +++++++++++++++---------
 src/jtag/hla/hla_interface.h  |  2 ++
 3 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index ce7ff4132..72b31c96a 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -579,8 +579,10 @@ static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 	return ERROR_OK;
 }
 
+static int stlink_usb_assert_srst(void *handle, int srst);
+
 /** */
-static int stlink_usb_init_mode(void *handle)
+static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 {
 	int res;
 	uint8_t mode;
@@ -674,6 +676,12 @@ static int stlink_usb_init_mode(void *handle)
 		return ERROR_FAIL;
 	}
 
+	if (connect_under_reset) {
+		res = stlink_usb_assert_srst(handle, 0);
+		if (res != ERROR_OK)
+			return res;
+	}
+
 	res = stlink_usb_mode_enter(handle, emode);
 
 	if (res != ERROR_OK)
@@ -1314,7 +1322,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	h->jtag_api = api;
 
 	/* initialize the debug hardware */
-	err = stlink_usb_init_mode(h);
+	err = stlink_usb_init_mode(h, param->connect_under_reset);
 
 	if (err != ERROR_OK) {
 		LOG_ERROR("init mode failed");
diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c
index 53ad8e717..8453a6ff5 100644
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -37,12 +37,21 @@
 
 #include <target/target.h>
 
-static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, 0, 0}, 0, 0 };
+static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, 0, false}, 0, 0 };
 
 int hl_interface_open(enum hl_transports tr)
 {
 	LOG_DEBUG("hl_interface_open");
 
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+		if (jtag_reset_config & RESET_SRST_NO_GATING)
+			hl_if.param.connect_under_reset = true;
+		else
+			LOG_WARNING("\'srst_nogate\' reset_config option is required");
+	}
+
 	/* set transport mode */
 	hl_if.param.transport = tr;
 
@@ -117,14 +126,11 @@ static int hl_interface_execute_queue(void)
 
 int hl_interface_init_reset(void)
 {
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-
-	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
-		if (jtag_reset_config & RESET_SRST_NO_GATING) {
-			jtag_add_reset(0, 1);
-			hl_if.layout->api->assert_srst(hl_if.fd, 0);
-		} else
-			LOG_WARNING("\'srst_nogate\' reset_config option is required");
+	/* incase the adapter has not already handled asserting srst
+	 * we will attempt it again */
+	if (hl_if.param.connect_under_reset) {
+		jtag_add_reset(0, 1);
+		hl_if.layout->api->assert_srst(hl_if.fd, 0);
 	}
 
 	return ERROR_OK;
diff --git a/src/jtag/hla/hla_interface.h b/src/jtag/hla/hla_interface.h
index 5b63ab855..980c6d9c0 100644
--- a/src/jtag/hla/hla_interface.h
+++ b/src/jtag/hla/hla_interface.h
@@ -46,6 +46,8 @@ struct hl_interface_param_s {
 	enum hl_transports transport;
 	/** */
 	int max_buffer;
+	/** */
+	bool connect_under_reset;
 };
 
 struct hl_interface_s {