[PATCH 3/7] spl: mtd: Add bad block handling for SPL image loading

[Anurag Dutta]

From: Santhosh Kumar K <s-k6 at ti.com>

Implement bad block skipping functionality in SPL MTD loader to ensure
reliable boot from flash devices with bad blocks. Without this, the
SPL would fail to load images if bad blocks were encountered.

Signed-off-by: Santhosh Kumar K <s-k6 at ti.com>
Signed-off-by: Anurag Dutta <a-dutta at ti.com>
---
 common/spl/spl_mtd.c | 78 ++++++++++++++++++++++++++++++++++++++------
 1 file changed, 68 insertions(+), 10 deletions(-)

diff --git a/common/spl/spl_mtd.c b/common/spl/spl_mtd.c
index 95c0c8ce8cd..d635aa2b476 100644
--- a/common/spl/spl_mtd.c
+++ b/common/spl/spl_mtd.c
@@ -17,15 +17,64 @@ uint32_t __weak spl_mtd_get_uboot_offs(void)
 	return CONFIG_SYS_MTD_U_BOOT_OFFS;
 }
 
+static ulong spl_mtd_read_skip_bad(struct mtd_info *mtd, loff_t offs,
+				   size_t size, void *dst)
+{
+	size_t remaining = size;
+	size_t ret_len;
+	loff_t current_offs = offs;
+	loff_t block_aligned_offs, next_block;
+	u_char *buf = (u_char *)dst;
+	int err;
+
+	while (remaining > 0) {
+		size_t read_size;
+		size_t block_remaining;
+
+		block_aligned_offs = current_offs & ~(mtd->erasesize - 1);
+
+		if (mtd_block_isbad(mtd, block_aligned_offs)) {
+			debug("SPL: Skipping bad block at 0x%llx, jumping to 0x%llx\n",
+			      block_aligned_offs, next_block);
+
+			next_block = block_aligned_offs + mtd->erasesize;
+			current_offs = next_block;
+			continue;
+		}
+
+		block_remaining = mtd->erasesize -
+				  (current_offs & (mtd->erasesize - 1));
+		read_size = remaining < block_remaining ? remaining : block_remaining;
+
+		err = mtd_read(mtd, current_offs, read_size, &ret_len, buf);
+		if (err) {
+			printf("SPL: Read error at offset 0x%llx: %d\n",
+			       current_offs, err);
+			return size - remaining;
+		}
+
+		buf += ret_len;
+		current_offs += ret_len;
+		remaining -= ret_len;
+
+		if (current_offs >= mtd->size) {
+			printf("SPL: Reached end of device, read %zu/%zu bytes\n",
+			       size - remaining, size);
+			break;
+		}
+	}
+
+	return size - remaining;
+}
+
 static ulong spl_mtd_fit_read(struct spl_load_info *load, ulong offs,
 			      ulong size, void *dst)
 {
 	struct mtd_info *mtd = load->priv;
-	int err;
 	size_t ret_len;
 
-	err = mtd_read(mtd, offs, size, &ret_len, dst);
-	if (!err)
+	ret_len = spl_mtd_read_skip_bad(mtd, offs, size, dst);
+	if (ret_len > 0)
 		return ret_len;
 
 	return 0;
@@ -52,17 +101,19 @@ struct mtd_info *spl_prepare_mtd(uint boot_device)
 int spl_mtd_load(struct spl_image_info *spl_image,
 		 struct mtd_info *mtd, struct spl_boot_device *bootdev)
 {
-	int err;
+	int err = 0;
 	struct legacy_img_hdr *header;
 	__maybe_unused struct spl_load_info load;
 	size_t ret_len;
 
 	header = spl_get_load_buffer(0, sizeof(*header));
-
-	err = mtd_read(mtd, spl_mtd_get_uboot_offs(), sizeof(*header),
-		       &ret_len, (void *)header);
-	if (err)
+	ret_len = spl_mtd_read_skip_bad(mtd, spl_mtd_get_uboot_offs(),
+					sizeof(*header), (void *)header);
+	if (ret_len != sizeof(*header)) {
+		printf("SPL: Failed to read image header\n");
+		err = -EIO;
 		goto out_err;
+	}
 
 	if (IS_ENABLED(CONFIG_SPL_LOAD_FIT) &&
 	    image_get_magic(header) == FDT_MAGIC) {
@@ -82,8 +133,15 @@ int spl_mtd_load(struct spl_image_info *spl_image,
 		err = spl_parse_image_header(spl_image, bootdev, header);
 		if (err)
 			goto out_err;
-		err = mtd_read(mtd, spl_mtd_get_uboot_offs(), spl_image->size,
-			       &ret_len, (void *)(ulong)spl_image->load_addr);
+
+		ret_len = spl_mtd_read_skip_bad(mtd, spl_mtd_get_uboot_offs(),
+						spl_image->size,
+						(void *)(ulong)spl_image->load_addr);
+		if (ret_len != spl_image->size) {
+			printf("SPL: Failed to read full image: %zu/%u bytes\n",
+			       ret_len, spl_image->size);
+			err = -EIO;
+		}
 	}
 
 out_err:
-- 
2.34.1