X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=c080964d07a1275840406af7ad0e8a51143542aa;hp=4c9897394a428c9938d3f6f3b3f7240c8698dea2;hb=830d0c55c0920606366a15560d1945f1e1942744;hpb=677b02b475870b7d9e5d86e9bf61dc28dae5a6e4

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 4c9897394a..c080964d07 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -57,179 +57,217 @@
 
 /* YUK! - but this is currently a global.... */
 extern struct jtag_interface *jtag_interface;
+static bool do_sync;
 
-static int swd_finish_read(struct adiv5_dap *dap)
+static void swd_finish_read(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
-	int retval = ERROR_OK;
 	if (dap->last_read != NULL) {
-		retval = swd->read_reg(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;
 	}
-	return retval;
 }
 
-static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg,
+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(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;
+
+	retval = swd->run();
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		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);
+
+	/* Make sure we don't try to perform any other accesses before the DPIDR read. */
+	dap->do_reconnect = false;
+	dap->select = 0;
+
+	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);
 
-	return swd->write_reg(swd_cmd(false,  false, DP_ABORT),
-		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+		DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+	return check_sync(dap);
 }
 
 /** Select the DP register bank matching bits 7:4 of reg. */
-static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
+static void swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
 {
-	uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
-
 	if (reg == DP_SELECT)
-		return ERROR_OK;
+		return;
 
-	if (select_dp_bank == dap->dp_bank_value)
-		return ERROR_OK;
+	uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
+	uint32_t select = select_dp_bank
+			| (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
 
-	dap->dp_bank_value = select_dp_bank;
-	select_dp_bank |= dap->ap_current | dap->ap_bank_value;
+	if (select == dap->select)
+		return;
 
-	return swd_queue_dp_write(dap, DP_SELECT, select_dp_bank);
+	dap->select = select;
+
+	swd_queue_dp_write(dap, DP_SELECT, select);
 }
 
 static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 		uint32_t *data)
 {
-	int retval;
-	/* REVISIT status return vs ack ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	retval = swd_queue_dp_bankselect(dap, reg);
+	int retval = swd_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = swd->read_reg(swd_cmd(true,  false, reg), data);
-
-	if (retval != ERROR_OK) {
-		/* fault response */
-		uint8_t ack = retval & 0xff;
-		swd_queue_ap_abort(dap, &ack);
-	}
+	swd_queue_dp_bankselect(dap, reg);
+	swd->read_reg(swd_cmd(true,  false, reg), data, 0);
 
-	return retval;
+	return check_sync(dap);
 }
 
-
-static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
-	int retval;
-	/* REVISIT status return vs ack ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	retval = swd_finish_read(dap);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = swd_queue_dp_bankselect(dap, reg);
+	int retval = swd_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = swd->write_reg(swd_cmd(false,  false, reg), data);
+	swd_finish_read(dap);
+	swd_queue_dp_bankselect(dap, reg);
+	swd->write_reg(swd_cmd(false,  false, reg), data, 0);
 
-	if (retval != ERROR_OK) {
-		/* fault response */
-		uint8_t ack = retval & 0xff;
-		swd_queue_ap_abort(dap, &ack);
-	}
-
-	return retval;
+	return check_sync(dap);
 }
 
 /** Select the AP register bank matching bits 7:4 of reg. */
-static int swd_queue_ap_bankselect(struct adiv5_dap *dap, unsigned reg)
+static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
-	uint32_t select_ap_bank = reg & 0x000000F0;
+	struct adiv5_dap *dap = ap->dap;
+	uint32_t select = ((uint32_t)ap->ap_num << 24)
+			| (reg & 0x000000F0)
+			| (dap->select & DP_SELECT_DPBANK);
 
-	if (select_ap_bank == dap->ap_bank_value)
-		return ERROR_OK;
+	if (select == dap->select)
+		return;
 
-	dap->ap_bank_value = select_ap_bank;
-	select_ap_bank |= dap->ap_current | dap->dp_bank_value;
+	dap->select = select;
 
-	return swd_queue_dp_write(dap, DP_SELECT, select_ap_bank);
+	swd_queue_dp_write(dap, DP_SELECT, select);
 }
 
-static int (swd_queue_ap_read)(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
 		uint32_t *data)
 {
-	/* REVISIT status return ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	int retval = swd_queue_ap_bankselect(dap, reg);
+	struct adiv5_dap *dap = ap->dap;
+
+	int retval = swd_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = swd->read_reg(swd_cmd(true,  true, reg), dap->last_read);
+	swd_queue_ap_bankselect(ap, reg);
+	swd->read_reg(swd_cmd(true,  true, reg), dap->last_read, ap->memaccess_tck);
 	dap->last_read = data;
 
-	if (retval != ERROR_OK) {
-		/* fault response */
-		uint8_t ack = retval & 0xff;
-		swd_queue_ap_abort(dap, &ack);
-		return retval;
-	}
-
-	return retval;
+	return check_sync(dap);
 }
 
-static int (swd_queue_ap_write)(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
 		uint32_t data)
 {
-	/* REVISIT status return ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
-	int retval;
 
-	retval = swd_finish_read(dap);
-	if (retval != ERROR_OK)
-		return retval;
+	struct adiv5_dap *dap = ap->dap;
 
-	retval = swd_queue_ap_bankselect(dap, reg);
+	int retval = swd_check_reconnect(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = swd->write_reg(swd_cmd(false,  true, reg), data);
-
-	if (retval != ERROR_OK) {
-		/* fault response */
-		uint8_t ack = retval & 0xff;
-		swd_queue_ap_abort(dap, &ack);
-	}
+	swd_finish_read(dap);
+	swd_queue_ap_bankselect(ap, reg);
+	swd->write_reg(swd_cmd(false,  true, reg), data, ap->memaccess_tck);
 
-	return retval;
+	return check_sync(dap);
 }
 
 /** Executes all queued DAP operations. */
 static int swd_run(struct adiv5_dap *dap)
 {
-	/* for now the SWD interface hard-wires a zero-size queue.  */
-
-	int retval = swd_finish_read(dap);
-
-	/* FIXME but we still need to check and scrub
-	 * any hardware errors ...
-	 */
-	return retval;
+	swd_finish_read(dap);
+	return swd_run_inner(dap);
 }
 
 const struct dap_ops swd_dap_ops = {
-	.is_swd = true,
-
 	.queue_dp_read = swd_queue_dp_read,
 	.queue_dp_write = swd_queue_dp_write,
 	.queue_ap_read = swd_queue_ap_read,
@@ -281,6 +319,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"
@@ -408,7 +451,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;
@@ -431,36 +474,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 */
-
-	uint8_t ack;
-
-	status = swd_queue_dp_read(dap, DP_IDCODE, &idcode);
-
-	if (status == ERROR_OK)
-		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
-
-	/* force clear all sticky faults */
-	swd_queue_ap_abort(dap, &ack);
-
-	/* this is a workaround to get polling working */
-	jtag_add_reset(0, 0);
-
-	return status;
+	return swd_connect(dap);
 }
 
 static struct transport swd_transport = {