kernel: mtdsplit: bcm_wfi: support Sercomm bootflags

Sercomm uses 2 bootflag partitions and boots the firmware with the highest
bootflag. Support splitting the firmware partition while keeping support for
unsplitted layout.

Signed-off-by: Álvaro Fernández Rojas <noltari@gmail.com>

target/linux/generic/files/drivers/mtd/mtdsplit/mtdsplit_bcm_wfi.c

@@ -24,6 +24,8 @@
 
 #include "mtdsplit.h"
 
+#define char_to_num(c)		((c >= '0' && c <= '9') ? (c - '0') : (0))
+
 #define BCM_WFI_PARTS		3
 
 #define CFERAM_NAME		"cferam"
@@ -35,6 +37,11 @@
 
 #define UBI_MAGIC		0x55424923
 
+#define SERCOMM_MAGIC_PFX	"eRcOmM."
+#define SERCOMM_MAGIC_PFX_LEN	(sizeof(SERCOMM_MAGIC_PFX) - 1)
+#define SERCOMM_MAGIC		"eRcOmM.000"
+#define SERCOMM_MAGIC_LEN	(sizeof(SERCOMM_MAGIC) - 1)
+
 static u32 jffs2_dirent_crc(struct jffs2_raw_dirent *node)
 {
 	return crc32(0, node, sizeof(struct jffs2_raw_dirent) - 8);
@@ -220,9 +227,150 @@ static struct mtd_part_parser mtdsplit_bcm_wfi_parser = {
 	.type = MTD_PARSER_TYPE_FIRMWARE,
 };
 
+static int sercomm_bootflag_value(struct mtd_info *mtd, uint8_t *buf)
+{
+	size_t retlen;
+	loff_t offs;
+	int rc;
+
+	for (offs = 0; offs < mtd->size; offs += mtd->erasesize) {
+		rc = mtd_read(mtd, offs, SERCOMM_MAGIC_LEN, &retlen, buf);
+		if (rc || retlen != SERCOMM_MAGIC_LEN)
+			continue;
+
+		if (memcmp(buf, SERCOMM_MAGIC_PFX, SERCOMM_MAGIC_PFX_LEN))
+			continue;
+
+		rc = char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 0]) * 100;
+		rc += char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 1]) * 10;
+		rc += char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 2]) * 1;
+
+		return rc;
+	}
+
+	return -ENOENT;
+}
+
+static int mtdsplit_parse_ser_wfi(struct mtd_info *master,
+				  const struct mtd_partition **pparts,
+				  struct mtd_part_parser_data *data)
+{
+	struct mtd_info *mtd_bf1, *mtd_bf2;
+	struct erase_info bf_erase;
+	struct mtd_partition *parts;
+	loff_t kernel_off, rootfs_off, img_size;
+	loff_t img2_off, img2_size = 0;
+	unsigned int num_parts = BCM_WFI_PARTS, cur_part = 0;
+	uint8_t *buf;
+	int bf1, bf2;
+	int ret;
+
+	mtd_bf1 = get_mtd_device_nm("bootflag1");
+	if (IS_ERR(mtd_bf1))
+		return -ENOENT;
+
+	mtd_bf2 = get_mtd_device_nm("bootflag2");
+	if (IS_ERR(mtd_bf2))
+		return -ENOENT;
+
+	buf = kzalloc(master->erasesize, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	bf1 = sercomm_bootflag_value(mtd_bf1, buf);
+	if (bf1 >= 0)
+		printk("sercomm: bootflag1=%d\n", bf1);
+
+	bf2 = sercomm_bootflag_value(mtd_bf2, buf);
+	if (bf2 >= 0)
+		printk("sercomm: bootflag2=%d\n", bf2);
+
+	if (bf1 == bf2 && bf2 >= 0) {
+		bf2 = -ENOENT;
+		bf_erase.addr = 0;
+		bf_erase.len = mtd_bf2->size;
+		mtd_erase(mtd_bf2, &bf_erase);
+	}
+
+	if (bf1 >= bf2) {
+		kernel_off = 0;
+		if (bf2 >= 0) {
+			img_size = master->size / 2;
+
+			img2_off = img_size;
+			img2_size = master->size - img2_off;
+			num_parts++;
+		} else {
+			img_size = master->size;
+		}
+	} else {
+		kernel_off = master->size / 2;
+		img_size = master->size;
+
+		img2_off = 0;
+		img2_size = kernel_off;
+		num_parts++;
+	}
+
+	ret = jffs2_find_file(master, buf, KERNEL_NAME, KERNEL_NAME_LEN,
+			      &kernel_off, img_size);
+	kfree(buf);
+	if (ret)
+		return ret;
+
+	rootfs_off = kernel_off + master->erasesize;
+	ret = ubifs_find(master, &rootfs_off, img_size);
+	if (ret)
+		return ret;
+
+	parts = kzalloc(num_parts * sizeof(*parts), GFP_KERNEL);
+	if (!parts)
+		return -ENOMEM;
+
+	parts[cur_part].name = "firmware";
+	parts[cur_part].offset = kernel_off;
+	parts[cur_part].size = img_size - kernel_off;
+	cur_part++;
+
+	parts[cur_part].name = KERNEL_PART_NAME;
+	parts[cur_part].offset = kernel_off;
+	parts[cur_part].size = rootfs_off - kernel_off;
+	cur_part++;
+
+	parts[cur_part].name = UBI_PART_NAME;
+	parts[cur_part].offset = rootfs_off;
+	parts[cur_part].size = img_size - rootfs_off;
+	cur_part++;
+
+	if (img2_size) {
+		parts[cur_part].name = "img2";
+		parts[cur_part].offset = img2_off;
+		parts[cur_part].size = img2_size;
+		cur_part++;
+	}
+
+	*pparts = parts;
+
+	return num_parts;
+}
+
+static const struct of_device_id mtdsplit_ser_wfi_of_match[] = {
+	{ .compatible = "sercomm,wfi" },
+	{ },
+};
+
+static struct mtd_part_parser mtdsplit_ser_wfi_parser = {
+	.owner = THIS_MODULE,
+	.name = "ser-wfi-fw",
+	.of_match_table = mtdsplit_ser_wfi_of_match,
+	.parse_fn = mtdsplit_parse_ser_wfi,
+	.type = MTD_PARSER_TYPE_FIRMWARE,
+};
+
 static int __init mtdsplit_bcm_wfi_init(void)
 {
 	register_mtd_parser(&mtdsplit_bcm_wfi_parser);
+	register_mtd_parser(&mtdsplit_ser_wfi_parser);
 
 	return 0;
 }