diff options
| -rw-r--r-- | boot.S | 27 | ||||
| -rw-r--r-- | monitor.S | 19 |
2 files changed, 39 insertions, 7 deletions
@@ -9,6 +9,7 @@ .syntax unified .arch_extension sec + .arch_extension virt .text #ifdef SEMIHOSTING @@ -104,12 +105,34 @@ _start: orr r0, r0, r1 mcr p15, 0, r0, c1, c1, 2 - @ Change to NS-mode + @ Leave monitor.S trap in place for the transition... mov r0, #0xf0000000 mcr p15, 0, r0, c12, c0, 1 @ Monitor vector base address + + @ Set up hvbar so hvc comes back here. + ldr r0, =vectors + mov r7, #0xfffffff0 + smc #0 @ Set HVBAR + + @ We can't call hvc from secure mode, so drop down first. mov r7, #0xffffffff smc #0 @ Change to NS-mode + @ This is how we enter hyp mode, for booting the next stage. + hvc #0 + +/* Once we get rid of monitor.S, use these smc vectors too! */ +vectors: + .word 0 /* reset */ + .word 0 /* undef */ + .word 0 /* svc */ + .word 0 /* pabt */ + .word 0 /* dabt */ + b into_hyp_mode /* hvc */ + .word 0 /* irq */ + .word 0 /* fiq */ + +into_hyp_mode: @ Check CPU nr again mrc p15, 0, r0, c0, c0, 5 @ MPIDR (ARMv7 only) and r0, r0, #15 @ CPU number @@ -485,7 +508,7 @@ sh_cmdline: @ Semihosting command line will be written here #else /* not SEMIHOSTING */ - .org 0x100 + .org 0x200 @ Static ATAGS for when kernel/etc are compiled into the ELF file atags: @ ATAG_CORE @@ -25,7 +25,7 @@ @ 1: ldr sp, =_monitor_stack - push {r11, r12} + push {r10-r12} cmp r7, #0xffffffff beq _non_sec @@ -36,15 +36,20 @@ movnes pc, lr and r12, r7, #0xf cmp r12, #0x0 - popgt {r11, r12} + popgt {r10-r12} movgts pc, lr @ Check the VMID is 0 + mrc p15, 0, r10, c1, c1, 0 @ SCR + orr r11, r10, #1 @ SCR.NS = 1 + mcr p15, 0, r11, c1, c1, 0 + isb mrrc p15, 6, r12, r11, c2 + mcr p15, 0, r10, c1, c1, 0 @ Restore SCR lsr r11, r11, #16 and r11, r11, #0xff cmp r11, #0 - popne {r11, r12} + popne {r10-r12} movnes pc, lr @ Jump to the right function @@ -68,15 +73,19 @@ _non_sec: ldr r11, =0x131 orr r12, r12, r11 mcr p15, 0, r12, c1, c1, 0 - pop {r11, r12} + pop {r10-r12} movs pc, lr @ @ Read/Write HVBAR @ _write_hvbar: + orr r11, r10, #1 @ SCR.NS = 1 (r10 already = SCR) + mcr p15, 0, r11, c1, c1, 0 + isb mcr p15, 4, r0, c12, c0, 0 - pop {r11, r12} + mcr p15, 0, r10, c1, c1, 0 @ Restore SCR + pop {r10-r12} movs pc, lr .ltorg |