1/* 2 * Copyright (c) 2021-2025 Arm Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7#include <arch.h> 8#include <asm_macros.S> 9#include <platform_def.h> 10 11 .globl plat_secondary_cold_boot_setup 12 .globl plat_get_my_entrypoint 13 .globl plat_is_my_cpu_primary 14 .globl plat_arm_calc_core_pos 15 16#ifdef CORSTONE1000_CORTEX_A320 17 .globl plat_my_core_pos 18 19func plat_my_core_pos 20 mrs x0, mpidr_el1 21 b plat_arm_calc_core_pos 22endfunc plat_my_core_pos 23 24func plat_arm_calc_core_pos 25 /* 26 * Aff0 is always 0 for Cortex-A320 MPIDR format: 27 * https://developer.arm.com/documentation/109551/0001/AArch64-registers/AArch64-Identification-registers-summary/MPIDR-EL1--Multiprocessor-Affinity-Register?lang=en 28 */ 29 /* Extract Aff1 (core ID) */ 30 ubfx x1, x0, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 31 32 /* Extract Aff2 (cluster lower bits) */ 33 ubfx x2, x0, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 34 35 /* Extract Aff3 (cluster upper bits) */ 36 ubfx x3, x0, #MPIDR_AFF3_SHIFT, #MPIDR_AFFINITY_BITS 37 38 /* cluster_id = (Aff3 << 8) | Aff2 */ 39 lsl x3, x3, #MPIDR_AFFINITY_BITS 40 orr x3, x3, x2 41 42 /* core_pos = core_id + (cluster_id * FVP_MAX_CPUS_PER_CLUSTER) */ 43 mov x4, #CORSTONE1000_MAX_CPUS_PER_CLUSTER 44 madd x0, x3, x4, x1 45 46 ret 47endfunc plat_arm_calc_core_pos 48#endif 49 50 /* -------------------------------------------------------------------- 51 * void plat_secondary_cold_boot_setup (void); 52 * 53 * For AArch32, cold-booting secondary CPUs is not yet 54 * implemented and they panic. 55 * -------------------------------------------------------------------- 56 */ 57func plat_secondary_cold_boot_setup 58#if defined(CORSTONE1000_FVP_MULTICORE) 59 60 /* Calculate the address of our hold entry */ 61 bl plat_my_core_pos 62 lsl x0, x0, #CORSTONE1000_SECONDARY_CORE_HOLD_SHIFT 63 mov_imm x2, CORSTONE1000_SECONDARY_CORE_HOLD_BASE 64 65 /* Set the wait state for the secondary core */ 66 mov_imm x3, CORSTONE1000_SECONDARY_CORE_STATE_WAIT 67 str x3, [x2, x0] 68 dmb ish 69 70 /* Poll until the primary core signals to go */ 71poll_mailbox: 72 ldr x1, [x2, x0] 73 cmp x1, #CORSTONE1000_SECONDARY_CORE_STATE_WAIT 74 beq 1f 75 mov_imm x0, PLAT_ARM_TRUSTED_MAILBOX_BASE 76 ldr x1, [x0] 77 br x1 781: 79 wfe 80 b poll_mailbox 81#else 82cb_panic: 83 b cb_panic 84#endif 85 86endfunc plat_secondary_cold_boot_setup 87 88 /* --------------------------------------------------------------------- 89 * unsigned long plat_get_my_entrypoint (void); 90 * 91 * Main job of this routine is to distinguish between a cold and warm 92 * boot. On corstone1000, this information can be queried from the power 93 * controller. The Power Control SYS Status Register (PSYSR) indicates 94 * the wake-up reason for the CPU. 95 * 96 * For a cold boot, return 0. 97 * For a warm boot, Not yet supported. 98 * 99 * TODO: PSYSR is a common register and should be 100 * accessed using locks. Since it is not possible 101 * to use locks immediately after a cold reset 102 * we are relying on the fact that after a cold 103 * reset all cpus will read the same WK field 104 * --------------------------------------------------------------------- 105 */ 106func plat_get_my_entrypoint 107 /* TODO support warm boot */ 108 /* Cold reset */ 109 mov x0, #0 110 ret 111endfunc plat_get_my_entrypoint 112 113 /* ----------------------------------------------------- 114 * unsigned int plat_is_my_cpu_primary (void); 115 * 116 * Find out whether the current CPU is the primary 117 * CPU. 118 * ----------------------------------------------------- 119 */ 120func plat_is_my_cpu_primary 121 mrs x0, mpidr_el1 122 mov_imm x1, MPIDR_AFFINITY_MASK 123 and x0, x0, x1 124 cmp x0, #CORSTONE1000_PRIMARY_CPU 125 cset w0, eq 126 ret 127endfunc plat_is_my_cpu_primary 128