ArmPkg/Library: Add ArmReadSctlr for AArch64

[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 2db812616a35c4f807af7af6b3015088cdcb2c0c..c9f3bd1e8810e10fbd2f01a27130fdd683525816 100644 (file)
--- a/ArmPkg/Library/ArmLib/Common/AArch64/ArmLibSupport.S
+++ b/ArmPkg/Library/ArmLib/Common/AArch64/ArmLibSupport.S
@@ -1,7 +1,7 @@
 #------------------------------------------------------------------------------\r
 #\r
 # Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>\r
-# Copyright (c) 2011 - 2013, ARM Limited. All rights reserved.\r
+# Copyright (c) 2011 - 2016, ARM Limited. All rights reserved.\r
 #\r
 # This program and the accompanying materials\r
 # are licensed and made available under the terms and conditions of the BSD License\r
@@ -17,7 +17,7 @@
 \r
 .text\r
 .align 3\r
-GCC_ASM_EXPORT (ArmMainIdCode)\r
+GCC_ASM_EXPORT (ArmReadMidr)\r
 GCC_ASM_EXPORT (ArmCacheInfo)\r
 GCC_ASM_EXPORT (ArmGetInterruptState)\r
 GCC_ASM_EXPORT (ArmGetFiqState)\r
@@ -32,18 +32,21 @@ GCC_ASM_EXPORT (ArmWriteAuxCr)
 GCC_ASM_EXPORT (ArmReadAuxCr)\r
 GCC_ASM_EXPORT (ArmInvalidateTlb)\r
 GCC_ASM_EXPORT (ArmUpdateTranslationTableEntry)\r
-GCC_ASM_EXPORT (ArmWriteNsacr)\r
+GCC_ASM_EXPORT (ArmWriteCptr)\r
 GCC_ASM_EXPORT (ArmWriteScr)\r
 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
+GCC_ASM_EXPORT (ArmReadSctlr)\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(ArmiMainIdCode):\r
+ASM_PFX(ArmReadMidr):\r
   mrs     x0, midr_el1        // Read from Main ID Register (MIDR)\r
   ret\r
 \r
@@ -53,18 +56,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
@@ -74,16 +73,16 @@ ASM_PFX(ArmWriteCpacr):
 ASM_PFX(ArmWriteAuxCr):\r
   EL1_OR_EL2(x1)\r
 1:msr     actlr_el1, x0      // Aux Control Reg (ACTLR) at EL1. Also available in EL2 and EL3\r
-  b       3f\r
+  ret\r
 2:msr     actlr_el2, x0      // Aux Control Reg (ACTLR) at EL1. Also available in EL2 and EL3\r
-3:ret\r
+  ret\r
 \r
 ASM_PFX(ArmReadAuxCr):\r
   EL1_OR_EL2(x1)\r
 1:mrs     x0, actlr_el1      // Aux Control Reg (ACTLR) at EL1. Also available in EL2 and EL3\r
-  b       3f\r
+  ret\r
 2:mrs     x0, actlr_el2      // Aux Control Reg (ACTLR) at EL1. Also available in EL2 and EL3\r
-3:ret\r
+  ret\r
 \r
 ASM_PFX(ArmSetTTBR0):\r
   EL1_OR_EL2_OR_EL3(x1)\r
@@ -176,16 +175,17 @@ ASM_PFX(ArmInvalidateTlb):
    isb\r
    ret\r
 \r
-ASM_PFX(ArmWriteNsacr):\r
+ASM_PFX(ArmWriteCptr):\r
   msr     cptr_el3, x0           // EL3 Coprocessor Trap Reg (CPTR)\r
-  ret                            // Non-Secure Access Control Reg (NSACR) in ARMv7\r
+  ret\r
 \r
 ASM_PFX(ArmWriteScr):\r
   msr     scr_el3, x0            // Secure configuration register EL3\r
+  isb\r
   ret\r
 \r
 ASM_PFX(ArmWriteMVBar):\r
-  msr    vbar_el3, x0            // Excpetion Vector Base address for Monitor on EL3\r
+  msr    vbar_el3, x0            // Exception Vector Base address for Monitor on EL3\r
   ret\r
 \r
 ASM_PFX(ArmCallWFE):\r
@@ -196,7 +196,23 @@ ASM_PFX(ArmCallSEV):
   sev\r
   ret\r
 \r
-dead:\r
-  b       dead\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_PFX(ArmReadSctlr):\r
+  EL1_OR_EL2_OR_EL3(x1)\r
+1:mrs   x0, sctlr_el1\r
+  ret\r
+2:mrs   x0, sctlr_el2\r
+  ret\r
+3:mrs   x0, sctlr_el3\r
+4:ret\r
 \r
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED\r