X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_swd.c;h=d1a4870334ad9770e74323be5cfebd1f9781ca3d;hp=6ff858a85d627b1334d553c03a7ce69c53adf039;hb=150b7d26f213398d717bf46744811b48834a3744;hpb=4dc8cd201c667bac72bc083ef1fa1b285eb093fc

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 6ff858a85d..d1a4870334 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -57,81 +57,218 @@
 
 /* YUK! - but this is currently a global.... */
 extern struct jtag_interface *jtag_interface;
+static bool do_sync;
 
+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(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)
+		uint32_t *data);
+
+static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 {
-	/* REVISIT status return vs ack ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	return swd->read_reg(swd_cmd(true,  false, reg), data);
+	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 dpidr;
+	int status;
+
+	/* FIXME validate transport config ... is the
+	 * configured DAP present (check IDCODE)?
+	 * Is *only* one DAP configured?
+	 *
+	 * MUST READ DPIDR
+	 */
+
+	/* Note, debugport_init() does setup too */
+	jtag_interface->swd->switch_seq(JTAG_TO_SWD);
+
+	/* Clear link state, including the SELECT cache. */
+	dap->do_reconnect = false;
+	dap->select = DP_SELECT_INVALID;
+
+	swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+
+	/* force clear all sticky faults */
+	swd_clear_sticky_errors(dap);
+
+	status = swd_run_inner(dap);
+
+	if (status == ERROR_OK) {
+		LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+		dap->do_reconnect = false;
+	} else
+		dap->do_reconnect = true;
+
+	return status;
 }
 
-static int swd_queue_idcode_read(struct adiv5_dap *dap,
-		uint8_t *ack, uint32_t *data)
+static inline int check_sync(struct adiv5_dap *dap)
 {
-	int status = swd_queue_dp_read(dap, DP_IDCODE, data);
-	if (status < 0)
-		return status;
-	*ack = status;
-	/* ?? */
+	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_dp_write)(struct adiv5_dap *dap, unsigned reg,
-		uint32_t data)
+static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 {
-	/* REVISIT status return vs ack ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	return swd->write_reg(swd_cmd(false,  false, reg), data);
+	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 void swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
+{
+	/* Only register address 4 is banked. */
+	if ((reg & 0xf) != 4)
+		return;
+
+	uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
+	uint32_t sel = select_dp_bank
+			| (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
+
+	if (sel == dap->select)
+		return;
+
+	dap->select = sel;
 
-static int (swd_queue_ap_read)(struct adiv5_dap *dap, unsigned reg,
+	swd_queue_dp_write(dap, DP_SELECT, sel);
+}
+
+static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 		uint32_t *data)
 {
-	/* REVISIT  APSEL ... */
-	/* REVISIT status return ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	return swd->read_reg(swd_cmd(true,  true, reg), data);
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	swd_queue_dp_bankselect(dap, reg);
+	swd->read_reg(swd_cmd(true,  false, reg), data, 0);
+
+	return check_sync(dap);
 }
 
-static int (swd_queue_ap_write)(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
-	/* REVISIT  APSEL ... */
-	/* REVISIT status return ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	return swd->write_reg(swd_cmd(false,  true, reg), data);
+	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(swd_cmd(false,  false, reg), data, 0);
+
+	return check_sync(dap);
+}
+
+/** Select the AP register bank matching bits 7:4 of reg. */
+static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
+{
+	struct adiv5_dap *dap = ap->dap;
+	uint32_t sel = ((uint32_t)ap->ap_num << 24)
+			| (reg & 0x000000F0)
+			| (dap->select & DP_SELECT_DPBANK);
+
+	if (sel == dap->select)
+		return;
+
+	dap->select = sel;
+
+	swd_queue_dp_write(dap, DP_SELECT, sel);
+}
+
+static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
+		uint32_t *data)
+{
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
+	struct adiv5_dap *dap = ap->dap;
+
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	swd_queue_ap_bankselect(ap, reg);
+	swd->read_reg(swd_cmd(true,  true, reg), dap->last_read, ap->memaccess_tck);
+	dap->last_read = data;
+
+	return check_sync(dap);
 }
 
-static int (swd_queue_ap_abort)(struct adiv5_dap *dap, uint8_t *ack)
+static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
+		uint32_t data)
 {
-	return ERROR_FAIL;
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
+	struct adiv5_dap *dap = ap->dap;
+
+	int retval = swd_check_reconnect(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	swd_finish_read(dap);
+	swd_queue_ap_bankselect(ap, reg);
+	swd->write_reg(swd_cmd(false,  true, reg), data, ap->memaccess_tck);
+
+	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.  */
-
-	/* FIXME but we still need to check and scrub
-	 * any hardware errors ...
-	 */
-	return ERROR_OK;
+	swd_finish_read(dap);
+	return swd_run_inner(dap);
 }
 
 const struct dap_ops swd_dap_ops = {
-	.is_swd = true,
-
-	.queue_idcode_read = swd_queue_idcode_read,
 	.queue_dp_read = swd_queue_dp_read,
 	.queue_dp_write = swd_queue_dp_write,
 	.queue_ap_read = swd_queue_ap_read,
@@ -183,6 +320,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"
@@ -200,61 +342,6 @@ int dap_to_swd(struct target *target)
 	return retval;
 }
 
-COMMAND_HANDLER(handle_swd_wcr)
-{
-	int retval;
-	struct target *target = get_current_target(CMD_CTX);
-	struct arm *arm = target_to_arm(target);
-	struct adiv5_dap *dap = arm->dap;
-	uint32_t wcr;
-	unsigned trn, scale = 0;
-
-	switch (CMD_ARGC) {
-	/* no-args: just dump state */
-	case 0:
-		/*retval = swd_queue_dp_read(dap, DP_WCR, &wcr); */
-		retval = dap_queue_dp_read(dap, DP_WCR, &wcr);
-		if (retval == ERROR_OK)
-			dap->ops->run(dap);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("can't read WCR?");
-			return retval;
-		}
-
-		command_print(CMD_CTX,
-			"turnaround=%" PRIu32 ", prescale=%" PRIu32,
-			WCR_TO_TRN(wcr),
-			WCR_TO_PRESCALE(wcr));
-	return ERROR_OK;
-
-	case 2:		/* TRN and prescale */
-		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], scale);
-		if (scale > 7) {
-			LOG_ERROR("prescale %d is too big", scale);
-			return ERROR_FAIL;
-		}
-		/* FALL THROUGH */
-
-	case 1:		/* TRN only */
-		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], trn);
-		if (trn < 1 || trn > 4) {
-			LOG_ERROR("turnaround %d is invalid", trn);
-			return ERROR_FAIL;
-		}
-
-		wcr = ((trn - 1) << 8) | scale;
-		/* FIXME
-		 * write WCR ...
-		 * then, re-init adapter with new TRN
-		 */
-		LOG_ERROR("can't yet modify WCR");
-		return ERROR_FAIL;
-
-	default:	/* too many arguments */
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-}
-
 static const struct command_registration swd_commands[] = {
 	{
 		/*
@@ -269,15 +356,6 @@ static const struct command_registration swd_commands[] = {
 		.mode = COMMAND_CONFIG,
 		.help = "declare a new SWD DAP"
 	},
-	{
-		.name = "wcr",
-		.handler = handle_swd_wcr,
-		.mode = COMMAND_ANY,
-		.help = "display or update DAP's WCR register",
-		.usage = "turnaround (1..4), prescale (0..7)",
-	},
-
-	/* REVISIT -- add a command for SWV trace on/off */
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -310,7 +388,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;
@@ -333,31 +411,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_idcode_read(dap, &ack, &idcode);
-
-	if (status == ERROR_OK)
-		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
-
-	return status;
-
+	return swd_connect(dap);
 }
 
 static struct transport swd_transport = {