mtd: Align erase_op to eraseblock boundaries

Our erase command used to align to eraseblocks when necessary.
This worked well until recently when the m25p80, mtd_dataflash
and cfi flash were added / converted to mtd. This patch aligns
the input to the erase fileoperation to eraseblock boundaries.
Also tested with non uniform flashes with multiple eraseregions.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 48 insertions, 0 deletions

diff --git a/drivers/mtd/core.c b/drivers/mtd/core.c
index f3580981f0..fe9ec1d1cd 100644
--- a/drivers/mtd/core.c
+++ b/drivers/mtd/core.c
@@ -98,12 +98,60 @@ static ssize_t mtd_op_write(struct cdev* cdev, const void *buf, size_t _count,
 	return ret ? ret : _count;
 }
 
+static struct mtd_erase_region_info *mtd_find_erase_region(struct mtd_info *mtd, loff_t offset)
+{
+	int i;
+
+	for (i = 0; i < mtd->numeraseregions; i++) {
+		struct mtd_erase_region_info *e = &mtd->eraseregions[i];
+		if (offset > e->offset + e->erasesize * e->numblocks)
+			continue;
+		return e;
+	}
+
+	return NULL;
+}
+
+static int mtd_erase_align(struct mtd_info *mtd, size_t *count, loff_t *offset)
+{
+	struct mtd_erase_region_info *e;
+	loff_t ofs;
+
+	if (mtd->numeraseregions == 0) {
+		ofs = *offset & ~(mtd->erasesize - 1);
+		*count += (*offset - ofs);
+		*count = ALIGN(*count, mtd->erasesize);
+		*offset = ofs;
+		return 0;
+	}
+
+	e = mtd_find_erase_region(mtd, *offset);
+	if (!e)
+		return -EINVAL;
+
+	ofs = *offset & ~(e->erasesize - 1);
+	*count += (*offset - ofs);
+
+	e = mtd_find_erase_region(mtd, *offset + *count);
+	if (!e)
+		return -EINVAL;
+
+	*count = ALIGN(*count, e->erasesize);
+	*offset = ofs;
+
+	return 0;
+}
+
 static int mtd_op_erase(struct cdev *cdev, size_t count, loff_t offset)
 {
 	struct mtd_info *mtd = cdev->priv;
 	struct erase_info erase;
 	int ret;
 
+	ret = mtd_erase_align(mtd, &count, &offset);
+	if (ret)
+		return ret;
+
 	memset(&erase, 0, sizeof(erase));
 	erase.mtd = mtd;
 	erase.addr = offset;