ArmPkg/ArmLib: AARCH64: allow the stack aligment (SA) bit to be managed

[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 886c420962b538bac196c43655514aa69ea846a4..6e8074a4868979d2fa3b850e1e588862179f829a 100644 (file)
--- a/ArmPkg/Library/ArmLib/AArch64/AArch64Support.S
+++ b/ArmPkg/Library/ArmLib/AArch64/AArch64Support.S
@@ -20,6 +20,7 @@
 .set CTRL_M_BIT,      (1 << 0)\r
 .set CTRL_A_BIT,      (1 << 1)\r
 .set CTRL_C_BIT,      (1 << 2)\r
+.set CTRL_SA_BIT,     (1 << 3)\r
 .set CTRL_I_BIT,      (1 << 12)\r
 .set CTRL_V_BIT,      (1 << 12)\r
 .set CPACR_VFP_BITS,  (3 << 20)\r
@@ -259,6 +260,39 @@ ASM_FUNC(ArmDisableAlignmentCheck)
    isb\r
    ret\r
 \r
+ASM_FUNC(ArmEnableStackAlignmentCheck)\r
+   EL1_OR_EL2(x1)\r
+1: mrs     x0, sctlr_el1        // Get control register EL1\r
+   b       3f\r
+2: mrs     x0, sctlr_el2        // Get control register EL2\r
+3: orr     x0, x0, #CTRL_SA_BIT // Set SA (stack alignment check) bit\r
+   EL1_OR_EL2(x1)\r
+1: msr     sctlr_el1, x0        // Write back control register\r
+   b       3f\r
+2: msr     sctlr_el2, x0        // Write back control register\r
+3: dsb     sy\r
+   isb\r
+   ret\r
+\r
+\r
+ASM_FUNC(ArmDisableStackAlignmentCheck)\r
+   EL1_OR_EL2_OR_EL3(x1)\r
+1: mrs     x0, sctlr_el1        // Get control register EL1\r
+   b       4f\r
+2: mrs     x0, sctlr_el2        // Get control register EL2\r
+   b       4f\r
+3: mrs     x0, sctlr_el3        // Get control register EL3\r
+4: bic     x0, x0, #CTRL_SA_BIT // Clear SA (stack alignment check) bit\r
+   EL1_OR_EL2_OR_EL3(x1)\r
+1: msr     sctlr_el1, x0        // Write back control register\r
+   b       4f\r
+2: msr     sctlr_el2, x0        // Write back control register\r
+   b       4f\r
+3: msr     sctlr_el3, x0        // Write back control register\r
+4: dsb     sy\r
+   isb\r
+   ret\r
+\r
 \r
 // Always turned on in AArch64. Else implementation specific. Leave in for C compatibility for now\r
 ASM_FUNC(ArmEnableBranchPrediction)\r