forked from OP-TEE/optee_os
-
Notifications
You must be signed in to change notification settings - Fork 5
/
entry_a64.S
748 lines (653 loc) · 16.1 KB
/
entry_a64.S
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
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
/* SPDX-License-Identifier: BSD-2-Clause */
/*
* Copyright (c) 2015-2022, Linaro Limited
* Copyright (c) 2021, Arm Limited
*/
#include <platform_config.h>
#include <arm64_macros.S>
#include <arm.h>
#include <asm.S>
#include <generated/asm-defines.h>
#include <keep.h>
#include <kernel/thread_private.h>
#include <sm/optee_smc.h>
#include <sm/teesmc_opteed.h>
#include <sm/teesmc_opteed_macros.h>
/*
* Setup SP_EL0 and SPEL1, SP will be set to SP_EL0.
* SP_EL0 is assigned:
* stack_tmp + (cpu_id + 1) * stack_tmp_stride - STACK_TMP_GUARD
* SP_EL1 is assigned thread_core_local[cpu_id]
*/
.macro set_sp
bl __get_core_pos
cmp x0, #CFG_TEE_CORE_NB_CORE
/* Unsupported CPU, park it before it breaks something */
bge unhandled_cpu
add x0, x0, #1
adr_l x1, stack_tmp_stride
ldr w1, [x1]
mul x1, x0, x1
/* x0 = stack_tmp - STACK_TMP_GUARD */
adr_l x2, stack_tmp_rel
ldr w0, [x2]
add x0, x0, x2
msr spsel, #0
add sp, x1, x0
bl thread_get_core_local
msr spsel, #1
mov sp, x0
msr spsel, #0
.endm
.macro read_feat_mte reg
mrs \reg, id_aa64pfr1_el1
ubfx \reg, \reg, #ID_AA64PFR1_EL1_MTE_SHIFT, #4
.endm
.macro set_sctlr_el1
mrs x0, sctlr_el1
orr x0, x0, #SCTLR_I
orr x0, x0, #SCTLR_SA
orr x0, x0, #SCTLR_SPAN
#if defined(CFG_CORE_RWDATA_NOEXEC)
orr x0, x0, #SCTLR_WXN
#endif
#if defined(CFG_SCTLR_ALIGNMENT_CHECK)
orr x0, x0, #SCTLR_A
#else
bic x0, x0, #SCTLR_A
#endif
#ifdef CFG_MEMTAG
read_feat_mte x1
cmp w1, #1
b.ls 111f
orr x0, x0, #(SCTLR_ATA | SCTLR_ATA0)
bic x0, x0, #SCTLR_TCF_MASK
bic x0, x0, #SCTLR_TCF0_MASK
111:
#endif
#if defined(CFG_TA_PAUTH) && defined(CFG_TA_BTI)
orr x0, x0, #SCTLR_BT0
#endif
#if defined(CFG_CORE_PAUTH) && defined(CFG_CORE_BTI)
orr x0, x0, #SCTLR_BT1
#endif
msr sctlr_el1, x0
.endm
.macro init_memtag_per_cpu
read_feat_mte x0
cmp w0, #1
b.ls 11f
#ifdef CFG_TEE_CORE_DEBUG
/*
* This together with GCR_EL1.RRND = 0 will make the tags
* acquired with the irg instruction deterministic.
*/
mov_imm x0, 0xcafe00
msr rgsr_el1, x0
/* Avoid tag = 0x0 and 0xf */
mov x0, #0
#else
/*
* Still avoid tag = 0x0 and 0xf as we use that tag for
* everything which isn't explicitly tagged. Setting
* GCR_EL1.RRND = 1 to allow an implementation specific
* method of generating the tags.
*/
mov x0, #GCR_EL1_RRND
#endif
orr x0, x0, #1
orr x0, x0, #(1 << 15)
msr gcr_el1, x0
/*
* Enable the tag checks on the current CPU.
*
* Depends on boot_init_memtag() having cleared tags for
* TEE core memory. Well, not really, addresses with the
* tag value 0b0000 will use unchecked access due to
* TCR_TCMA0.
*/
mrs x0, tcr_el1
orr x0, x0, #TCR_TBI0
orr x0, x0, #TCR_TCMA0
msr tcr_el1, x0
mrs x0, sctlr_el1
orr x0, x0, #SCTLR_TCF_SYNC
orr x0, x0, #SCTLR_TCF0_SYNC
msr sctlr_el1, x0
isb
11:
.endm
.macro init_pauth_per_cpu
msr spsel, #1
ldp x0, x1, [sp, #THREAD_CORE_LOCAL_KEYS]
msr spsel, #0
write_apiakeyhi x0
write_apiakeylo x1
mrs x0, sctlr_el1
orr x0, x0, #SCTLR_ENIA
msr sctlr_el1, x0
isb
.endm
FUNC _start , :
/******************************************************************************
* RPi4:
* secure monitor is not running yet, therefore temporarily enable a conventional return (see below)
*/
stp x29, x30, [sp, -16]!
mov x29, sp
/*
* If CFG_CORE_FFA is enabled, then x0 if non-NULL holds the TOS FW
* config [1] address, else x0 if non-NULL holds the pagable part
* address.
*
* [1] A TF-A concept: TOS_FW_CONFIG - Trusted OS Firmware
* configuration file. Used by Trusted OS (BL32), that is, OP-TEE
* here.
*/
mov x19, x0
#if defined(CFG_DT_ADDR)
ldr x20, =CFG_DT_ADDR
#else
mov x20, x2 /* Save DT address */
#endif
adr x0, reset_vect_table
msr vbar_el1, x0
isb
set_sctlr_el1
isb
#ifdef CFG_WITH_PAGER
/*
* Move init code into correct location and move hashes to a
* temporary safe location until the heap is initialized.
*
* The binary is built as:
* [Pager code, rodata and data] : In correct location
* [Init code and rodata] : Should be copied to __init_start
* [struct boot_embdata + data] : Should be saved before
* initializing pager, first uint32_t tells the length of the data
*/
adr x0, __init_start /* dst */
adr x1, __data_end /* src */
adr x2, __init_end
sub x2, x2, x0 /* init len */
ldr w4, [x1, x2] /* length of hashes etc */
add x2, x2, x4 /* length of init and hashes etc */
/* Copy backwards (as memmove) in case we're overlapping */
add x0, x0, x2 /* __init_start + len */
add x1, x1, x2 /* __data_end + len */
adr x3, cached_mem_end
str x0, [x3]
adr x2, __init_start
copy_init:
ldp x3, x4, [x1, #-16]!
stp x3, x4, [x0, #-16]!
cmp x0, x2
b.gt copy_init
#else
/*
* The binary is built as:
* [Core, rodata and data] : In correct location
* [struct boot_embdata + data] : Should be moved to __end, first
* uint32_t tells the length of the struct + data
*/
adr_l x0, __end /* dst */
adr_l x1, __data_end /* src */
ldr w2, [x1] /* struct boot_embdata::total_len */
/* Copy backwards (as memmove) in case we're overlapping */
add x0, x0, x2
add x1, x1, x2
adr x3, cached_mem_end
str x0, [x3]
adr_l x2, __end
copy_init:
ldp x3, x4, [x1, #-16]!
stp x3, x4, [x0, #-16]!
cmp x0, x2
b.gt copy_init
#endif
/*
* Clear .bss, this code obviously depends on the linker keeping
* start/end of .bss at least 8 byte aligned.
*/
adr_l x0, __bss_start
adr_l x1, __bss_end
clear_bss:
str xzr, [x0], #8
cmp x0, x1
b.lt clear_bss
#ifdef CFG_NS_VIRTUALIZATION
/*
* Clear .nex_bss, this code obviously depends on the linker keeping
* start/end of .bss at least 8 byte aligned.
*/
adr x0, __nex_bss_start
adr x1, __nex_bss_end
clear_nex_bss:
str xzr, [x0], #8
cmp x0, x1
b.lt clear_nex_bss
#endif
/******************************************************************************
* RPi4:
* temporarily do not set a new stack to enable the conventional return (see below)
*/
/* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */
// set_sp
bl thread_init_thread_core_local
/* Enable aborts now that we can receive exceptions */
msr daifclr, #DAIFBIT_ABT
/*
* Invalidate dcache for all memory used during initialization to
* avoid nasty surprices when the cache is turned on. We must not
* invalidate memory not used by OP-TEE since we may invalidate
* entries used by for instance ARM Trusted Firmware.
*/
adr_l x0, __text_start
ldr x1, cached_mem_end
sub x1, x1, x0
bl dcache_cleaninv_range
/* Enable Console */
bl console_init
#ifdef CFG_MEMTAG
/*
* If FEAT_MTE2 is available, initializes the memtag callbacks.
* Tags for OP-TEE core memory are then cleared to make it safe to
* enable MEMTAG below.
*/
bl boot_init_memtag
#endif
#ifdef CFG_CORE_ASLR
mov x0, x20
bl get_aslr_seed
#else
mov x0, #0
#endif
adr x1, boot_mmu_config
bl core_init_mmu_map
#ifdef CFG_CORE_ASLR
/*
* Process relocation information again updating for the new
* offset. We're doing this now before MMU is enabled as some of
* the memory will become write protected.
*/
ldr x0, boot_mmu_config + CORE_MMU_CONFIG_LOAD_OFFSET
/*
* Update cached_mem_end address with load offset since it was
* calculated before relocation.
*/
adr x5, cached_mem_end
ldr x6, [x5]
add x6, x6, x0
str x6, [x5]
bl relocate
#endif
bl __get_core_pos
/******************************************************************************
* RPi4:
* enable_mmu hangs
*/
// bl enable_mmu
bl checkpoint
#ifdef CFG_CORE_ASLR
/*
* Reinitialize console, since register_serial_console() has
* previously registered a PA and with ASLR the VA is different
* from the PA.
*/
bl console_init
#endif
#ifdef CFG_NS_VIRTUALIZATION
/*
* Initialize partition tables for each partition to
* default_partition which has been relocated now to a different VA
*/
bl core_mmu_set_default_prtn_tbl
#endif
/******************************************************************************
* RPi4:
* Indicate that it ran to here.
*/
bl checkpoint1
mov x0, x19 /* pagable part address */
mov x1, #-1
bl boot_init_primary_early
/******************************************************************************
* RPi4:
* Indicate that it returned from boot_init_primary_early.
*/
bl checkpoint2
#ifdef CFG_MEMTAG
init_memtag_per_cpu
#endif
#ifndef CFG_NS_VIRTUALIZATION
mov x21, sp
adr_l x0, threads
ldr x0, [x0, #THREAD_CTX_STACK_VA_END]
mov sp, x0
bl thread_get_core_local
mov x22, x0
str wzr, [x22, #THREAD_CORE_LOCAL_FLAGS]
#endif
mov x0, x20 /* DT address */
bl boot_init_primary_late
/******************************************************************************
* RPi4:
* Indicate that it returned from boot_init_primary_late.
*/
bl checkpoint
#ifdef CFG_CORE_PAUTH
init_pauth_per_cpu
#endif
#ifndef CFG_NS_VIRTUALIZATION
mov x0, #THREAD_CLF_TMP
str w0, [x22, #THREAD_CORE_LOCAL_FLAGS]
mov sp, x21
#endif
#ifdef _CFG_CORE_STACK_PROTECTOR
/* Update stack canary value */
bl plat_get_random_stack_canary
adr_l x5, __stack_chk_guard
str x0, [x5]
#endif
/*
* In case we've touched memory that secondary CPUs will use before
* they have turned on their D-cache, clean and invalidate the
* D-cache before exiting to normal world.
*/
adr_l x0, __text_start
ldr x1, cached_mem_end
sub x1, x1, x0
bl dcache_cleaninv_range
/*
* Clear current thread id now to allow the thread to be reused on
* next entry. Matches the thread_init_boot_thread in
* boot.c.
*/
#ifndef CFG_NS_VIRTUALIZATION
bl thread_clr_boot_thread
#endif
#ifdef CFG_CORE_FFA
adr x0, cpu_on_handler
/*
* Compensate for the load offset since cpu_on_handler() is
* called with MMU off.
*/
ldr x1, boot_mmu_config + CORE_MMU_CONFIG_LOAD_OFFSET
sub x0, x0, x1
bl thread_spmc_register_secondary_ep
b thread_ffa_msg_wait
#else
/******************************************************************************
* RPi4:
* secure monitor is not running yet, therefore temporarily execute a conventional return
*/
bl checkpoint3
ldp x29, x30, [sp], 16
ret
/*
* Pass the vector address returned from main_init
* Compensate for the load offset since cpu_on_handler() is
* called with MMU off.
*/
ldr x0, boot_mmu_config + CORE_MMU_CONFIG_LOAD_OFFSET
adr x1, thread_vector_table
sub x1, x1, x0
mov x0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
smc #0
/* SMC should not return */
panic_at_smc_return
#endif
END_FUNC _start
DECLARE_KEEP_INIT _start
.section .identity_map.data
.balign 8
LOCAL_DATA cached_mem_end , :
.skip 8
END_DATA cached_mem_end
#ifdef CFG_CORE_ASLR
LOCAL_FUNC relocate , :
/* x0 holds load offset */
#ifdef CFG_WITH_PAGER
adr_l x6, __init_end
#else
adr_l x6, __end
#endif
ldp w2, w3, [x6, #BOOT_EMBDATA_RELOC_OFFSET]
mov_imm x1, TEE_RAM_START
add x2, x2, x6 /* start of relocations */
add x3, x3, x2 /* end of relocations */
/*
* Relocations are not formatted as Rela64, instead they are in a
* compressed format created by get_reloc_bin() in
* scripts/gen_tee_bin.py
*
* All the R_AARCH64_RELATIVE relocations are translated into a
* list list of 32-bit offsets from TEE_RAM_START. At each address
* a 64-bit value pointed out which increased with the load offset.
*/
#ifdef CFG_WITH_PAGER
/*
* With pager enabled we can only relocate the pager and init
* parts, the rest has to be done when a page is populated.
*/
sub x6, x6, x1
#endif
b 2f
/* Loop over the relocation addresses and process all entries */
1: ldr w4, [x2], #4
#ifdef CFG_WITH_PAGER
/* Skip too large addresses */
cmp x4, x6
b.ge 2f
#endif
add x4, x4, x1
ldr x5, [x4]
add x5, x5, x0
str x5, [x4]
2: cmp x2, x3
b.ne 1b
ret
END_FUNC relocate
#endif
/*
* void enable_mmu(unsigned long core_pos);
*
* This function depends on being mapped with in the identity map where
* physical address and virtual address is the same. After MMU has been
* enabled the instruction pointer will be updated to execute as the new
* offset instead. Stack pointers and the return address are updated.
*/
LOCAL_FUNC enable_mmu , : , .identity_map
adr x1, boot_mmu_config
load_xregs x1, 0, 2, 6
/*
* x0 = core_pos
* x2 = tcr_el1
* x3 = mair_el1
* x4 = ttbr0_el1_base
* x5 = ttbr0_core_offset
* x6 = load_offset
*/
msr tcr_el1, x2
msr mair_el1, x3
/*
* ttbr0_el1 = ttbr0_el1_base + ttbr0_core_offset * core_pos
*/
madd x1, x5, x0, x4
msr ttbr0_el1, x1
msr ttbr1_el1, xzr
isb
/* Invalidate TLB */
tlbi vmalle1
/*
* Make sure translation table writes have drained into memory and
* the TLB invalidation is complete.
*/
dsb sy
isb
/* Enable the MMU */
mrs x1, sctlr_el1
orr x1, x1, #SCTLR_M
msr sctlr_el1, x1
isb
/* Update vbar */
mrs x1, vbar_el1
add x1, x1, x6
msr vbar_el1, x1
isb
/* Invalidate instruction cache and branch predictor */
ic iallu
isb
/* Enable I and D cache */
mrs x1, sctlr_el1
orr x1, x1, #SCTLR_I
orr x1, x1, #SCTLR_C
msr sctlr_el1, x1
isb
/* Adjust stack pointers and return address */
msr spsel, #1
add sp, sp, x6
msr spsel, #0
add sp, sp, x6
add x30, x30, x6
ret
END_FUNC enable_mmu
.section .identity_map.data
.balign 8
DATA boot_mmu_config , : /* struct core_mmu_config */
.skip CORE_MMU_CONFIG_SIZE
END_DATA boot_mmu_config
FUNC cpu_on_handler , :
mov x19, x0
mov x20, x1
mov x21, x30
adr x0, reset_vect_table
msr vbar_el1, x0
isb
set_sctlr_el1
isb
/* Enable aborts now that we can receive exceptions */
msr daifclr, #DAIFBIT_ABT
bl __get_core_pos
bl enable_mmu
/* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */
set_sp
#ifdef CFG_MEMTAG
init_memtag_per_cpu
#endif
#ifdef CFG_CORE_PAUTH
init_pauth_per_cpu
#endif
mov x0, x19
mov x1, x20
#ifdef CFG_CORE_FFA
bl boot_cpu_on_handler
b thread_ffa_msg_wait
#else
mov x30, x21
b boot_cpu_on_handler
#endif
END_FUNC cpu_on_handler
DECLARE_KEEP_PAGER cpu_on_handler
LOCAL_FUNC unhandled_cpu , :
wfi
b unhandled_cpu
END_FUNC unhandled_cpu
LOCAL_DATA stack_tmp_rel , :
.word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD
END_DATA stack_tmp_rel
/*
* This macro verifies that the a given vector doesn't exceed the
* architectural limit of 32 instructions. This is meant to be placed
* immedately after the last instruction in the vector. It takes the
* vector entry as the parameter
*/
.macro check_vector_size since
.if (. - \since) > (32 * 4)
.error "Vector exceeds 32 instructions"
.endif
.endm
.section .identity_map, "ax", %progbits
.align 11
LOCAL_FUNC reset_vect_table , :, .identity_map, , nobti
/* -----------------------------------------------------
* Current EL with SP0 : 0x0 - 0x180
* -----------------------------------------------------
*/
SynchronousExceptionSP0:
b SynchronousExceptionSP0
check_vector_size SynchronousExceptionSP0
.align 7
IrqSP0:
b IrqSP0
check_vector_size IrqSP0
.align 7
FiqSP0:
b FiqSP0
check_vector_size FiqSP0
.align 7
SErrorSP0:
b SErrorSP0
check_vector_size SErrorSP0
/* -----------------------------------------------------
* Current EL with SPx: 0x200 - 0x380
* -----------------------------------------------------
*/
.align 7
SynchronousExceptionSPx:
b SynchronousExceptionSPx
check_vector_size SynchronousExceptionSPx
.align 7
IrqSPx:
b IrqSPx
check_vector_size IrqSPx
.align 7
FiqSPx:
b FiqSPx
check_vector_size FiqSPx
.align 7
SErrorSPx:
b SErrorSPx
check_vector_size SErrorSPx
/* -----------------------------------------------------
* Lower EL using AArch64 : 0x400 - 0x580
* -----------------------------------------------------
*/
.align 7
SynchronousExceptionA64:
b SynchronousExceptionA64
check_vector_size SynchronousExceptionA64
.align 7
IrqA64:
b IrqA64
check_vector_size IrqA64
.align 7
FiqA64:
b FiqA64
check_vector_size FiqA64
.align 7
SErrorA64:
b SErrorA64
check_vector_size SErrorA64
/* -----------------------------------------------------
* Lower EL using AArch32 : 0x0 - 0x180
* -----------------------------------------------------
*/
.align 7
SynchronousExceptionA32:
b SynchronousExceptionA32
check_vector_size SynchronousExceptionA32
.align 7
IrqA32:
b IrqA32
check_vector_size IrqA32
.align 7
FiqA32:
b FiqA32
check_vector_size FiqA32
.align 7
SErrorA32:
b SErrorA32
check_vector_size SErrorA32
END_FUNC reset_vect_table
BTI(emit_aarch64_feature_1_and GNU_PROPERTY_AARCH64_FEATURE_1_BTI)