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
403
404
405
406
407
408
|
;
; 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 |monmode_vectors|, CODE, ALIGN=5
PRESERVE8
SMC_SEC_INIT EQU 0x0
SMC_SEC_SAVE EQU 0x1
SMC_SEC_SHUTDOWN EQU 0x2
L1 EQU 0x0
L2 EQU 0x1
INV EQU 0x0
CLN EQU 0x1
CLN_INV EQU 0x2
CR_M EQU (1<<0)
CR_C EQU (1<<2)
CR_I EQU (1<<12)
CR_Z EQU (1<<11)
CR_U EQU (1<<22)
CR_TRE EQU (1<<28)
SCR_NS EQU 0x01
PT_IRGN EQU (1<<0)
PT_RGN EQU (1<<3)
PT_SH EQU (1<<1)
PT_NOS EQU (1<<5)
TTBR0_PROP EQU (PT_NOS :OR: PT_SH :OR: PT_RGN :OR: PT_IRGN)
SO_MEM EQU 0x0
DV_MEM EQU 0x1
NM_MEM EQU 0x2
I_SH EQU 0x1
SH EQU 0x1
PRRR_TR0 EQU (SO_MEM<<0)
PRRR_TR1 EQU (DV_MEM<<2)
PRRR_TR4 EQU (NM_MEM<<8)
PRRR_TR7 EQU (NM_MEM<<14)
PRRR_DS1 EQU (SH<<17)
PRRR_NS1 EQU (SH<<19)
PRRR_NOS1 EQU (I_SH<<25)
PRRR_NOS4 EQU (I_SH<<28)
PRRR_NOS7 EQU (I_SH<<31)
NC EQU 0x0
WBWA EQU 0x1
NMRR_OR4 EQU (NC<<24)
NMRR_OR7 EQU (WBWA<<30)
NMRR_IR4 EQU (NC<<8)
NMRR_IR7 EQU (WBWA<<14)
; ==============================================================================
; These should be the same the defines in misc.h
; ==============================================================================
MAX_CLUSTERS EQU 2
MAX_CPUS EQU 8
STACK_SIZE EQU (96 << 2)
; ==============================================================================
; Simple vector table
; ==============================================================================
IMPORT ns_entry_ptr
IMPORT secure_context_save
IMPORT enable_caches
IMPORT inv_icache_all
IMPORT flat_pagetables
IMPORT read_sctlr
IMPORT write_sctlr
IMPORT read_ttbr0
IMPORT write_ttbr0
IMPORT inv_tlb_all
IMPORT inv_bpred_all
IMPORT write_dacr
IMPORT write_prrr
IMPORT write_nmrr
IMPORT get_sp
IMPORT secure_context_restore
IMPORT do_power_op
IMPORT get_powerdown_stack
IMPORT wfi
IMPORT read_cpuid
IMPORT add_dv_page
EXPORT monmode_vector_table
EXPORT warm_reset
; ----------------------------------------------------
; Macro to initialise MMU. Corrupts 'r0'
; ----------------------------------------------------
MACRO
setup_mmu $r1, $r2
MOV $r1, #0x5555
MOVT $r1, #0x5555
; Enable our page tables if not
LDR r0, =flat_pagetables
ORR r0, #TTBR0_PROP
; Write TTBR0
MCR p15, 0, r0, c2, c0, 0
; Write DACR
MCR p15, 0, $r1, c3, c0, 0
; Enable the remap registers to treat OSH memory as ISH memory
MOV $r1, #PRRR_TR0
ORR $r1, #PRRR_TR1
ORR $r1, #PRRR_TR4
ORR $r1, #PRRR_TR7
ORR $r1, #PRRR_NOS1
ORR $r1, #PRRR_NOS4
ORR $r1, #PRRR_NOS7
ORR $r1, #PRRR_NS1
ORR $r1, #PRRR_DS1
MOV $r2, #NMRR_IR4
ORR $r2, #NMRR_IR7
ORR $r2, #NMRR_OR4
ORR $r2, #NMRR_OR7
MCR p15, 0, $r1, c10, c2, 0
MCR p15, 0, $r2, c10, c2, 1
; Enable Dcache, TEX Remap & MMU
MRC p15, 0, r0, c1, c0, 0
ORR r0, #CR_M
ORR r0, #CR_C
ORR r0, #CR_TRE
MCR p15, 0, r0, c1, c0, 0
DSB
ISB
MEND
; ----------------------------------------------------
; Macro to setup secure stacks, Corrupts 'r0-r3'
; ----------------------------------------------------
MACRO
setup_stack
LDR r0, =secure_stacks
MOV r1, #STACK_SIZE
BL get_sp
MOV sp, r0
MEND
ALIGN 32
monmode_vector_table
monmode_reset_vec
B monmode_reset_vec
monmode_undef_vec
B monmode_undef_vec
monmode_smc_vec
B do_smc
monmode_pabort_vec
B monmode_pabort_vec
monmode_dabort_vec
B monmode_dabort_vec
monmode_unused_vec
B monmode_unused_vec
monmode_irq_vec
B monmode_irq_vec
monmode_fiq_vec
B monmode_fiq_vec
; SMC handler. Currently accepts three types of calls:
; 1. Init: Sets up stack, mmu, caches & coherency
; 2. Context Save: Saves the secure world context
; 3. Powerdown: Cleans the caches and power downs the cluster
; Also assumes the availability of r4-r7
do_smc FUNCTION
; Switch to non-secure banked registers
MRC p15, 0, r3, c1, c1, 0
BIC r3, #SCR_NS
MCR p15, 0, r3, c1, c1, 0
ISB
; Check if we are being called to setup the world
CMP r0, #SMC_SEC_INIT
BEQ setup_secure
CMP r0, #SMC_SEC_SAVE
BEQ save_secure
CMP r0, #SMC_SEC_SHUTDOWN
BEQ shutdown
smc_done
; Return to non-secure banked registers
MRC p15, 0, r0, c1, c1, 0
ORR r0, #SCR_NS
MCR p15, 0, r0, c1, c1, 0
ISB
ERET
ENDFUNC
shutdown
MOV r4, r1
MOV r5, r2
; ----------------------------------------------------
; We need to switch stacks from being resident in normal
; WBWA/S memory to SO memory to prevent potential stack
; corruption after turning off the C bit in the SCTLR.
; Subsequent accesses will be SO while there will be
; valid cache lines of the stack from prior accesses
; ----------------------------------------------------
BL read_cpuid
BL get_powerdown_stack
MOV sp, r0
MOV r0, r4
MOV r1, r5
BL do_power_op
enter_wfi
BL wfi
B enter_wfi
save_secure
PUSH {lr}
MOV r0, r1
MOV r1, r2
BL secure_context_save
POP {lr}
B smc_done
setup_secure
; Save the LR
MOV r4, lr
; Turn on the I cache, branch predictor and alingment
BL read_sctlr
ORR r0, #CR_I
ORR r0, #CR_U
ORR r0, #CR_Z
BL write_sctlr
dsb
isb
setup_stack
; ----------------------------------------------------
; Safely turn on caches
; TODO: Expensive usage of stacks as we are executing
; out of SO memory. Done only once so can live with it
; ----------------------------------------------------
BL enable_caches
; ----------------------------------------------------
; Add a page backed by device memory for locks & stacks
; ----------------------------------------------------
LDR r0, =flat_pagetables
BL add_dv_page
setup_mmu r1, r2
; Restore LR
MOV lr, r4
B smc_done
warm_reset FUNCTION
; ----------------------------------------------------
; Start the SO load of the pagetables asap
; ----------------------------------------------------
LDR r4, =flat_pagetables
; ----------------------------------------------------
; Enable I, C, Z, U bits in the SCTLR and SMP bit in
; the ACTLR right after reset
; ----------------------------------------------------
MRC p15, 0, r0, c1, c0, 0
ORR r0, r0, #CR_I
ORR r0, r0, #CR_U
ORR r0, r0, #CR_Z
ORR r0, r0, #CR_C
MCR p15, 0, r0, c1, c0, 0
MRC p15, 0, r1, c1, c0, 1
ORR r1, r1, #0x40
MCR p15, 0, r1, c1, c0, 1
ISB
; ----------------------------------------------------
; Enable the MMU even though CCI snoops have not been
; enabled. Should not be a problem as we will not
; access any inter-cluster data till we do so
; ----------------------------------------------------
MOV r2, #0x5555
MOVT r2, #0x5555
; Enable our page tables if not
ORR r4, #TTBR0_PROP
; Write TTBR0
MCR p15, 0, r4, c2, c0, 0
; Write DACR
MCR p15, 0, r2, c3, c0, 0
; Enable the remap registers to treat OSH memory as ISH memory
MOV r2, #PRRR_TR0
ORR r2, #PRRR_TR1
ORR r2, #PRRR_TR4
ORR r2, #PRRR_TR7
ORR r2, #PRRR_NOS1
ORR r2, #PRRR_NOS4
ORR r2, #PRRR_NOS7
ORR r2, #PRRR_NS1
ORR r2, #PRRR_DS1
MOV r3, #NMRR_IR4
ORR r3, #NMRR_IR7
ORR r3, #NMRR_OR4
ORR r3, #NMRR_OR7
MCR p15, 0, r2, c10, c2, 0
MCR p15, 0, r3, c10, c2, 1
; Enable Dcache, TEX Remap & MMU
MRC p15, 0, r0, c1, c0, 0
ORR r0, #CR_M
ORR r0, #CR_C
ORR r0, #CR_TRE
MCR p15, 0, r0, c1, c0, 0
ISB
; ----------------------------------------------------
; Try Preloading the literal pools before they are
; accessed.
; ----------------------------------------------------
ADR r4, warm_reset_ltrls
PLD [r4]
PLD warm_reset_ltrls
LDR r6, =secure_stacks
; ----------------------------------------------------
; Safely turn on CCI snoops
; ----------------------------------------------------
MOV r4, #0x0
MOVT r4, #0x2c09
MRC p15, 0, r0, c0, c0, 5
UBFX r1, r0, #0, #8
UBFX r2, r0, #8, #8
MOV r3, #3
CMP r2, #0
BEQ a15_snoops
MOV r5, #0x5000
CMP r1, #0
BNE cci_snoop_status
STR r3, [r4, r5]
B cci_snoop_status
a15_snoops
MOV r5, #0x4000
CMP r1, #0
BNE cci_snoop_status
STR r3, [r4, r5]
cci_snoop_status
LDR r0, [r4, r5]
TST r0, #3
BEQ cci_snoop_status
LDR r0, [r4, #0xc]
TST r0, #1
BNE cci_snoop_status
; ----------------------------------------------------
; Switch to Monitor mode straight away as we do not want to worry
; about setting up Secure SVC stacks. All Secure world save/restore
; takes place in the monitor mode.
; ----------------------------------------------------
MRS r5, cpsr ; Get current mode (SVC) in r0
BIC r1, r5, #0x1f ; Clear all mode bits
ORR r1, r1, #0x16 ; Set bits for Monitor mode
MSR cpsr_cxsf, r1 ; We are now in Monitor Mode
BIC r1, r5, #0x1f ; Clear all mode bits
ORR r1, r1, #0x1a ; Set bits for a return to the HYP mode
MSR spsr_cxsf, r1
MOV r0, r6
MOV r1, #STACK_SIZE
BL get_sp
MOV sp, r0
; Restore secure world context & enable MMU
BL secure_context_restore
; Switch to non-secure registers for HYP &
; later non-secure world restore.
MRC p15, 0, r1, c1, c1, 0
ORR r1, #SCR_NS
MCR p15, 0, r1, c1, c1, 0
ISB
; Setup the NS link register
MRC p15, 0, r0, c0, c0, 5
ANDS r0, r0, #0xf
LDR r1, =ns_entry_ptr
ADD r1, r1, r0, lsl #2
LDR lr, [r1]
; Switch to Non-secure world
ERET
warm_reset_ltrls
ENDFUNC
AREA stacks, DATA, ALIGN=6
secure_stacks SPACE MAX_CLUSTERS*MAX_CPUS*STACK_SIZE
END
|