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

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

diff --git a/ArmPlatformPkg/Sec/Helper.asm b/ArmPlatformPkg/Sec/Helper.asm
index 43a0749138acf40392c60179ed95cf45f3be0a1c..1649df399adae8f1dbf01f0c9d80858b4dc1b45e 100644 (file)
--- a/ArmPlatformPkg/Sec/Helper.asm
+++ b/ArmPlatformPkg/Sec/Helper.asm
@@ -15,6 +15,7 @@
     EXPORT  return_from_exception\r
     EXPORT  enter_monitor_mode\r
     EXPORT  copy_cpsr_into_spsr\r
+    EXPORT  set_non_secure_mode\r
     \r
     AREA   Helper, CODE, READONLY\r
 \r
@@ -60,6 +61,18 @@ copy_cpsr_into_spsr
     msr     spsr_cxsf, r0\r
     bx      lr\r
 \r
+// Set the Non Secure Mode\r
+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