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 bnez a0, reset_secondary 152 jal reset_primary 153 j . 154END_FUNC _start 155 156LOCAL_FUNC reset_primary , : , .identity_map 157UNWIND( .cantunwind) 158 159 bootargs_entry 160 161 /* 162 * Zero bss 163 */ 164 lla t0, __bss_start 165 lla t1, __bss_end 166 beq t0, t1, 1f 1670: 168 STR zero, (t0) 169 add t0, t0, RISCV_XLEN_BYTES 170 bne t0, t1, 0b 1711: 172#ifdef CFG_RISCV_S_MODE 173 lla t0, _start 174 lla t1, start_addr 175 STR t0, (t1) 176#endif 177 178 csrw CSR_SATP, zero 179 set_sp 180 set_tp 181 182 jal thread_init_thread_core_local 183 jal plat_primary_init_early 184 jal console_init 185 186 mv a0, x0 187 la a1, boot_mmu_config 188 jal core_init_mmu_map 189 190 set_satp 191 192 jal boot_init_primary_early 193 194 /* 195 * Before entering boot_init_primary_late(), we do these two steps: 196 * 1. Save current sp to s2, and set sp as threads[0].stack_va_end 197 * 2. Clear the flag which indicates usage of the temporary stack in the 198 * current hart's thread_core_local structure. 199 */ 200 mv s2, sp 201 la a0, threads 202 LDR a0, THREAD_CTX_STACK_VA_END(a0) 203 mv sp, a0 204 jal thread_get_core_local 205 mv s3, a0 206 STR x0, THREAD_CORE_LOCAL_FLAGS(s3) 207 208 mv a0, s1 /* s1 contains saved device tree address */ 209 mv a1, x0 /* unused */ 210 jal boot_init_primary_late 211 212 /* 213 * After returning from boot_init_primary_late(), the flag and sp are 214 * restored. 215 */ 216 li a0, THREAD_CLF_TMP 217 STR a0, THREAD_CORE_LOCAL_FLAGS(s3) 218 mv sp, s2 219 220 cpu_is_ready 221 flush_cpu_semaphores 222 wait_secondary 223 224 jal thread_clr_boot_thread 225 226 li a0, TEEABI_OPTEED_RETURN_ENTRY_DONE 227 la a1, thread_vector_table 228 j thread_return_to_ree 229END_FUNC reset_primary 230 231LOCAL_FUNC reset_secondary , : , .identity_map 232UNWIND( .cantunwind) 233 wait_primary 234 csrw CSR_SATP, zero 235 set_sp 236 set_tp 237 set_satp 238 cpu_is_ready 239 240 jal boot_init_secondary 241 j . 242END_FUNC reset_secondary 243 244LOCAL_FUNC unhandled_cpu , : 245 wfi 246 j unhandled_cpu 247END_FUNC unhandled_cpu 248 249#ifdef CFG_BOOT_SYNC_CPU 250LOCAL_DATA sem_cpu_sync_start , : 251 .word sem_cpu_sync 252END_DATA sem_cpu_sync_start 253 254LOCAL_DATA sem_cpu_sync_end , : 255 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 256END_DATA sem_cpu_sync_end 257#endif 258 259LOCAL_DATA stack_tmp_rel , : 260 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 261END_DATA stack_tmp_rel 262 263LOCAL_DATA stack_tmp_stride_rel , : 264 .word stack_tmp_stride - stack_tmp_stride_rel 265END_DATA stack_tmp_stride_rel 266 267 .balign 8 268LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 269 .skip CORE_MMU_CONFIG_SIZE 270END_DATA boot_mmu_config 271