1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2015-2022, Linaro Limited 4 * Copyright (c) 2021-2023, Arm Limited 5 */ 6 7#include <platform_config.h> 8 9#include <arm64_macros.S> 10#include <arm.h> 11#include <asm.S> 12#include <generated/asm-defines.h> 13#include <keep.h> 14#include <kernel/thread_private.h> 15#include <sm/optee_smc.h> 16#include <sm/teesmc_opteed.h> 17#include <sm/teesmc_opteed_macros.h> 18 19 /* 20 * Setup SP_EL0 and SPEL1, SP will be set to SP_EL0. 21 * SP_EL0 is assigned: 22 * stack_tmp + (cpu_id + 1) * stack_tmp_stride - STACK_TMP_GUARD 23 * SP_EL1 is assigned thread_core_local[cpu_id] 24 */ 25 .macro set_sp 26 bl __get_core_pos 27 cmp x0, #CFG_TEE_CORE_NB_CORE 28 /* Unsupported CPU, park it before it breaks something */ 29 bge unhandled_cpu 30 add x0, x0, #1 31 adr_l x1, stack_tmp_stride 32 ldr w1, [x1] 33 mul x1, x0, x1 34 35 /* x0 = stack_tmp - STACK_TMP_GUARD */ 36 adr_l x2, stack_tmp_rel 37 ldr w0, [x2] 38 add x0, x0, x2 39 40 msr spsel, #0 41 add sp, x1, x0 42 bl thread_get_core_local 43 msr spsel, #1 44 mov sp, x0 45 msr spsel, #0 46 .endm 47 48 .macro read_feat_mte reg 49 mrs \reg, id_aa64pfr1_el1 50 ubfx \reg, \reg, #ID_AA64PFR1_EL1_MTE_SHIFT, #4 51 .endm 52 53 .macro set_sctlr_el1 54 mrs x0, sctlr_el1 55 orr x0, x0, #SCTLR_I 56 orr x0, x0, #SCTLR_SA 57 orr x0, x0, #SCTLR_SPAN 58#if defined(CFG_CORE_RWDATA_NOEXEC) 59 orr x0, x0, #SCTLR_WXN 60#endif 61#if defined(CFG_SCTLR_ALIGNMENT_CHECK) 62 orr x0, x0, #SCTLR_A 63#else 64 bic x0, x0, #SCTLR_A 65#endif 66#ifdef CFG_MEMTAG 67 read_feat_mte x1 68 cmp w1, #1 69 b.ls 111f 70 orr x0, x0, #(SCTLR_ATA | SCTLR_ATA0) 71 bic x0, x0, #SCTLR_TCF_MASK 72 bic x0, x0, #SCTLR_TCF0_MASK 73111: 74#endif 75#if defined(CFG_TA_PAUTH) && defined(CFG_TA_BTI) 76 orr x0, x0, #SCTLR_BT0 77#endif 78#if defined(CFG_CORE_PAUTH) && defined(CFG_CORE_BTI) 79 orr x0, x0, #SCTLR_BT1 80#endif 81 msr sctlr_el1, x0 82 .endm 83 84 .macro init_memtag_per_cpu 85 read_feat_mte x0 86 cmp w0, #1 87 b.ls 11f 88 89#ifdef CFG_TEE_CORE_DEBUG 90 /* 91 * This together with GCR_EL1.RRND = 0 will make the tags 92 * acquired with the irg instruction deterministic. 93 */ 94 mov_imm x0, 0xcafe00 95 msr rgsr_el1, x0 96 /* Avoid tag = 0x0 and 0xf */ 97 mov x0, #0 98#else 99 /* 100 * Still avoid tag = 0x0 and 0xf as we use that tag for 101 * everything which isn't explicitly tagged. Setting 102 * GCR_EL1.RRND = 1 to allow an implementation specific 103 * method of generating the tags. 104 */ 105 mov x0, #GCR_EL1_RRND 106#endif 107 orr x0, x0, #1 108 orr x0, x0, #(1 << 15) 109 msr gcr_el1, x0 110 111 /* 112 * Enable the tag checks on the current CPU. 113 * 114 * Depends on boot_init_memtag() having cleared tags for 115 * TEE core memory. Well, not really, addresses with the 116 * tag value 0b0000 will use unchecked access due to 117 * TCR_TCMA0. 118 */ 119 mrs x0, tcr_el1 120 orr x0, x0, #TCR_TBI0 121 orr x0, x0, #TCR_TCMA0 122 msr tcr_el1, x0 123 124 mrs x0, sctlr_el1 125 orr x0, x0, #SCTLR_TCF_SYNC 126 orr x0, x0, #SCTLR_TCF0_SYNC 127 msr sctlr_el1, x0 128 129 isb 13011: 131 .endm 132 133 .macro init_pauth_per_cpu 134 msr spsel, #1 135 ldp x0, x1, [sp, #THREAD_CORE_LOCAL_KEYS] 136 msr spsel, #0 137 write_apiakeyhi x0 138 write_apiakeylo x1 139 mrs x0, sctlr_el1 140 orr x0, x0, #SCTLR_ENIA 141 msr sctlr_el1, x0 142 isb 143 .endm 144 145FUNC _start , : 146 /* 147 * If CFG_CORE_FFA is enabled, then x0 if non-NULL holds the TOS FW 148 * config [1] address, else x0 if non-NULL holds the pagable part 149 * address. 150 * 151 * [1] A TF-A concept: TOS_FW_CONFIG - Trusted OS Firmware 152 * configuration file. Used by Trusted OS (BL32), that is, OP-TEE 153 * here. 154 */ 155 mov x19, x0 156#if defined(CFG_DT_ADDR) 157 ldr x20, =CFG_DT_ADDR 158#else 159 mov x20, x2 /* Save DT address */ 160#endif 161 162 adr x0, reset_vect_table 163 msr vbar_el1, x0 164 isb 165 166 set_sctlr_el1 167 isb 168 169#ifdef CFG_WITH_PAGER 170 /* 171 * Move init code into correct location and move hashes to a 172 * temporary safe location until the heap is initialized. 173 * 174 * The binary is built as: 175 * [Pager code, rodata and data] : In correct location 176 * [Init code and rodata] : Should be copied to __init_start 177 * [struct boot_embdata + data] : Should be saved before 178 * initializing pager, first uint32_t tells the length of the data 179 */ 180 adr x0, __init_start /* dst */ 181 adr x1, __data_end /* src */ 182 adr x2, __init_end 183 sub x2, x2, x0 /* init len */ 184 ldr w4, [x1, x2] /* length of hashes etc */ 185 add x2, x2, x4 /* length of init and hashes etc */ 186 /* Copy backwards (as memmove) in case we're overlapping */ 187 add x0, x0, x2 /* __init_start + len */ 188 add x1, x1, x2 /* __data_end + len */ 189 adr x3, cached_mem_end 190 str x0, [x3] 191 adr x2, __init_start 192copy_init: 193 ldp x3, x4, [x1, #-16]! 194 stp x3, x4, [x0, #-16]! 195 cmp x0, x2 196 b.gt copy_init 197#else 198 /* 199 * The binary is built as: 200 * [Core, rodata and data] : In correct location 201 * [struct boot_embdata + data] : Should be moved to __end, first 202 * uint32_t tells the length of the struct + data 203 */ 204 adr_l x0, __end /* dst */ 205 adr_l x1, __data_end /* src */ 206 ldr w2, [x1] /* struct boot_embdata::total_len */ 207 /* Copy backwards (as memmove) in case we're overlapping */ 208 add x0, x0, x2 209 add x1, x1, x2 210 adr x3, cached_mem_end 211 str x0, [x3] 212 adr_l x2, __end 213 214copy_init: 215 ldp x3, x4, [x1, #-16]! 216 stp x3, x4, [x0, #-16]! 217 cmp x0, x2 218 b.gt copy_init 219#endif 220 221 /* 222 * Clear .bss, this code obviously depends on the linker keeping 223 * start/end of .bss at least 8 byte aligned. 224 */ 225 adr_l x0, __bss_start 226 adr_l x1, __bss_end 227clear_bss: 228 str xzr, [x0], #8 229 cmp x0, x1 230 b.lt clear_bss 231 232#ifdef CFG_NS_VIRTUALIZATION 233 /* 234 * Clear .nex_bss, this code obviously depends on the linker keeping 235 * start/end of .bss at least 8 byte aligned. 236 */ 237 adr x0, __nex_bss_start 238 adr x1, __nex_bss_end 239clear_nex_bss: 240 str xzr, [x0], #8 241 cmp x0, x1 242 b.lt clear_nex_bss 243#endif 244 245 /* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */ 246 set_sp 247 248 bl thread_init_thread_core_local 249 250 /* Enable aborts now that we can receive exceptions */ 251 msr daifclr, #DAIFBIT_ABT 252 253 /* 254 * Invalidate dcache for all memory used during initialization to 255 * avoid nasty surprices when the cache is turned on. We must not 256 * invalidate memory not used by OP-TEE since we may invalidate 257 * entries used by for instance ARM Trusted Firmware. 258 */ 259 adr_l x0, __text_start 260 ldr x1, cached_mem_end 261 sub x1, x1, x0 262 bl dcache_cleaninv_range 263 264 /* Enable Console */ 265 bl console_init 266 267#ifdef CFG_MEMTAG 268 /* 269 * If FEAT_MTE2 is available, initializes the memtag callbacks. 270 * Tags for OP-TEE core memory are then cleared to make it safe to 271 * enable MEMTAG below. 272 */ 273 bl boot_init_memtag 274#endif 275 276#ifdef CFG_CORE_ASLR 277 mov x0, x20 278 bl get_aslr_seed 279#else 280 mov x0, #0 281#endif 282 283 adr x1, boot_mmu_config 284 bl core_init_mmu_map 285 286#ifdef CFG_CORE_ASLR 287 /* 288 * Process relocation information again updating for the new 289 * offset. We're doing this now before MMU is enabled as some of 290 * the memory will become write protected. 291 */ 292 ldr x0, boot_mmu_config + CORE_MMU_CONFIG_LOAD_OFFSET 293 /* 294 * Update cached_mem_end address with load offset since it was 295 * calculated before relocation. 296 */ 297 adr x5, cached_mem_end 298 ldr x6, [x5] 299 add x6, x6, x0 300 str x6, [x5] 301 bl relocate 302#endif 303 304 bl __get_core_pos 305 bl enable_mmu 306#ifdef CFG_CORE_ASLR 307 /* 308 * Reinitialize console, since register_serial_console() has 309 * previously registered a PA and with ASLR the VA is different 310 * from the PA. 311 */ 312 bl console_init 313#endif 314 315#ifdef CFG_NS_VIRTUALIZATION 316 /* 317 * Initialize partition tables for each partition to 318 * default_partition which has been relocated now to a different VA 319 */ 320 bl core_mmu_set_default_prtn_tbl 321#endif 322 323#ifdef CFG_CORE_SEL1_SPMC 324 mov x0, xzr /* pager not used */ 325#else 326 mov x0, x19 /* pagable part address */ 327#endif 328 mov x1, #-1 329 bl boot_init_primary_early 330 331#ifdef CFG_MEMTAG 332 init_memtag_per_cpu 333#endif 334 335#ifndef CFG_NS_VIRTUALIZATION 336 mov x21, sp 337 adr_l x0, threads 338 ldr x0, [x0, #THREAD_CTX_STACK_VA_END] 339 mov sp, x0 340 bl thread_get_core_local 341 mov x22, x0 342 str wzr, [x22, #THREAD_CORE_LOCAL_FLAGS] 343#endif 344 mov x0, x20 /* DT address also known as HW_CONFIG */ 345#ifdef CFG_CORE_SEL1_SPMC 346 mov x1, x19 /* TOS_FW_CONFIG DT address */ 347#else 348 mov x1, xzr /* unused */ 349#endif 350 bl boot_init_primary_late 351#ifdef CFG_CORE_PAUTH 352 init_pauth_per_cpu 353#endif 354 355#ifndef CFG_NS_VIRTUALIZATION 356 mov x0, #THREAD_CLF_TMP 357 str w0, [x22, #THREAD_CORE_LOCAL_FLAGS] 358 mov sp, x21 359#endif 360 361#ifdef _CFG_CORE_STACK_PROTECTOR 362 /* Update stack canary value */ 363 bl plat_get_random_stack_canary 364 adr_l x5, __stack_chk_guard 365 str x0, [x5] 366#endif 367 368 /* 369 * In case we've touched memory that secondary CPUs will use before 370 * they have turned on their D-cache, clean and invalidate the 371 * D-cache before exiting to normal world. 372 */ 373 adr_l x0, __text_start 374 ldr x1, cached_mem_end 375 sub x1, x1, x0 376 bl dcache_cleaninv_range 377 378 379 /* 380 * Clear current thread id now to allow the thread to be reused on 381 * next entry. Matches the thread_init_boot_thread in 382 * boot.c. 383 */ 384#ifndef CFG_NS_VIRTUALIZATION 385 bl thread_clr_boot_thread 386#endif 387 388#ifdef CFG_CORE_FFA 389 adr x0, cpu_on_handler 390 /* 391 * Compensate for the load offset since cpu_on_handler() is 392 * called with MMU off. 393 */ 394 ldr x1, boot_mmu_config + CORE_MMU_CONFIG_LOAD_OFFSET 395 sub x0, x0, x1 396 bl thread_spmc_register_secondary_ep 397 b thread_ffa_msg_wait 398#else 399 /* 400 * Pass the vector address returned from main_init 401 * Compensate for the load offset since cpu_on_handler() is 402 * called with MMU off. 403 */ 404 ldr x0, boot_mmu_config + CORE_MMU_CONFIG_LOAD_OFFSET 405 adr x1, thread_vector_table 406 sub x1, x1, x0 407 mov x0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 408 smc #0 409 /* SMC should not return */ 410 panic_at_smc_return 411#endif 412END_FUNC _start 413DECLARE_KEEP_INIT _start 414 415 .section .identity_map.data 416 .balign 8 417LOCAL_DATA cached_mem_end , : 418 .skip 8 419END_DATA cached_mem_end 420 421#ifdef CFG_CORE_ASLR 422LOCAL_FUNC relocate , : 423 /* x0 holds load offset */ 424#ifdef CFG_WITH_PAGER 425 adr_l x6, __init_end 426#else 427 adr_l x6, __end 428#endif 429 ldp w2, w3, [x6, #BOOT_EMBDATA_RELOC_OFFSET] 430 431 mov_imm x1, TEE_RAM_START 432 add x2, x2, x6 /* start of relocations */ 433 add x3, x3, x2 /* end of relocations */ 434 435 /* 436 * Relocations are not formatted as Rela64, instead they are in a 437 * compressed format created by get_reloc_bin() in 438 * scripts/gen_tee_bin.py 439 * 440 * All the R_AARCH64_RELATIVE relocations are translated into a 441 * list list of 32-bit offsets from TEE_RAM_START. At each address 442 * a 64-bit value pointed out which increased with the load offset. 443 */ 444 445#ifdef CFG_WITH_PAGER 446 /* 447 * With pager enabled we can only relocate the pager and init 448 * parts, the rest has to be done when a page is populated. 449 */ 450 sub x6, x6, x1 451#endif 452 453 b 2f 454 /* Loop over the relocation addresses and process all entries */ 4551: ldr w4, [x2], #4 456#ifdef CFG_WITH_PAGER 457 /* Skip too large addresses */ 458 cmp x4, x6 459 b.ge 2f 460#endif 461 add x4, x4, x1 462 ldr x5, [x4] 463 add x5, x5, x0 464 str x5, [x4] 465 4662: cmp x2, x3 467 b.ne 1b 468 469 ret 470END_FUNC relocate 471#endif 472 473/* 474 * void enable_mmu(unsigned long core_pos); 475 * 476 * This function depends on being mapped with in the identity map where 477 * physical address and virtual address is the same. After MMU has been 478 * enabled the instruction pointer will be updated to execute as the new 479 * offset instead. Stack pointers and the return address are updated. 480 */ 481LOCAL_FUNC enable_mmu , : , .identity_map 482 adr x1, boot_mmu_config 483 load_xregs x1, 0, 2, 6 484 /* 485 * x0 = core_pos 486 * x2 = tcr_el1 487 * x3 = mair_el1 488 * x4 = ttbr0_el1_base 489 * x5 = ttbr0_core_offset 490 * x6 = load_offset 491 */ 492 msr tcr_el1, x2 493 msr mair_el1, x3 494 495 /* 496 * ttbr0_el1 = ttbr0_el1_base + ttbr0_core_offset * core_pos 497 */ 498 madd x1, x5, x0, x4 499 msr ttbr0_el1, x1 500 msr ttbr1_el1, xzr 501 isb 502 503 /* Invalidate TLB */ 504 tlbi vmalle1 505 506 /* 507 * Make sure translation table writes have drained into memory and 508 * the TLB invalidation is complete. 509 */ 510 dsb sy 511 isb 512 513 /* Enable the MMU */ 514 mrs x1, sctlr_el1 515 orr x1, x1, #SCTLR_M 516 msr sctlr_el1, x1 517 isb 518 519 /* Update vbar */ 520 mrs x1, vbar_el1 521 add x1, x1, x6 522 msr vbar_el1, x1 523 isb 524 525 /* Invalidate instruction cache and branch predictor */ 526 ic iallu 527 isb 528 529 /* Enable I and D cache */ 530 mrs x1, sctlr_el1 531 orr x1, x1, #SCTLR_I 532 orr x1, x1, #SCTLR_C 533 msr sctlr_el1, x1 534 isb 535 536 /* Adjust stack pointers and return address */ 537 msr spsel, #1 538 add sp, sp, x6 539 msr spsel, #0 540 add sp, sp, x6 541 add x30, x30, x6 542 543 ret 544END_FUNC enable_mmu 545 546 .section .identity_map.data 547 .balign 8 548DATA boot_mmu_config , : /* struct core_mmu_config */ 549 .skip CORE_MMU_CONFIG_SIZE 550END_DATA boot_mmu_config 551 552FUNC cpu_on_handler , : 553 mov x19, x0 554 mov x20, x1 555 mov x21, x30 556 557 adr x0, reset_vect_table 558 msr vbar_el1, x0 559 isb 560 561 set_sctlr_el1 562 isb 563 564 /* Enable aborts now that we can receive exceptions */ 565 msr daifclr, #DAIFBIT_ABT 566 567 bl __get_core_pos 568 bl enable_mmu 569 570 /* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */ 571 set_sp 572 573#ifdef CFG_MEMTAG 574 init_memtag_per_cpu 575#endif 576#ifdef CFG_CORE_PAUTH 577 init_pauth_per_cpu 578#endif 579 580 mov x0, x19 581 mov x1, x20 582#ifdef CFG_CORE_FFA 583 bl boot_cpu_on_handler 584 b thread_ffa_msg_wait 585#else 586 mov x30, x21 587 b boot_cpu_on_handler 588#endif 589END_FUNC cpu_on_handler 590DECLARE_KEEP_PAGER cpu_on_handler 591 592LOCAL_FUNC unhandled_cpu , : 593 wfi 594 b unhandled_cpu 595END_FUNC unhandled_cpu 596 597LOCAL_DATA stack_tmp_rel , : 598 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 599END_DATA stack_tmp_rel 600 601 /* 602 * This macro verifies that the a given vector doesn't exceed the 603 * architectural limit of 32 instructions. This is meant to be placed 604 * immedately after the last instruction in the vector. It takes the 605 * vector entry as the parameter 606 */ 607 .macro check_vector_size since 608 .if (. - \since) > (32 * 4) 609 .error "Vector exceeds 32 instructions" 610 .endif 611 .endm 612 613 .section .identity_map, "ax", %progbits 614 .align 11 615LOCAL_FUNC reset_vect_table , :, .identity_map, , nobti 616 /* ----------------------------------------------------- 617 * Current EL with SP0 : 0x0 - 0x180 618 * ----------------------------------------------------- 619 */ 620SynchronousExceptionSP0: 621 b SynchronousExceptionSP0 622 check_vector_size SynchronousExceptionSP0 623 624 .align 7 625IrqSP0: 626 b IrqSP0 627 check_vector_size IrqSP0 628 629 .align 7 630FiqSP0: 631 b FiqSP0 632 check_vector_size FiqSP0 633 634 .align 7 635SErrorSP0: 636 b SErrorSP0 637 check_vector_size SErrorSP0 638 639 /* ----------------------------------------------------- 640 * Current EL with SPx: 0x200 - 0x380 641 * ----------------------------------------------------- 642 */ 643 .align 7 644SynchronousExceptionSPx: 645 b SynchronousExceptionSPx 646 check_vector_size SynchronousExceptionSPx 647 648 .align 7 649IrqSPx: 650 b IrqSPx 651 check_vector_size IrqSPx 652 653 .align 7 654FiqSPx: 655 b FiqSPx 656 check_vector_size FiqSPx 657 658 .align 7 659SErrorSPx: 660 b SErrorSPx 661 check_vector_size SErrorSPx 662 663 /* ----------------------------------------------------- 664 * Lower EL using AArch64 : 0x400 - 0x580 665 * ----------------------------------------------------- 666 */ 667 .align 7 668SynchronousExceptionA64: 669 b SynchronousExceptionA64 670 check_vector_size SynchronousExceptionA64 671 672 .align 7 673IrqA64: 674 b IrqA64 675 check_vector_size IrqA64 676 677 .align 7 678FiqA64: 679 b FiqA64 680 check_vector_size FiqA64 681 682 .align 7 683SErrorA64: 684 b SErrorA64 685 check_vector_size SErrorA64 686 687 /* ----------------------------------------------------- 688 * Lower EL using AArch32 : 0x0 - 0x180 689 * ----------------------------------------------------- 690 */ 691 .align 7 692SynchronousExceptionA32: 693 b SynchronousExceptionA32 694 check_vector_size SynchronousExceptionA32 695 696 .align 7 697IrqA32: 698 b IrqA32 699 check_vector_size IrqA32 700 701 .align 7 702FiqA32: 703 b FiqA32 704 check_vector_size FiqA32 705 706 .align 7 707SErrorA32: 708 b SErrorA32 709 check_vector_size SErrorA32 710 711END_FUNC reset_vect_table 712 713BTI(emit_aarch64_feature_1_and GNU_PROPERTY_AARCH64_FEATURE_1_BTI) 714