1 files changed, 152 insertions, 0 deletions
diff --git a/drivers/mtd/parsers/tplink_safeloader.c b/drivers/mtd/parsers/tplink_safeloader.c
new file mode 100644
index 000000000..1c689dafc
--- /dev/null
+++ b/drivers/mtd/parsers/tplink_safeloader.c
@@ -0,0 +1,152 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright © 2022 Rafał Miłecki <rafal@milecki.pl>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+
+#define TPLINK_SAFELOADER_DATA_OFFSET		4
+#define TPLINK_SAFELOADER_MAX_PARTS		32
+
+struct safeloader_cmn_header {
+	__be32 size;
+	uint32_t unused;
+} __packed;
+
+static void *mtd_parser_tplink_safeloader_read_table(struct mtd_info *mtd)
+{
+	struct safeloader_cmn_header hdr;
+	struct device_node *np;
+	size_t bytes_read;
+	size_t size;
+	u32 offset;
+	char *buf;
+	int err;
+
+	np = mtd_get_of_node(mtd);
+	if (mtd_is_partition(mtd))
+		of_node_get(np);
+	else
+		np = of_get_child_by_name(np, "partitions");
+
+	if (of_property_read_u32(np, "partitions-table-offset", &offset)) {
+		pr_err("Failed to get partitions table offset\n");
+		goto err_put;
+	}
+
+	err = mtd_read(mtd, offset, sizeof(hdr), &bytes_read, (uint8_t *)&hdr);
+	if (err && !mtd_is_bitflip(err)) {
+		pr_err("Failed to read from %s at 0x%x\n", mtd->name, offset);
+		goto err_put;
+	}
+
+	size = be32_to_cpu(hdr.size);
+
+	buf = kmalloc(size + 1, GFP_KERNEL);
+	if (!buf)
+		goto err_put;
+
+	err = mtd_read(mtd, offset + sizeof(hdr), size, &bytes_read, buf);
+	if (err && !mtd_is_bitflip(err)) {
+		pr_err("Failed to read from %s at 0x%zx\n", mtd->name, offset + sizeof(hdr));
+		goto err_kfree;
+	}
+
+	buf[size] = '\0';
+
+	of_node_put(np);
+
+	return buf;
+
+err_kfree:
+	kfree(buf);
+err_put:
+	of_node_put(np);
+	return NULL;
+}
+
+static int mtd_parser_tplink_safeloader_parse(struct mtd_info *mtd,
+					      const struct mtd_partition **pparts,
+					      struct mtd_part_parser_data *data)
+{
+	struct mtd_partition *parts;
+	char name[65];
+	size_t offset;
+	size_t bytes;
+	char *buf;
+	int idx;
+	int err;
+
+	parts = kcalloc(TPLINK_SAFELOADER_MAX_PARTS, sizeof(*parts), GFP_KERNEL);
+	if (!parts) {
+		err = -ENOMEM;
+		goto err_out;
+	}
+
+	buf = mtd_parser_tplink_safeloader_read_table(mtd);
+	if (!buf) {
+		err = -ENOENT;
+		goto err_free_parts;
+	}
+
+	for (idx = 0, offset = TPLINK_SAFELOADER_DATA_OFFSET;
+	     idx < TPLINK_SAFELOADER_MAX_PARTS &&
+	     sscanf(buf + offset, "partition %64s base 0x%llx size 0x%llx%zn\n",
+		    name, &parts[idx].offset, &parts[idx].size, &bytes) == 3;
+	     idx++, offset += bytes + 1) {
+		parts[idx].name = kstrdup(name, GFP_KERNEL);
+		if (!parts[idx].name) {
+			err = -ENOMEM;
+			goto err_free;
+		}
+	}
+
+	if (idx == TPLINK_SAFELOADER_MAX_PARTS)
+		pr_warn("Reached maximum number of partitions!\n");
+
+	kfree(buf);
+
+	*pparts = parts;
+
+	return idx;
+
+err_free:
+	for (idx -= 1; idx >= 0; idx--)
+		kfree(parts[idx].name);
+err_free_parts:
+	kfree(parts);
+err_out:
+	return err;
+};
+
+static void mtd_parser_tplink_safeloader_cleanup(const struct mtd_partition *pparts,
+						 int nr_parts)
+{
+	int i;
+
+	for (i = 0; i < nr_parts; i++)
+		kfree(pparts[i].name);
+
+	kfree(pparts);
+}
+
+static const struct of_device_id mtd_parser_tplink_safeloader_of_match_table[] = {
+	{ .compatible = "tplink,safeloader-partitions" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mtd_parser_tplink_safeloader_of_match_table);
+
+static struct mtd_part_parser mtd_parser_tplink_safeloader = {
+	.parse_fn = mtd_parser_tplink_safeloader_parse,
+	.cleanup = mtd_parser_tplink_safeloader_cleanup,
+	.name = "tplink-safeloader",
+	.of_match_table = mtd_parser_tplink_safeloader_of_match_table,
+};
+module_mtd_part_parser(mtd_parser_tplink_safeloader);
+
+MODULE_LICENSE("GPL");