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