ArmPlatformPkg/Sec: Added support for Non Cold Boot Paths

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

diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S
index 94bd68f8bc010446d5129571a77619cd8a5e28f5..4eede5faba6ba517caefa3057c55f31e4b5d3faf 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,35 +15,36 @@
 .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
+GCC_ASM_EXPORT(set_non_secure_mode)\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
-\r
-# arg0: Secure Monitor mode stack\r
+# r0: Monitor World EntryPoint\r
+# r1: MpId\r
+# r2: SecBootMode\r
+# r3: Secure Monitor mode stack\r
 ASM_PFX(enter_monitor_mode):\r
-    mov     r2, lr                      @ Save current lr\r
+    cmp     r3, #0                      @ If a Secure Monitor stack base has not been defined then use the Secure stack\r
+    moveq   r3, sp\r
+\r
+    mrs     r4, cpsr                    @ Save current mode (SVC) in r4\r
+    bic     r5, r4, #0x1f               @ Clear all mode bits\r
+    orr     r5, r5, #0x16               @ Set bits for Monitor mode\r
+    msr     cpsr_cxsf, r5               @ We are now in Monitor Mode\r
 \r
-    mrs     r1, cpsr                    @ Save current mode (SVC) in r1\r
-    bic     r3, r1, #0x1f               @ Clear all mode bits\r
-    orr     r3, r3, #0x16               @ Set bits for Monitor mode\r
-    msr     cpsr_cxsf, r3               @ We are now in Monitor Mode\r
+    mov     sp, r3                      @ Set the stack of the Monitor Mode\r
 \r
-    mov     sp, r0                      @ Use the passed sp\r
-    mov     lr, r2                      @ Use the same lr as before\r
+    mov     lr, r0                      @ Use the pass entrypoint as lr\r
     \r
-    msr     spsr_cxsf, r1               @ Use saved mode for the MOVS jump to the kernel\r
-    bx      lr\r
+    msr     spsr_cxsf, r4               @ Use saved mode for the MOVS jump to the kernel\r
+\r
+    mov     r4, r0                      @ Swap EntryPoint and MpId registers\r
+    mov     r0, r1\r
+    mov     r1, r2\r
+    mov     r2, r3\r
+\r
+    bx      r4\r
 \r
 # We cannot use the instruction 'movs pc, lr' because the caller can be written either in ARM or Thumb2 assembler.\r
 # When we will jump into this function, we will set the CPSR flag to ARM assembler. By copying directly 'lr' into\r
@@ -68,7 +69,19 @@ 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
+    b       dead\r
     \r
-.end\r
+ASM_FUNCTION_REMOVE_IF_UNREFERENCED\r