From 7202db586038843d1d4f01403ce6e1285ac3520c Mon Sep 17 00:00:00 2001
From: Gareth McMullin <gareth@blacksphere.co.nz>
Date: Tue, 24 Mar 2015 20:45:59 -0700
Subject: [PATCH] Add new functions to wrap flash driver erase/write/done
 operations.

---
 src/include/target.h | 12 ++++------
 src/target.c         | 54 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 58 insertions(+), 8 deletions(-)

diff --git a/src/include/target.h b/src/include/target.h
index 7085ff2..bcabc23 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -95,14 +95,10 @@ target *target_attach(target *t, target_destroy_callback destroy_cb);
 
 
 /* Flash memory access functions */
-#define target_flash_erase(target, addr, len)	\
-	(target)->flash_erase((target), (addr), (len))
-
-#define target_flash_write(target, dest, src, len)	\
-	(target)->flash_write((target), (dest), (src), (len))
-
-#define target_flash_done(target)	\
-	((target)->flash_done ? (target)->flash_done(target) : 0)
+int target_flash_erase(target *t, uint32_t addr, size_t len);
+int target_flash_write(target *t,
+                       uint32_t dest, const void *src, size_t len);
+int target_flash_done(target *t);
 
 /* Host I/O */
 #define target_hostio_reply(target, recode, errcode)	\
diff --git a/src/target.c b/src/target.c
index 86d88ed..6b05deb 100644
--- a/src/target.c
+++ b/src/target.c
@@ -154,3 +154,57 @@ const char *target_mem_map(target *t)
 	return t->dyn_mem_map;
 }
 
+static struct target_flash *flash_for_addr(target *t, uint32_t addr)
+{
+	for (struct target_flash *f = t->flash; f; f = f->next)
+		if ((f->start <= addr) &&
+		    (addr < (f->start + f->length)))
+			return f;
+	return NULL;
+}
+
+int target_flash_erase(target *t, uint32_t addr, size_t len)
+{
+	if (t->flash_write)
+		return t->flash_erase(t, addr, len);
+
+	int ret = 0;
+	while (len) {
+		struct target_flash *f = flash_for_addr(t, addr);
+		size_t tmplen = MIN(len, f->length - (addr % f->length));
+		ret |= f->erase(f, addr, tmplen);
+		addr += tmplen;
+		len -= tmplen;
+	}
+	return ret;
+}
+
+int target_flash_write(target *t,
+                       uint32_t dest, const void *src, size_t len)
+{
+	if (t->flash_write)
+		return t->flash_write(t, dest, src, len);
+
+	int ret = 0;
+	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);
+		src += tmplen;
+		len -= tmplen;
+	}
+	return ret;
+}
+
+int target_flash_done(target *t)
+{
+	for (struct target_flash *f = t->flash; f; f = f->next) {
+		if (f->done) {
+			int tmp = f->done(f);
+			if (tmp)
+				return tmp;
+		}
+	}
+	return 0;
+}
+