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: 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 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
+ 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, r0 @ Use the passed sp\r
- mov lr, r2 @ Use the same lr as before\r
+ mov sp, r3 @ Set the stack of the Monitor Mode\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
+ 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