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 <[email protected]>

Author: povergoing

arch/arm64/core/reset.S

@@ -109,9 +109,18 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start)
 
 	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 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start)
 	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)

arch/arm64/core/smp.c

@@ -213,9 +213,9 @@ void flush_fpu_ipi_handler(const void *unused)
 
 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