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, #THREAD_CTX_STACK_VA_END] 728 mov sp, r0 729 bl thread_get_core_local 730 mov r8, r0 731 mov r0, #0 732 str r0, [r8, #THREAD_CORE_LOCAL_FLAGS] 733#endif 734 bl boot_init_primary_runtime 735 bl boot_init_primary_final 736#ifndef CFG_NS_VIRTUALIZATION 737 mov r0, #THREAD_CLF_TMP 738 str r0, [r8, #THREAD_CORE_LOCAL_FLAGS] 739 mov sp, r9 740#endif 741 742#ifdef _CFG_CORE_STACK_PROTECTOR 743 /* Update stack canary value */ 744 sub sp, sp, #0x8 745 mov r0, sp 746 mov r1, #1 747 mov r2, #0x4 748 bl plat_get_random_stack_canaries 749 ldr r0, [sp] 750 ldr r1, =__stack_chk_guard 751 str r0, [r1] 752 add sp, sp, #0x8 753#endif 754 755 /* 756 * In case we've touched memory that secondary CPUs will use before 757 * they have turned on their D-cache, clean and invalidate the 758 * D-cache before exiting to normal world. 759 */ 760 flush_cache_vrange(cached_mem_start, boot_cached_mem_end) 761 762 /* release secondary boot cores and sync with them */ 763 cpu_is_ready 764 flush_cpu_semaphores 765 wait_secondary 766 767#ifdef CFG_PL310_LOCKED 768#ifdef CFG_PL310_SIP_PROTOCOL 769#error "CFG_PL310_LOCKED must not be defined when CFG_PL310_SIP_PROTOCOL=y" 770#endif 771 /* lock/invalidate all lines: pl310 behaves as if disable */ 772 bl pl310_base 773 bl arm_cl2_lockallways 774 bl pl310_base 775 bl arm_cl2_cleaninvbyway 776#endif 777 778 /* 779 * Clear current thread id now to allow the thread to be reused on 780 * next entry. Matches the thread_init_boot_thread() in 781 * boot.c. 782 */ 783#ifndef CFG_NS_VIRTUALIZATION 784 bl thread_clr_boot_thread 785#endif 786 787#ifdef CFG_CORE_FFA 788 ldr r0, =cpu_on_handler 789 /* 790 * Compensate for the virtual map offset since cpu_on_handler() is 791 * called with MMU off. 792 */ 793 ldr r1, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 794 sub r0, r0, r1 795 bl thread_spmc_register_secondary_ep 796 b thread_ffa_msg_wait 797#else /* CFG_CORE_FFA */ 798 799#if defined(CFG_WITH_ARM_TRUSTED_FW) 800 ldr r0, =boot_mmu_config 801 ldr r0, [r0, #CORE_MMU_CONFIG_MAP_OFFSET] 802 ldr r1, =thread_vector_table 803 /* Pass the vector address returned from main_init */ 804 sub r1, r1, r0 805#else 806 /* Initialize secure monitor */ 807 add r0, sp, #__STACK_TMP_OFFS 808 bl sm_init 809 ldr r0, =boot_arg_nsec_entry 810 ldr r0, [r0] 811 bl init_sec_mon 812 813 /* Relay standard bootarg #1 and #2 to non-secure entry */ 814 mov r4, #0 815 mov r3, r6 /* std bootarg #2 for register R2 */ 816 mov r2, r5 /* std bootarg #1 for register R1 */ 817 mov r1, #0 818#endif /* CFG_WITH_ARM_TRUSTED_FW */ 819 820 mov r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 821 smc #0 822 /* SMC should not return */ 823 panic_at_smc_return 824#endif /* CFG_CORE_FFA */ 825END_FUNC reset_primary 826 827#ifdef CFG_BOOT_SYNC_CPU 828LOCAL_DATA sem_cpu_sync_start , : 829 .word sem_cpu_sync 830END_DATA sem_cpu_sync_start 831 832LOCAL_DATA sem_cpu_sync_end , : 833 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 834END_DATA sem_cpu_sync_end 835#endif 836 837LOCAL_DATA cached_mem_start , : 838 .word __text_start 839END_DATA cached_mem_start 840 841#ifndef CFG_WITH_PAGER 842LOCAL_DATA boot_embdata_ptr , : 843 .skip 4 844END_DATA boot_embdata_ptr 845#endif 846 847LOCAL_FUNC unhandled_cpu , : 848 wfi 849 b unhandled_cpu 850END_FUNC unhandled_cpu 851 852#ifdef CFG_CORE_ASLR 853LOCAL_FUNC relocate , : 854 push {r4-r5} 855 /* r0 holds load offset */ 856#ifdef CFG_WITH_PAGER 857 ldr r12, =__init_end 858#else 859 ldr r12, =boot_embdata_ptr 860 ldr r12, [r12] 861#endif 862 ldr r2, [r12, #BOOT_EMBDATA_RELOC_OFFSET] 863 ldr r3, [r12, #BOOT_EMBDATA_RELOC_LEN] 864 865 mov_imm r1, TEE_LOAD_ADDR 866 add r2, r2, r12 /* start of relocations */ 867 add r3, r3, r2 /* end of relocations */ 868 869 /* 870 * Relocations are not formatted as Rel32, instead they are in a 871 * compressed format created by get_reloc_bin() in 872 * scripts/gen_tee_bin.py 873 * 874 * All the R_ARM_RELATIVE relocations are translated into a list of 875 * 32-bit offsets from TEE_LOAD_ADDR. At each address a 32-bit 876 * value pointed out which increased with the load offset. 877 */ 878 879#ifdef CFG_WITH_PAGER 880 /* 881 * With pager enabled we can only relocate the pager and init 882 * parts, the rest has to be done when a page is populated. 883 */ 884 sub r12, r12, r1 885#endif 886 887 b 2f 888 /* Loop over the relocation addresses and process all entries */ 8891: ldr r4, [r2], #4 890#ifdef CFG_WITH_PAGER 891 /* Skip too large addresses */ 892 cmp r4, r12 893 bge 2f 894#endif 895 ldr r5, [r4, r1] 896 add r5, r5, r0 897 str r5, [r4, r1] 898 8992: cmp r2, r3 900 bne 1b 901 902 pop {r4-r5} 903 bx lr 904END_FUNC relocate 905#endif 906 907/* 908 * void enable_mmu(unsigned long core_pos); 909 * 910 * This function depends on being mapped with in the identity map where 911 * physical address and virtual address is the same. After MMU has been 912 * enabled the instruction pointer will be updated to execute as the new 913 * offset instead. Stack pointers and the return address are updated. 914 */ 915LOCAL_FUNC enable_mmu , : , .identity_map 916 /* r0 = core pos */ 917 adr r1, boot_mmu_config 918 919#ifdef CFG_WITH_LPAE 920 ldm r1!, {r2, r3} 921 /* 922 * r2 = ttbcr 923 * r3 = mair0 924 */ 925 write_ttbcr r2 926 write_mair0 r3 927 928 ldm r1!, {r2, r3} 929 /* 930 * r2 = ttbr0_base 931 * r3 = ttbr0_core_offset 932 */ 933 934 /* 935 * ttbr0_el1 = ttbr0_base + ttbr0_core_offset * core_pos 936 */ 937 mla r12, r0, r3, r2 938 mov r0, #0 939 write_ttbr0_64bit r12, r0 940 write_ttbr1_64bit r0, r0 941#else 942 ldm r1!, {r2, r3} 943 /* 944 * r2 = prrr 945 * r3 = nmrr 946 */ 947 write_prrr r2 948 write_nmrr r3 949 950 ldm r1!, {r2, r3} 951 /* 952 * r2 = dacr 953 * r3 = ttbcr 954 */ 955 write_dacr r2 956 write_ttbcr r3 957 958 ldm r1!, {r2} 959 /* r2 = ttbr */ 960 write_ttbr0 r2 961 write_ttbr1 r2 962 963 mov r2, #0 964 write_contextidr r2 965#endif 966 ldm r1!, {r2} 967 /* r2 = load_offset (always 0 if CFG_CORE_ASLR=n) */ 968 isb 969 970 /* Invalidate TLB */ 971 write_tlbiall 972 973 /* 974 * Make sure translation table writes have drained into memory and 975 * the TLB invalidation is complete. 976 */ 977 dsb sy 978 isb 979 980 read_sctlr r0 981 orr r0, r0, #SCTLR_M 982#ifndef CFG_WITH_LPAE 983 /* Enable Access flag (simplified access permissions) and TEX remap */ 984 orr r0, r0, #(SCTLR_AFE | SCTLR_TRE) 985#endif 986 write_sctlr r0 987 isb 988 989 /* Update vbar */ 990 read_vbar r1 991 add r1, r1, r2 992 write_vbar r1 993 isb 994 995 /* Invalidate instruction cache and branch predictor */ 996 write_iciallu 997 write_bpiall 998 isb 999 1000 read_sctlr r0 1001 /* Enable I and D cache */ 1002 orr r0, r0, #SCTLR_I 1003 orr r0, r0, #SCTLR_C 1004#if defined(CFG_ENABLE_SCTLR_Z) 1005 /* 1006 * This is only needed on ARMv7 architecture and hence conditionned 1007 * by configuration directive CFG_ENABLE_SCTLR_Z. For recent 1008 * architectures, the program flow prediction is automatically 1009 * enabled upon MMU enablement. 1010 */ 1011 orr r0, r0, #SCTLR_Z 1012#endif 1013 write_sctlr r0 1014 isb 1015 1016 /* Adjust stack pointer and return address */ 1017 add sp, sp, r2 1018 add lr, lr, r2 1019 1020 bx lr 1021END_FUNC enable_mmu 1022 1023#if !defined(CFG_DYN_STACK_CONFIG) 1024LOCAL_DATA stack_tmp_rel , : 1025 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 1026END_DATA stack_tmp_rel 1027 1028LOCAL_DATA stack_tmp_stride_rel , : 1029 .word stack_tmp_stride - stack_tmp_stride_rel 1030END_DATA stack_tmp_stride_rel 1031#endif 1032 1033DATA boot_mmu_config , : /* struct core_mmu_config */ 1034 .skip CORE_MMU_CONFIG_SIZE 1035END_DATA boot_mmu_config 1036 1037#if defined(CFG_WITH_ARM_TRUSTED_FW) 1038FUNC cpu_on_handler , : , .identity_map 1039UNWIND( .cantunwind) 1040 mov r4, r0 1041 mov r5, r1 1042 mov r6, lr 1043 1044 set_sctlr 1045 isb 1046 1047 ldr r0, =reset_vect_table 1048 write_vbar r0 1049 1050 mov r4, lr 1051 1052 bl __get_core_pos 1053 bl enable_mmu 1054 1055 /* 1056 * Use the stacks from thread_core_local. 1057 */ 1058 bl __get_core_pos 1059 ldr r1, =thread_core_local 1060 ldr r1, [r1] 1061 mov r2, #THREAD_CORE_LOCAL_SIZE 1062 /* r3 = r2 * r0 + r1 */ 1063 mla r3, r2, r0, r1 1064 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 1065 mov sp, r0 1066 cps #CPSR_MODE_IRQ 1067 mov sp, r0 1068 cps #CPSR_MODE_FIQ 1069 mov sp, r0 1070 cps #CPSR_MODE_ABT 1071 mov sp, r3 1072 cps #CPSR_MODE_UND 1073 mov sp, r3 1074 cps #CPSR_MODE_SVC 1075 1076 mov r0, r4 1077 mov r1, r5 1078 bl boot_cpu_on_handler 1079#ifdef CFG_CORE_FFA 1080 b thread_ffa_msg_wait 1081#else 1082 bx r6 1083#endif 1084END_FUNC cpu_on_handler 1085DECLARE_KEEP_PAGER cpu_on_handler 1086 1087#else /* defined(CFG_WITH_ARM_TRUSTED_FW) */ 1088 1089LOCAL_FUNC reset_secondary , : , .identity_map 1090UNWIND( .cantunwind) 1091 ldr r0, =reset_vect_table 1092 write_vbar r0 1093 1094 wait_primary 1095 1096 /* 1097 * Initialize stack pointer from thread_core_local, compensate for 1098 * ASLR if enabled. 1099 */ 1100#ifdef CFG_CORE_ASLR 1101 ldr r4, boot_mmu_config + CORE_MMU_CONFIG_MAP_OFFSET 1102#endif 1103 bl __get_core_pos 1104 ldr r1, =thread_core_local_pa 1105#ifdef CFG_CORE_ASLR 1106 sub r1, r1, r4 1107#endif 1108 ldr r1, [r1] 1109 mov r2, #THREAD_CORE_LOCAL_SIZE 1110 /* r3 = r2 * r0 + r1 */ 1111 mla r3, r2, r0, r1 1112 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_PA_END] 1113 mov sp, r0 1114 1115#if defined (CFG_BOOT_SECONDARY_REQUEST) 1116 /* if L1 is not invalidated before, do it here */ 1117 mov r0, #DCACHE_OP_INV 1118 bl dcache_op_level1 1119#endif 1120 1121 bl __get_core_pos 1122 bl enable_mmu 1123 1124 /* 1125 * Use the stacks from thread_core_local. 1126 */ 1127 bl __get_core_pos 1128 ldr r1, =thread_core_local 1129 ldr r1, [r1] 1130 mov r2, #THREAD_CORE_LOCAL_SIZE 1131 /* r3 = r2 * r0 + r1 */ 1132 mla r3, r2, r0, r1 1133 ldr r0, [r3, #THREAD_CORE_LOCAL_TMP_STACK_VA_END] 1134 mov sp, r0 1135 cps #CPSR_MODE_IRQ 1136 mov sp, r0 1137 cps #CPSR_MODE_FIQ 1138 mov sp, r0 1139 cps #CPSR_MODE_ABT 1140 mov sp, r3 1141 cps #CPSR_MODE_UND 1142 mov sp, r3 1143 cps #CPSR_MODE_SVC 1144 1145 cpu_is_ready 1146 1147#if defined (CFG_BOOT_SECONDARY_REQUEST) 1148 /* 1149 * boot_core_hpen() return value (r0) is address of 1150 * ns entry context structure 1151 */ 1152 bl boot_core_hpen 1153 ldm r0, {r0, r6} 1154 mov r8, r0 1155#else 1156 mov r6, #0 1157#endif 1158 bl boot_init_secondary 1159 1160 /* Initialize secure monitor */ 1161 add r0, sp, #__STACK_TMP_OFFS 1162 bl sm_init 1163 mov r0, r8 /* ns-entry address */ 1164 bl init_sec_mon 1165 1166 mov r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE 1167 mov r1, r6 1168 mov r2, #0 1169 mov r3, #0 1170 mov r4, #0 1171 smc #0 1172 /* SMC should not return */ 1173 panic_at_smc_return 1174END_FUNC reset_secondary 1175DECLARE_KEEP_PAGER reset_secondary 1176#endif /* defined(CFG_WITH_ARM_TRUSTED_FW) */ 1177