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 16.section .data 17.balign 4 18 19#ifdef CFG_BOOT_SYNC_CPU 20.equ SEM_CPU_READY, 1 21#endif 22 23 /* 24 * Setup sp to point to the top of the tmp stack for the current CPU: 25 * sp is assigned: 26 * stack_tmp + (hartid + 1) * stack_tmp_stride - STACK_TMP_GUARD 27 */ 28.macro set_sp 29 /* Unsupported CPU, park it before it breaks something */ 30 li t1, CFG_TEE_CORE_NB_CORE 31 csrr t0, CSR_XSCRATCH 32 bge t0, t1, unhandled_cpu 33 addi t0, t0, 1 34 lw t1, stack_tmp_stride 35 /* 36 * t0 = (hartid + 1) 37 * t1 = value of stack_tmp_stride 38 * value of stack_tmp_rel = stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 39 * sp = stack_tmp + (hartid + 1) * stack_tmp_stride - STACK_TMP_GUARD 40 * = stack_tmp_rel + (value of stack_tmp_rel) + (t0 * t1) 41 */ 42 mul t1, t0, t1 43 la t2, stack_tmp_rel 44 lw t0, 0(t2) 45 add t0, t0, t2 46 add sp, t1, t0 47.endm 48 49.macro cpu_is_ready 50#ifdef CFG_BOOT_SYNC_CPU 51 csrr t0, CSR_XSCRATCH 52 la t1, sem_cpu_sync 53 slli t0, t0, 2 54 add t1, t1, t0 55 li t2, SEM_CPU_READY 56 sw t2, 0(t1) 57 fence 58#endif 59.endm 60 61.macro set_tp 62 csrr a0, CSR_XSCRATCH 63 li a1, THREAD_CORE_LOCAL_SIZE 64 la tp, thread_core_local 65 mul a2, a1, a0 66 add tp, tp, a2 67 sw a0, THREAD_CORE_LOCAL_HART_ID(tp) 68.endm 69 70.macro set_satp 71 la a1, boot_mmu_config 72 LDR a0, CORE_MMU_CONFIG_SATP(a1) 73 csrw CSR_SATP, a0 74 sfence.vma zero, zero 75.endm 76 77.macro wait_primary 78#ifdef CFG_BOOT_SYNC_CPU 79 la t0, sem_cpu_sync 80 li t2, SEM_CPU_READY 811: 82 fence w, w 83 lw t1, 0(t0) 84 bne t1, t2, 1b 85#endif 86.endm 87 88.macro wait_secondary 89#ifdef CFG_BOOT_SYNC_CPU 90 la t0, sem_cpu_sync 91 li t1, CFG_TEE_CORE_NB_CORE 92 li t2, SEM_CPU_READY 931: 94 addi t1, t1, -1 95 beqz t1, 3f 96 addi t0, t0, 4 972: 98 fence 99 lw t1, 0(t0) 100 bne t1, t2, 2b 101 j 1b 1023: 103#endif 104.endm 105 106#ifdef CFG_BOOT_SYNC_CPU 107#define flush_cpu_semaphores \ 108 la t0, sem_cpu_sync_start 109 la t1, sem_cpu_sync_end 110 fence 111#else 112#define flush_cpu_semaphores 113#endif 114 115.macro bootargs_entry 116 /* 117 * Save boot arguments 118 */ 119 la t0, boot_args 120 /* Save boot hart */ 121 STR a0, REGOFF(0)(t0) 122 /* Save FDT address */ 123 STR a1, REGOFF(1)(t0) 124.endm 125 126FUNC _start , : 127 /* 128 * Register usage: 129 * a0 - if non-NULL holds the hart ID 130 * a1 - if non-NULL holds the system DTB address 131 * 132 * CSR_XSCRATCH - saved a0 133 * s1 - saved a1 134 */ 135.option push 136.option norelax 137 la gp, __global_pointer$ 138.option pop 139#ifdef CFG_RISCV_M_MODE 140 csrr a0, CSR_MHARTID 141#endif 142 csrw CSR_XSCRATCH, a0 143#if defined(CFG_DT_ADDR) 144 li s1, CFG_DT_ADDR 145#else 146 mv s1, a1 /* Save device tree address into s1 */ 147#endif 148 bnez a0, reset_secondary 149 jal reset_primary 150 j . 151END_FUNC _start 152 153LOCAL_FUNC reset_primary , : , .identity_map 154UNWIND( .cantunwind) 155 156 bootargs_entry 157 158 /* 159 * Zero bss 160 */ 161 lla t0, __bss_start 162 lla t1, __bss_end 163 beq t0, t1, 1f 1640: 165 STR zero, (t0) 166 add t0, t0, RISCV_XLEN_BYTES 167 bne t0, t1, 0b 1681: 169#ifdef CFG_RISCV_S_MODE 170 lla t0, _start 171 lla t1, start_addr 172 STR t0, (t1) 173#endif 174 175 csrw CSR_SATP, zero 176 set_sp 177 set_tp 178 179 jal thread_init_thread_core_local 180 jal plat_primary_init_early 181 jal console_init 182 183 mv a0, x0 184 la a1, boot_mmu_config 185 jal core_init_mmu_map 186 187 set_satp 188 189 jal boot_init_primary_early 190 191 /* 192 * Before entering boot_init_primary_late(), we do these two steps: 193 * 1. Save current sp to s2, and set sp as threads[0].stack_va_end 194 * 2. Clear the flag which indicates usage of the temporary stack in the 195 * current hart's thread_core_local structure. 196 */ 197 mv s2, sp 198 la a0, threads 199 LDR a0, THREAD_CTX_STACK_VA_END(a0) 200 mv sp, a0 201 jal thread_get_core_local 202 mv s3, a0 203 STR x0, THREAD_CORE_LOCAL_FLAGS(s3) 204 205 mv a0, s1 /* s1 contains saved device tree address */ 206 mv a1, x0 /* unused */ 207 jal boot_init_primary_late 208 209 /* 210 * After returning from boot_init_primary_late(), the flag and sp are 211 * restored. 212 */ 213 li a0, THREAD_CLF_TMP 214 STR a0, THREAD_CORE_LOCAL_FLAGS(s3) 215 mv sp, s2 216 217 cpu_is_ready 218 flush_cpu_semaphores 219 wait_secondary 220 221 jal thread_clr_boot_thread 222 j mu_service 223END_FUNC reset_primary 224 225LOCAL_FUNC reset_secondary , : , .identity_map 226UNWIND( .cantunwind) 227 wait_primary 228 csrw CSR_SATP, zero 229 set_sp 230 set_tp 231 set_satp 232 cpu_is_ready 233 234 jal boot_init_secondary 235 j . 236END_FUNC reset_secondary 237 238LOCAL_FUNC unhandled_cpu , : 239 wfi 240 j unhandled_cpu 241END_FUNC unhandled_cpu 242 243#ifdef CFG_BOOT_SYNC_CPU 244LOCAL_DATA sem_cpu_sync_start , : 245 .word sem_cpu_sync 246END_DATA sem_cpu_sync_start 247 248LOCAL_DATA sem_cpu_sync_end , : 249 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 250END_DATA sem_cpu_sync_end 251#endif 252 253LOCAL_DATA stack_tmp_rel , : 254 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 255END_DATA stack_tmp_rel 256 257LOCAL_DATA stack_tmp_stride_rel , : 258 .word stack_tmp_stride - stack_tmp_stride_rel 259END_DATA stack_tmp_stride_rel 260 261 .balign 8 262LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 263 .skip CORE_MMU_CONFIG_SIZE 264END_DATA boot_mmu_config 265