X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=2cb61f830bedc42dd95549feda8df0636a21c87e;hb=4f1738388df4c6eed285e80ace50302daa3f5175;hp=1827c59dbffc31278266bf98142a603dcceecf6e;hpb=9f6d4d1302e778a4f57033f3d86c757718f2ca9f;p=fw%2Fopenocd

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 1827c59db..2cb61f830 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);