From a5d126631efb959f51295fcc4ba0e25846628e79 Mon Sep 17 00:00:00 2001
From: Ard Biesheuvel <ardb@kernel.org>
Date: Thu, 12 Sep 2024 11:08:34 +0200
Subject: [PATCH] Platform/BeagleBoard: Incorporate set/way data cache handling

Cache maintenance by set/way should only be used to manage the state of
a particular level in the cache hierarchy while the CPU is not taking
part in the coherency protocol. Use for any other purpose is not
supported by the architecture, and so the ArmLib routines that rely on
this will be removed.

BeagleBoard is a uni-processor system with non-cache coherent DMA,
where the use of set/way instructions to remove junk from the D-caches
at power-on is not entirely unreasonable. So incorporate the D-cache
invalidation code from ArmLib before dropping it from the library.

Signed-off-by: Ard Biesheuvel <ardb@kernel.org>
---
 .../BeagleBoardLib/BeagleBoardHelper.S        | 45 +++++++++++++++++++
 .../BeagleBoard/BeagleBoardPkg/PrePi/PrePi.c  |  2 -
 2 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/Platform/BeagleBoard/BeagleBoardPkg/Library/BeagleBoardLib/BeagleBoardHelper.S b/Platform/BeagleBoard/BeagleBoardPkg/Library/BeagleBoardLib/BeagleBoardHelper.S
index 007126e8634..34f101e5932 100644
--- a/Platform/BeagleBoard/BeagleBoardPkg/Library/BeagleBoardLib/BeagleBoardHelper.S
+++ b/Platform/BeagleBoard/BeagleBoardPkg/Library/BeagleBoardLib/BeagleBoardHelper.S
@@ -14,6 +14,51 @@
 GCC_ASM_EXPORT(ArmPlatformPeiBootAction)
 
 ASM_PFX(ArmPlatformPeiBootAction):
+
+  // Invalidate the entire data cache hierarchy by set/way
+  mrc   p15, 1, R6, c0, c0, 1   @ Read CLIDR
+  ands  R3, R6, #0x7000000      @ Mask out all but Level of Coherency (LoC)
+  mov   R3, R3, LSR #23         @ Cache level value (naturally aligned)
+  beq   L_Finished
+  mov   R10, #0
+
+Loop1:
+  add   R2, R10, R10, LSR #1    @ Work out 3xcachelevel
+  mov   R12, R6, LSR R2         @ bottom 3 bits are the Cache type for this level
+  and   R12, R12, #7            @ get those 3 bits alone
+  cmp   R12, #2
+  blt   L_Skip                  @ no cache or only instruction cache at this level
+  mcr   p15, 2, R10, c0, c0, 0  @ write the Cache Size selection register (CSSELR)
+  isb                           @ isb to sync the change to the CacheSizeID reg
+  mrc   p15, 1, R12, c0, c0, 0  @ reads current Cache Size ID register (CCSIDR)
+  and   R2, R12, #0x7           @ extract the line length field
+  add   R2, R2, #4              @ add 4 for the line length offset (log2 16 bytes)
+  movw  R4, #0x3ff
+  ands  R4, R4, R12, LSR #3     @ R4 is the max number on the way size (right aligned)
+  clz   R5, R4                  @ R5 is the bit position of the way size increment
+  movw  R7, #0x7fff
+  ands  R7, R7, R12, LSR #13    @ R7 is the max number of the index size (right aligned)
+
+Loop2:
+  mov   R9, R4                  @ R9 working copy of the max way size (right aligned)
+
+Loop3:
+  orr   R0, R10, R9, LSL R5     @ factor in the way number and cache number into R11
+  orr   R0, R0, R7, LSL R2      @ factor in the index number
+
+  mcr   p15, 0, r0, c7, c14, 2  @ Invalidate this line
+
+  subs  R9, R9, #1              @ decrement the way number
+  bge   Loop3
+  subs  R7, R7, #1              @ decrement the index
+  bge   Loop2
+L_Skip:
+  add   R10, R10, #2            @ increment the cache number
+  cmp   R3, R10
+  bgt   Loop1
+
+L_Finished:
+  dsb
   bx    lr
 
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED
diff --git a/Platform/BeagleBoard/BeagleBoardPkg/PrePi/PrePi.c b/Platform/BeagleBoard/BeagleBoardPkg/PrePi/PrePi.c
index 5c2fb4007e6..440970eb089 100644
--- a/Platform/BeagleBoard/BeagleBoardPkg/PrePi/PrePi.c
+++ b/Platform/BeagleBoard/BeagleBoardPkg/PrePi/PrePi.c
@@ -165,8 +165,6 @@ CEntryPoint (
 
   // Data Cache enabled on Primary core when MMU is enabled.
   ArmDisableDataCache ();
-  // Invalidate Data cache
-  ArmInvalidateDataCache ();
   // Invalidate instruction cache
   ArmInvalidateInstructionCache ();
   // Enable Instruction Caches on all cores.