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/thread_private.h> 12#include <mm/core_mmu.h> 13#include <platform_config.h> 14#include <riscv.h> 15#include <riscv_macros.S> 16#include <tee/optee_abi.h> 17#include <tee/teeabi_opteed.h> 18#include <tee/teeabi_opteed_macros.h> 19 20.section .data 21.balign 4 22 23#ifdef CFG_BOOT_SYNC_CPU 24.equ SEM_CPU_READY, 1 25#endif 26 27 /* 28 * Setup sp to point to the top of the tmp stack for the current CPU: 29 * sp is assigned: 30 * stack_tmp + (hart_index + 1) * stack_tmp_stride - STACK_TMP_GUARD 31 */ 32.macro set_sp 33 /* Unsupported CPU, park it before it breaks something */ 34 li t1, CFG_TEE_CORE_NB_CORE 35 csrr t0, CSR_XSCRATCH /* t0: hart_index */ 36 bge t0, t1, unhandled_cpu 37 addi t0, t0, 1 38 lw t1, stack_tmp_stride 39 mul t1, t0, t1 40 la t2, stack_tmp_rel 41 lw t0, 0(t2) 42 add t0, t0, t2 43 add sp, t1, t0 44.endm 45 46.macro cpu_is_ready 47#ifdef CFG_BOOT_SYNC_CPU 48 csrr t0, CSR_XSCRATCH 49 la t1, sem_cpu_sync 50 slli t0, t0, 2 51 add t1, t1, t0 52 li t2, SEM_CPU_READY 53 sw t2, 0(t1) 54 fence 55#endif 56.endm 57 58.macro set_tp 59 csrr a3, CSR_XSCRATCH /* a3: hart_index */ 60 li a1, THREAD_CORE_LOCAL_SIZE 61 la tp, thread_core_local 62 LDR tp, 0(tp) 63 mul a2, a1, a3 64 add tp, tp, a2 65 sw a0, THREAD_CORE_LOCAL_HART_ID(tp) 66 sw a3, THREAD_CORE_LOCAL_HART_INDEX(tp) 67.endm 68 69.macro set_satp 70 /* 71 * a0 = hart_index 72 * a1 = address of boot_mmu_config.satp[0] 73 * a2 = size of CSR SATP 74 * 75 * This hart's SATP is of value (a1 + (a0 * a2)). 76 */ 77 csrr a0, CSR_XSCRATCH 78 la a1, boot_mmu_config 79 addi a1, a1, CORE_MMU_CONFIG_SATP 80 li a2, CORE_MMU_CONFIG_SATP_SIZE 81 mul a0, a0, a2 82 add a1, a1, a0 83 LDR a2, 0(a1) 84 csrw CSR_SATP, a2 85 sfence.vma zero, zero 86.endm 87 88.macro wait_primary 89#ifdef CFG_BOOT_SYNC_CPU 90 la t0, sem_cpu_sync 91 li t2, SEM_CPU_READY 921: 93 fence w, w 94 lw t1, 0(t0) 95 bne t1, t2, 1b 96#endif 97.endm 98 99.macro wait_secondary 100#ifdef CFG_BOOT_SYNC_CPU 101 la t0, sem_cpu_sync 102 li t1, CFG_TEE_CORE_NB_CORE 103 li t2, SEM_CPU_READY 1041: 105 addi t1, t1, -1 106 beqz t1, 3f 107 addi t0, t0, 4 1082: 109 fence 110 lw t1, 0(t0) 111 bne t1, t2, 2b 112 j 1b 1133: 114#endif 115.endm 116 117#ifdef CFG_BOOT_SYNC_CPU 118#define flush_cpu_semaphores \ 119 la t0, sem_cpu_sync_start 120 la t1, sem_cpu_sync_end 121 fence 122#else 123#define flush_cpu_semaphores 124#endif 125 126FUNC _start , : 127 /* 128 * Register usage: 129 * a0 - if non-NULL holds the hart ID 130 * a1 - if non-NULL holds the system DTB address 131 * 132 * s1 - saved a1 133 */ 134.option push 135.option norelax 136 la gp, __global_pointer$ 137.option pop 138#ifdef CFG_RISCV_M_MODE 139 csrr a0, CSR_MHARTID 140#endif 141 142#if defined(CFG_DT_ADDR) 143 li s1, CFG_DT_ADDR 144#else 145 mv s1, a1 /* Save device tree address into s1 */ 146#endif 147 /* Only first hart who wins lottery runs the primary boot sequence. */ 148 la a3, hart_lottery 149 li a2, 1 150 amoadd.w a3, a2, (a3) 151 /* a3 read from hart_lottery also represents the hart_index */ 152 csrw CSR_XSCRATCH, a3 153 154 bnez a3, reset_secondary 155 jal reset_primary 156 j . 157END_FUNC _start 158 159LOCAL_FUNC reset_primary , : , .identity_map 160UNWIND( .cantunwind) 161 /* 162 * Zero bss 163 */ 164 lla t0, __bss_start 165 lla t1, __bss_end 166 beq t0, t1, 1f 1670: 168 STR zero, (t0) 169 add t0, t0, RISCV_XLEN_BYTES 170 bne t0, t1, 0b 1711: 172#ifdef CFG_RISCV_S_MODE 173 lla t0, _start 174 lla t1, start_addr 175 STR t0, (t1) 176#endif 177 178 csrw CSR_SATP, zero 179 set_sp 180 set_tp 181 182 /* Initialize thread_core_local[hart_index] for early boot */ 183 jal thread_get_abt_stack 184 mv a1, sp 185 STR a1, THREAD_CORE_LOCAL_TMP_STACK_VA_END(tp) 186 STR a0, THREAD_CORE_LOCAL_ABT_STACK_VA_END(tp) 187 li a0, THREAD_ID_INVALID 188 sh a0, THREAD_CORE_LOCAL_CURR_THREAD(tp) 189 li a0, THREAD_CLF_TMP 190 sw a0, THREAD_CORE_LOCAL_FLAGS(tp) 191 192 jal plat_primary_init_early 193 jal console_init 194 195 la a0, __vcore_free_start 196 la a1, __vcore_free_end 197 la a2, __vcore_free_end 198 jal boot_mem_init 199 200 mv a0, x0 201 la a1, boot_mmu_config 202 jal core_init_mmu_map 203 204 set_satp 205 206 jal boot_init_primary_early 207 208 mv a0, s1 /* s1 contains saved device tree address */ 209 mv a1, x0 /* unused */ 210 jal boot_init_primary_late 211 212 /* 213 * Before entering boot_init_primary_runtime(), we do these two steps: 214 * 1. Save current sp to s2, and set sp as threads[0].stack_va_end 215 * 2. Clear the flag which indicates usage of the temporary stack in the 216 * current hart's thread_core_local structure. 217 */ 218 mv s2, sp 219 la a0, threads 220 LDR a0, 0(a0) 221 LDR a0, THREAD_CTX_STACK_VA_END(a0) 222 mv sp, a0 223 jal thread_get_core_local 224 mv s3, a0 225 sw zero, THREAD_CORE_LOCAL_FLAGS(s3) 226 227 jal boot_init_primary_runtime 228 jal boot_init_primary_final 229 230 /* 231 * After returning from boot_init_primary_late(), the flag and sp are 232 * restored. 233 */ 234 li a0, THREAD_CLF_TMP 235 sw a0, THREAD_CORE_LOCAL_FLAGS(s3) 236 mv sp, s2 237 238#ifdef _CFG_CORE_STACK_PROTECTOR 239 /* Update stack canary value */ 240 addi sp, sp, -STACK_ALIGNMENT 241 mv a0, sp 242 li a1, 1 243#ifdef RV32 244 li a2, 4 245#else 246 li a2, 8 247#endif 248 jal plat_get_random_stack_canaries 249 LDR s0, 0(sp) 250 la s1, __stack_chk_guard 251 STR s0, 0(s1) 252 addi sp, sp, STACK_ALIGNMENT 253#endif 254 255 cpu_is_ready 256 flush_cpu_semaphores 257 wait_secondary 258 259 jal thread_clr_boot_thread 260 261 li a0, TEEABI_OPTEED_RETURN_ENTRY_DONE 262 la a1, thread_vector_table 263 li a2, 0 264 li a3, 0 265 li a4, 0 266 li a5, 0 267 j thread_return_to_udomain 268END_FUNC reset_primary 269 270LOCAL_FUNC reset_secondary , : , .identity_map 271UNWIND( .cantunwind) 272 wait_primary 273 csrw CSR_SATP, zero 274 set_sp 275 set_tp 276 set_satp 277 cpu_is_ready 278 279 jal boot_init_secondary 280#ifdef CFG_RISCV_WITH_M_MODE_SM 281 /* Return to untrusted domain */ 282 li a0, TEEABI_OPTEED_RETURN_ON_DONE 283 li a1, 0 284 li a2, 0 285 li a3, 0 286 li a4, 0 287 li a5, 0 288 j thread_return_to_udomain 289#endif 290 j . 291END_FUNC reset_secondary 292 293LOCAL_FUNC unhandled_cpu , : 294 wfi 295 j unhandled_cpu 296END_FUNC unhandled_cpu 297 298 .section .identity_map.data 299 .balign 8 300LOCAL_DATA hart_lottery , : 301 /* The hart who first increments this variable will be primary hart. */ 302 .word 0 303END_DATA hart_lottery 304 305#ifdef CFG_BOOT_SYNC_CPU 306LOCAL_DATA sem_cpu_sync_start , : 307 .word sem_cpu_sync 308END_DATA sem_cpu_sync_start 309 310LOCAL_DATA sem_cpu_sync_end , : 311 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 312END_DATA sem_cpu_sync_end 313#endif 314 315LOCAL_DATA stack_tmp_rel , : 316 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 317END_DATA stack_tmp_rel 318 319LOCAL_DATA stack_tmp_stride_rel , : 320 .word stack_tmp_stride - stack_tmp_stride_rel 321END_DATA stack_tmp_stride_rel 322 323 .balign 8 324LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 325 .skip CORE_MMU_CONFIG_SIZE 326END_DATA boot_mmu_config 327