diff --git a/src/include/target.h b/src/include/target.h index bcabc23e..f49bc58b 100644 --- a/src/include/target.h +++ b/src/include/target.h @@ -131,6 +131,8 @@ struct target_flash { flash_done_func done; target *t; struct target_flash *next; + int align; + uint8_t erased; }; struct target_s { diff --git a/src/target.c b/src/target.c index 6b05deb6..3ab59bc1 100644 --- a/src/target.c +++ b/src/target.c @@ -189,7 +189,15 @@ int target_flash_write(target *t, while (len) { struct target_flash *f = flash_for_addr(t, dest); size_t tmplen = MIN(len, f->length - (dest % f->length)); - ret |= f->write(f, dest, src, tmplen); + if (f->align > 1) { + uint32_t offset = dest % f->align; + uint8_t data[ALIGN(offset + len, f->align)]; + memset(data, f->erased, sizeof(data)); + memcpy((uint8_t *)data + offset, src, len); + ret |= f->write(f, dest - offset, data, sizeof(data)); + } else { + ret |= f->write(f, dest, src, tmplen); + } src += tmplen; len -= tmplen; }