ArmPkg/ArmMmuLib AARCH64: cache-invalidate initial page table entries

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

diff --git a/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S b/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S
index 1adf960377a2893d1ca60d056c915ba302d33850..f744cd6738b9bd1139181523b75e1d2bd593585a 100644 (file)
--- a/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S
+++ b/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S
@@ -13,6 +13,8 @@
 .set DAIF_RD_FIQ_BIT,   (1 << 6)\r
 .set DAIF_RD_IRQ_BIT,   (1 << 7)\r
 \r
+.set SCTLR_ELx_M_BIT_POS, (0)\r
+\r
 ASM_FUNC(ArmReadMidr)\r
   mrs     x0, midr_el1        // Read from Main ID Register (MIDR)\r
   ret\r
@@ -122,11 +124,16 @@ ASM_FUNC(ArmUpdateTranslationTableEntry)
    lsr     x1, x1, #12\r
    EL1_OR_EL2_OR_EL3(x0)\r
 1: tlbi    vaae1, x1             // TLB Invalidate VA , EL1\r
+   mrs     x2, sctlr_el1\r
    b       4f\r
 2: tlbi    vae2, x1              // TLB Invalidate VA , EL2\r
+   mrs     x2, sctlr_el2\r
    b       4f\r
 3: tlbi    vae3, x1              // TLB Invalidate VA , EL3\r
-4: dsb     nsh\r
+   mrs     x2, sctlr_el3\r
+4: tbnz    x2, SCTLR_ELx_M_BIT_POS, 5f\r
+   dc      ivac, x0              // invalidate in Dcache if MMU is still off\r
+5: dsb     nsh\r
    isb\r
    ret\r
 \r