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