1 files changed, 49 insertions, 41 deletions

diff --git a/drivers/mtd/nand/raw/mxs_nand_spl.c b/drivers/mtd/nand/raw/mxs_nand_spl.c
index 59a67ee..2bfb181 100644
--- a/drivers/mtd/nand/raw/mxs_nand_spl.c
+++ b/drivers/mtd/nand/raw/mxs_nand_spl.c
@@ -218,14 +218,14 @@ void nand_init(void)
 	mxs_nand_setup_ecc(mtd);
 }
 
-int nand_spl_load_image(uint32_t offs, unsigned int size, void *buf)
+int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
 {
-	struct nand_chip *chip;
-	unsigned int page;
+	unsigned int sz;
+	unsigned int block, lastblock;
+	unsigned int page, page_offset;
 	unsigned int nand_page_per_block;
-	unsigned int sz = 0;
+	struct nand_chip *chip;
 	u8 *page_buf = NULL;
-	u32 page_off;
 
 	chip = mtd_to_nand(mtd);
 	if (!chip->numchips)
@@ -235,47 +235,42 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *buf)
 	if (!page_buf)
 		return -ENOMEM;
 
-	page = offs >> chip->page_shift;
-	page_off = offs & (mtd->writesize - 1);
+	/* offs has to be aligned to a page address! */
+	block = offs / mtd->erasesize;
+	lastblock = (offs + size - 1) / mtd->erasesize;
+	page = (offs % mtd->erasesize) / mtd->writesize;
+	page_offset = offs % mtd->writesize;
 	nand_page_per_block = mtd->erasesize / mtd->writesize;
 
-	debug("%s offset:0x%08x len:%d page:%x\n", __func__, offs, size, page);
-
-	while (size) {
-		if (mxs_read_page_ecc(mtd, page_buf, page) < 0)
-			return -1;
-
-		if (size > (mtd->writesize - page_off))
-			sz = (mtd->writesize - page_off);
-		else
-			sz = size;
-
-		memcpy(buf, page_buf + page_off, sz);
-
-		offs += mtd->writesize;
-		page++;
-		buf += (mtd->writesize - page_off);
-		page_off = 0;
-		size -= sz;
-
-		/*
-		 * Check if we have crossed a block boundary, and if so
-		 * check for bad block.
-		 */
-		if (!(page % nand_page_per_block)) {
-			/*
-			 * Yes, new block. See if this block is good. If not,
-			 * loop until we find a good block.
-			 */
-			while (is_badblock(mtd, offs, 1)) {
-				page = page + nand_page_per_block;
-				/* Check i we've reached the end of flash. */
-				if (page >= mtd->size >> chip->page_shift) {
+	while (block <= lastblock && size > 0) {
+		if (!is_badblock(mtd, mtd->erasesize * block, 1)) {
+			/* Skip bad blocks */
+			while (page < nand_page_per_block) {
+				int curr_page = nand_page_per_block * block + page;
+
+				if (mxs_read_page_ecc(mtd, page_buf, curr_page) < 0) {
 					free(page_buf);
-					return -ENOMEM;
+					return -EIO;
 				}
+
+				if (size > (mtd->writesize - page_offset))
+					sz = (mtd->writesize - page_offset);
+				else
+					sz = size;
+
+				memcpy(dst, page_buf + page_offset, sz);
+				dst += sz;
+				size -= sz;
+				page_offset = 0;
+				page++;
 			}
+
+			page = 0;
+		} else {
+			lastblock++;
 		}
+
+		block++;
 	}
 
 	free(page_buf);
@@ -294,6 +289,19 @@ void nand_deselect(void)
 
 u32 nand_spl_adjust_offset(u32 sector, u32 offs)
 {
-	/* Handle the offset adjust in nand_spl_load_image,*/
+	unsigned int block, lastblock;
+
+	block = sector / mtd->erasesize;
+	lastblock = (sector + offs) / mtd->erasesize;
+
+	while (block <= lastblock) {
+		if (is_badblock(mtd, block * mtd->erasesize, 1)) {
+			offs += mtd->erasesize;
+			lastblock++;
+		}
+
+		block++;
+	}
+
 	return offs;
 }