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;
 }