--- /dev/null
+#========================================================================================\r
+# Copyright (c) 2011, 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
+# which accompanies this distribution. The full text of the license may be found at \r
+# http:#opensource.org/licenses/bsd-license.php \r
+#\r
+# THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS, \r
+# WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED. \r
+#\r
+#=======================================================================================\r
+\r
+#start of the code section\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
+\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
+ASM_PFX(enter_monitor_mode):\r
+ mov r2, lr @ Save current lr\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
+\r
+ mov sp, r0 @ Use the passed sp\r
+ mov lr, r2 @ Use the same lr as before\r
+ \r
+ msr spsr_cxsf, r1 @ Use saved mode for the MOVS jump to the kernel\r
+ bx lr\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
+# 'pc'; we will not change the CPSR flag and it will crash.\r
+# The way to fix this limitation is to do the movs into the ARM assmbler code and then do a 'bx'.\r
+ASM_PFX(return_from_exception):\r
+ ldr lr, returned_exception\r
+\r
+ #The following instruction breaks the code.\r
+ #movs pc, lr\r
+ mrs r2, cpsr\r
+ bic r2, r2, #0x1f\r
+ orr r2, r2, #0x13\r
+ msr cpsr_c, r2\r
+\r
+returned_exception: @ We are now in non-secure state\r
+ bx r0\r
+\r
+# Save the current Program Status Register (PSR) into the Saved PSR\r
+ASM_PFX(copy_cpsr_into_spsr):\r
+ mrs r0, cpsr\r
+ msr spsr_cxsf, r0\r
+ bx lr\r
+\r
+dead:\r
+ B dead\r
+ \r
+.end\r