From 89e675cba383c6140a4715c8897d8d16237a262c Mon Sep 17 00:00:00 2001
From: Simon Qian <simonqian.openocd@gmail.com>
Date: Sat, 17 Mar 2012 13:16:36 +0800
Subject: [PATCH 2/2] add mem_ap_read_buf_u32_swd

---
 src/target/arm_adi_v5.c |  118 ++++++++++++++++++++++++++++++++++++++++++++++-
 1 files changed, 116 insertions(+), 2 deletions(-)

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index b5c7692..f6832a1 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -562,14 +562,14 @@ extern int adi_jtag_dp_scan(struct adiv5_dap *dap,
 		uint8_t *outvalue, uint8_t *invalue, uint8_t *ack);
 
 /**
- * Synchronously read a block of 32-bit words into a buffer
+ * Synchronously read a block of 32-bit words into a buffer via JTAG
  * @param dap The DAP connected to the MEM-AP.
  * @param buffer where the words will be stored (in host byte order).
  * @param count How many words to read.
  * @param address Memory address from which to read words; all the
  *	words must be readable by the currently selected MEM-AP.
  */
-int mem_ap_read_buf_u32(struct adiv5_dap *dap, uint8_t *buffer,
+static int mem_ap_read_buf_u32_jtag(struct adiv5_dap *dap, uint8_t *buffer,
 		int count, uint32_t address)
 {
 	int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
@@ -665,6 +665,120 @@ int mem_ap_read_buf_u32(struct adiv5_dap *dap, uint8_t *buffer,
 	return retval;
 }
 
+/**
+ * Synchronously read a block of 32-bit words into a buffer via SWD
+ * @param dap The DAP connected to the MEM-AP.
+ * @param buffer where the words will be stored (in host byte order).
+ * @param count How many words to read.
+ * @param address Memory address from which to read words; all the
+ *	words must be readable by the currently selected MEM-AP.
+ */
+static int mem_ap_read_buf_u32_swd(struct adiv5_dap *dap, uint8_t *buffer,
+		int count, uint32_t address)
+{
+	int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
+	uint32_t adr = address;
+	uint8_t *pBuffer = buffer;
+
+	count >>= 2;
+	wcount = count;
+
+	while (wcount > 0) {
+		/* Adjust to read blocks within boundaries aligned to the
+		 * TAR autoincrement size (at least 2^10).  Autoincrement
+		 * mode avoids an extra per-word roundtrip to update TAR.
+		 */
+		blocksize = max_tar_block_size(dap->tar_autoincr_block,
+				address);
+		if (wcount < blocksize)
+			blocksize = wcount;
+
+		/* handle unaligned data at 4k boundary */
+		if (blocksize == 0)
+			blocksize = 1;
+
+		retval = dap_setup_accessport(dap, CSW_32BIT | CSW_ADDRINC_SINGLE,
+				address);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* FIXME remove these three calls to adi_jtag_dp_scan(),
+		 * so this routine becomes transport-neutral.  Be careful
+		 * not to cause performance problems with JTAG; would it
+		 * suffice to loop over dap_queue_ap_read(), or would that
+		 * be slower when JTAG is the chosen transport?
+		 */
+
+		/* Scan out first read */
+		retval = dap_queue_ap_read(dap, AP_REG_DRW, NULL);
+		if (retval != ERROR_OK)
+			return retval;
+		for (readcount = 0; readcount < blocksize - 1; readcount++) {
+			/* Scan out next read; scan in posted value for the
+			 * previous one.  Assumes read is acked "OK/FAULT",
+			 * and CTRL_STAT says that meant "OK".
+			 */
+			retval = dap_queue_ap_read(dap, AP_REG_DRW,
+					(uint32_t *)(buffer + 4 * readcount));
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		/* Scan in last posted value; RDBUFF has no other effect,
+		 * assuming ack is OK/FAULT and CTRL_STAT says "OK".
+		 */
+		retval = dap_queue_dp_read(dap, DP_RDBUFF,
+				(uint32_t *)(buffer + 4 * readcount));
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = dap_run(dap);
+		if (retval != ERROR_OK) {
+			errorcount++;
+			if (errorcount <= 1) {
+				/* try again */
+				continue;
+			}
+			LOG_WARNING("Block read error address 0x%" PRIx32, address);
+			return retval;
+		}
+		wcount = wcount - blocksize;
+		address += 4 * blocksize;
+		buffer += 4 * blocksize;
+	}
+
+	/* if we have an unaligned access - reorder data */
+	if (adr & 0x3u) {
+		for (readcount = 0; readcount < count; readcount++) {
+			int i;
+			uint32_t data;
+			memcpy(&data, pBuffer, sizeof(uint32_t));
+
+			for (i = 0; i < 4; i++) {
+				*((uint8_t *)pBuffer) =
+						(data >> 8 * (adr & 0x3));
+				pBuffer++;
+				adr++;
+			}
+		}
+	}
+
+	return retval;
+}
+
+int mem_ap_read_buf_u32(struct adiv5_dap *dap, uint8_t *buffer,
+		int count, uint32_t address)
+{
+	if (transport_is_swd()) {
+		return mem_ap_read_buf_u32_swd(dap, buffer, count, address);
+    } else if (transport_is_jtag()) {
+		return mem_ap_read_buf_u32_jtag(dap, buffer, count, address);
+    } else {
+		LOG_ERROR("unsupported transport!");
+		return ERROR_FAIL;
+	}
+}
+
 static int mem_ap_read_buf_packed_u16(struct adiv5_dap *dap,
 		uint8_t *buffer, int count, uint32_t address)
 {
-- 
1.7.3.1.msysgit.0