adi_v5_swd: wait for readable DPIDR, ABORT if stalled

Reading of DPIDR is the very first operation after JTAG to SWD sequence.
Without this change if DPIDR read fails then swd connect fails.

Keep trying JTAG to SWD sequence and DPIDR read until success
or timeout 0.5 sec. It makes setting of adapter srst delay on SWD transport
mostly unnecessary.

Also test for ERROR_WAIT (which should not occur according to
IHI 0031E B4.3.2 but a quirk is known) and if bus is kept stalled
then issue abort to make the next connect possible.

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

1 files changed, 56 insertions, 13 deletions

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index ee30ff7..b25181e 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -118,26 +118,69 @@ static int swd_connect(struct adiv5_dap *dap)
 		}
 	}
 
-	/* Note, debugport_init() does setup too */
-	swd->switch_seq(JTAG_TO_SWD);
 
-	/* Clear link state, including the SELECT cache. */
-	dap->do_reconnect = false;
-	dap_invalidate_cache(dap);
+	int64_t timeout = timeval_ms() + 500;
 
-	swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+	do {
+		/* Note, debugport_init() does setup too */
+		swd->switch_seq(JTAG_TO_SWD);
 
-	/* force clear all sticky faults */
-	swd_clear_sticky_errors(dap);
+		/* Clear link state, including the SELECT cache. */
+		dap->do_reconnect = false;
+		dap_invalidate_cache(dap);
+
+		status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+		if (status == ERROR_OK) {
+			status = swd_run_inner(dap);
+			if (status == ERROR_OK)
+				break;
+		}
+
+		alive_sleep(1);
+
+	} while (timeval_ms() < timeout);
+
+	if (status != ERROR_OK) {
+		LOG_ERROR("Error connecting DP: cannot read IDR");
+		return status;
+	}
+
+	LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+
+	do {
+		dap->do_reconnect = false;
 
-	status = swd_run_inner(dap);
+		/* force clear all sticky faults */
+		swd_clear_sticky_errors(dap);
+
+		status = swd_run_inner(dap);
+		if (status != ERROR_WAIT)
+			break;
+
+		alive_sleep(10);
+
+	} while (timeval_ms() < timeout);
+
+	/* IHI 0031E B4.3.2:
+	 * "A WAIT response must not be issued to the ...
+	 * ... writes to the ABORT register"
+	 * swd_clear_sticky_errors() writes to the ABORT register only.
+	 *
+	 * Unfortunately at least Microchip SAMD51/E53/E54 returns WAIT
+	 * in a corner case. Just try if ABORT resolves the problem.
+	 */
+	if (status == ERROR_WAIT) {
+		LOG_WARNING("Connecting DP: stalled AP operation, issuing ABORT");
 
-	if (status == ERROR_OK) {
-		LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
 		dap->do_reconnect = false;
+
+		swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+			DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+		status = swd_run_inner(dap);
+	}
+
+	if (status == ERROR_OK)
 		status = dap_dp_init(dap);
-	} else
-		dap->do_reconnect = true;
 
 	return status;
 }