ArmPlatformPkg: Move the Secure Monitor initialization into a separate 'ArmTrustedMon...

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

diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S
index da3b0e033b53140c209b1f92e874912c7a5ea256..50cd733f97c1fc2ebf2d51346788b2924ccbdd9e 100644 (file)
--- a/ArmPlatformPkg/Sec/Helper.S
+++ b/ArmPlatformPkg/Sec/Helper.S
@@ -1,5 +1,5 @@
 #========================================================================================\r
-#  Copyright (c) 2011, ARM Limited. All rights reserved.\r
+#  Copyright (c) 2011-2012, 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
@@ -15,20 +15,10 @@
 .text \r
 .align 3\r
 \r
-GCC_ASM_EXPORT(monitor_vector_table)\r
 GCC_ASM_EXPORT(return_from_exception)\r
 GCC_ASM_EXPORT(enter_monitor_mode)\r
 GCC_ASM_EXPORT(copy_cpsr_into_spsr)\r
-\r
-ASM_PFX(monitor_vector_table):\r
-    ldr pc, dead\r
-    ldr pc, dead\r
-    ldr pc, dead\r
-    ldr pc, dead\r
-    ldr pc, dead\r
-    ldr pc, dead\r
-    ldr pc, dead\r
-    ldr pc, dead\r
+GCC_ASM_EXPORT(set_non_secure_mode)\r
 \r
 # arg0: Secure Monitor mode stack\r
 ASM_PFX(enter_monitor_mode):\r
@@ -68,6 +58,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