[PATCH v6 4/5] 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              |   30 +++++++++++++++++
 xen/arch/arm/vgic-v3.c            |   65 +++++++++++++++++++++++++++++++++----
 xen/include/asm-arm/gic_v3_defs.h |    7 ++++
 xen/include/asm-arm/sysregs.h     |    3 ++
 xen/include/asm-arm/vgic.h        |    1 +
 5 files changed, 100 insertions(+), 6 deletions(-)

diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index 686d8b7..adc316b 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -41,6 +41,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
@@ -496,6 +497,18 @@ static void inject_dabt_exception(struct cpu_user_regs *regs,
 #endif
 }
 
+static void inject_undef_exception(struct cpu_user_regs *regs,
+                                   register_t addr,
+                                   int instr_len)
+{
+    if ( is_32bit_domain(current->domain) )
+        inject_undef32_exception(regs);
+#ifdef CONFIG_ARM_64
+    else
+        inject_undef64_exception(regs, instr_len);
+#endif
+}
+
 struct reg_ctxt {
     /* Guest-side state */
     uint32_t sctlr_el1;
@@ -1591,6 +1604,7 @@ static void do_sysreg(struct cpu_user_regs *regs,
                       union hsr hsr)
 {
     register_t *x = select_user_reg(regs, hsr.sysreg.reg);
+    register_t addr;
 
     switch ( hsr.bits & HSR_SYSREG_REGS_MASK )
     {
@@ -1641,6 +1655,22 @@ static void do_sysreg(struct cpu_user_regs *regs,
             domain_crash_synchronous();
         }
         break;
+    case HSR_SYSREG_ICC_SGI1R_EL1:
+        if ( !vgic_emulate(regs, hsr) )
+        {
+            addr = READ_SYSREG64(FAR_EL2);
+            dprintk(XENLOG_WARNING,
+                    "failed emulation of sysreg ICC_SGI1R_EL1 access\n");
+            inject_undef_exception(regs, addr, hsr.len);
+        }
+        break;
+    case HSR_SYSREG_ICC_SGI0R_EL1:
+    case HSR_SYSREG_ICC_ASGI1R_EL1:
+        /* TBD: Implement to support secure grp0/1 SGI forwarding */
+        dprintk(XENLOG_WARNING,
+                "Emulation of sysreg ICC_SGI0R_EL1/ASGI1R_EL1 not supported\n");
+        addr = READ_SYSREG64(FAR_EL2);
+        inject_undef_exception(regs, addr, hsr.len);
     default:
  bad_sysreg:
         {
diff --git a/xen/arch/arm/vgic-v3.c b/xen/arch/arm/vgic-v3.c
index 63948ed..7beed17 100644
--- a/xen/arch/arm/vgic-v3.c
+++ b/xen/arch/arm/vgic-v3.c
@@ -35,7 +35,7 @@
 #include <asm/vgic.h>
 
 static int __vgic_v3_rdistr_rd_mmio_read(struct vcpu *v, mmio_info_t *info,
-                                        uint32_t gicr_reg)
+                                         uint32_t gicr_reg)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
@@ -141,7 +141,7 @@ read_as_zero:
 }
 
 static int __vgic_v3_rdistr_rd_mmio_write(struct vcpu *v, mmio_info_t *info,
-                                    uint32_t gicr_reg)
+                                          uint32_t gicr_reg)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
@@ -214,7 +214,7 @@ write_ignore:
 }
 
 static int __vgic_v3_distr_common_mmio_read(struct vcpu *v, mmio_info_t *info,
-                                           uint32_t reg)
+                                            uint32_t reg)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
@@ -313,7 +313,7 @@ read_as_zero:
 }
 
 static int __vgic_v3_distr_common_mmio_write(struct vcpu *v, mmio_info_t *info,
-                                            uint32_t reg)
+                                             uint32_t reg)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
@@ -423,7 +423,7 @@ write_ignore:
 }
 
 static int vgic_v3_rdistr_sgi_mmio_read(struct vcpu *v, mmio_info_t *info,
-                                     uint32_t gicr_reg)
+                                        uint32_t gicr_reg)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
@@ -484,7 +484,7 @@ read_as_zero:
 }
 
 static int vgic_v3_rdistr_sgi_mmio_write(struct vcpu *v, mmio_info_t *info,
-                                      uint32_t gicr_reg)
+                                         uint32_t gicr_reg)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
@@ -835,6 +835,57 @@ write_ignore_64:
     return 1;
 }
 
+static int vgicv3_to_sgi(struct vcpu *v, register_t sgir)
+{
+    int virq;
+    int irqmode;
+    enum gic_sgi_mode sgi_mode;
+    unsigned long vcpu_mask = 0;
+
+    irqmode = (sgir >> ICH_SGI_IRQMODE_SHIFT) & ICH_SGI_IRQMODE_MASK;
+    virq = (sgir >> ICH_SGI_IRQ_SHIFT ) & ICH_SGI_IRQ_MASK;
+    vcpu_mask = sgir & ICH_SGI_TARGETLIST_MASK;
+
+    /* Map GIC sgi value to enum value */
+    switch ( irqmode )
+    {
+    case ICH_SGI_TARGET_LIST:
+        sgi_mode = SGI_TARGET_LIST;
+        break;
+    case ICH_SGI_TARGET_OTHERS:
+        sgi_mode = SGI_TARGET_OTHERS;
+        break;
+    default:
+        BUG();
+    }
+
+    return vgic_to_sgi(v, sgir, sgi_mode, virq, vcpu_mask);
+}
+
+int vgic_emulate(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);
+
+    ASSERT (hsr.ec == HSR_EC_SYSREG);
+
+    switch ( hsr.bits & HSR_SYSREG_REGS_MASK )
+    {
+    case HSR_SYSREG_ICC_SGI1R_EL1:
+        /* WO */
+        if ( !sysreg.read )
+            return vgic_send_sgi(v, *r);
+        else
+        {
+            gdprintk(XENLOG_WARNING, "Reading SGI1R_EL1 - WO register\n");
+            return 0;
+        }
+    default:
+        return 0;
+    }
+}
+
 static const struct mmio_handler_ops vgic_rdistr_mmio_handler = {
     .read_handler  = vgic_v3_rdistr_mmio_read,
     .write_handler = vgic_v3_rdistr_mmio_write,
@@ -866,6 +917,7 @@ static int vgicv3_domain_init(struct domain *d)
 {
     int i;
 
+     /* We rely on gicv init to get dbase and size */
     register_mmio_handler(d, &vgic_distr_mmio_handler, d->arch.vgic.dbase,
                           d->arch.vgic.dbase_size);
 
@@ -884,6 +936,7 @@ static int vgicv3_domain_init(struct domain *d)
 static const struct vgic_ops v3_ops = {
     .vcpu_init   = vgicv3_vcpu_init,
     .domain_init = vgicv3_domain_init,
+    .send_sgi    = vgicv3_to_sgi,
 };
 
 int vgic_v3_init(struct domain *d)
diff --git a/xen/include/asm-arm/gic_v3_defs.h b/xen/include/asm-arm/gic_v3_defs.h
index 1c32e0c..b94ba20 100644
--- a/xen/include/asm-arm/gic_v3_defs.h
+++ b/xen/include/asm-arm/gic_v3_defs.h
@@ -147,6 +147,13 @@
 #define GICH_VMCR_PRIORITY_MASK      0xff
 #define GICH_VMCR_PRIORITY_SHIFT     24
 
+#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 b00871c..48ea498 100644
--- a/xen/include/asm-arm/sysregs.h
+++ b/xen/include/asm-arm/sysregs.h
@@ -78,6 +78,9 @@
 #define HSR_SYSREG_PMINTENCLR_EL1 HSR_SYSREG(3,0,c9,c14,2)
 #define HSR_SYSREG_MAIR_EL1       HSR_SYSREG(3,0,c10,c2,0)
 #define HSR_SYSREG_AMAIR_EL1      HSR_SYSREG(3,0,c10,c3,0)
+#define HSR_SYSREG_ICC_SGI1R_EL1  HSR_SYSREG(3,0,c12,c11,5)
+#define HSR_SYSREG_ICC_ASGI1R_EL1 HSR_SYSREG(3,1,c12,c11,6)
+#define HSR_SYSREG_ICC_SGI0R_EL1  HSR_SYSREG(3,2,c12,c11,7)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)
diff --git a/xen/include/asm-arm/vgic.h b/xen/include/asm-arm/vgic.h
index 2fc6b72..fa30616 100644
--- a/xen/include/asm-arm/vgic.h
+++ b/xen/include/asm-arm/vgic.h
@@ -167,6 +167,7 @@ extern void vgic_vcpu_inject_irq(struct vcpu *v, unsigned int irq);
 extern void vgic_clear_pending_irqs(struct vcpu *v);
 extern struct pending_irq *irq_to_pending(struct vcpu *v, unsigned int irq);
 extern struct vgic_irq_rank *vgic_rank_offset(struct vcpu *v, int b, int n, int s);
+extern int vgic_emulate(struct cpu_user_regs *regs, union hsr hsr);
 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 void register_vgic_ops(struct domain *d, const struct vgic_ops *ops);
-- 
1.7.9.5