From c28d6b7172450ecbb2e5e4083bd8ef0a1d5b4d8f Mon Sep 17 00:00:00 2001
From: Nick Chan <towinchenmi@gmail.com>
Date: Tue, 8 Oct 2024 11:51:12 +0800
Subject: [PATCH] smp: Add A7-A9X cache errata workaround

Signed-off-by: Nick Chan <towinchenmi@gmail.com>
---
 src/chickens.c | 6 ++++--
 src/memory.c   | 8 ++++++++
 src/smp.c      | 7 ++++++-
 src/utils.h    | 3 ++-
 4 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/chickens.c b/src/chickens.c
index 2f7bcd8a..da132b7f 100644
--- a/src/chickens.c
+++ b/src/chickens.c
@@ -70,7 +70,7 @@ void init_t6031_sawtooth(void);
 void init_t6031_everest(int rev);
 
 bool cpufeat_actlr_el2, cpufeat_fast_ipi, cpufeat_mmu_sprr;
-bool cpufeat_global_sleep;
+bool cpufeat_global_sleep, cpufeat_workaround_cyclone_cache;
 
 const char *init_cpu(void)
 {
@@ -244,9 +244,11 @@ const char *init_cpu(void)
 
     if (part >= MIDR_PART_T8010_2_HURRICANE)
         cpufeat_global_sleep = true;
-    else
+    else {
         /* Disable deep sleep */
         reg_clr(SYS_IMP_APL_ACC_CFG, ACC_CFG_DEEP_SLEEP);
+        cpufeat_workaround_cyclone_cache = true;
+    }
 
     /* Unmask external IRQs, set WFI mode to up (2) */
     reg_mask(SYS_IMP_APL_CYC_OVRD,
diff --git a/src/memory.c b/src/memory.c
index 7835060a..c88086e7 100644
--- a/src/memory.c
+++ b/src/memory.c
@@ -19,12 +19,20 @@
 #define CACHE_RANGE_OP(func, op)                                                                   \
     void func(void *addr, size_t length)                                                           \
     {                                                                                              \
+        if (func == dc_civac_range && cpufeat_workaround_cyclone_cache) {                          \
+            reg_clr(SYS_IMP_APL_HID4, HID4_DISABLE_DC_MVA);                                        \
+            sysop("isb");                                                                          \
+        }                                                                                          \
         u64 p = (u64)addr;                                                                         \
         u64 end = p + length;                                                                      \
         while (p < end) {                                                                          \
             cacheop(op, p);                                                                        \
             p += CACHE_LINE_SIZE;                                                                  \
         }                                                                                          \
+        if (func == dc_civac_range && cpufeat_workaround_cyclone_cache) {                          \
+            reg_set(SYS_IMP_APL_HID4, HID4_DISABLE_DC_MVA);                                        \
+            sysop("isb");                                                                          \
+        }                                                                                          \
     }
 
 CACHE_RANGE_OP(ic_ivau_range, "ic ivau")
diff --git a/src/smp.c b/src/smp.c
index efbde3b6..0367e2e8 100644
--- a/src/smp.c
+++ b/src/smp.c
@@ -6,6 +6,7 @@
 #include "aic_regs.h"
 #include "cpu_regs.h"
 #include "malloc.h"
+#include "memory.h"
 #include "pmgr.h"
 #include "soc.h"
 #include "string.h"
@@ -129,10 +130,14 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u
         secondary_stacks_el3[index] = memalign(0x4000, SECONDARY_STACK_SIZE);
         _reset_stack = secondary_stacks_el3[index] + SECONDARY_STACK_SIZE; // EL3
         _reset_stack_el1 = secondary_stacks[index] + SECONDARY_STACK_SIZE; // EL1
+
+        dc_civac_range(&_reset_stack_el1, sizeof(void *));
     } else
         _reset_stack = secondary_stacks[index] + SECONDARY_STACK_SIZE;
 
-    sysop("dmb sy");
+    dc_civac_range(&_reset_stack, sizeof(void *));
+
+    sysop("dsb sy");
 
     write64(impl, (u64)_vectors_start);
 
diff --git a/src/utils.h b/src/utils.h
index 256ed5aa..6ce9e042 100644
--- a/src/utils.h
+++ b/src/utils.h
@@ -3,6 +3,7 @@
 #ifndef UTILS_H
 #define UTILS_H
 
+#include "cpu_regs.h"
 #include "soc.h"
 #include "types.h"
 
@@ -460,7 +461,7 @@ extern u32 board_id, chip_id;
 
 extern bool is_mac, has_dcp;
 extern bool cpufeat_actlr_el2, cpufeat_fast_ipi, cpufeat_mmu_sprr;
-extern bool cpufeat_global_sleep;
+extern bool cpufeat_global_sleep, cpufeat_workaround_cyclone_cache;
 
 extern struct vector_args next_stage;
 extern u64 boot_flags, mem_size_actual;