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