swd: Convert API to asynchronous

Change-Id: I859568dbb2ad4e92411980751c3f747bd70638b8
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/1959
Tested-by: jenkins
Reviewed-by: Andrey Yurovsky <yurovsky@gmail.com>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>

3 files changed, 102 insertions, 147 deletions

diff --git a/src/target/adi_v5_cmsis_dap.c b/src/target/adi_v5_cmsis_dap.c
index 9028c9d5..9c13e5a9 100644
--- a/src/target/adi_v5_cmsis_dap.c
+++ b/src/target/adi_v5_cmsis_dap.c
@@ -61,8 +61,9 @@ static int cmsis_dap_clear_sticky_errors(struct adiv5_dap *dap)
 	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(dap, (CMSIS_CMD_DP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(DP_ABORT)),
+			STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	return ERROR_OK;
 }
 
 static int cmsis_dap_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
@@ -72,21 +73,20 @@ static int cmsis_dap_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),
-			      DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	swd->write_reg(dap, (CMSIS_CMD_DP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(DP_ABORT)),
+			DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	return ERROR_OK;
 }
 
 static int cmsis_dap_queue_dp_read(struct adiv5_dap *dap, unsigned reg, uint32_t *data)
 {
 	LOG_DEBUG("reg = %d", reg);
 
-	int retval = jtag_interface->swd->read_reg(
-			(CMSIS_CMD_DP | CMSIS_CMD_READ | CMSIS_CMD_A32(reg)), data);
-
-	if (retval != ERROR_OK)
-		cmsis_dap_clear_sticky_errors(dap);
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
 
-	return retval;
+	swd->read_reg(dap, (CMSIS_CMD_DP | CMSIS_CMD_READ | CMSIS_CMD_A32(reg)), data);
+	return ERROR_OK;
 }
 
 static int (cmsis_dap_queue_dp_write)(struct adiv5_dap *dap, unsigned reg, uint32_t data)
@@ -100,13 +100,11 @@ static int (cmsis_dap_queue_dp_write)(struct adiv5_dap *dap, unsigned reg, uint3
 		data &= ~CORUNDETECT;
 	}
 
-	int retval = jtag_interface->swd->write_reg(
-			(CMSIS_CMD_DP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(reg)), data);
-
-	if (retval != ERROR_OK)
-		cmsis_dap_clear_sticky_errors(dap);
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
 
-	return retval;
+	swd->write_reg(dap, (CMSIS_CMD_DP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(reg)), data);
+	return ERROR_OK;
 }
 
 /** Select the AP register bank matching bits 7:4 of reg. */
@@ -120,58 +118,47 @@ static int cmsis_dap_ap_q_bankselect(struct adiv5_dap *dap, unsigned reg)
 	dap->ap_bank_value = select_ap_bank;
 	select_ap_bank |= dap->ap_current;
 
-	return cmsis_dap_queue_dp_write(dap, DP_SELECT, select_ap_bank);
+	cmsis_dap_queue_dp_write(dap, DP_SELECT, select_ap_bank);
+	return ERROR_OK;
 }
 
 static int (cmsis_dap_queue_ap_read)(struct adiv5_dap *dap, unsigned reg, uint32_t *data)
 {
-	int retval = cmsis_dap_ap_q_bankselect(dap, reg);
-	if (retval != ERROR_OK)
-		return retval;
+	cmsis_dap_ap_q_bankselect(dap, reg);
 
 	LOG_DEBUG("reg = %d", reg);
 
-	retval = jtag_interface->swd->read_reg(
-			(CMSIS_CMD_AP | CMSIS_CMD_READ | CMSIS_CMD_A32(reg)), data);
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
 
-	if (retval != ERROR_OK)
-		cmsis_dap_clear_sticky_errors(dap);
+	swd->read_reg(dap, (CMSIS_CMD_AP | CMSIS_CMD_READ | CMSIS_CMD_A32(reg)), data);
 
-	return retval;
+	return ERROR_OK;
 }
 
 static int (cmsis_dap_queue_ap_write)(struct adiv5_dap *dap, unsigned reg, uint32_t data)
 {
-
-
 	/* TODO: CSW_DBGSWENABLE (bit31) causes issues for some targets
 	 * disable until we find out why */
 	if (reg == AP_REG_CSW)
 		data &= ~CSW_DBGSWENABLE;
 
-	int retval = cmsis_dap_ap_q_bankselect(dap, reg);
-	if (retval != ERROR_OK)
-		return retval;
+	cmsis_dap_ap_q_bankselect(dap, reg);
 
 	LOG_DEBUG("reg = %d, data = 0x%08" PRIx32, reg, data);
 
-	retval = jtag_interface->swd->write_reg(
-			(CMSIS_CMD_AP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(reg)), data);
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
 
-	if (retval != ERROR_OK)
-		cmsis_dap_clear_sticky_errors(dap);
+	swd->write_reg(dap, (CMSIS_CMD_AP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(reg)), data);
 
-	return retval;
+	return ERROR_OK;
 }
 
 /** Executes all queued DAP operations. */
 static int cmsis_dap_run(struct adiv5_dap *dap)
 {
 	LOG_DEBUG(" ");
-	/* FIXME: for now the CMSIS-DAP interface hard-wires a zero-size queue. */
-	int ret;
-	uint32_t ctrlstat;
-
 	/*
 	  Some debug dongles do more than asked for(e.g. EDBG from
 	  Atmel) behind the scene and issuing an AP write
@@ -182,23 +169,22 @@ static int cmsis_dap_run(struct adiv5_dap *dap)
 	  differently and not guarantee to be report those failures
 	  via status byte of the return USB packet from CMSIS-DAP, so
 	  we need to check CTRL/STAT and if that happens to clear it.
+	  Note that once the CMSIS-DAP SWD implementation starts queueing
+	  transfers this will cause loss of the transfers after the
+	  failed one. At least a warning is printed.
 	*/
-	ret = cmsis_dap_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
-	if (ret != ERROR_OK) {
-		LOG_ERROR("Failed to read CTRL/STAT register");
-		return ret;
-	}
+	uint32_t ctrlstat;
+	cmsis_dap_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
 
-	if (ctrlstat & SSTICKYERR) {
-		LOG_WARNING("SSTICKYERR was set, clearing it");
-		ret = cmsis_dap_clear_sticky_errors(dap);
-		if (ret != ERROR_OK) {
-			LOG_ERROR("Failed to clear sticky errors");
-			return ret;
-		}
-	}
+	int retval = jtag_interface->swd->run(dap);
+
+	if (retval == ERROR_OK && (ctrlstat & SSTICKYERR))
+		LOG_WARNING("Adapter returned success despite SSTICKYERR being set.");
 
-	return ret;
+	if (retval != ERROR_OK || (ctrlstat & SSTICKYERR))
+		cmsis_dap_clear_sticky_errors(dap);
+
+	return retval;
 }
 
 const struct dap_ops cmsis_dap_ops = {
@@ -240,7 +226,7 @@ static const struct command_registration cmsis_dap_handlers[] = {
 
 static int cmsis_dap_select(struct command_context *ctx)
 {
-	LOG_DEBUG(" ");
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_select");
 
 	int retval = register_commands(ctx, NULL, cmsis_dap_handlers);
 
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 104832e6..31c219e5 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -57,179 +57,146 @@
 
 /* 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(dap, swd_cmd(true, false, DP_RDBUFF), dap->last_read);
 		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_clear_sticky_errors(struct adiv5_dap *dap)
+static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
 
-	return swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+	swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
 		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
 }
 
+static int swd_run_inner(struct adiv5_dap *dap)
+{
+	const struct swd_driver *swd = jtag_interface->swd;
+
+	int retval = swd->run(dap);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		swd_clear_sticky_errors(dap);
+	}
+
+	return retval;
+}
+
+static inline int check_sync(struct adiv5_dap *dap)
+{
+	return do_sync ? swd_run_inner(dap) : 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),
+	swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
 		DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+	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;
+		return;
 
 	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);
+	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);
 
-	retval = swd_queue_dp_bankselect(dap, reg);
-	if (retval != ERROR_OK)
-		return retval;
+	swd_queue_dp_bankselect(dap, reg);
+	swd->read_reg(dap, swd_cmd(true,  false, reg), data);
 
-	retval = swd->read_reg(swd_cmd(true,  false, reg), data);
-
-	if (retval != ERROR_OK) {
-		/* fault response */
-		swd_clear_sticky_errors(dap);
-	}
-
-	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);
-	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(dap, swd_cmd(false,  false, reg), data);
 
-	if (retval != ERROR_OK) {
-		/* fault response */
-		swd_clear_sticky_errors(dap);
-	}
-
-	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_dap *dap, unsigned reg)
 {
 	uint32_t select_ap_bank = reg & 0x000000F0;
 
 	if (select_ap_bank == dap->ap_bank_value)
-		return ERROR_OK;
+		return;
 
 	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);
+	swd_queue_dp_write(dap, DP_SELECT, select_ap_bank);
 }
 
-static int (swd_queue_ap_read)(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_ap_read(struct adiv5_dap *dap, 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);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = swd->read_reg(swd_cmd(true,  true, reg), dap->last_read);
+	swd_queue_ap_bankselect(dap, reg);
+	swd->read_reg(dap, swd_cmd(true,  true, reg), dap->last_read);
 	dap->last_read = data;
 
-	if (retval != ERROR_OK) {
-		/* fault response */
-		swd_clear_sticky_errors(dap);
-		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_dap *dap, 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;
-
-	retval = swd_queue_ap_bankselect(dap, reg);
-	if (retval != ERROR_OK)
-		return retval;
 
-	retval = swd->write_reg(swd_cmd(false,  true, reg), data);
+	swd_finish_read(dap);
+	swd_queue_ap_bankselect(dap, reg);
+	swd->write_reg(dap, swd_cmd(false,  true, reg), data);
 
-	if (retval != ERROR_OK) {
-		/* fault response */
-		swd_clear_sticky_errors(dap);
-	}
-
-	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 = {
@@ -452,14 +419,16 @@ static int swd_init(struct command_context *ctx)
 
  /* Note, debugport_init() does setup too */
 
-	status = swd_queue_dp_read(dap, DP_IDCODE, &idcode);
-
-	if (status == ERROR_OK)
-		LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
+	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);
 
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 09b6b0d7..002e60f5 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -40,9 +40,9 @@
 #define JTAG_DP_APACC		0xB
 
 /* three-bit ACK values for SWD access (sent LSB first) */
-#define SWD_ACK_OK		0x4
-#define SWD_ACK_WAIT		0x2
-#define SWD_ACK_FAULT		0x1
+#define SWD_ACK_OK    0x1
+#define SWD_ACK_WAIT  0x2
+#define SWD_ACK_FAULT 0x4
 
 #define DPAP_WRITE		0
 #define DPAP_READ		1