adi_v5_swd: Read RDBUFF once after a sequence of AP reads

Increases performance by a factor of two for long reads.

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

3 files changed, 34 insertions, 3 deletions

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index c9c3ae5..c78aef2 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -58,6 +58,17 @@
 /* YUK! - but this is currently a global.... */
 extern struct jtag_interface *jtag_interface;
 
+static int 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);
+		dap->last_read = NULL;
+	}
+	return retval;
+}
+
 static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data);
 
@@ -129,6 +140,10 @@ static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg,
 	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;
@@ -169,12 +184,14 @@ static int (swd_queue_ap_read)(struct adiv5_dap *dap, unsigned reg,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = swd->read_reg(swd_cmd(true,  true, reg), data);
+	retval = swd->read_reg(swd_cmd(true,  true, reg), dap->last_read);
+	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;
@@ -186,8 +203,13 @@ static int (swd_queue_ap_write)(struct adiv5_dap *dap, unsigned reg,
 	/* REVISIT status return ... */
 	const struct swd_driver *swd = jtag_interface->swd;
 	assert(swd);
+	int retval;
 
-	int retval = swd_queue_ap_bankselect(dap, reg);
+	retval = swd_finish_read(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = swd_queue_ap_bankselect(dap, reg);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -207,10 +229,12 @@ 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 ERROR_OK;
+	return retval;
 }
 
 const struct dap_ops swd_dap_ops = {
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 2e56396..a18101d 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -661,6 +661,7 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 	 */
 	dap->ap_current = !0;
 	dap_ap_select(dap, 0);
+	dap->last_read = NULL;
 
 	/* DP initialization */
 
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index d132c57..c51e8f8 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -188,6 +188,12 @@ struct adiv5_dap {
 	uint8_t  ack;
 
 	/**
+	 * Holds the pointer to the destination word for the last queued read,
+	 * for use with posted AP read sequence optimization.
+	 */
+	uint32_t *last_read;
+
+	/**
 	 * Configures how many extra tck clocks are added after starting a
 	 * MEM-AP access before we try to read its status (and/or result).
 	 */