From 7005662f2d0db768239708eacfa01cc13de7cab7 Mon Sep 17 00:00:00 2001
From: chao an <anchao@lixiang.com>
Date: Mon, 14 Oct 2024 12:33:50 +0800
Subject: [PATCH] arch/arm64: add support to config SPI interrupt affinity
 routing to CPU0 by default

Signed-off-by: chao an <anchao@lixiang.com>
---
 arch/arm64/Kconfig                  |  7 +++++++
 arch/arm64/src/common/arm64_arch.h  |  2 +-
 arch/arm64/src/common/arm64_gicv3.c | 13 +++++++++++++
 3 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 6e81f24db5174..b7dfd66752bfc 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -226,6 +226,13 @@ config ARMV8A_GIC_SPI_EDGE
 	---help---
 		Configure all SPIs(Shared Peripheral Interrupts) as edge-triggered by default.
 
+config ARMV8A_GIC_SPI_ROUTING_CPU0
+	bool "Configure SPI interrupt affinity routing to CPU0 by default"
+	default y if SMP
+	default n
+	---help---
+		Configure SPI interrupt affinity routing to CPU0 by default.
+
 endif # ARMV8A_HAVE_GICv3
 
 config ARCH_CORTEX_A53
diff --git a/arch/arm64/src/common/arm64_arch.h b/arch/arm64/src/common/arm64_arch.h
index fbd47f5437517..fdbd74a55db37 100644
--- a/arch/arm64/src/common/arm64_arch.h
+++ b/arch/arm64/src/common/arm64_arch.h
@@ -506,7 +506,7 @@ void arm64_cpu_enable(void);
 #ifdef CONFIG_SMP
 uint64_t arm64_get_mpid(int cpu);
 #else
-#  define arm64_get_mpid(cpu) GET_MPIDR()
+#  define arm64_get_mpid(cpu) (GET_MPIDR() & MPIDR_ID_MASK)
 #endif /* CONFIG_SMP */
 
 /****************************************************************************
diff --git a/arch/arm64/src/common/arm64_gicv3.c b/arch/arm64/src/common/arm64_gicv3.c
index de56bec9041b8..e02469e9940fb 100644
--- a/arch/arm64/src/common/arm64_gicv3.c
+++ b/arch/arm64/src/common/arm64_gicv3.c
@@ -254,10 +254,12 @@ void arm64_gic_irq_enable(unsigned int intid)
    * SPI's affinity, now set it to be the PE on which it is enabled.
    */
 
+#ifndef CONFIG_ARMV8A_GIC_SPI_ROUTING_CPU0
   if (GIC_IS_SPI(intid))
     {
       arm64_gic_write_irouter((GET_MPIDR() & MPIDR_ID_MASK), intid);
     }
+#endif
 
   putreg32(mask, ISENABLER(GET_DIST_BASE(intid), idx));
 }
@@ -618,6 +620,17 @@ static void gicv3_dist_init(void)
 #endif
     }
 
+  /* Configure SPI interrupt affinity routing to CPU0 */
+
+#ifdef CONFIG_ARMV8A_GIC_SPI_ROUTING_CPU0
+  uint64_t mpid = arm64_get_mpid(0);
+
+  for (intid = GIC_SPI_INT_BASE; intid < num_ints; intid++)
+    {
+      putreg64(mpid, IROUTER(base, intid));
+    }
+#endif
+
   /* TODO: Some arrch64 Cortex-A core maybe without security state
    * it has different GIC configure with standard arrch64 A or R core
    */