1 files changed, 93 insertions, 0 deletions

diff --git a/ports/stm32/flash.c b/ports/stm32/flash.c
index 499129a6f3..d399ece866 100644
--- a/ports/stm32/flash.c
+++ b/ports/stm32/flash.c
@@ -29,6 +29,21 @@
 #include "py/mphal.h"
 #include "flash.h"
 
+#if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+// See WB55 specific documentation in AN5289 Rev 3, and in particular, Figure 10.
+
+#include "rfcore.h"
+#include "stm32wbxx_ll_hsem.h"
+
+// Protects all flash registers.
+#define SEMID_FLASH_REGISTERS (2)
+// Used by CPU1 to prevent CPU2 from writing/erasing data in flash memory.
+#define SEMID_FLASH_CPU1 (6)
+// Used by CPU2 to prevent CPU1 from writing/erasing data in flash memory.
+#define SEMID_FLASH_CPU2 (7)
+
+#endif
+
 typedef struct {
     uint32_t base_address;
     uint32_t sector_size;
@@ -181,9 +196,27 @@ int flash_erase(uint32_t flash_dest, uint32_t num_word32) {
         return 0;
     }
 
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Acquire lock on the flash peripheral.
+    while (LL_HSEM_1StepLock(HSEM, SEMID_FLASH_REGISTERS)) {
+    }
+    #endif
+
     // Unlock the flash for erase.
     HAL_FLASH_Unlock();
 
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Tell the HCI controller stack we're starting an erase, so it
+    // avoids radio activity for a while.
+    rfcore_start_flash_erase();
+    // Wait for PES.
+    while (LL_FLASH_IsActiveFlag_OperationSuspended()) {
+    }
+    // Wait for flash lock.
+    while (LL_HSEM_1StepLock(HSEM, SEMID_FLASH_CPU2)) {
+    }
+    #endif
+
     // Clear pending flags (if any) and set up EraseInitStruct.
 
     FLASH_EraseInitTypeDef EraseInitStruct;
@@ -233,9 +266,23 @@ int flash_erase(uint32_t flash_dest, uint32_t num_word32) {
     uint32_t SectorError = 0;
     HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError);
 
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Release flash lock.
+    while (__HAL_FLASH_GET_FLAG(FLASH_FLAG_CFGBSY)) {
+    }
+    LL_HSEM_ReleaseLock(HSEM, SEMID_FLASH_CPU2, 0);
+    // Tell HCI controller that erase is over.
+    rfcore_end_flash_erase();
+    #endif
+
     // Lock the flash after erase.
     HAL_FLASH_Lock();
 
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Release lock on the flash peripheral.
+    LL_HSEM_ReleaseLock(HSEM, SEMID_FLASH_REGISTERS, 0);
+    #endif
+
     return mp_hal_status_to_neg_errno(status);
 }
 
@@ -269,9 +316,21 @@ void flash_erase_it(uint32_t flash_dest, uint32_t num_word32) {
 */
 
 int flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Acquire lock on the flash peripheral.
+    while (LL_HSEM_1StepLock(HSEM, SEMID_FLASH_REGISTERS)) {
+    }
+    #endif
+
     // Unlock the flash for write.
     HAL_FLASH_Unlock();
 
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Wait for PES.
+    while (LL_FLASH_IsActiveFlag_OperationSuspended()) {
+    }
+    #endif
+
     HAL_StatusTypeDef status = HAL_OK;
 
     #if defined(STM32L4) || defined(STM32WB)
@@ -279,7 +338,22 @@ int flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
     // program the flash uint64 by uint64
     for (int i = 0; i < num_word32 / 2; i++) {
         uint64_t val = *(uint64_t *)src;
+
+        #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+        // Wait for flash lock.
+        while (LL_HSEM_1StepLock(HSEM, SEMID_FLASH_CPU2)) {
+        }
+        #endif
+
         status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, flash_dest, val);
+
+        #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+        // Release flash lock.
+        LL_HSEM_ReleaseLock(HSEM, SEMID_FLASH_CPU2, 0);
+        while (__HAL_FLASH_GET_FLAG(FLASH_FLAG_CFGBSY)) {
+        }
+        #endif
+
         if (status != HAL_OK) {
             num_word32 = 0; // don't write any odd word after this loop
             break;
@@ -290,7 +364,21 @@ int flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
     if ((num_word32 & 0x01) == 1) {
         uint64_t val = *(uint64_t *)flash_dest;
         val = (val & 0xffffffff00000000uL) | (*src);
+
+        #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+        // Wait for flash lock.
+        while (LL_HSEM_1StepLock(HSEM, SEMID_FLASH_CPU2)) {
+        }
+        #endif
+
         status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, flash_dest, val);
+
+        #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+        // Release flash lock.
+        LL_HSEM_ReleaseLock(HSEM, SEMID_FLASH_CPU2, 0);
+        while (__HAL_FLASH_GET_FLAG(FLASH_FLAG_CFGBSY)) {
+        }
+        #endif
     }
 
     #elif defined(STM32H7)
@@ -322,6 +410,11 @@ int flash_write(uint32_t flash_dest, const uint32_t *src, uint32_t num_word32) {
     // Lock the flash after write.
     HAL_FLASH_Lock();
 
+    #if MICROPY_HW_STM32WB_FLASH_SYNCRONISATION
+    // Release lock on the flash peripheral.
+    LL_HSEM_ReleaseLock(HSEM, SEMID_FLASH_REGISTERS, 0);
+    #endif
+
     return mp_hal_status_to_neg_errno(status);
 }