\r
// r0: Monitor World EntryPoint\r
// r1: MpId\r
-// r2: Secure Monitor mode stack\r
-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
+// r2: SecBootMode\r
+// r3: Secure Monitor mode stack\r
+enter_monitor_mode FUNCTION\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
\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
+ ENDFUNC\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