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