1 // 2 // Copyright (c) 2011-2012, ARM Limited. All rights reserved. 3 // 4 // This program and the accompanying materials 5 // are licensed and made available under the terms and conditions of the BSD License 6 // which accompanies this distribution. The full text of the license may be found at 7 // http://opensource.org/licenses/bsd-license.php 8 // 9 // THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS, 10 // WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED. 11 // 12 // 13 14 EXPORT return_from_exception 15 EXPORT enter_monitor_mode 16 EXPORT copy_cpsr_into_spsr 17 EXPORT set_non_secure_mode 18 19 AREA Helper, CODE, READONLY 20 21 // r0: Monitor World EntryPoint 22 // r1: MpId 23 // r2: SecBootMode 24 // r3: Secure Monitor mode stack 25 enter_monitor_mode FUNCTION 26 cmp r3, #0 // If a Secure Monitor stack base has not been defined then use the Secure stack 27 moveq r3, sp 28 29 mrs r4, cpsr // Save current mode (SVC) in r4 30 bic r5, r4, #0x1f // Clear all mode bits 31 orr r5, r5, #0x16 // Set bits for Monitor mode 32 msr cpsr_cxsf, r5 // We are now in Monitor Mode 33 34 mov sp, r3 // Set the stack of the Monitor Mode 35 36 mov lr, r0 // Use the pass entrypoint as lr 37 38 msr spsr_cxsf, r4 // Use saved mode for the MOVS jump to the kernel 39 40 mov r4, r0 // Swap EntryPoint and MpId registers 41 mov r0, r1 42 mov r1, r2 43 mov r2, r3 44 45 bx r4 46 ENDFUNC 47 48 // Return-from-exception is not an interworking return, so we must do it 49 // in two steps, in case r0 has the Thumb bit set. 50 return_from_exception 51 adr lr, returned_exception 52 movs pc, lr 53 returned_exception // We are now in non-secure state 54 bx r0 55 56 // Save the current Program Status Register (PSR) into the Saved PSR 57 copy_cpsr_into_spsr 58 mrs r0, cpsr 59 msr spsr_cxsf, r0 60 bx lr 61 62 // Set the Non Secure Mode 63 set_non_secure_mode 64 push { r1 } 65 and r0, r0, #0x1f // Keep only the mode bits 66 mrs r1, spsr // Read the spsr 67 bic r1, r1, #0x1f // Clear all mode bits 68 orr r1, r1, r0 69 msr spsr_cxsf, r1 // write back spsr (may have caused a mode switch) 70 isb 71 pop { r1 } 72 bx lr // return (hopefully thumb-safe!) 73 74 dead 75 B dead 76 77 END 78