diff options
| -rw-r--r-- | src/flash/nor/cfi.c | 69 |
1 files changed, 15 insertions, 54 deletions
diff --git a/src/flash/nor/cfi.c b/src/flash/nor/cfi.c index a6165c6f..b2d184d4 100644 --- a/src/flash/nor/cfi.c +++ b/src/flash/nor/cfi.c @@ -1838,7 +1838,7 @@ static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, struct cfi_flash_bank *cfi_info = bank->driver_priv; struct target *target = bank->target; uint32_t address = bank->base + offset; /* address of first byte to be programmed */ - uint32_t write_p, copy_p; + uint32_t write_p; int align; /* number of unaligned bytes */ int blk_count; /* number of bus_width bytes for block copy */ uint8_t current_word[CFI_MAX_BUS_WIDTH * 4]; /* word (bus_width size) currently being programmed */ @@ -1863,46 +1863,18 @@ static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, { LOG_INFO("Fixup %d unaligned head bytes", align); - for (i = 0; i < bank->bus_width; i++) - current_word[i] = 0; - copy_p = write_p; - - /* copy bytes before the first write address */ - for (i = 0; i < align; ++i, ++copy_p) - { - uint8_t byte; - /* FIXME: access flash at bus_width size */ - if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK) - { - return retval; - } - cfi_add_byte(bank, current_word, byte); - } - - /* add bytes from the buffer */ - for (; (i < bank->bus_width) && (count > 0); i++) - { - cfi_add_byte(bank, current_word, *buffer++); - count--; - copy_p++; - } + /* read a complete word from flash */ + if ((retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word)) != ERROR_OK) + return retval; - /* if the buffer is already finished, copy bytes after the last write address */ - for (; (count == 0) && (i < bank->bus_width); ++i, ++copy_p) - { - uint8_t byte; - /* FIXME: access flash at bus_width size */ - if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK) - { - return retval; - } - cfi_add_byte(bank, current_word, byte); - } + /* replace only bytes that must be written */ + for (i = align; (i < bank->bus_width) && (count > 0); i++, count--) + current_word[i] = *buffer++; retval = cfi_write_word(bank, current_word, write_p); if (retval != ERROR_OK) return retval; - write_p = copy_p; + write_p += bank->bus_width; } /* handle blocks of bus_size aligned bytes */ @@ -1995,25 +1967,14 @@ static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, { LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count); - copy_p = write_p; - for (i = 0; i < bank->bus_width; i++) - current_word[i] = 0; + /* read a complete word from flash */ + if ((retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word)) != ERROR_OK) + return retval; + + /* replace only bytes that must be written */ + for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--) + current_word[i] = *buffer++; - for (i = 0; (i < bank->bus_width) && (count > 0); ++i, ++copy_p) - { - cfi_add_byte(bank, current_word, *buffer++); - count--; - } - for (; i < bank->bus_width; ++i, ++copy_p) - { - uint8_t byte; - /* FIXME: access flash at bus_width size */ - if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK) - { - return retval; - } - cfi_add_byte(bank, current_word, byte); - } retval = cfi_write_word(bank, current_word, write_p); if (retval != ERROR_OK) return retval; |