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 { |
17 | __be32 ; |
18 | uint32_t ; |
19 | } __packed; |
20 | |
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(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 | |
66 | err_kfree: |
67 | kfree(objp: buf); |
68 | err_put: |
69 | of_node_put(node: np); |
70 | return NULL; |
71 | } |
72 | |
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, 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 | |
118 | err_free: |
119 | for (idx -= 1; idx >= 0; idx--) |
120 | kfree(objp: parts[idx].name); |
121 | err_free_parts: |
122 | kfree(objp: parts); |
123 | err_out: |
124 | return err; |
125 | }; |
126 | |
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(objp: pparts[i].name); |
134 | |
135 | kfree(objp: 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 | |