ArmPkg: CpuDxe: fix AArch64 interrupt read masks

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

diff --git a/ArmPkg/Library/ArmLib/Common/AArch64/ArmLibSupport.S b/ArmPkg/Library/ArmLib/Common/AArch64/ArmLibSupport.S
index 12e6d0c82ba636bc500cb5d1408450a285f58df6..a6fd5e34454f4018f59f18733cf8ac3d1de8c4fb 100644 (file)
--- a/ArmPkg/Library/ArmLib/Common/AArch64/ArmLibSupport.S
+++ b/ArmPkg/Library/ArmLib/Common/AArch64/ArmLibSupport.S
@@ -37,11 +37,13 @@ GCC_ASM_EXPORT (ArmWriteScr)
 GCC_ASM_EXPORT (ArmWriteMVBar)\r
 GCC_ASM_EXPORT (ArmCallWFE)\r
 GCC_ASM_EXPORT (ArmCallSEV)\r
+GCC_ASM_EXPORT (ArmReadCpuActlr)\r
+GCC_ASM_EXPORT (ArmWriteCpuActlr)\r
 \r
 #------------------------------------------------------------------------------\r
 \r
-.set DAIF_FIQ_BIT,   (1 << 0)\r
-.set DAIF_IRQ_BIT,   (1 << 1)\r
+.set DAIF_RD_FIQ_BIT,   (1 << 6)\r
+.set DAIF_RD_IRQ_BIT,   (1 << 7)\r
 \r
 ASM_PFX(ArmReadMidr):\r
   mrs     x0, midr_el1        // Read from Main ID Register (MIDR)\r
@@ -53,18 +55,14 @@ ASM_PFX(ArmCacheInfo):
 \r
 ASM_PFX(ArmGetInterruptState):\r
   mrs     x0, daif\r
-  tst     w0, #DAIF_IRQ_BIT   // Check if IRQ is enabled. Enabled if 0.\r
-  mov     w0, #0\r
-  mov     w1, #1\r
-  csel    w0, w1, w0, ne\r
+  tst     w0, #DAIF_RD_IRQ_BIT  // Check if IRQ is enabled. Enabled if 0 (Z=1)\r
+  cset    w0, eq                // if Z=1 return 1, else 0\r
   ret\r
 \r
 ASM_PFX(ArmGetFiqState):\r
   mrs     x0, daif\r
-  tst     w0, #DAIF_FIQ_BIT   // Check if FIQ is enabled. Enabled if 0.\r
-  mov     w0, #0\r
-  mov     w1, #1\r
-  csel    w0, w1, w0, ne\r
+  tst     w0, #DAIF_RD_FIQ_BIT  // Check if FIQ is enabled. Enabled if 0 (Z=1)\r
+  cset    w0, eq                // if Z=1 return 1, else 0\r
   ret\r
 \r
 ASM_PFX(ArmWriteCpacr):\r
@@ -182,6 +180,7 @@ ASM_PFX(ArmWriteCptr):
 \r
 ASM_PFX(ArmWriteScr):\r
   msr     scr_el3, x0            // Secure configuration register EL3\r
+  isb\r
   ret\r
 \r
 ASM_PFX(ArmWriteMVBar):\r
@@ -196,5 +195,14 @@ ASM_PFX(ArmCallSEV):
   sev\r
   ret\r
 \r
+ASM_PFX(ArmReadCpuActlr):\r
+  mrs   x0, S3_1_c15_c2_0\r
+  ret\r
+\r
+ASM_PFX(ArmWriteCpuActlr):\r
+  msr   S3_1_c15_c2_0, x0\r
+  dsb   sy\r
+  isb\r
+  ret\r
 \r
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED\r