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