diff --git a/src/flash/nor/stm32f1x.c b/src/flash/nor/stm32f1x.c index d06016a4d..baf6b2752 100644 --- 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_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, sizeof(uint16_t)); + + 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; + + words_remaining--; + buffer += 2; + offset += 2; } } - if ((retval != ERROR_OK) && (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE)) - goto reset_pg_and_lock; - - while (words_remaining > 0) { - uint16_t value; - memcpy(&value, buffer + bytes_written, sizeof(uint16_t)); - - 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; - - bytes_written += 2; - words_remaining--; - address += 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; - } - - return target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK); - reset_pg_and_lock: - target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK); + retval2 = target_write_u32(target, stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK); + if (retval == ERROR_OK) + retval = retval2; + +cleanup: + if (new_buffer) + free(new_buffer); + return retval; }