flash: stm32f1x: Pad odd byte writes early to avoid 16-bit writes

[fw/openocd] / src / flash / nor / stm32f1x.c

diff --git a/src/flash/nor/stm32f1x.c b/src/flash/nor/stm32f1x.c
index d06016a4d849ff3ae46fd88115616cacd642e591..baf6b2752c10ea8ec252cfea03285989fece8f69 100644 (file)
--- a/src/flash/nor/stm32f1x.c
+++ b/src/flash/nor/stm32f1x.c
@@ -724,11 +724,7 @@ static int stm32x_write(struct flash_bank *bank, uint8_t *buffer,
                uint32_t offset, uint32_t count)
 {
        struct target *target = bank->target;
-       uint32_t words_remaining = (count / 2);
-       uint32_t bytes_remaining = (count & 0x00000001);
-       uint32_t address = bank->base + offset;
-       uint32_t bytes_written = 0;
-       int retval;
+       uint8_t *new_buffer = NULL;
 
        if (bank->target->state != TARGET_HALTED) {
                LOG_ERROR("Target not halted");
@@ -736,76 +732,75 @@ static int stm32x_write(struct flash_bank *bank, uint8_t *buffer,
        }
 
        if (offset & 0x1) {
-               LOG_WARNING("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
+               LOG_ERROR("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
                return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
        }
 
+       /* If there's an odd number of bytes, the data has to be padded. Duplicate
+        * the buffer and use the normal code path with a single block write since
+        * it's probably cheaper than to special case the last odd write using
+        * discrete accesses. */
+       if (count & 1) {
+               new_buffer = malloc(count + 1);
+               if (new_buffer == NULL) {
+                       LOG_ERROR("odd number of bytes to write and no memory for padding buffer");
+                       return ERROR_FAIL;
+               }
+               LOG_INFO("odd number of bytes to write, padding with 0xff");
+               buffer = memcpy(new_buffer, buffer, count);
+               buffer[count++] = 0xff;
+       }
+
+       uint32_t words_remaining = count / 2;
+       int retval, retval2;
+
        /* unlock flash registers */
        retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY1);
        if (retval != ERROR_OK)
-               return retval;
+               goto cleanup;
        retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_KEYR), KEY2);
        if (retval != ERROR_OK)
-               return retval;
+               goto cleanup;
 
        retval = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_PG);
        if (retval != ERROR_OK)
-               return retval;
+               goto cleanup;
 
-       /* multiple half words (2-byte) to be programmed? */
-       if (words_remaining > 0) {
-               /* try using a block write */
-               retval = stm32x_write_block(bank, buffer, offset, words_remaining);
-               if (retval != ERROR_OK) {
-                       if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
-                               /* if block write failed (no sufficient working area),
-                                * we use normal (slow) single dword accesses */
-                               LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
-                       }
-               } else {
-                       buffer += words_remaining * 2;
-                       address += words_remaining * 2;
-                       words_remaining = 0;
-               }
-       }
+       /* try using a block write */
+       retval = stm32x_write_block(bank, buffer, offset, words_remaining);
 
-       if ((retval != ERROR_OK) && (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE))
-               goto reset_pg_and_lock;
+       if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+               /* if block write failed (no sufficient working area),
+                * we use normal (slow) single halfword accesses */
+               LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
 
-       while (words_remaining > 0) {
-               uint16_t value;
-               memcpy(&value, buffer + bytes_written, sizeof(uint16_t));
+               while (words_remaining > 0) {
+                       uint16_t value;
+                       memcpy(&value, buffer, sizeof(uint16_t));
 
-               retval = target_write_u16(target, address, value);
-               if (retval != ERROR_OK)
-                       goto reset_pg_and_lock;
+                       retval = target_write_u16(target, bank->base + offset, value);
+                       if (retval != ERROR_OK)
+                               goto reset_pg_and_lock;
 
-               retval = stm32x_wait_status_busy(bank, 5);
-               if (retval != ERROR_OK)
-                       goto reset_pg_and_lock;
+                       retval = stm32x_wait_status_busy(bank, 5);
+                       if (retval != ERROR_OK)
+                               goto reset_pg_and_lock;
 
-               bytes_written += 2;
-               words_remaining--;
-               address += 2;
+                       words_remaining--;
+                       buffer += 2;
+                       offset += 2;
+               }
        }
 
-       if (bytes_remaining) {
-               uint16_t value = 0xffff;
-               memcpy(&value, buffer + bytes_written, bytes_remaining);
-
-               retval = target_write_u16(target, address, value);
-               if (retval != ERROR_OK)
-                       goto reset_pg_and_lock;
-
-               retval = stm32x_wait_status_busy(bank, 5);
-               if (retval != ERROR_OK)
-                       goto reset_pg_and_lock;
-       }
+reset_pg_and_lock:
+       retval2 = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
+       if (retval == ERROR_OK)
+               retval = retval2;
 
-       return target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
+cleanup:
+       if (new_buffer)
+               free(new_buffer);
 
-reset_pg_and_lock:
-       target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
        return retval;
 }