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(node: np);
34 else
35 np = of_get_child_by_name(node: np, name: "partitions");
36
37 if (of_property_read_u32(np, propname: "partitions-table-offset", out_value: &offset)) {
38 pr_err("Failed to get partitions table offset\n");
39 goto err_put;
40 }
41
42 err = mtd_read(mtd, from: offset, len: sizeof(hdr), retlen: &bytes_read, buf: (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: size + 1, GFP_KERNEL);
51 if (!buf)
52 goto err_put;
53
54 err = mtd_read(mtd, from: offset + sizeof(hdr), len: size, retlen: &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(node: np);
63
64 return buf;
65
66err_kfree:
67 kfree(objp: buf);
68err_put:
69 of_node_put(node: 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, size: 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(s: 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(objp: buf);
113
114 *pparts = parts;
115
116 return idx;
117
118err_free:
119 for (idx -= 1; idx >= 0; idx--)
120 kfree(objp: parts[idx].name);
121err_free_parts:
122 kfree(objp: 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(objp: pparts[i].name);
134
135 kfree(objp: 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

source code of linux/drivers/mtd/parsers/tplink_safeloader.c