arm_gic: Trigger SGIs with GICv3 affinity routing

GICv3 uses a different format for the SGIR register, defining writes to
it based on affinity values. This adds support for this type of routing,
and allows platform-specific override of the mapping from logical cpu
number to affinities via arch_cpu_num_to_gic_affinities in case an
architecture has a more exotic affinity layout. A platform will need to
define this if they are using GICv3 with affinity routing, and have a
custom arch_curr_cpu_num implementation.

Bug: 143179093
Change-Id: I7896cc88c910144d0df67c590ae926707c8afa55

4 files changed, 63 insertions, 12 deletions

diff --git a/dev/interrupt/arm_gic/arm_gic.c b/dev/interrupt/arm_gic/arm_gic.c
index ba29ad17..c38decff 100644
--- a/dev/interrupt/arm_gic/arm_gic.c
+++ b/dev/interrupt/arm_gic/arm_gic.c
@@ -388,15 +388,20 @@ static status_t arm_gic_set_priority_locked(u_int irq, uint8_t priority)
 
 status_t arm_gic_sgi(u_int irq, u_int flags, u_int cpu_mask)
 {
+    if (irq >= 16) {
+        return ERR_INVALID_ARGS;
+    }
+
 #if GIC_VERSION > 2
-    /* This assumes that all CPUs are in the same cluster */
-    u_int val = ((irq & 0xf) << 24) | (cpu_mask & 0xffff);
+    for (size_t cpu = 0; cpu < SMP_MAX_CPUS; cpu++) {
+        if (!((cpu_mask >> cpu) & 1)) {
+            continue;
+        }
 
-    if (irq >= (1U << 16))
-        return ERR_INVALID_ARGS;
+        uint64_t val = arm_gicv3_sgir_val(irq, cpu);
 
-    LTRACEF("GICC_PRIMARY_SGIR: %x\n", val);
-    GICCREG_WRITE(0, GICC_PRIMARY_SGIR, val);
+        GICCREG_WRITE(0, GICC_PRIMARY_SGIR, val);
+    }
 
 #else /* else GIC_VERSION > 2 */
 
@@ -406,9 +411,6 @@ status_t arm_gic_sgi(u_int irq, u_int flags, u_int cpu_mask)
         ((flags & ARM_GIC_SGI_FLAG_NS) ? (1U << 15) : 0) |
         (irq & 0xf);
 
-    if (irq >= 16)
-        return ERR_INVALID_ARGS;
-
     LTRACEF("GICD_SGIR: %x\n", val);
 
     GICDREG_WRITE(0, GICD_SGIR, val);
@@ -695,13 +697,12 @@ status_t sm_intc_fiq_enter(void)
     if (irq >= 1020) {
 #if ARM_GIC_USE_DOORBELL_NS_IRQ
         uint cpu = arch_curr_cpu_num();
-        uint cpu_mask = 1U << cpu;
-        u_int val = (ARM_GIC_DOORBELL_IRQ << 24) | (cpu_mask & 0xffff);
+        uint64_t val = arm_gicv3_sgir_val(ARM_GIC_DOORBELL_IRQ, cpu);
 
         GICCREG_WRITE(0, icc_igrpen1_el1, 0); /* Disable secure Group 1 */
 
         if (doorbell_enabled) {
-            LTRACEF("GICD_SGIR: %x\n", val);
+            LTRACEF("GICD_SGIR: %llx\n", val);
             GICCREG_WRITE(0, icc_asgi1r_el1, val);
         }
 #else
diff --git a/dev/interrupt/arm_gic/gic_v3.c b/dev/interrupt/arm_gic/gic_v3.c
index ddf28423..7f13a244 100644
--- a/dev/interrupt/arm_gic/gic_v3.c
+++ b/dev/interrupt/arm_gic/gic_v3.c
@@ -26,6 +26,8 @@
 #include <bits.h>
 #include <stdint.h>
 
+#include <dev/interrupt/arm_gic.h>
+
 #include "arm_gic_common.h"
 #include "gic_v3.h"
 
@@ -278,3 +280,41 @@ void arm_gicv3_resume_cpu_locked(unsigned int cpu, bool gicd) {
         }
     }
 }
+
+STATIC_ASSERT(SMP_CPU_CLUSTER_SHIFT <= 8);
+/* SMP_MAX_CPUs needs to be addressable with only two affinities */
+STATIC_ASSERT((SMP_MAX_CPUS >> SMP_CPU_CLUSTER_SHIFT) <= 0x100U);
+
+__WEAK struct arm_gic_affinities arch_cpu_num_to_gic_affinities(size_t cpu_num) {
+    const size_t max_cluster_size = 1U << SMP_CPU_CLUSTER_SHIFT;
+    const uint8_t cluster_mask = max_cluster_size - 1;
+    struct arm_gic_affinities out = {
+        .aff0 = cpu_num & cluster_mask,
+        .aff1 = cpu_num >> SMP_CPU_CLUSTER_SHIFT,
+        .aff2 = 0,
+        .aff3 = 0,
+    };
+    return out;
+}
+
+#define SGIR_AFF1_SHIFT (16)
+#define SGIR_AFF2_SHIFT (32)
+#define SGIR_AFF3_SHIFT (48)
+#define SGIR_IRQ_SHIFT (24)
+#define SGIR_RS_SHIFT (44)
+#define SGIR_TARGET_LIST_SHIFT (0)
+#define SGIR_ASSEMBLE(val, shift) ((uint64_t)val << shift)
+
+uint64_t arm_gicv3_sgir_val(u_int irq, size_t cpu_num) {
+    struct arm_gic_affinities affs = arch_cpu_num_to_gic_affinities(cpu_num);
+    DEBUG_ASSERT(irq < 16);
+
+    uint8_t range_selector = affs.aff0 >> 4;
+    uint16_t target_list = 1U << (affs.aff0 & 0xf);
+    return SGIR_ASSEMBLE(irq, SGIR_IRQ_SHIFT) |
+        SGIR_ASSEMBLE(affs.aff3, SGIR_AFF3_SHIFT) |
+        SGIR_ASSEMBLE(affs.aff2, SGIR_AFF2_SHIFT) |
+        SGIR_ASSEMBLE(affs.aff1, SGIR_AFF1_SHIFT) |
+        SGIR_ASSEMBLE(range_selector, SGIR_RS_SHIFT) |
+        SGIR_ASSEMBLE(target_list, SGIR_TARGET_LIST_SHIFT);
+}
diff --git a/dev/interrupt/arm_gic/gic_v3.h b/dev/interrupt/arm_gic/gic_v3.h
index 1f3c6358..6c3b74f6 100644
--- a/dev/interrupt/arm_gic/gic_v3.h
+++ b/dev/interrupt/arm_gic/gic_v3.h
@@ -28,3 +28,4 @@ void arm_gicv3_init_percpu(void);
 void arm_gicv3_configure_irq_locked(unsigned int cpu, unsigned int vector);
 void arm_gicv3_suspend_cpu(unsigned int cpu);
 void arm_gicv3_resume_cpu_locked(unsigned int cpu, bool gicd);
+uint64_t arm_gicv3_sgir_val(u_int irq, size_t cpu_num);
diff --git a/dev/interrupt/arm_gic/include/dev/interrupt/arm_gic.h b/dev/interrupt/arm_gic/include/dev/interrupt/arm_gic.h
index b40fb077..b008cf69 100644
--- a/dev/interrupt/arm_gic/include/dev/interrupt/arm_gic.h
+++ b/dev/interrupt/arm_gic/include/dev/interrupt/arm_gic.h
@@ -39,5 +39,14 @@ enum {
 };
 status_t arm_gic_sgi(u_int irq, u_int flags, u_int cpu_mask);
 
+struct arm_gic_affinities {
+    uint8_t aff0;
+    uint8_t aff1;
+    uint8_t aff2;
+    uint8_t aff3;
+};
+
+struct arm_gic_affinities arch_cpu_num_to_gic_affinities(size_t cpu_num);
+
 #endif