ArmPlatformPkg/Sec/Helper.S

   1 #========================================================================================
   2 #  Copyright (c) 2011, 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 #start of the code section
  15 .text
  16 .align 3
  17
  18 GCC_ASM_EXPORT(monitor_vector_table)
  19 GCC_ASM_EXPORT(return_from_exception)
  20 GCC_ASM_EXPORT(enter_monitor_mode)
  21 GCC_ASM_EXPORT(copy_cpsr_into_spsr)
  22 GCC_ASM_EXPORT(set_non_secure_mode)
  23
  24 ASM_PFX(monitor_vector_table):
  25     ldr pc, dead
  26     ldr pc, dead
  27     ldr pc, dead
  28     ldr pc, dead
  29     ldr pc, dead
  30     ldr pc, dead
  31     ldr pc, dead
  32     ldr pc, dead
  33
  34 # arg0: Secure Monitor mode stack
  35 ASM_PFX(enter_monitor_mode):
  36     mov     r2, lr                      @ Save current lr
  37
  38     mrs     r1, cpsr                    @ Save current mode (SVC) in r1
  39     bic     r3, r1, #0x1f               @ Clear all mode bits
  40     orr     r3, r3, #0x16               @ Set bits for Monitor mode
  41     msr     cpsr_cxsf, r3               @ We are now in Monitor Mode
  42
  43     mov     sp, r0                      @ Use the passed sp
  44     mov     lr, r2                      @ Use the same lr as before
  45
  46     msr     spsr_cxsf, r1               @ Use saved mode for the MOVS jump to the kernel
  47     bx      lr
  48
  49 # We cannot use the instruction 'movs pc, lr' because the caller can be written either in ARM or Thumb2 assembler.
  50 # When we will jump into this function, we will set the CPSR flag to ARM assembler. By copying directly 'lr' into
  51 # 'pc'; we will not change the CPSR flag and it will crash.
  52 # The way to fix this limitation is to do the movs into the ARM assmbler code and then do a 'bx'.
  53 ASM_PFX(return_from_exception):
  54     ldr     lr, returned_exception
  55
  56     #The following instruction breaks the code.
  57     #movs    pc, lr
  58     mrs     r2, cpsr
  59     bic     r2, r2, #0x1f
  60     orr     r2, r2, #0x13
  61     msr     cpsr_c, r2
  62
  63 returned_exception:                           @ We are now in non-secure state
  64     bx      r0
  65
  66 # Save the current Program Status Register (PSR) into the Saved PSR
  67 ASM_PFX(copy_cpsr_into_spsr):
  68     mrs     r0, cpsr
  69     msr     spsr_cxsf, r0
  70     bx      lr
  71
  72 # Set the Non Secure Mode
  73 ASM_PFX(set_non_secure_mode):
  74     push    { r1 }
  75     and     r0, r0, #0x1f     @ Keep only the mode bits
  76     mrs     r1, spsr          @ Read the spsr
  77     bic     r1, r1, #0x1f     @ Clear all mode bits
  78     orr     r1, r1, r0
  79     msr     spsr_cxsf, r1     @ write back spsr (may have caused a mode switch)
  80     isb
  81     pop     { r1 }
  82     bx      lr                @ return (hopefully thumb-safe!)
  83
  84 dead:
  85     b       dead
  86
  87 ASM_FUNCTION_REMOVE_IF_UNREFERENCED