flash/nor/stm32l4x: lock flash after error

[openocd.git] / src / flash / nor / stm32l4x.c

diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index c8055cd9ca473fdd2e99475d4ce97a5a08a03d0f..e9fb77e035c52afda149376ebae78bf140283fdb 100644 (file)
--- 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)