diff --git a/stm-firmware/updater/ram-code/flash-writer.c b/stm-firmware/updater/ram-code/flash-writer.c index e8498b2..c5782ba 100644 --- a/stm-firmware/updater/ram-code/flash-writer.c +++ b/stm-firmware/updater/ram-code/flash-writer.c @@ -54,17 +54,43 @@ int flash_writer_write_to_memory(void *dest, const void *src, uint32_t size) uint32_t *word_dest_ptr; const char *char_src_ptr; char *char_dest_ptr; + uint32_t pre_byte_count; flash_writer_enable_access(); while (flash_op_busy()); - /* Do the full words */ + /* Number of full words to program */ full_word_cnt = size / 4u; byte_cnt = size % 4; word_dest_ptr = dest; word_src_ptr = src; + /* Do the first bytes, in case the destination is not word aligned */ + pre_byte_count = (4 - ((uint32_t)dest % 4u)) % 4; + if (pre_byte_count) { + FLASH->CR = 0u; + FLASH->CR |= FLASH_CR_PG; + + char_src_ptr = src; + char_dest_ptr = dest; + + /* Write bytes to memory until we hit the next word aligned address */ + for (idx = 0; idx < pre_byte_count; idx++) { + *(char_dest_ptr++) = *(char_src_ptr++); + } + + /* Correct the word addresses set above */ + full_word_cnt = (size - pre_byte_count) / 4u; + byte_cnt = (size - pre_byte_count) % 4; + word_dest_ptr = (uint32_t *)char_dest_ptr; + word_src_ptr = (uint32_t *)char_src_ptr; + + while(flash_op_busy()); + FLASH->CR = 0u; + } + + /* Do the full word flash write */ if (full_word_cnt) { FLASH->CR = FLASH_CR_PSIZE_1; FLASH->CR |= FLASH_CR_PG; @@ -76,7 +102,7 @@ int flash_writer_write_to_memory(void *dest, const void *src, uint32_t size) } while (flash_op_busy()); - FLASH->CR = 0; + FLASH->CR = 0u; } /* write remaining bytes */ @@ -84,7 +110,7 @@ int flash_writer_write_to_memory(void *dest, const void *src, uint32_t size) char_src_ptr = (char *)word_src_ptr; char_dest_ptr = (char *)word_dest_ptr; - FLASH->CR = 0; + FLASH->CR = 0u; FLASH->CR |= FLASH_CR_PG; for (idx = 0; idx < byte_cnt; idx++) { @@ -94,7 +120,7 @@ int flash_writer_write_to_memory(void *dest, const void *src, uint32_t size) } while (flash_op_busy()); - FLASH->CR = 0; + FLASH->CR = 0u; } lock_flash_cr();