[PATCH v4 23/25] arm: vgic emulation

[<stefano.stabellini@eu.citrix.com>]

From: Stefano Stabellini <stefano.stabellini@eu.citrix.com>

- emulation of the GICD interface for the guest;

- interrupt injection into the guest;

- keep track of inflight irqs using a list, ordered by priority.

Signed-off-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
Signed-off-by: Tim Deegan <Tim.Deegan@citrix.com>
---
 xen/arch/arm/domain.c        |    6 +
 xen/arch/arm/gic.h           |    3 +
 xen/arch/arm/io.c            |    1 +
 xen/arch/arm/io.h            |    2 +
 xen/arch/arm/irq.c           |    3 +-
 xen/arch/arm/vgic.c          |  605 ++++++++++++++++++++++++++++++++++++++++++
 xen/include/asm-arm/domain.h |   30 ++
 7 files changed, 649 insertions(+), 1 deletions(-)
 create mode 100644 xen/arch/arm/vgic.c

diff --git a/xen/arch/arm/domain.c b/xen/arch/arm/domain.c
index 0844b37..7e681ab 100644
--- a/xen/arch/arm/domain.c
+++ b/xen/arch/arm/domain.c
@@ -212,6 +212,9 @@ int vcpu_initialise(struct vcpu *v)
 {
     int rc = 0;
 
+    if ( (rc = vcpu_vgic_init(v)) != 0 )
+        return rc;
+
     return rc;
 }
 
@@ -230,6 +233,9 @@ int arch_domain_create(struct domain *d, unsigned int domcr_flags)
 
     d->max_vcpus = 8;
 
+    if ( (rc = domain_vgic_init(d)) != 0 )
+        goto fail;
+
     rc = 0;
 fail:
     return rc;
diff --git a/xen/arch/arm/gic.h b/xen/arch/arm/gic.h
index 63b6648..81c388d 100644
--- a/xen/arch/arm/gic.h
+++ b/xen/arch/arm/gic.h
@@ -121,6 +121,9 @@
 #define GICH_LR_CPUID_SHIFT     9
 #define GICH_VTR_NRLRGS         0x3f
 
+extern int domain_vgic_init(struct domain *d);
+extern int vcpu_vgic_init(struct vcpu *v);
+extern void vgic_vcpu_inject_irq(struct vcpu *v, unsigned int irq,int virtual);
 extern struct pending_irq *irq_to_pending(struct vcpu *v, unsigned int irq);
 
 extern void gic_route_irqs(void);
diff --git a/xen/arch/arm/io.c b/xen/arch/arm/io.c
index 8789705..4461225 100644
--- a/xen/arch/arm/io.c
+++ b/xen/arch/arm/io.c
@@ -24,6 +24,7 @@
 
 static const struct mmio_handler *const mmio_handlers[] =
 {
+    &vgic_distr_mmio_handler,
 };
 #define MMIO_HANDLER_NR ARRAY_SIZE(mmio_handlers)
 
diff --git a/xen/arch/arm/io.h b/xen/arch/arm/io.h
index d7847e3..8cc5ca7 100644
--- a/xen/arch/arm/io.h
+++ b/xen/arch/arm/io.h
@@ -39,6 +39,8 @@ struct mmio_handler {
     mmio_write_t write_handler;
 };
 
+extern const struct mmio_handler vgic_distr_mmio_handler;
+
 extern int handle_mmio(mmio_info_t *info);
 
 #endif
diff --git a/xen/arch/arm/irq.c b/xen/arch/arm/irq.c
index 5663762..7820310 100644
--- a/xen/arch/arm/irq.c
+++ b/xen/arch/arm/irq.c
@@ -136,7 +136,8 @@ void do_IRQ(struct cpu_user_regs *regs, unsigned int irq, int is_fiq)
 
         desc->status |= IRQ_INPROGRESS;
 
-        /* XXX: inject irq into the guest */
+        /* XXX: inject irq into all guest vcpus */
+        vgic_vcpu_inject_irq(d->vcpu[0], irq, 0);
         goto out_no_end;
     }
 
diff --git a/xen/arch/arm/vgic.c b/xen/arch/arm/vgic.c
new file mode 100644
index 0000000..26eae55
--- /dev/null
+++ b/xen/arch/arm/vgic.c
@@ -0,0 +1,605 @@
+/*
+ * xen/arch/arm/vgic.c
+ *
+ * ARM Virtual Generic Interrupt Controller support
+ *
+ * Ian Campbell <ian.campbell@citrix.com>
+ * Copyright (c) 2011 Citrix Systems.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <xen/config.h>
+#include <xen/lib.h>
+#include <xen/init.h>
+#include <xen/softirq.h>
+#include <xen/irq.h>
+#include <xen/sched.h>
+
+#include <asm/current.h>
+
+#include "io.h"
+#include "gic.h"
+
+#define VGIC_DISTR_BASE_ADDRESS 0x000000002c001000
+
+#define REG(n) (n/4)
+
+/* Number of ranks of interrupt registers for a domain */
+#define DOMAIN_NR_RANKS(d) (((d)->arch.vgic.nr_lines+31)/32)
+
+/*
+ * Rank containing GICD_<FOO><n> for GICD_<FOO> with
+ * <b>-bits-per-interrupt
+ */
+static inline int REG_RANK_NR(int b, uint32_t n)
+{
+    switch ( b )
+    {
+    case 8: return n >> 3;
+    case 4: return n >> 2;
+    case 2: return n >> 1;
+    default: BUG();
+    }
+}
+
+/*
+ * Offset of GICD_<FOO><n> with its rank, for GICD_<FOO> with
+ * <b>-bits-per-interrupt.
+ */
+#define REG_RANK_INDEX(b, n) ((n) & ((b)-1))
+
+/*
+ * Returns rank corresponding to a GICD_<FOO><n> register for
+ * GICD_<FOO> with <b>-bits-per-interrupt.
+ */
+static struct vgic_irq_rank *vgic_irq_rank(struct vcpu *v, int b, int n)
+{
+    int rank = REG_RANK_NR(b, n);
+
+    if ( rank == 0 )
+        return &v->arch.vgic.private_irqs;
+    else if ( rank <= DOMAIN_NR_RANKS(v->domain) )
+        return &v->domain->arch.vgic.shared_irqs[rank - 1];
+    else
+        return NULL;
+}
+
+int domain_vgic_init(struct domain *d)
+{
+    int i;
+
+    d->arch.vgic.ctlr = 0;
+    d->arch.vgic.nr_lines = 32;
+    d->arch.vgic.shared_irqs =
+        xmalloc_array(struct vgic_irq_rank, DOMAIN_NR_RANKS(d));
+    d->arch.vgic.pending_irqs =
+        xmalloc_array(struct pending_irq,
+                d->arch.vgic.nr_lines + (32 * d->max_vcpus));
+    for (i=0; i<d->arch.vgic.nr_lines + (32 * d->max_vcpus); i++)
+        INIT_LIST_HEAD(&d->arch.vgic.pending_irqs[i].link);
+    for (i=0; i<DOMAIN_NR_RANKS(d); i++)
+        spin_lock_init(&d->arch.vgic.shared_irqs[i].lock);
+    return 0;
+}
+
+int vcpu_vgic_init(struct vcpu *v)
+{
+    int i;
+    memset(&v->arch.vgic.private_irqs, 0, sizeof(v->arch.vgic.private_irqs));
+
+    spin_lock_init(&v->arch.vgic.private_irqs.lock);
+
+    /* For SGI and PPI the target is always this CPU */
+    for ( i = 0 ; i < 8 ; i++ )
+        v->arch.vgic.private_irqs.itargets[i] =
+              (1<<(v->vcpu_id+0))
+            | (1<<(v->vcpu_id+8))
+            | (1<<(v->vcpu_id+16))
+            | (1<<(v->vcpu_id+24));
+    INIT_LIST_HEAD(&v->arch.vgic.inflight_irqs);
+    spin_lock_init(&v->arch.vgic.lock);
+
+    return 0;
+}
+
+#define vgic_lock(v)   spin_lock(&(v)->domain->arch.vgic.lock)
+#define vgic_unlock(v) spin_unlock(&(v)->domain->arch.vgic.lock)
+
+#define vgic_lock_rank(v, r) spin_lock(&(r)->lock)
+#define vgic_unlock_rank(v, r) spin_unlock(&(r)->lock)
+
+static uint32_t byte_read(uint32_t val, int sign, int offset)
+{
+    int byte = offset & 0x3;
+
+    val = val >> (8*byte);
+    if ( sign && (val & 0x80) )
+        val |= 0xffffff00;
+    else
+        val &= 0x000000ff;
+    return val;
+}
+
+static void byte_write(uint32_t *reg, uint32_t var, int offset)
+{
+    int byte = offset & 0x3;
+
+    var &= (0xff << (8*byte));
+
+    *reg &= ~(0xff << (8*byte));
+    *reg |= var;
+}
+
+static int vgic_distr_mmio_read(struct vcpu *v, mmio_info_t *info)
+{
+    struct hsr_dabt dabt = info->dabt;
+    struct cpu_user_regs *regs = guest_cpu_user_regs();
+    uint32_t *r = &regs->r0 + dabt.reg;
+    struct vgic_irq_rank *rank;
+    int offset = (int)(info->gpa - VGIC_DISTR_BASE_ADDRESS);
+    int gicd_reg = REG(offset);
+
+    switch ( gicd_reg )
+    {
+    case GICD_CTLR:
+        if ( dabt.size != 2 ) goto bad_width;
+        vgic_lock(v);
+        *r = v->domain->arch.vgic.ctlr;
+        vgic_unlock(v);
+        return 1;
+    case GICD_TYPER:
+        if ( dabt.size != 2 ) goto bad_width;
+        /* No secure world support for guests. */
+        vgic_lock(v);
+        *r = ( (v->domain->max_vcpus<<5) & GICD_TYPE_CPUS )
+            |( ((v->domain->arch.vgic.nr_lines/32)) & GICD_TYPE_LINES );
+        vgic_unlock(v);
+        return 1;
+    case GICD_IIDR:
+        if ( dabt.size != 2 ) goto bad_width;
+        /*
+         * XXX Do we need a JEP106 manufacturer ID?
+         * Just use the physical h/w value for now
+         */
+        *r = 0x0000043b;
+        return 1;
+
+    /* Implementation defined -- read as zero */
+    case REG(0x020) ... REG(0x03c):
+        goto read_as_zero;
+
+    case GICD_IGROUPR ... GICD_IGROUPRN:
+        /* We do not implement security extensions for guests, read zero */
+        goto read_as_zero;
+
+    case GICD_ISENABLER ... GICD_ISENABLERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ISENABLER);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = rank->ienable;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICENABLER ... GICD_ICENABLERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ICENABLER);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = rank->ienable;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ISPENDR ... GICD_ISPENDRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ISPENDR);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = byte_read(rank->ipend, dabt.sign, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICPENDR ... GICD_ICPENDRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ICPENDR);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = byte_read(rank->ipend, dabt.sign, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ISACTIVER ... GICD_ISACTIVERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ISACTIVER);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = rank->iactive;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICACTIVER ... GICD_ICACTIVERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ICACTIVER);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = rank->iactive;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ITARGETSR ... GICD_ITARGETSRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ITARGETSR);
+        if ( rank == NULL) goto read_as_zero;
+
+        vgic_lock_rank(v, rank);
+        *r = rank->itargets[REG_RANK_INDEX(8, gicd_reg - GICD_ITARGETSR)];
+        if ( dabt.size == 0 )
+            *r = byte_read(*r, dabt.sign, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_IPRIORITYR ... GICD_IPRIORITYRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_IPRIORITYR);
+        if ( rank == NULL) goto read_as_zero;
+
+        vgic_lock_rank(v, rank);
+        *r = rank->ipriority[REG_RANK_INDEX(8, gicd_reg - GICD_IPRIORITYR)];
+        if ( dabt.size == 0 )
+            *r = byte_read(*r, dabt.sign, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICFGR ... GICD_ICFGRN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 2, gicd_reg - GICD_ICFGR);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = rank->icfg[REG_RANK_INDEX(2, gicd_reg - GICD_ICFGR)];
+        vgic_unlock_rank(v, rank);
+        return 0;
+
+    case GICD_NSACR ... GICD_NSACRN:
+        /* We do not implement securty extensions for guests, read zero */
+        goto read_as_zero;
+
+    case GICD_SGIR:
+        if ( dabt.size != 2 ) goto bad_width;
+        /* Write only -- read unknown */
+        *r = 0xdeadbeef;
+        return 1;
+
+    case GICD_CPENDSGIR ... GICD_CPENDSGIRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_CPENDSGIR);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = byte_read(rank->pendsgi, dabt.sign, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_SPENDSGIR ... GICD_SPENDSGIRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_SPENDSGIR);
+        if ( rank == NULL) goto read_as_zero;
+        vgic_lock_rank(v, rank);
+        *r = byte_read(rank->pendsgi, dabt.sign, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    /* Implementation defined -- read as zero */
+    case REG(0xfd0) ... REG(0xfe4):
+        goto read_as_zero;
+
+    case GICD_ICPIDR2:
+        if ( dabt.size != 2 ) goto bad_width;
+        printk("vGICD: unhandled read from ICPIDR2\n");
+        return 0;
+
+    /* Implementation defined -- read as zero */
+    case REG(0xfec) ... REG(0xffc):
+        goto read_as_zero;
+
+    /* Reserved -- read as zero */
+    case REG(0x00c) ... REG(0x01c):
+    case REG(0x040) ... REG(0x07c):
+    case REG(0x7fc):
+    case REG(0xbfc):
+    case REG(0xf04) ... REG(0xf0c):
+    case REG(0xf30) ... REG(0xfcc):
+        goto read_as_zero;
+
+    default:
+        printk("vGICD: unhandled read r%d offset %#08x\n",
+               dabt.reg, offset);
+        return 0;
+    }
+
+bad_width:
+    printk("vGICD: bad read width %d r%d offset %#08x\n",
+           dabt.size, dabt.reg, offset);
+    domain_crash_synchronous();
+    return 0;
+
+read_as_zero:
+    if ( dabt.size != 2 ) goto bad_width;
+    *r = 0;
+    return 1;
+}
+
+static int vgic_distr_mmio_write(struct vcpu *v, mmio_info_t *info)
+{
+    struct hsr_dabt dabt = info->dabt;
+    struct cpu_user_regs *regs = guest_cpu_user_regs();
+    uint32_t *r = &regs->r0 + dabt.reg;
+    struct vgic_irq_rank *rank;
+    int offset = (int)(info->gpa - VGIC_DISTR_BASE_ADDRESS);
+    int gicd_reg = REG(offset);
+
+    switch ( gicd_reg )
+    {
+    case GICD_CTLR:
+        if ( dabt.size != 2 ) goto bad_width;
+        /* Ignore all but the enable bit */
+        v->domain->arch.vgic.ctlr = (*r) & GICD_CTL_ENABLE;
+        return 1;
+
+    /* R/O -- write ignored */
+    case GICD_TYPER:
+    case GICD_IIDR:
+        goto write_ignore;
+
+    /* Implementation defined -- write ignored */
+    case REG(0x020) ... REG(0x03c):
+        goto write_ignore;
+
+    case GICD_IGROUPR ... GICD_IGROUPRN:
+        /* We do not implement securty extensions for guests, write ignore */
+        goto write_ignore;
+
+    case GICD_ISENABLER ... GICD_ISENABLERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ISENABLER);
+        if ( rank == NULL) goto write_ignore;
+        vgic_lock_rank(v, rank);
+        rank->ienable |= *r;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICENABLER ... GICD_ICENABLERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ICENABLER);
+        if ( rank == NULL) goto write_ignore;
+        vgic_lock_rank(v, rank);
+        rank->ienable &= ~*r;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ISPENDR ... GICD_ISPENDRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        printk("vGICD: unhandled %s write %#"PRIx32" to ISPENDR%d\n",
+               dabt.size ? "word" : "byte", *r, gicd_reg - GICD_ISPENDR);
+        return 0;
+
+    case GICD_ICPENDR ... GICD_ICPENDRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        printk("vGICD: unhandled %s write %#"PRIx32" to ICPENDR%d\n",
+               dabt.size ? "word" : "byte", *r, gicd_reg - GICD_ICPENDR);
+        return 0;
+
+    case GICD_ISACTIVER ... GICD_ISACTIVERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ISACTIVER);
+        if ( rank == NULL) goto write_ignore;
+        vgic_lock_rank(v, rank);
+        rank->iactive &= ~*r;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICACTIVER ... GICD_ICACTIVERN:
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ICACTIVER);
+        if ( rank == NULL) goto write_ignore;
+        vgic_lock_rank(v, rank);
+        rank->iactive &= ~*r;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ITARGETSR ... GICD_ITARGETSR + 7:
+        /* SGI/PPI target is read only */
+        goto write_ignore;
+
+    case GICD_ITARGETSR + 8 ... GICD_ITARGETSRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_ITARGETSR);
+        if ( rank == NULL) goto write_ignore;
+        vgic_lock_rank(v, rank);
+        if ( dabt.size == 2 )
+            rank->itargets[REG_RANK_INDEX(8, gicd_reg - GICD_ITARGETSR)] = *r;
+        else
+            byte_write(&rank->itargets[REG_RANK_INDEX(8, gicd_reg - GICD_ITARGETSR)],
+                       *r, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_IPRIORITYR ... GICD_IPRIORITYRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 8, gicd_reg - GICD_IPRIORITYR);
+        if ( rank == NULL) goto write_ignore;
+        vgic_lock_rank(v, rank);
+        if ( dabt.size == 2 )
+            rank->ipriority[REG_RANK_INDEX(8, gicd_reg - GICD_IPRIORITYR)] = *r;
+        else
+            byte_write(&rank->ipriority[REG_RANK_INDEX(8, gicd_reg - GICD_IPRIORITYR)],
+                       *r, offset);
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_ICFGR: /* SGIs */
+        goto write_ignore;
+    case GICD_ICFGR + 1: /* PPIs */
+        /* It is implementation defined if these are writeable. We chose not */
+        goto write_ignore;
+    case GICD_ICFGR + 2 ... GICD_ICFGRN: /* SPIs */
+        if ( dabt.size != 2 ) goto bad_width;
+        rank = vgic_irq_rank(v, 2, gicd_reg - GICD_ICFGR);
+        vgic_lock_rank(v, rank);
+        if ( rank == NULL) goto write_ignore;
+        rank->icfg[REG_RANK_INDEX(2, gicd_reg - GICD_ICFGR)] = *r;
+        vgic_unlock_rank(v, rank);
+        return 1;
+
+    case GICD_NSACR ... GICD_NSACRN:
+        /* We do not implement securty extensions for guests, write ignore */
+        goto write_ignore;
+
+    case GICD_SGIR:
+        if ( dabt.size != 2 ) goto bad_width;
+        printk("vGICD: unhandled write %#"PRIx32" to ICFGR%d\n",
+               *r, gicd_reg - GICD_ICFGR);
+        return 0;
+
+    case GICD_CPENDSGIR ... GICD_CPENDSGIRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        printk("vGICD: unhandled %s write %#"PRIx32" to ICPENDSGIR%d\n",
+               dabt.size ? "word" : "byte", *r, gicd_reg - GICD_CPENDSGIR);
+        return 0;
+
+    case GICD_SPENDSGIR ... GICD_SPENDSGIRN:
+        if ( dabt.size != 0 && dabt.size != 2 ) goto bad_width;
+        printk("vGICD: unhandled %s write %#"PRIx32" to ISPENDSGIR%d\n",
+               dabt.size ? "word" : "byte", *r, gicd_reg - GICD_SPENDSGIR);
+        return 0;
+
+    /* Implementation defined -- write ignored */
+    case REG(0xfd0) ... REG(0xfe4):
+        goto write_ignore;
+
+    /* R/O -- write ignore */
+    case GICD_ICPIDR2:
+        goto write_ignore;
+
+    /* Implementation defined -- write ignored */
+    case REG(0xfec) ... REG(0xffc):
+        goto write_ignore;
+
+    /* Reserved -- write ignored */
+    case REG(0x00c) ... REG(0x01c):
+    case REG(0x040) ... REG(0x07c):
+    case REG(0x7fc):
+    case REG(0xbfc):
+    case REG(0xf04) ... REG(0xf0c):
+    case REG(0xf30) ... REG(0xfcc):
+        goto write_ignore;
+
+    default:
+        printk("vGICD: unhandled write r%d=%"PRIx32" offset %#08x\n",
+               dabt.reg, *r, offset);
+        return 0;
+    }
+
+bad_width:
+    printk("vGICD: bad write width %d r%d=%"PRIx32" offset %#08x\n",
+           dabt.size, dabt.reg, *r, offset);
+    domain_crash_synchronous();
+    return 0;
+
+write_ignore:
+    if ( dabt.size != 2 ) goto bad_width;
+    return 0;
+}
+
+static int vgic_distr_mmio_check(struct vcpu *v, paddr_t addr)
+{
+    return addr >= VGIC_DISTR_BASE_ADDRESS && addr < (VGIC_DISTR_BASE_ADDRESS+PAGE_SIZE);
+}
+
+const struct mmio_handler vgic_distr_mmio_handler = {
+    .check_handler = vgic_distr_mmio_check,
+    .read_handler  = vgic_distr_mmio_read,
+    .write_handler = vgic_distr_mmio_write,
+};
+
+struct pending_irq *irq_to_pending(struct vcpu *v, unsigned int irq)
+{
+    struct pending_irq *n;
+    /* Pending irqs allocation strategy: the first vgic.nr_lines irqs
+     * are used for SPIs; the rests are used for per cpu irqs */
+    if ( irq < 32 )
+        n = &v->domain->arch.vgic.pending_irqs[irq + (v->vcpu_id * 32)
+            + v->domain->arch.vgic.nr_lines];
+    else
+        n = &v->domain->arch.vgic.pending_irqs[irq - 32];
+    return n;
+}
+
+void vgic_vcpu_inject_irq(struct vcpu *v, unsigned int irq, int virtual)
+{
+    int idx = irq >> 2, byte = irq & 0x3;
+    uint8_t priority;
+    struct vgic_irq_rank *rank = vgic_irq_rank(v, 8, idx);
+    struct pending_irq *iter, *n = irq_to_pending(v, irq);
+
+    /* irq still pending */
+    if (!list_empty(&n->link))
+        return;
+
+    priority = byte_read(rank->ipriority[REG_RANK_INDEX(8, idx)], 0, byte);
+
+    n->irq = irq;
+    n->priority = priority;
+    if (!virtual)
+        n->desc = irq_to_desc(irq);
+    else
+        n->desc = NULL;
+
+    gic_set_guest_irq(irq, GICH_LR_PENDING, priority);
+
+    spin_lock(&v->arch.vgic.lock);
+    list_for_each_entry ( iter, &v->arch.vgic.inflight_irqs, link )
+    {
+        if ( iter->priority < priority )
+        {
+            list_add_tail(&n->link, &iter->link);
+            spin_unlock(&v->arch.vgic.lock);
+            return;
+        }
+    }
+    list_add(&n->link, &v->arch.vgic.inflight_irqs);
+    spin_unlock(&v->arch.vgic.lock);
+    /* we have a new higher priority irq, inject it into the guest */
+    cpu_raise_softirq(v->processor, VGIC_SOFTIRQ);
+}
+
+static void vgic_softirq(void)
+{
+    if (list_empty(&current->arch.vgic.inflight_irqs))
+        return;
+
+    gic_inject_irq_start();
+}
+
+static int __init init_vgic_softirq(void)
+{
+    open_softirq(VGIC_SOFTIRQ, vgic_softirq);
+    return 0;
+}
+__initcall(init_vgic_softirq);
+/*
+ * Local variables:
+ * mode: C
+ * c-set-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
+
diff --git a/xen/include/asm-arm/domain.h b/xen/include/asm-arm/domain.h
index 2226a24..2cd0bd3 100644
--- a/xen/include/asm-arm/domain.h
+++ b/xen/include/asm-arm/domain.h
@@ -6,6 +6,15 @@
 #include <asm/page.h>
 #include <asm/p2m.h>
 
+/* Represents state corresponding to a block of 32 interrupts */
+struct vgic_irq_rank {
+    spinlock_t lock; /* Covers access to all other members of this struct */
+    uint32_t ienable, iactive, ipend, pendsgi;
+    uint32_t icfg[2];
+    uint32_t ipriority[8];
+    uint32_t itargets[8];
+};
+
 struct pending_irq
 {
     int irq;
@@ -18,6 +27,22 @@ struct arch_domain
 {
     struct p2m_domain p2m;
 
+    struct {
+        /*
+         * Covers access to other members of this struct _except_ for
+         * shared_irqs where each member contains its own locking.
+         *
+         * If both class of lock is required then this lock must be
+         * taken first. If multiple rank locks are required (including
+         * the per-vcpu private_irqs rank) then they must be taken in
+         * rank order.
+         */
+        spinlock_t lock;
+        int ctlr;
+        int nr_lines;
+        struct vgic_irq_rank *shared_irqs;
+        struct pending_irq *pending_irqs;
+    } vgic;
 }  __cacheline_aligned;
 
 struct arch_vcpu
@@ -27,6 +52,11 @@ struct arch_vcpu
     uint32_t sctlr;
     uint32_t ttbr0, ttbr1, ttbcr;
 
+    struct {
+        struct vgic_irq_rank private_irqs;
+        struct list_head inflight_irqs;
+        spinlock_t lock;
+    } vgic;
 }  __cacheline_aligned;
 
 void vcpu_show_execution_state(struct vcpu *);
-- 
1.7.2.5