[PATCH] bsps/arm: Update GICv3 to use newer affinity interface

[Jeff Kubascik]

The GICv3 implementation uses the GICD_ITARGETSR register to configure
interrupt affinity. The GIC uses this register only when affinity
routing is disabled, which is called legacy operation mode. However,
affinity routing is being enabled, by setting the ARE_NS bit in the
GICD_CTLR register; therefore GICD_ITARGETSR will be ignored.

While affinity routing could be disabled, this legacy operation mode is
optional for a GIC implementation - it should be avoided.

This change uses the newer affinity register GICD_IROUTER. The register
is 64 bits wide, so any access would need to be performed with two store
operations on a 32 bit RTEMS build. This had to be done with assembly
code, as the compiler optimizer will try to use the 64 bit floating
point store operation VSTM to write to the register. This does not work
with virtual GICD registers on the Xen hypervisor, as the VSTM
instruction data abort does not provide syndrome information.

Affinity routing allows for hierarchical routing of interrupts. To take
advantage of this feature, it would require an understanding of the
processor/core hierarchy. RTEMS specifies the desired processor affinity
with a mask, which does not provide this information. For now, assume
a simple single level routing hierarchy.

Signed-off-by: Jeff Kubascik <jeff.kubascik at dornerworks.com>
---
 bsps/arm/include/bsp/arm-gic-regs.h | 15 +++++++++++++++
 bsps/arm/shared/irq/irq-gicv3.c     | 26 +++++++++++++++++++++++---
 2 files changed, 38 insertions(+), 3 deletions(-)

diff --git a/bsps/arm/include/bsp/arm-gic-regs.h b/bsps/arm/include/bsp/arm-gic-regs.h
index 8a65294b6f..13b715a3b1 100644
--- a/bsps/arm/include/bsp/arm-gic-regs.h
+++ b/bsps/arm/include/bsp/arm-gic-regs.h
@@ -151,6 +151,21 @@ typedef struct {
 #define GIC_DIST_ICDSGIR_SGIINTID(val) BSP_FLD32(val, 0, 3)
 #define GIC_DIST_ICDSGIR_SGIINTID_GET(reg) BSP_FLD32GET(reg, 0, 3)
 #define GIC_DIST_ICDSGIR_SGIINTID_SET(reg, val) BSP_FLD32SET(reg, val, 0, 3)
+  uint32_t reserved_f04[5247];
+  uint64_t icdirr[988];
+#define GIC_DIST_ICDIRR_AFF3(val) BSP_FLD64(val, 32, 39)
+#define GIC_DIST_ICDIRR_AFF3_GET(reg) BSP_FLD64GET(val, 32, 39)
+#define GIC_DIST_ICDIRR_AFF3_SET(reg, val) BSP_FLD64SET(reg, val, 32, 39)
+#define GIC_DIST_ICDIRR_INTERRUPT_ROUTING_MODE BSP_BIT64(31)
+#define GIC_DIST_ICDIRR_AFF2(val) BSP_FLD64(val, 16, 23)
+#define GIC_DIST_ICDIRR_AFF2_GET(reg) BSP_FLD64GET(val, 16, 23)
+#define GIC_DIST_ICDIRR_AFF2_SET(reg, val) BSP_FLD64SET(reg, val, 16, 23)
+#define GIC_DIST_ICDIRR_AFF1(val) BSP_FLD64(val, 8, 15)
+#define GIC_DIST_ICDIRR_AFF1_GET(reg) BSP_FLD64GET(val, 8, 15)
+#define GIC_DIST_ICDIRR_AFF1_SET(reg, val) BSP_FLD64SET(reg, val, 8, 15)
+#define GIC_DIST_ICDIRR_AFF0(val) BSP_FLD64(val, 0, 7)
+#define GIC_DIST_ICDIRR_AFF0_GET(reg) BSP_FLD64GET(val, 0, 7)
+#define GIC_DIST_ICDIRR_AFF0_SET(reg, val) BSP_FLD64SET(reg, val, 0, 7)
 } gic_dist;
 
 /* GICv3 only */
diff --git a/bsps/arm/shared/irq/irq-gicv3.c b/bsps/arm/shared/irq/irq-gicv3.c
index 138b565b9b..1b127fb4a1 100644
--- a/bsps/arm/shared/irq/irq-gicv3.c
+++ b/bsps/arm/shared/irq/irq-gicv3.c
@@ -107,6 +107,12 @@
 #define WRITE64_SR(SR_NAME, VALUE) \
     __asm__ volatile("mcrr    " SR_NAME "  \n" : : "r" (VALUE) );
 
+#define WRITE64_REG(ADDR, VALUE) \
+({ \
+  __asm__ volatile("str %0, [%1, #0]"::"r"(VALUE), "r"(ADDR)); \
+  __asm__ volatile("str %0, [%1, #4]"::"r"(VALUE >> 32), "r"(ADDR)); \
+})
+
 #define ARM_GIC_REDIST ((volatile gic_redist *) BSP_ARM_GIC_REDIST_BASE)
 #define ARM_GIC_SGI_PPI (((volatile gic_sgi_ppi *) ((char*)BSP_ARM_GIC_REDIST_BASE + (1 << 16))))
 
@@ -166,6 +172,16 @@ static inline uint32_t get_id_count(volatile gic_dist *dist)
   return id_count;
 }
 
+static uint64_t cpu_to_affinity(uint32_t cpu)
+{
+  /* CPU logical mapping is not present - keep it simple with a single level
+     routing hierarchy */
+  return (GIC_DIST_ICDIRR_AFF3(0) |
+          GIC_DIST_ICDIRR_AFF2(0) |
+          GIC_DIST_ICDIRR_AFF1(0) |
+          GIC_DIST_ICDIRR_AFF0(cpu));
+}
+
 static void init_cpu_interface(void)
 {
   uint32_t sre_value = 0x7;
@@ -222,7 +238,7 @@ rtems_status_code bsp_interrupt_facility_initialize(void)
   }
 
   for (id = 32; id < id_count; ++id) {
-    gic_id_set_targets(dist, id, 0x01);
+    WRITE64_REG(&dist->icdirr[id-32], cpu_to_affinity(0));
   }
 
   init_cpu_interface();
@@ -293,9 +309,13 @@ void bsp_interrupt_set_affinity(
 )
 {
   volatile gic_dist *dist = ARM_GIC_DIST;
-  uint8_t targets = (uint8_t) _Processor_mask_To_uint32_t(affinity, 0);
+  uint32_t cpu = _Processor_mask_Find_last_set(affinity);
 
-  gic_id_set_targets(dist, vector, targets);
+  if (vector < 32) {
+    /* Affinity doesn't apply to SGIs/PPIs */
+  } else {
+    WRITE64_REG(&dist->icdirr[vector-32], cpu_to_affinity(cpu));
+  }
 }
 
 void bsp_interrupt_get_affinity(
-- 
2.17.1