flash/nor/stm32l4x: lock flash after error

Also add locking after option write, it was missing at all.

Change-Id: I0227c6a74866f0fe8e40aa58616f0b3115ad5af0
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/5361
Tested-by: jenkins
Reviewed-by: Andreas Bolsch <hyphen0break@gmail.com>
Reviewed-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>

1 files changed, 31 insertions, 22 deletions

diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index c8055cd..e9fb77e 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -412,34 +412,39 @@ static int stm32l4_unlock_option_reg(struct flash_bank *bank)
 static int stm32l4_write_option(struct flash_bank *bank, uint32_t reg_offset, uint32_t value, uint32_t mask)
 {
 	uint32_t optiondata;
+	int retval, retval2;
 
-	int retval = stm32l4_read_flash_reg(bank, reg_offset, &optiondata);
+	retval = stm32l4_read_flash_reg(bank, reg_offset, &optiondata);
 	if (retval != ERROR_OK)
 		return retval;
 
 	retval = stm32l4_unlock_reg(bank);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	retval = stm32l4_unlock_option_reg(bank);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	optiondata = (optiondata & ~mask) | (value & mask);
 
 	retval = stm32l4_write_flash_reg(bank, reg_offset, optiondata);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_OPTSTRT);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
+
+err_lock:
+	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK | FLASH_OPTLOCK);
+
 	if (retval != ERROR_OK)
 		return retval;
 
-	return retval;
+	return retval2;
 }
 
 static int stm32l4_protect_check(struct flash_bank *bank)
@@ -489,7 +494,7 @@ static int stm32l4_erase(struct flash_bank *bank, int first, int last)
 {
 	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 	int i;
-	int retval;
+	int retval, retval2;
 
 	assert(first < bank->num_sectors);
 	assert(last < bank->num_sectors);
@@ -501,7 +506,7 @@ static int stm32l4_erase(struct flash_bank *bank, int first, int last)
 
 	retval = stm32l4_unlock_reg(bank);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	/*
 	Sector Erase
@@ -526,20 +531,22 @@ static int stm32l4_erase(struct flash_bank *bank, int first, int last)
 			erase_flags |= i << FLASH_PAGE_SHIFT;
 		retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, erase_flags);
 		if (retval != ERROR_OK)
-			return retval;
+			break;
 
 		retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 		if (retval != ERROR_OK)
-			return retval;
+			break;
 
 		bank->sectors[i].is_erased = 1;
 	}
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+err_lock:
+	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+
 	if (retval != ERROR_OK)
 		return retval;
 
-	return ERROR_OK;
+	return retval2;
 }
 
 static int stm32l4_protect(struct flash_bank *bank, int set, int first, int last)
@@ -690,10 +697,11 @@ static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
 
 	retval = stm32l4_unlock_reg(bank);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	retval = stm32l4_write_block(bank, buffer, offset, count / 8);
 
+err_lock:
 	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
 
 	if (retval != ERROR_OK) {
@@ -924,7 +932,7 @@ static int get_stm32l4_info(struct flash_bank *bank, char *buf, int buf_size)
 
 static int stm32l4_mass_erase(struct flash_bank *bank)
 {
-	int retval;
+	int retval, retval2;
 	struct target *target = bank->target;
 	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 
@@ -940,29 +948,30 @@ static int stm32l4_mass_erase(struct flash_bank *bank)
 
 	retval = stm32l4_unlock_reg(bank);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	/* mass erase flash memory */
 	retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT / 10);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, action);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
+
 	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, action | FLASH_STRT);
 	if (retval != ERROR_OK)
-		return retval;
+		goto err_lock;
 
 	retval = stm32l4_wait_status_busy(bank,  FLASH_ERASE_TIMEOUT);
-	if (retval != ERROR_OK)
-		return retval;
 
-	retval = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+err_lock:
+	retval2 = stm32l4_write_flash_reg(bank, STM32_FLASH_CR, FLASH_LOCK);
+
 	if (retval != ERROR_OK)
 		return retval;
 
-	return ERROR_OK;
+	return retval2;
 }
 
 COMMAND_HANDLER(stm32l4_handle_mass_erase_command)