adi_v5_swd: Improve SWD support

Fix bug in parity calculation macro.

Cache and update the selected DP bank when necessary.

Add aborts when the Ack code signals a failure (we should really only
clear the sticky bits, but this will do for now).

Change-Id: I38a4da136ba1d9e989b33c1875a80c0b1b2be874
Signed-off-by: Fatih Aşıcı <fatih.asici@gmail.com>
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/1950
Tested-by: jenkins

src/jtag/swd.h

@@ -33,7 +33,7 @@
 /* followed by TRN, 3-bits of ACK, TRN */
 
 /* pbit16 holds precomputed parity bits for each nibble */
-#define pbit(parity, nibble) (parity << nibble)
+#define pbit(parity, nibble) ((parity) << (nibble))
 
 static const uint16_t pbit16 =
 	pbit(0, 0) | pbit(1, 1) | pbit(1, 2) | pbit(0, 3)
@@ -41,7 +41,7 @@ static const uint16_t pbit16 =
 	| pbit(1, 8) | pbit(0, 9) | pbit(0, 0xa) | pbit(1, 0xb)
 	| pbit(0, 0xc) | pbit(1, 0xd) | pbit(1, 0xe) | pbit(0, 0xf);
 
-#define nibble_parity(nibble) (pbit16 & pbit(1, nibble))
+#define nibble_parity(nibble) (pbit16 & pbit(1, (nibble)))
 
 /**
  * Construct a "cmd" byte, in lSB bit order, which swd_driver.read_reg()

src/target/adi_v5_swd.c

@@ -58,14 +58,56 @@
 /* YUK! - but this is currently a global.... */
 extern struct jtag_interface *jtag_interface;
 
+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)
+{
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
+	return swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+}
+
+/** Select the DP register bank matching bits 7:4 of reg. */
+static int 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;
+
+	if (select_dp_bank == dap->dp_bank_value)
+		return ERROR_OK;
+
+	dap->dp_bank_value = select_dp_bank;
+	select_dp_bank |= dap->ap_current | dap->ap_bank_value;
+
+	return swd_queue_dp_write(dap, DP_SELECT, select_dp_bank);
+}
+
 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);
 
-	return swd->read_reg(swd_cmd(true,  false, reg), data);
+	retval = swd_queue_dp_bankselect(dap, reg);
+	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);
+	}
+
+	return retval;
 }
 
 static int swd_queue_idcode_read(struct adiv5_dap *dap,
@@ -82,39 +124,82 @@ static int swd_queue_idcode_read(struct adiv5_dap *dap,
 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);
 
-	return swd->write_reg(swd_cmd(false,  false, reg), data);
+	retval = swd_queue_dp_bankselect(dap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = swd->write_reg(swd_cmd(false,  false, reg), data);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		swd_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
 }
 
+/** Select the AP register bank matching bits 7:4 of reg. */
+static int swd_queue_ap_bankselect(struct adiv5_dap *dap, unsigned reg)
+{
+	uint32_t select_ap_bank = reg & 0x000000F0;
+
+	if (select_ap_bank == dap->ap_bank_value)
+		return ERROR_OK;
+
+	dap->ap_bank_value = select_ap_bank;
+	select_ap_bank |= dap->ap_current | dap->dp_bank_value;
+
+	return swd_queue_dp_write(dap, DP_SELECT, select_ap_bank);
+}
 
 static int (swd_queue_ap_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_queue_ap_bankselect(dap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = swd->read_reg(swd_cmd(true,  true, reg), data);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		swd_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
 }
 
 static int (swd_queue_ap_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_queue_ap_bankselect(dap, reg);
-}
+	if (retval != ERROR_OK)
+		return retval;
 
-static int (swd_queue_ap_abort)(struct adiv5_dap *dap, uint8_t *ack)
+	retval = swd->write_reg(swd_cmd(false,  true, reg), data);
-{
+
-	return ERROR_FAIL;
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		swd_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
 }
 
 /** Executes all queued DAP operations. */
@@ -356,8 +441,13 @@ static int swd_init(struct command_context *ctx)
 	if (status == ERROR_OK)
 		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
 
-	return status;
+	/* 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;
 }
 
 static struct transport swd_transport = {

src/target/arm_adi_v5.c

@@ -664,6 +664,8 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 
 	/* DP initialization */
 
+	dap->dp_bank_value = 0;
+
 	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
 		return retval;

src/target/arm_adi_v5.h

@@ -47,18 +47,20 @@
 #define DPAP_WRITE		0
 #define DPAP_READ		1
 
+#define BANK_REG(bank, reg)	(((bank) << 4) | (reg))
+
 /* A[3:0] for DP registers; A[1:0] are always zero.
  * - JTAG accesses all of these via JTAG_DP_DPACC, except for
  *   IDCODE (JTAG_DP_IDCODE) and ABORT (JTAG_DP_ABORT).
  * - SWD accesses these directly, sometimes needing SELECT.CTRLSEL
  */
-#define DP_IDCODE		0		/* SWD: read */
+#define DP_IDCODE		BANK_REG(0x0, 0x0)	/* SWD: read */
-#define DP_ABORT		0		/* SWD: write */
+#define DP_ABORT		BANK_REG(0x0, 0x0)	/* SWD: write */
-#define DP_CTRL_STAT		0x4		/* r/w */
+#define DP_CTRL_STAT		BANK_REG(0x0, 0x4)	/* r/w */
-#define DP_WCR			0x4		/* SWD: r/w (mux CTRLSEL) */
+#define DP_RESEND		BANK_REG(0x0, 0x8)	/* SWD: read */
-#define DP_RESEND		0x8		/* SWD: read */
+#define DP_SELECT		BANK_REG(0x0, 0x8)	/* JTAG: r/w; SWD: write */
-#define DP_SELECT		0x8		/* JTAG: r/w; SWD: write */
+#define DP_RDBUFF		BANK_REG(0x0, 0xC)	/* read-only */
-#define DP_RDBUFF		0xC		/* read-only */
+#define DP_WCR			BANK_REG(0x1, 0x4)	/* SWD: r/w */
 
 #define WCR_TO_TRN(wcr) ((uint32_t)(1 + (3 & ((wcr)) >> 8)))	/* 1..4 clocks */
 #define WCR_TO_PRESCALE(wcr) ((uint32_t)(7 & ((wcr))))		/* impl defined */
@@ -161,6 +163,13 @@ struct adiv5_dap {
 	 */
 	uint32_t ap_bank_value;
 
+	/**
+	 * Cache for DP_SELECT bits identifying the current four-word DP
+	 * register bank.  This caches DP register addresss bits 7:4; JTAG
+	 * and SWD access primitves pass address bits 3:2; bits 1:0 are zero.
+	 */
+	uint32_t dp_bank_value;
+
 	/**
 	 * Cache for (MEM-AP) AP_REG_CSW register value.  This is written to
 	 * configure an access mode, such as autoincrementing AP_REG_TAR during