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