From: =?UTF-8?q?Rafa=C5=82=20Mi=C5=82ecki?= <zajec5@gmail.com>
Date: Sat, 2 Jan 2016 01:04:52 +0100
Subject: [PATCH] mtd: bcm47xxpart: check for bad blocks when calculating
 offsets
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Rafał Miłecki <zajec5@gmail.com>
---
 drivers/mtd/bcm47xxpart.c | 50 +++++++++++++++++++++++++++++++++++++----------
 1 file changed, 40 insertions(+), 10 deletions(-)

--- a/drivers/mtd/bcm47xxpart.c
+++ b/drivers/mtd/bcm47xxpart.c
@@ -61,6 +61,34 @@ static void bcm47xxpart_add_part(struct
 	part->mask_flags = mask_flags;
 }
 
+/*
+ * Calculate real end offset (address) for a given amount of data. It checks
+ * all blocks skipping bad ones.
+ */
+static size_t bcm47xxpart_real_offset(struct mtd_info *master, size_t offset,
+				      size_t bytes)
+{
+	size_t real_offset = offset;
+
+	if (mtd_block_isbad(master, real_offset))
+		pr_warn("Base offset shouldn't be at bad block");
+
+	while (bytes >= master->erasesize) {
+		bytes -= master->erasesize;
+		real_offset += master->erasesize;
+		while (mtd_block_isbad(master, real_offset)) {
+			real_offset += master->erasesize;
+
+			if (real_offset >= master->size)
+				return real_offset - master->erasesize;
+		}
+	}
+
+	real_offset += bytes;
+
+	return real_offset;
+}
+
 static const char *bcm47xxpart_trx_data_part_name(struct mtd_info *master,
 						  size_t offset)
 {
@@ -182,6 +210,8 @@ static int bcm47xxpart_parse(struct mtd_
 
 		/* TRX */
 		if (buf[0x000 / 4] == TRX_MAGIC) {
+			uint32_t tmp;
+
 			if (BCM47XXPART_MAX_PARTS - curr_part < 4) {
 				pr_warn("Not enough partitions left to register trx, scanning stopped!\n");
 				break;
@@ -196,18 +226,18 @@ static int bcm47xxpart_parse(struct mtd_
 			i = 0;
 			/* We have LZMA loader if offset[2] points to sth */
 			if (trx->offset[2]) {
+				tmp = bcm47xxpart_real_offset(master, offset,
+							      trx->offset[i]);
 				bcm47xxpart_add_part(&parts[curr_part++],
-						     "loader",
-						     offset + trx->offset[i],
-						     0);
+						     "loader", tmp, 0);
 				i++;
 			}
 
 			if (trx->offset[i]) {
+				tmp = bcm47xxpart_real_offset(master, offset,
+							      trx->offset[i]);
 				bcm47xxpart_add_part(&parts[curr_part++],
-						     "linux",
-						     offset + trx->offset[i],
-						     0);
+						     "linux", tmp, 0);
 				i++;
 			}
 
@@ -219,11 +249,11 @@ static int bcm47xxpart_parse(struct mtd_
 			if (trx->offset[i]) {
 				const char *name;
 
-				name = bcm47xxpart_trx_data_part_name(master, offset + trx->offset[i]);
+				tmp = bcm47xxpart_real_offset(master, offset,
+							      trx->offset[i]);
+				name = bcm47xxpart_trx_data_part_name(master, tmp);
 				bcm47xxpart_add_part(&parts[curr_part++],
-						     name,
-						     offset + trx->offset[i],
-						     0);
+						     name, tmp, 0);
 				i++;
 			}