DHDAXCW-Rockchip-OpenWrt/target/linux/ramips/files/drivers/mtd/parsers/scpart.c

// SPDX-License-Identifier: GPL-2.0-or-later OR MIT
/*
 * Sercomm/Netgear FLASH partition table.
 */


#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/mtd/mtd.h>
#include <linux/mtd/partitions.h>
#include <linux/module.h>


#define	MOD_NAME	"scpart"

static const char sc_part_magic[] = {
	'S', 'C', 'F', 'L', 'M', 'A', 'P', 'O', 'K', '\0',
};
#define	PART_MAGIC_LEN	sizeof(sc_part_magic)

/* assumes that all fields are set by CPU native endian */
struct sc_part_desc {
	uint32_t	part_id;
	uint32_t	part_offs;
	uint32_t	part_bytes;
};
#define	ID_ALREADY_FOUND	(0xFFFFFFFFUL)

#define	MAP_OFFS_IN_BLK	(0x800)

#define	MAP_MIRROR_NUM	(2)

static int scpart_desc_is_valid(struct sc_part_desc *pdesc)
{
	return !!((pdesc->part_id != 0xFFFFFFFFUL) &&
			(pdesc->part_offs != 0xFFFFFFFFUL) &&
			(pdesc->part_bytes != 0xFFFFFFFFUL));
}

static int scpart_scan_partmap(struct mtd_info *master, loff_t partmap_offs,
				struct sc_part_desc **ppdesc)
{
	uint8_t *buf;
	loff_t offs;
	size_t retlen;
	struct sc_part_desc *pdesc = NULL;
	struct sc_part_desc *tmpdesc;
	int cnt = 0;
	int res2;
	int res = 0;

	buf = kzalloc(master->erasesize, GFP_KERNEL);
	if (!buf) {
		res = -ENOMEM;
		goto out;
	}

	res2 = mtd_read(master, partmap_offs, master->erasesize, &retlen, buf);
	if (res2 || (retlen != master->erasesize)) {
		res = -EIO;
		goto free;
	}

	offs = MAP_OFFS_IN_BLK;
	while ((offs + sizeof(*tmpdesc)) < master->erasesize) {
		tmpdesc = (struct sc_part_desc *)&(buf[offs]);
		if (!scpart_desc_is_valid(tmpdesc))
			break;
		cnt++;
		offs += sizeof(*tmpdesc);
	}

	if (cnt > 0) {
		int bytes = cnt * sizeof(*pdesc);

		pdesc = kzalloc(bytes, GFP_KERNEL);
		if (!pdesc) {
			res = -ENOMEM;
			goto free;
		}
		memcpy(pdesc, &(buf[MAP_OFFS_IN_BLK]), bytes);

		*ppdesc = pdesc;
		res = cnt;
	}

free:
	kfree(buf);

out:
	return res;
}

static int scpart_find_partmap(struct mtd_info *master,
				struct sc_part_desc **ppdesc)
{
	loff_t offs;
	uint8_t rdbuf[PART_MAGIC_LEN];
	size_t retlen;
	int magic_found = 0;
	int res2;
	int res = 0;

	offs = 0;
	while ((magic_found < MAP_MIRROR_NUM) &&
			(offs < master->size) && !mtd_block_isbad(master, offs)) {
		res2 = mtd_read(master, offs, PART_MAGIC_LEN, &retlen, rdbuf);
		if (res2 || (retlen != PART_MAGIC_LEN)) {
			res = -EIO;
			goto out;
		}
		if (!memcmp(rdbuf, sc_part_magic, PART_MAGIC_LEN)) {
			pr_debug("%s: signature found at 0x%llx\n", MOD_NAME, offs);
			magic_found++;
			res = scpart_scan_partmap(master, offs, ppdesc);
			if (res > 0)
				goto out;
		}
		offs += master->erasesize;
	}

out:
	if (res > 0)
		pr_info("%s: valid 'SC PART MAP' found (%d partitions)\n", MOD_NAME, res);
	else
		pr_info("%s: no valid 'SC PART MAP'\n", MOD_NAME);

	return res;
}

static int scpart_parse(struct mtd_info *master,
			const struct mtd_partition **pparts,
			struct mtd_part_parser_data *data)
{
	struct sc_part_desc *scpart_map = NULL;
	struct mtd_partition *parts = NULL;
	struct device_node *mtd_node;
	struct device_node *ofpart_node;
	struct device_node *pp;
	const char *partname;
	int nr_scparts;
	int nr_parts = 0;
	int n;
	int res = 0;

	mtd_node = mtd_get_of_node(master);
	if (!mtd_node)
		goto out;

	ofpart_node = of_get_child_by_name(mtd_node, "partitions");
	if (!ofpart_node)
		goto out;

	nr_scparts = scpart_find_partmap(master, &scpart_map);
	if (nr_scparts <= 0) {
		res = nr_scparts;
		goto free;
	}

	for_each_child_of_node(ofpart_node, pp) {
		u32 scpart_id;
		struct mtd_partition *parts_tmp;

		if (of_property_read_u32(pp, "scpart-id", &scpart_id))
			continue;

		for (n = 0 ; n < nr_scparts ; n++)
			if ((scpart_map[n].part_id != ID_ALREADY_FOUND) &&
					(scpart_id == scpart_map[n].part_id))
				break;
		if (n >= nr_scparts)
			/* not match */
			continue;

		/* add the partition found in OF into MTD partition array */

		/* reallocate partition array */
		parts_tmp = parts;
		parts = kzalloc((nr_parts + 1) * sizeof(*parts), GFP_KERNEL);
		if (!parts) {
			kfree(parts_tmp);
			res = -ENOMEM;
			goto free;
		}
		if (parts_tmp) {
			memcpy(parts, parts_tmp, nr_parts * sizeof(*parts));
			kfree(parts_tmp);
		}

		parts[nr_parts].offset = scpart_map[n].part_offs;
		parts[nr_parts].size = scpart_map[n].part_bytes;
		parts[nr_parts].of_node = pp;

		if (!of_property_read_string(pp, "label", &partname) ||
				!of_property_read_string(pp, "name", &partname))
			parts[nr_parts].name = partname;

		if (of_property_read_bool(pp, "read-only"))
			parts[nr_parts].mask_flags |= MTD_WRITEABLE;
		if (of_property_read_bool(pp, "lock"))
			parts[nr_parts].mask_flags |= MTD_POWERUP_LOCK;

		/* mark as 'done' */
		scpart_map[n].part_id = ID_ALREADY_FOUND;

		nr_parts++;
	}
	if (nr_parts > 0) {
		*pparts = parts;
		res = nr_parts;
	} else
		pr_info("%s: No partition in OF matches partition ID with 'SC PART MAP'.\n",
				MOD_NAME);

	of_node_put(pp);

free:
	kfree(scpart_map);
	if (res <= 0)
		kfree(parts);

out:
	return res;
}

static const struct of_device_id scpart_parser_of_match_table[] = {
	{ .compatible = "sercomm,sc-partitions" },
	{},
};
MODULE_DEVICE_TABLE(of, scpart_parser_of_match_table);

static struct mtd_part_parser scpart_parser = {
	.parse_fn = scpart_parse,
	.name = "scpart",
	.of_match_table = scpart_parser_of_match_table,
};
module_mtd_part_parser(scpart_parser);

/* mtd parsers will request the module by parser name */
MODULE_ALIAS("scpart");
MODULE_LICENSE("GPL");
MODULE_AUTHOR("NOGUCHI Hiroshi <drvlabo@gmail.com>");
MODULE_DESCRIPTION("Parsing code for Sercomm/Netgear partition table");