diff options
Diffstat (limited to 'ArmPlatformPkg/Sec/Helper.S')
| -rw-r--r-- | ArmPlatformPkg/Sec/Helper.S | 24 |
1 files changed, 15 insertions, 9 deletions
diff --git a/ArmPlatformPkg/Sec/Helper.S b/ArmPlatformPkg/Sec/Helper.S index 50cd733f97..9e3b13b38f 100644 --- 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)
GCC_ASM_EXPORT(set_non_secure_mode)
-# arg0: Secure Monitor mode stack
+# r0: Monitor World EntryPoint
+# r1: MpId
+# r2: Secure Monitor mode stack
ASM_PFX(enter_monitor_mode):
- mov r2, lr @ Save current lr
-
- mrs r1, cpsr @ Save current mode (SVC) in r1
- bic r3, r1, #0x1f @ Clear all mode bits
+ mrs r4, cpsr @ Save current mode (SVC) in r1
+ bic r3, r4, #0x1f @ Clear all mode bits
orr r3, r3, #0x16 @ Set bits for Monitor mode
msr cpsr_cxsf, r3 @ We are now in Monitor Mode
- mov sp, r0 @ Use the passed sp
- mov lr, r2 @ Use the same lr as before
+ cmp r2, #0 @ If a Secure Monitor stack base has been passed, used it
+ movne sp, r2 @ Use the passed sp
+
+ mov lr, r0 @ Use the pass entrypoint as lr
- msr spsr_cxsf, r1 @ Use saved mode for the MOVS jump to the kernel
- bx lr
+ msr spsr_cxsf, r4 @ Use saved mode for the MOVS jump to the kernel
+
+ mov r4, r0 @ Swap EntryPoint and MpId registers
+ mov r0, r1
+
+ bx r4
# We cannot use the instruction 'movs pc, lr' because the caller can be written either in ARM or Thumb2 assembler.
# When we will jump into this function, we will set the CPSR flag to ARM assembler. By copying directly 'lr' into
|