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 <mm/core_mmu.h> 10#include <platform_config.h> 11#include <riscv.h> 12#include <riscv_macros.S> 13 14.section .data 15.balign 4 16 17#ifdef CFG_BOOT_SYNC_CPU 18.equ SEM_CPU_READY, 1 19#endif 20 21 /* 22 * Setup sp to point to the top of the tmp stack for the current CPU: 23 * sp is assigned: 24 * stack_tmp + (hartid + 1) * stack_tmp_stride - STACK_TMP_GUARD 25 */ 26.macro set_sp 27 /* Unsupported CPU, park it before it breaks something */ 28 li t1, CFG_TEE_CORE_NB_CORE 29 csrr t0, CSR_XSCRATCH 30 bge t0, t1, unhandled_cpu 31 addi t0, t0, 1 32 lw t1, stack_tmp_stride 33 /* 34 * t0 = (hartid + 1) 35 * t1 = value of stack_tmp_stride 36 * value of stack_tmp_rel = stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 37 * sp = stack_tmp + (hartid + 1) * stack_tmp_stride - STACK_TMP_GUARD 38 * = stack_tmp_rel + (value of stack_tmp_rel) + (t0 * t1) 39 */ 40 mul t1, t0, t1 41 la t2, stack_tmp_rel 42 lw t0, 0(t2) 43 add t0, t0, t2 44 add sp, t1, t0 45.endm 46 47.macro cpu_is_ready 48#ifdef CFG_BOOT_SYNC_CPU 49 csrr t0, CSR_XSCRATCH 50 la t1, sem_cpu_sync 51 slli t0, t0, 2 52 add t1, t1, t0 53 li t2, SEM_CPU_READY 54 sw t2, 0(t1) 55 fence 56#endif 57.endm 58 59.macro set_tp 60 csrr a0, CSR_XSCRATCH 61 li a1, THREAD_CORE_LOCAL_SIZE 62 la tp, thread_core_local 63 mul a2, a1, a0 64 add tp, tp, a2 65 sw a0, THREAD_CORE_LOCAL_HART_ID(tp) 66.endm 67 68.macro set_satp 69 la a1, boot_mmu_config 70 LDR a0, CORE_MMU_CONFIG_SATP(a1) 71 csrw CSR_SATP, a0 72 sfence.vma zero, zero 73.endm 74 75.macro wait_primary 76#ifdef CFG_BOOT_SYNC_CPU 77 la t0, sem_cpu_sync 78 li t2, SEM_CPU_READY 791: 80 fence w, w 81 lw t1, 0(t0) 82 bne t1, t2, 1b 83#endif 84.endm 85 86.macro wait_secondary 87#ifdef CFG_BOOT_SYNC_CPU 88 la t0, sem_cpu_sync 89 li t1, CFG_TEE_CORE_NB_CORE 90 li t2, SEM_CPU_READY 911: 92 addi t1, t1, -1 93 beqz t1, 3f 94 addi t0, t0, 4 952: 96 fence 97 lw t1, 0(t0) 98 bne t1, t2, 2b 99 j 1b 1003: 101#endif 102.endm 103 104#ifdef CFG_BOOT_SYNC_CPU 105#define flush_cpu_semaphores \ 106 la t0, sem_cpu_sync_start 107 la t1, sem_cpu_sync_end 108 fence 109#else 110#define flush_cpu_semaphores 111#endif 112 113.macro bootargs_entry 114 /* 115 * Save boot arguments 116 */ 117 la t0, boot_args 118 /* Save boot hart */ 119 STR a0, REGOFF(0)(t0) 120 /* Save FDT address */ 121 STR a1, REGOFF(1)(t0) 122.endm 123 124FUNC _start , : 125.option push 126.option norelax 127 la gp, __global_pointer$ 128.option pop 129#ifdef CFG_RISCV_M_MODE 130 csrr a0, CSR_MHARTID 131#endif 132 csrw CSR_XSCRATCH, a0 133 bnez a0, reset_secondary 134 jal reset_primary 135 j . 136END_FUNC _start 137 138LOCAL_FUNC reset_primary , : , .identity_map 139UNWIND( .cantunwind) 140 141 bootargs_entry 142 143 /* 144 * Zero bss 145 */ 146 lla t0, __bss_start 147 lla t1, __bss_end 148 beq t0, t1, 1f 1490: 150 STR zero, (t0) 151 add t0, t0, RISCV_XLEN_BYTES 152 bne t0, t1, 0b 1531: 154#ifdef CFG_RISCV_S_MODE 155 lla t0, _start 156 lla t1, start_addr 157 STR t0, (t1) 158#endif 159 160 csrw CSR_SATP, zero 161 set_sp 162 set_tp 163 164 jal thread_init_thread_core_local 165 jal plat_primary_init_early 166 jal console_init 167 168 mv a0, x0 169 la a1, boot_mmu_config 170 jal core_init_mmu_map 171 172 set_satp 173 174 jal boot_init_primary_early 175 jal boot_init_primary_late 176 177 cpu_is_ready 178 flush_cpu_semaphores 179 wait_secondary 180 181 jal thread_clr_boot_thread 182 j mu_service 183END_FUNC reset_primary 184 185LOCAL_FUNC reset_secondary , : , .identity_map 186UNWIND( .cantunwind) 187 wait_primary 188 csrw CSR_SATP, zero 189 set_sp 190 set_tp 191 set_satp 192 cpu_is_ready 193 194 jal boot_init_secondary 195 j . 196END_FUNC reset_secondary 197 198LOCAL_FUNC unhandled_cpu , : 199 wfi 200 j unhandled_cpu 201END_FUNC unhandled_cpu 202 203#ifdef CFG_BOOT_SYNC_CPU 204LOCAL_DATA sem_cpu_sync_start , : 205 .word sem_cpu_sync 206END_DATA sem_cpu_sync_start 207 208LOCAL_DATA sem_cpu_sync_end , : 209 .word sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2) 210END_DATA sem_cpu_sync_end 211#endif 212 213LOCAL_DATA stack_tmp_rel , : 214 .word stack_tmp - stack_tmp_rel - STACK_TMP_GUARD 215END_DATA stack_tmp_rel 216 217LOCAL_DATA stack_tmp_stride_rel , : 218 .word stack_tmp_stride - stack_tmp_stride_rel 219END_DATA stack_tmp_stride_rel 220 221 .balign 8 222LOCAL_DATA boot_mmu_config , : /* struct core_mmu_config */ 223 .skip CORE_MMU_CONFIG_SIZE 224END_DATA boot_mmu_config 225