From efd4fb6bf2b3c133c376c4a574d50d547bda2be7 Mon Sep 17 00:00:00 2001
From: "Matthias J. Kannwischer" <matthias@kannwischer.eu>
Date: Sun, 20 Oct 2024 16:20:22 +0800
Subject: [PATCH] enable ICache and DCache

---
 common/hal-opencm3.c | 56 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 56 insertions(+)

diff --git a/common/hal-opencm3.c b/common/hal-opencm3.c
index a72f44a..877bbb0 100644
--- a/common/hal-opencm3.c
+++ b/common/hal-opencm3.c
@@ -11,6 +11,8 @@
 #include <libopencm3/stm32/gpio.h>
 #include <libopencm3/stm32/rcc.h>
 #include <libopencm3/stm32/usart.h>
+#include <libopencm3/cm3/scb.h>
+
 
 #if defined(STM32F767ZI)
 #include <libopencm3/stm32/f7/rcc.h>
@@ -82,6 +84,55 @@ void systick_setup() {
   systick_interrupt_enable();
   systick_counter_enable();
 }
+
+
+
+
+#if defined(STM32F7)
+#define DWT_LAR_PTR                         ((volatile unsigned int *) 0xe0001fb0)
+#define DWT_LAR_KEY                         0xC5ACCE55
+#define SCB_DCISW_SET_Pos                   5U                                            /*!< SCB DCISW: Set Position */
+#define SCB_DCISW_SET_Msk                  (0x1FFUL << SCB_DCISW_SET_Pos)                 /*!< SCB DCISW: Set Mask */
+#define SCB_CCR_DC_Pos                      16U                                           /*!< SCB CCR: Cache enable bit Position */
+#define SCB_CCR_DC_Msk                     (1UL << SCB_CCR_DC_Pos)                        /*!< SCB CCR: Cache enable bit Mask */
+#define SCB_CCSIDR_NUMSETS_Pos             13U                                            /*!< SCB CCSIDR: NumSets Position */
+#define SCB_CCSIDR_NUMSETS_Msk             (0x7FFFUL << SCB_CCSIDR_NUMSETS_Pos)           /*!< SCB CCSIDR: NumSets Mask */
+#define SCB_DCISW_WAY_Pos                  30U                                            /*!< SCB DCISW: Way Position */
+#define SCB_DCISW_WAY_Msk                  (3UL << SCB_DCISW_WAY_Pos)                     /*!< SCB DCISW: Way Mask */
+#define SCB_CCSIDR_ASSOCIATIVITY_Pos        3U                                            /*!< SCB CCSIDR: Associativity Position */
+#define SCB_CCSIDR_ASSOCIATIVITY_Msk       (0x3FFUL << SCB_CCSIDR_ASSOCIATIVITY_Pos)      /*!< SCB CCSIDR: Associativity Mask */
+#define SCB_CCR_DC_Pos                      16U                                           /*!< SCB CCR: Cache enable bit Position */
+#define SCB_CCR_DC_Msk                     (1UL << SCB_CCR_DC_Pos)                        /*!< SCB CCR: Cache enable bit Mask */
+#define SCB_CCR_IC_Pos                      17U                                           /*!< SCB CCR: Instruction cache enable bit Position */
+#define SCB_CCR_IC_Msk                     (1UL << SCB_CCR_IC_Pos)                        /*!< SCB CCR: Instruction cache enable bit Mask */
+
+#define CCSIDR_WAYS(x) (((x) & SCB_CCSIDR_ASSOCIATIVITY_Msk) >> SCB_CCSIDR_ASSOCIATIVITY_Pos)
+#define CCSIDR_SETS(x) (((x) & SCB_CCSIDR_NUMSETS_Msk ) >> SCB_CCSIDR_NUMSETS_Pos )
+
+static void SCB_EnableICache(void){
+    SCB_ICIALLU = 0L;
+    SCB_CCR |= (uint32_t) SCB_CCR_IC_Msk ;
+}
+
+static void SCB_EnableDCache(void){
+    uint32_t ccsidr;
+    uint32_t sets;
+    uint32_t ways;
+
+    ccsidr = SCB_CCSIDR;
+    /* invalidate D-Cache */
+    sets = (uint32_t)(CCSIDR_SETS(ccsidr));
+    do {
+      ways = (uint32_t)(CCSIDR_WAYS(ccsidr));
+      do {
+        SCB_DCISW = (((sets << SCB_DCISW_SET_Pos) & SCB_DCISW_SET_Msk) |
+                      ((ways << SCB_DCISW_WAY_Pos) & SCB_DCISW_WAY_Msk)  );
+      } while (ways-- != 0U);
+    } while(sets-- != 0U);
+    SCB_CCR |= (uint32_t)SCB_CCR_DC_Msk;  /* enable D-Cache */
+}
+#endif
+
 static volatile unsigned long long overflowcnt = 0;
 void hal_setup(const enum clock_mode clock) {
   clock_setup(clock);
@@ -89,6 +140,11 @@ void hal_setup(const enum clock_mode clock) {
   systick_setup();
   rng_enable();
 
+    #if defined(STM32F7)
+    SCB_EnableICache();
+    SCB_EnableDCache();
+    #endif
+
   // wait for the first systick overflow
   // improves reliability of the benchmarking scripts since it makes it much
   // less likely that the host will miss the start of the output