BPI-CM9-Linux/aml-comp/uboot/bl33/v2015/drivers/mtd/mtdblock.c

// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
/*
 * Copyright (c) 2019 Amlogic, Inc. All rights reserved.
 */

#include <common.h>
#include <part.h>
#include <linux/mtd/mtd.h>
#include <linux/mtd/partitions.h>
#include <jffs2/load_kernel.h>

static loff_t offset_skip_bad(struct mtd_info *mtd, loff_t offset)
{
	struct part_info *part = get_partition_by_offset(mtd, offset);
	loff_t block_index = part->offset >> mtd->erasesize_shift;
	loff_t max_blocks = (part->offset + part->size) >> mtd->erasesize_shift;
	loff_t lba = part->offset;
	loff_t bad_blocks_size = 0;

	while (lba < offset && block_index < max_blocks) {
		if (mtd_block_isbad(mtd, block_index << mtd->erasesize_shift)) {
			bad_blocks_size += mtd->erasesize;
			block_index++;
			continue;
		}
		lba += mtd->erasesize;
		block_index++;
	}

	return offset + bad_blocks_size;
}

static unsigned long mtd_block_read(int dev, lbaint_t start, lbaint_t blkcnt,
			   void *dst)
{
	block_dev_desc_t *block_dev = mtd_get_block_dev(dev);
	struct mtd_info *mtd = block_desc_to_mtd(block_dev);
	unsigned int sect_size = block_dev->blksz;
	lbaint_t cur = (offset_skip_bad(mtd, (start * sect_size)) / sect_size);
	unsigned long read_cnt = 0;
	int ret;
	loff_t offset = cur * sect_size;
	size_t retlen;
	loff_t bad_blocks_size = 0;

	while (read_cnt < blkcnt) {
		if (mtd_block_isbad(mtd, offset & ~(mtd->erasesize - 1))) {
			printf("Skipping bad block 0x%08llx\n",
				offset & ~(mtd->erasesize - 1));
			offset += mtd->erasesize;
			bad_blocks_size += mtd->erasesize;
			continue;
		}

		ret = mtd_read(mtd, offset, sect_size, &retlen, dst);
		if (ret)
			return ret;

		if (retlen != sect_size) {
			pr_err("mtdblock: failed to read block 0x" LBAF "\n", cur);
			return -EIO;
		}

		cur++;
		offset = cur * sect_size + bad_blocks_size;
		dst += sect_size;
		read_cnt++;
	}

	return read_cnt;
}

static int mtd_erase_write(struct mtd_info *mtd, uint64_t start, const void *src)
{
	int ret;
	size_t retlen;
	struct erase_info erase = { 0 };

	erase.mtd = mtd;
	erase.addr = start;
	erase.len = mtd->erasesize;

	ret = mtd_erase(mtd, &erase);
	if (ret)
		return ret;

	ret = mtd_write(mtd, start, mtd->erasesize, &retlen, src);
	if (ret)
		return ret;

	if (retlen != mtd->erasesize) {
		pr_err("mtdblock: failed to write block at 0x%llx\n", start);
		return -EIO;
	}

	return 0;
}

static unsigned long mtd_block_write(int dev, lbaint_t start, lbaint_t blkcnt,
			   const void *src)
{
	block_dev_desc_t *block_dev = mtd_get_block_dev(dev);
	struct mtd_info *mtd = block_desc_to_mtd(block_dev);
	unsigned int sect_size = block_dev->blksz;
	lbaint_t cur = (offset_skip_bad(mtd, (start * sect_size)) / sect_size);
	lbaint_t blocks_todo = blkcnt;
	loff_t bad_blocks_size = 0;
	unsigned long write_cnt = 0;
	u8 *buf;
	int ret = 0;

	buf = malloc(mtd->erasesize);
	if (!buf)
		return -ENOMEM;

	while (blocks_todo > 0) {
		loff_t sect_start = cur * sect_size + bad_blocks_size;
		loff_t erase_start = ALIGN_DOWN(sect_start, mtd->erasesize);
		u32 offset = sect_start - erase_start;
		size_t cur_size = min_t(size_t,  mtd->erasesize - offset,
					blocks_todo * sect_size);
		lbaint_t written;

		if (mtd_block_isbad(mtd, erase_start)) {
			printf("Skipping bad block 0x%08llx\n", erase_start);
			bad_blocks_size += mtd->erasesize;
			continue;
		}

		size_t retlen;

		ret = mtd_read(mtd, erase_start, mtd->erasesize, &retlen, buf);
		if (ret)
			goto out;

		if (retlen != mtd->erasesize) {
			pr_err("mtdblock: failed to read block 0x" LBAF "\n", cur);
			ret = -EIO;
			goto out;
		}

		memcpy(buf + offset, src, cur_size);

		ret = mtd_erase_write(mtd, erase_start, buf);
		if (ret)
			goto out;

		written = cur_size / sect_size;

		blocks_todo -= written;
		cur += written;
		src += cur_size;
		write_cnt += written;
	}

out:
	free(buf);

	if (ret)
		return ret;

	return write_cnt;
}

void mtd_block_device_init(struct mtd_info *mtd, block_dev_desc_t *block_dev, int dev)
{
	int block;

	block_dev->if_type = IF_TYPE_MTD;
	block_dev->part_type = PART_TYPE_MTD;
	block_dev->dev = dev;
	block_dev->blksz = 512;
	block_dev->lba = mtd->size;

	block_dev->block_read = mtd_block_read;
	block_dev->block_write = mtd_block_write;

	if (mtd_can_have_bb(mtd)) {
		for (block = 0; block < mtd->size >> mtd->erasesize_shift; block++) {
			if (mtd_block_isbad(mtd, block << mtd->erasesize_shift))
				block_dev->lba -= (mtd->erasesize >> block_dev->blksz);
		}
	}

	if (mtd_type_is_nand(mtd))
		pr_warn("MTD device '%s' is NAND, please use UBI devices instead\n",
			mtd->name);
}