Loading...
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 | // SPDX-License-Identifier: GPL-2.0-only /* * Copyright © 2022 Rafał Miłecki <rafal@milecki.pl> */ #include <linux/kernel.h> #include <linux/module.h> #include <linux/mtd/mtd.h> #include <linux/mtd/partitions.h> #include <linux/of.h> #include <linux/slab.h> #define TPLINK_SAFELOADER_DATA_OFFSET 4 #define TPLINK_SAFELOADER_MAX_PARTS 32 struct safeloader_cmn_header { __be32 size; uint32_t unused; } __packed; static void *mtd_parser_tplink_safeloader_read_table(struct mtd_info *mtd) { struct safeloader_cmn_header hdr; struct device_node *np; size_t bytes_read; size_t size; u32 offset; char *buf; int err; np = mtd_get_of_node(mtd); if (mtd_is_partition(mtd)) of_node_get(np); else np = of_get_child_by_name(np, "partitions"); if (of_property_read_u32(np, "partitions-table-offset", &offset)) { pr_err("Failed to get partitions table offset\n"); goto err_put; } err = mtd_read(mtd, offset, sizeof(hdr), &bytes_read, (uint8_t *)&hdr); if (err && !mtd_is_bitflip(err)) { pr_err("Failed to read from %s at 0x%x\n", mtd->name, offset); goto err_put; } size = be32_to_cpu(hdr.size); buf = kmalloc(size + 1, GFP_KERNEL); if (!buf) goto err_put; err = mtd_read(mtd, offset + sizeof(hdr), size, &bytes_read, buf); if (err && !mtd_is_bitflip(err)) { pr_err("Failed to read from %s at 0x%zx\n", mtd->name, offset + sizeof(hdr)); goto err_kfree; } buf[size] = '\0'; of_node_put(np); return buf; err_kfree: kfree(buf); err_put: of_node_put(np); return NULL; } static int mtd_parser_tplink_safeloader_parse(struct mtd_info *mtd, const struct mtd_partition **pparts, struct mtd_part_parser_data *data) { struct mtd_partition *parts; char name[65]; size_t offset; size_t bytes; char *buf; int idx; int err; parts = kcalloc(TPLINK_SAFELOADER_MAX_PARTS, sizeof(*parts), GFP_KERNEL); if (!parts) { err = -ENOMEM; goto err_out; } buf = mtd_parser_tplink_safeloader_read_table(mtd); if (!buf) { err = -ENOENT; goto err_free_parts; } for (idx = 0, offset = TPLINK_SAFELOADER_DATA_OFFSET; idx < TPLINK_SAFELOADER_MAX_PARTS && sscanf(buf + offset, "partition %64s base 0x%llx size 0x%llx%zn\n", name, &parts[idx].offset, &parts[idx].size, &bytes) == 3; idx++, offset += bytes + 1) { parts[idx].name = kstrdup(name, GFP_KERNEL); if (!parts[idx].name) { err = -ENOMEM; goto err_free; } } if (idx == TPLINK_SAFELOADER_MAX_PARTS) pr_warn("Reached maximum number of partitions!\n"); kfree(buf); *pparts = parts; return idx; err_free: for (idx -= 1; idx >= 0; idx--) kfree(parts[idx].name); err_free_parts: kfree(parts); err_out: return err; }; static void mtd_parser_tplink_safeloader_cleanup(const struct mtd_partition *pparts, int nr_parts) { int i; for (i = 0; i < nr_parts; i++) kfree(pparts[i].name); kfree(pparts); } static const struct of_device_id mtd_parser_tplink_safeloader_of_match_table[] = { { .compatible = "tplink,safeloader-partitions" }, {}, }; MODULE_DEVICE_TABLE(of, mtd_parser_tplink_safeloader_of_match_table); static struct mtd_part_parser mtd_parser_tplink_safeloader = { .parse_fn = mtd_parser_tplink_safeloader_parse, .cleanup = mtd_parser_tplink_safeloader_cleanup, .name = "tplink-safeloader", .of_match_table = mtd_parser_tplink_safeloader_of_match_table, }; module_mtd_part_parser(mtd_parser_tplink_safeloader); MODULE_LICENSE("GPL"); |