1 files changed, 77 insertions, 0 deletions
diff --git a/stmhal/flash.c b/stmhal/flash.c
index 6ed3b04324..ce16fe271b 100644
--- a/stmhal/flash.c
+++ b/stmhal/flash.c
@@ -49,7 +49,81 @@ uint32_t flash_get_sector_info(uint32_t addr, uint32_t *start_addr, uint32_t *si
     return 0;
 }
 
+void flash_erase(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
+    // check there is something to write
+    if (num_word32 == 0) {
+        return;
+    }
+
+    // unlock
+    HAL_FLASH_Unlock();
+
+    // Clear pending flags (if any)
+    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |
+                           FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR|FLASH_FLAG_PGSERR);
+
+    // erase the sector(s)
+    FLASH_EraseInitTypeDef EraseInitStruct;
+    EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
+    EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3; // voltage range needs to be 2.7V to 3.6V
+    EraseInitStruct.Sector = flash_get_sector_info(flash_dest, NULL, NULL);
+    EraseInitStruct.NbSectors = flash_get_sector_info(flash_dest + 4 * num_word32 - 1, NULL, NULL) - EraseInitStruct.Sector + 1;
+    uint32_t SectorError = 0;
+    if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) {
+        // error occurred during sector erase
+        HAL_FLASH_Lock(); // lock the flash
+        return;
+    }
+}
+
+/*
+// erase the sector using an interrupt
+void flash_erase_it(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
+    // check there is something to write
+    if (num_word32 == 0) {
+        return;
+    }
+
+    // unlock
+    HAL_FLASH_Unlock();
+
+    // Clear pending flags (if any)
+    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |
+                           FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR|FLASH_FLAG_PGSERR);
+
+    // erase the sector(s)
+    FLASH_EraseInitTypeDef EraseInitStruct;
+    EraseInitStruct.TypeErase = TYPEERASE_SECTORS;
+    EraseInitStruct.VoltageRange = VOLTAGE_RANGE_3; // voltage range needs to be 2.7V to 3.6V
+    EraseInitStruct.Sector = flash_get_sector_info(flash_dest, NULL, NULL);
+    EraseInitStruct.NbSectors = flash_get_sector_info(flash_dest + 4 * num_word32 - 1, NULL, NULL) - EraseInitStruct.Sector + 1;
+    if (HAL_FLASHEx_Erase_IT(&EraseInitStruct) != HAL_OK) {
+        // error occurred during sector erase
+        HAL_FLASH_Lock(); // lock the flash
+        return;
+    }
+}
+*/
+
 void flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
+    // program the flash word by word
+    for (int i = 0; i < num_word32; i++) {
+        if (HAL_FLASH_Program(TYPEPROGRAM_WORD, flash_dest, *src) != HAL_OK) {
+            // error occurred during flash write
+            HAL_FLASH_Lock(); // lock the flash
+            return;
+        }
+        flash_dest += 4;
+        src += 1;
+    }
+
+    // lock the flash
+    HAL_FLASH_Lock();
+}
+
+/*
+ use erase, then write
+void flash_erase_and_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
     // check there is something to write
     if (num_word32 == 0) {
         return;
@@ -71,6 +145,7 @@ void flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32)
     uint32_t SectorError = 0;
     if (HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError) != HAL_OK) {
         // error occurred during sector erase
+        HAL_FLASH_Lock(); // lock the flash
         return;
     }
 
@@ -78,6 +153,7 @@ void flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32)
     for (int i = 0; i < num_word32; i++) {
         if (HAL_FLASH_Program(TYPEPROGRAM_WORD, flash_dest, *src) != HAL_OK) {
             // error occurred during flash write
+            HAL_FLASH_Lock(); // lock the flash
             return;
         }
         flash_dest += 4;
@@ -87,3 +163,4 @@ void flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32)
     // lock the flash
     HAL_FLASH_Lock();
 }
+*/