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