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");
153