[PATCH 3/3] spl: fit: nand: fix fit loading in case of bad blocks

[Dario Binacchi]

The offset at which the image to be loaded from NAND is located is
retrieved from the itb header. The presence of bad blocks in the area
of the NAND where the itb image is located could invalidate the offset
which must therefore be adjusted taking into account the state of the
sectors concerned.

Signed-off-by: Dario Binacchi <dariobin at libero.it>
---

 common/spl/spl_fit.c                    |  7 ++++---
 common/spl/spl_nand.c                   |  7 +++++++
 drivers/mtd/nand/raw/nand_spl_loaders.c | 28 +++++++++++++++++++++++++
 include/nand.h                          |  1 +
 include/spl.h                           |  4 ++++
 5 files changed, 44 insertions(+), 3 deletions(-)

diff --git a/common/spl/spl_fit.c b/common/spl/spl_fit.c
index cc518383e8..70e8fa95c9 100644
--- a/common/spl/spl_fit.c
+++ b/common/spl/spl_fit.c
@@ -284,10 +284,11 @@ static int spl_load_fit_image(struct spl_load_info *info, ulong sector,
 
 		overhead = get_aligned_image_overhead(info, offset);
 		nr_sectors = get_aligned_image_size(info, length, offset);
+		offset = get_aligned_image_offset(info, offset);
+		offset = info->adjust_offset(info, sector, offset);
 
-		if (info->read(info,
-			       sector + get_aligned_image_offset(info, offset),
-			       nr_sectors, (void *)load_ptr) != nr_sectors)
+		if (info->read(info, sector + offset, nr_sectors,
+			       (void *)load_ptr) != nr_sectors)
 			return -EIO;
 
 		debug("External data: dst=%lx, offset=%x, size=%lx\n",
diff --git a/common/spl/spl_nand.c b/common/spl/spl_nand.c
index 5f8a111a2f..1aa82d7298 100644
--- a/common/spl/spl_nand.c
+++ b/common/spl/spl_nand.c
@@ -36,6 +36,12 @@ static int spl_nand_load_image(struct spl_image_info *spl_image,
 }
 #else
 
+static ulong spl_nand_adjust_offset(struct spl_load_info *load, ulong sector,
+				    ulong offset)
+{
+	return nand_spl_adjust_offset(sector, offset);
+}
+
 static ulong spl_nand_fit_read(struct spl_load_info *load, ulong offs,
 			       ulong size, void *dst)
 {
@@ -67,6 +73,7 @@ static int spl_nand_load_element(struct spl_image_info *spl_image,
 		load.filename = NULL;
 		load.bl_len = 1;
 		load.read = spl_nand_fit_read;
+		load.adjust_offset = spl_nand_adjust_offset;
 		return spl_load_simple_fit(spl_image, &load, offset, header);
 	} else if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER)) {
 		struct spl_load_info load;
diff --git a/drivers/mtd/nand/raw/nand_spl_loaders.c b/drivers/mtd/nand/raw/nand_spl_loaders.c
index 177c12b581..4befc75c04 100644
--- a/drivers/mtd/nand/raw/nand_spl_loaders.c
+++ b/drivers/mtd/nand/raw/nand_spl_loaders.c
@@ -41,6 +41,34 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
 	return 0;
 }
 
+/**
+ * nand_spl_adjust_offset - Adjust offset from a starting sector
+ * @sector:	Address of the sector
+ * @offs:	Offset starting from @sector
+ *
+ * If one or more bad blocks are in the address space between @sector
+ * and @sector + @offs, @offs is increased by the NAND block size for
+ * each bad block found.
+ */
+u32 nand_spl_adjust_offset(u32 sector, u32 offs)
+{
+	unsigned int block, lastblock;
+
+	block = sector / CONFIG_SYS_NAND_BLOCK_SIZE;
+	lastblock = (sector + offs) / CONFIG_SYS_NAND_BLOCK_SIZE;
+
+	while (block <= lastblock) {
+		if (nand_is_bad_block(block)) {
+			offs += CONFIG_SYS_NAND_BLOCK_SIZE;
+			lastblock++;
+		}
+
+		block++;
+	}
+
+	return offs;
+}
+
 #ifdef CONFIG_SPL_UBI
 /*
  * Temporary storage for non NAND page aligned and non NAND page sized
diff --git a/include/nand.h b/include/nand.h
index 93cbe1e25d..80dd6469bc 100644
--- a/include/nand.h
+++ b/include/nand.h
@@ -120,6 +120,7 @@ int nand_unlock(struct mtd_info *mtd, loff_t start, size_t length,
 		int allexcept);
 int nand_get_lock_status(struct mtd_info *mtd, loff_t offset);
 
+u32 nand_spl_adjust_offset(u32 sector, u32 offs);
 int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst);
 int nand_spl_read_block(int block, int offset, int len, void *dst);
 void nand_deselect(void);
diff --git a/include/spl.h b/include/spl.h
index e2fbd6d765..bb31a20595 100644
--- a/include/spl.h
+++ b/include/spl.h
@@ -159,6 +159,8 @@ struct spl_image_info {
  * @bl_len: Block length for reading in bytes
  * @filename: Name of the fit image file.
  * @read: Function to call to read from the device
+ * @adjust_offset: Function to call to adjust offset used to read from the
+ * device
  */
 struct spl_load_info {
 	void *dev;
@@ -167,6 +169,8 @@ struct spl_load_info {
 	const char *filename;
 	ulong (*read)(struct spl_load_info *load, ulong sector, ulong count,
 		      void *buf);
+	ulong (*adjust_offset)(struct spl_load_info *load, ulong sector,
+			       ulong offset);
 };
 
 /*
-- 
2.17.1