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