@@ -36,6 +36,25 @@ static struct dt_device_node *find_last_descendants_node(
return child_node;
}
+/*
+ * Returns next node to the input node. If node has children then return
+ * last descendant's next node.
+*/
+static struct dt_device_node *dt_find_next_node(struct dt_device_node *dt,
+ const struct dt_device_node *node)
+{
+ struct dt_device_node *np;
+
+ dt_for_each_device_node(dt, np)
+ if ( np == node )
+ break;
+
+ if ( np->child )
+ np = find_last_descendants_node(np);
+
+ return np->allnext;
+}
+
static int dt_overlay_remove_node(struct dt_device_node *device_node)
{
struct dt_device_node *np;
@@ -109,6 +128,72 @@ static int dt_overlay_remove_node(struct dt_device_node *device_node)
return 0;
}
+static int dt_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, *np_last_descendant;
+ struct dt_device_node *next_node;
+ struct dt_device_node *device_node_last_descendant;
+
+ parent_node = dt_find_node_by_path(parent_node_path);
+
+ if ( parent_node == NULL )
+ {
+ dt_dprintk("Node not found. Overlay node will not be added\n");
+ return -EINVAL;
+ }
+
+ /* If parent has no child. */
+ if ( parent_node->child == NULL )
+ {
+ next_node = parent_node->allnext;
+ device_node->parent = parent_node;
+ parent_node->allnext = device_node;
+ parent_node->child = device_node;
+ }
+ else
+ {
+ /* If parent has at least one child node.
+ * Iterate to the last child node of parent.
+ */
+ for ( np = parent_node->child; np->sibling != NULL; np = np->sibling );
+
+ /* Iterate over all child nodes of np node. */
+ if ( np->child )
+ {
+ np_last_descendant = find_last_descendants_node(np);
+
+ next_node = np_last_descendant->allnext;
+ np_last_descendant->allnext = device_node;
+ }
+ else
+ {
+ next_node = np->allnext;
+ np->allnext = device_node;
+ }
+
+ device_node->parent = parent_node;
+ np->sibling = device_node;
+ np->sibling->sibling = NULL;
+ }
+
+ /* Iterate over all child nodes of device_node to add children too. */
+ if ( device_node->child )
+ {
+ device_node_last_descendant = find_last_descendants_node(device_node);
+ /* Plug next_node at the end of last children of device_node. */
+ device_node_last_descendant->allnext = next_node;
+ }
+ else
+ {
+ /* Now plug next_node at the end of device_node. */
+ device_node->allnext = next_node;
+ }
+
+ return 0;
+}
+
/* Basic sanity check for the dtbo tool stack provided to Xen. */
static int check_overlay_fdt(const void *overlay_fdt, uint32_t overlay_fdt_size)
{
@@ -148,6 +233,79 @@ static unsigned int overlay_node_count(void *fdto)
return num_overlay_nodes;
}
+/*
+ * overlay_get_nodes_info will get full name with path for all the nodes which
+ * are in one level of __overlay__ tag. This is useful when checking node for
+ * duplication i.e. dtbo tries to add nodes which already exists in device tree.
+ */
+static int overlay_get_nodes_info(const void *fdto, char ***nodes_full_path,
+ unsigned int num_overlay_nodes)
+{
+ int fragment;
+
+ *nodes_full_path = xzalloc_bytes(num_overlay_nodes * sizeof(char *));
+
+ if ( *nodes_full_path == NULL )
+ return -ENOMEM;
+
+ fdt_for_each_subnode(fragment, fdto, 0)
+ {
+ int target;
+ int overlay;
+ int subnode;
+ const char *target_path;
+
+ target = fdt_overlay_target_offset(device_tree_flattened, fdto,
+ fragment, &target_path);
+ if ( target < 0 )
+ return target;
+
+ overlay = fdt_subnode_offset(fdto, fragment, "__overlay__");
+
+ /*
+ * overlay value can be < 0. But fdt_for_each_subnode() loop checks for
+ * overlay >= 0. So, no need for a overlay>=0 check here.
+ */
+ fdt_for_each_subnode(subnode, fdto, overlay)
+ {
+ const char *node_name = NULL;
+ int node_name_len;
+ unsigned int target_path_len = strlen(target_path);
+ unsigned int node_full_name_len;
+ unsigned int node_num = 0;
+
+ node_name = fdt_get_name(fdto, subnode, &node_name_len);
+
+ if ( node_name == NULL )
+ return node_name_len;
+
+ /*
+ * Magic number 2 is for adding '/'. This is done to keep the
+ * node_full_name in the correct full node name format.
+ */
+ node_full_name_len = target_path_len + node_name_len + 2;
+
+ (*nodes_full_path)[node_num] = xmalloc_bytes(node_full_name_len);
+
+ if ( (*nodes_full_path)[node_num] == NULL )
+ return -ENOMEM;
+
+ memcpy((*nodes_full_path)[node_num], target_path, target_path_len);
+
+ (*nodes_full_path)[node_num][target_path_len] = '/';
+
+ memcpy((*nodes_full_path)[node_num] + target_path_len + 1,
+ node_name, node_name_len);
+
+ (*nodes_full_path)[node_num][node_full_name_len - 1] = '\0';
+
+ node_num++;
+ }
+ }
+
+ return 0;
+}
+
static int handle_remove_irq_iommu(struct dt_device_node *device_node)
{
int rc = 0;
@@ -367,6 +525,322 @@ out:
return rc;
}
+/*
+ * Handles IRQ and IOMMU mapping for the overlay_node and all descendants of the
+ * overlay_nodes.
+ */
+static int handle_add_irq_iommu(struct domain *d,
+ struct dt_device_node *overlay_node)
+{
+ int rc;
+ unsigned int naddr, i, len;
+ uint64_t addr, size;
+ struct dt_device_node *np;
+
+ /* First let's handle the interrupts. */
+ rc = handle_device_interrupts(d, overlay_node, false);
+ if ( rc < 0 )
+ {
+ printk(XENLOG_ERR "Interrupt failed\n");
+ return rc;
+ }
+
+ /* Check if iommu property exists. */
+ if ( dt_get_property(overlay_node, "iommus", &len) )
+ {
+
+ /* Add device to IOMMUs. */
+ rc = iommu_add_dt_device(overlay_node);
+ if ( rc < 0 )
+ {
+ printk(XENLOG_ERR "Failed to add %s to the IOMMU\n",
+ dt_node_full_name(overlay_node));
+ return rc;
+ }
+ }
+
+ /* 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 for map MMIOs */
+ for ( i = 0; i < naddr; i++ )
+ {
+ /*
+ * For now, we skip_mapping which means it will only permit iomem access
+ * to hardware_domain using iomem_permit_access() but will never map as
+ * map_range_p2mt() will not be called.
+ */
+ struct map_range_data mr_data = { .d = d,
+ .p2mt = p2m_mmio_direct_c,
+ .skip_mapping = true };
+
+ 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));
+ return rc;
+ }
+
+ rc = map_range_to_domain(overlay_node, addr, size, &mr_data);
+ if ( rc )
+ return rc;
+ }
+
+ /* Map IRQ and IOMMU for overlay_node's children. */
+ for ( np = overlay_node->child; np != NULL; np = np->sibling)
+ {
+ rc = handle_add_irq_iommu(d, np);
+ if ( rc )
+ {
+ printk(XENLOG_ERR "Adding IRQ and IOMMU failed\n");
+ return rc;
+ }
+ }
+
+ return rc;
+}
+
+/*
+ * Adds device tree nodes under target node.
+ * We use tr->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, j, i;
+ struct dt_device_node *overlay_node, *prev_node, *next_node;
+ struct domain *d = hardware_domain;
+ struct overlay_track *tr = NULL;
+ char **nodes_full_path = NULL;
+ unsigned int new_fdt_size;
+
+ tr = xzalloc(struct overlay_track);
+ if ( tr == NULL )
+ return -ENOMEM;
+
+ new_fdt_size = fdt_totalsize(device_tree_flattened) +
+ fdt_totalsize(overlay_fdt);
+
+ tr->fdt = xzalloc_bytes(new_fdt_size);
+ if ( tr->fdt == NULL )
+ {
+ xfree(tr);
+ return -ENOMEM;
+ }
+
+ tr->num_nodes = overlay_node_count(overlay_fdt);
+ if ( tr->num_nodes == 0 )
+ {
+ xfree(tr->fdt);
+ xfree(tr);
+ return -ENOMEM;
+ }
+
+ tr->nodes_address = xzalloc_bytes(tr->num_nodes * sizeof(unsigned long));
+ if ( tr->nodes_address == NULL )
+ {
+ xfree(tr->fdt);
+ xfree(tr);
+ return -ENOMEM;
+ }
+
+ rc = check_overlay_fdt(overlay_fdt, overlay_fdt_size);
+ if ( rc )
+ {
+ xfree(tr->nodes_address);
+ xfree(tr->fdt);
+ xfree(tr);
+ return rc;
+ }
+
+ /*
+ * Keep a copy of overlay_fdt as fdt_overlay_apply will change the input
+ * overlay's content(magic) when applying overlay.
+ */
+ tr->overlay_fdt = xzalloc_bytes(overlay_fdt_size);
+ if ( tr->overlay_fdt == NULL )
+ {
+ xfree(tr->nodes_address);
+ xfree(tr->fdt);
+ xfree(tr);
+ return -ENOMEM;
+ }
+
+ memcpy(tr->overlay_fdt, overlay_fdt, overlay_fdt_size);
+
+ spin_lock(&overlay_lock);
+
+ memcpy(tr->fdt, device_tree_flattened,
+ fdt_totalsize(device_tree_flattened));
+
+ /* Open tr->fdt with more space to accommodate the overlay_fdt. */
+ fdt_open_into(tr->fdt, tr->fdt, new_fdt_size);
+
+ /*
+ * overlay_get_nodes_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.
+ */
+ rc = overlay_get_nodes_info(overlay_fdt, &nodes_full_path,
+ tr->num_nodes);
+ if ( rc )
+ {
+ printk(XENLOG_ERR "Getting nodes information failed with error %d\n",
+ rc);
+ goto err;
+ }
+
+ rc = fdt_overlay_apply(tr->fdt, overlay_fdt);
+ if ( rc )
+ {
+ printk(XENLOG_ERR "Adding overlay node failed with error %d\n", rc);
+ goto err;
+ }
+
+ /*
+ * Check if any of the node already exists in dt_host. If node already exits
+ * we can return here as this overlay_fdt is not suitable for overlay ops.
+ */
+ for ( j = 0; j < tr->num_nodes; j++ )
+ {
+ overlay_node = dt_find_node_by_path(nodes_full_path[j]);
+ if ( overlay_node != NULL )
+ {
+ printk(XENLOG_ERR "node %s exists in device tree\n",
+ nodes_full_path[j]);
+ rc = -EINVAL;
+ goto err;
+ }
+ }
+
+ /* Unflatten the tr->fdt into a new dt_host. */
+ unflatten_device_tree(tr->fdt, &tr->dt_host_new);
+
+ for ( j = 0; j < tr->num_nodes; j++ )
+ {
+ dt_dprintk("Adding node: %s\n", nodes_full_path[j]);
+
+ /* Find the newly added node in tr->dt_host_new by it's full path. */
+ overlay_node = device_tree_find_node_by_path(tr->dt_host_new,
+ nodes_full_path[j]);
+ if ( overlay_node == NULL )
+ {
+ dt_dprintk("%s node not found\n", nodes_full_path[j]);
+ rc = -EFAULT;
+ goto remove_node;
+ }
+
+ /*
+ * Find previous and next node to overlay_node in dt_host_new. We will
+ * need these nodes to fix the dt_host_new mapping. When overlay_node is
+ * take out of dt_host_new tree and added to dt_host, link between
+ * previous node and next_node is broken. We will need to refresh
+ * dt_host_new with correct linking for any other overlay nodes
+ * extraction in future.
+ */
+ dt_for_each_device_node(tr->dt_host_new, prev_node)
+ if ( prev_node->allnext == overlay_node )
+ break;
+
+ next_node = dt_find_next_node(tr->dt_host_new, overlay_node);
+
+ read_lock(&dt_host->lock);
+
+ /* Add the node to dt_host. */
+ rc = dt_overlay_add_node(overlay_node, overlay_node->parent->full_name);
+ if ( rc )
+ {
+ read_unlock(&dt_host->lock);
+
+ /* Node not added in dt_host. */
+ goto remove_node;
+ }
+
+ read_unlock(&dt_host->lock);
+
+ prev_node->allnext = next_node;
+
+ overlay_node = dt_find_node_by_path(overlay_node->full_name);
+ if ( overlay_node == NULL )
+ {
+ /* Sanity check. But code will never come here. */
+ ASSERT_UNREACHABLE();
+ goto remove_node;
+ }
+
+ rc = handle_add_irq_iommu(d, overlay_node);
+ if ( rc )
+ {
+ printk(XENLOG_ERR "Adding IRQ and IOMMU failed\n");
+ return rc;
+ }
+
+ /* Keep overlay_node address in tracker. */
+ tr->nodes_address[j] = (unsigned long)overlay_node;
+ }
+
+ INIT_LIST_HEAD(&tr->entry);
+ list_add_tail(&tr->entry, &overlay_tracker);
+
+ spin_unlock(&overlay_lock);
+
+ if ( nodes_full_path != NULL )
+ {
+ for ( i = 0; i < tr->num_nodes && nodes_full_path[i] != NULL;
+ i++ )
+ {
+ xfree(nodes_full_path[i]);
+ }
+ xfree(nodes_full_path);
+ }
+
+ return rc;
+
+/*
+ * Failure case. We need to remove the nodes, free tracker(if tr exists) and
+ * tr->dt_host_new.
+ */
+remove_node:
+ tr->num_nodes = j;
+ rc = remove_nodes(tr);
+
+ if ( rc )
+ {
+ /* If removing node fails, this may cause memory leaks. */
+ printk(XENLOG_ERR "Removing node failed.\n");
+ spin_unlock(&overlay_lock);
+ return rc;
+ }
+
+err:
+ spin_unlock(&overlay_lock);
+
+ xfree(tr->dt_host_new);
+ xfree(tr->fdt);
+ xfree(tr->overlay_fdt);
+ xfree(tr->nodes_address);
+
+ if ( nodes_full_path != NULL )
+ {
+ for ( i = 0; i < tr->num_nodes && nodes_full_path[i] != NULL;
+ i++ )
+ {
+ xfree(nodes_full_path[i]);
+ }
+ xfree(nodes_full_path);
+ }
+
+ xfree(tr);
+
+ return rc;
+}
+
long dt_sysctl(struct xen_sysctl_dt_overlay *op)
{
long ret;
@@ -391,6 +865,14 @@ long dt_sysctl(struct xen_sysctl_dt_overlay *op)
switch ( op->overlay_op )
{
+ case XEN_SYSCTL_DT_OVERLAY_ADD:
+ ret = handle_add_overlay_nodes(overlay_fdt, op->overlay_fdt_size);
+
+ if ( ret )
+ xfree(overlay_fdt);
+
+ break;
+
case XEN_SYSCTL_DT_OVERLAY_REMOVE:
ret = handle_remove_overlay_nodes(overlay_fdt, op->overlay_fdt_size);
xfree(overlay_fdt);
Update sysctl XEN_SYSCTL_dt_overlay to enable support for dtbo nodes addition using device tree overlay. xl dt-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. The main purpose of this to address first part of dynamic programming i.e. making xen aware of new device tree node which means updating the dt_host with overlay node information. Here we are adding/removing node from dt_host, and checking/setting IOMMU and IRQ permission but never mapping them to any domain. Right now, mapping/Un-mapping will happen only when a new domU is created/destroyed using "xl create". Signed-off-by: Vikram Garhwal <vikram.garhwal@amd.com> --- xen/common/dt_overlay.c | 482 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 482 insertions(+)