Fix FlashIAP

Author: Jan Kamidra

targets/TARGET_STM/TARGET_STM32U0/flash_api.c

@@ -30,9 +30,6 @@ static uint32_t GetPage(uint32_t Addr)
     if (Addr < (FLASH_BASE + FLASH_BANK_SIZE)) {
         /* Bank 1 */
         page = (Addr - FLASH_BASE) / FLASH_PAGE_SIZE;
-    } else {
-        /* Bank 2 */
-        page = (Addr - (FLASH_BASE + FLASH_BANK_SIZE)) / FLASH_PAGE_SIZE;
     }
 
     return page;
@@ -49,8 +46,6 @@ static uint32_t GetBank(uint32_t Addr)
 
     if (Addr < (FLASH_BASE + FLASH_BANK_SIZE)) {
         bank = FLASH_BANK_1;
-    } else {
-        bank = FLASH_BANK_2;
     }
 
     return bank;
@@ -93,19 +88,14 @@ int32_t flash_erase_sector(flash_t *obj, uint32_t address)
         return -1;
     }
 
-    if (HAL_ICACHE_Disable() != HAL_OK)
-    {
-        return -1;
-    }
-
     if (HAL_FLASH_Unlock() != HAL_OK) {
         return -1;
     }
 
     core_util_critical_section_enter();
 
     /* Clear error programming flags */
-    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
+    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR);
 
     /* Get the 1st page to erase */
     FirstPage = GetPage(address);
@@ -129,11 +119,6 @@ int32_t flash_erase_sector(flash_t *obj, uint32_t address)
         return -1;
     }
 
-    if (HAL_ICACHE_Enable() != HAL_OK)
-    {
-        return -1;
-    }
-
     return 0;
 }
 
@@ -162,36 +147,47 @@ int32_t flash_program_page(flash_t *obj, uint32_t address,
         return -1;
     }
 
-    if (HAL_ICACHE_Disable() != HAL_OK)
-    {
-        return -1;
-    }
-
     if (HAL_FLASH_Unlock() != HAL_OK) {
         return -1;
     }
 
     /* Clear error programming flags */
-    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
+    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR);
 
     /* Program the user Flash area word by word */
     StartAddress = address;
 
-    while ((address < (StartAddress + size)) && (status == 0)) {
-        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_QUADWORD, address, ((uint32_t) data)) == HAL_OK) {
-            address = address + 16;
-            data = data + 16;
-        } else {
-            status = -1;
+    /*  HW needs an aligned address to program flash, which data
+     *  parameters doesn't ensure  */
+    if ((uint32_t) data % 8 != 0) {
+        volatile uint64_t data64;
+        while ((address < (StartAddress + size)) && (status == 0)) {
+            for (uint8_t i = 0; i < 8; i++) {
+                *(((uint8_t *) &data64) + i) = *(data + i);
+            }
+
+            if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address, data64)
+                    == HAL_OK) {
+                address = address + 8;
+                data = data + 8;
+            } else {
+                status = -1;
+            }
+        }
+    } else { /*  case where data is aligned, so let's avoid any copy */
+        while ((address < (StartAddress + size)) && (status == 0)) {
+            if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address,
+                                  *((uint64_t *) data))
+                    == HAL_OK) {
+                address = address + 8;
+                data = data + 8;
+            } else {
+                status = -1;
+            }
         }
     }
 
-    if (HAL_FLASH_Unlock() != HAL_OK) {
-        return -1;
-    }
-
-    if (HAL_ICACHE_Enable() != HAL_OK)
-    {
+    if (HAL_FLASH_Lock() != HAL_OK) {
         return -1;
     }