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 la a1, boot_mmu_config 75 LDR a0, CORE_MMU_CONFIG_SATP(a1) 76 csrw CSR_SATP, a0 77 sfence.vma zero, zero 78.endm 79 80.macro wait_primary 81#ifdef CFG_BOOT_SYNC_CPU 82 la t0, sem_cpu_sync 83 li t2, SEM_CPU_READY 841: 85 fence w, w 86 lw t1, 0(t0) 87 bne t1, t2, 1b 88#endif 89.endm 90 91.macro wait_secondary 92#ifdef CFG_BOOT_SYNC_CPU 93 la t0, sem_cpu_sync 94 li t1, CFG_TEE_CORE_NB_CORE 95 li t2, SEM_CPU_READY 961: 97 addi t1, t1, -1 98 beqz t1, 3f 99 addi t0, t0, 4 1002: 101 fence 102 lw t1, 0(t0) 103 bne t1, t2, 2b 104 j 1b 1053: 106#endif 107.endm 108 109#ifdef CFG_BOOT_SYNC_CPU 110#define flush_cpu_semaphores \ 111 la t0, sem_cpu_sync_start 112 la t1, sem_cpu_sync_end 113 fence 114#else 115#define flush_cpu_semaphores 116#endif 117 118.macro bootargs_entry 119 /* 120 * Save boot arguments 121 */ 122 la t0, boot_args 123 /* Save boot hart */ 124 STR a0, REGOFF(0)(t0) 125 /* Save FDT address */ 126 STR a1, REGOFF(1)(t0) 127.endm 128 129FUNC _start , : 130 /* 131 * Register usage: 132 * a0 - if non-NULL holds the hart ID 133 * a1 - if non-NULL holds the system DTB address 134 * 135 * CSR_XSCRATCH - saved a0 136 * s1 - saved a1 137 */ 138.option push 139.option norelax 140 la gp, __global_pointer$ 141.option pop 142#ifdef CFG_RISCV_M_MODE 143 csrr a0, CSR_MHARTID 144#endif 145 csrw CSR_XSCRATCH, a0 146#if defined(CFG_DT_ADDR) 147 li s1, CFG_DT_ADDR 148#else 149 mv s1, a1 /* Save device tree address into s1 */ 150#endif 151 152 /* Only first hart who wins lottery runs the primary boot sequence. */ 153 la a3, hart_lottery 154 li a2, 1 155 amoadd.w a3, a2, (a3) 156 bnez a3, reset_secondary 157 jal reset_primary 158 j . 159END_FUNC _start 160 161LOCAL_FUNC reset_primary , : , .identity_map 162UNWIND( .cantunwind) 163 164 bootargs_entry 165 166 /* 167 * Zero bss 168 */ 169 lla t0, __bss_start 170 lla t1, __bss_end 171 beq t0, t1, 1f 1720: 173 STR zero, (t0) 174 add t0, t0, RISCV_XLEN_BYTES 175 bne t0, t1, 0b 1761: 177#ifdef CFG_RISCV_S_MODE 178 lla t0, _start 179 lla t1, start_addr 180 STR t0, (t1) 181#endif 182 183 csrw CSR_SATP, zero 184 set_sp 185 set_tp 186 187 jal thread_init_thread_core_local 188 jal plat_primary_init_early 189 jal console_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, THREAD_CTX_STACK_VA_END(a0) 208 mv sp, a0 209 jal thread_get_core_local 210 mv s3, a0 211 STR x0, THREAD_CORE_LOCAL_FLAGS(s3) 212 213 mv a0, s1 /* s1 contains saved device tree address */ 214 mv a1, x0 /* unused */ 215 jal boot_init_primary_late 216 217 /* 218 * After returning from boot_init_primary_late(), the flag and sp are 219 * restored. 220 */ 221 li a0, THREAD_CLF_TMP 222 STR a0, THREAD_CORE_LOCAL_FLAGS(s3) 223 mv sp, s2 224 225 cpu_is_ready 226 flush_cpu_semaphores 227 wait_secondary 228 229 jal thread_clr_boot_thread 230 231 li a0, TEEABI_OPTEED_RETURN_ENTRY_DONE 232 la a1, thread_vector_table 233 j thread_return_to_udomain 234END_FUNC reset_primary 235 236LOCAL_FUNC reset_secondary , : , .identity_map 237UNWIND( .cantunwind) 238 wait_primary 239 csrw CSR_SATP, zero 240 set_sp 241 set_tp 242 set_satp 243 cpu_is_ready 244 245 jal boot_init_secondary 246#ifdef CFG_RISCV_WITH_M_MODE_SM 247 /* Return to untrusted domain */ 248 li a0, TEEABI_OPTEED_RETURN_ON_DONE 249 j thread_return_to_udomain 250#endif 251 j . 252END_FUNC reset_secondary 253 254LOCAL_FUNC unhandled_cpu , : 255 wfi 256 j unhandled_cpu 257END_FUNC unhandled_cpu 258 259 .section .identity_map.data 260 .balign 8 261LOCAL_DATA hart_lottery , : 262 /* The hart who first increments this variable will be primary hart. */ 263 .word 0 264END_DATA hart_lottery 265 266#ifdef CFG_BOOT_SYNC_CPU 267LOCAL_DATA sem_cpu_sync_start , : 268 .word sem_cpu_sync 269END_DATA sem_cpu_sync_start 270 271LOCAL_DATA sem_cpu_sync_end , : 272 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 273END_DATA sem_cpu_sync_end 274#endif 275 276LOCAL_DATA stack_tmp_rel , : 277 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 278END_DATA stack_tmp_rel 279 280LOCAL_DATA stack_tmp_stride_rel , : 281 .word stack_tmp_stride - stack_tmp_stride_rel 282END_DATA stack_tmp_stride_rel 283 284 .balign 8 285LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 286 .skip CORE_MMU_CONFIG_SIZE 287END_DATA boot_mmu_config 288