diff --git a/arch/arm64/core/reset.S b/arch/arm64/core/reset.S
index 94997535ac8..6c12f0d0914 100644
--- a/arch/arm64/core/reset.S
+++ b/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)
diff --git a/arch/arm64/core/smp.c b/arch/arm64/core/smp.c
index e23a8ada4c4..ea84914e676 100644
--- a/arch/arm64/core/smp.c
+++ b/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