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 130FUNC _start , : 131 /* 132 * Register usage: 133 * a0 - if non-NULL holds the hart ID 134 * a1 - if non-NULL holds the system DTB address 135 * 136 * CSR_XSCRATCH - saved a0 137 * s1 - saved a1 138 */ 139.option push 140.option norelax 141 la gp, __global_pointer$ 142.option pop 143#ifdef CFG_RISCV_M_MODE 144 csrr a0, CSR_MHARTID 145#endif 146 csrw CSR_XSCRATCH, a0 147#if defined(CFG_DT_ADDR) 148 li s1, CFG_DT_ADDR 149#else 150 mv s1, a1 /* Save device tree address into s1 */ 151#endif 152 153 /* Only first hart who wins lottery runs the primary boot sequence. */ 154 la a3, hart_lottery 155 li a2, 1 156 amoadd.w a3, a2, (a3) 157 bnez a3, reset_secondary 158 jal reset_primary 159 j . 160END_FUNC _start 161 162LOCAL_FUNC reset_primary , : , .identity_map 163UNWIND( .cantunwind) 164 /* 165 * Zero bss 166 */ 167 lla t0, __bss_start 168 lla t1, __bss_end 169 beq t0, t1, 1f 1700: 171 STR zero, (t0) 172 add t0, t0, RISCV_XLEN_BYTES 173 bne t0, t1, 0b 1741: 175#ifdef CFG_RISCV_S_MODE 176 lla t0, _start 177 lla t1, start_addr 178 STR t0, (t1) 179#endif 180 181 csrw CSR_SATP, zero 182 set_sp 183 set_tp 184 185 jal thread_init_thread_core_local 186 jal plat_primary_init_early 187 jal console_init 188 189 la a0, __vcore_free_start 190 la a1, __vcore_free_end 191 la a2, __vcore_free_end 192 jal boot_mem_init 193 194 mv a0, x0 195 la a1, boot_mmu_config 196 jal core_init_mmu_map 197 198 set_satp 199 200 jal boot_init_primary_early 201 202 /* 203 * Before entering boot_init_primary_late(), we do these two steps: 204 * 1. Save current sp to s2, and set sp as threads[0].stack_va_end 205 * 2. Clear the flag which indicates usage of the temporary stack in the 206 * current hart's thread_core_local structure. 207 */ 208 mv s2, sp 209 la a0, threads 210 LDR a0, THREAD_CTX_STACK_VA_END(a0) 211 mv sp, a0 212 jal thread_get_core_local 213 mv s3, a0 214 sw zero, THREAD_CORE_LOCAL_FLAGS(s3) 215 216 mv a0, s1 /* s1 contains saved device tree address */ 217 mv a1, x0 /* unused */ 218 jal boot_init_primary_late 219 jal boot_init_primary_final 220 221 /* 222 * After returning from boot_init_primary_late(), the flag and sp are 223 * restored. 224 */ 225 li a0, THREAD_CLF_TMP 226 sw a0, THREAD_CORE_LOCAL_FLAGS(s3) 227 mv sp, s2 228 229 cpu_is_ready 230 flush_cpu_semaphores 231 wait_secondary 232 233 jal thread_clr_boot_thread 234 235 li a0, TEEABI_OPTEED_RETURN_ENTRY_DONE 236 la a1, thread_vector_table 237 li a2, 0 238 li a3, 0 239 li a4, 0 240 li a5, 0 241 j thread_return_to_udomain 242END_FUNC reset_primary 243 244LOCAL_FUNC reset_secondary , : , .identity_map 245UNWIND( .cantunwind) 246 wait_primary 247 csrw CSR_SATP, zero 248 set_sp 249 set_tp 250 set_satp 251 cpu_is_ready 252 253 jal boot_init_secondary 254#ifdef CFG_RISCV_WITH_M_MODE_SM 255 /* Return to untrusted domain */ 256 li a0, TEEABI_OPTEED_RETURN_ON_DONE 257 li a1, 0 258 li a2, 0 259 li a3, 0 260 li a4, 0 261 li a5, 0 262 j thread_return_to_udomain 263#endif 264 j . 265END_FUNC reset_secondary 266 267LOCAL_FUNC unhandled_cpu , : 268 wfi 269 j unhandled_cpu 270END_FUNC unhandled_cpu 271 272 .section .identity_map.data 273 .balign 8 274LOCAL_DATA hart_lottery , : 275 /* The hart who first increments this variable will be primary hart. */ 276 .word 0 277END_DATA hart_lottery 278 279#ifdef CFG_BOOT_SYNC_CPU 280LOCAL_DATA sem_cpu_sync_start , : 281 .word sem_cpu_sync 282END_DATA sem_cpu_sync_start 283 284LOCAL_DATA sem_cpu_sync_end , : 285 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 286END_DATA sem_cpu_sync_end 287#endif 288 289LOCAL_DATA stack_tmp_rel , : 290 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 291END_DATA stack_tmp_rel 292 293LOCAL_DATA stack_tmp_stride_rel , : 294 .word stack_tmp_stride - stack_tmp_stride_rel 295END_DATA stack_tmp_stride_rel 296 297 .balign 8 298LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 299 .skip CORE_MMU_CONFIG_SIZE 300END_DATA boot_mmu_config 301