kernel: mtdsplit: bcm_wfi: refactor code
[openwrt/openwrt.git] / target / linux / generic / files / drivers / mtd / mtdsplit / mtdsplit_bcm_wfi.c
1 /*
2 * MTD split for Broadcom Whole Flash Image
3 *
4 * Copyright (C) 2020 Álvaro Fernández Rojas <noltari@gmail.com>
5 *
6 * This program is free software; you can redistribute it and/or modify it
7 * under the terms of the GNU General Public License version 2 as published
8 * by the Free Software Foundation.
9 *
10 */
11
12 #define je16_to_cpu(x) ((x).v16)
13 #define je32_to_cpu(x) ((x).v32)
14
15 #include <linux/crc32.h>
16 #include <linux/init.h>
17 #include <linux/jffs2.h>
18 #include <linux/kernel.h>
19 #include <linux/module.h>
20 #include <linux/slab.h>
21 #include <linux/byteorder/generic.h>
22 #include <linux/mtd/mtd.h>
23 #include <linux/mtd/partitions.h>
24
25 #include "mtdsplit.h"
26
27 #define char_to_num(c) ((c >= '0' && c <= '9') ? (c - '0') : (0))
28
29 #define BCM_WFI_PARTS 3
30
31 #define CFERAM_NAME "cferam"
32 #define CFERAM_NAME_LEN (sizeof(CFERAM_NAME) - 1)
33 #define KERNEL_NAME "vmlinux.lz"
34 #define KERNEL_NAME_LEN (sizeof(KERNEL_NAME) - 1)
35 #define OPENWRT_NAME "1-openwrt"
36 #define OPENWRT_NAME_LEN (sizeof(OPENWRT_NAME) - 1)
37
38 #define UBI_MAGIC 0x55424923
39
40 #define SERCOMM_MAGIC_PFX "eRcOmM."
41 #define SERCOMM_MAGIC_PFX_LEN (sizeof(SERCOMM_MAGIC_PFX) - 1)
42 #define SERCOMM_MAGIC "eRcOmM.000"
43 #define SERCOMM_MAGIC_LEN (sizeof(SERCOMM_MAGIC) - 1)
44
45 static u32 jffs2_dirent_crc(struct jffs2_raw_dirent *node)
46 {
47 return crc32(0, node, sizeof(struct jffs2_raw_dirent) - 8);
48 }
49
50 static bool jffs2_dirent_valid(struct jffs2_raw_dirent *node)
51 {
52 return ((je16_to_cpu(node->magic) == JFFS2_MAGIC_BITMASK) &&
53 (je16_to_cpu(node->nodetype) == JFFS2_NODETYPE_DIRENT) &&
54 je32_to_cpu(node->ino) &&
55 je32_to_cpu(node->node_crc) == jffs2_dirent_crc(node));
56 }
57
58 static int jffs2_find_file(struct mtd_info *mtd, uint8_t *buf,
59 const char *name, size_t name_len,
60 loff_t *offs, loff_t size)
61 {
62 const loff_t end = *offs + size;
63 struct jffs2_raw_dirent *node;
64 bool valid = false;
65 size_t retlen;
66 uint16_t magic;
67 int rc;
68
69 for (; *offs < end; *offs += mtd->erasesize) {
70 unsigned int block_offs = 0;
71
72 /* Skip CFE erased blocks */
73 rc = mtd_read(mtd, *offs, sizeof(magic), &retlen,
74 (void *) &magic);
75 if (rc || retlen != sizeof(magic)) {
76 continue;
77 }
78
79 /* Skip blocks not starting with JFFS2 magic */
80 if (magic != JFFS2_MAGIC_BITMASK)
81 continue;
82
83 /* Read full block */
84 rc = mtd_read(mtd, *offs, mtd->erasesize, &retlen,
85 (void *) buf);
86 if (rc)
87 return rc;
88 if (retlen != mtd->erasesize)
89 return -EINVAL;
90
91 while (block_offs < mtd->erasesize) {
92 node = (struct jffs2_raw_dirent *) &buf[block_offs];
93
94 if (!jffs2_dirent_valid(node)) {
95 block_offs += 4;
96 continue;
97 }
98
99 if (!memcmp(node->name, OPENWRT_NAME,
100 OPENWRT_NAME_LEN))
101 valid = true;
102 else if (!memcmp(node->name, name, name_len))
103 return valid ? 0 : -EINVAL;
104
105 block_offs += je32_to_cpu(node->totlen);
106 block_offs = (block_offs + 0x3) & ~0x3;
107 }
108 }
109
110 return -ENOENT;
111 }
112
113 static int ubifs_find(struct mtd_info *mtd, loff_t *offs, loff_t size)
114 {
115 const loff_t end = *offs + size;
116 uint32_t magic;
117 size_t retlen;
118 int rc;
119
120 for (; *offs < end; *offs += mtd->erasesize) {
121 rc = mtd_read(mtd, *offs, sizeof(magic), &retlen,
122 (unsigned char *) &magic);
123 if (rc || retlen != sizeof(magic))
124 continue;
125
126 if (be32_to_cpu(magic) == UBI_MAGIC)
127 return 0;
128 }
129
130 return -ENOENT;
131 }
132
133 static int parse_bcm_wfi(struct mtd_info *master,
134 const struct mtd_partition **pparts,
135 uint8_t *buf, loff_t off, loff_t size, bool cfe_part)
136 {
137 struct mtd_partition *parts;
138 loff_t cfe_off, kernel_off, rootfs_off;
139 unsigned int num_parts = BCM_WFI_PARTS, cur_part = 0;
140 int ret;
141
142 if (cfe_part) {
143 num_parts++;
144 cfe_off = off;
145
146 ret = jffs2_find_file(master, buf, CFERAM_NAME,
147 CFERAM_NAME_LEN, &cfe_off,
148 size - (cfe_off - off));
149 if (ret)
150 return ret;
151
152 kernel_off = cfe_off + master->erasesize;
153 } else {
154 kernel_off = off;
155 }
156
157 ret = jffs2_find_file(master, buf, KERNEL_NAME, KERNEL_NAME_LEN,
158 &kernel_off, size - (kernel_off - off));
159 if (ret)
160 return ret;
161
162 rootfs_off = kernel_off + master->erasesize;
163 ret = ubifs_find(master, &rootfs_off, size - (rootfs_off - off));
164 if (ret)
165 return ret;
166
167 parts = kzalloc(num_parts * sizeof(*parts), GFP_KERNEL);
168 if (!parts)
169 return -ENOMEM;
170
171 if (cfe_part) {
172 parts[cur_part].name = "cferam";
173 parts[cur_part].mask_flags = MTD_WRITEABLE;
174 parts[cur_part].offset = cfe_off;
175 parts[cur_part].size = kernel_off - cfe_off;
176 cur_part++;
177 }
178
179 parts[cur_part].name = "firmware";
180 parts[cur_part].offset = kernel_off;
181 parts[cur_part].size = size - (kernel_off - off);
182 cur_part++;
183
184 parts[cur_part].name = KERNEL_PART_NAME;
185 parts[cur_part].offset = kernel_off;
186 parts[cur_part].size = rootfs_off - kernel_off;
187 cur_part++;
188
189 parts[cur_part].name = UBI_PART_NAME;
190 parts[cur_part].offset = rootfs_off;
191 parts[cur_part].size = size - (rootfs_off - off);
192 cur_part++;
193
194 *pparts = parts;
195
196 return num_parts;
197 }
198
199 static int mtdsplit_parse_bcm_wfi(struct mtd_info *master,
200 const struct mtd_partition **pparts,
201 struct mtd_part_parser_data *data)
202 {
203 struct device_node *mtd_node;
204 bool cfe_part = true;
205 uint8_t *buf;
206 int ret;
207
208 mtd_node = mtd_get_of_node(master);
209 if (!mtd_node)
210 return -EINVAL;
211
212 buf = kzalloc(master->erasesize, GFP_KERNEL);
213 if (!buf)
214 return -ENOMEM;
215
216 if (of_property_read_bool(mtd_node, "brcm,no-cferam"))
217 cfe_part = false;
218
219 ret = parse_bcm_wfi(master, pparts, buf, 0, master->size, cfe_part);
220
221 kfree(buf);
222
223 return ret;
224 }
225
226 static const struct of_device_id mtdsplit_bcm_wfi_of_match[] = {
227 { .compatible = "brcm,wfi" },
228 { },
229 };
230
231 static struct mtd_part_parser mtdsplit_bcm_wfi_parser = {
232 .owner = THIS_MODULE,
233 .name = "bcm-wfi-fw",
234 .of_match_table = mtdsplit_bcm_wfi_of_match,
235 .parse_fn = mtdsplit_parse_bcm_wfi,
236 .type = MTD_PARSER_TYPE_FIRMWARE,
237 };
238
239 static int sercomm_bootflag_value(struct mtd_info *mtd, uint8_t *buf)
240 {
241 size_t retlen;
242 loff_t offs;
243 int rc;
244
245 for (offs = 0; offs < mtd->size; offs += mtd->erasesize) {
246 rc = mtd_read(mtd, offs, SERCOMM_MAGIC_LEN, &retlen, buf);
247 if (rc || retlen != SERCOMM_MAGIC_LEN)
248 continue;
249
250 if (memcmp(buf, SERCOMM_MAGIC_PFX, SERCOMM_MAGIC_PFX_LEN))
251 continue;
252
253 rc = char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 0]) * 100;
254 rc += char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 1]) * 10;
255 rc += char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 2]) * 1;
256
257 return rc;
258 }
259
260 return -ENOENT;
261 }
262
263 static int mtdsplit_parse_ser_wfi(struct mtd_info *master,
264 const struct mtd_partition **pparts,
265 struct mtd_part_parser_data *data)
266 {
267 struct mtd_info *mtd_bf1, *mtd_bf2;
268 struct erase_info bf_erase;
269 loff_t img1_off = 0;
270 loff_t img2_off = master->size / 2;
271 loff_t img1_size = (img2_off - img1_off);
272 loff_t img2_size = (master->size - img2_off);
273 loff_t active_off, inactive_off;
274 loff_t active_size, inactive_size;
275 uint8_t *buf;
276 int bf1, bf2;
277 int ret;
278
279 mtd_bf1 = get_mtd_device_nm("bootflag1");
280 if (IS_ERR(mtd_bf1))
281 return -ENOENT;
282
283 mtd_bf2 = get_mtd_device_nm("bootflag2");
284 if (IS_ERR(mtd_bf2))
285 return -ENOENT;
286
287 buf = kzalloc(master->erasesize, GFP_KERNEL);
288 if (!buf)
289 return -ENOMEM;
290
291 bf1 = sercomm_bootflag_value(mtd_bf1, buf);
292 if (bf1 >= 0)
293 printk("sercomm: bootflag1=%d\n", bf1);
294
295 bf2 = sercomm_bootflag_value(mtd_bf2, buf);
296 if (bf2 >= 0)
297 printk("sercomm: bootflag2=%d\n", bf2);
298
299 if (bf1 == bf2 && bf2 >= 0) {
300 bf2 = -ENOENT;
301 bf_erase.addr = 0;
302 bf_erase.len = mtd_bf2->size;
303 mtd_erase(mtd_bf2, &bf_erase);
304 }
305
306 if (bf1 >= bf2) {
307 active_off = img1_off;
308 active_size = img1_size;
309 inactive_off = img2_off;
310 inactive_size = img2_size;
311 } else {
312 active_off = img2_off;
313 active_size = img2_size;
314 inactive_off = img1_off;
315 inactive_size = img1_size;
316 }
317
318 ret = parse_bcm_wfi(master, pparts, buf, active_off, active_size, false);
319
320 kfree(buf);
321
322 if (ret > 0) {
323 struct mtd_partition *parts;
324
325 parts = kzalloc((ret + 1) * sizeof(*parts), GFP_KERNEL);
326 if (!parts)
327 return -ENOMEM;
328
329 memcpy(parts, *pparts, ret * sizeof(*parts));
330 kfree(*pparts);
331
332 parts[ret].name = "img2";
333 parts[ret].offset = inactive_off;
334 parts[ret].size = inactive_size;
335 ret++;
336
337 *pparts = parts;
338 }
339
340 return ret;
341 }
342
343 static const struct of_device_id mtdsplit_ser_wfi_of_match[] = {
344 { .compatible = "sercomm,wfi" },
345 { },
346 };
347
348 static struct mtd_part_parser mtdsplit_ser_wfi_parser = {
349 .owner = THIS_MODULE,
350 .name = "ser-wfi-fw",
351 .of_match_table = mtdsplit_ser_wfi_of_match,
352 .parse_fn = mtdsplit_parse_ser_wfi,
353 .type = MTD_PARSER_TYPE_FIRMWARE,
354 };
355
356 static int __init mtdsplit_bcm_wfi_init(void)
357 {
358 register_mtd_parser(&mtdsplit_bcm_wfi_parser);
359 register_mtd_parser(&mtdsplit_ser_wfi_parser);
360
361 return 0;
362 }
363
364 module_init(mtdsplit_bcm_wfi_init);