diff --git a/cpu/native/mtd/mtd_native.c b/cpu/native/mtd/mtd_native.c index 208ab3e5ee17..aa549c12ef61 100644 --- a/cpu/native/mtd/mtd_native.c +++ b/cpu/native/mtd/mtd_native.c @@ -27,6 +27,8 @@ #define ENABLE_DEBUG 0 #include "debug.h" +#define MIN(a, b) ((a) > (b) ? (b) : (a)) + static int _init(mtd_dev_t *dev) { mtd_native_dev_t *_dev = (mtd_native_dev_t*) dev; @@ -103,6 +105,41 @@ static int _write(mtd_dev_t *dev, const void *buff, uint32_t addr, uint32_t size return 0; } +static int _write_page(mtd_dev_t *dev, const void *buff, uint32_t page, uint32_t offset, + uint32_t size) +{ + mtd_native_dev_t *_dev = (mtd_native_dev_t*) dev; + uint32_t addr = page * dev->page_size + offset; + + DEBUG("mtd_native: write from page %" PRIx32 ", offset 0x%" PRIx32 " count %" PRIu32 "\n", + page, offset, size); + + if (page > dev->sector_count * dev->pages_per_sector) { + return -EOVERFLOW; + } + + if (offset > dev->page_size) { + return -EOVERFLOW; + } + + uint32_t remaining = dev->page_size - offset; + size = MIN(remaining, size); + + FILE *f = real_fopen(_dev->fname, "r+"); + if (!f) { + return -EIO; + } + real_fseek(f, addr, SEEK_SET); + for (size_t i = 0; i < size; i++) { + uint8_t c = real_fgetc(f); + real_fseek(f, -1, SEEK_CUR); + real_fputc(c & ((uint8_t*)buff)[i], f); + } + real_fclose(f); + + return size; +} + static int _erase(mtd_dev_t *dev, uint32_t addr, uint32_t size) { mtd_native_dev_t *_dev = (mtd_native_dev_t*) dev; @@ -144,6 +181,7 @@ const mtd_desc_t native_flash_driver = { .read = _read, .power = _power, .write = _write, + .write_page = _write_page, .erase = _erase, .init = _init, };