diff --git a/drivers/mtd_mapper/mtd_mapper.c b/drivers/mtd_mapper/mtd_mapper.c index 59d9eb01c9..76576413e6 100644 --- a/drivers/mtd_mapper/mtd_mapper.c +++ b/drivers/mtd_mapper/mtd_mapper.c @@ -52,6 +52,11 @@ static uint32_t _byte_offset(mtd_mapper_region_t *region) region->sector; } +static uint32_t _page_offset(mtd_mapper_region_t *region) +{ + return region->mtd.pages_per_sector * region->sector; +} + static int _init_target(mtd_mapper_region_t *region) { mtd_mapper_parent_t *parent = region->parent; @@ -101,6 +106,19 @@ static int _write(mtd_dev_t *mtd, const void *src, uint32_t addr, return res; } +static int _write_page(mtd_dev_t *mtd, const void *src, uint32_t page, + uint32_t offset, uint32_t count) +{ + mtd_mapper_region_t *region = container_of(mtd, mtd_mapper_region_t, mtd); + + _lock(region); + int res = mtd_write_page(region->parent->mtd, src, + page + _page_offset(region), + offset, count); + _unlock(region); + return res; +} + static int _read(mtd_dev_t *mtd, void *dest, uint32_t addr, uint32_t count) { mtd_mapper_region_t *region = container_of(mtd, mtd_mapper_region_t, mtd); @@ -115,6 +133,19 @@ static int _read(mtd_dev_t *mtd, void *dest, uint32_t addr, uint32_t count) return res; } +static int _read_page(mtd_dev_t *mtd, void *dest, uint32_t page, + uint32_t offset, uint32_t count) +{ + mtd_mapper_region_t *region = container_of(mtd, mtd_mapper_region_t, mtd); + + _lock(region); + int res = mtd_read_page(region->parent->mtd, dest, + page + _page_offset(region), + offset, count); + _unlock(region); + return res; +} + static int _erase(mtd_dev_t *mtd, uint32_t addr, uint32_t count) { mtd_mapper_region_t *region = container_of(mtd, mtd_mapper_region_t, mtd); @@ -129,9 +160,22 @@ static int _erase(mtd_dev_t *mtd, uint32_t addr, uint32_t count) return res; } +static int _erase_sector(mtd_dev_t *mtd, uint32_t sector, uint32_t count) +{ + mtd_mapper_region_t *region = container_of(mtd, mtd_mapper_region_t, mtd); + + _lock(region); + int res = mtd_erase_sector(region->parent->mtd, region->sector + sector, count); + _unlock(region); + return res; +} + const mtd_desc_t mtd_mapper_driver = { .init = _init, .read = _read, + .read_page = _read_page, .write = _write, + .write_page = _write_page, .erase = _erase, + .erase_sector = _erase_sector, }; diff --git a/tests/mtd_mapper/main.c b/tests/mtd_mapper/main.c index a756e236c2..5b073a6ab8 100644 --- a/tests/mtd_mapper/main.c +++ b/tests/mtd_mapper/main.c @@ -42,6 +42,8 @@ #define REGION_FLASH_SIZE MEMORY_SIZE / 2 +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) + static uint8_t _dummy_memory[MEMORY_SIZE]; static uint8_t _buffer[PAGE_SIZE]; @@ -65,6 +67,25 @@ static int _read(mtd_dev_t *dev, void *buff, uint32_t addr, uint32_t size) return 0; } +static int _read_page(mtd_dev_t *dev, void *buff, uint32_t page, uint32_t offset, uint32_t size) +{ + uint32_t addr = page * dev->page_size + offset; + + if (page >= dev->sector_count * dev->pages_per_sector) { + return -EOVERFLOW; + } + + if (offset > dev->page_size) { + return -EOVERFLOW; + } + + size = MIN(dev->page_size - offset, size); + + memcpy(buff, _dummy_memory + addr, size); + + return size; +} + static int _write(mtd_dev_t *dev, const void *buff, uint32_t addr, uint32_t size) { @@ -81,6 +102,25 @@ static int _write(mtd_dev_t *dev, const void *buff, uint32_t addr, return 0; } +static int _write_page(mtd_dev_t *dev, const void *buff, uint32_t page, uint32_t offset, uint32_t size) +{ + uint32_t addr = page * dev->page_size + offset; + + if (page >= dev->sector_count * dev->pages_per_sector) { + return -EOVERFLOW; + } + + if (offset > dev->page_size) { + return -EOVERFLOW; + } + + size = MIN(dev->page_size - offset, size); + + memcpy(_dummy_memory + addr, buff, size); + + return size; +} + static int _erase(mtd_dev_t *dev, uint32_t addr, uint32_t size) { (void)dev; @@ -99,6 +139,20 @@ static int _erase(mtd_dev_t *dev, uint32_t addr, uint32_t size) return 0; } +static int _erase_sector(mtd_dev_t *dev, uint32_t sector, uint32_t count) +{ + uint32_t addr = sector * dev->page_size * dev->pages_per_sector; + + if (sector + count > dev->sector_count) { + return -EOVERFLOW; + } + + memset(_dummy_memory + addr, 0xff, + count * dev->page_size * dev->pages_per_sector); + + return 0; +} + static int _power(mtd_dev_t *dev, enum mtd_power_state power) { (void)dev; @@ -112,6 +166,9 @@ static const mtd_desc_t driver = { .write = _write, .erase = _erase, .power = _power, + .read_page = _read_page, + .write_page = _write_page, + .erase_sector = _erase_sector, }; static mtd_dev_t dev = {