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