[PATCH v4 07/17] xen/arm: ITS: Add virtual ITS commands support

[vijay.kilari@gmail.com]

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

Add Virtual ITS command processing support to Virtual ITS driver

Signed-off-by: Vijaya Kumar K <Vijaya.Kumar@caviumnetworks.com>
---
v4: - Use helper function to read from command queue
    - Add MOVALL
    - Removed check for entry in device in domain RB-tree
---
 xen/arch/arm/gic-v3-its.c     |    7 +
 xen/arch/arm/vgic-v3-its.c    |  391 +++++++++++++++++++++++++++++++++++++++++
 xen/include/asm-arm/gic-its.h |   19 ++
 xen/include/asm-arm/gic.h     |    1 +
 4 files changed, 418 insertions(+)

diff --git a/xen/arch/arm/gic-v3-its.c b/xen/arch/arm/gic-v3-its.c
index b98d396..9161053 100644
--- a/xen/arch/arm/gic-v3-its.c
+++ b/xen/arch/arm/gic-v3-its.c
@@ -91,6 +91,7 @@ static LIST_HEAD(its_nodes);
 static DEFINE_SPINLOCK(its_lock);
 static struct rdist_prop  *gic_rdists;
 static struct rb_root rb_its_dev;
+static struct gic_its_info its_data;
 
 #define gic_data_rdist()    (per_cpu(rdist, smp_processor_id()))
 
@@ -102,6 +103,11 @@ void dump_cmd(its_cmd_block *cmd)
 }
 #endif
 
+u32 its_get_nr_events(void)
+{
+    return (1 << its_data.id_bits);
+}
+
 /* RB-tree helpers for its_device */
 struct its_device *its_find_device(u32 devid)
 {
@@ -940,6 +946,7 @@ static int its_probe(struct dt_device_node *node)
     its->phys_size = its_size;
     typer = readl_relaxed(its_base + GITS_TYPER);
     its->ite_size = ((typer >> 4) & 0xf) + 1;
+    its_data.id_bits = GITS_TYPER_IDBITS(typer);
 
     its->cmd_base = xzalloc_bytes(ITS_CMD_QUEUE_SZ);
     if ( !its->cmd_base )
diff --git a/xen/arch/arm/vgic-v3-its.c b/xen/arch/arm/vgic-v3-its.c
index c63f478..af2bacd 100644
--- a/xen/arch/arm/vgic-v3-its.c
+++ b/xen/arch/arm/vgic-v3-its.c
@@ -31,6 +31,22 @@
 #include <asm/gic-its.h>
 #include <xen/log2.h>
 
+#define DEBUG_ITS
+
+#ifdef DEBUG_ITS
+# define DPRINTK(fmt, args...) dprintk(XENLOG_DEBUG, fmt, ##args)
+#else
+# define DPRINTK(fmt, args...) do {} while ( 0 )
+#endif
+
+#ifdef DEBUG_ITS
+static void dump_cmd(its_cmd_block *cmd)
+{
+    printk("CMD[0] = 0x%lx CMD[1] = 0x%lx CMD[2] = 0x%lx CMD[3] = 0x%lx\n",
+           cmd->bits[0], cmd->bits[1], cmd->bits[2], cmd->bits[3]);
+}
+#endif
+
 static int vits_entry(struct domain *d, paddr_t entry, void *addr,
                       uint32_t size, bool_t set)
 {
@@ -202,6 +218,381 @@ void vits_remove_device(struct rb_root *root, struct vits_device *dev)
         rb_erase(&dev->node, root);
 }
 
+static int vgic_its_process_sync(struct vcpu *v, struct vgic_its *vits,
+                                 its_cmd_block *virt_cmd)
+{
+    DPRINTK("%pv: vITS: SYNC: ta 0x%x \n", v, virt_cmd->sync.ta);
+
+    return 0;
+}
+
+static int vgic_its_process_mapvi(struct vcpu *v, struct vgic_its *vits,
+                                  its_cmd_block *virt_cmd)
+{
+    struct vitt entry;
+    struct domain *d = v->domain;
+    uint8_t vcol_id, cmd;
+    uint32_t vid, dev_id, event;
+
+    vcol_id = virt_cmd->mapvi.col;
+    vid = virt_cmd->mapvi.phy_id;
+    cmd = virt_cmd->mapvi.cmd;
+    dev_id = virt_cmd->mapvi.devid;
+
+    DPRINTK("%pv: vITS: MAPVI: dev_id 0x%x vcol_id %d vid %d \n",
+             v, dev_id, vcol_id, vid);
+
+    if ( vcol_id > (d->max_vcpus + 1) ||  vid > its_get_nr_events() )
+        return -EINVAL;
+
+    entry.valid = true;
+    entry.vcollection = vcol_id;
+    entry.vlpi = vid;
+
+    if ( cmd == GITS_CMD_MAPI )
+        vits_set_vitt_entry(d, dev_id, vid, &entry);
+    else
+    {
+        event = virt_cmd->mapvi.event;
+        vits_set_vitt_entry(d, dev_id, event, &entry);
+    }
+
+    return 0;
+}
+
+static int vgic_its_process_movi(struct vcpu *v, struct vgic_its *vits,
+                                 its_cmd_block *virt_cmd)
+{
+    struct vitt entry;
+    struct domain *d = v->domain;
+    uint32_t dev_id, event;
+    uint8_t vcol_id;
+
+    vcol_id = virt_cmd->movi.col;
+    event = virt_cmd->movi.event;
+    dev_id = virt_cmd->movi.devid;
+
+    DPRINTK("%pv vITS: MOVI: dev_id 0x%x vcol_id %d event %d\n",
+            v, dev_id, vcol_id, event);
+
+    if ( vcol_id > (d->max_vcpus + 1)  || event > its_get_nr_events() )
+        return -EINVAL;
+
+    if ( vits_get_vitt_entry(d, dev_id, event, &entry) )
+        return -EINVAL;
+
+    entry.vcollection = vcol_id;
+
+    if ( vits_set_vitt_entry(d, dev_id, event, &entry) )
+        return -EINVAL;
+
+    return 0;
+}
+   
+static int vgic_its_process_movall(struct vcpu *v, struct vgic_its *vits,
+                                   its_cmd_block *virt_cmd)
+{
+    return 0;
+}
+
+static int vgic_its_process_discard(struct vcpu *v, struct vgic_its *vits,
+                                    its_cmd_block *virt_cmd)
+{
+    struct vitt entry;
+    struct domain *d = v->domain;
+    uint32_t event, dev_id;
+
+    event = virt_cmd->discard.event;
+    dev_id = virt_cmd->discard.devid;
+
+    DPRINTK("%pv vITS: DISCARD: dev_id 0x%x id %d\n",
+            v, virt_cmd->discard.devid, event);
+
+    if ( event > its_get_nr_events() )
+        return -EINVAL;
+
+    if ( vits_get_vitt_entry(d, dev_id, event, &entry) )
+        return -EINVAL;
+
+    entry.valid = false;
+
+    if ( vits_set_vitt_entry(d, dev_id, event, &entry) )
+        return -EINVAL;
+
+    return 0;
+}
+
+static int vgic_its_process_inv(struct vcpu *v, struct vgic_its *vits,
+                                its_cmd_block *virt_cmd)
+{
+    DPRINTK("%pv vITS: INV: dev_id 0x%x id %d\n",
+            v, virt_cmd->inv.devid, virt_cmd->inv.event);
+
+    return 0;
+}
+
+static int vgic_its_process_clear(struct vcpu *v, struct vgic_its *vits,
+                                  its_cmd_block *virt_cmd)
+{
+    DPRINTK("%pv: vITS: CLEAR: dev_id 0x%x id %d\n",
+             v, virt_cmd->clear.devid, virt_cmd->clear.event);
+
+    return 0;
+}
+
+static int vgic_its_process_invall(struct vcpu *v, struct vgic_its *vits,
+                                   its_cmd_block *virt_cmd)
+{
+    DPRINTK("%pv: vITS: INVALL: vCID %d\n", v, virt_cmd->invall.col);
+
+    return 0;
+}
+
+static int vgic_its_process_int(struct vcpu *v, struct vgic_its *vits,
+                                its_cmd_block *virt_cmd)
+{
+    struct vitt vitt_entry;
+    struct domain *d = v->domain;
+    uint32_t event, dev_id, col_id;
+
+    event = virt_cmd->int_cmd.cmd;
+    dev_id = virt_cmd->int_cmd.devid;
+
+    DPRINTK("%pv: vITS: INT: Device 0x%x id %d\n", v, dev_id, event);
+    if ( event > its_get_nr_events() )
+        return -EINVAL;
+
+    if ( vits_get_vitt_entry(d, dev_id, event, &vitt_entry) )
+        return -EINVAL;
+
+    if ( !vitt_entry.valid )
+    {
+        dprintk(XENLOG_G_ERR,
+                "%pv: vITS: INT CMD invalid event %d for dev 0x%x\n",
+                v, event, dev_id);
+        return -EINVAL;
+    }
+
+    col_id = vitt_entry.vcollection;
+    if ( col_id < d->max_vcpus )
+    {
+        dprintk(XENLOG_G_ERR,
+                "%pv: vITS: INT CMD invalid col_id  %d for dev 0x%x\n",
+                v, col_id, dev_id);
+        return -EINVAL;
+    }
+
+    vgic_vcpu_inject_irq(d->vcpu[col_id], vitt_entry.vlpi);
+
+    return 0;
+}
+
+static int vgic_its_add_device(struct vcpu *v, struct vgic_its *vits,
+                               its_cmd_block *virt_cmd)
+{
+    struct domain *d = v->domain;
+    struct vdevice_table dt_entry;
+    uint32_t dev_id = virt_cmd->mapd.devid;
+
+    DPRINTK("%pv: vITS: Add device dev_id 0x%x vitt_ipa = 0x%lx size %d\n",
+            v, dev_id, (u64)virt_cmd->mapd.itt << 8,
+            virt_cmd->mapd.size);
+
+    if ( virt_cmd->mapd.valid )
+    {
+        /* itt field is 40 bit. extract 48 bit address by shifting */
+        dt_entry.vitt_ipa = virt_cmd->mapd.itt << 8;
+        dt_entry.vitt_size = (1 << (virt_cmd->mapd.size + 1)) *
+                              sizeof(struct vitt);
+    }
+    else
+    {
+        dt_entry.vitt_ipa = INVALID_PADDR;
+        dt_entry.vitt_size = 0;
+    }
+
+    if ( vits_set_vdevice_entry(d, dev_id, &dt_entry) )
+        return -EINVAL;
+
+    return 0;
+}
+
+static int vgic_its_process_mapc(struct vcpu *v, struct vgic_its *vits,
+                                 its_cmd_block *virt_cmd)
+{
+    struct domain *d = v->domain;
+    uint8_t vcol_id;
+    uint64_t vta = 0;
+
+    vcol_id = virt_cmd->mapc.col;
+    vta = virt_cmd->mapc.ta;
+
+    DPRINTK("%pv: vITS: MAPC: vCID %d vTA 0x%lx valid %d \n",
+            v, vcol_id, vta, virt_cmd->mapc.valid);
+
+    if ( vcol_id > (d->max_vcpus + 1) || vta > v->domain->max_vcpus )
+        return -EINVAL;
+
+    if ( virt_cmd->mapc.valid )
+        d->arch.vits->collections[vcol_id].target_address = vta;
+    else
+        d->arch.vits->collections[vcol_id].target_address = ~0UL;
+
+    return 0;
+}
+
+static void vgic_its_update_read_ptr(struct vcpu *v, struct vgic_its *vits)
+{
+    vits->cmd_read = vits->cmd_write_save;
+}
+
+#ifdef DEBUG_ITS
+char *cmd_str[] = {
+        [GITS_CMD_MOVI]    = "MOVI",
+        [GITS_CMD_INT]     = "INT",
+        [GITS_CMD_CLEAR]   = "CLEAR",
+        [GITS_CMD_SYNC]    = "SYNC",
+        [GITS_CMD_MAPD]    = "MAPD",
+        [GITS_CMD_MAPC]    = "MAPC",
+        [GITS_CMD_MAPVI]   = "MAPVI",
+        [GITS_CMD_MAPI]    = "MAPI",
+        [GITS_CMD_INV]     = "INV",
+        [GITS_CMD_INVALL]  = "INVALL",
+        [GITS_CMD_MOVALL]  = "MOVALL",
+        [GITS_CMD_DISCARD] = "DISCARD",
+    };
+#endif
+
+static int vgic_its_parse_its_command(struct vcpu *v, struct vgic_its *vits,
+                                      its_cmd_block *virt_cmd)
+{
+    uint8_t cmd = virt_cmd->hdr.cmd;
+    int ret;
+
+#ifdef DEBUG_ITS
+    DPRINTK("%pv: vITS: Received cmd %s (0x%x)\n", v, cmd_str[cmd], cmd);
+    DPRINTK("Dump Virt cmd: ");
+    dump_cmd(virt_cmd);
+#endif
+
+    switch ( cmd )
+    {
+    case GITS_CMD_MAPD:
+        ret = vgic_its_add_device(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_MAPC:
+        ret =  vgic_its_process_mapc(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_MAPI:
+        /* MAPI is same as MAPVI */
+    case GITS_CMD_MAPVI:
+        ret = vgic_its_process_mapvi(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_MOVI:
+        ret = vgic_its_process_movi(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_MOVALL:
+        ret = vgic_its_process_movall(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_DISCARD:
+        ret = vgic_its_process_discard(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_INV:
+        ret = vgic_its_process_inv(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_INVALL:
+        ret = vgic_its_process_invall(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_INT:
+        ret = vgic_its_process_int(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_CLEAR:
+        ret = vgic_its_process_clear(v, vits, virt_cmd);
+        break;
+    case GITS_CMD_SYNC:
+        ret = vgic_its_process_sync(v, vits, virt_cmd);
+        break;
+    default:
+       dprintk(XENLOG_G_ERR, "%pv: vITS: Unhandled command cmd %d\n", v, cmd);
+       return 1;
+    }
+
+    if ( ret )
+    {
+       dprintk(XENLOG_G_ERR, "%pv: vITS: Failed to handle cmd %d\n", v, cmd);
+       return 1;
+    }
+
+    return 0;
+}
+
+static int vgic_its_read_virt_cmd(struct vcpu *v,
+                                  struct vgic_its *vits,
+                                  its_cmd_block *virt_cmd)
+{
+    paddr_t maddr;
+    struct domain *d = v->domain;
+
+    ASSERT(spin_is_locked(&vits->lock));
+
+    if ( !(vits->cmd_base & GITS_CBASER_VALID) )
+    {
+        dprintk(XENLOG_G_ERR, "%pv: vITS: Invalid CBASER\n", v);
+        return 0;
+    }
+
+    /* CMD Q can be more than 1 page. Map only page that is required */
+    maddr = (vits->cmd_base & BIT_48_12_MASK) + vits->cmd_write_save;
+
+    DPRINTK("%pv: vITS: Mapping CMD Q maddr 0x%lx write_save 0x%lx\n",
+            v, maddr, vits->cmd_write_save);
+
+    if ( vits_entry(d, maddr, (void *)virt_cmd, sizeof(its_cmd_block), 0) )
+    {
+        dprintk(XENLOG_G_ERR,
+                "%pv: vITS: Failed to get command page @page 0x%lx\n",
+                v, vits->cmd_write_save);
+        return -EINVAL;
+    }
+
+    /* No command queue is created by vits to check on Q full */
+    vits->cmd_write_save += sizeof(its_cmd_block);
+    if ( vits->cmd_write_save == vits->cmd_qsize )
+    {
+         DPRINTK("%pv: vITS: Reset write_save 0x%lx qsize 0x%lx \n",
+                 v, vits->cmd_write_save, vits->cmd_qsize);
+
+         vits->cmd_write_save = 0x0;
+    }
+
+    return 0;
+}
+
+int vgic_its_process_cmd(struct vcpu *v, struct vgic_its *vits)
+{
+    its_cmd_block virt_cmd;
+
+    ASSERT(spin_is_locked(&vits->lock));
+
+    do {
+        if ( vgic_its_read_virt_cmd(v, vits, &virt_cmd) )
+            goto err;
+        if ( vgic_its_parse_its_command(v, vits, &virt_cmd) )
+            goto err;
+        vgic_its_update_read_ptr(v, vits);
+    } while ( vits->cmd_write != vits->cmd_write_save );
+
+    DPRINTK("%pv: vITS: write_save 0x%lx write 0x%lx\n",
+            v, vits->cmd_write_save,
+            vits->cmd_write);
+
+    return 1;
+err:
+    dprintk(XENLOG_G_ERR, "%pv: vITS: Failed to process guest cmd\n", v);
+    /*XXX: Be nice to guest though we cannot process command? */
+    return 0;
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/xen/include/asm-arm/gic-its.h b/xen/include/asm-arm/gic-its.h
index d21aefe..a143003 100644
--- a/xen/include/asm-arm/gic-its.h
+++ b/xen/include/asm-arm/gic-its.h
@@ -36,12 +36,25 @@ struct its_collection {
  */
 struct vgic_its
 {
+   spinlock_t lock;
+   /* Command queue base */
+   paddr_t cmd_base;
+   /* Command queue write pointer */
+   paddr_t cmd_write;
+   /* Command queue write saved pointer */
+   paddr_t cmd_write_save;
+   /* Command queue read pointer */
+   paddr_t cmd_read;
+   /* Command queue size */
+   unsigned long cmd_qsize;
    /* vITT device table ipa */
    paddr_t dt_ipa;
    /* vITT device table size */
    uint64_t dt_size;
    /* Radix-tree root of devices attached to this domain */
    struct rb_root dev_root;
+   /* collections mapped */
+   struct its_collection *collections;
 };
 
 /* ITS command structures */
@@ -203,6 +216,12 @@ struct vitt {
     uint32_t vlpi;
 };
 
+struct gic_its_info {
+    uint32_t id_bits;
+    uint32_t dev_bits;
+};
+
+u32 its_get_nr_events(void);
 int its_lpi_init(u32 id_bits);
 int its_init(struct rdist_prop *rdists);
 int its_cpu_init(void);
diff --git a/xen/include/asm-arm/gic.h b/xen/include/asm-arm/gic.h
index 44c2317..fdd96c8 100644
--- a/xen/include/asm-arm/gic.h
+++ b/xen/include/asm-arm/gic.h
@@ -24,6 +24,7 @@
 #define NR_GIC_LPI         4096
 #define MAX_LPI            (FIRST_GIC_LPI + NR_GIC_LPI)
 #define MAX_RDIST_COUNT    4
+#define BIT_48_12_MASK     0xfffffffff000UL
 
 #define GICD_CTLR       (0x000)
 #define GICD_TYPER      (0x004)
-- 
1.7.9.5