//\r
-// Copyright (c) 2011, ARM Limited. All rights reserved.\r
+// Copyright (c) 2011-2012, ARM Limited. All rights reserved.\r
// \r
// This program and the accompanying materials \r
// are licensed and made available under the terms and conditions of the BSD License \r
//\r
//\r
\r
- EXPORT monitor_vector_table\r
EXPORT return_from_exception\r
EXPORT enter_monitor_mode\r
EXPORT copy_cpsr_into_spsr\r
+ EXPORT set_non_secure_mode\r
\r
AREA Helper, CODE, READONLY\r
\r
- ALIGN 32\r
-monitor_vector_table\r
- ldr pc, dead\r
- ldr pc, dead\r
- ldr pc, dead\r
- ldr pc, dead\r
- ldr pc, dead\r
- ldr pc, dead\r
- ldr pc, dead\r
- ldr pc, dead\r
+// r0: Monitor World EntryPoint\r
+// r1: MpId\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
-// arg0: Secure Monitor mode stack\r
-enter_monitor_mode\r
- mov r2, lr // Save current lr\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
- 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
+ mov sp, r3 // Set the stack of the Monitor Mode\r
\r
- mov sp, r0 // Use the passed sp\r
- mov lr, r2 // Use the same lr as before\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
+ 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
msr spsr_cxsf, r0\r
bx lr\r
\r
+// Set the Non Secure Mode\r
+set_non_secure_mode\r
+ push { r1 }\r
+ and r0, r0, #0x1f // Keep only the mode bits\r
+ mrs r1, spsr // Read the spsr\r
+ bic r1, r1, #0x1f // Clear all mode bits\r
+ orr r1, r1, r0\r
+ msr spsr_cxsf, r1 // write back spsr (may have caused a mode switch)\r
+ isb\r
+ pop { r1 }\r
+ bx lr // return (hopefully thumb-safe!)\r
+\r
dead\r
B dead\r
\r