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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
|
;
; Copyright (c) 2012, ARM Limited. All rights reserved.
;
; Redistribution and use in source and binary forms, with
; or without modification, are permitted provided that the
; following conditions are met:
;
; Redistributions of source code must retain the above
; copyright notice, this list of conditions and the
; following disclaimer.
;
; Redistributions in binary form must reproduce the
; above copyright notice, this list of conditions and
; the following disclaimer in the documentation
; and/or other materials provided with the distribution.
;
; Neither the name of ARM nor the names of its
; contributors may be used to endorse or promote products
; derived from this software without specific prior written
; permission.
;
AREA HypVectors, CODE, READONLY, ALIGN=5
PRESERVE8
IMPORT handle_interrupt
IMPORT HandleVirtualisor
IMPORT guestos_state
IMPORT guestos_state_size
IMPORT get_sp
IMPORT output_string
IMPORT virt_dead
IMPORT SetupVirtExtPageTables
IMPORT Enable2ndStagePageTables
IMPORT restore_context
IMPORT read_hsctlr
IMPORT write_hsctlr
IMPORT write_hmair0
IMPORT write_httbr
IMPORT write_htcr
IMPORT bl_rest_init
IMPORT hyp_l1_pagetable
IMPORT find_restore_op_type
IF ASYNC_SWITCH = {FALSE}
IMPORT is_hvc
IMPORT HandleHVC
ENDIF
EXPORT vectors
EXPORT iabt_entry
EXPORT dabt_entry
EXPORT undef_entry
EXPORT svc_hyp_entry
EXPORT fiq_entry
EXPORT bl_setup
EXPORT hyp_warm_reset_handler
MACRO
hyp_entry $reg
SUB $reg, $reg, #72
; ---------------------------------------------------
; Save all GP registers
; Save User mode LR which the HYP mode will use now.
; Save HYP mode ELR & SPSR in case we are re-entrant
; Pass saved context as argument to next bit of code
; ---------------------------------------------------
STMIA $reg, {r0-r12}
MRS r0, ELR_hyp
MRS r1, SPSR
MRS r2, LR_usr
ADD r3, $reg, #60
STMIA r3, {r0-r2}
MEND
MACRO
hyp_exit $reg
ADD r3, $reg, #60
LDMIA r3, {r0-r2}
MSR ELR_hyp, r0
MSR SPSR_cxsf, r1
MSR LR_usr, r2
; ----------------------------------------------------
; We do need to clear the BTAC though since it is
; virtually-addressed with no regard for the NS bit
; ----------------------------------------------------
MCR p15, 0, r0, c7, c5, 6 ; invalidate BTAC
LDMIA $reg, {r0-r12}
ADD $reg, $reg, #72
ERET
MEND
IF {FALSE}
dabort_string
DCB " Virtualisor-DAB!\n", 0
undef_string
DCB " Virtualisor-UND!\n", 0
pabort_string
DCB " Virtualisor-PAB!\n", 0
swi_string
DCB " Virtualisor-SWI!\n", 0
irq_string
DCB " Virtualisor-IRQ!\n", 0
fiq_string
DCB " Virtualisor-FIQ!\n", 0
unused_string
DCB " Virtualisor-UNU!\n", 0
ALIGN
ENDIF
; ----------------------------------------------------
; Defines for enabling HYP mode MMU
; ----------------------------------------------------
ENABLE EQU 0x1
DISABLE EQU 0x0
; ----------------------------------------------------
; HMAIR attributes relevant to us
; ----------------------------------------------------
HMAIR_INNER_WB_RWA_MEM EQU 0x0f
HMAIR_OUTER_WB_RWA_MEM EQU 0xf0
HMAIR_DEVICE_MEM EQU 0x04
HMAIR_SO_MEM EQU 0x00
IDX0 EQU (HMAIR_DEVICE_MEM << 0)
IDX1 EQU ((HMAIR_INNER_WB_RWA_MEM :OR: HMAIR_OUTER_WB_RWA_MEM) << 8)
IDX2 EQU (HMAIR_SO_MEM << 16)
; ----------------------------------------------------
; HSCTLR defines
; ----------------------------------------------------
ICACHE EQU (ENABLE << 12)
ALIGN EQU (ENABLE << 1)
DCACHE EQU (ENABLE << 2)
MMU EQU (ENABLE << 0)
; ----------------------------------------------------
; HTCR defines
; ----------------------------------------------------
CR_C_WBWA EQU 0x1
CR_OUTER_SH EQU 0x2
CR_INNER_SH EQU 0x3
CR_ADDR_SPC_4GB EQU 0x0
EAE EQU (ENABLE << 31)
T0SZ EQU (CR_ADDR_SPC_4GB << 0)
IRGN0 EQU (CR_C_WBWA << 8)
ORGN0 EQU (CR_C_WBWA << 10)
SH0 EQU (CR_INNER_SH << 12)
vectors
B bl_setup ; reset
B undef_entry ; undef
B svc_hyp_entry ; swi
B iabt_entry ; pabt
B dabt_entry ; dabt
B hvc_entry ; HVC
B irq_entry ; irq
B fiq_entry ; fiq
bl_setup FUNCTION
; ----------------------------------------------------
; This function is called after a reset. 'r0-r3' can
; be corrupted after a cold reset.
; Its also assumed that we are taking part in coherency
; already (entered in secure world)
; ----------------------------------------------------
; ----------------------------------------------------
; Enable Caches
; ----------------------------------------------------
mrc p15, 4, r0, c1, c0, 0
orr r0, #ICACHE
orr r0, #ALIGN
orr r0, #DCACHE
mcr p15, 4, r0, c1, c0, 0
isb
msr elr_hyp, lr
; ----------------------------------------------------
; Give yourself a stack without enabling the MMU so
; that the pagetables can be created in C code.
; ----------------------------------------------------
; ----------------------------------------------------
; Allocate the HYP stack first up to do greater things
; ----------------------------------------------------
ldr r0, =guestos_state
ldr r1, =guestos_state_size
ldr r1, [r1]
bl get_sp
mov sp, r0
; ----------------------------------------------------
; Create the 2nd stage and HYP mode page tables
; ----------------------------------------------------
bl SetupVirtExtPageTables
; ----------------------------------------------------
; Enable the HYP mode MMU before doing anything further
; ----------------------------------------------------
ldr r0, =hyp_l1_pagetable
MOV r1, #0
mcrr p15, 4, r0, r1, c2
ldr r0, =(IDX2 :OR: IDX1 :OR: IDX0)
mcr p15, 4, r0, c10, c2, 0
ldr r0, =(EAE :OR: SH0 :OR: ORGN0 :OR: IRGN0 :OR: T0SZ)
mcr p15, 4, r0, c2, c0, 2
mrc p15, 4, r0, c1, c0, 0
orr r0, #MMU
mcr p15, 4, r0, c1, c0, 0
dsb
isb
; ----------------------------------------------------
; Initialise the remaining bits now that the MMU is on
; ----------------------------------------------------
hyp_entry sp
bl bl_rest_init
hyp_exit sp
ENDFUNC
IF {FALSE}
common_abt
PUSH {lr}
BL hexword ; print r0
MRC p15, 0, r0, c5, c0, 0 ; DFSR
BL hexword
MRC p15, 0, r0, c6, c0, 0 ; DFAR
BL hexword
MRC p15, 4, r0, c5, c2, 0 ; HSR
BL hexword
MRC p15, 4, r0, c6, c0, 0 ; HDFAR
BL hexword
MRC p15, 4, r0, c6, c0, 2 ; HIFAR
BL hexword
MRC p15, 4, r0, c6, c0, 4 ; HPFAR
BL hexword
POP {lr}
BX lr
dabt_entry
MOV r0, lr ; save lr, just in case it's interesting
IF {FALSE}
BL common_abt
ENDIF
LDR r0, =dabort_string
BL output_string
B dead
iabt_entry
MOV r0, lr ; save lr, just in case it's interesting
IF {FALSE}
BL common_abt
ENDIF
LDR r0, =pabort_string
BL output_string
B dead
undef_entry
MOV r0, lr ; save lr, just in case it's interesting
IF {FALSE}
BL common_abt
ENDIF
LDR r0, =undef_string
BL output_string
B dead
dead
B dead
ENDIF
dabt_entry
B dabt_entry
iabt_entry
B iabt_entry
undef_entry
B undef_entry
irq_entry
hyp_entry sp
; ----------------------------------------------------
; Pass SP as arg if we intend to initiate a switchover
; ----------------------------------------------------
MOV r0, sp
BL handle_interrupt
hyp_exit sp
svc_hyp_entry
B svc_hyp_entry
fiq_entry
B fiq_entry
hvc_entry
hyp_entry sp
; ----------------------------------------------------
; Check if we have an HVC call. The Switcher handles
; it first. If its unable to, its passed to the
; Virtualisor. It should be possible to cascade an HVC
; across the two, but not for the time being.
; ----------------------------------------------------
IF ASYNC_SWITCH = {FALSE}
BL is_hvc
CMP r0, #0
BEQ next
MOV r0, sp
BL HandleHVC
TST r0, #1
BNE out
ENDIF
next
MOV r0, sp
BL HandleVirtualisor
out
hyp_exit sp
hyp_warm_reset_handler FUNCTION
; ----------------------------------------------------
; Enable Caches
; ----------------------------------------------------
mrc p15, 4, r0, c1, c0, 0
orr r0, #ICACHE
orr r0, #ALIGN
orr r0, #DCACHE
mcr p15, 4, r0, c1, c0, 0
isb
; ----------------------------------------------------
; Enable the HYP mode MMU before doing anything further
; ----------------------------------------------------
ldr r0, =hyp_l1_pagetable
MOV r1, #0
mcrr p15, 4, r0, r1, c2
ldr r0, =(IDX2 :OR: IDX1 :OR: IDX0)
mcr p15, 4, r0, c10, c2, 0
ldr r0, =(EAE :OR: SH0 :OR: ORGN0 :OR: IRGN0 :OR: T0SZ)
mcr p15, 4, r0, c2, c0, 2
mrc p15, 4, r0, c1, c0, 0
orr r0, #MMU
mcr p15, 4, r0, c1, c0, 0
dsb
isb
; ----------------------------------------------------
; Initialise the remaining bits now that the MMU is on
; ----------------------------------------------------
; ----------------------------------------------------
; Allocate the HYP stack first up to do greater things
; ----------------------------------------------------
ldr r0, =guestos_state
ldr r1, =guestos_state_size
ldr r1, [r1]
bl get_sp
mov sp, r0
; ----------------------------------------------------
; Initialise the HVBAR
; ----------------------------------------------------
adr r0, vectors
mcr p15, 4, r0, c12, c0, 0
; ----------------------------------------------------
; Initialise the 2nd stage translations for NS PL0/1
; ----------------------------------------------------
bl Enable2ndStagePageTables
; ----------------------------------------------------
; Restore the context now. CPU0 is the first cpu
; ----------------------------------------------------
hyp_entry sp
bl find_restore_op_type
mov r1, r0
mov r0, #0
bl restore_context
hyp_exit sp
ENDFUNC
END
|
