From a92c1b9ad960e45c7dc03fdd06c269473f1eee52 Mon Sep 17 00:00:00 2001
From: David Garske <david@wolfssl.com>
Date: Tue, 25 Jun 2024 08:24:31 -0700
Subject: [PATCH] STM32U5 cache support. Including cache invalidate on
 `nvm_select_fresh_sector`. ZD 18210

---
 hal/stm32u5.c     | 26 ++++++++++++++++++++++++++
 hal/stm32u5.h     | 22 ++++++++++++++++++++++
 include/image.h   |  8 ++++++++
 src/libwolfboot.c |  7 +++++++
 4 files changed, 63 insertions(+)

diff --git a/hal/stm32u5.c b/hal/stm32u5.c
index 4fa58125..cdbc5653 100644
--- a/hal/stm32u5.c
+++ b/hal/stm32u5.c
@@ -526,3 +526,29 @@ void hal_prepare_boot(void)
     led_unsecure();
 #endif
 }
+
+void hal_cache_enable(int way)
+{
+    ICACHE_CR |= (way ? ICACHE_CR_2WAYS : ICACHE_CR_1WAY);
+    ICACHE_CR |= ICACHE_CR_CEN;
+}
+
+void hal_cache_disable(void)
+{
+    ICACHE_CR &= ~ICACHE_CR_CEN;
+}
+
+void hal_cache_invalidate(void)
+{
+    /* Check if no ongoing operation */
+    if ((ICACHE_SR & ICACHE_SR_BUSYF) == 0) {
+        /* Launch cache invalidation */
+        ICACHE_CR |= ICACHE_CR_CACHEINV;
+    }
+
+    if (ICACHE_SR & ICACHE_SR_BUSYF) {
+        while ((ICACHE_SR & ICACHE_SR_BSYENDF) == 0);
+    }
+    /* Clear BSYENDF */
+    ICACHE_SR |= ICACHE_SR_BSYENDF;
+}
diff --git a/hal/stm32u5.h b/hal/stm32u5.h
index 5aa9a509..0f041dc1 100644
--- a/hal/stm32u5.h
+++ b/hal/stm32u5.h
@@ -263,3 +263,25 @@
 #define AIRCR_VKEY        (0x05FA << 16)
 #define AIRCR_SYSRESETREQ (1 << 2)
 
+/* Cache */
+#if (TZ_SECURE())
+#define ICACHE_BASE          (0x50030400)   /* RM0456 - Table 4 */
+#else
+#define ICACHE_BASE          (0x40030400)   /* RM0456 - Table 4 */
+#endif
+#define ICACHE_CR    *(volatile uint32_t *)(ICACHE_BASE + 0x00)
+#define ICACHE_CR_WAYSEL   (1 << 2)
+#define ICACHE_CR_1WAY     0U               /* 1-way cache (direct mapped cache) */
+#define ICACHE_CR_2WAYS    ICACHE_CR_WAYSEL /* 2-ways set associative cache */
+
+#define ICACHE_CR_CACHEINV (1 << 1)
+#define ICACHE_CR_CEN      (1 << 0)
+
+#define ICACHE_SR    *(volatile uint32_t *)(ICACHE_BASE + 0x04)
+#define ICACHE_SR_BUSYF    (1 << 0) /* busy flag */
+#define ICACHE_SR_BSYENDF  (1 << 1) /* busy end flag */
+#define ICACHE_SR_ERRF     (1 << 2) /* cache error flag */
+
+void hal_cache_invalidate(void);
+void hal_cache_enable(int way);
+void hal_cache_disable(void);
diff --git a/include/image.h b/include/image.h
index 6c1a5f17..04307fcd 100644
--- a/include/image.h
+++ b/include/image.h
@@ -58,6 +58,14 @@ int wolfBot_get_dts_size(void *dts_addr);
 #endif
 #endif
 
+#ifndef WEAKFUNCTION
+#  if defined(__GNUC__) || defined(__CC_ARM)
+#    define WEAKFUNCTION __attribute__((weak))
+#  else
+#    define WEAKFUNCTION
+#  endif
+#endif
+
 
 #ifndef WOLFBOOT_FLAGS_INVERT
 #define SECT_FLAG_NEW      0x0F
diff --git a/src/libwolfboot.c b/src/libwolfboot.c
index 23c62ccc..ad2793b4 100644
--- a/src/libwolfboot.c
+++ b/src/libwolfboot.c
@@ -189,6 +189,11 @@ static uint8_t get_base_offset(uint8_t *base, uintptr_t off)
     #pragma GCC diagnostic pop
 #endif
 
+void WEAKFUNCTION hal_cache_invalidate(void)
+{
+    /* if cache flushing is required implement in hal */
+}
+
 static int RAMFUNCTION nvm_select_fresh_sector(int part)
 {
     int sel;
@@ -198,6 +203,8 @@ static int RAMFUNCTION nvm_select_fresh_sector(int part)
     uint32_t word_0;
     uint32_t word_1;
 
+    hal_cache_invalidate();
+
     /* if FLAGS_HOME check both boot and update for changes */
 #ifdef FLAGS_HOME
     base = (uint8_t *)PART_BOOT_ENDFLAGS;