1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2014, Linaro Limited 4 * Copyright (c) 2021-2023, Arm Limited 5 */ 6 7#include <arm.h> 8#include <arm32_macros.S> 9#include <asm.S> 10#include <generated/asm-defines.h> 11#include <keep.h> 12#include <kernel/asan.h> 13#include <kernel/cache_helpers.h> 14#include <kernel/thread.h> 15#include <kernel/thread_private.h> 16#include <mm/core_mmu.h> 17#include <platform_config.h> 18#include <sm/optee_smc.h> 19#include <sm/teesmc_opteed.h> 20#include <sm/teesmc_opteed_macros.h> 21 22.arch_extension sec 23 24.section .data 25.balign 4 26 27#ifdef CFG_BOOT_SYNC_CPU 28.equ SEM_CPU_READY, 1 29#endif 30 31#ifdef CFG_PL310 32.section .rodata.init 33panic_boot_file: 34 .asciz __FILE__ 35 36/* 37 * void assert_flat_mapped_range(uint32_t vaddr, uint32_t line) 38 */ 39LOCAL_FUNC __assert_flat_mapped_range , : 40UNWIND( .cantunwind) 41 push { r4-r6, lr } 42 mov r4, r0 43 mov r5, r1 44 bl cpu_mmu_enabled 45 cmp r0, #0 46 beq 1f 47 mov r0, r4 48 bl virt_to_phys 49 cmp r0, r4 50 beq 1f 51 /* 52 * this must be compliant with the panic generic routine: 53 * __do_panic(__FILE__, __LINE__, __func__, str) 54 */ 55 ldr r0, =panic_boot_file 56 mov r1, r5 57 mov r2, #0 58 mov r3, #0 59 bl __do_panic 60 b . /* should NOT return */ 611: pop { r4-r6, pc } 62END_FUNC __assert_flat_mapped_range 63 64 /* panic if mmu is enable and vaddr != paddr (scratch lr) */ 65 .macro assert_flat_mapped_range va, line 66 ldr r0, \va 67 ldr r1, =\line 68 bl __assert_flat_mapped_range 69 .endm 70#endif /* CFG_PL310 */ 71 72WEAK_FUNC plat_cpu_reset_early , : 73 bx lr 74END_FUNC plat_cpu_reset_early 75DECLARE_KEEP_PAGER plat_cpu_reset_early 76 77 .section .identity_map, "ax" 78 .align 5 79LOCAL_FUNC reset_vect_table , : , .identity_map 80 b . 81 b . /* Undef */ 82 b . /* Syscall */ 83 b . /* Prefetch abort */ 84 b . /* Data abort */ 85 b . /* Reserved */ 86 b . /* IRQ */ 87 b . /* FIQ */ 88END_FUNC reset_vect_table 89 90 .macro cpu_is_ready 91#ifdef CFG_BOOT_SYNC_CPU 92 bl __get_core_pos 93 lsl r0, r0, #2 94 ldr r1,=sem_cpu_sync 95 ldr r2, =SEM_CPU_READY 96 str r2, [r1, r0] 97 dsb 98 sev 99#endif 100 .endm 101 102 .macro wait_primary 103#ifdef CFG_BOOT_SYNC_CPU 104 ldr r0, =sem_cpu_sync 105 mov r2, #SEM_CPU_READY 106 sev 1071: 108 ldr r1, [r0] 109 cmp r1, r2 110 wfene 111 bne 1b 112#endif 113 .endm 114 115 .macro wait_secondary 116#ifdef CFG_BOOT_SYNC_CPU 117 ldr r0, =sem_cpu_sync 118 mov r3, #CFG_TEE_CORE_NB_CORE 119 mov r2, #SEM_CPU_READY 120 sev 1211: 122 subs r3, r3, #1 123 beq 3f 124 add r0, r0, #4 1252: 126 ldr r1, [r0] 127 cmp r1, r2 128 wfene 129 bne 2b 130 b 1b 1313: 132#endif 133 .endm 134 135 /* 136 * set_sctlr : Setup some core configuration in CP15 SCTLR 137 * 138 * Setup required by current implementation of the OP-TEE core: 139 * - Disable data and instruction cache. 140 * - MMU is expected off and exceptions trapped in ARM mode. 141 * - Enable or disable alignment checks upon platform configuration. 142 * - Optionally enable write-implies-execute-never. 143 * - Optionally enable round robin strategy for cache replacement. 144 * 145 * Clobbers r0. 146 */ 147 .macro set_sctlr 148 read_sctlr r0 149 bic r0, r0, #(SCTLR_M | SCTLR_C) 150 bic r0, r0, #SCTLR_I 151 bic r0, r0, #SCTLR_TE 152 orr r0, r0, #SCTLR_SPAN 153#if defined(CFG_SCTLR_ALIGNMENT_CHECK) 154 orr r0, r0, #SCTLR_A 155#else 156 bic r0, r0, #SCTLR_A 157#endif 158#if defined(CFG_HWSUPP_MEM_PERM_WXN) && defined(CFG_CORE_RWDATA_NOEXEC) 159 orr r0, r0, #(SCTLR_WXN | SCTLR_UWXN) 160#endif 161#if defined(CFG_ENABLE_SCTLR_RR) 162 orr r0, r0, #SCTLR_RR 163#endif 164 write_sctlr r0 165 .endm 166 167 .macro maybe_init_spectre_workaround 168#if !defined(CFG_WITH_ARM_TRUSTED_FW) && \ 169 (defined(CFG_CORE_WORKAROUND_SPECTRE_BP) || \ 170 defined(CFG_CORE_WORKAROUND_SPECTRE_BP_SEC)) 171 read_midr r0 172 ubfx r1, r0, #MIDR_IMPLEMENTER_SHIFT, #MIDR_IMPLEMENTER_WIDTH 173 cmp r1, #MIDR_IMPLEMENTER_ARM 174 bne 1f 175 ubfx r1, r0, #MIDR_PRIMARY_PART_NUM_SHIFT, \ 176 #MIDR_PRIMARY_PART_NUM_WIDTH 177 178 movw r2, #CORTEX_A8_PART_NUM 179 cmp r1, r2 180 moveq r2, #ACTLR_CA8_ENABLE_INVALIDATE_BTB 181 beq 2f 182 183 movw r2, #CORTEX_A15_PART_NUM 184 cmp r1, r2 185 moveq r2, #ACTLR_CA15_ENABLE_INVALIDATE_BTB 186 bne 1f /* Skip it for all other CPUs */ 1872: 188 read_actlr r0 189 orr r0, r0, r2 190 write_actlr r0 191 isb 1921: 193#endif 194 .endm 195 196FUNC _start , : 197UNWIND( .cantunwind) 198 /* 199 * Temporary copy of boot argument registers, will be passed to 200 * boot_save_args() further down. 201 */ 202 mov r4, r0 203 mov r5, r1 204 mov r6, r2 205 mov r7, r3 206 mov r8, lr 207 208 /* 209 * 32bit entry is expected to execute Supervisor mode, 210 * some bootloader may enter in Supervisor or Monitor 211 */ 212 cps #CPSR_MODE_SVC 213 214 /* Early ARM secure MP specific configuration */ 215 bl plat_cpu_reset_early 216 maybe_init_spectre_workaround 217 218 set_sctlr 219 isb 220 221 ldr r0, =reset_vect_table 222 write_vbar r0 223 224#if defined(CFG_WITH_ARM_TRUSTED_FW) 225 b reset_primary 226#else 227 bl __get_core_pos 228 cmp r0, #0 229 beq reset_primary 230 b reset_secondary 231#endif 232END_FUNC _start 233DECLARE_KEEP_INIT _start 234 235 /* 236 * Setup sp to point to the top of the tmp stack for the current CPU: 237 * sp is assigned: 238 * stack_tmp + (cpu_id + 1) * stack_tmp_stride - STACK_TMP_GUARD 239 */ 240 .macro set_sp 241 bl __get_core_pos 242 cmp r0, #CFG_TEE_CORE_NB_CORE 243 /* Unsupported CPU, park it before it breaks something */ 244 bge unhandled_cpu 245 add r0, r0, #1 246 247 /* r2 = stack_tmp - STACK_TMP_GUARD */ 248 adr r3, stack_tmp_rel 249 ldr r2, [r3] 250 add r2, r2, r3 251 252 /* 253 * stack_tmp_stride and stack_tmp_stride_rel are the 254 * equivalent of: 255 * extern const u32 stack_tmp_stride; 256 * u32 stack_tmp_stride_rel = (u32)&stack_tmp_stride - 257 * (u32)&stack_tmp_stride_rel 258 * 259 * To load the value of stack_tmp_stride we do the equivalent 260 * of: 261 * *(u32 *)(stack_tmp_stride + (u32)&stack_tmp_stride_rel) 262 */ 263 adr r3, stack_tmp_stride_rel 264 ldr r1, [r3] 265 ldr r1, [r1, r3] 266 267 /* 268 * r0 is core pos + 1 269 * r1 is value of stack_tmp_stride 270 * r2 is value of stack_tmp + guard 271 */ 272 mul r1, r0, r1 273 add sp, r1, r2 274 .endm 275 276 /* 277 * Cache maintenance during entry: handle outer cache. 278 * End address is exclusive: first byte not to be changed. 279 * Note however arm_clX_inv/cleanbyva operate on full cache lines. 280 * 281 * Use ANSI #define to trap source file line number for PL310 assertion 282 */ 283 .macro __inval_cache_vrange vbase, vend, line 284#if defined(CFG_PL310) && !defined(CFG_PL310_SIP_PROTOCOL) 285 assert_flat_mapped_range (\vbase), (\line) 286 bl pl310_base 287 ldr r1, \vbase 288 ldr r2, =\vend 289 ldr r2, [r2] 290 bl arm_cl2_invbypa 291#endif 292 ldr r0, \vbase 293 ldr r1, =\vend 294 ldr r1, [r1] 295 sub r1, r1, r0 296 bl dcache_inv_range 297 .endm 298 299 .macro __flush_cache_vrange vbase, vend, line 300#if defined(CFG_PL310) && !defined(CFG_PL310_SIP_PROTOCOL) 301 assert_flat_mapped_range (\vbase), (\line) 302 ldr r0, \vbase 303 ldr r1, =\vend 304 ldr r1, [r1] 305 sub r1, r1, r0 306 bl dcache_clean_range 307 bl pl310_base 308 ldr r1, \vbase 309 ldr r2, =\vend 310 ldr r2, [r2] 311 bl arm_cl2_cleaninvbypa 312#endif 313 ldr r0, \vbase 314 ldr r1, =\vend 315 ldr r1, [r1] 316 sub r1, r1, r0 317 bl dcache_cleaninv_range 318 .endm 319 320#define inval_cache_vrange(vbase, vend) \ 321 __inval_cache_vrange vbase, vend, __LINE__ 322 323#define flush_cache_vrange(vbase, vend) \ 324 __flush_cache_vrange vbase, vend, __LINE__ 325 326#ifdef CFG_BOOT_SYNC_CPU 327#define flush_cpu_semaphores \ 328 flush_cache_vrange(sem_cpu_sync_start, sem_cpu_sync_end) 329#else 330#define flush_cpu_semaphores 331#endif 332 333LOCAL_FUNC reset_primary , : , .identity_map 334UNWIND( .cantunwind) 335 336 /* preserve r4-r8: bootargs */ 337 338#ifdef CFG_WITH_PAGER 339 /* 340 * Move init code into correct location and move hashes to a 341 * temporary safe location until the heap is initialized. 342 * 343 * The binary is built as: 344 * [Pager code, rodata and data] : In correct location 345 * [Init code and rodata] : Should be copied to __init_start 346 * [struct boot_embdata + data] : Should be saved before 347 * initializing pager, first uint32_t tells the length of the data 348 */ 349 ldr r0, =__init_start /* dst */ 350 ldr r1, =__data_end /* src */ 351 ldr r2, =__init_end 352 sub r2, r2, r0 /* init len */ 353 ldr r12, [r1, r2] /* length of hashes etc */ 354 add r2, r2, r12 /* length of init and hashes etc */ 355 /* Copy backwards (as memmove) in case we're overlapping */ 356 add r0, r0, r2 /* __init_start + len */ 357 add r1, r1, r2 /* __data_end + len */ 358 ldr r3, =boot_cached_mem_end 359 str r0, [r3] 360 ldr r2, =__init_start 361copy_init: 362 ldmdb r1!, {r3, r9-r12} 363 stmdb r0!, {r3, r9-r12} 364 cmp r0, r2 365 bgt copy_init 366#else 367 /* 368 * The binary is built as: 369 * [Core, rodata and data] : In correct location 370 * [struct boot_embdata + data] : Should be moved to right before 371 * __vcore_free_end, the first uint32_t tells the length of the 372 * struct + data 373 */ 374 ldr r1, =__data_end /* src */ 375 ldr r2, [r1] /* struct boot_embdata::total_len */ 376 /* dst */ 377 ldr r0, =__vcore_free_end 378 sub r0, r0, r2 379 /* round down to beginning of page */ 380 mov r3, #(SMALL_PAGE_SIZE - 1) 381 bic r0, r0, r3 382 ldr r3, =boot_embdata_ptr 383 str r0, [r3] 384 /* Copy backwards (as memmove) in case we're overlapping */ 385 add r1, r1, r2 386 add r2, r0, r2 387 ldr r3, =boot_cached_mem_end 388 str r2, [r3] 389 390copy_init: 391 ldmdb r1!, {r3, r9-r12} 392 stmdb r2!, {r3, r9-r12} 393 cmp r2, r0 394 bgt copy_init 395#endif 396 397 /* 398 * Clear .bss, this code obviously depends on the linker keeping 399 * start/end of .bss at least 8 byte aligned. 400 */ 401 ldr r0, =__bss_start 402 ldr r1, =__bss_end 403 mov r2, #0 404 mov r3, #0 405clear_bss: 406 stmia r0!, {r2, r3} 407 cmp r0, r1 408 bls clear_bss 409 410#ifdef CFG_NS_VIRTUALIZATION 411 /* 412 * Clear .nex_bss, this code obviously depends on the linker keeping 413 * start/end of .bss at least 8 byte aligned. 414 */ 415 ldr r0, =__nex_bss_start 416 ldr r1, =__nex_bss_end 417 mov r2, #0 418 mov r3, #0 419clear_nex_bss: 420 stmia r0!, {r2, r3} 421 cmp r0, r1 422 bls clear_nex_bss 423#endif 424 425#ifdef CFG_CORE_SANITIZE_KADDRESS 426 /* First initialize the entire shadow area with no access */ 427 ldr r0, =__asan_shadow_start /* start */ 428 ldr r1, =__asan_shadow_end /* limit */ 429 mov r2, #ASAN_DATA_RED_ZONE 430shadow_no_access: 431 str r2, [r0], #4 432 cmp r0, r1 433 bls shadow_no_access 434 435 /* Mark the entire stack area as OK */ 436 ldr r2, =CFG_ASAN_SHADOW_OFFSET 437 ldr r0, =__nozi_stack_start /* start */ 438 lsr r0, r0, #ASAN_BLOCK_SHIFT 439 add r0, r0, r2 440 ldr r1, =__nozi_stack_end /* limit */ 441 lsr r1, r1, #ASAN_BLOCK_SHIFT 442 add r1, r1, r2 443 mov r2, #0 444shadow_stack_access_ok: 445 strb r2, [r0], #1 446 cmp r0, r1 447 bls shadow_stack_access_ok 448#endif 449 450 set_sp 451 452 /* Initialize thread_core_local[0] for early boot */ 453 bl thread_get_core_local 454 push {r0,r1} 455 bl thread_get_abt_stack 456 pop {r1,r2} 457 mov r3, sp 458 459 cps #CPSR_MODE_IRQ 460 mov sp, r3 461 cps #CPSR_MODE_FIQ 462 mov sp, r3 463 cps #CPSR_MODE_ABT 464 mov sp, r1 465 cps #CPSR_MODE_UND 466 mov sp, r1 467 cps #CPSR_MODE_SVC 468 469#ifdef CFG_CORE_DEBUG_CHECK_STACKS 470 mov r2, #1 471 strb r2, [r1, #THREAD_CORE_LOCAL_STACKCHECK_RECURSION] 472#endif 473 474 str sp, [r1, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 475 str r0, [r1, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 476 mov r0, #THREAD_ID_INVALID 477 str r0, [r1, #THREAD_CORE_LOCAL_CURR_THREAD] 478 mov r0, #THREAD_CLF_TMP 479 str r0, [r1, #THREAD_CORE_LOCAL_FLAGS] 480 481 /* complete ARM secure MP common configuration */ 482 bl plat_primary_init_early 483 484 /* Enable Console */ 485 bl console_init 486 487 mov r0, r8 488 mov r1, #0 489 push {r0, r1} 490 mov r0, r4 491 mov r1, r5 492 mov r2, r6 493 mov r3, r7 494 bl boot_save_args 495 add sp, sp, #(2 * 4) 496 497#ifdef CFG_WITH_PAGER 498 ldr r0, =__init_end /* pointer to boot_embdata */ 499 ldr r1, [r0] /* struct boot_embdata::total_len */ 500 add r0, r0, r1 501 mov_imm r1, 0xfff 502 add r0, r0, r1 /* round up */ 503 bic r0, r0, r1 /* to next page */ 504 mov_imm r1, (TEE_RAM_PH_SIZE + TEE_RAM_START) 505 mov r2, r1 506#else 507 ldr r0, =__vcore_free_start 508 ldr r1, =boot_embdata_ptr 509 ldr r1, [r1] 510 ldr r2, =__vcore_free_end 511#endif 512 bl boot_mem_init 513 514#ifdef CFG_PL310 515 bl pl310_base 516 bl arm_cl2_config 517#endif 518 519 /* 520 * Invalidate dcache for all memory used during initialization to 521 * avoid nasty surprices when the cache is turned on. We must not 522 * invalidate memory not used by OP-TEE since we may invalidate 523 * entries used by for instance ARM Trusted Firmware. 524 */ 525 inval_cache_vrange(cached_mem_start, boot_cached_mem_end) 526 527#if defined(CFG_PL310) && !defined(CFG_PL310_SIP_PROTOCOL) 528 /* Enable PL310 if not yet enabled */ 529 bl pl310_base 530 bl arm_cl2_enable 531#endif 532 533#ifdef CFG_CORE_ASLR 534 bl get_aslr_seed 535#ifdef CFG_CORE_ASLR_SEED 536 mov_imm r0, CFG_CORE_ASLR_SEED 537#endif 538#else 539 mov r0, #0 540#endif 541 542 ldr r1, =boot_mmu_config 543 bl core_init_mmu_map 544 545#ifdef CFG_CORE_ASLR 546 /* 547 * Save a pointer to thread_core_local[core_pos] since we can't 548 * call thread_get_core_local() again before the recorded end_va's 549 * have been updated below. 550 */ 551 bl thread_get_core_local 552 mov r4, r0 553 554 /* 555 * Process relocation information for updating with the virtual map 556 * offset. We're doing this now before MMU is enabled as some of 557 * the memory will become write protected. 558 */ 559 ldr r0, =boot_mmu_config 560 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 561 /* 562 * Update boot_cached_mem_end address with load offset since it was 563 * calculated before relocation. 564 */ 565 ldr r3, =boot_cached_mem_end 566 ldr r2, [r3] 567 add r2, r2, r0 568 str r2, [r3] 569 570 bl relocate 571#endif 572 573 bl __get_core_pos 574 bl enable_mmu 575#ifdef CFG_CORE_ASLR 576 ldr r0, =boot_mmu_config 577 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 578 bl boot_mem_relocate 579 /* 580 * Update recorded end_va, we depend on r4 pointing to the 581 * pre-relocated thread_core_local[core_pos]. 582 */ 583 ldr r1, =boot_mmu_config 584 ldr r1, [r1, #CORE_MMU_CONFIG_MAP_OFFSET] 585 add r4, r4, r1 586 ldr r0, [r4, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 587 add r0, r0, r1 588 str r0, [r4, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 589 ldr r0, [r4, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 590 add r0, r0, r1 591 str r0, [r4, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 592 593 cps #CPSR_MODE_IRQ 594 mov sp, r0 595 cps #CPSR_MODE_FIQ 596 mov sp, r0 597 cps #CPSR_MODE_ABT 598 mov sp, r4 599 cps #CPSR_MODE_UND 600 mov sp, r4 601 cps #CPSR_MODE_SVC 602 603#if defined(CFG_CORE_DEBUG_CHECK_STACKS) 604 /* 605 * Clear thread_core_local[0].stackcheck_recursion now that the 606 * stack pointer matches recorded information. 607 */ 608 mov r2, #0 609 strb r2, [r4, #THREAD_CORE_LOCAL_STACKCHECK_RECURSION] 610#endif 611 /* 612 * Reinitialize console, since register_serial_console() has 613 * previously registered a PA and with ASLR the VA is different 614 * from the PA. 615 */ 616 bl console_init 617#endif 618 619#ifdef CFG_NS_VIRTUALIZATION 620 /* 621 * Initialize partition tables for each partition to 622 * default_partition which has been relocated now to a different VA 623 */ 624 bl core_mmu_set_default_prtn_tbl 625#endif 626 627 bl boot_init_primary_early 628 bl boot_init_primary_late 629#ifndef CFG_NS_VIRTUALIZATION 630 mov r9, sp 631 ldr r0, =threads 632 ldr r0, [r0, #THREAD_CTX_STACK_VA_END] 633 mov sp, r0 634 bl thread_get_core_local 635 mov r8, r0 636 mov r0, #0 637 str r0, [r8, #THREAD_CORE_LOCAL_FLAGS] 638#endif 639 bl boot_init_primary_runtime 640 bl boot_init_primary_final 641#ifndef CFG_NS_VIRTUALIZATION 642 mov r0, #THREAD_CLF_TMP 643 str r0, [r8, #THREAD_CORE_LOCAL_FLAGS] 644 mov sp, r9 645#endif 646 647#ifdef _CFG_CORE_STACK_PROTECTOR 648 /* Update stack canary value */ 649 sub sp, sp, #0x8 650 mov r0, sp 651 mov r1, #1 652 mov r2, #0x4 653 bl plat_get_random_stack_canaries 654 ldr r0, [sp] 655 ldr r1, =__stack_chk_guard 656 str r0, [r1] 657 add sp, sp, #0x8 658#endif 659 660 /* 661 * In case we've touched memory that secondary CPUs will use before 662 * they have turned on their D-cache, clean and invalidate the 663 * D-cache before exiting to normal world. 664 */ 665 flush_cache_vrange(cached_mem_start, boot_cached_mem_end) 666 667 /* release secondary boot cores and sync with them */ 668 cpu_is_ready 669 flush_cpu_semaphores 670 wait_secondary 671 672#ifdef CFG_PL310_LOCKED 673#ifdef CFG_PL310_SIP_PROTOCOL 674#error "CFG_PL310_LOCKED must not be defined when CFG_PL310_SIP_PROTOCOL=y" 675#endif 676 /* lock/invalidate all lines: pl310 behaves as if disable */ 677 bl pl310_base 678 bl arm_cl2_lockallways 679 bl pl310_base 680 bl arm_cl2_cleaninvbyway 681#endif 682 683 /* 684 * Clear current thread id now to allow the thread to be reused on 685 * next entry. Matches the thread_init_boot_thread() in 686 * boot.c. 687 */ 688#ifndef CFG_NS_VIRTUALIZATION 689 bl thread_clr_boot_thread 690#endif 691 692#ifdef CFG_CORE_FFA 693 ldr r0, =cpu_on_handler 694 /* 695 * Compensate for the virtual map offset since cpu_on_handler() is 696 * called with MMU off. 697 */ 698 ldr r1, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 699 sub r0, r0, r1 700 bl thread_spmc_register_secondary_ep 701 b thread_ffa_msg_wait 702#else /* CFG_CORE_FFA */ 703 704#if defined(CFG_WITH_ARM_TRUSTED_FW) 705 ldr r0, =boot_mmu_config 706 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 707 ldr r1, =thread_vector_table 708 /* Pass the vector address returned from main_init */ 709 sub r1, r1, r0 710#else 711 /* Initialize secure monitor */ 712 add r0, sp, #__STACK_TMP_OFFS 713 bl sm_init 714 ldr r0, =boot_arg_nsec_entry 715 ldr r0, [r0] 716 bl init_sec_mon 717 718 /* Relay standard bootarg #1 and #2 to non-secure entry */ 719 mov r4, #0 720 mov r3, r6 /* std bootarg #2 for register R2 */ 721 mov r2, r5 /* std bootarg #1 for register R1 */ 722 mov r1, #0 723#endif /* CFG_WITH_ARM_TRUSTED_FW */ 724 725 mov r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 726 smc #0 727 /* SMC should not return */ 728 panic_at_smc_return 729#endif /* CFG_CORE_FFA */ 730END_FUNC reset_primary 731 732#ifdef CFG_BOOT_SYNC_CPU 733LOCAL_DATA sem_cpu_sync_start , : 734 .word sem_cpu_sync 735END_DATA sem_cpu_sync_start 736 737LOCAL_DATA sem_cpu_sync_end , : 738 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 739END_DATA sem_cpu_sync_end 740#endif 741 742LOCAL_DATA cached_mem_start , : 743 .word __text_start 744END_DATA cached_mem_start 745 746#ifndef CFG_WITH_PAGER 747LOCAL_DATA boot_embdata_ptr , : 748 .skip 4 749END_DATA boot_embdata_ptr 750#endif 751 752LOCAL_FUNC unhandled_cpu , : 753 wfi 754 b unhandled_cpu 755END_FUNC unhandled_cpu 756 757#ifdef CFG_CORE_ASLR 758LOCAL_FUNC relocate , : 759 push {r4-r5} 760 /* r0 holds load offset */ 761#ifdef CFG_WITH_PAGER 762 ldr r12, =__init_end 763#else 764 ldr r12, =boot_embdata_ptr 765 ldr r12, [r12] 766#endif 767 ldr r2, [r12, #BOOT_EMBDATA_RELOC_OFFSET] 768 ldr r3, [r12, #BOOT_EMBDATA_RELOC_LEN] 769 770 mov_imm r1, TEE_LOAD_ADDR 771 add r2, r2, r12 /* start of relocations */ 772 add r3, r3, r2 /* end of relocations */ 773 774 /* 775 * Relocations are not formatted as Rel32, instead they are in a 776 * compressed format created by get_reloc_bin() in 777 * scripts/gen_tee_bin.py 778 * 779 * All the R_ARM_RELATIVE relocations are translated into a list of 780 * 32-bit offsets from TEE_LOAD_ADDR. At each address a 32-bit 781 * value pointed out which increased with the load offset. 782 */ 783 784#ifdef CFG_WITH_PAGER 785 /* 786 * With pager enabled we can only relocate the pager and init 787 * parts, the rest has to be done when a page is populated. 788 */ 789 sub r12, r12, r1 790#endif 791 792 b 2f 793 /* Loop over the relocation addresses and process all entries */ 7941: ldr r4, [r2], #4 795#ifdef CFG_WITH_PAGER 796 /* Skip too large addresses */ 797 cmp r4, r12 798 bge 2f 799#endif 800 ldr r5, [r4, r1] 801 add r5, r5, r0 802 str r5, [r4, r1] 803 8042: cmp r2, r3 805 bne 1b 806 807 pop {r4-r5} 808 bx lr 809END_FUNC relocate 810#endif 811 812/* 813 * void enable_mmu(unsigned long core_pos); 814 * 815 * This function depends on being mapped with in the identity map where 816 * physical address and virtual address is the same. After MMU has been 817 * enabled the instruction pointer will be updated to execute as the new 818 * offset instead. Stack pointers and the return address are updated. 819 */ 820LOCAL_FUNC enable_mmu , : , .identity_map 821 /* r0 = core pos */ 822 adr r1, boot_mmu_config 823 824#ifdef CFG_WITH_LPAE 825 ldm r1!, {r2, r3} 826 /* 827 * r2 = ttbcr 828 * r3 = mair0 829 */ 830 write_ttbcr r2 831 write_mair0 r3 832 833 ldm r1!, {r2, r3} 834 /* 835 * r2 = ttbr0_base 836 * r3 = ttbr0_core_offset 837 */ 838 839 /* 840 * ttbr0_el1 = ttbr0_base + ttbr0_core_offset * core_pos 841 */ 842 mla r12, r0, r3, r2 843 mov r0, #0 844 write_ttbr0_64bit r12, r0 845 write_ttbr1_64bit r0, r0 846#else 847 ldm r1!, {r2, r3} 848 /* 849 * r2 = prrr 850 * r3 = nmrr 851 */ 852 write_prrr r2 853 write_nmrr r3 854 855 ldm r1!, {r2, r3} 856 /* 857 * r2 = dacr 858 * r3 = ttbcr 859 */ 860 write_dacr r2 861 write_ttbcr r3 862 863 ldm r1!, {r2} 864 /* r2 = ttbr */ 865 write_ttbr0 r2 866 write_ttbr1 r2 867 868 mov r2, #0 869 write_contextidr r2 870#endif 871 ldm r1!, {r2} 872 /* r2 = load_offset (always 0 if CFG_CORE_ASLR=n) */ 873 isb 874 875 /* Invalidate TLB */ 876 write_tlbiall 877 878 /* 879 * Make sure translation table writes have drained into memory and 880 * the TLB invalidation is complete. 881 */ 882 dsb sy 883 isb 884 885 read_sctlr r0 886 orr r0, r0, #SCTLR_M 887#ifndef CFG_WITH_LPAE 888 /* Enable Access flag (simplified access permissions) and TEX remap */ 889 orr r0, r0, #(SCTLR_AFE | SCTLR_TRE) 890#endif 891 write_sctlr r0 892 isb 893 894 /* Update vbar */ 895 read_vbar r1 896 add r1, r1, r2 897 write_vbar r1 898 isb 899 900 /* Invalidate instruction cache and branch predictor */ 901 write_iciallu 902 write_bpiall 903 isb 904 905 read_sctlr r0 906 /* Enable I and D cache */ 907 orr r0, r0, #SCTLR_I 908 orr r0, r0, #SCTLR_C 909#if defined(CFG_ENABLE_SCTLR_Z) 910 /* 911 * This is only needed on ARMv7 architecture and hence conditionned 912 * by configuration directive CFG_ENABLE_SCTLR_Z. For recent 913 * architectures, the program flow prediction is automatically 914 * enabled upon MMU enablement. 915 */ 916 orr r0, r0, #SCTLR_Z 917#endif 918 write_sctlr r0 919 isb 920 921 /* Adjust stack pointer and return address */ 922 add sp, sp, r2 923 add lr, lr, r2 924 925 bx lr 926END_FUNC enable_mmu 927 928LOCAL_DATA stack_tmp_rel , : 929 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 930END_DATA stack_tmp_rel 931 932LOCAL_DATA stack_tmp_stride_rel , : 933 .word stack_tmp_stride - stack_tmp_stride_rel 934END_DATA stack_tmp_stride_rel 935 936DATA boot_mmu_config , : /* struct core_mmu_config */ 937 .skip CORE_MMU_CONFIG_SIZE 938END_DATA boot_mmu_config 939 940#if defined(CFG_WITH_ARM_TRUSTED_FW) 941FUNC cpu_on_handler , : , .identity_map 942UNWIND( .cantunwind) 943 mov r4, r0 944 mov r5, r1 945 mov r6, lr 946 947 set_sctlr 948 isb 949 950 ldr r0, =reset_vect_table 951 write_vbar r0 952 953 mov r4, lr 954 955 bl __get_core_pos 956 bl enable_mmu 957 958 set_sp 959 960 mov r0, r4 961 mov r1, r5 962 bl boot_cpu_on_handler 963#ifdef CFG_CORE_FFA 964 b thread_ffa_msg_wait 965#else 966 bx r6 967#endif 968END_FUNC cpu_on_handler 969DECLARE_KEEP_PAGER cpu_on_handler 970 971#else /* defined(CFG_WITH_ARM_TRUSTED_FW) */ 972 973LOCAL_FUNC reset_secondary , : , .identity_map 974UNWIND( .cantunwind) 975 ldr r0, =reset_vect_table 976 write_vbar r0 977 978 wait_primary 979 980 set_sp 981 982#if defined (CFG_BOOT_SECONDARY_REQUEST) 983 /* if L1 is not invalidated before, do it here */ 984 mov r0, #DCACHE_OP_INV 985 bl dcache_op_level1 986#endif 987 988 bl __get_core_pos 989 bl enable_mmu 990 991 cpu_is_ready 992 993#if defined (CFG_BOOT_SECONDARY_REQUEST) 994 /* 995 * boot_core_hpen() return value (r0) is address of 996 * ns entry context structure 997 */ 998 bl boot_core_hpen 999 ldm r0, {r0, r6} 1000 mov r8, r0 1001#else 1002 mov r6, #0 1003#endif 1004 bl boot_init_secondary 1005 1006 /* Initialize secure monitor */ 1007 add r0, sp, #__STACK_TMP_OFFS 1008 bl sm_init 1009 mov r0, r8 /* ns-entry address */ 1010 bl init_sec_mon 1011 1012 mov r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 1013 mov r1, r6 1014 mov r2, #0 1015 mov r3, #0 1016 mov r4, #0 1017 smc #0 1018 /* SMC should not return */ 1019 panic_at_smc_return 1020END_FUNC reset_secondary 1021DECLARE_KEEP_PAGER reset_secondary 1022#endif /* defined(CFG_WITH_ARM_TRUSTED_FW) */ 1023