From 36f749fed92597e0469909e5399146359e5fdfdf Mon Sep 17 00:00:00 2001
From: Gareth McMullin <gareth@blacksphere.co.nz>
Date: Thu, 26 Mar 2015 22:18:18 -0700
Subject: [PATCH] Fix flash buffer alignment in target layer.

---
 src/include/target.h |  2 ++
 src/target.c         | 10 +++++++++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/include/target.h b/src/include/target.h
index bcabc23e..f49bc58b 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -131,6 +131,8 @@ struct target_flash {
 	flash_done_func done;
 	target *t;
 	struct target_flash *next;
+	int align;
+	uint8_t erased;
 };
 
 struct target_s {
diff --git a/src/target.c b/src/target.c
index 6b05deb6..3ab59bc1 100644
--- a/src/target.c
+++ b/src/target.c
@@ -189,7 +189,15 @@ int target_flash_write(target *t,
 	while (len) {
 		struct target_flash *f = flash_for_addr(t, dest);
 		size_t tmplen = MIN(len, f->length - (dest % f->length));
-		ret |= f->write(f, dest, src, tmplen);
+		if (f->align > 1) {
+			uint32_t offset = dest % f->align;
+			uint8_t data[ALIGN(offset + len, f->align)];
+			memset(data, f->erased, sizeof(data));
+			memcpy((uint8_t *)data + offset, src, len);
+			ret |= f->write(f, dest - offset, data, sizeof(data));
+		} else {
+			ret |= f->write(f, dest, src, tmplen);
+		}
 		src += tmplen;
 		len -= tmplen;
 	}