arch: arm64: Fix coherence issue of SMP boot code
The current SMP boot code doesn't consider that the cores can boot at
the same time. Possibly, more than one core can boot into primary core
boot sequence. Fix it by using the atomic operation to make sure only
one core act as the primary core.
Correspondingly, sgi_raise_ipi should transfer CPU id to mpidr.
Signed-off-by: Jaxson Han <jaxson.han@arm.com>
diff --git a/arch/arm64/core/reset.S b/arch/arm64/core/reset.S
index 9499753..6c12f0d 100644
--- a/arch/arm64/core/reset.S
+++ b/arch/arm64/core/reset.S
@@ -109,9 +109,18 @@
ldr x0, =arm64_cpu_boot_params
get_cpu_id x1
- ldr x2, [x0, #BOOT_PARAM_MPID_OFFSET]
+
+ /*
+ * If the cores start up at the same time, we should atomically load and
+ * store the mpid into arm64_cpu_boot_params.
+ */
+ ldaxr x2, [x0, #BOOT_PARAM_MPID_OFFSET]
cmp x2, #-1
- beq primary_core
+ bne 1f
+ /* try to store x1 (mpid) */
+ stlxr w3, x1, [x0]
+ /* If succeed, go to primary_core */
+ cbz w3, primary_core
/* loop until our turn comes */
1: dmb ld
@@ -125,8 +134,6 @@
b 2f
primary_core:
- /* advertise ourself */
- str x1, [x0, #BOOT_PARAM_MPID_OFFSET]
#endif
/* load primary stack and entry point */
ldr x24, =(z_interrupt_stacks + CONFIG_ISR_STACK_SIZE)
diff --git a/arch/arm64/core/smp.c b/arch/arm64/core/smp.c
index e23a8ad..ea84914 100644
--- a/arch/arm64/core/smp.c
+++ b/arch/arm64/core/smp.c
@@ -213,9 +213,9 @@
void z_arm64_flush_fpu_ipi(unsigned int cpu)
{
- const uint64_t mpidr = GET_MPIDR();
+ const uint64_t mpidr = cpu_node_list[cpu];
- gic_raise_sgi(SGI_FPU_IPI, mpidr, (1 << cpu));
+ gic_raise_sgi(SGI_FPU_IPI, mpidr, 1 << MPIDR_TO_CORE(mpidr));
}
#endif
