arm_adi_v5: prevent possibly endless recursion in dap_dp_init()

If dap_dp_read_atomic() in 30 trials loop fails, dap->do_reconnect is set.
Following dap_dp_read_atomic() calls dap_queue_dp_read() which in case
of SWD transport calls swd_queue_dp_read(). It starts
with swd_check_reconnect() and it calls swd_connect() because
dap->do_reconnect is set. swd_connect() does some initialization,
reads DPIDR and calls dap_dp_init() again!

Moreover if dap_dp_init() is called from cortex_m_reset_(de)assert()
one level of recursion is necessary to reconnect the target.

Introduce dap_dp_init_or_reconnect() for use in cortex_m reset
and similar.
Remove loop of 30 atomic reads of DP_STAT to prevent unwanted recursion.

Change-Id: I54052fdefe50bf5f7c7b59fe751fe2063d5710c9
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/5729
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>

3 files changed, 43 insertions, 23 deletions

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 31c1459..59bb186 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -652,35 +652,22 @@ int dap_dp_init(struct adiv5_dap *dap)
 
 	LOG_DEBUG("%s", adiv5_dap_name(dap));
 
+	dap->do_reconnect = false;
 	dap_invalidate_cache(dap);
 
 	/*
 	 * Early initialize dap->dp_ctrl_stat.
-	 * In jtag mode only, if the following atomic reads fail and set the
-	 * sticky error, it will trigger the clearing of the sticky. Without this
-	 * initialization system and debug power would be disabled while clearing
-	 * the sticky error bit.
+	 * In jtag mode only, if the following queue run (in dap_dp_poll_register)
+	 * fails and sets the sticky error, it will trigger the clearing
+	 * of the sticky. Without this initialization system and debug power
+	 * would be disabled while clearing the sticky error bit.
 	 */
 	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 
-	for (size_t i = 0; i < 30; i++) {
-		/* DP initialization */
-
-		retval = dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
-		if (retval == ERROR_OK)
-			break;
-	}
-
 	/*
 	 * This write operation clears the sticky error bit in jtag mode only and
 	 * is ignored in swd mode. It also powers-up system and debug domains in
 	 * both jtag and swd modes, if not done before.
-	 * Actually we do not need to clear the sticky error here because it has
-	 * been already cleared (if it was set) in the previous atomic read. This
-	 * write could be removed, but this initial part of dap_dp_init() is the
-	 * result of years of fine tuning and there are strong concerns about any
-	 * unnecessary code change. It doesn't harm, so let's keep it here and
-	 * preserve the historical sequence of read/write operations!
 	 */
 	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat | SSTICKYERR);
 	if (retval != ERROR_OK)
@@ -732,6 +719,35 @@ int dap_dp_init(struct adiv5_dap *dap)
 }
 
 /**
+ * Initialize a DAP or do reconnect if DAP is not accessible.
+ *
+ * @param dap The DAP being initialized.
+ */
+int dap_dp_init_or_reconnect(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("%s", adiv5_dap_name(dap));
+
+	/*
+	 * Early initialize dap->dp_ctrl_stat.
+	 * In jtag mode only, if the following atomic reads fail and set the
+	 * sticky error, it will trigger the clearing of the sticky. Without this
+	 * initialization system and debug power would be disabled while clearing
+	 * the sticky error bit.
+	 */
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+
+	dap->do_reconnect = false;
+
+	dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
+	if (dap->do_reconnect) {
+		/* dap connect calls dap_dp_init() after transport dependent initialization */
+		return dap->ops->connect(dap);
+	} else {
+		return dap_dp_init(dap);
+	}
+}
+
+/**
  * Initialize a DAP.  This sets up the power domains, prepares the DP
  * for further use, and arranges to use AP #0 for all AP operations
  * until dap_ap-select() changes that policy.
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index f319a06..8edfaa8 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -550,6 +550,7 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
 
 /* Initialisation of the debug system, power domains and registers */
 int dap_dp_init(struct adiv5_dap *dap);
+int dap_dp_init_or_reconnect(struct adiv5_dap *dap);
 int mem_ap_init(struct adiv5_ap *ap);
 
 /* Invalidate cached DP select and cached TAR and CSW of all APs */
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 94cf824..fae2aac 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1202,11 +1202,13 @@ static int cortex_m_assert_reset(struct target *target)
 		if (retval3 != ERROR_OK)
 			LOG_DEBUG("Ignoring AP write error right after reset");
 
-		retval3 = dap_dp_init(armv7m->debug_ap->dap);
-		if (retval3 != ERROR_OK)
+		retval3 = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
+		if (retval3 != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
-
-		else {
+			/* The error return value must not be propagated in this case.
+			 * SYSRESETREQ or VECTRESET have been possibly triggered
+			 * so reset processing should continue */
+		} else {
 			/* I do not know why this is necessary, but it
 			 * fixes strange effects (step/resume cause NMI
 			 * after reset) on LM3S6918 -- Michael Schwingen
@@ -1249,7 +1251,8 @@ static int cortex_m_deassert_reset(struct target *target)
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
 	    !(jtag_reset_config & RESET_SRST_NO_GATING) &&
 		target_was_examined(target)) {
-		int retval = dap_dp_init(armv7m->debug_ap->dap);
+
+		int retval = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
 			return retval;