diff -drupN a/drivers/block/panicpart/sunxi-panicpart.c b/drivers/block/panicpart/sunxi-panicpart.c --- a/drivers/block/panicpart/sunxi-panicpart.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/block/panicpart/sunxi-panicpart.c 2022-06-12 05:28:14.000000000 +0300 @@ -0,0 +1,267 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * + * Copyright (C) 2019 liaoweixiong + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ +#define MODNAME "panicpart" +#define pr_fmt(fmt) MODNAME ": " fmt + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define CMDLINE_PREFIX "blkoops.blkdev=" +static const char *blkdev = CONFIG_PSTORE_BLKOOPS_BLKDEV; +static struct panic_part *sunxi_part; + +static ssize_t sunxi_panic_read(char *buf, size_t len, loff_t off) +{ + struct panic_part *part = sunxi_part; + size_t rlen, extra; + ssize_t ret; + char sect_buf[SECTOR_SIZE]; + + if (!sunxi_part || !sunxi_part->panic_read) + return -EINVAL; + + if (off > part->sects * SECTOR_SIZE) + return -EINVAL; + + rlen = min_t(size_t, len, part->sects * SECTOR_SIZE - off); + /* off not align to sector */ + extra = off % SECTOR_SIZE; + if (rlen && extra) { + ret = part->panic_read(part, off / SECTOR_SIZE, 1, sect_buf); + if (ret != 1) + return ret; + memcpy(buf, sect_buf + extra, min(SECTOR_SIZE - extra, rlen)); + + buf += SECTOR_SIZE - extra; + off += SECTOR_SIZE - extra; + rlen -= SECTOR_SIZE - extra; + } + /* main read */ + if (rlen) { + ret = part->panic_read(part, off / SECTOR_SIZE, + rlen / SECTOR_SIZE, buf); + if (ret != rlen / SECTOR_SIZE) + return ret; + buf += (rlen / SECTOR_SIZE) * SECTOR_SIZE; + off += (rlen / SECTOR_SIZE) * SECTOR_SIZE; + rlen -= (rlen / SECTOR_SIZE) * SECTOR_SIZE; + } + /* off + len not align to sector */ + if (rlen) { + ret = part->panic_read(part, off / SECTOR_SIZE, 1, sect_buf); + if (ret != 1) + return ret; + memcpy(buf, sect_buf, rlen); + } + return len; +} + +static ssize_t sunxi_panic_write(const char *buf, size_t len, loff_t off) +{ + struct panic_part *part = sunxi_part; + size_t wlen, extra; + ssize_t ret; + char sect_buf[SECTOR_SIZE]; + + if (!part || !part->panic_write) + return -EINVAL; + + if (off > part->sects * SECTOR_SIZE) + return -EINVAL; + + wlen = min_t(size_t, len, part->sects * SECTOR_SIZE - off); + /* off not align to sector */ + extra = off % SECTOR_SIZE; + if (wlen && extra) { + if (!part->panic_read) + return -EINVAL; + + ret = part->panic_read(part, off / SECTOR_SIZE, 1, sect_buf); + if (ret != 1) + return ret; + memcpy(sect_buf + extra, buf, min(SECTOR_SIZE - extra, wlen)); + ret = part->panic_write(part, off / SECTOR_SIZE, 1, sect_buf); + if (ret != 1) + return ret; + + buf += SECTOR_SIZE - extra; + off += SECTOR_SIZE - extra; + wlen -= SECTOR_SIZE - extra; + } + /* main write */ + if (wlen) { + ret = part->panic_write(part, off / SECTOR_SIZE, + wlen / SECTOR_SIZE, buf); + if (ret != wlen / SECTOR_SIZE) + return ret; + buf += (wlen / SECTOR_SIZE) * SECTOR_SIZE; + off += (wlen / SECTOR_SIZE) * SECTOR_SIZE; + wlen -= (wlen / SECTOR_SIZE) * SECTOR_SIZE; + } + /* off + len not align to sector */ + if (wlen) { + if (!part->panic_read) + return -EINVAL; + + ret = part->panic_read(part, off / SECTOR_SIZE, 1, sect_buf); + if (ret != 1) + return ret; + memcpy(sect_buf, buf, wlen); + ret = part->panic_write(part, off / SECTOR_SIZE, 1, sect_buf); + if (ret != 1) + return ret; + } + return len; +} + +struct match_part { + dev_t devt; + size_t start_sect; + size_t sects; +}; +#define MATCH_DEV_RETURN 1024 + +static int match_by_devt(struct device *dev, void *data) +{ + struct match_part *match = (struct match_part *)data; + struct hd_struct *part; + const char *disk_name; + + part = dev_to_part(dev); + disk_name = dev_to_disk(dev)->disk_name; + if (!strncmp(disk_name, "nand", 4) && + (disk_name[4] >= 'a' && disk_name[4] <= 'z')) { + /* (nand) MBR partition table */ + if (match->devt == dev->devt) { + match->sects = part_nr_sects_read(part); + return MATCH_DEV_RETURN; + } + match->start_sect += part_nr_sects_read(part); + } else { + /* GPT partition table */ + if (match->devt == dev->devt) { + match->sects = part_nr_sects_read(part); + match->start_sect = part->start_sect; + return MATCH_DEV_RETURN; + } + } + return 0; +} + +static int parse_part_info(dev_t devt, size_t *start_sect, size_t *sects) +{ + struct match_part part = {0}; + int ret; + + part.devt = devt; + ret = class_for_each_device(&block_class, NULL, &part, match_by_devt); + if (ret != MATCH_DEV_RETURN) + return -ENODEV; + + *start_sect = part.start_sect; + *sects = part.sects; + return 0; +} + +int sunxi_parse_blkdev(char *bdev, int len) +{ + char *tok, *cmdline; + int ret = 0; + + cmdline = kstrdup(saved_command_line, GFP_KERNEL); + if (!cmdline) + return PTR_ERR(cmdline); + + tok = strstr(cmdline, CMDLINE_PREFIX); + if (tok) { + tok = strsep(&tok, " "); + tok += strlen(CMDLINE_PREFIX); + if (bdev && len) + strncpy(bdev, tok, len); + goto free_cmdline; + } + + if (strlen(blkdev)) { + if (bdev && len) + strncpy(bdev, blkdev, len); + goto free_cmdline; + } + + ret = -ENOTBLK; +free_cmdline: + kfree(cmdline); + return ret; +} +EXPORT_SYMBOL(sunxi_parse_blkdev); + +int sunxi_panicpart_init(struct panic_part *part) +{ + char *cmp, bdev[100] = {0}; + size_t start_sect, sects; + int ret; + dev_t devt; + + if (!part->panic_read && !part->panic_write) + return -EINVAL; + + ret = sunxi_parse_blkdev(bdev, 100); + if (ret) + return ret; + + devt = name_to_dev_t(bdev); + if (!devt) + return -ENOTBLK; + + ret = parse_part_info(devt, &start_sect, §s); + if (ret) + return ret; + + if (part->type == SUNXI_FLASH_MMC) + cmp = "/dev/mmcblk"; + else if (part->type == SUNXI_FLASH_NAND) + cmp = "/dev/nand"; + else if (part->type == SUNXI_FLASH_NOR) + cmp = "/dev/mtdblock"; + else + cmp = NULL; + if (!cmp || strncmp(bdev, cmp, strlen(cmp))) + return -EINVAL; + + if (sunxi_part) + return -EBUSY; + + ret = blkoops_add_panic_ops(sunxi_panic_read, sunxi_panic_write); + if (ret) + return ret; + + part->start_sect = start_sect; + part->sects = sects; + part->bdev = kstrdup(bdev, GFP_KERNEL); + sunxi_part = part; + pr_info("add panic partition %s\n", part->bdev); + return 0; +} +EXPORT_SYMBOL(sunxi_panicpart_init); + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("liaoweixiong ");