ArmPlatformPkg/Sec: Replaced hardcode SCR and NSACR values by PCDs to enable CPU...

[mirror_edk2.git] / ArmPlatformPkg / Sec / Helper.S

diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S
index da3b0e033b53140c209b1f92e874912c7a5ea256..61491b565561c211fa588f688ba56dddcd8d93df 100644 (file)
--- a/ArmPlatformPkg/Sec/Helper.S
+++ b/ArmPlatformPkg/Sec/Helper.S
@@ -19,6 +19,7 @@ GCC_ASM_EXPORT(monitor_vector_table)
 GCC_ASM_EXPORT(return_from_exception)\r
 GCC_ASM_EXPORT(enter_monitor_mode)\r
 GCC_ASM_EXPORT(copy_cpsr_into_spsr)\r
+GCC_ASM_EXPORT(set_non_secure_mode)\r
 \r
 ASM_PFX(monitor_vector_table):\r
     ldr pc, dead\r
@@ -68,6 +69,18 @@ ASM_PFX(copy_cpsr_into_spsr):
     msr     spsr_cxsf, r0\r
     bx      lr\r
 \r
+# Set the Non Secure Mode\r
+ASM_PFX(set_non_secure_mode):\r
+    push    { r1 }\r
+    and            r0, r0, #0x1f     @ Keep only the mode bits\r
+    mrs     r1, spsr          @ Read the spsr\r
+    bic     r1, r1, #0x1f     @ Clear all mode bits\r
+    orr            r1, r1, r0\r
+    msr     spsr_cxsf, r1     @ write back spsr (may have caused a mode switch)\r
+    isb\r
+    pop     { r1 }\r
+    bx      lr                @ return (hopefully thumb-safe!)\r
+\r
 dead:\r
     b       dead\r
     \r