#========================================================================================\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
.text \r
.align 3\r
\r
-GCC_ASM_EXPORT(monitor_vector_table)\r
GCC_ASM_EXPORT(return_from_exception)\r
GCC_ASM_EXPORT(enter_monitor_mode)\r
GCC_ASM_EXPORT(copy_cpsr_into_spsr)\r
+GCC_ASM_EXPORT(set_non_secure_mode)\r
\r
-ASM_PFX(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
-\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 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
\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
+ASM_PFX(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