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

[Kinsey Moore]

This patch looks good to me.

Kinsey Moore

> -----Original Message-----
> From: Jeff Kubascik <jeff.kubascik at dornerworks.com>
> Sent: Thursday, January 23, 2020 08:49
> To: devel at rtems.org
> Cc: Kinsey Moore <kinsey.moore at oarcorp.com>; Joel Sherrill
> <joel.sherrill at oarcorp.com>
> Subject: [PATCH] bsps/arm: Update GICv3 to use newer affinity interface
> 
> 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