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 <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 /* Mark the entire stack area as OK */ 437 ldr r2, =CFG_ASAN_SHADOW_OFFSET 438 ldr r0, =__nozi_stack_start /* start */ 439 lsr r0, r0, #ASAN_BLOCK_SHIFT 440 add r0, r0, r2 441 ldr r1, =__nozi_stack_end /* limit */ 442 lsr r1, r1, #ASAN_BLOCK_SHIFT 443 add r1, r1, r2 444 mov r2, #0 445shadow_stack_access_ok: 446 strb r2, [r0], #1 447 cmp r0, r1 448 bls shadow_stack_access_ok 449#endif 450 451#if defined(CFG_DYN_STACK_CONFIG) 452 ldr r0, =boot_embdata_ptr 453 ldr r0, [r0] 454 sub r1, r0, #THREAD_BOOT_INIT_TMP_ALLOC 455 456 /* Clear the allocated struct thread_core_local */ 457 add r2, r1, #THREAD_CORE_LOCAL_SIZE 458 mov r3, #0 4591: str r3, [r2, #-4]! 460 cmp r2, r1 461 bgt 1b 462 463 sub r0, r0, #(__STACK_TMP_OFFS + __STACK_CANARY_SIZE) 464 mov r2, #THREAD_ID_INVALID 465 str r2, [r1, #THREAD_CORE_LOCAL_CURR_THREAD] 466 mov r2, #THREAD_CLF_TMP 467 str r2, [r1, #THREAD_CORE_LOCAL_FLAGS] 468 str r0, [r1, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 469 add r2, r1, #(THREAD_BOOT_INIT_TMP_ALLOC / 2) 470 sub r2, r2, #__STACK_CANARY_SIZE 471 str r2, [r1, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 472 mov sp, r0 473 cps #CPSR_MODE_IRQ 474 mov sp, r0 475 cps #CPSR_MODE_FIQ 476 mov sp, r0 477 cps #CPSR_MODE_ABT 478 mov sp, r1 479 cps #CPSR_MODE_UND 480 mov sp, r1 481 cps #CPSR_MODE_SVC 482 /* 483 * Record a single core, to be changed later before secure world 484 * boot is done. 485 */ 486 ldr r2, =thread_core_local 487 str r1, [r2] 488 ldr r2, =thread_core_count 489 mov r0, #1 490 str r0, [r2] 491#else 492 set_sp 493 494 /* Initialize thread_core_local[current_cpu_id] for early boot */ 495 bl thread_get_core_local 496 push {r0,r1} 497 bl thread_get_abt_stack 498 pop {r1,r2} 499 mov r3, sp 500 501 cps #CPSR_MODE_IRQ 502 mov sp, r3 503 cps #CPSR_MODE_FIQ 504 mov sp, r3 505 cps #CPSR_MODE_ABT 506 mov sp, r1 507 cps #CPSR_MODE_UND 508 mov sp, r1 509 cps #CPSR_MODE_SVC 510 511 str sp, [r1, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 512 str r0, [r1, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 513 mov r0, #THREAD_ID_INVALID 514 str r0, [r1, #THREAD_CORE_LOCAL_CURR_THREAD] 515 mov r0, #THREAD_CLF_TMP 516 str r0, [r1, #THREAD_CORE_LOCAL_FLAGS] 517#endif 518 519 /* complete ARM secure MP common configuration */ 520 bl plat_primary_init_early 521 522 /* Enable Console */ 523 bl console_init 524 525 mov r0, r8 526 mov r1, #0 527 push {r0, r1} 528 mov r0, r4 529 mov r1, r5 530 mov r2, r6 531 mov r3, r7 532 bl boot_save_args 533 add sp, sp, #(2 * 4) 534 535#ifdef CFG_WITH_PAGER 536 ldr r0, =__init_end /* pointer to boot_embdata */ 537 ldr r1, [r0] /* struct boot_embdata::total_len */ 538 add r0, r0, r1 539 mov_imm r1, 0xfff 540 add r0, r0, r1 /* round up */ 541 bic r0, r0, r1 /* to next page */ 542 mov_imm r1, (TEE_RAM_PH_SIZE + TEE_RAM_START) 543 mov r2, r1 544#else 545 ldr r0, =__vcore_free_start 546 ldr r1, =boot_embdata_ptr 547 ldr r1, [r1] 548#ifdef CFG_DYN_STACK_CONFIG 549 sub r1, r1, #THREAD_BOOT_INIT_TMP_ALLOC 550#endif 551 ldr r2, =__vcore_free_end 552#endif 553 bl boot_mem_init 554 555#ifdef CFG_PL310 556 bl pl310_base 557 bl arm_cl2_config 558#endif 559 560 /* 561 * Invalidate dcache for all memory used during initialization to 562 * avoid nasty surprices when the cache is turned on. We must not 563 * invalidate memory not used by OP-TEE since we may invalidate 564 * entries used by for instance ARM Trusted Firmware. 565 */ 566 inval_cache_vrange(cached_mem_start, boot_cached_mem_end) 567 568#if defined(CFG_PL310) && !defined(CFG_PL310_SIP_PROTOCOL) 569 /* Enable PL310 if not yet enabled */ 570 bl pl310_base 571 bl arm_cl2_enable 572#endif 573 574#if !defined(CFG_WITH_ARM_TRUSTED_FW) 575 ldr r0, =thread_core_local 576 ldr r0, [r0] 577 ldr r1, =thread_core_local_pa 578 str r0, [r1] 579#endif 580 581#ifdef CFG_CORE_ASLR 582 bl get_aslr_seed 583#ifdef CFG_CORE_ASLR_SEED 584 mov_imm r0, CFG_CORE_ASLR_SEED 585#endif 586#else 587 mov r0, #0 588#endif 589 590 ldr r1, =boot_mmu_config 591 bl core_init_mmu_map 592 593#ifdef CFG_CORE_ASLR 594 /* 595 * Save a pointer to thread_core_local[core_pos] since we can't 596 * call thread_get_core_local() again before the recorded end_va's 597 * have been updated below. 598 */ 599 bl thread_get_core_local 600 mov r4, r0 601 602 /* 603 * Process relocation information for updating with the virtual map 604 * offset. We're doing this now before MMU is enabled as some of 605 * the memory will become write protected. 606 */ 607 ldr r0, =boot_mmu_config 608 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 609 /* 610 * Update boot_cached_mem_end address with load offset since it was 611 * calculated before relocation. 612 */ 613 ldr r3, =boot_cached_mem_end 614 ldr r2, [r3] 615 add r2, r2, r0 616 str r2, [r3] 617 618 bl relocate 619#endif 620 621 bl __get_core_pos 622 bl enable_mmu 623#ifdef CFG_CORE_ASLR 624 /* 625 * Update recorded end_va, we depend on r4 pointing to the 626 * pre-relocated thread_core_local[core_pos]. 627 * 628 * This must be done before calling into C code to make sure that 629 * the stack pointer matches what we have in thread_core_local[]. 630 */ 631 ldr r1, =boot_mmu_config 632 ldr r1, [r1, #CORE_MMU_CONFIG_MAP_OFFSET] 633#if defined(CFG_DYN_STACK_CONFIG) 634 ldr r0, =thread_core_local 635 add r2, r4, r1 636 str r2, [r0] 637#endif 638 add r4, r4, r1 639 ldr r0, [r4, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 640 add r0, r0, r1 641 str r0, [r4, #THREAD_CORE_LOCAL_ABT_STACK_VA_END] 642 ldr r0, [r4, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 643 add r0, r0, r1 644 str r0, [r4, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 645 646 cps #CPSR_MODE_IRQ 647 mov sp, r0 648 cps #CPSR_MODE_FIQ 649 mov sp, r0 650 cps #CPSR_MODE_ABT 651 mov sp, r4 652 cps #CPSR_MODE_UND 653 mov sp, r4 654 cps #CPSR_MODE_SVC 655 656 /* Update relocations recorded with boot_mem_add_reloc() */ 657 ldr r0, =boot_mmu_config 658 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 659 bl boot_mem_relocate 660 /* 661 * Reinitialize console, since register_serial_console() has 662 * previously registered a PA and with ASLR the VA is different 663 * from the PA. 664 */ 665 bl console_init 666#endif 667 668#ifdef CFG_NS_VIRTUALIZATION 669 /* 670 * Initialize partition tables for each partition to 671 * default_partition which has been relocated now to a different VA 672 */ 673 bl core_mmu_set_default_prtn_tbl 674#endif 675 676 bl boot_init_primary_early 677 bl boot_init_primary_late 678 679#if defined(CFG_DYN_STACK_CONFIG) 680#if !defined(CFG_WITH_ARM_TRUSTED_FW) 681 /* Update thread_core_local_pa with a new physical address */ 682 ldr r0, =__thread_core_local_new 683 ldr r0, [r0] 684 bl virt_to_phys 685 ldr r1, =thread_core_local_pa 686 str r0, [r1] 687#endif 688 bl __get_core_pos 689 690 /* 691 * Switch to the new thread_core_local and thread_core_count and 692 * keep the pointer to the new thread_core_local in r1. 693 */ 694 ldr r1, =__thread_core_count_new 695 ldr r1, [r1] 696 ldr r2, =thread_core_count; 697 str r1, [r2] 698 ldr r1, =__thread_core_local_new 699 ldr r1, [r1] 700 ldr r2, =thread_core_local 701 str r1, [r2] 702 703 /* 704 * Update to use the new stacks and thread_core_local. Clear 705 * thread_core_local[0].stackcheck_recursion now that the stack 706 * pointer matches recorded information. 707 */ 708 mov r2, #THREAD_CORE_LOCAL_SIZE 709 /* r3 = r2 * r0 + r1 */ 710 mla r3, r2, r0, r1 711 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 712 mov sp, r0 713 cps #CPSR_MODE_IRQ 714 mov sp, r0 715 cps #CPSR_MODE_FIQ 716 mov sp, r0 717 cps #CPSR_MODE_ABT 718 mov sp, r3 719 cps #CPSR_MODE_UND 720 mov sp, r3 721 cps #CPSR_MODE_SVC 722#endif 723 724#ifndef CFG_NS_VIRTUALIZATION 725 mov r9, sp 726 ldr r0, =threads 727 ldr r0, [r0] 728 ldr r0, [r0, #THREAD_CTX_STACK_VA_END] 729 mov sp, r0 730 bl thread_get_core_local 731 mov r8, r0 732 mov r0, #0 733 str r0, [r8, #THREAD_CORE_LOCAL_FLAGS] 734#endif 735 bl boot_init_primary_runtime 736 bl boot_init_primary_final 737#ifndef CFG_NS_VIRTUALIZATION 738 mov r0, #THREAD_CLF_TMP 739 str r0, [r8, #THREAD_CORE_LOCAL_FLAGS] 740 mov sp, r9 741#endif 742 743#ifdef _CFG_CORE_STACK_PROTECTOR 744 /* Update stack canary value */ 745 sub sp, sp, #0x8 746 mov r0, sp 747 mov r1, #1 748 mov r2, #0x4 749 bl plat_get_random_stack_canaries 750 ldr r0, [sp] 751 ldr r1, =__stack_chk_guard 752 str r0, [r1] 753 add sp, sp, #0x8 754#endif 755 756 /* 757 * In case we've touched memory that secondary CPUs will use before 758 * they have turned on their D-cache, clean and invalidate the 759 * D-cache before exiting to normal world. 760 */ 761 flush_cache_vrange(cached_mem_start, boot_cached_mem_end) 762 763 /* release secondary boot cores and sync with them */ 764 cpu_is_ready 765 flush_cpu_semaphores 766 wait_secondary 767 768#ifdef CFG_PL310_LOCKED 769#ifdef CFG_PL310_SIP_PROTOCOL 770#error "CFG_PL310_LOCKED must not be defined when CFG_PL310_SIP_PROTOCOL=y" 771#endif 772 /* lock/invalidate all lines: pl310 behaves as if disable */ 773 bl pl310_base 774 bl arm_cl2_lockallways 775 bl pl310_base 776 bl arm_cl2_cleaninvbyway 777#endif 778 779 /* 780 * Clear current thread id now to allow the thread to be reused on 781 * next entry. Matches the thread_init_boot_thread() in 782 * boot.c. 783 */ 784#ifndef CFG_NS_VIRTUALIZATION 785 bl thread_clr_boot_thread 786#endif 787 788#ifdef CFG_CORE_FFA 789 ldr r0, =cpu_on_handler 790 /* 791 * Compensate for the virtual map offset since cpu_on_handler() is 792 * called with MMU off. 793 */ 794 ldr r1, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 795 sub r0, r0, r1 796 bl thread_spmc_register_secondary_ep 797 b thread_ffa_msg_wait 798#else /* CFG_CORE_FFA */ 799 800#if defined(CFG_WITH_ARM_TRUSTED_FW) 801 ldr r0, =boot_mmu_config 802 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 803 ldr r1, =thread_vector_table 804 /* Pass the vector address returned from main_init */ 805 sub r1, r1, r0 806#else 807 /* Initialize secure monitor */ 808 add r0, sp, #__STACK_TMP_OFFS 809 bl sm_init 810 ldr r0, =boot_arg_nsec_entry 811 ldr r0, [r0] 812 bl init_sec_mon 813 814 /* Relay standard bootarg #1 and #2 to non-secure entry */ 815 mov r4, #0 816 mov r3, r6 /* std bootarg #2 for register R2 */ 817 mov r2, r5 /* std bootarg #1 for register R1 */ 818 mov r1, #0 819#endif /* CFG_WITH_ARM_TRUSTED_FW */ 820 821 mov r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 822 smc #0 823 /* SMC should not return */ 824 panic_at_smc_return 825#endif /* CFG_CORE_FFA */ 826END_FUNC reset_primary 827 828#ifdef CFG_BOOT_SYNC_CPU 829LOCAL_DATA sem_cpu_sync_start , : 830 .word sem_cpu_sync 831END_DATA sem_cpu_sync_start 832 833LOCAL_DATA sem_cpu_sync_end , : 834 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 835END_DATA sem_cpu_sync_end 836#endif 837 838LOCAL_DATA cached_mem_start , : 839 .word __text_start 840END_DATA cached_mem_start 841 842#ifndef CFG_WITH_PAGER 843LOCAL_DATA boot_embdata_ptr , : 844 .skip 4 845END_DATA boot_embdata_ptr 846#endif 847 848LOCAL_FUNC unhandled_cpu , : 849 wfi 850 b unhandled_cpu 851END_FUNC unhandled_cpu 852 853#ifdef CFG_CORE_ASLR 854LOCAL_FUNC relocate , : 855 push {r4-r5} 856 /* r0 holds load offset */ 857#ifdef CFG_WITH_PAGER 858 ldr r12, =__init_end 859#else 860 ldr r12, =boot_embdata_ptr 861 ldr r12, [r12] 862#endif 863 ldr r2, [r12, #BOOT_EMBDATA_RELOC_OFFSET] 864 ldr r3, [r12, #BOOT_EMBDATA_RELOC_LEN] 865 866 mov_imm r1, TEE_LOAD_ADDR 867 add r2, r2, r12 /* start of relocations */ 868 add r3, r3, r2 /* end of relocations */ 869 870 /* 871 * Relocations are not formatted as Rel32, instead they are in a 872 * compressed format created by get_reloc_bin() in 873 * scripts/gen_tee_bin.py 874 * 875 * All the R_ARM_RELATIVE relocations are translated into a list of 876 * 32-bit offsets from TEE_LOAD_ADDR. At each address a 32-bit 877 * value pointed out which increased with the load offset. 878 */ 879 880#ifdef CFG_WITH_PAGER 881 /* 882 * With pager enabled we can only relocate the pager and init 883 * parts, the rest has to be done when a page is populated. 884 */ 885 sub r12, r12, r1 886#endif 887 888 b 2f 889 /* Loop over the relocation addresses and process all entries */ 8901: ldr r4, [r2], #4 891#ifdef CFG_WITH_PAGER 892 /* Skip too large addresses */ 893 cmp r4, r12 894 bge 2f 895#endif 896 ldr r5, [r4, r1] 897 add r5, r5, r0 898 str r5, [r4, r1] 899 9002: cmp r2, r3 901 bne 1b 902 903 pop {r4-r5} 904 bx lr 905END_FUNC relocate 906#endif 907 908/* 909 * void enable_mmu(unsigned long core_pos); 910 * 911 * This function depends on being mapped with in the identity map where 912 * physical address and virtual address is the same. After MMU has been 913 * enabled the instruction pointer will be updated to execute as the new 914 * offset instead. Stack pointers and the return address are updated. 915 */ 916LOCAL_FUNC enable_mmu , : , .identity_map 917 /* r0 = core pos */ 918 adr r1, boot_mmu_config 919 920#ifdef CFG_WITH_LPAE 921 ldm r1!, {r2, r3} 922 /* 923 * r2 = ttbcr 924 * r3 = mair0 925 */ 926 write_ttbcr r2 927 write_mair0 r3 928 929 ldm r1!, {r2, r3} 930 /* 931 * r2 = ttbr0_base 932 * r3 = ttbr0_core_offset 933 */ 934 935 /* 936 * ttbr0_el1 = ttbr0_base + ttbr0_core_offset * core_pos 937 */ 938 mla r12, r0, r3, r2 939 mov r0, #0 940 write_ttbr0_64bit r12, r0 941 write_ttbr1_64bit r0, r0 942#else 943 ldm r1!, {r2, r3} 944 /* 945 * r2 = prrr 946 * r3 = nmrr 947 */ 948 write_prrr r2 949 write_nmrr r3 950 951 ldm r1!, {r2, r3} 952 /* 953 * r2 = dacr 954 * r3 = ttbcr 955 */ 956 write_dacr r2 957 write_ttbcr r3 958 959 ldm r1!, {r2} 960 /* r2 = ttbr */ 961 write_ttbr0 r2 962 write_ttbr1 r2 963 964 mov r2, #0 965 write_contextidr r2 966#endif 967 ldm r1!, {r2} 968 /* r2 = load_offset (always 0 if CFG_CORE_ASLR=n) */ 969 isb 970 971 /* Invalidate TLB */ 972 write_tlbiall 973 974 /* 975 * Make sure translation table writes have drained into memory and 976 * the TLB invalidation is complete. 977 */ 978 dsb sy 979 isb 980 981 read_sctlr r0 982 orr r0, r0, #SCTLR_M 983#ifndef CFG_WITH_LPAE 984 /* Enable Access flag (simplified access permissions) and TEX remap */ 985 orr r0, r0, #(SCTLR_AFE | SCTLR_TRE) 986#endif 987 write_sctlr r0 988 isb 989 990 /* Update vbar */ 991 read_vbar r1 992 add r1, r1, r2 993 write_vbar r1 994 isb 995 996 /* Invalidate instruction cache and branch predictor */ 997 write_iciallu 998 write_bpiall 999 isb 1000 1001 read_sctlr r0 1002 /* Enable I and D cache */ 1003 orr r0, r0, #SCTLR_I 1004 orr r0, r0, #SCTLR_C 1005#if defined(CFG_ENABLE_SCTLR_Z) 1006 /* 1007 * This is only needed on ARMv7 architecture and hence conditionned 1008 * by configuration directive CFG_ENABLE_SCTLR_Z. For recent 1009 * architectures, the program flow prediction is automatically 1010 * enabled upon MMU enablement. 1011 */ 1012 orr r0, r0, #SCTLR_Z 1013#endif 1014 write_sctlr r0 1015 isb 1016 1017 /* Adjust stack pointer and return address */ 1018 add sp, sp, r2 1019 add lr, lr, r2 1020 1021 bx lr 1022END_FUNC enable_mmu 1023 1024#if !defined(CFG_DYN_STACK_CONFIG) 1025LOCAL_DATA stack_tmp_rel , : 1026 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 1027END_DATA stack_tmp_rel 1028 1029LOCAL_DATA stack_tmp_stride_rel , : 1030 .word stack_tmp_stride - stack_tmp_stride_rel 1031END_DATA stack_tmp_stride_rel 1032#endif 1033 1034DATA boot_mmu_config , : /* struct core_mmu_config */ 1035 .skip CORE_MMU_CONFIG_SIZE 1036END_DATA boot_mmu_config 1037 1038#if defined(CFG_WITH_ARM_TRUSTED_FW) 1039FUNC cpu_on_handler , : , .identity_map 1040UNWIND( .cantunwind) 1041 mov r4, r0 1042 mov r5, r1 1043 mov r6, lr 1044 1045 set_sctlr 1046 isb 1047 1048 ldr r0, =reset_vect_table 1049 write_vbar r0 1050 1051 mov r4, lr 1052 1053 bl __get_core_pos 1054 bl enable_mmu 1055 1056 /* 1057 * Use the stacks from thread_core_local. 1058 */ 1059 bl __get_core_pos 1060 ldr r1, =thread_core_local 1061 ldr r1, [r1] 1062 mov r2, #THREAD_CORE_LOCAL_SIZE 1063 /* r3 = r2 * r0 + r1 */ 1064 mla r3, r2, r0, r1 1065 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 1066 mov sp, r0 1067 cps #CPSR_MODE_IRQ 1068 mov sp, r0 1069 cps #CPSR_MODE_FIQ 1070 mov sp, r0 1071 cps #CPSR_MODE_ABT 1072 mov sp, r3 1073 cps #CPSR_MODE_UND 1074 mov sp, r3 1075 cps #CPSR_MODE_SVC 1076 1077 mov r0, r4 1078 mov r1, r5 1079 bl boot_cpu_on_handler 1080#ifdef CFG_CORE_FFA 1081 b thread_ffa_msg_wait 1082#else 1083 bx r6 1084#endif 1085END_FUNC cpu_on_handler 1086DECLARE_KEEP_PAGER cpu_on_handler 1087 1088#else /* defined(CFG_WITH_ARM_TRUSTED_FW) */ 1089 1090LOCAL_FUNC reset_secondary , : , .identity_map 1091UNWIND( .cantunwind) 1092 ldr r0, =reset_vect_table 1093 write_vbar r0 1094 1095 wait_primary 1096 1097 /* 1098 * Initialize stack pointer from thread_core_local, compensate for 1099 * ASLR if enabled. 1100 */ 1101#ifdef CFG_CORE_ASLR 1102 ldr r4, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 1103#endif 1104 bl __get_core_pos 1105 ldr r1, =thread_core_local_pa 1106#ifdef CFG_CORE_ASLR 1107 sub r1, r1, r4 1108#endif 1109 ldr r1, [r1] 1110 mov r2, #THREAD_CORE_LOCAL_SIZE 1111 /* r3 = r2 * r0 + r1 */ 1112 mla r3, r2, r0, r1 1113 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_PA_END] 1114 mov sp, r0 1115 1116#if defined (CFG_BOOT_SECONDARY_REQUEST) 1117 /* if L1 is not invalidated before, do it here */ 1118 mov r0, #DCACHE_OP_INV 1119 bl dcache_op_level1 1120#endif 1121 1122 bl __get_core_pos 1123 bl enable_mmu 1124 1125 /* 1126 * Use the stacks from thread_core_local. 1127 */ 1128 bl __get_core_pos 1129 ldr r1, =thread_core_local 1130 ldr r1, [r1] 1131 mov r2, #THREAD_CORE_LOCAL_SIZE 1132 /* r3 = r2 * r0 + r1 */ 1133 mla r3, r2, r0, r1 1134 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 1135 mov sp, r0 1136 cps #CPSR_MODE_IRQ 1137 mov sp, r0 1138 cps #CPSR_MODE_FIQ 1139 mov sp, r0 1140 cps #CPSR_MODE_ABT 1141 mov sp, r3 1142 cps #CPSR_MODE_UND 1143 mov sp, r3 1144 cps #CPSR_MODE_SVC 1145 1146 cpu_is_ready 1147 1148#if defined (CFG_BOOT_SECONDARY_REQUEST) 1149 /* 1150 * boot_core_hpen() return value (r0) is address of 1151 * ns entry context structure 1152 */ 1153 bl boot_core_hpen 1154 ldm r0, {r0, r6} 1155 mov r8, r0 1156#else 1157 mov r6, #0 1158#endif 1159 bl boot_init_secondary 1160 1161 /* Initialize secure monitor */ 1162 add r0, sp, #__STACK_TMP_OFFS 1163 bl sm_init 1164 mov r0, r8 /* ns-entry address */ 1165 bl init_sec_mon 1166 1167 mov r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 1168 mov r1, r6 1169 mov r2, #0 1170 mov r3, #0 1171 mov r4, #0 1172 smc #0 1173 /* SMC should not return */ 1174 panic_at_smc_return 1175END_FUNC reset_secondary 1176DECLARE_KEEP_PAGER reset_secondary 1177#endif /* defined(CFG_WITH_ARM_TRUSTED_FW) */ 1178