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 
16 struct safeloader_cmn_header {
17 	__be32 size;
18 	uint32_t unused;
19 } __packed;
20 
mtd_parser_tplink_safeloader_read_table(struct mtd_info * mtd)21 static 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 
66 err_kfree:
67 	kfree(buf);
68 err_put:
69 	of_node_put(np);
70 	return NULL;
71 }
72 
mtd_parser_tplink_safeloader_parse(struct mtd_info * mtd,const struct mtd_partition ** pparts,struct mtd_part_parser_data * data)73 static 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 
118 err_free:
119 	for (idx -= 1; idx >= 0; idx--)
120 		kfree(parts[idx].name);
121 err_free_parts:
122 	kfree(parts);
123 err_out:
124 	return err;
125 };
126 
mtd_parser_tplink_safeloader_cleanup(const struct mtd_partition * pparts,int nr_parts)127 static 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 
138 static const struct of_device_id mtd_parser_tplink_safeloader_of_match_table[] = {
139 	{ .compatible = "tplink,safeloader-partitions" },
140 	{},
141 };
142 MODULE_DEVICE_TABLE(of, mtd_parser_tplink_safeloader_of_match_table);
143 
144 static 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 };
150 module_mtd_part_parser(mtd_parser_tplink_safeloader);
151 
152 MODULE_LICENSE("GPL");
153