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 9e3b13b38f4af1c9e0a855a6659914c36ae1090d..4eede5faba6ba517caefa3057c55f31e4b5d3faf 100644 (file)
--- a/ArmPlatformPkg/Sec/Helper.S
+++ b/ArmPlatformPkg/Sec/Helper.S
@@ -22,15 +22,18 @@ GCC_ASM_EXPORT(set_non_secure_mode)
 \r
 # r0: Monitor World EntryPoint\r
 # r1: MpId\r
-# r2: Secure Monitor mode stack\r
+# r2: SecBootMode\r
+# r3: Secure Monitor mode stack\r
 ASM_PFX(enter_monitor_mode):\r
-    mrs     r4, cpsr                    @ Save current mode (SVC) in r1\r
-    bic     r3, r4, #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
+    cmp     r3, #0                      @ If a Secure Monitor stack base has not been defined then use the Secure stack\r
+    moveq   r3, sp\r
 \r
-    cmp     r2, #0                      @ If a Secure Monitor stack base has been passed, used it\r
-    movne   sp, r2                      @ Use the passed sp\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
+    mov     sp, r3                      @ Set the stack of the Monitor Mode\r
 \r
     mov     lr, r0                      @ Use the pass entrypoint as lr\r
     \r
@@ -38,6 +41,8 @@ ASM_PFX(enter_monitor_mode):
 \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