154 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*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 414#define TPLINK_SAFELOADER_MAX_PARTS 3215 16struct safeloader_cmn_header {17 __be32 size;18 uint32_t unused;19} __packed;20 21static 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 else35 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 66err_kfree:67 kfree(buf);68err_put:69 of_node_put(np);70 return NULL;71}72 73static 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 118err_free:119 for (idx -= 1; idx >= 0; idx--)120 kfree(parts[idx].name);121err_free_parts:122 kfree(parts);123err_out:124 return err;125};126 127static 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 138static const struct of_device_id mtd_parser_tplink_safeloader_of_match_table[] = {139 { .compatible = "tplink,safeloader-partitions" },140 {},141};142MODULE_DEVICE_TABLE(of, mtd_parser_tplink_safeloader_of_match_table);143 144static 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};150module_mtd_part_parser(mtd_parser_tplink_safeloader);151 152MODULE_DESCRIPTION("TP-Link Safeloader partitions parser");153MODULE_LICENSE("GPL");154