From 7f2dcc30460b37165882a996f4130749cdd055bd Mon Sep 17 00:00:00 2001
From: Satoshi Togawa <togawa@develcb210.centurysys.co.jp>
Date: Sun, 26 Jan 2020 12:15:37 +0900
Subject: [PATCH] STM32L4: Add workaround of data cache corruption on RWW.

Some STM32L4 chips has eratta "Data cache might be corrupted during Flash Read While Write operation". This is also in STM32, and arch/arm/src/stm32/stm32f20xxf40xx_flash.c has workaround.

To enable this workaround, define CONFIG_STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW.
---
 arch/arm/src/stm32l4/Kconfig         |  9 +++++++
 arch/arm/src/stm32l4/stm32l4_flash.c | 40 ++++++++++++++++++++++++++--
 2 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/arch/arm/src/stm32l4/Kconfig b/arch/arm/src/stm32l4/Kconfig
index d5e5bda9e4ff0..3b02a978e1a79 100644
--- a/arch/arm/src/stm32l4/Kconfig
+++ b/arch/arm/src/stm32l4/Kconfig
@@ -1657,6 +1657,15 @@ config STM32L4_FLASH_PREFETCH
 	---help---
 	Enable FLASH prefetch
 
+config STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW
+	bool "Workaround for FLASH data cache corruption"
+	default n
+	depends on STM32L4_STM32L475XX || STM32L4_STM32L476XX || STM32L4_STM32L486XX || STM32L4_STM32L496XX || STM32L4_STM32L4A6XX
+	---help---
+		Enable the workaround to fix flash data cache corruption when reading
+		from one flash bank while writing on other flash bank.  See your STM32
+		errata to check if your STM32 is affected by this problem.
+
 config STM32L4_DISABLE_IDLE_SLEEP_DURING_DEBUG
 	bool "Disable IDLE Sleep (WFI) in debug mode"
 	default n
diff --git a/arch/arm/src/stm32l4/stm32l4_flash.c b/arch/arm/src/stm32l4/stm32l4_flash.c
index 170ec89dbfcb9..38f4fb597ddb1 100644
--- a/arch/arm/src/stm32l4/stm32l4_flash.c
+++ b/arch/arm/src/stm32l4/stm32l4_flash.c
@@ -187,6 +187,25 @@ static inline void flash_erase(size_t page)
   modifyreg32(STM32L4_FLASH_CR, FLASH_CR_PAGE_ERASE, 0);
 }
 
+
+#if defined(CONFIG_STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW)
+static void data_cache_disable(void)
+{
+  modifyreg32(STM32L4_FLASH_ACR, FLASH_ACR_DCEN, 0);
+}
+
+static void data_cache_enable(void)
+{
+  /* Reset data cache */
+
+  modifyreg32(STM32L4_FLASH_ACR, 0, FLASH_ACR_DCRST);
+
+  /* Enable data cache */
+
+  modifyreg32(STM32L4_FLASH_ACR, 0, FLASH_ACR_DCEN);
+}
+#endif /* defined(CONFIG_STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW) */
+
 /************************************************************************************
  * Public Functions
  ************************************************************************************/
@@ -370,6 +389,7 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t buflen)
   size_t xfrsize;
   size_t offset;
   size_t page;
+  bool set_pg_bit = false;
   int i;
   int ret = OK;
 
@@ -447,7 +467,12 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t buflen)
 
       /* Write the page. Must be with double-words. */
 
+#if defined(CONFIG_STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW)
+      data_cache_disable();
+#endif
+
       modifyreg32(STM32L4_FLASH_CR, 0, FLASH_CR_PG);
+      set_pg_bit = true;
 
       for (i = 0; i < FLASH_PAGE_WORDS; i += 2)
         {
@@ -463,20 +488,23 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t buflen)
 
           if (getreg32(STM32L4_FLASH_SR) & FLASH_SR_WRITE_PROTECTION_ERROR)
             {
-              modifyreg32(STM32L4_FLASH_CR, FLASH_CR_PG, 0);
               ret = -EROFS;
               goto out;
             }
 
           if (getreg32(dest-1) != *(src-1) || getreg32(dest-2) != *(src-2))
             {
-              modifyreg32(STM32L4_FLASH_CR, FLASH_CR_PG, 0);
               ret = -EIO;
               goto out;
             }
         }
 
       modifyreg32(STM32L4_FLASH_CR, FLASH_CR_PG, 0);
+      set_pg_bit = false;
+
+#if defined(CONFIG_STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW)
+      data_cache_enable();
+#endif
 
       /* Adjust pointers and counts for the next time through the loop */
 
@@ -489,6 +517,14 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t buflen)
     }
 
 out:
+  if (set_pg_bit)
+    {
+      modifyreg32(STM32L4_FLASH_CR, FLASH_CR_PG, 0);
+#if defined(CONFIG_STM32L4_FLASH_WORKAROUND_DATA_CACHE_CORRUPTION_ON_RWW)
+      data_cache_enable();
+#endif
+    }
+
   /* If there was an error, clear all error flags in status
    * register (rc_w1 register so do this by writing the
    * error bits).