1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2023 Andes Technology Corporation 4 * Copyright 2022-2023 NXP 5 */ 6 7#include <asm.S> 8#include <generated/asm-defines.h> 9#include <keep.h> 10#include <kernel/thread.h> 11#include <kernel/riscv_elf.h> 12#include <kernel/thread_private.h> 13#include <kernel/thread_private_arch.h> 14#include <mm/core_mmu.h> 15#include <platform_config.h> 16#include <riscv.h> 17#include <riscv_macros.S> 18#include <tee/optee_abi.h> 19#include <tee/teeabi_opteed.h> 20#include <tee/teeabi_opteed_macros.h> 21 22.section .data 23.balign 4 24 25#ifdef CFG_BOOT_SYNC_CPU 26.equ SEM_CPU_READY, 1 27#endif 28 29 /* 30 * Setup sp to point to the top of the tmp stack for the current CPU: 31 * sp is assigned: 32 * stack_tmp + (hart_index + 1) * stack_tmp_stride - STACK_TMP_GUARD 33 */ 34.macro set_sp 35 /* Unsupported CPU, park it before it breaks something */ 36 li t1, CFG_TEE_CORE_NB_CORE 37 csrr t0, CSR_XSCRATCH /* t0: hart_index */ 38 bge t0, t1, unhandled_cpu 39 addi t0, t0, 1 40 lw t1, stack_tmp_stride 41 mul t1, t0, t1 42 la t2, stack_tmp_rel 43 lw t0, 0(t2) 44 add t0, t0, t2 45 add sp, t1, t0 46.endm 47 48.macro cpu_is_ready 49#ifdef CFG_BOOT_SYNC_CPU 50 csrr t0, CSR_XSCRATCH 51 la t1, sem_cpu_sync 52 slli t0, t0, 2 53 add t1, t1, t0 54 li t2, SEM_CPU_READY 55 sw t2, 0(t1) 56 fence 57#endif 58.endm 59 60.macro set_tp 61 csrr t0, CSR_XSCRATCH /* t0: hart_index */ 62 li t1, THREAD_CORE_LOCAL_SIZE 63 mul t2, t1, t0 64 la tp, thread_core_local 65 LDR tp, 0(tp) 66 add tp, tp, t2 67 /* Save hart_id and hart_index into thread_core_local */ 68 sw s0, THREAD_CORE_LOCAL_HART_ID(tp) 69 sw t0, THREAD_CORE_LOCAL_HART_INDEX(tp) 70.endm 71 72.macro wait_primary 73#ifdef CFG_BOOT_SYNC_CPU 74 la t0, sem_cpu_sync 75 li t2, SEM_CPU_READY 761: 77 fence w, w 78 lw t1, 0(t0) 79 bne t1, t2, 1b 80#endif 81.endm 82 83.macro wait_secondary 84#ifdef CFG_BOOT_SYNC_CPU 85 la t0, sem_cpu_sync 86 li t1, CFG_TEE_CORE_NB_CORE 87 li t2, SEM_CPU_READY 881: 89 addi t1, t1, -1 90 beqz t1, 3f 91 addi t0, t0, 4 922: 93 fence 94 lw t1, 0(t0) 95 bne t1, t2, 2b 96 j 1b 973: 98#endif 99.endm 100 101#ifdef CFG_BOOT_SYNC_CPU 102#define flush_cpu_semaphores \ 103 la t0, sem_cpu_sync_start 104 la t1, sem_cpu_sync_end 105 fence 106#else 107#define flush_cpu_semaphores 108#endif 109 110FUNC _start , : 111 /* 112 * Register usage: 113 * a0 - if non-NULL holds the hart ID 114 * a1 - if non-NULL holds the system DTB address 115 * 116 * s0 - saved a0 117 * s1 - saved a1 118 */ 119.option push 120.option norelax 121 la gp, __global_pointer$ 122.option pop 123#ifdef CFG_RISCV_M_MODE 124 csrr a0, CSR_MHARTID 125#endif 126 mv s0, a0 /* Save hart ID into s0 */ 127 128#if defined(CFG_DT_ADDR) 129 li s1, CFG_DT_ADDR 130#else 131 mv s1, a1 /* Save device tree address into s1 */ 132#endif 133 /* Only first hart who wins lottery runs the primary boot sequence. */ 134 la a3, hart_lottery 135 li a2, 1 136 amoadd.w a3, a2, (a3) 137 /* a3 read from hart_lottery also represents the hart_index */ 138 csrw CSR_XSCRATCH, a3 139 140 bnez a3, reset_secondary 141 jal reset_primary 142 j . 143END_FUNC _start 144 145LOCAL_FUNC reset_primary , : , .identity_map 146UNWIND( .cantunwind) 147#ifdef CFG_CORE_ASLR 148 li a0, 0 149 jal relocate 150#endif 151 /* 152 * Zero bss 153 */ 154 lla t0, __bss_start 155 lla t1, __bss_end 156 beq t0, t1, 1f 1570: 158 STR zero, (t0) 159 add t0, t0, RISCV_XLEN_BYTES 160 bne t0, t1, 0b 1611: 162#ifdef CFG_RISCV_S_MODE 163 lla t0, _start 164 lla t1, start_addr 165 STR t0, (t1) 166#endif 167 168 csrw CSR_SATP, zero 169 170 /* Setup sp and tp */ 171#if defined(CFG_DYN_CONFIG) 172 /* 173 * Point sp to a temporary stack at the end of mapped core memory. 174 * Point tp to a temporary struct thread_core_local before the temporary 175 * stack. 176 */ 177 la t0, __vcore_free_end 178 li t1, THREAD_BOOT_INIT_TMP_ALLOC 179 sub t1, t0, t1 180 181 /* Clear the allocated struct thread_core_local */ 182 add t2, t1, THREAD_CORE_LOCAL_SIZE 1831: addi t2, t2, -RISCV_XLEN_BYTES 184 STR zero, (t2) 185 bgt t2, t1, 1b 186 187 li t2, THREAD_ID_INVALID 188 sh t2, THREAD_CORE_LOCAL_CURR_THREAD(t1) 189 li t2, THREAD_CLF_TMP 190 sw t2, THREAD_CORE_LOCAL_FLAGS(t1) 191 li t2, (__STACK_CANARY_SIZE / 2) 192 sub t0, t0, t2 193 STR t0, THREAD_CORE_LOCAL_TMP_STACK_VA_END(t1) 194 li t2, (THREAD_BOOT_INIT_TMP_ALLOC / 2) 195 sub t2, t0, t2 196 STR t2, THREAD_CORE_LOCAL_ABT_STACK_VA_END(t1) 197 csrr t2, CSR_XSCRATCH /* t2: hart_index */ 198 sw s0, THREAD_CORE_LOCAL_HART_ID(t1) 199 sw t2, THREAD_CORE_LOCAL_HART_INDEX(t1) 200 201 mv sp, t0 202 mv tp, t1 203 /* 204 * Record a single core, to be changed later before secure world 205 * boot is done. 206 */ 207 la t2, thread_core_local 208 STR tp, 0(t2) 209 la t2, thread_core_count 210 li t0, 1 211 STR t0, 0(t2) 212#else 213 set_sp 214 set_tp 215 216 /* Initialize thread_core_local[hart_index] for early boot */ 217 jal thread_get_abt_stack 218 mv a1, sp 219 STR a1, THREAD_CORE_LOCAL_TMP_STACK_VA_END(tp) 220 STR a0, THREAD_CORE_LOCAL_ABT_STACK_VA_END(tp) 221 li a0, THREAD_ID_INVALID 222 sh a0, THREAD_CORE_LOCAL_CURR_THREAD(tp) 223 li a0, THREAD_CLF_TMP 224 sw a0, THREAD_CORE_LOCAL_FLAGS(tp) 225#endif 226 227 jal plat_primary_init_early 228 jal console_init 229 230 la a0, __vcore_free_start 231 la a1, __vcore_free_end 232#ifdef CFG_DYN_CONFIG 233 li a2, THREAD_BOOT_INIT_TMP_ALLOC 234 sub a1, a1, a2 235#endif 236 la a2, __vcore_free_end 237 jal boot_mem_init 238 239#ifdef CFG_CORE_ASLR 240#ifdef CFG_CORE_ASLR_SEED 241 li a0, CFG_CORE_ASLR_SEED 242#else 243 jal get_aslr_seed 244#endif 245#else 246 mv a0, x0 247#endif 248 la a1, boot_mmu_config 249 jal core_init_mmu_map 250 251#ifdef CFG_CORE_ASLR 252 la a0, boot_mmu_config 253 LDR a0, CORE_MMU_CONFIG_MAP_OFFSET(a0) 254 beqz a0, 1f /* no offset, skip dynamic relocation */ 255 jal relocate 2561: 257#endif 258 259 jal enable_mmu 260 261#ifdef CFG_CORE_ASLR 262#if defined(CFG_DYN_CONFIG) 263 /* 264 * thread_core_local holds only one core and thread_core_count is 1 265 * so tp points to the updated pointer for thread_core_local. 266 */ 267 la t0, thread_core_local 268 STR tp, 0(t0) 269#endif 270 271 /* 272 * Update recorded end_va. This must be done before calling into C 273 * code to make sure that the stack pointer matches what we have in 274 * thread_core_local[]. 275 */ 276 la a0, boot_mmu_config 277 LDR a0, CORE_MMU_CONFIG_MAP_OFFSET(a0) 278 LDR a1, THREAD_CORE_LOCAL_TMP_STACK_VA_END(tp) 279 add a1, a1, a0 280 STR a1, THREAD_CORE_LOCAL_TMP_STACK_VA_END(tp) 281 LDR a1, THREAD_CORE_LOCAL_ABT_STACK_VA_END(tp) 282 add a1, a1, a0 283 STR a1, THREAD_CORE_LOCAL_ABT_STACK_VA_END(tp) 284 285 /* Update relocations recorded with boot_mem_add_reloc() */ 286 jal boot_mem_relocate 287 /* 288 * Reinitialize console, since register_serial_console() has 289 * previously registered a PA and with ASLR the VA is different 290 * from the PA. 291 */ 292 jal console_init 293#endif 294 295 jal boot_init_primary_early 296 297 mv a0, s1 /* s1 contains saved device tree address */ 298 mv a1, x0 /* unused */ 299 jal boot_init_primary_late 300 301#if defined(CFG_DYN_CONFIG) 302 /* Get hart index */ 303 jal __get_core_pos 304 305 /* 306 * Switch to the new thread_core_local and thread_core_count and 307 * keep the pointer to the new thread_core_local in a1. 308 */ 309 LDR a1, __thread_core_count_new 310 la a2, thread_core_count 311 STR a1, 0(a2) 312 LDR a1, __thread_core_local_new 313 la a2, thread_core_local 314 STR a1, 0(a2) 315 316 /* 317 * Update tp to point the new thread_core_local. 318 * Update sp to use the new tmp stack. 319 */ 320 li a2, THREAD_CORE_LOCAL_SIZE 321 /* tp = a2 * a0(hart index) + a1(thread_core_local) */ 322 mul a2, a2, a0 323 add tp, a2, a1 324 LDR sp, THREAD_CORE_LOCAL_TMP_STACK_VA_END(tp) 325#endif 326 327 /* 328 * Before entering boot_init_primary_runtime(), we do these two steps: 329 * 1. Save current sp to s2, and set sp as threads[0].stack_va_end 330 * 2. Clear the flag which indicates usage of the temporary stack in the 331 * current hart's thread_core_local structure. 332 */ 333 mv s2, sp 334 la a0, threads 335 LDR a0, 0(a0) 336 LDR a0, THREAD_CTX_STACK_VA_END(a0) 337 mv sp, a0 338 jal thread_get_core_local 339 mv s3, a0 340 sw zero, THREAD_CORE_LOCAL_FLAGS(s3) 341 342 jal boot_init_primary_runtime 343 jal boot_init_primary_final 344 345 /* 346 * After returning from boot_init_primary_late(), the flag and sp are 347 * restored. 348 */ 349 li a0, THREAD_CLF_TMP 350 sw a0, THREAD_CORE_LOCAL_FLAGS(s3) 351 mv sp, s2 352 353#ifdef _CFG_CORE_STACK_PROTECTOR 354 /* Update stack canary value */ 355 addi sp, sp, -STACK_ALIGNMENT 356 mv a0, sp 357 li a1, 1 358#ifdef RV32 359 li a2, 4 360#else 361 li a2, 8 362#endif 363 jal plat_get_random_stack_canaries 364 LDR s0, 0(sp) 365 la s1, __stack_chk_guard 366 STR s0, 0(s1) 367 addi sp, sp, STACK_ALIGNMENT 368#endif 369 370 cpu_is_ready 371 flush_cpu_semaphores 372 wait_secondary 373 374 jal thread_clr_boot_thread 375 376 li a0, TEEABI_OPTEED_RETURN_ENTRY_DONE 377 la a1, thread_vector_table 378 li a2, 0 379 li a3, 0 380 li a4, 0 381 li a5, 0 382 j thread_return_to_udomain 383END_FUNC reset_primary 384 385LOCAL_FUNC reset_secondary , : , .identity_map 386UNWIND( .cantunwind) 387 wait_primary 388 csrw CSR_SATP, zero 389 jal enable_mmu 390#if defined(CFG_DYN_CONFIG) 391 /* 392 * Update tp to point the new thread_core_local. 393 * Update sp to use the new tmp stack. 394 */ 395 csrr t0, CSR_XSCRATCH /* t0: hart_index */ 396 LDR t1, thread_core_local 397 li t2, THREAD_CORE_LOCAL_SIZE 398 /* tp = t2 * t0(hart index) + t1(thread_core_local) */ 399 mul t2, t2, t0 400 add tp, t2, t1 401 sw s0, THREAD_CORE_LOCAL_HART_ID(tp) 402 sw t0, THREAD_CORE_LOCAL_HART_INDEX(tp) 403 LDR sp, THREAD_CORE_LOCAL_TMP_STACK_VA_END(tp) 404#else 405 set_sp 406 set_tp 407#endif 408 cpu_is_ready 409 410 jal boot_init_secondary 411#ifdef CFG_RISCV_WITH_M_MODE_SM 412 /* Return to untrusted domain */ 413 li a0, TEEABI_OPTEED_RETURN_ON_DONE 414 li a1, 0 415 li a2, 0 416 li a3, 0 417 li a4, 0 418 li a5, 0 419 j thread_return_to_udomain 420#endif 421 j . 422END_FUNC reset_secondary 423 424LOCAL_FUNC unhandled_cpu , : 425 wfi 426 j unhandled_cpu 427END_FUNC unhandled_cpu 428 429#if defined(CFG_CORE_ASLR) 430/* 431 * void relocate(unsigned long offset); 432 * 433 * This function updates dynamic relocations. 434 */ 435LOCAL_FUNC relocate , : 436 /* 437 * a0 holds relocate offset 438 */ 439 la t0, __rel_dyn_start 440 la t1, __rel_dyn_end 441 beq t0, t1, 5f 4422: 443 LDR t5, RISCV_XLEN_BYTES(t0) /* t5: relocation info:type */ 444 li t3, R_RISCV_RELATIVE 445 bne t5, t3, 3f 446 LDR t3, 0(t0) /* t3: offset */ 447 LDR t5, (RISCV_XLEN_BYTES * 2)(t0) /* t5: addend */ 448 add t5, t5, a0 /* t5: add ASLR offset */ 449 STR t5, 0(t3) /* update address */ 450 j 4f 451 4523: 453 la t4, __dyn_sym_start 454 srli t6, t5, SYM_INDEX /* t6: sym table index */ 455 andi t5, t5, 0xFF /* t5: relocation type */ 456 li t3, RELOC_TYPE 457 bne t5, t3, 4f 458 459 /* address R_RISCV_64 or R_RISCV_32 cases */ 460 LDR t3, 0(t0) 461 li t5, SYM_SIZE 462 mul t6, t6, t5 463 add t5, t4, t6 464 LDR t6, (RISCV_XLEN_BYTES * 2)(t0) /* t6: addend */ 465 LDR t5, RISCV_XLEN_BYTES(t5) /* t5: sym value */ 466 add t5, t5, t6 467 add t5, t5, a0 /* t5: add ASLR offset */ 468 STR t5, 0(t3) /* update address */ 469 4704: 471 addi t0, t0, (RISCV_XLEN_BYTES * 3) 472 blt t0, t1, 2b 4735: 474 ret 475END_FUNC relocate 476#endif 477 478/* 479 * void enable_mmu(void); 480 * 481 * Initializes and enables the Memory Management Unit (MMU). 482 * This function is designed to be called while executing in 483 * an identity-mapped region, where physical and virtual 484 * addresses are identical. When CFG_CORE_ASLR=y: 485 * - Execution is switched to the new virtual address region, 486 * based on the randomized offset. 487 * - CPU registers (global pointer, thread pointer, stack 488 * pointer, return address) are updated so execution 489 * continue correctly in a new address space. 490 */ 491LOCAL_FUNC enable_mmu , : , .identity_map 492 /* Set SATP from boot_mmu_config.satp[hartidx] */ 493 csrr a0, CSR_XSCRATCH 494 la a1, boot_mmu_config 495 LDR a3, CORE_MMU_CONFIG_MAP_OFFSET(a1) 496 addi a1, a1, CORE_MMU_CONFIG_SATP 497 li a2, CORE_MMU_CONFIG_SATP_SIZE 498 mul a0, a0, a2 499 add a1, a1, a0 500 LDR a2, 0(a1) 501 csrw CSR_SATP, a2 502 sfence.vma zero, zero 503#ifdef CFG_CORE_ASLR 504 /* Update CPU registers with the ASLR offset */ 505 add gp, gp, a3 506 add tp, tp, a3 507 add sp, sp, a3 508 add ra, ra, a3 509#endif 510 ret 511END_FUNC enable_mmu 512 513 .section .identity_map.data 514 .balign 8 515LOCAL_DATA hart_lottery , : 516 /* The hart who first increments this variable will be primary hart. */ 517 .word 0 518END_DATA hart_lottery 519 520#ifdef CFG_BOOT_SYNC_CPU 521LOCAL_DATA sem_cpu_sync_start , : 522 .word sem_cpu_sync 523END_DATA sem_cpu_sync_start 524 525LOCAL_DATA sem_cpu_sync_end , : 526 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 527END_DATA sem_cpu_sync_end 528#endif 529 530#if !defined(CFG_DYN_CONFIG) 531LOCAL_DATA stack_tmp_rel , : 532 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 533END_DATA stack_tmp_rel 534#endif 535 536 .section .identity_map.data 537 .balign 8 538DATA boot_mmu_config , : /* struct core_mmu_config */ 539 .skip CORE_MMU_CONFIG_SIZE 540END_DATA boot_mmu_config 541