[XEN][RFC PATCH v2 09/12] xen/arm: Implement device tree node addition functionalities

[Vikram Garhwal <fnu.vikram@xxxxxxxxxx>]

Update sysctl XEN_SYSCTL_overlay to enable support for dtbo nodes addition using
device tree overlay.

xl overlay add file.dtbo:
    Each time overlay nodes are added using .dtbo, a new fdt(memcpy of
    device_tree_flattened) is created and updated with overlay nodes. This
    updated fdt is further unflattened to a dt_host_new. Next, it checks if any
    of the overlay nodes already exists in the dt_host. If overlay nodes doesn't
    exist then find the overlay nodes in dt_host_new, find the overlay node's
    parent in dt_host and add the nodes as child under their parent in the
    dt_host. The node is attached as the last node under target parent.

    Finally, add IRQs, add device to IOMMUs, set permissions and map MMIO for 
the
    overlay node.

When a node is added using overlay, a new entry is allocated in the
overlay_track to keep the track of memory allocation due to addition of overlay
node. This is helpful for freeing the memory allocated when a device tree node
is removed.

Signed-off-by: Vikram Garhwal <fnu.vikram@xxxxxxxxxx>
---
 xen/common/device_tree.c      |  41 +++++++++
 xen/common/sysctl.c           | 199 ++++++++++++++++++++++++++++++++++++++++++
 xen/include/xen/device_tree.h |   2 +
 3 files changed, 242 insertions(+)

diff --git a/xen/common/device_tree.c b/xen/common/device_tree.c
index 19320e1..5dff64c 100644
--- a/xen/common/device_tree.c
+++ b/xen/common/device_tree.c
@@ -386,6 +386,47 @@ void dt_print_node_names(struct dt_device_node *dt)
 }
 
 #if defined (CONFIG_OVERLAY_DTB)
+int overlay_add_node(struct dt_device_node *device_node,
+                  const char *parent_node_path)
+{
+    struct dt_device_node *parent_node;
+    struct dt_device_node *np;
+    struct dt_device_node *next_node;
+    struct dt_device_node *new_node;
+
+    parent_node = dt_find_node_by_path(parent_node_path);
+
+    new_node = device_node;
+
+    if ( new_node == NULL )
+        return -EINVAL;
+
+    if ( parent_node == NULL )
+    {
+        dt_dprintk("Node not found. Partial dtb will not be added");
+        return -EINVAL;
+    }
+
+    /*
+     * If node is found. We can attach the device_node as a child of the
+     * parent node. Iterate to the last child node of parent.
+     */
+
+    for ( np = parent_node->child; np->sibling != NULL; np = np->sibling )
+    {
+    }
+
+    next_node = np->allnext;
+    new_node->parent = parent_node;
+    np->sibling = new_node;
+    np->allnext = new_node;
+    /* Now plug next_node at the end of device_node. */
+    new_node->sibling = next_node;
+    new_node->allnext = next_node;
+    np->sibling->sibling = NULL;
+    return 0;
+}
+
 int overlay_remove_node(struct dt_device_node *device_node)
 {
     struct dt_device_node *np;
diff --git a/xen/common/sysctl.c b/xen/common/sysctl.c
index fca47f5..38824b2 100644
--- a/xen/common/sysctl.c
+++ b/xen/common/sysctl.c
@@ -331,6 +331,205 @@ out:
     spin_unlock(&overlay_lock);
     return rc;
 }
+
+/*
+ * Adds device tree nodes under target node.
+ * We use dt_host_new to unflatten the updated device_tree_flattened. This is
+ * done to avoid the removal of device_tree generation, iomem regions mapping 
to
+ * hardware domain done by handle_node().
+ */
+static long handle_add_overlay_nodes(void *overlay_fdt,
+                                     uint32_t overlay_fdt_size)
+{
+    int rc = 0;
+    struct dt_device_node *overlay_node;
+    char **node_full_path = NULL;
+    void *fdt = NULL;
+    struct dt_device_node *dt_host_new;
+    struct domain *d = hardware_domain;
+    struct overlay_track *tr = NULL;
+    unsigned int naddr;
+    unsigned int i, j;
+    int num_overlay_nodes;
+    u64 addr, size;
+
+    fdt = xmalloc_bytes(fdt_totalsize(device_tree_flattened));
+    if ( fdt == NULL )
+        return -ENOMEM;
+
+    num_overlay_nodes = overlay_node_count(overlay_fdt);
+    if ( num_overlay_nodes == 0 )
+        return -ENOMEM;
+
+    spin_lock(&overlay_lock);
+
+    memcpy(fdt, device_tree_flattened, fdt_totalsize(device_tree_flattened));
+
+    rc = check_overlay_fdt(overlay_fdt, overlay_fdt_size);
+    if ( rc )
+        goto err;
+
+    /*
+     * overlay_get_node_info is called to get the node information from dtbo.
+     * This is done before fdt_overlay_apply() because the overlay apply will
+     * erase the magic of overlay_fdt.
+     */
+    overlay_get_node_info(overlay_fdt, &node_full_path, num_overlay_nodes);
+
+    rc = fdt_overlay_apply(fdt, overlay_fdt);
+    if ( rc )
+    {
+        printk(XENLOG_ERR "Adding overlay node failed with error %d\n", rc);
+        goto err;
+    }
+
+    for ( j = 0; j < num_overlay_nodes; j++ ) {
+        /* Check if any of the node already exists in dt_host. */
+        overlay_node = dt_find_node_by_path(node_full_path[j]);
+        if ( overlay_node != NULL )
+        {
+            printk(XENLOG_ERR "node %s exists in device tree\n",
+                   node_full_path[j]);
+            rc = -EINVAL;
+            xfree(node_full_path);
+            goto err;
+        }
+    }
+
+    /* Unflatten the fdt into a new dt_host. */
+    unflatten_device_tree(fdt, &dt_host_new);
+
+    for ( j = 0; j < num_overlay_nodes; j++ ) {
+        dt_dprintk("Adding node: %s\n", node_full_path[j]);
+
+        /* Find the newly added node in dt_host_new by it's full path. */
+        overlay_node = _dt_find_node_by_path(dt_host_new, node_full_path[j]);
+        if ( overlay_node == NULL )
+        {
+            dt_dprintk("%s node not found\n", node_full_path[j]);
+            rc = -EFAULT;
+            goto remove_node;
+        }
+
+        /* Add the node to dt_host. */
+        rc = overlay_add_node(overlay_node, overlay_node->parent->full_name);
+        if ( rc )
+        {
+            /* Node not added in dt_host. */
+            goto remove_node;
+        }
+
+        /* Get the node from dt_host and add interrupt and IOMMUs. */
+        overlay_node = dt_find_node_by_path(overlay_node->full_name);
+        if ( overlay_node == NULL )
+        {
+            /* Sanity check. But code will never come here. */
+            printk(XENLOG_ERR "Cannot find %s node under updated dt_host\n",
+                   overlay_node->name);
+            goto remove_node;
+        }
+
+        /* First let's handle the interrupts. */
+        rc = handle_device_interrupts(d, overlay_node, false);
+        if ( rc )
+        {
+            printk(XENLOG_G_ERR "Interrupt failed\n");
+            goto remove_node;
+        }
+
+        /* Add device to IOMMUs */
+        rc = iommu_add_dt_device(overlay_node);
+        if ( rc < 0 )
+        {
+            printk(XENLOG_G_ERR "Failed to add %s to the IOMMU\n",
+                   dt_node_full_name(overlay_node));
+            goto remove_node;
+        }
+
+        /* Set permissions. */
+        naddr = dt_number_of_address(overlay_node);
+
+        dt_dprintk("%s passthrough = %d naddr = %u\n",
+                   dt_node_full_name(overlay_node), false, naddr);
+
+        /* Give permission and map MMIOs */
+        for ( i = 0; i < naddr; i++ )
+        {
+            struct map_range_data mr_data = { .d = d,
+                                              .p2mt = p2m_mmio_direct_c };
+            rc = dt_device_get_address(overlay_node, i, &addr, &size);
+            if ( rc )
+            {
+                printk(XENLOG_ERR "Unable to retrieve address %u for %s\n",
+                       i, dt_node_full_name(overlay_node));
+                goto remove_node;
+            }
+
+            rc = map_range_to_domain(overlay_node, addr, size, &mr_data);
+            if ( rc )
+                goto remove_node;
+        }
+    }
+
+    /* This will happen if everything above goes right. */
+    tr = xzalloc(struct overlay_track);
+    if ( tr == NULL )
+    {
+        rc = -ENOMEM;
+        goto remove_node;
+    }
+
+    tr->dt_host_new = dt_host_new;
+    tr->node_fullname = node_full_path;
+    tr->num_nodes = num_overlay_nodes;
+
+    if ( tr->node_fullname == NULL )
+    {
+        rc = -ENOMEM;
+        goto remove_node;
+    }
+
+    INIT_LIST_HEAD(&tr->entry);
+    list_add_tail(&tr->entry, &overlay_tracker);
+
+err:
+    spin_unlock(&overlay_lock);
+    xfree(fdt);
+    return rc;
+
+/*
+ * Failure case. We need to remove the nodes, free tracker(if tr exists) and
+ * dt_host_new.
+ */
+remove_node:
+    xfree(fdt);
+    rc = check_nodes(node_full_path, j);
+
+    if ( rc ) {
+        spin_unlock(&overlay_lock);
+        return rc;
+    }
+
+    rc = remove_nodes(node_full_path, j);
+
+    if ( rc ) {
+        printk(XENLOG_G_ERR "Removing node failed\n");
+        spin_unlock(&overlay_lock);
+        return rc;
+    }
+
+    spin_unlock(&overlay_lock);
+
+    xfree(dt_host_new);
+
+    if ( tr )
+        xfree(tr);
+
+    if ( node_full_path )
+        xfree(node_full_path);
+
+    return rc;
+}
 #endif
 
 long do_sysctl(XEN_GUEST_HANDLE_PARAM(xen_sysctl_t) u_sysctl)
diff --git a/xen/include/xen/device_tree.h b/xen/include/xen/device_tree.h
index cf29cf5..eafb269 100644
--- a/xen/include/xen/device_tree.h
+++ b/xen/include/xen/device_tree.h
@@ -554,6 +554,8 @@ int dt_find_node_by_gpath(XEN_GUEST_HANDLE(char) u_path, 
uint32_t u_plen,
 void dt_print_node_names(struct dt_device_node *dt);
 
 #if defined (CONFIG_OVERLAY_DTB)
+int overlay_add_node(struct dt_device_node *device_node,
+                  const char *parent_node_path);
 int overlay_remove_node(struct dt_device_node *device_node);
 #endif
 
-- 
2.7.4