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 CFE_MAGIC_PFX           "cferam."
  41 #define CFE_MAGIC_PFX_LEN       (sizeof(CFE_MAGIC_PFX) - 1)
  42 #define CFE_MAGIC               "cferam.000"
  43 #define CFE_MAGIC_LEN           (sizeof(CFE_MAGIC) - 1)
  44 #define SERCOMM_MAGIC_PFX       "eRcOmM."
  45 #define SERCOMM_MAGIC_PFX_LEN   (sizeof(SERCOMM_MAGIC_PFX) - 1)
  46 #define SERCOMM_MAGIC           "eRcOmM.000"
  47 #define SERCOMM_MAGIC_LEN       (sizeof(SERCOMM_MAGIC) - 1)
  48
  49 static u32 jffs2_dirent_crc(struct jffs2_raw_dirent *node)
  50 {
  51         return crc32(0, node, sizeof(struct jffs2_raw_dirent) - 8);
  52 }
  53
  54 static bool jffs2_dirent_valid(struct jffs2_raw_dirent *node)
  55 {
  56         return ((je16_to_cpu(node->magic) == JFFS2_MAGIC_BITMASK) &&
  57                 (je16_to_cpu(node->nodetype) == JFFS2_NODETYPE_DIRENT) &&
  58                 je32_to_cpu(node->ino) &&
  59                 je32_to_cpu(node->node_crc) == jffs2_dirent_crc(node));
  60 }
  61
  62 static int jffs2_find_file(struct mtd_info *mtd, uint8_t *buf,
  63                            const char *name, size_t name_len,
  64                            loff_t *offs, loff_t size,
  65                            char **out_name, size_t *out_name_len)
  66 {
  67         const loff_t end = *offs + size;
  68         struct jffs2_raw_dirent *node;
  69         bool valid = false;
  70         size_t retlen;
  71         uint16_t magic;
  72         int rc;
  73
  74         for (; *offs < end; *offs += mtd->erasesize) {
  75                 unsigned int block_offs = 0;
  76
  77                 /* Skip CFE erased blocks */
  78                 rc = mtd_read(mtd, *offs, sizeof(magic), &retlen,
  79                               (void *) &magic);
  80                 if (rc || retlen != sizeof(magic)) {
  81                         continue;
  82                 }
  83
  84                 /* Skip blocks not starting with JFFS2 magic */
  85                 if (magic != JFFS2_MAGIC_BITMASK)
  86                         continue;
  87
  88                 /* Read full block */
  89                 rc = mtd_read(mtd, *offs, mtd->erasesize, &retlen,
  90                               (void *) buf);
  91                 if (rc)
  92                         return rc;
  93                 if (retlen != mtd->erasesize)
  94                         return -EINVAL;
  95
  96                 while (block_offs < mtd->erasesize) {
  97                         node = (struct jffs2_raw_dirent *) &buf[block_offs];
  98
  99                         if (!jffs2_dirent_valid(node)) {
 100                                 block_offs += 4;
 101                                 continue;
 102                         }
 103
 104                         if (!memcmp(node->name, OPENWRT_NAME,
 105                                     OPENWRT_NAME_LEN)) {
 106                                 valid = true;
 107                         } else if (!memcmp(node->name, name, name_len)) {
 108                                 if (!valid)
 109                                         return -EINVAL;
 110
 111                                 if (out_name)
 112                                         *out_name = kstrndup(node->name,
 113                                                              node->nsize,
 114                                                              GFP_KERNEL);
 115
 116                                 if (out_name_len)
 117                                         *out_name_len = node->nsize;
 118
 119                                 return 0;
 120                         }
 121
 122                         block_offs += je32_to_cpu(node->totlen);
 123                         block_offs = (block_offs + 0x3) & ~0x3;
 124                 }
 125         }
 126
 127         return -ENOENT;
 128 }
 129
 130 static int ubifs_find(struct mtd_info *mtd, loff_t *offs, loff_t size)
 131 {
 132         const loff_t end = *offs + size;
 133         uint32_t magic;
 134         size_t retlen;
 135         int rc;
 136
 137         for (; *offs < end; *offs += mtd->erasesize) {
 138                 rc = mtd_read(mtd, *offs, sizeof(magic), &retlen,
 139                               (unsigned char *) &magic);
 140                 if (rc || retlen != sizeof(magic))
 141                         continue;
 142
 143                 if (be32_to_cpu(magic) == UBI_MAGIC)
 144                         return 0;
 145         }
 146
 147         return -ENOENT;
 148 }
 149
 150 static int parse_bcm_wfi(struct mtd_info *master,
 151                          const struct mtd_partition **pparts,
 152                          uint8_t *buf, loff_t off, loff_t size, bool cfe_part)
 153 {
 154         struct mtd_partition *parts;
 155         loff_t cfe_off, kernel_off, rootfs_off;
 156         unsigned int num_parts = BCM_WFI_PARTS, cur_part = 0;
 157         int ret;
 158
 159         if (cfe_part) {
 160                 num_parts++;
 161                 cfe_off = off;
 162
 163                 ret = jffs2_find_file(master, buf, CFERAM_NAME,
 164                                       CFERAM_NAME_LEN, &cfe_off,
 165                                       size - (cfe_off - off), NULL, NULL);
 166                 if (ret)
 167                         return ret;
 168
 169                 kernel_off = cfe_off + master->erasesize;
 170         } else {
 171                 kernel_off = off;
 172         }
 173
 174         ret = jffs2_find_file(master, buf, KERNEL_NAME, KERNEL_NAME_LEN,
 175                               &kernel_off, size - (kernel_off - off),
 176                               NULL, NULL);
 177         if (ret)
 178                 return ret;
 179
 180         rootfs_off = kernel_off + master->erasesize;
 181         ret = ubifs_find(master, &rootfs_off, size - (rootfs_off - off));
 182         if (ret)
 183                 return ret;
 184
 185         parts = kzalloc(num_parts * sizeof(*parts), GFP_KERNEL);
 186         if (!parts)
 187                 return -ENOMEM;
 188
 189         if (cfe_part) {
 190                 parts[cur_part].name = "cferam";
 191                 parts[cur_part].mask_flags = MTD_WRITEABLE;
 192                 parts[cur_part].offset = cfe_off;
 193                 parts[cur_part].size = kernel_off - cfe_off;
 194                 cur_part++;
 195         }
 196
 197         parts[cur_part].name = "firmware";
 198         parts[cur_part].offset = kernel_off;
 199         parts[cur_part].size = size - (kernel_off - off);
 200         cur_part++;
 201
 202         parts[cur_part].name = KERNEL_PART_NAME;
 203         parts[cur_part].offset = kernel_off;
 204         parts[cur_part].size = rootfs_off - kernel_off;
 205         cur_part++;
 206
 207         parts[cur_part].name = UBI_PART_NAME;
 208         parts[cur_part].offset = rootfs_off;
 209         parts[cur_part].size = size - (rootfs_off - off);
 210         cur_part++;
 211
 212         *pparts = parts;
 213
 214         return num_parts;
 215 }
 216
 217 static int mtdsplit_parse_bcm_wfi(struct mtd_info *master,
 218                                   const struct mtd_partition **pparts,
 219                                   struct mtd_part_parser_data *data)
 220 {
 221         struct device_node *mtd_node;
 222         bool cfe_part = true;
 223         uint8_t *buf;
 224         int ret;
 225
 226         mtd_node = mtd_get_of_node(master);
 227         if (!mtd_node)
 228                 return -EINVAL;
 229
 230         buf = kzalloc(master->erasesize, GFP_KERNEL);
 231         if (!buf)
 232                 return -ENOMEM;
 233
 234         if (of_property_read_bool(mtd_node, "brcm,no-cferam"))
 235                 cfe_part = false;
 236
 237         ret = parse_bcm_wfi(master, pparts, buf, 0, master->size, cfe_part);
 238
 239         kfree(buf);
 240
 241         return ret;
 242 }
 243
 244 static const struct of_device_id mtdsplit_bcm_wfi_of_match[] = {
 245         { .compatible = "brcm,wfi" },
 246         { },
 247 };
 248
 249 static struct mtd_part_parser mtdsplit_bcm_wfi_parser = {
 250         .owner = THIS_MODULE,
 251         .name = "bcm-wfi-fw",
 252         .of_match_table = mtdsplit_bcm_wfi_of_match,
 253         .parse_fn = mtdsplit_parse_bcm_wfi,
 254         .type = MTD_PARSER_TYPE_FIRMWARE,
 255 };
 256
 257 static int cferam_bootflag_value(const char *name, size_t name_len)
 258 {
 259         int rc = -ENOENT;
 260
 261         if (name &&
 262             (name_len >= CFE_MAGIC_LEN) &&
 263             !memcmp(name, CFE_MAGIC_PFX, CFE_MAGIC_PFX_LEN)) {
 264                 rc = char_to_num(name[CFE_MAGIC_PFX_LEN + 0]) * 100;
 265                 rc += char_to_num(name[CFE_MAGIC_PFX_LEN + 1]) * 10;
 266                 rc += char_to_num(name[CFE_MAGIC_PFX_LEN + 2]) * 1;
 267         }
 268
 269         return rc;
 270 }
 271
 272 static int mtdsplit_parse_bcm_wfi_split(struct mtd_info *master,
 273                                         const struct mtd_partition **pparts,
 274                                         struct mtd_part_parser_data *data)
 275 {
 276         loff_t cfe_off;
 277         loff_t img1_off = 0;
 278         loff_t img2_off = master->size / 2;
 279         loff_t img1_size = (img2_off - img1_off);
 280         loff_t img2_size = (master->size - img2_off);
 281         loff_t active_off, inactive_off;
 282         loff_t active_size, inactive_size;
 283         uint8_t *buf;
 284         char *cfe1_name = NULL, *cfe2_name = NULL;
 285         size_t cfe1_size = 0, cfe2_size = 0;
 286         int ret;
 287         int bf1, bf2;
 288
 289         buf = kzalloc(master->erasesize, GFP_KERNEL);
 290         if (!buf)
 291                 return -ENOMEM;
 292
 293         cfe_off = img1_off;
 294         ret = jffs2_find_file(master, buf, CFERAM_NAME, CFERAM_NAME_LEN,
 295                               &cfe_off, img1_size, &cfe1_name, &cfe1_size);
 296
 297         cfe_off = img2_off;
 298         ret = jffs2_find_file(master, buf, CFERAM_NAME, CFERAM_NAME_LEN,
 299                               &cfe_off, img2_size, &cfe2_name, &cfe2_size);
 300
 301         bf1 = cferam_bootflag_value(cfe1_name, cfe1_size);
 302         if (bf1 >= 0)
 303                 printk("cferam: bootflag1=%d\n", bf1);
 304
 305         bf2 = cferam_bootflag_value(cfe2_name, cfe2_size);
 306         if (bf2 >= 0)
 307                 printk("cferam: bootflag2=%d\n", bf2);
 308
 309         kfree(cfe1_name);
 310         kfree(cfe2_name);
 311
 312         if (bf1 >= bf2) {
 313                 active_off = img1_off;
 314                 active_size = img1_size;
 315                 inactive_off = img2_off;
 316                 inactive_size = img2_size;
 317         } else {
 318                 active_off = img2_off;
 319                 active_size = img2_size;
 320                 inactive_off = img1_off;
 321                 inactive_size = img1_size;
 322         }
 323
 324         ret = parse_bcm_wfi(master, pparts, buf, active_off, active_size, true);
 325
 326         kfree(buf);
 327
 328         if (ret > 0) {
 329                 struct mtd_partition *parts;
 330
 331                 parts = kzalloc((ret + 1) * sizeof(*parts), GFP_KERNEL);
 332                 if (!parts)
 333                         return -ENOMEM;
 334
 335                 memcpy(parts, *pparts, ret * sizeof(*parts));
 336                 kfree(*pparts);
 337
 338                 parts[ret].name = "img2";
 339                 parts[ret].offset = inactive_off;
 340                 parts[ret].size = inactive_size;
 341                 ret++;
 342
 343                 *pparts = parts;
 344         }
 345
 346         return ret;
 347 }
 348
 349 static const struct of_device_id mtdsplit_bcm_wfi_split_of_match[] = {
 350         { .compatible = "brcm,wfi-split" },
 351         { },
 352 };
 353
 354 static struct mtd_part_parser mtdsplit_bcm_wfi_split_parser = {
 355         .owner = THIS_MODULE,
 356         .name = "bcm-wfi-split-fw",
 357         .of_match_table = mtdsplit_bcm_wfi_split_of_match,
 358         .parse_fn = mtdsplit_parse_bcm_wfi_split,
 359         .type = MTD_PARSER_TYPE_FIRMWARE,
 360 };
 361
 362 static int sercomm_bootflag_value(struct mtd_info *mtd, uint8_t *buf)
 363 {
 364         size_t retlen;
 365         loff_t offs;
 366         int rc;
 367
 368         for (offs = 0; offs < mtd->size; offs += mtd->erasesize) {
 369                 rc = mtd_read(mtd, offs, SERCOMM_MAGIC_LEN, &retlen, buf);
 370                 if (rc || retlen != SERCOMM_MAGIC_LEN)
 371                         continue;
 372
 373                 if (memcmp(buf, SERCOMM_MAGIC_PFX, SERCOMM_MAGIC_PFX_LEN))
 374                         continue;
 375
 376                 rc = char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 0]) * 100;
 377                 rc += char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 1]) * 10;
 378                 rc += char_to_num(buf[SERCOMM_MAGIC_PFX_LEN + 2]) * 1;
 379
 380                 return rc;
 381         }
 382
 383         return -ENOENT;
 384 }
 385
 386 static int mtdsplit_parse_ser_wfi(struct mtd_info *master,
 387                                   const struct mtd_partition **pparts,
 388                                   struct mtd_part_parser_data *data)
 389 {
 390         struct mtd_info *mtd_bf1, *mtd_bf2;
 391         struct erase_info bf_erase;
 392         loff_t img1_off = 0;
 393         loff_t img2_off = master->size / 2;
 394         loff_t img1_size = (img2_off - img1_off);
 395         loff_t img2_size = (master->size - img2_off);
 396         loff_t active_off, inactive_off;
 397         loff_t active_size, inactive_size;
 398         uint8_t *buf;
 399         int bf1, bf2;
 400         int ret;
 401
 402         mtd_bf1 = get_mtd_device_nm("bootflag1");
 403         if (IS_ERR(mtd_bf1))
 404                 return -ENOENT;
 405
 406         mtd_bf2 = get_mtd_device_nm("bootflag2");
 407         if (IS_ERR(mtd_bf2))
 408                 return -ENOENT;
 409
 410         buf = kzalloc(master->erasesize, GFP_KERNEL);
 411         if (!buf)
 412                 return -ENOMEM;
 413
 414         bf1 = sercomm_bootflag_value(mtd_bf1, buf);
 415         if (bf1 >= 0)
 416                 printk("sercomm: bootflag1=%d\n", bf1);
 417
 418         bf2 = sercomm_bootflag_value(mtd_bf2, buf);
 419         if (bf2 >= 0)
 420                 printk("sercomm: bootflag2=%d\n", bf2);
 421
 422         if (bf1 == bf2 && bf2 >= 0) {
 423                 bf2 = -ENOENT;
 424                 bf_erase.addr = 0;
 425                 bf_erase.len = mtd_bf2->size;
 426                 mtd_erase(mtd_bf2, &bf_erase);
 427         }
 428
 429         if (bf1 >= bf2) {
 430                 active_off = img1_off;
 431                 active_size = img1_size;
 432                 inactive_off = img2_off;
 433                 inactive_size = img2_size;
 434         } else {
 435                 active_off = img2_off;
 436                 active_size = img2_size;
 437                 inactive_off = img1_off;
 438                 inactive_size = img1_size;
 439         }
 440
 441         ret = parse_bcm_wfi(master, pparts, buf, active_off, active_size, false);
 442
 443         kfree(buf);
 444
 445         if (ret > 0) {
 446                 struct mtd_partition *parts;
 447
 448                 parts = kzalloc((ret + 1) * sizeof(*parts), GFP_KERNEL);
 449                 if (!parts)
 450                         return -ENOMEM;
 451
 452                 memcpy(parts, *pparts, ret * sizeof(*parts));
 453                 kfree(*pparts);
 454
 455                 parts[ret].name = "img2";
 456                 parts[ret].offset = inactive_off;
 457                 parts[ret].size = inactive_size;
 458                 ret++;
 459
 460                 *pparts = parts;
 461         }
 462
 463         return ret;
 464 }
 465
 466 static const struct of_device_id mtdsplit_ser_wfi_of_match[] = {
 467         { .compatible = "sercomm,wfi" },
 468         { },
 469 };
 470
 471 static struct mtd_part_parser mtdsplit_ser_wfi_parser = {
 472         .owner = THIS_MODULE,
 473         .name = "ser-wfi-fw",
 474         .of_match_table = mtdsplit_ser_wfi_of_match,
 475         .parse_fn = mtdsplit_parse_ser_wfi,
 476         .type = MTD_PARSER_TYPE_FIRMWARE,
 477 };
 478
 479 static int __init mtdsplit_bcm_wfi_init(void)
 480 {
 481         register_mtd_parser(&mtdsplit_bcm_wfi_parser);
 482         register_mtd_parser(&mtdsplit_bcm_wfi_split_parser);
 483         register_mtd_parser(&mtdsplit_ser_wfi_parser);
 484
 485         return 0;
 486 }
 487
 488 module_init(mtdsplit_bcm_wfi_init);