From 6a2c499d7aa68fef379b41d8eabb20333c28d99c Mon Sep 17 00:00:00 2001
From: "S.J.R. van Schaik" <stephan@whiteboxsystems.nl>
Date: Fri, 19 May 2017 18:04:52 +0200
Subject: [PATCH] ftl: use byte-offset from VA in ftl_write()

---
 include/ftl.h    |  3 ++-
 source/ftl/dev.c |  5 +----
 source/ftl/ftl.c | 22 ++++++++++++++++++----
 3 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/include/ftl.h b/include/ftl.h
index c231790..bddc7cf 100644
--- a/include/ftl.h
+++ b/include/ftl.h
@@ -30,7 +30,8 @@ struct ftl_map {
 
 int ftl_init_map(struct ftl_map *map, struct flash_dev *dev);
 int ftl_resume_map(struct ftl_map *map);
-int ftl_write(struct ftl_map *map, uint32_t addr, const uint8_t *data);
+int ftl_write(struct ftl_map *map, uint32_t addr, const void *data,
+	size_t len);
 int ftl_read(struct ftl_map *map, void *data, size_t len, uint32_t va);
 int ftl_trim(struct ftl_map *map, uint32_t va);
 uint32_t ftl_get_size(const struct ftl_map *map);
diff --git a/source/ftl/dev.c b/source/ftl/dev.c
index 5d2923b..5b48ac7 100644
--- a/source/ftl/dev.c
+++ b/source/ftl/dev.c
@@ -53,10 +53,7 @@ static int ftl_flash_write(struct flash_dev *dev, uint32_t addr,
 {
 	struct ftl_map *map = dev->priv;
 
-	/* TODO: support len */
-	(void)len;
-
-	return ftl_write(map, addr, data);
+	return ftl_write(map, addr, data, len);
 }
 
 static int ftl_flash_erase(struct flash_dev *dev, uint32_t addr)
diff --git a/source/ftl/ftl.c b/source/ftl/ftl.c
index 8a57162..d81942b 100644
--- a/source/ftl/ftl.c
+++ b/source/ftl/ftl.c
@@ -30,27 +30,41 @@ int ftl_read(struct ftl_map *map, void *data, size_t len, uint32_t va)
 		return 0;
 
 	return flash_read(map->dev, (page << map->log2_page_size) + offset, data,
-		min(map->log2_page_size - offset, len));
+		min((1 << map->log2_page_size) - offset, len));
 }
 
-int ftl_write(struct ftl_map *map, uint32_t va, const uint8_t *data)
+int ftl_write(struct ftl_map *map, uint32_t va, const void *udata,
+	size_t len)
 {
+	uint8_t data[1 << map->log2_page_size];
 	struct ftl_page_desc page_desc;
 	int ret;
+	uint32_t mask, offset, page;
 
 	if (va >= ftl_get_capacity(map) &&
 	    !is_aligned(va, 1 << map->log2_page_size))
 		return -1;
 
-	if ((ret = trace_path(map, &page_desc, NULL, va)) < 0 &&
+	mask = ((1 << map->log2_page_size) - 1);
+	offset = va & mask;
+	va = va & ~mask;
+
+	if ((ret = trace_path(map, &page_desc, &page, va)) < 0 &&
 		ret != -ERR_NOT_FOUND)
 		return -1;
 
-	if (ret == -ERR_NOT_FOUND)
+	if (ret == -ERR_NOT_FOUND) {
+		memset(data, 0, 1 << map->log2_page_size);
 		++map->nused_pages;
+	} else if (flash_read(map->dev, page << map->log2_page_size, data,
+		1 << map->log2_page_size) < 0) {
+		return -1;
+	}
 
 	page_desc.nused_pages = map->nused_pages;
 
+	memcpy(data + offset, udata, min((1 << map->log2_page_size) - offset, len));
+
 	return write_upage(map, data, &page_desc);
 }