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