[PATCH v3 16/16] xen/arm: add SGI handling for GICv3

[vijay.kilari@gmail.com]

From: Vijaya Kumar K <Vijaya.Kumar@caviumnetworks.com>

In ARMv8, write to ICC_SGI1R_EL1 register raises trap to EL2.
Handle the trap and inject SGI to vcpu.

Signed-off-by: Vijaya Kumar K <Vijaya.Kumar@caviumnetworks.com>
---
 xen/arch/arm/traps.c              |    9 +++++
 xen/arch/arm/vgic-v3.c            |   79 +++++++++++++++++++++++++++++++++++++
 xen/include/asm-arm/gic_v3_defs.h |   14 +++++++
 xen/include/asm-arm/sysregs.h     |    2 +-
 xen/include/asm-arm/vgic.h        |    1 +
 5 files changed, 104 insertions(+), 1 deletion(-)

diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index e0184b1..dc1f202 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -40,6 +40,7 @@
 #include "decode.h"
 #include "vtimer.h"
 #include <asm/gic.h>
+#include <asm/vgic.h>
 
 /* The base of the stack must always be double-word aligned, which means
  * that both the kernel half of struct cpu_user_regs (which is pushed in
@@ -1406,6 +1407,14 @@ static void do_sysreg(struct cpu_user_regs *regs,
             domain_crash_synchronous();
         }
         break;
+    case HSR_SYSREG_ICC_SGI1R_EL1:
+        if ( !vgic_emulate(regs, hsr) )
+        {
+            dprintk(XENLOG_ERR,
+                    "failed emulation of 64-bit vgic sysreg access\n");
+            domain_crash_synchronous();
+        }
+        break;
     default:
         printk("%s %d, %d, c%d, c%d, %d %s x%d @ 0x%"PRIregister"\n",
                sysreg.read ? "mrs" : "msr",
diff --git a/xen/arch/arm/vgic-v3.c b/xen/arch/arm/vgic-v3.c
index e3d773a..2bef977 100644
--- a/xen/arch/arm/vgic-v3.c
+++ b/xen/arch/arm/vgic-v3.c
@@ -774,6 +774,85 @@ write_ignore:
     return 1;
 }
 
+static int vgic_to_sgi(struct vcpu *v, register_t sgir)
+{
+    struct domain *d = v->domain;
+    int virq;
+    int irqmode;
+    int vcpuid;
+    int i;
+    unsigned long vcpu_mask = 0;
+
+    ASSERT(d->max_vcpus < 8*sizeof(vcpu_mask));
+
+    irqmode = (sgir >> ICH_SGI_IRQMODE_SHIFT) & ICH_SGI_IRQMODE_MASK;
+    virq = (sgir >> ICH_SGI_IRQ_SHIFT ) & ICH_SGI_IRQ_MASK;
+
+    ASSERT( virq < 16 );
+
+    switch ( irqmode )
+    {
+    case ICH_SGI_TARGET_LIST:
+        vcpu_mask = sgir & ICH_SGI_TARGETLIST_MASK;
+        break;
+    case ICH_SGI_TARGET_OTHERS:
+        for ( i = 0; i < d->max_vcpus; i++ )
+        {
+            if ( i != current->vcpu_id && is_vcpu_running(d, i) )
+                set_bit(i, &vcpu_mask);
+        }
+        break;
+    default:
+        gdprintk(XENLOG_WARNING, "vGICD: unhandled GICD_SGIR write %"PRIregister" with wrong mode\n",
+                 sgir);
+       return 0;
+    }
+
+    for_each_set_bit( vcpuid, &vcpu_mask, d->max_vcpus )
+    {
+        if ( !is_vcpu_running(d, vcpuid) )
+        {
+            gdprintk(XENLOG_WARNING, " write r=%"PRIregister" vcpu_mask=%lx, wrong CPUTargetList\n",
+                     sgir, vcpu_mask);
+            continue;
+        }
+        vgic_vcpu_inject_irq(d->vcpu[vcpuid], virq);
+    }
+    return 1;
+}
+
+static int vgic_emulate_sysreg(struct cpu_user_regs *regs, union hsr hsr)
+{
+    struct vcpu *v = current;
+    struct hsr_sysreg sysreg = hsr.sysreg;
+    register_t *r = select_user_reg(regs, sysreg.reg);
+    switch ( hsr.bits & HSR_SYSREG_REGS_MASK )
+    {
+    case HSR_SYSREG_ICC_SGI1R_EL1:
+        /* WO */
+        if ( !sysreg.read )
+            return vgic_to_sgi(v, *r);
+        else
+        {
+            gdprintk(XENLOG_WARNING, "Reading SGI1R_EL1 - WO register\n");
+            return 0;
+        }
+    default:
+        return 0;
+    }
+}
+
+int vgic_emulate(struct cpu_user_regs *regs, union hsr hsr)
+{
+    switch ( hsr.ec )
+    {
+    case HSR_EC_SYSREG:
+        return vgic_emulate_sysreg(regs, hsr);
+    default:
+        return 0;
+    }
+}
+
 static struct mmio_handler vgic_rdistr_mmio_handler = {
     .read_handler  = vgic_rdistr_mmio_read,
     .write_handler = vgic_rdistr_mmio_write,
diff --git a/xen/include/asm-arm/gic_v3_defs.h b/xen/include/asm-arm/gic_v3_defs.h
index f7f7932..750cd5b 100644
--- a/xen/include/asm-arm/gic_v3_defs.h
+++ b/xen/include/asm-arm/gic_v3_defs.h
@@ -188,6 +188,20 @@
 #define GICH_VMCR_PRIORITY_MASK   0xff
 #define GICH_VMCR_PRIORITY_SHIFT  24
 
+#define ICH_SGIR_AFFINITY_3_SHIFT 48
+#define ICH_SGIR_AFFINITY_3_MASK  0xff
+#define ICH_SGIR_AFFINITY_2_SHIFT 32
+#define ICH_SGIR_AFFINITY_2_MASK  0xff
+#define ICH_SGIR_AFFINITY_1_SHIFT 16
+#define ICH_SGIR_AFFINITY_1_MASK  0xff
+#define ICH_SGI_IRQMODE_SHIFT     40
+#define ICH_SGI_IRQMODE_MASK      0x1
+#define ICH_SGI_TARGET_OTHERS     1
+#define ICH_SGI_TARGET_LIST       0
+#define ICH_SGI_IRQ_SHIFT         24
+#define ICH_SGI_IRQ_MASK          0xf
+#define ICH_SGI_TARGETLIST_MASK   0xffff
+
 #endif /* __ASM_ARM_GIC_V3_DEFS_H__ */
 
 /*
diff --git a/xen/include/asm-arm/sysregs.h b/xen/include/asm-arm/sysregs.h
index 0cee0e9..18e5a45 100644
--- a/xen/include/asm-arm/sysregs.h
+++ b/xen/include/asm-arm/sysregs.h
@@ -56,7 +56,7 @@
 #define HSR_SYSREG_CNTP_CTL_EL0   HSR_SYSREG(3,3,c14,c2,1)
 #define HSR_SYSREG_CNTP_TVAL_EL0  HSR_SYSREG(3,3,c14,c2,0)
 
-
+#define HSR_SYSREG_ICC_SGI1R_EL1  HSR_SYSREG(3,0,c12,c11,5)
 #endif
 
 #endif
diff --git a/xen/include/asm-arm/vgic.h b/xen/include/asm-arm/vgic.h
index 0de74eb..5bc3650 100644
--- a/xen/include/asm-arm/vgic.h
+++ b/xen/include/asm-arm/vgic.h
@@ -89,6 +89,7 @@ extern void vgic_clear_pending_irqs(struct vcpu *v);
 extern int vcpu_vgic_free(struct vcpu *v);
 extern void vgic_disable_irqs(struct vcpu *v, uint32_t r, int n);
 extern void vgic_enable_irqs(struct vcpu *v, uint32_t r, int n);
+extern int vgic_emulate(struct cpu_user_regs *regs, union hsr hsr);
 
 uint32_t byte_read(uint32_t val, int sign, int offset);
 void byte_write(uint32_t *reg, uint32_t var, int offset);
-- 
1.7.9.5