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