target: Rewrite read/write buffer default implementations

Rewrite the target_*_buffer_default to generate as large accesses as
possible while maintaining natural alignment.

These versions are easy to extend to generate 8-byte accesses to support
64-bit targets, although it requires some conformity from all target
implementations (i.e. they need to refuse unsupported access sizes with
some defined error code, so we can try again with a smaller one).

Change-Id: I00ddcbb1d2fd33f9f8b99cb448cc93505a2421fc
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/1221
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

1 files changed, 51 insertions, 88 deletions

diff --git a/src/target/target.c b/src/target/target.c
index 6aee098..d34e144 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -60,9 +60,9 @@
 #define DEFAULT_HALT_TIMEOUT 5000
 
 static int target_read_buffer_default(struct target *target, uint32_t address,
-		uint32_t size, uint8_t *buffer);
+		uint32_t count, uint8_t *buffer);
 static int target_write_buffer_default(struct target *target, uint32_t address,
-		uint32_t size, const uint8_t *buffer);
+		uint32_t count, const uint8_t *buffer);
 static int target_array2mem(Jim_Interp *interp, struct target *target,
 		int argc, Jim_Obj * const *argv);
 static int target_mem2array(Jim_Interp *interp, struct target *target,
@@ -1766,50 +1766,37 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
 	return target->type->write_buffer(target, address, size, buffer);
 }
 
-static int target_write_buffer_default(struct target *target, uint32_t address, uint32_t size, const uint8_t *buffer)
+static int target_write_buffer_default(struct target *target, uint32_t address, uint32_t count, const uint8_t *buffer)
 {
-	int retval = ERROR_OK;
-
-	if (((address % 2) == 0) && (size == 2))
-		return target_write_memory(target, address, 2, 1, buffer);
-
-	/* handle unaligned head bytes */
-	if (address % 4) {
-		uint32_t unaligned = 4 - (address % 4);
-
-		if (unaligned > size)
-			unaligned = size;
-
-		retval = target_write_memory(target, address, 1, unaligned, buffer);
-		if (retval != ERROR_OK)
-			return retval;
-
-		buffer += unaligned;
-		address += unaligned;
-		size -= unaligned;
-	}
-
-	/* handle aligned words */
-	if (size >= 4) {
-		int aligned = size - (size % 4);
-
-		retval = target_write_memory(target, address, 4, aligned / 4, buffer);
-		if (retval != ERROR_OK)
-			return retval;
+	uint32_t size;
 
-		buffer += aligned;
-		address += aligned;
-		size -= aligned;
+	/* Align up to maximum 4 bytes. The loop condition makes sure the next pass
+	 * will have something to do with the size we leave to it. */
+	for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) {
+		if (address & size) {
+			int retval = target_write_memory(target, address, size, 1, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+			address += size;
+			count -= size;
+			buffer += size;
+		}
 	}
 
-	/* handle tail writes of less than 4 bytes */
-	if (size > 0) {
-		retval = target_write_memory(target, address, 1, size, buffer);
-		if (retval != ERROR_OK)
-			return retval;
+	/* Write the data with as large access size as possible. */
+	for (; size > 0; size /= 2) {
+		uint32_t aligned = count - count % size;
+		if (aligned > 0) {
+			int retval = target_write_memory(target, address, size, aligned / size, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+			address += aligned;
+			count -= aligned;
+			buffer += aligned;
+		}
 	}
 
-	return retval;
+	return ERROR_OK;
 }
 
 /* Single aligned words are guaranteed to use 16 or 32 bit access
@@ -1840,58 +1827,34 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
 	return target->type->read_buffer(target, address, size, buffer);
 }
 
-static int target_read_buffer_default(struct target *target, uint32_t address, uint32_t size, uint8_t *buffer)
+static int target_read_buffer_default(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
 {
-	int retval = ERROR_OK;
+	uint32_t size;
 
-	if (((address % 2) == 0) && (size == 2))
-		return target_read_memory(target, address, 2, 1, buffer);
-
-	/* handle unaligned head bytes */
-	if (address % 4) {
-		uint32_t unaligned = 4 - (address % 4);
-
-		if (unaligned > size)
-			unaligned = size;
-
-		retval = target_read_memory(target, address, 1, unaligned, buffer);
-		if (retval != ERROR_OK)
-			return retval;
-
-		buffer += unaligned;
-		address += unaligned;
-		size -= unaligned;
-	}
-
-	/* handle aligned words */
-	if (size >= 4) {
-		int aligned = size - (size % 4);
-
-		retval = target_read_memory(target, address, 4, aligned / 4, buffer);
-		if (retval != ERROR_OK)
-			return retval;
-
-		buffer += aligned;
-		address += aligned;
-		size -= aligned;
+	/* Align up to maximum 4 bytes. The loop condition makes sure the next pass
+	 * will have something to do with the size we leave to it. */
+	for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) {
+		if (address & size) {
+			int retval = target_read_memory(target, address, size, 1, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+			address += size;
+			count -= size;
+			buffer += size;
+		}
 	}
 
-	/*prevent byte access when possible (avoid AHB access limitations in some cases)*/
-	if (size	>= 2) {
-		int aligned = size - (size % 2);
-		retval = target_read_memory(target, address, 2, aligned / 2, buffer);
-		if (retval != ERROR_OK)
-			return retval;
-
-		buffer += aligned;
-		address += aligned;
-		size -= aligned;
-	}
-	/* handle tail writes of less than 4 bytes */
-	if (size > 0) {
-		retval = target_read_memory(target, address, 1, size, buffer);
-		if (retval != ERROR_OK)
-			return retval;
+	/* Read the data with as large access size as possible. */
+	for (; size > 0; size /= 2) {
+		uint32_t aligned = count - count % size;
+		if (aligned > 0) {
+			int retval = target_read_memory(target, address, size, aligned / size, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+			address += aligned;
+			count -= aligned;
+			buffer += aligned;
+		}
 	}
 
 	return ERROR_OK;