target/adi_v5_swd, cortex_m: properly handle more cases requiring reconnect

This brings SWD reconnection procedure in line with the ARM
documentation and changes cortex_m reset procedure to make use of it.

The motivation behind this patch is to make SAM4L "reset" and "reset
halt" properly without SRST. The complication here is that EDBG issues
an additional read of DP_RDBUFF automatically right after writing
SYSRESETREQ, that leads to a FAULT which needs to be dealt with
properly. With this patch the very first ahbap_debugport_init DAP
access will make SWD layer properly reinitialise the link before
continuing.

Runtime tested with mbed CMIS-DAP + KL25 only.

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

1 files changed, 27 insertions, 6 deletions

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 1827c59..2cb61f8 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -112,6 +112,8 @@ static int swd_connect(struct adiv5_dap *dap)
 	/* Note, debugport_init() does setup too */
 	jtag_interface->swd->switch_seq(dap, JTAG_TO_SWD);
 
+	dap->do_reconnect = false;
+
 	swd_queue_dp_read(dap, DP_IDCODE, &idcode);
 
 	/* force clear all sticky faults */
@@ -122,7 +124,8 @@ static int swd_connect(struct adiv5_dap *dap)
 	if (status == ERROR_OK) {
 		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
 		dap->do_reconnect = false;
-	}
+	} else
+		dap->do_reconnect = true;
 
 	return status;
 }
@@ -132,6 +135,14 @@ static inline int check_sync(struct adiv5_dap *dap)
 	return do_sync ? swd_run_inner(dap) : ERROR_OK;
 }
 
+static int swd_check_reconnect(struct adiv5_dap *dap)
+{
+	if (dap->do_reconnect)
+		return swd_connect(dap);
+
+	return ERROR_OK;
+}
+
 static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
@@ -165,6 +176,10 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd_queue_dp_bankselect(dap, reg);
 	swd->read_reg(dap, swd_cmd(true,  false, reg), data);
 
@@ -177,6 +192,10 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd_finish_read(dap);
 	swd_queue_dp_bankselect(dap, reg);
 	swd->write_reg(dap, swd_cmd(false,  false, reg), data);
@@ -204,11 +223,9 @@ static int swd_queue_ap_read(struct adiv5_dap *dap, unsigned reg,
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	if (dap->do_reconnect) {
-		int retval = swd_connect(dap);
-		if (retval != ERROR_OK)
-			return retval;
-	}
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
 
 	swd_queue_ap_bankselect(dap, reg);
 	swd->read_reg(dap, swd_cmd(true,  true, reg), dap->last_read);
@@ -223,6 +240,10 @@ static int swd_queue_ap_write(struct adiv5_dap *dap, unsigned reg,
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
 	swd_finish_read(dap);
 	swd_queue_ap_bankselect(dap, reg);
 	swd->write_reg(dap, swd_cmd(false,  true, reg), data);