1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * Copyright © 2022 Rafał Miłecki <rafal@milecki.pl> 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_out; 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_out: 122 return err; 123 }; 124 125 static void mtd_parser_tplink_safeloader_cleanup(const struct mtd_partition *pparts, 126 int nr_parts) 127 { 128 int i; 129 130 for (i = 0; i < nr_parts; i++) 131 kfree(pparts[i].name); 132 133 kfree(pparts); 134 } 135 136 static const struct of_device_id mtd_parser_tplink_safeloader_of_match_table[] = { 137 { .compatible = "tplink,safeloader-partitions" }, 138 {}, 139 }; 140 MODULE_DEVICE_TABLE(of, mtd_parser_tplink_safeloader_of_match_table); 141 142 static struct mtd_part_parser mtd_parser_tplink_safeloader = { 143 .parse_fn = mtd_parser_tplink_safeloader_parse, 144 .cleanup = mtd_parser_tplink_safeloader_cleanup, 145 .name = "tplink-safeloader", 146 .of_match_table = mtd_parser_tplink_safeloader_of_match_table, 147 }; 148 module_mtd_part_parser(mtd_parser_tplink_safeloader); 149 150 MODULE_LICENSE("GPL"); 151