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