1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
#include <linux/linkage.h>
#include <asm/system.h>
#include <asm/opcodes-virt.h>
#include <init.h>
.arch_extension sec
.arch_extension virt
__BARE_INIT
.data
.align 2
ENTRY(__boot_cpu_mode)
.long 0
.text
ENTRY(__hyp_install)
mrs r12, cpsr
and r12, r12, #MODE_MASK
@ Save the initial CPU state
adr r0, .L__boot_cpu_mode_offset
ldr r1, [r0]
str r12, [r0, r1]
cmp r12, #HYP_MODE
movne pc, lr @ give up if the CPU is not in HYP mode
@ Now install the hypervisor stub:
adr r12, __hyp_vectors
mcr p15, 4, r12, c12, c0, 0 @ set hypervisor vector base (HVBAR)
@ Disable all traps, so we don't get any nasty surprise
mov r12, #0
mcr p15, 4, r12, c1, c1, 0 @ HCR
mcr p15, 4, r12, c1, c1, 2 @ HCPTR
mcr p15, 4, r12, c1, c1, 3 @ HSTR
THUMB( orr r12, #(1 << 30) ) @ HSCTLR.TE
mcr p15, 4, r12, c1, c0, 0 @ HSCTLR
mrc p15, 4, r12, c1, c1, 1 @ HDCR
and r12, #0x1f @ Preserve HPMN
mcr p15, 4, r12, c1, c1, 1 @ HDCR
@ Make sure NS-SVC is initialised appropriately
mrc p15, 0, r12, c1, c0, 0 @ SCTLR
orr r12, #(1 << 5) @ CP15 barriers enabled
bic r12, #(3 << 7) @ Clear SED/ITD for v8 (RES0 for v7)
bic r12, #(3 << 19) @ WXN and UWXN disabled
mcr p15, 0, r12, c1, c0, 0 @ SCTLR
mrc p15, 0, r12, c0, c0, 0 @ MIDR
mcr p15, 4, r12, c0, c0, 0 @ VPIDR
mrc p15, 0, r12, c0, c0, 5 @ MPIDR
mcr p15, 4, r12, c0, c0, 5 @ VMPIDR
bx lr
ENDPROC(__hyp_install)
ENTRY(armv7_hyp_install)
mov r2, lr
bl __hyp_install
/* set the cpu to SVC32 mode, mask irq and fiq */
mrs r12, cpsr
eor r12, r12, #HYP_MODE
tst r12, #MODE_MASK
bic r12, r12, #MODE_MASK
orr r12, r12, #(PSR_I_BIT | PSR_F_BIT | SVC_MODE)
THUMB( orr r12, r12, #PSR_T_BIT )
bne 1f
orr r12, r12, #PSR_A_BIT
adr lr, 2f
msr spsr_cxsf, r12
__MSR_ELR_HYP(14)
__ERET
1: msr cpsr_c, r12
2:
mov pc, r2
ENDPROC(armv7_hyp_install)
ENTRY(armv7_switch_to_hyp)
mov r0, lr
mov r1, sp @ save SVC copy of LR and SP
isb
hvc #0 @ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
mov sp, r1
mov lr, r0 @ restore SVC copy of LR and SP
bx lr
ENDPROC(armv7_switch_to_hyp)
.align 2
.L__boot_cpu_mode_offset:
.long __boot_cpu_mode - .
/* The HYP trap is crafted to match armv7_switch_to_hyp() */
__hyp_do_trap:
mov lr, r0
mov sp, r1
bx lr
ENDPROC(__hyp_do_trap)
.align 5
__hyp_vectors:
__hyp_reset: W(b) .
__hyp_und: W(b) .
__hyp_svc: W(b) .
__hyp_pabort: W(b) .
__hyp_dabort: W(b) .
__hyp_trap: W(b) __hyp_do_trap
__hyp_irq: W(b) .
__hyp_fiq: W(b) .
ENDPROC(__hyp_vectors)
|