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 61491b565561c211fa588f688ba56dddcd8d93df..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,36 +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