kernel: mtdsplit: add BCM WFI support

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

6 files changed, 212 insertions, 0 deletions

diff --git a/target/linux/generic/config-4.14 b/target/linux/generic/config-4.14
index a592f2563b..5c61df2cc3 100644
--- a/target/linux/generic/config-4.14
+++ b/target/linux/generic/config-4.14
@@ -2886,6 +2886,7 @@ CONFIG_MTD_ROOTFS_ROOT_DEV=y
 # CONFIG_MTD_SPI_NOR_USE_4K_SECTORS is not set
 CONFIG_MTD_SPI_NOR_USE_4K_SECTORS_LIMIT=4096
 CONFIG_MTD_SPLIT=y
+# CONFIG_MTD_SPLIT_BCM_WFI_FW is not set
 # CONFIG_MTD_SPLIT_BRNIMAGE_FW is not set
 # CONFIG_MTD_SPLIT_EVA_FW is not set
 # CONFIG_MTD_SPLIT_FIRMWARE is not set
diff --git a/target/linux/generic/config-4.19 b/target/linux/generic/config-4.19
index ae1be9e075..d3de674623 100644
--- a/target/linux/generic/config-4.19
+++ b/target/linux/generic/config-4.19
@@ -3034,6 +3034,7 @@ CONFIG_MTD_ROOTFS_ROOT_DEV=y
 # CONFIG_MTD_SPI_NOR_USE_4K_SECTORS is not set
 CONFIG_MTD_SPI_NOR_USE_4K_SECTORS_LIMIT=4096
 CONFIG_MTD_SPLIT=y
+# CONFIG_MTD_SPLIT_BCM_WFI_FW is not set
 # CONFIG_MTD_SPLIT_BRNIMAGE_FW is not set
 # CONFIG_MTD_SPLIT_EVA_FW is not set
 # CONFIG_MTD_SPLIT_FIRMWARE is not set
diff --git a/target/linux/generic/config-5.4 b/target/linux/generic/config-5.4
index cdee4c973b..325caec40e 100644
--- a/target/linux/generic/config-5.4
+++ b/target/linux/generic/config-5.4
@@ -3231,6 +3231,7 @@ CONFIG_MTD_ROOTFS_ROOT_DEV=y
 # CONFIG_MTD_SPI_NOR_USE_4K_SECTORS is not set
 CONFIG_MTD_SPI_NOR_USE_4K_SECTORS_LIMIT=4096
 CONFIG_MTD_SPLIT=y
+# CONFIG_MTD_SPLIT_BCM_WFI_FW is not set
 # CONFIG_MTD_SPLIT_BRNIMAGE_FW is not set
 # CONFIG_MTD_SPLIT_EVA_FW is not set
 # CONFIG_MTD_SPLIT_FIRMWARE is not set
diff --git a/target/linux/generic/files/drivers/mtd/mtdsplit/Kconfig b/target/linux/generic/files/drivers/mtd/mtdsplit/Kconfig
index 81ece43db8..0447df585c 100644
--- a/target/linux/generic/files/drivers/mtd/mtdsplit/Kconfig
+++ b/target/linux/generic/files/drivers/mtd/mtdsplit/Kconfig
@@ -20,6 +20,11 @@ config MTD_SPLIT_SQUASHFS_ROOT
 
 comment "Firmware partition parsers"
 
+config MTD_SPLIT_BCM_WFI_FW
+	bool "Broadcom Whole Flash Image parser"
+	depends on MTD_SPLIT_SUPPORT
+	select MTD_SPLIT
+
 config MTD_SPLIT_SEAMA_FW
 	bool "Seama firmware parser"
 	depends on MTD_SPLIT_SUPPORT
diff --git a/target/linux/generic/files/drivers/mtd/mtdsplit/Makefile b/target/linux/generic/files/drivers/mtd/mtdsplit/Makefile
index 206e754a18..d3634e78db 100644
--- a/target/linux/generic/files/drivers/mtd/mtdsplit/Makefile
+++ b/target/linux/generic/files/drivers/mtd/mtdsplit/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_MTD_SPLIT)		+= mtdsplit.o
+obj-$(CONFIG_MTD_SPLIT_BCM_WFI_FW) += mtdsplit_bcm_wfi.o
 obj-$(CONFIG_MTD_SPLIT_SEAMA_FW) += mtdsplit_seama.o
 obj-$(CONFIG_MTD_SPLIT_SQUASHFS_ROOT) += mtdsplit_squashfs.o
 obj-$(CONFIG_MTD_SPLIT_UIMAGE_FW) += mtdsplit_uimage.o
diff --git a/target/linux/generic/files/drivers/mtd/mtdsplit/mtdsplit_bcm_wfi.c b/target/linux/generic/files/drivers/mtd/mtdsplit/mtdsplit_bcm_wfi.c
new file mode 100644
index 0000000000..9fc3c8ac50
--- /dev/null
+++ b/target/linux/generic/files/drivers/mtd/mtdsplit/mtdsplit_bcm_wfi.c
@@ -0,0 +1,203 @@
+/*
+ * MTD split for Broadcom Whole Flash Image
+ *
+ * Copyright (C) 2020 Álvaro Fernández Rojas <noltari@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ */
+
+#define je16_to_cpu(x) ((x).v16)
+#define je32_to_cpu(x) ((x).v32)
+
+#include <linux/crc32.h>
+#include <linux/init.h>
+#include <linux/jffs2.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/byteorder/generic.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+
+#include "mtdsplit.h"
+
+#define BCM_WFI_PARTS		4
+
+#define CFERAM_NAME		"cferam"
+#define CFERAM_NAME_LEN		(sizeof(CFERAM_NAME) - 1)
+#define KERNEL_NAME		"vmlinux.lz"
+#define KERNEL_NAME_LEN		(sizeof(KERNEL_NAME) - 1)
+#define OPENWRT_NAME		"1-openwrt"
+#define OPENWRT_NAME_LEN	(sizeof(OPENWRT_NAME) - 1)
+
+#define UBI_MAGIC		0x55424923
+
+static u32 jffs2_dirent_crc(struct jffs2_raw_dirent *node)
+{
+	return crc32(0, node, sizeof(struct jffs2_raw_dirent) - 8);
+}
+
+static bool jffs2_dirent_valid(struct jffs2_raw_dirent *node)
+{
+	return ((je16_to_cpu(node->magic) == JFFS2_MAGIC_BITMASK) &&
+		(je16_to_cpu(node->nodetype) == JFFS2_NODETYPE_DIRENT) &&
+		je32_to_cpu(node->ino) &&
+		je32_to_cpu(node->node_crc) == jffs2_dirent_crc(node));
+}
+
+static int jffs2_find_file(struct mtd_info *master, uint8_t *buf,
+			   const char *name, size_t name_len,
+			   loff_t *offs)
+{
+	struct jffs2_raw_dirent *node;
+	bool valid = false;
+	size_t retlen;
+	uint16_t magic;
+	int rc;
+
+	for (; *offs < master->size; *offs += master->erasesize) {
+		unsigned int block_offs = 0;
+
+		/* Skip CFE erased blocks */
+		rc = mtd_read(master, *offs, sizeof(magic), &retlen,
+			      (void *) &magic);
+		if (rc || retlen != sizeof(magic)) {
+			continue;
+		}
+
+		/* Skip blocks not starting with JFFS2 magic */
+		if (magic != JFFS2_MAGIC_BITMASK)
+			continue;
+
+		/* Read full block */
+		rc = mtd_read(master, *offs, master->erasesize, &retlen,
+			      (void *) buf);
+		if (rc)
+			return rc;
+		if (retlen != master->erasesize)
+			return -EINVAL;
+
+		while (block_offs < master->erasesize) {
+			node = (struct jffs2_raw_dirent *) &buf[block_offs];
+
+			if (!jffs2_dirent_valid(node)) {
+				block_offs += 4;
+				continue;
+			}
+
+			if (!memcmp(node->name, OPENWRT_NAME,
+				    OPENWRT_NAME_LEN))
+				valid = true;
+			else if (!memcmp(node->name, name, name_len))
+				return valid ? 0 : -EINVAL;
+
+			block_offs += je32_to_cpu(node->totlen);
+			block_offs = (block_offs + 0x3) & ~0x3;
+		}
+	}
+
+	return -ENOENT;
+}
+
+static int ubifs_find(struct mtd_info *master, loff_t *offs)
+{
+	uint32_t magic;
+	size_t retlen;
+	int rc;
+
+	for (; *offs < master->size; *offs += master->erasesize) {
+		rc = mtd_read(master, *offs, sizeof(magic), &retlen,
+			      (unsigned char *) &magic);
+		if (rc || retlen != sizeof(magic))
+			continue;
+
+		if (be32_to_cpu(magic) == UBI_MAGIC)
+			return 0;
+	}
+
+	return -ENOENT;
+}
+
+static int mtdsplit_parse_bcm_wfi(struct mtd_info *master,
+				  const struct mtd_partition **pparts,
+				  struct mtd_part_parser_data *data)
+{
+	struct mtd_partition *parts;
+	loff_t cfe_off, kernel_off, rootfs_off;
+	uint8_t *buf;
+	int ret;
+
+	buf = kzalloc(master->erasesize, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	cfe_off = 0;
+	ret = jffs2_find_file(master, buf, CFERAM_NAME, CFERAM_NAME_LEN,
+			      &cfe_off);
+	if (ret) {
+		kfree(buf);
+		return ret;
+	}
+
+	kernel_off = cfe_off + master->erasesize;
+	ret = jffs2_find_file(master, buf, KERNEL_NAME, KERNEL_NAME_LEN,
+			      &kernel_off);
+	kfree(buf);
+	if (ret)
+		return ret;
+
+	rootfs_off = kernel_off + master->erasesize;
+	ret = ubifs_find(master, &rootfs_off);
+	if (ret)
+		return ret;
+
+	parts = kzalloc(BCM_WFI_PARTS * sizeof(*parts), GFP_KERNEL);
+	if (!parts)
+		return -ENOMEM;
+
+	parts[0].name = "cferam";
+	parts[0].mask_flags = MTD_WRITEABLE;
+	parts[0].offset = 0;
+	parts[0].size = kernel_off;
+
+	parts[1].name = "firmware";
+	parts[1].offset = kernel_off;
+	parts[1].size = master->size - kernel_off;
+
+	parts[2].name = KERNEL_PART_NAME;
+	parts[2].offset = kernel_off;
+	parts[2].size = rootfs_off - kernel_off;
+
+	parts[3].name = UBI_PART_NAME;
+	parts[3].offset = rootfs_off;
+	parts[3].size = master->size - rootfs_off;
+
+	*pparts = parts;
+
+	return BCM_WFI_PARTS;
+}
+
+static const struct of_device_id mtdsplit_fit_of_match_table[] = {
+	{ .compatible = "brcm,wfi" },
+	{ },
+};
+
+static struct mtd_part_parser mtdsplit_bcm_wfi_parser = {
+	.owner = THIS_MODULE,
+	.name = "bcm-wfi-fw",
+	.of_match_table = mtdsplit_fit_of_match_table,
+	.parse_fn = mtdsplit_parse_bcm_wfi,
+	.type = MTD_PARSER_TYPE_FIRMWARE,
+};
+
+static int __init mtdsplit_bcm_wfi_init(void)
+{
+	register_mtd_parser(&mtdsplit_bcm_wfi_parser);
+
+	return 0;
+}
+
+module_init(mtdsplit_bcm_wfi_init);