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, #THREAD_CTX_STACK_VA_END] 524 mov sp, x0 525 bl thread_get_core_local 526 mov x24, x0 527 str wzr, [x24, #THREAD_CORE_LOCAL_FLAGS] 528#endif 529 bl boot_init_primary_runtime 530#ifdef CFG_CORE_PAUTH 531 adr_l x0, threads 532 ldp x1, x2, [x0, #THREAD_CTX_KEYS] 533 write_apiakeyhi x1 534 write_apiakeylo x2 535 mrs x0, sctlr_el1 536 orr x0, x0, #SCTLR_ENIA 537 msr sctlr_el1, x0 538 isb 539#endif 540 bl boot_init_primary_final 541 542#ifndef CFG_NS_VIRTUALIZATION 543 mov x0, #THREAD_CLF_TMP 544 str w0, [x24, #THREAD_CORE_LOCAL_FLAGS] 545 mov sp, x23 546#ifdef CFG_CORE_PAUTH 547 ldp x0, x1, [x24, #THREAD_CORE_LOCAL_KEYS] 548 write_apiakeyhi x0 549 write_apiakeylo x1 550 isb 551#endif 552#endif 553 554#ifdef _CFG_CORE_STACK_PROTECTOR 555 /* Update stack canary value */ 556 sub sp, sp, #0x10 557 mov x0, sp 558 mov x1, #1 559 mov x2, #0x8 560 bl plat_get_random_stack_canaries 561 ldr x0, [sp] 562 adr_l x5, __stack_chk_guard 563 str x0, [x5] 564 add sp, sp, #0x10 565#endif 566 567 /* 568 * In case we've touched memory that secondary CPUs will use before 569 * they have turned on their D-cache, clean and invalidate the 570 * D-cache before exiting to normal world. 571 */ 572 adr_l x0, __text_start 573 adr_l x1, boot_cached_mem_end 574 ldr x1, [x1] 575 sub x1, x1, x0 576 bl dcache_cleaninv_range 577 578 /* 579 * Clear current thread id now to allow the thread to be reused on 580 * next entry. Matches the thread_init_boot_thread in 581 * boot.c. 582 */ 583#ifndef CFG_NS_VIRTUALIZATION 584 bl thread_clr_boot_thread 585#endif 586 587#ifdef CFG_CORE_FFA 588 adr x0, cpu_on_handler 589 /* 590 * Compensate for the virtual map offset since cpu_on_handler() is 591 * called with MMU off. 592 */ 593 ldr x1, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 594 sub x0, x0, x1 595 bl thread_spmc_register_secondary_ep 596 b thread_ffa_msg_wait 597#else 598 /* 599 * Pass the vector address returned from main_init Compensate for 600 * the virtual map offset since cpu_on_handler() is called with MMU 601 * off. 602 */ 603 ldr x0, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 604 adr x1, thread_vector_table 605 sub x1, x1, x0 606 mov x0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 607 smc #0 608 /* SMC should not return */ 609 panic_at_smc_return 610#endif 611END_FUNC _start 612DECLARE_KEEP_INIT _start 613 614#ifndef CFG_WITH_PAGER 615 .section .identity_map.data 616 .balign 8 617LOCAL_DATA boot_embdata_ptr , : 618 .skip 8 619END_DATA boot_embdata_ptr 620#endif 621 622#if defined(CFG_CORE_ASLR) || defined(CFG_CORE_PHYS_RELOCATABLE) 623LOCAL_FUNC relocate , : 624 /* 625 * x0 holds relocate offset 626 * x1 holds load address 627 */ 628#ifdef CFG_WITH_PAGER 629 adr_l x6, __init_end 630#else 631 adr_l x6, boot_embdata_ptr 632 ldr x6, [x6] 633#endif 634 ldp w2, w3, [x6, #BOOT_EMBDATA_RELOC_OFFSET] 635 636 add x2, x2, x6 /* start of relocations */ 637 add x3, x3, x2 /* end of relocations */ 638 639 /* 640 * Relocations are not formatted as Rela64, instead they are in a 641 * compressed format created by get_reloc_bin() in 642 * scripts/gen_tee_bin.py 643 * 644 * All the R_AARCH64_RELATIVE relocations are translated into a 645 * list of 32-bit offsets from TEE_LOAD_ADDR. At each address a 646 * 64-bit value pointed out which increased with the load offset. 647 */ 648 649#ifdef CFG_WITH_PAGER 650 /* 651 * With pager enabled we can only relocate the pager and init 652 * parts, the rest has to be done when a page is populated. 653 */ 654 sub x6, x6, x1 655#endif 656 657 b 2f 658 /* Loop over the relocation addresses and process all entries */ 6591: ldr w4, [x2], #4 660#ifdef CFG_WITH_PAGER 661 /* Skip too large addresses */ 662 cmp x4, x6 663 b.ge 2f 664#endif 665 add x4, x4, x1 666 ldr x5, [x4] 667 add x5, x5, x0 668 str x5, [x4] 669 6702: cmp x2, x3 671 b.ne 1b 672 673 ret 674END_FUNC relocate 675#endif 676 677/* 678 * void enable_mmu(unsigned long core_pos); 679 * 680 * This function depends on being mapped with in the identity map where 681 * physical address and virtual address is the same. After MMU has been 682 * enabled the instruction pointer will be updated to execute as the new 683 * offset instead. Stack pointers and the return address are updated. 684 */ 685LOCAL_FUNC enable_mmu , : , .identity_map 686 adr x1, boot_mmu_config 687 load_xregs x1, 0, 2, 6 688 /* 689 * x0 = core_pos 690 * x2 = tcr_el1 691 * x3 = mair_el1 692 * x4 = ttbr0_el1_base 693 * x5 = ttbr0_core_offset 694 * x6 = load_offset 695 */ 696 msr tcr_el1, x2 697 msr mair_el1, x3 698 699 /* 700 * ttbr0_el1 = ttbr0_el1_base + ttbr0_core_offset * core_pos 701 */ 702 madd x1, x5, x0, x4 703 msr ttbr0_el1, x1 704 msr ttbr1_el1, xzr 705 isb 706 707 /* Invalidate TLB */ 708 tlbi vmalle1 709 710 /* 711 * Make sure translation table writes have drained into memory and 712 * the TLB invalidation is complete. 713 */ 714 dsb sy 715 isb 716 717 /* Enable the MMU */ 718 mrs x1, sctlr_el1 719 orr x1, x1, #SCTLR_M 720 msr sctlr_el1, x1 721 isb 722 723 /* Update vbar */ 724 mrs x1, vbar_el1 725 add x1, x1, x6 726 msr vbar_el1, x1 727 isb 728 729 /* Invalidate instruction cache and branch predictor */ 730 ic iallu 731 isb 732 733 /* Enable I and D cache */ 734 mrs x1, sctlr_el1 735 orr x1, x1, #SCTLR_I 736 orr x1, x1, #SCTLR_C 737 msr sctlr_el1, x1 738 isb 739 740 /* Adjust stack pointers and return address */ 741 msr spsel, #1 742 add sp, sp, x6 743 msr spsel, #0 744 add sp, sp, x6 745 add x30, x30, x6 746 747 ret 748END_FUNC enable_mmu 749 750 .section .identity_map.data 751 .balign 8 752DATA boot_mmu_config , : /* struct core_mmu_config */ 753 .skip CORE_MMU_CONFIG_SIZE 754END_DATA boot_mmu_config 755 756FUNC cpu_on_handler , : 757 mov x19, x0 758 mov x20, x1 759 mov x21, x30 760 761 adr x0, reset_vect_table 762 msr vbar_el1, x0 763 isb 764 765 set_sctlr_el1 766 isb 767 768#ifdef CFG_PAN 769 init_pan 770#endif 771 772 /* Enable aborts now that we can receive exceptions */ 773 msr daifclr, #DAIFBIT_ABT 774 775 bl __get_core_pos 776 bl enable_mmu 777 778#if defined(CFG_DYN_STACK_CONFIG) 779 /* 780 * Update SP_EL0 to use the new tmp stack and update SP_EL1 to 781 * point the new thread_core_local. 782 */ 783 bl __get_core_pos 784 adr_l x1, thread_core_local 785 ldr x1, [x1] 786 mov x2, #THREAD_CORE_LOCAL_SIZE 787 /* x3 = x2 * x0 + x1 */ 788 madd x3, x2, x0, x1 789 ldr x0, [x3, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 790 mov sp, x0 791 msr spsel, #1 792 mov sp, x3 793 msr spsel, #0 794#else 795 /* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */ 796 set_sp 797#endif 798 799#ifdef CFG_MEMTAG 800 init_memtag_per_cpu 801#endif 802#ifdef CFG_CORE_PAUTH 803 init_pauth_secondary_cpu 804#endif 805 806 mov x0, x19 807 mov x1, x20 808#ifdef CFG_CORE_FFA 809 bl boot_cpu_on_handler 810 b thread_ffa_msg_wait 811#else 812 mov x30, x21 813 b boot_cpu_on_handler 814#endif 815END_FUNC cpu_on_handler 816DECLARE_KEEP_PAGER cpu_on_handler 817 818LOCAL_FUNC unhandled_cpu , : 819 wfi 820 b unhandled_cpu 821END_FUNC unhandled_cpu 822 823#if !defined(CFG_DYN_STACK_CONFIG) 824LOCAL_DATA stack_tmp_rel , : 825 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 826END_DATA stack_tmp_rel 827#endif 828 829 /* 830 * This macro verifies that the a given vector doesn't exceed the 831 * architectural limit of 32 instructions. This is meant to be placed 832 * immedately after the last instruction in the vector. It takes the 833 * vector entry as the parameter 834 */ 835 .macro check_vector_size since 836 .if (. - \since) > (32 * 4) 837 .error "Vector exceeds 32 instructions" 838 .endif 839 .endm 840 841 .section .identity_map, "ax", %progbits 842 .align 11 843LOCAL_FUNC reset_vect_table , :, .identity_map, , nobti 844 /* ----------------------------------------------------- 845 * Current EL with SP0 : 0x0 - 0x180 846 * ----------------------------------------------------- 847 */ 848SynchronousExceptionSP0: 849 b SynchronousExceptionSP0 850 check_vector_size SynchronousExceptionSP0 851 852 .align 7 853IrqSP0: 854 b IrqSP0 855 check_vector_size IrqSP0 856 857 .align 7 858FiqSP0: 859 b FiqSP0 860 check_vector_size FiqSP0 861 862 .align 7 863SErrorSP0: 864 b SErrorSP0 865 check_vector_size SErrorSP0 866 867 /* ----------------------------------------------------- 868 * Current EL with SPx: 0x200 - 0x380 869 * ----------------------------------------------------- 870 */ 871 .align 7 872SynchronousExceptionSPx: 873 b SynchronousExceptionSPx 874 check_vector_size SynchronousExceptionSPx 875 876 .align 7 877IrqSPx: 878 b IrqSPx 879 check_vector_size IrqSPx 880 881 .align 7 882FiqSPx: 883 b FiqSPx 884 check_vector_size FiqSPx 885 886 .align 7 887SErrorSPx: 888 b SErrorSPx 889 check_vector_size SErrorSPx 890 891 /* ----------------------------------------------------- 892 * Lower EL using AArch64 : 0x400 - 0x580 893 * ----------------------------------------------------- 894 */ 895 .align 7 896SynchronousExceptionA64: 897 b SynchronousExceptionA64 898 check_vector_size SynchronousExceptionA64 899 900 .align 7 901IrqA64: 902 b IrqA64 903 check_vector_size IrqA64 904 905 .align 7 906FiqA64: 907 b FiqA64 908 check_vector_size FiqA64 909 910 .align 7 911SErrorA64: 912 b SErrorA64 913 check_vector_size SErrorA64 914 915 /* ----------------------------------------------------- 916 * Lower EL using AArch32 : 0x0 - 0x180 917 * ----------------------------------------------------- 918 */ 919 .align 7 920SynchronousExceptionA32: 921 b SynchronousExceptionA32 922 check_vector_size SynchronousExceptionA32 923 924 .align 7 925IrqA32: 926 b IrqA32 927 check_vector_size IrqA32 928 929 .align 7 930FiqA32: 931 b FiqA32 932 check_vector_size FiqA32 933 934 .align 7 935SErrorA32: 936 b SErrorA32 937 check_vector_size SErrorA32 938 939END_FUNC reset_vect_table 940 941BTI(emit_aarch64_feature_1_and GNU_PROPERTY_AARCH64_FEATURE_1_BTI) 942