ArmPlatformPkg/Sec: Fix transition to Trusted Monitor World with ARMGCC

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

diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S
index 50cd733f97c1fc2ebf2d51346788b2924ccbdd9e..9e3b13b38f4af1c9e0a855a6659914c36ae1090d 100644 (file)
--- a/ArmPlatformPkg/Sec/Helper.S
+++ b/ArmPlatformPkg/Sec/Helper.S
@@ -20,20 +20,26 @@ GCC_ASM_EXPORT(enter_monitor_mode)
 GCC_ASM_EXPORT(copy_cpsr_into_spsr)\r
 GCC_ASM_EXPORT(set_non_secure_mode)\r
 \r
-# arg0: Secure Monitor mode stack\r
+# r0: Monitor World EntryPoint\r
+# r1: MpId\r
+# r2: Secure Monitor mode stack\r
 ASM_PFX(enter_monitor_mode):\r
-    mov     r2, lr                      @ Save current lr\r
-\r
-    mrs     r1, cpsr                    @ Save current mode (SVC) in r1\r
-    bic     r3, r1, #0x1f               @ Clear all mode bits\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
 \r
-    mov     sp, r0                      @ Use the passed sp\r
-    mov     lr, r2                      @ Use the same lr as before\r
+    cmp     r2, #0                      @ If a Secure Monitor stack base has been passed, used it\r
+    movne   sp, r2                      @ Use the passed sp\r
+\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
+\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