[PATCH v2] ArmPkg/Library/ArmLib: add ArmWriteSctlr

[Michael Zimmermann <sigmaepsilon92@gmail.com>]

This currently isn't needed by anything in the edk2 tree but
it's useful for externally maintained platforms which have
to set this register e.g. to disable alignment aborts.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Michael Zimmermann <sigmaepsilon92@gmail.com>
---
 ArmPkg/Include/Library/ArmLib.h               | 6 ++++++
 ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S | 9 +++++++++
 ArmPkg/Library/ArmLib/Arm/ArmLibSupport.S     | 4 ++++
 ArmPkg/Library/ArmLib/Arm/ArmLibSupport.asm   | 3 +++
 4 files changed, 22 insertions(+)

diff --git a/ArmPkg/Include/Library/ArmLib.h b/ArmPkg/Include/Library/ArmLib.h
index 24e84c7a1965..ffda50e9d767 100644
--- a/ArmPkg/Include/Library/ArmLib.h
+++ b/ArmPkg/Include/Library/ArmLib.h
@@ -558,6 +558,12 @@ ArmReadSctlr (
   VOID
   );
 
+VOID
+EFIAPI
+ArmWriteSctlr (
+  IN  UINT32   Value
+  );
+
 UINTN
 EFIAPI
 ArmReadHVBar (
diff --git a/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S b/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S
index 9d3dd66b10eb..1ef2f61f5979 100644
--- a/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S
+++ b/ArmPkg/Library/ArmLib/AArch64/ArmLibSupport.S
@@ -187,4 +187,13 @@ ASM_FUNC(ArmReadSctlr)
 3:mrs   x0, sctlr_el3
 4:ret
 
+ASM_FUNC(ArmWriteSctlr)
+  EL1_OR_EL2_OR_EL3(x1)
+1:msr   sctlr_el1, x0
+  ret
+2:msr   sctlr_el2, x0
+  ret
+3:msr   sctlr_el3, x0
+4:ret
+
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED
diff --git a/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.S b/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.S
index a0b5ed500298..149b57e059ee 100644
--- a/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.S
+++ b/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.S
@@ -153,6 +153,10 @@ ASM_FUNC(ArmReadSctlr)
   mrc     p15, 0, r0, c1, c0, 0      @ Read SCTLR into R0 (Read control register configuration data)
   bx      lr
 
+ASM_FUNC(ArmWriteSctlr)
+  mcr     p15, 0, r0, c1, c0, 0
+  bx      lr
+
 ASM_FUNC(ArmReadCpuActlr)
   mrc     p15, 0, r0, c1, c0, 1
   bx      lr
diff --git a/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.asm b/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.asm
index 85b0feee20d4..219140c22b13 100644
--- a/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.asm
+++ b/ArmPkg/Library/ArmLib/Arm/ArmLibSupport.asm
@@ -155,6 +155,9 @@
   mrc     p15, 0, r0, c1, c0, 0      // Read SCTLR into R0 (Read control register configuration data)
   bx      lr
 
+ RVCT_ASM_EXPORT ArmWriteSctlr
+  mcr     p15, 0, r0, c1, c0, 0
+  bx      lr
 
  RVCT_ASM_EXPORT ArmReadCpuActlr
   mrc     p15, 0, r0, c1, c0, 1
-- 
2.15.1