| Christoffer Dall | 3d28a18 | 2012-11-17 22:46:19 -0500 | [diff] [blame^] | 1 | /* |
| 2 | * |
| 3 | * Copyright (c) 2012 Christoffer Dall <c.dall@virtualopensystems.com> |
| 4 | * <cdall@cs.columbia.edu> |
| 5 | * |
| 6 | * See file CREDITS for list of people who contributed to this |
| 7 | * project. |
| 8 | * |
| 9 | * This program is free software; you can redistribute it and/or |
| 10 | * modify it under the terms of the GNU General Public License as |
| 11 | * published by the Free Software Foundation; either version 2 of |
| 12 | * the License, or (at your option) any later version. |
| 13 | * |
| 14 | * This program is distributed in the hope that it will be useful, |
| 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 17 | * GNU General Public License for more details. |
| 18 | * |
| 19 | * You should have received a copy of the GNU General Public License |
| 20 | * along with this program; if not, write to the Free Software |
| 21 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, |
| 22 | * MA 02111-1307 USA |
| 23 | */ |
| 24 | |
| 25 | #include <config.h> |
| 26 | |
| 27 | .syntax unified |
| 28 | .arch_extension sec |
| 29 | .arch_extension virt |
| 30 | .text |
| 31 | |
| 32 | .align 5 |
| 33 | |
| 34 | /* We use the same vector table for Hyp and Monitor mode, since |
| 35 | * we will only use each once and they don't overlap. |
| 36 | */ |
| 37 | mon_vectors: |
| 38 | .word 0 /* reset */ |
| 39 | .word 0 /* undef */ |
| 40 | b 2f /* smc */ |
| 41 | .word 0 /* pabt */ |
| 42 | .word 0 /* dabt */ |
| 43 | b 1f |
| 44 | .word 0 /* irq */ |
| 45 | .word 0 /* fiq */ |
| 46 | |
| 47 | /* Return directly back to the caller without leaving Hyp mode: */ |
| 48 | 1: mrs lr, elr_hyp |
| 49 | mov pc, lr |
| 50 | |
| 51 | /* In monitor mode, set up HVBAR and SCR then return to caller in NS-SVC. */ |
| 52 | 2: |
| 53 | mrc p15, 0, r1, c1, c1, 0 @ SCR |
| 54 | /* |
| 55 | * Set SCR.NS=1 (needed for setting HVBAR and also returning to NS state) |
| 56 | * .IRQ,FIQ,EA=0 (don't take aborts/exceptions to Monitor mode) |
| 57 | * .FW,AW=1 (CPSR.A,F modifiable in NS state) |
| 58 | * .nET=0 (early termination OK) |
| 59 | * .SCD=0 (SMC in NS mode OK, so we can call secure firmware) |
| 60 | * .HCE=1 (HVC does Hyp call) |
| 61 | */ |
| 62 | bic r1, r1, #0x07f |
| 63 | ldr r2, =0x131 |
| 64 | orr r1, r1, r2 |
| 65 | mcr p15, 0, r2, c1, c1, 0 @ SCR |
| 66 | isb |
| 67 | ldr r2, =mon_vectors |
| 68 | mcr p15, 4, r2, c12, c0, 0 @ set HVBAR |
| 69 | /* ...and return to calling code in NS state */ |
| 70 | movs pc, lr |
| 71 | |
| 72 | /****************************************************************************** |
| 73 | * This code is called from u-boot into the above handler |
| 74 | */ |
| 75 | |
| 76 | .globl monitor_init |
| 77 | monitor_init: |
| 78 | ldr ip, =mon_vectors |
| 79 | mcr p15, 0, ip, c12, c0, 1 @ Monitor vector base address |
| 80 | mov pc, lr |
| 81 | |
| 82 | /* Try to go into NS-SVC: void enter_ns(void); */ |
| 83 | .globl enter_ns |
| 84 | enter_ns: |
| 85 | smc #0 |
| 86 | mov pc, lr |
| 87 | |
| 88 | /* void enter_hyp(void); */ |
| 89 | .globl enter_hyp |
| 90 | enter_hyp: |
| 91 | /* Now we're in NS-SVC, make a Hyp call to get into Hyp mode */ |
| 92 | mov r0, lr |
| 93 | mov r1, sp |
| 94 | hvc #0 |
| 95 | /* We will end up here in NS-Hyp. */ |
| 96 | mov sp, r1 |
| 97 | mov pc, r0 |