ArmPkg/ArmLib: don't invalidate entire I-cache on range operation

[mirror_edk2.git] / ArmPkg / Library / ArmLib / AArch64 / AArch64Support.S

diff --git a/ArmPkg/Library/ArmLib/AArch64/AArch64Support.S b/ArmPkg/Library/ArmLib/AArch64/AArch64Support.S
index 1a3023b794878f91ace57b24b7d0d88f377a7faf..9441f47e30bae46332ae4a7c3de171ddac753e0b 100644 (file)
--- a/ArmPkg/Library/ArmLib/AArch64/AArch64Support.S
+++ b/ArmPkg/Library/ArmLib/AArch64/AArch64Support.S
@@ -23,6 +23,7 @@ GCC_ASM_EXPORT (ArmInvalidateInstructionCache)
 GCC_ASM_EXPORT (ArmInvalidateDataCacheEntryByMVA)\r
 GCC_ASM_EXPORT (ArmCleanDataCacheEntryByMVA)\r
 GCC_ASM_EXPORT (ArmCleanDataCacheEntryToPoUByMVA)\r
+GCC_ASM_EXPORT (ArmInvalidateInstructionCacheEntryToPoUByMVA)\r
 GCC_ASM_EXPORT (ArmCleanInvalidateDataCacheEntryByMVA)\r
 GCC_ASM_EXPORT (ArmInvalidateDataCacheEntryBySetWay)\r
 GCC_ASM_EXPORT (ArmCleanDataCacheEntryBySetWay)\r
@@ -56,6 +57,8 @@ GCC_ASM_EXPORT (ArmReadIdPfr1)
 GCC_ASM_EXPORT (ArmWriteHcr)\r
 GCC_ASM_EXPORT (ArmReadHcr)\r
 GCC_ASM_EXPORT (ArmReadCurrentEL)\r
+GCC_ASM_EXPORT (ArmReplaceLiveTranslationEntry)\r
+GCC_ASM_EXPORT (ArmReplaceLiveTranslationEntrySize)\r
 \r
 .set CTRL_M_BIT,      (1 << 0)\r
 .set CTRL_A_BIT,      (1 << 1)\r
@@ -78,6 +81,10 @@ ASM_PFX(ArmCleanDataCacheEntryToPoUByMVA):
   dc      cvau, x0    // Clean single data cache line to PoU\r
   ret\r
 \r
+ASM_PFX(ArmInvalidateInstructionCacheEntryToPoUByMVA):\r
+  ic      ivau, x0    // Invalidate single instruction cache line to PoU\r
+  ret\r
+\r
 \r
 ASM_PFX(ArmCleanInvalidateDataCacheEntryByMVA):\r
   dc      civac, x0   // Clean and invalidate single data cache line\r
@@ -481,4 +488,64 @@ ASM_PFX(ArmReadCurrentEL):
   mrs   x0, CurrentEL\r
   ret\r
 \r
+\r
+  .macro __replace_entry, el\r
+\r
+  // disable the MMU\r
+  mrs   x8, sctlr_el\el\r
+  bic   x9, x8, #CTRL_M_BIT\r
+  msr   sctlr_el\el, x9\r
+  isb\r
+\r
+  // write updated entry\r
+  str   x1, [x0]\r
+\r
+  // invalidate again to get rid of stale clean cachelines that may\r
+  // have been filled speculatively since the last invalidate\r
+  dmb   sy\r
+  dc    ivac, x0\r
+\r
+  // flush the TLBs\r
+  .if   \el == 1\r
+  tlbi  vmalle1\r
+  .else\r
+  tlbi  alle\el\r
+  .endif\r
+  dsb   sy\r
+\r
+  // re-enable the MMU\r
+  msr   sctlr_el\el, x8\r
+  isb\r
+  .endm\r
+\r
+//VOID\r
+//ArmReplaceLiveTranslationEntry (\r
+//  IN  UINT64  *Entry,\r
+//  IN  UINT64  Value\r
+//  )\r
+ASM_PFX(ArmReplaceLiveTranslationEntry):\r
+\r
+  // disable interrupts\r
+  mrs   x2, daif\r
+  msr   daifset, #0xf\r
+  isb\r
+\r
+  // clean and invalidate first so that we don't clobber\r
+  // adjacent entries that are dirty in the caches\r
+  dc    civac, x0\r
+  dsb   ish\r
+\r
+  EL1_OR_EL2_OR_EL3(x3)\r
+1:__replace_entry 1\r
+  b     4f\r
+2:__replace_entry 2\r
+  b     4f\r
+3:__replace_entry 3\r
+\r
+4:msr   daif, x2\r
+  ret\r
+\r
+ASM_PFX(ArmReplaceLiveTranslationEntrySize):\r
+  .long   . - ArmReplaceLiveTranslationEntry\r
+\r
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED\r