From 525254b32b99a1a899e7cef25d657fe5e338afb5 Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Mon, 12 Jul 2021 18:30:01 -0700
Subject: [PATCH] kern: update initial cache management to match latest kernel

---
 .../mesosphere/arch/arm64/kern_cpu.hpp        |  4 +-
 libmesosphere/source/arch/arm64/kern_cpu.cpp  | 84 +++++++++++--------
 2 files changed, 52 insertions(+), 36 deletions(-)

diff --git a/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp b/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp
index e9ca675e..c3693bfa 100644
--- a/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp
+++ b/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp
@@ -172,10 +172,8 @@ namespace ams::kern::arch::arm64::cpu {
 
     /* Cache management helpers. */
     void ClearPageToZeroImpl(void *);
-    void FlushEntireDataCacheSharedForInit();
-    void FlushEntireDataCacheLocalForInit();
-    void InvalidateEntireInstructionCacheForInit();
     void StoreEntireCacheForInit();
+    void FlushEntireCacheForInit();
 
     void FlushEntireDataCache();
 
diff --git a/libmesosphere/source/arch/arm64/kern_cpu.cpp b/libmesosphere/source/arch/arm64/kern_cpu.cpp
index 3da383f0..b5ebee32 100644
--- a/libmesosphere/source/arch/arm64/kern_cpu.cpp
+++ b/libmesosphere/source/arch/arm64/kern_cpu.cpp
@@ -262,27 +262,6 @@ namespace ams::kern::arch::arm64::cpu {
             __asm__ __volatile__("dc csw, %[v]" :: [v]"r"(sw_value) : "memory");
         }
 
-        template<bool Init, typename F>
-        ALWAYS_INLINE void PerformCacheOperationBySetWayShared(F f) {
-            CacheLineIdRegisterAccessor clidr_el1;
-            const int levels_of_coherency   = clidr_el1.GetLevelsOfCoherency();
-            const int levels_of_unification = clidr_el1.GetLevelsOfUnification();
-
-            for (int level = levels_of_coherency; level >= levels_of_unification; level--) {
-                PerformCacheOperationBySetWayImpl<Init>(level, f);
-            }
-        }
-
-        template<bool Init, typename F>
-        ALWAYS_INLINE void PerformCacheOperationBySetWayLocal(F f) {
-            CacheLineIdRegisterAccessor clidr_el1;
-            const int levels_of_unification = clidr_el1.GetLevelsOfUnification();
-
-            for (int level = levels_of_unification - 1; level >= 0; level--) {
-                PerformCacheOperationBySetWayImpl<Init>(level, f);
-            }
-        }
-
         void StoreDataCacheBySetWay(int level) {
             PerformCacheOperationBySetWayImpl<false>(level, StoreDataCacheLineBySetWayImpl);
             cpu::DataSynchronizationBarrier();
@@ -361,24 +340,63 @@ namespace ams::kern::arch::arm64::cpu {
 
     }
 
-    void FlushEntireDataCacheSharedForInit() {
-        return PerformCacheOperationBySetWayShared<true>(FlushDataCacheLineBySetWayImpl);
+    void StoreEntireCacheForInit() {
+        /* Store local. */
+        {
+            CacheLineIdRegisterAccessor clidr_el1;
+            const int levels_of_unification = clidr_el1.GetLevelsOfUnification();
+
+            for (int level = 0; level != levels_of_unification; ++level) {
+                PerformCacheOperationBySetWayImpl<true>(level, StoreDataCacheLineBySetWayImpl);
+            }
+        }
+
+        /* Store shared. */
+        {
+            CacheLineIdRegisterAccessor clidr_el1;
+            const int levels_of_coherency   = clidr_el1.GetLevelsOfCoherency();
+            const int levels_of_unification = clidr_el1.GetLevelsOfUnification();
+
+            for (int level = levels_of_unification; level <= levels_of_coherency; ++level) {
+                PerformCacheOperationBySetWayImpl<true>(level, StoreDataCacheLineBySetWayImpl);
+            }
+        }
+
+        /* Data synchronization barrier. */
+        DataSynchronizationBarrierInnerShareable();
+
+        /* Invalidate instruction cache. */
+        InvalidateEntireInstructionCacheLocalImpl();
+
+        /* Ensure local instruction consistency. */
+        DataSynchronizationBarrierInnerShareable();
+        InstructionMemoryBarrier();
     }
 
-    void FlushEntireDataCacheLocalForInit() {
-        return PerformCacheOperationBySetWayLocal<true>(FlushDataCacheLineBySetWayImpl);
-    }
+    void FlushEntireCacheForInit() {
+        /* Flush data cache. */
+        {
+            /* Get levels of coherence/unificaiton. */
+            CacheLineIdRegisterAccessor clidr_el1;
+            const int levels_of_coherency   = clidr_el1.GetLevelsOfCoherency();
 
-    void InvalidateEntireInstructionCacheForInit() {
+            /* Store cache from L1 up to (level of coherence - 1). */
+            for (int level = 0; level < levels_of_coherency - 1; ++level) {
+                PerformCacheOperationBySetWayImpl<true>(level, StoreDataCacheLineBySetWayImpl);
+            }
+
+            /* Flush cache from (level of coherence - 1) down to L0. */
+            for (int level = levels_of_coherency; level > 0; --level) {
+                PerformCacheOperationBySetWayImpl<true>(level - 1, FlushDataCacheLineBySetWayImpl);
+            }
+        }
+
+        /* Invalidate instruction cache. */
         InvalidateEntireInstructionCacheLocalImpl();
         EnsureInstructionConsistency();
-    }
 
-    void StoreEntireCacheForInit() {
-        PerformCacheOperationBySetWayLocal<true>(StoreDataCacheLineBySetWayImpl);
-        PerformCacheOperationBySetWayShared<true>(StoreDataCacheLineBySetWayImpl);
-        DataSynchronizationBarrierInnerShareable();
-        InvalidateEntireInstructionCacheForInit();
+        /* Invalidate entire TLB. */
+        InvalidateEntireTlb();
     }
 
     void FlushEntireDataCache() {