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 mv a0, x0 204 la a1, boot_mmu_config 205 jal core_init_mmu_map 206 207 set_satp 208 209 jal boot_init_primary_early 210 211 /* 212 * Before entering boot_init_primary_late(), we do these two steps: 213 * 1. Save current sp to s2, and set sp as threads[0].stack_va_end 214 * 2. Clear the flag which indicates usage of the temporary stack in the 215 * current hart's thread_core_local structure. 216 */ 217 mv s2, sp 218 la a0, threads 219 LDR a0, THREAD_CTX_STACK_VA_END(a0) 220 mv sp, a0 221 jal thread_get_core_local 222 mv s3, a0 223 sw zero, THREAD_CORE_LOCAL_FLAGS(s3) 224 225 mv a0, s1 /* s1 contains saved device tree address */ 226 mv a1, x0 /* unused */ 227 jal boot_init_primary_late 228 229 /* 230 * After returning from boot_init_primary_late(), the flag and sp are 231 * restored. 232 */ 233 li a0, THREAD_CLF_TMP 234 sw a0, THREAD_CORE_LOCAL_FLAGS(s3) 235 mv sp, s2 236 237 cpu_is_ready 238 flush_cpu_semaphores 239 wait_secondary 240 241 jal thread_clr_boot_thread 242 243 li a0, TEEABI_OPTEED_RETURN_ENTRY_DONE 244 la a1, thread_vector_table 245 li a2, 0 246 li a3, 0 247 li a4, 0 248 li a5, 0 249 j thread_return_to_udomain 250END_FUNC reset_primary 251 252LOCAL_FUNC reset_secondary , : , .identity_map 253UNWIND( .cantunwind) 254 wait_primary 255 csrw CSR_SATP, zero 256 set_sp 257 set_tp 258 set_satp 259 cpu_is_ready 260 261 jal boot_init_secondary 262#ifdef CFG_RISCV_WITH_M_MODE_SM 263 /* Return to untrusted domain */ 264 li a0, TEEABI_OPTEED_RETURN_ON_DONE 265 li a1, 0 266 li a2, 0 267 li a3, 0 268 li a4, 0 269 li a5, 0 270 j thread_return_to_udomain 271#endif 272 j . 273END_FUNC reset_secondary 274 275LOCAL_FUNC unhandled_cpu , : 276 wfi 277 j unhandled_cpu 278END_FUNC unhandled_cpu 279 280 .section .identity_map.data 281 .balign 8 282LOCAL_DATA hart_lottery , : 283 /* The hart who first increments this variable will be primary hart. */ 284 .word 0 285END_DATA hart_lottery 286 287#ifdef CFG_BOOT_SYNC_CPU 288LOCAL_DATA sem_cpu_sync_start , : 289 .word sem_cpu_sync 290END_DATA sem_cpu_sync_start 291 292LOCAL_DATA sem_cpu_sync_end , : 293 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 294END_DATA sem_cpu_sync_end 295#endif 296 297LOCAL_DATA stack_tmp_rel , : 298 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 299END_DATA stack_tmp_rel 300 301LOCAL_DATA stack_tmp_stride_rel , : 302 .word stack_tmp_stride - stack_tmp_stride_rel 303END_DATA stack_tmp_stride_rel 304 305 .balign 8 306LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 307 .skip CORE_MMU_CONFIG_SIZE 308END_DATA boot_mmu_config 309