1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * Copyright © 2022 Rafał Miłecki <rafal@milecki.pl>
  4 */
  5
  6#include <linux/kernel.h>
  7#include <linux/module.h>
  8#include <linux/mtd/mtd.h>
  9#include <linux/mtd/partitions.h>
 10#include <linux/of.h>
 11#include <linux/slab.h>
 12
 13#define TPLINK_SAFELOADER_DATA_OFFSET		4
 14#define TPLINK_SAFELOADER_MAX_PARTS		32
 15
 16struct safeloader_cmn_header {
 17	__be32 size;
 18	uint32_t unused;
 19} __packed;
 20
 21static void *mtd_parser_tplink_safeloader_read_table(struct mtd_info *mtd)
 22{
 23	struct safeloader_cmn_header hdr;
 24	struct device_node *np;
 25	size_t bytes_read;
 26	size_t size;
 27	u32 offset;
 28	char *buf;
 29	int err;
 30
 31	np = mtd_get_of_node(mtd);
 32	if (mtd_is_partition(mtd))
 33		of_node_get(np);
 34	else
 35		np = of_get_child_by_name(np, "partitions");
 36
 37	if (of_property_read_u32(np, "partitions-table-offset", &offset)) {
 38		pr_err("Failed to get partitions table offset\n");
 39		goto err_put;
 40	}
 41
 42	err = mtd_read(mtd, offset, sizeof(hdr), &bytes_read, (uint8_t *)&hdr);
 43	if (err && !mtd_is_bitflip(err)) {
 44		pr_err("Failed to read from %s at 0x%x\n", mtd->name, offset);
 45		goto err_put;
 46	}
 47
 48	size = be32_to_cpu(hdr.size);
 49
 50	buf = kmalloc(size + 1, GFP_KERNEL);
 51	if (!buf)
 52		goto err_put;
 53
 54	err = mtd_read(mtd, offset + sizeof(hdr), size, &bytes_read, buf);
 55	if (err && !mtd_is_bitflip(err)) {
 56		pr_err("Failed to read from %s at 0x%zx\n", mtd->name, offset + sizeof(hdr));
 57		goto err_kfree;
 58	}
 59
 60	buf[size] = '\0';
 61
 62	of_node_put(np);
 63
 64	return buf;
 65
 66err_kfree:
 67	kfree(buf);
 68err_put:
 69	of_node_put(np);
 70	return NULL;
 71}
 72
 73static int mtd_parser_tplink_safeloader_parse(struct mtd_info *mtd,
 74					      const struct mtd_partition **pparts,
 75					      struct mtd_part_parser_data *data)
 76{
 77	struct mtd_partition *parts;
 78	char name[65];
 79	size_t offset;
 80	size_t bytes;
 81	char *buf;
 82	int idx;
 83	int err;
 84
 85	parts = kcalloc(TPLINK_SAFELOADER_MAX_PARTS, sizeof(*parts), GFP_KERNEL);
 86	if (!parts) {
 87		err = -ENOMEM;
 88		goto err_out;
 89	}
 90
 91	buf = mtd_parser_tplink_safeloader_read_table(mtd);
 92	if (!buf) {
 93		err = -ENOENT;
 94		goto err_free_parts;
 95	}
 96
 97	for (idx = 0, offset = TPLINK_SAFELOADER_DATA_OFFSET;
 98	     idx < TPLINK_SAFELOADER_MAX_PARTS &&
 99	     sscanf(buf + offset, "partition %64s base 0x%llx size 0x%llx%zn\n",
100		    name, &parts[idx].offset, &parts[idx].size, &bytes) == 3;
101	     idx++, offset += bytes + 1) {
102		parts[idx].name = kstrdup(name, GFP_KERNEL);
103		if (!parts[idx].name) {
104			err = -ENOMEM;
105			goto err_free;
106		}
107	}
108
109	if (idx == TPLINK_SAFELOADER_MAX_PARTS)
110		pr_warn("Reached maximum number of partitions!\n");
111
112	kfree(buf);
113
114	*pparts = parts;
115
116	return idx;
117
118err_free:
119	for (idx -= 1; idx >= 0; idx--)
120		kfree(parts[idx].name);
121err_free_parts:
122	kfree(parts);
123err_out:
124	return err;
125};
126
127static void mtd_parser_tplink_safeloader_cleanup(const struct mtd_partition *pparts,
128						 int nr_parts)
129{
130	int i;
131
132	for (i = 0; i < nr_parts; i++)
133		kfree(pparts[i].name);
134
135	kfree(pparts);
136}
137
138static const struct of_device_id mtd_parser_tplink_safeloader_of_match_table[] = {
139	{ .compatible = "tplink,safeloader-partitions" },
140	{},
141};
142MODULE_DEVICE_TABLE(of, mtd_parser_tplink_safeloader_of_match_table);
143
144static struct mtd_part_parser mtd_parser_tplink_safeloader = {
145	.parse_fn = mtd_parser_tplink_safeloader_parse,
146	.cleanup = mtd_parser_tplink_safeloader_cleanup,
147	.name = "tplink-safeloader",
148	.of_match_table = mtd_parser_tplink_safeloader_of_match_table,
149};
150module_mtd_part_parser(mtd_parser_tplink_safeloader);
151
152MODULE_LICENSE("GPL");