X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=82c2b5ab6958e3a56b635a67f69cc6982fd77cb0;hp=31c219e59643d002ed575716233d9bd42729d985;hb=a6c4eb03455f6e97fc25183aae249d6ccdcbfb0f;hpb=d2bb14e36a03cc187ca090a0da418acccb4098b4

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 31c219e596..82c2b5ab69 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -63,49 +63,93 @@ static void swd_finish_read(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
 	if (dap->last_read != NULL) {
-		swd->read_reg(dap, swd_cmd(true, false, DP_RDBUFF), dap->last_read);
+		swd->read_reg(swd_cmd(true, false, DP_RDBUFF), dap->last_read, 0);
 		dap->last_read = NULL;
 	}
 }
 
 static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data);
+static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
+		uint32_t *data);
 
 static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
-		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 }
 
 static int swd_run_inner(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
+	int retval;
 
-	int retval = swd->run(dap);
+	retval = swd->run();
 
 	if (retval != ERROR_OK) {
 		/* fault response */
-		swd_clear_sticky_errors(dap);
+		dap->do_reconnect = true;
 	}
 
 	return retval;
 }
 
+static int swd_connect(struct adiv5_dap *dap)
+{
+	uint32_t idcode;
+	int status;
+
+	/* FIXME validate transport config ... is the
+	 * configured DAP present (check IDCODE)?
+	 * Is *only* one DAP configured?
+	 *
+	 * MUST READ IDCODE
+	 */
+
+	/* Note, debugport_init() does setup too */
+	jtag_interface->swd->switch_seq(JTAG_TO_SWD);
+
+	dap->do_reconnect = false;
+
+	swd_queue_dp_read(dap, DP_IDCODE, &idcode);
+
+	/* force clear all sticky faults */
+	swd_clear_sticky_errors(dap);
+
+	status = swd_run_inner(dap);
+
+	if (status == ERROR_OK) {
+		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
+		dap->do_reconnect = false;
+	} else
+		dap->do_reconnect = true;
+
+	return status;
+}
+
 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;
 	assert(swd);
 
-	swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
-		DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+		DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 	return check_sync(dap);
 }
 
@@ -132,22 +176,29 @@ 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);
+	swd->read_reg(swd_cmd(true,  false, reg), data, 0);
 
 	return check_sync(dap);
 }
 
-
 static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
 	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);
+	swd->write_reg(swd_cmd(false,  false, reg), data, 0);
 
 	return check_sync(dap);
 }
@@ -172,8 +223,12 @@ static int swd_queue_ap_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_ap_bankselect(dap, reg);
-	swd->read_reg(dap, swd_cmd(true,  true, reg), dap->last_read);
+	swd->read_reg(swd_cmd(true,  true, reg), dap->last_read, dap->ap[dap_ap_get_select(dap)].memaccess_tck);
 	dap->last_read = data;
 
 	return check_sync(dap);
@@ -185,9 +240,13 @@ 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);
+	swd->write_reg(swd_cmd(false,  true, reg), data, dap->ap[dap_ap_get_select(dap)].memaccess_tck);
 
 	return check_sync(dap);
 }
@@ -253,6 +312,11 @@ int dap_to_swd(struct target *target)
 	struct arm *arm = target_to_arm(target);
 	int retval;
 
+	if (!arm->dap) {
+		LOG_ERROR("SWD mode is not available");
+		return ERROR_FAIL;
+	}
+
 	LOG_DEBUG("Enter SWD mode");
 
 	/* REVISIT it's ugly to need to make calls to a "jtag"
@@ -380,7 +444,7 @@ static int swd_select(struct command_context *ctx)
 		return ERROR_FAIL;
 	}
 
-	retval = swd->init(1);
+	retval = swd->init();
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("can't init SWD driver");
 		return retval;
@@ -403,36 +467,11 @@ static int swd_init(struct command_context *ctx)
 	struct target *target = get_current_target(ctx);
 	struct arm *arm = target_to_arm(target);
 	struct adiv5_dap *dap = arm->dap;
-	uint32_t idcode;
-	int status;
-
 	/* Force the DAP's ops vector for SWD mode.
 	 * messy - is there a better way? */
 	arm->dap->ops = &swd_dap_ops;
 
-	/* FIXME validate transport config ... is the
-	 * configured DAP present (check IDCODE)?
-	 * Is *only* one DAP configured?
-	 *
-	 * MUST READ IDCODE
-	 */
-
- /* Note, debugport_init() does setup too */
-
-	swd_queue_dp_read(dap, DP_IDCODE, &idcode);
-
-	/* force clear all sticky faults */
-	swd_clear_sticky_errors(dap);
-
-	status = swd_run(dap);
-
-	if (status == ERROR_OK)
-		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
-
-	/* this is a workaround to get polling working */
-	jtag_add_reset(0, 0);
-
-	return status;
+	return swd_connect(dap);
 }
 
 static struct transport swd_transport = {