drivers/mtd/parsers/tplink_safeloader.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright © 2022 Rafał Miłecki <[email protected]>
   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 safeloader_cmn_header {
  17         __be32 size;
  18         uint32_t unused;
  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(np);
  34         else
  35                 np = of_get_child_by_name(np, "partitions");
  36
  37         if (of_property_read_u32(np, "partitions-table-offset", &offset)) {
  38                 pr_err("Failed to get partitions table offset\n");
  39                 goto err_put;
  40         }
  41
  42         err = mtd_read(mtd, offset, sizeof(hdr), &bytes_read, (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 + 1, GFP_KERNEL);
  51         if (!buf)
  52                 goto err_put;
  53
  54         err = mtd_read(mtd, offset + sizeof(hdr), size, &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(np);
  63
  64         return buf;
  65
  66 err_kfree:
  67         kfree(buf);
  68 err_put:
  69         of_node_put(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, 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(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(buf);
 113
 114         *pparts = parts;
 115
 116         return idx;
 117
 118 err_free:
 119         for (idx -= 1; idx >= 0; idx--)
 120                 kfree(parts[idx].name);
 121 err_free_parts:
 122         kfree(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(pparts[i].name);
 134
 135         kfree(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");