gic_v3: generalize ipi_send_target

While porting the Orin, we found that ipi_send_target made assumptions
about how the clusters were identified by aff1. For the Orin SoC, the
cluster is actually identified by aff2.

This commit allows us to ipi_send_target on any platform that uses the
gic_v3.

Signed-off-by: Andy Bui <andy.bui@nio.io>

include/arch/arm/arch/machine/gic_v3.h

@@ -49,6 +49,29 @@
 
 #define DEFAULT_PMR_VALUE            0xff
 
+/* SGI1R register. */
+#define ICC_SGI1R_TARGET_LIST_SHIFT         0
+#define ICC_SGI1R_TARGET_LIST_MASK          (0xffff << ICC_SGI1R_TARGET_LIST_SHIFT)
+#define ICC_SGI1R_TARGET_LIST_VAL(sgi1r)    (((sgi1r) & ICC_SGI1R_TARGET_LIST_MASK) >> ICC_SGI1R_TARGET_LIST_SHIFT)
+#define ICC_SGI1R_AFF1_SHIFT            16
+#define ICC_SGI1R_AFF1_MASK             (0xff << ICC_SGI1R_AFF1_SHIFT)
+#define ICC_SGI1R_AFF1_VAL(sgi1r)       (((sgi1r) & ICC_SGI1R_AFF1_MASK) >> ICC_SGI1R_AFF1_SHIFT)
+#define ICC_SGI1R_INT_ID_SHIFT          24
+#define ICC_SGI1R_INT_ID_MASK           (0xfull << ICC_SGI1R_INT_ID_SHIFT)
+#define ICC_SGI1R_INT_ID_VAL(sgi1r)     (((sgi1r) & ICC_SGI1R_INT_ID_MASK) >> ICC_SGI1R_INT_ID_SHIFT)
+#define ICC_SGI1R_AFF2_SHIFT            32
+#define ICC_SGI1R_AFF2_MASK             (0xffull << ICC_SGI1R_AFF2_SHIFT)
+#define ICC_SGI1R_AFF2_VAL(sgi1r)       (((sgi1r) & ICC_SGI1R_AFF2_MASK) >> ICC_SGI1R_AFF2_SHIFT)
+#define ICC_SGI1R_IRM_SHIFT             40
+#define ICC_SGI1R_IRM_MASK              (0x1ull << ICC_SGI1R_IRM_SHIFT)
+#define ICC_SGI1R_IRM_VAL(sgi1r)        (((sgi1r) & ICC_SGI1R_IRM_MASK) >> ICC_SGI1R_IRM_SHIFT)
+#define ICC_SGI1R_RS_SHIFT              44
+#define ICC_SGI1R_RS_MASK               (0xfull << ICC_SGI1R_RS_SHIFT)
+#define ICC_SGI1R_RS_VAL(sgi1r)         (((sgi1r) & ICC_SGI1R_RS_MASK) >> ICC_SGI1R_RS_SHIFT)
+#define ICC_SGI1R_AFF3_SHIFT            48
+#define ICC_SGI1R_AFF3_MASK             (0xffull << ICC_SGI1R_AFF3_SHIFT)
+#define ICC_SGI1R_AFF3_VAL(sgi1r)       (((sgi1r) & ICC_SGI1R_AFF3_MASK) >> ICC_SGI1R_AFF3_SHIFT)
+
 /* System registers for GIC CPU interface */
 #ifdef CONFIG_ARCH_AARCH64
 #define ICC_IAR1_EL1    "S3_0_C12_C12_0"

include/arch/arm/arch/model/smp.h

@@ -13,7 +13,11 @@
 #ifdef ENABLE_SMP_SUPPORT
 static inline cpu_id_t cpuIndexToID(word_t index)
 {
+#ifdef CONFIG_ARM_GIC_V3_SUPPORT
+    return index;
+#else
     return BIT(index);
+#endif
 }
 
 static inline bool_t try_arch_atomic_exchange_rlx(void *ptr, void *new_val, void **prev)

src/arch/arm/machine/gic_v3.c

@@ -28,11 +28,6 @@
 #define ICC_SGI1R_EL1 "p15, 0, %Q0, %R0, c12"
 #endif
 
-#define ICC_SGI1R_INTID_SHIFT          (24)
-#define ICC_SGI1R_AFF1_SHIFT           (16)
-#define ICC_SGI1R_IRM_BIT              (40)
-#define ICC_SGI1R_CPUTARGETLIST_MASK   0xffff
-
 volatile struct gic_dist_map *const gic_dist = (volatile struct gic_dist_map *)(GICD_PPTR);
 volatile void *const gicr_base = (volatile uint8_t *)(GICR_PPTR);
 
@@ -345,34 +340,19 @@ BOOT_CODE void cpu_initLocalIRQController(void)
 }
 
 #ifdef ENABLE_SMP_SUPPORT
-#define MPIDR_MT(x)   (x & BIT(24))
-
-void ipi_send_target(irq_t irq, word_t cpuTargetList)
+/* This function is called with a single node. */
+void ipi_send_target(irq_t irq, word_t cpuID)
 {
-    uint64_t sgi1r_base = ((word_t) IRQT_TO_IRQ(irq)) << ICC_SGI1R_INTID_SHIFT;
-    word_t sgi1r[CONFIG_MAX_NUM_NODES];
-    word_t last_aff1 = 0;
-
-    for (word_t i = 0; i < CONFIG_MAX_NUM_NODES; i++) {
-        sgi1r[i] = 0;
-        if (cpuTargetList & BIT(i)) {
-            word_t mpidr = mpidr_map[i];
-            word_t aff1 = MPIDR_AFF1(mpidr);
-            word_t aff0 = MPIDR_AFF0(mpidr);
-            // AFF1 is assumed to be contiguous and less than CONFIG_MAX_NUM_NODES.
-            // The targets are grouped by AFF1.
-            assert(aff1 >= 0 && aff1 < CONFIG_MAX_NUM_NODES);
-            sgi1r[aff1] |= sgi1r_base | (aff1 << ICC_SGI1R_AFF1_SHIFT) | (1 << aff0);
-            if (aff1 > last_aff1) {
-                last_aff1 = aff1;
-            }
-        }
-    }
-    for (word_t i = 0; i <= last_aff1; i++) {
-        if (sgi1r[i] != 0) {
-            SYSTEM_WRITE_64(ICC_SGI1R_EL1, sgi1r[i]);
-        }
-    }
+    assert(cpuID < CONFIG_MAX_NUM_NODES);
+    word_t sgi1r_base = ((word_t) IRQT_TO_IRQ(irq)) << ICC_SGI1R_INT_ID_SHIFT;
+    word_t mpidr = mpidr_map[cpuID];
+    word_t aff0  = MPIDR_AFF0(mpidr);
+    word_t aff1  = MPIDR_AFF1(mpidr);
+    word_t aff2  = MPIDR_AFF2(mpidr);
+    word_t aff3  = MPIDR_AFF3(mpidr);
+    word_t sgi = sgi1r_base | (aff3 << ICC_SGI1R_AFF3_SHIFT) | (aff2 << ICC_SGI1R_AFF2_SHIFT);
+    sgi |= (aff1 << ICC_SGI1R_AFF1_SHIFT) | (1 << aff0);
+    SYSTEM_WRITE_64(ICC_SGI1R_EL1, sgi);
     isb();
 }