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 STR x0, 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 STR 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 j thread_return_to_udomain 246END_FUNC reset_primary 247 248LOCAL_FUNC reset_secondary , : , .identity_map 249UNWIND( .cantunwind) 250 wait_primary 251 csrw CSR_SATP, zero 252 set_sp 253 set_tp 254 set_satp 255 cpu_is_ready 256 257 jal boot_init_secondary 258#ifdef CFG_RISCV_WITH_M_MODE_SM 259 /* Return to untrusted domain */ 260 li a0, TEEABI_OPTEED_RETURN_ON_DONE 261 j thread_return_to_udomain 262#endif 263 j . 264END_FUNC reset_secondary 265 266LOCAL_FUNC unhandled_cpu , : 267 wfi 268 j unhandled_cpu 269END_FUNC unhandled_cpu 270 271 .section .identity_map.data 272 .balign 8 273LOCAL_DATA hart_lottery , : 274 /* The hart who first increments this variable will be primary hart. */ 275 .word 0 276END_DATA hart_lottery 277 278#ifdef CFG_BOOT_SYNC_CPU 279LOCAL_DATA sem_cpu_sync_start , : 280 .word sem_cpu_sync 281END_DATA sem_cpu_sync_start 282 283LOCAL_DATA sem_cpu_sync_end , : 284 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 285END_DATA sem_cpu_sync_end 286#endif 287 288LOCAL_DATA stack_tmp_rel , : 289 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 290END_DATA stack_tmp_rel 291 292LOCAL_DATA stack_tmp_stride_rel , : 293 .word stack_tmp_stride - stack_tmp_stride_rel 294END_DATA stack_tmp_stride_rel 295 296 .balign 8 297LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 298 .skip CORE_MMU_CONFIG_SIZE 299END_DATA boot_mmu_config 300