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 25enter_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. 50return_from_exception 51 adr lr, returned_exception 52 movs pc, lr 53returned_exception // We are now in non-secure state 54 bx r0 55 56// Save the current Program Status Register (PSR) into the Saved PSR 57copy_cpsr_into_spsr 58 mrs r0, cpsr 59 msr spsr_cxsf, r0 60 bx lr 61 62// Set the Non Secure Mode 63set_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 74dead 75 B dead 76 77 END 78