[PATCH v4 08/17] xen/arm: ITS: Add APIs to add and assign device

[vijay.kilari@gmail.com]

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

Add APIs to add devices to RB-tree, assign and remove
devices to domain.

Signed-off-by: Vijaya Kumar K <Vijaya.Kumar@caviumnetworks.com>
---
v4: - Introduced helper to populate its_device struct
    - Fixed freeing of its_device memory
    - its_device struct holds domain id
---
 xen/arch/arm/gic-v3-its.c     |  362 +++++++++++++++++++++++++++++++++++++++++
 xen/arch/arm/irq.c            |    8 +
 xen/include/asm-arm/gic-its.h |   18 ++
 xen/include/asm-arm/irq.h     |    1 +
 4 files changed, 389 insertions(+)

diff --git a/xen/arch/arm/gic-v3-its.c b/xen/arch/arm/gic-v3-its.c
index 9161053..1d2fdde 100644
--- a/xen/arch/arm/gic-v3-its.c
+++ b/xen/arch/arm/gic-v3-its.c
@@ -92,6 +92,7 @@ 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;
+static DEFINE_SPINLOCK(rb_its_dev_lock);
 
 #define gic_data_rdist()    (per_cpu(rdist, smp_processor_id()))
 
@@ -108,6 +109,14 @@ u32 its_get_nr_events(void)
     return (1 << its_data.id_bits);
 }
 
+static struct its_node * its_get_phys_node(u32 dev_id)
+{
+    /* TODO: For now return ITS0 node.
+     * Need Query PCI helper function to get on which
+     * ITS node the device is attached
+     */
+    return list_first_entry(&its_nodes, struct its_node, entry);
+}
 /* RB-tree helpers for its_device */
 struct its_device *its_find_device(u32 devid)
 {
@@ -314,6 +323,30 @@ static void its_send_inv(struct its_device *dev, struct its_collection *col,
     its_send_single_command(dev->its, &cmd, col);
 }
 
+static void its_send_mapd(struct its_device *dev, int valid)
+{
+    its_cmd_block cmd;
+    unsigned long itt_addr;
+    u8 size;
+
+    size = max(ilog2(dev->nr_ites), 1);
+    itt_addr = __pa(dev->itt_addr);
+    itt_addr = ROUNDUP(itt_addr, ITS_ITT_ALIGN);
+
+    memset(&cmd, 0x0, sizeof(its_cmd_block));
+    cmd.mapd.cmd = GITS_CMD_MAPD;
+    cmd.mapd.devid = dev->device_id;
+    cmd.mapd.size = size - 1;
+    /* 
+     * ITT address field of MAPD command holds bit[48:8] of
+     * itt address. Hence shift by 8.
+     */
+    cmd.mapd.itt = itt_addr >> 8;
+    cmd.mapd.valid =  !!valid;
+
+    its_send_single_command(dev->its, &cmd, &dev->its->collections[0]);
+}
+
 static void its_send_mapc(struct its_node *its, struct its_collection *col,
                           int valid)
 {
@@ -328,6 +361,21 @@ static void its_send_mapc(struct its_node *its, struct its_collection *col,
     its_send_single_command(its, &cmd, col);
 }
 
+static void its_send_mapvi(struct its_device *dev, struct its_collection *col,
+                    u32 phys_id, u32 event)
+{
+    its_cmd_block cmd;
+
+    memset(&cmd, 0x0, sizeof(its_cmd_block));
+    cmd.mapvi.cmd = GITS_CMD_MAPVI;
+    cmd.mapvi.devid = dev->device_id;
+    cmd.mapvi.event = event;
+    cmd.mapvi.phy_id = phys_id;
+    cmd.mapvi.col = col->col_id;
+
+    its_send_single_command(dev->its, &cmd, col);
+}
+
 static void its_send_invall(struct its_node *its, struct its_collection *col)
 {
     its_cmd_block cmd;
@@ -473,12 +521,18 @@ static const struct its_hw_operations its_ops = {
 
 static unsigned long *lpi_bitmap;
 static u32 lpi_chunks;
+static DEFINE_SPINLOCK(lpi_lock);
 
 static int its_lpi_to_chunk(int lpi)
 {
     return (lpi - 8192) >> IRQS_PER_CHUNK_SHIFT;
 }
 
+static int its_chunk_to_lpi(int chunk)
+{
+    return (chunk << IRQS_PER_CHUNK_SHIFT) + 8192;
+}
+
 int its_lpi_init(u32 id_bits)
 {
     lpi_chunks = its_lpi_to_chunk(1UL << id_bits);
@@ -495,6 +549,314 @@ int its_lpi_init(u32 id_bits)
     return 0;
 }
 
+static unsigned long *its_lpi_alloc_chunks(int nirqs, int *base, int *nr_ids)
+{
+    unsigned long *bitmap = NULL;
+    int chunk_id;
+    int nr_chunks;
+    int i;
+
+    nr_chunks = DIV_ROUND_UP(nirqs, IRQS_PER_CHUNK);
+
+    spin_lock(&lpi_lock);
+
+    do {
+        chunk_id = bitmap_find_next_zero_area(lpi_bitmap, lpi_chunks,
+                                              0, nr_chunks, 0);
+        if ( chunk_id < lpi_chunks )
+            break;
+
+        nr_chunks--;
+    } while ( nr_chunks > 0 );
+
+    if ( !nr_chunks )
+        goto out;
+
+    bitmap = xzalloc_bytes(BITS_TO_LONGS(nr_chunks * IRQS_PER_CHUNK) *
+                           sizeof (long));
+    if ( !bitmap )
+        goto out;
+
+    for ( i = 0; i < nr_chunks; i++ )
+        set_bit(chunk_id + i, lpi_bitmap);
+
+    *base = its_chunk_to_lpi(chunk_id);
+    *nr_ids = nr_chunks * IRQS_PER_CHUNK;
+
+out:
+    spin_unlock(&lpi_lock);
+
+    return bitmap;
+}
+
+static void its_lpi_free(unsigned long *bitmap, int base, int nr_ids)
+{
+    int lpi;
+
+    spin_lock(&lpi_lock);
+
+    for ( lpi = base; lpi < (base + nr_ids); lpi += IRQS_PER_CHUNK )
+    {
+        int chunk = its_lpi_to_chunk(lpi);
+
+        BUG_ON(chunk > lpi_chunks);
+        if ( test_bit(chunk, lpi_bitmap) )
+            clear_bit(chunk, lpi_bitmap);
+        else
+            its_err("Bad LPI chunk %d\n", chunk);
+    }
+
+    spin_unlock(&lpi_lock);
+
+    xfree(bitmap);
+}
+
+static inline u32 its_get_plpi(struct its_device *dev, u32 event)
+{
+    return dev->lpi_base + event;
+}
+
+static int its_alloc_device_irq(struct its_device *dev, u32 *hwirq)
+{
+    int idx;
+
+    idx = find_first_zero_bit(dev->lpi_map, dev->nr_lpis);
+    if ( idx == dev->nr_lpis )
+        return -ENOSPC;
+
+    *hwirq = its_get_plpi(dev, idx);
+    set_bit(idx, dev->lpi_map);
+
+    return 0;
+}
+
+static void its_free_device(struct its_device *dev)
+{
+    xfree(dev->itt_addr);
+    xfree(dev->lpi_map);
+    xfree(dev);
+}
+
+static struct its_device *its_alloc_device(u32 devid)
+{
+    struct its_device *dev;
+    paddr_t *itt;
+    unsigned long *lpi_map;
+    int lpi_base, nr_lpis, sz;
+    u32 nr_ites;
+
+    dev = xzalloc(struct its_device);
+    if ( dev == NULL )
+        return NULL;
+
+    dev->its = its_get_phys_node(devid);
+    /* TODO: Use pci helper to get nvecs */
+    nr_ites = 64;
+    sz = nr_ites * dev->its->ite_size;
+    sz = max(sz, ITS_ITT_ALIGN) + ITS_ITT_ALIGN - 1;
+    itt = xzalloc_bytes(sz);
+    if ( !itt )
+        goto err;
+
+    lpi_map = its_lpi_alloc_chunks(nr_ites, &lpi_base, &nr_lpis);
+    if ( !lpi_map || (nr_lpis < nr_ites) )
+        goto lpi_err;
+
+    dev->itt_addr = itt;
+    dev->nr_ites = nr_ites;
+    dev->lpi_map = lpi_map;
+    dev->lpi_base = lpi_base;
+    dev->nr_lpis = nr_lpis;
+    dev->device_id = devid;
+
+    return dev;
+
+lpi_err:
+    xfree(itt);
+    xfree(lpi_map);
+err:
+    xfree(dev);
+
+    return NULL;
+}
+
+/* Device assignment. Should be called from PHYSDEVOPS_pci_device_add */
+int its_add_device(u32 devid)
+{
+    struct its_device *dev;
+    u32 i, plpi, nr_cpus;
+    struct its_collection *col;
+    struct irq_desc *desc;
+
+    spin_lock(&rb_its_dev_lock);
+    dev = its_find_device(devid);
+    if ( dev )
+    {
+        spin_unlock(&rb_its_dev_lock);
+        dprintk(XENLOG_G_ERR, "%pv: ITS: Device already exists dev 0x%x\n",
+                current, dev->device_id);
+        return -EEXIST;
+    }
+
+    DPRINTK("%pv: ITS: Add device devid 0x%x\n", current, devid);
+
+    dev = its_alloc_device(devid);
+    if ( !dev )
+    {
+        spin_unlock(&rb_its_dev_lock);
+        return -ENOMEM;
+    }
+
+    if ( its_insert_device(dev) )
+    {
+        spin_unlock(&rb_its_dev_lock);
+        its_free_device(dev);
+        dprintk(XENLOG_G_ERR, "%pv: ITS: failed to insert device 0x%x\n",
+                current, devid);
+        return -EINVAL;
+    }
+
+    DPRINTK("%pv: ITS: Adding Device with id 0x%x nvecs %d lpi_base 0x%x\n",
+            current, dev->device_id, dev->nr_lpis, dev->lpi_base);
+
+    /* Map device to its ITT */
+    its_send_mapd(dev, 1);
+
+    /* TODO: Use nr_cpu_ids? */
+    nr_cpus = num_online_cpus();
+    for ( i = 0; i < dev->nr_lpis; i++ )
+    {
+        /* Reserve pLPI */
+        if ( its_alloc_device_irq(dev, &plpi) )
+        {
+            /* Cannot revert MAPVI */
+            its_send_mapd(dev, 0);
+            its_lpi_free(dev->lpi_map, dev->lpi_base, dev->nr_lpis);
+            its_free_device(dev);
+            dprintk(XENLOG_G_ERR, "%pv: ITS: Cannot add device 0x%x\n",
+                    current, devid);
+            return -ENOSPC;
+        }
+
+        /* For each pLPI send MAPVI command */
+        col = &dev->its->collections[(i % nr_cpus)];
+        /* Store collection for this plpi in irq_desc */
+        desc = irq_to_desc(plpi);
+        spin_lock(&desc->lock);
+        gic_set_irq_collection(plpi, col->col_id);
+        spin_unlock(&desc->lock);
+        its_send_mapvi(dev, col, plpi, i);
+    }
+    spin_unlock(&rb_its_dev_lock);
+
+    return 0;
+}
+
+int its_assign_device(struct domain *d, u32 vdevid, u32 pdevid)
+{
+    struct its_device *pdev;
+    struct vits_device *vdev;
+    struct irq_desc *desc;
+    u32 plpi, i;
+
+    DPRINTK("%pv: ITS: Assign request for device 0x%x to domain %d\n",
+            current, vdevid, d->domain_id);
+
+    spin_lock(&d->arch.vits->dev_lock);
+    vdev = vits_find_device(&d->arch.vits->dev_root, vdevid);
+    if ( vdev )
+    {
+        spin_unlock(&d->arch.vits->dev_lock);
+        return -EEXIST;
+    }
+
+    spin_lock(&rb_its_dev_lock);
+    pdev = its_find_device(pdevid);
+    if ( !pdev )
+    {
+        spin_unlock(&rb_its_dev_lock);
+        return -ENODEV;
+    }
+    spin_unlock(&rb_its_dev_lock);
+
+    vdev = xzalloc_bytes(sizeof(struct vits_device));
+    if ( !pdev )
+        return -ENOMEM;
+
+    vdev->its_dev = pdev;
+    pdev->domain_id = d->domain_id;
+    pdev->virt_device_id = vdevid;
+    vdev->vdevid = vdevid;
+
+    /* Insert to domains' list */
+    if ( vits_insert_device(&d->arch.vits->dev_root, vdev) )
+    {
+        spin_unlock(&d->arch.vits->dev_lock);
+        return -EINVAL;
+    }
+    spin_unlock(&d->arch.vits->dev_lock);
+
+    DPRINTK("%pv: ITS: Assigned pdevid 0x%x with nvecs %d for domain %d\n",
+            current, pdevid, pdev->nr_lpis, d->domain_id);
+
+    for ( i = 0; i < pdev->nr_lpis; i++ )
+    {
+        plpi = its_get_plpi(pdev, i);
+        route_irq_to_guest(d, i, plpi, "LPI");
+        desc = irq_to_desc(plpi);
+        spin_lock(&desc->lock);
+        set_irq_device(desc, pdev);
+        lpi_set_config(desc, 1);
+        spin_unlock(&desc->lock);
+    }
+
+    return 0;
+}
+
+int its_detach_device(struct domain *d, u32 vdevid, u32 pdevid)
+{
+    struct its_device *pdev;
+    struct vits_device *vdev;
+    struct irq_desc *desc;
+    u32 plpi, i;
+
+    DPRINTK("%pv: ITS: Detach request for device 0x%x domain %d\n",
+            current, pdevid, d->domain_id);
+
+    spin_lock(&d->arch.vits->dev_lock);
+    vdev = vits_find_device(&d->arch.vits->dev_root, vdevid);
+    if ( !vdev )
+    {
+        spin_unlock(&d->arch.vits->dev_lock);
+        return -EINVAL;
+    }
+
+    pdev = vdev->its_dev;
+    pdev->domain_id = 0;
+
+    DPRINTK("%pv: ITS: Detach pdevid 0x%x with nvecs %d from domain %d\n",
+            current, pdev->device_id, pdev->nr_lpis, d->domain_id);
+    /* Remove vits_device from domain list */
+    vits_remove_device(&d->arch.vits->dev_root, vdev);
+    spin_unlock(&d->arch.vits->dev_lock);
+
+    for ( i = 0; i < pdev->nr_lpis; i++ )
+    {
+        plpi = its_get_plpi(pdev, i);
+        desc = irq_to_desc(plpi);
+        spin_lock(&desc->lock);
+        lpi_set_config(desc, 0);
+        set_irq_device(desc, NULL);
+        /* TODO: Fix for lpi */
+        release_irq(plpi, d);
+        spin_unlock(&desc->lock);
+    }
+
+    xfree(vdev);
+
+    return 0;
+}
+
 /*
  * We allocate 64kB for PROPBASE. That gives us at most 64K LPIs to
  * deal with (one configuration byte per interrupt). PENDBASE has to
diff --git a/xen/arch/arm/irq.c b/xen/arch/arm/irq.c
index ba8528a..3806d98 100644
--- a/xen/arch/arm/irq.c
+++ b/xen/arch/arm/irq.c
@@ -181,6 +181,14 @@ u16 gic_get_irq_collection(unsigned int irq)
     return desc->arch.col_id;
 }
 
+void gic_set_irq_collection(unsigned int irq, u16 col_id)
+{
+    struct irq_desc *desc = irq_to_desc(irq);
+
+    ASSERT(spin_is_locked(&desc->lock));
+    desc->arch.col_id = col_id;
+}
+
 void irq_set_affinity(struct irq_desc *desc, const cpumask_t *cpu_mask)
 {
     if ( desc != NULL )
diff --git a/xen/include/asm-arm/gic-its.h b/xen/include/asm-arm/gic-its.h
index a143003..f041efc 100644
--- a/xen/include/asm-arm/gic-its.h
+++ b/xen/include/asm-arm/gic-its.h
@@ -53,6 +53,8 @@ struct vgic_its
    uint64_t dt_size;
    /* Radix-tree root of devices attached to this domain */
    struct rb_root dev_root;
+   /* Lock to manange virtual devices in rb-tree*/
+   spinlock_t dev_lock;
    /* collections mapped */
    struct its_collection *collections;
 };
@@ -189,10 +191,24 @@ typedef union {
 struct its_device {
     /* Physical ITS */
     struct its_node         *its;
+    /* Device ITT address */
+    paddr_t                 *itt_addr;
+    /* Device ITT size */
+    unsigned long           itt_size;
+    /* Physical LPI map */
+    unsigned long           *lpi_map;
+    /* First Physical LPI number assigned */
+    u32                     lpi_base;
     /* Number of Physical LPIs assigned */
     int                     nr_lpis;
+    /* Number of ITES entries */
+    u32                     nr_ites;
     /* Physical Device id */
     u32                     device_id;
+    /* Virtual Device id */
+    u32                     virt_device_id;
+    /* Domain id */
+    int                     domain_id;
     /* RB-tree entry */
     struct rb_node          node;
 };
@@ -227,6 +243,8 @@ int its_init(struct rdist_prop *rdists);
 int its_cpu_init(void);
 struct its_device *its_find_device(u32 devid);
 int its_insert_device(struct its_device *dev);
+int its_add_device(u32 devid);
+int its_assign_device(struct domain *d, u32 vdevid, u32 pdevid);
 int vits_set_vitt_entry(struct domain *d, uint32_t devid,
                         uint32_t event, struct vitt *entry);
 int vits_get_vitt_entry(struct domain *d, uint32_t devid,
diff --git a/xen/include/asm-arm/irq.h b/xen/include/asm-arm/irq.h
index 55e219f..6bf8fcb 100644
--- a/xen/include/asm-arm/irq.h
+++ b/xen/include/asm-arm/irq.h
@@ -57,6 +57,7 @@ unsigned int irq_to_vid(struct irq_desc *desc);
 struct its_device *get_irq_device(struct irq_desc *desc);
 void set_irq_device(struct irq_desc *desc, struct its_device *dev);
 u16 gic_get_irq_collection(unsigned int irq);
+void gic_set_irq_collection(unsigned int irq, u16 col_id);
 int platform_get_irq(const struct dt_device_node *device, int index);
 
 void irq_set_affinity(struct irq_desc *desc, const cpumask_t *cpu_mask);
-- 
1.7.9.5