Fix misc issues and enable v8r64 FPU

arch/arm64/core/Kconfig

@@ -23,6 +23,9 @@ config CPU_AARCH64_CORTEX_R
 	select HAS_FLASH_LOAD_OFFSET
 	select CPU_HAS_DCACHE
 	select CPU_HAS_ICACHE
+	select CPU_HAS_FPU
+	imply FPU
+	imply FPU_SHARING
 	help
 	  This option signifies the use of a CPU of the Cortex-R 64-bit family.
 

arch/arm64/core/cortex_r/arm_mpu.c

@@ -181,8 +181,6 @@ void z_arm64_mm_init(bool is_primary_core)
 		return;
 	}
 
-	LOG_DBG("total region count: %d", get_num_regions());
-
 	arm_core_mpu_disable();
 
 	/* Architecture-specific configuration */

arch/arm64/core/reset.S

@@ -155,7 +155,8 @@ primary_core:
 	 * so that we don't clobber any data.
 	 */
 #ifdef CONFIG_INIT_STACKS
-	ldr	x0, =(z_interrupt_stacks)
+	mov_imm	x0, CONFIG_ISR_STACK_SIZE
+	sub	x0, sp, x0
 	sub     x9, sp, #8
 	mov     x10, 0xaaaaaaaaaaaaaaaa
 stack_init_loop:

arch/arm64/core/smp.c

@@ -26,6 +26,8 @@
 #include <zephyr/irq.h>
 #include "boot.h"
 
+#define INV_MPID	UINT64_MAX
+
 #define SGI_SCHED_IPI	0
 #define SGI_MMCFG_IPI	1
 #define SGI_FPU_IPI	2
@@ -50,6 +52,11 @@ static const uint64_t cpu_node_list[] = {
 	DT_FOREACH_CHILD_STATUS_OKAY_SEP(DT_PATH(cpus), DT_REG_ADDR, (,))
 };
 
+/* cpu_map saves the maping of core id and mpid */
+static uint64_t cpu_map[CONFIG_MP_MAX_NUM_CPUS] = {
+	[0 ... (CONFIG_MP_MAX_NUM_CPUS - 1)] = INV_MPID
+};
+
 extern void z_arm64_mm_init(bool is_primary_core);
 
 /* Called from Zephyr initialization */
@@ -106,6 +113,9 @@ void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz,
 	while (arm64_cpu_boot_params.fn) {
 		wfe();
 	}
+
+	cpu_map[cpu_num] = cpu_mpid;
+
 	printk("Secondary CPU core %d (MPID:%#llx) is up\n", cpu_num, cpu_mpid);
 }
 
@@ -166,13 +176,14 @@ static void broadcast_ipi(unsigned int ipi)
 	unsigned int num_cpus = arch_num_cpus();
 
 	for (int i = 0; i < num_cpus; i++) {
-		uint64_t target_mpidr = cpu_node_list[i];
-		uint8_t aff0 = MPIDR_AFFLVL(target_mpidr, 0);
+		uint64_t target_mpidr = cpu_map[i];
+		uint8_t aff0;
 
-		if (mpidr == target_mpidr) {
+		if (mpidr == target_mpidr || mpidr == INV_MPID) {
 			continue;
 		}
 
+		aff0 = MPIDR_AFFLVL(target_mpidr, 0);
 		gic_raise_sgi(ipi, target_mpidr, 1 << aff0);
 	}
 }
@@ -220,9 +231,14 @@ void flush_fpu_ipi_handler(const void *unused)
 
 void z_arm64_flush_fpu_ipi(unsigned int cpu)
 {
-	const uint64_t mpidr = cpu_node_list[cpu];
-	uint8_t aff0 = MPIDR_AFFLVL(mpidr, 0);
+	const uint64_t mpidr = cpu_map[cpu];
+	uint8_t aff0;
+
+	if (mpidr == INV_MPID) {
+		return;
+	}
 
+	aff0 = MPIDR_AFFLVL(mpidr, 0);
 	gic_raise_sgi(SGI_FPU_IPI, mpidr, 1 << aff0);
 }
 
@@ -248,6 +264,7 @@ void arch_spin_relax(void)
 
 static int arm64_smp_init(void)
 {
+	cpu_map[0] = MPIDR_TO_CORE(GET_MPIDR());
 
 	/*
 	 * SGI0 is use for sched ipi, this might be changed to use Kconfig

tests/kernel/fpu_sharing/generic/prj.conf

@@ -3,6 +3,6 @@ CONFIG_FPU=y
 CONFIG_FPU_SHARING=y
 CONFIG_STDOUT_CONSOLE=y
 CONFIG_CBPRINTF_FP_SUPPORT=y
-CONFIG_MAIN_STACK_SIZE=1024
+CONFIG_MAIN_STACK_SIZE=2048
 CONFIG_ZTEST_NEW_API=y
 CONFIG_MP_MAX_NUM_CPUS=1