[PATCH v3 04/13] xen/dts: Add dt_parse_phandle_with_args and dt_parse_phandle

[Julien Grall <julien.grall@linaro.org>]

Code adapted from linux drivers/of/base.c (commit ef42c58).

Signed-off-by: Julien Grall <julien.grall@linaro.org>

---
    Changes in v2:
        - Remove hard tabs in dt_parse_phandle
---
 xen/common/device_tree.c      |  151 ++++++++++++++++++++++++++++++++++++++++-
 xen/include/xen/device_tree.h |   54 +++++++++++++++
 2 files changed, 203 insertions(+), 2 deletions(-)

diff --git a/xen/common/device_tree.c b/xen/common/device_tree.c
index ccdb7ff..564f2bb 100644
--- a/xen/common/device_tree.c
+++ b/xen/common/device_tree.c
@@ -1090,9 +1090,9 @@ int dt_device_get_address(const struct dt_device_node *dev, int index,
  *
  * Returns a node pointer.
  */
-static const struct dt_device_node *dt_find_node_by_phandle(dt_phandle handle)
+static struct dt_device_node *dt_find_node_by_phandle(dt_phandle handle)
 {
-    const struct dt_device_node *np;
+    struct dt_device_node *np;
 
     dt_for_each_device_node(dt_host, np)
         if ( np->phandle == handle )
@@ -1477,6 +1477,153 @@ bool_t dt_device_is_available(const struct dt_device_node *device)
     return 0;
 }
 
+static int __dt_parse_phandle_with_args(const struct dt_device_node *np,
+                                        const char *list_name,
+                                        const char *cells_name,
+                                        int cell_count, int index,
+                                        struct dt_phandle_args *out_args)
+{
+    const __be32 *list, *list_end;
+    int rc = 0, cur_index = 0;
+    u32 size, count = 0;
+    struct dt_device_node *node = NULL;
+    dt_phandle phandle;
+
+    /* Retrieve the phandle list property */
+    list = dt_get_property(np, list_name, &size);
+    if ( !list )
+        return -ENOENT;
+    list_end = list + size / sizeof(*list);
+
+    /* Loop over the phandles until all the requested entry is found */
+    while ( list < list_end )
+    {
+        rc = -EINVAL;
+        count = 0;
+
+        /*
+         * If phandle is 0, then it is an empty entry with no
+         * arguments.  Skip forward to the next entry.
+         * */
+        phandle = be32_to_cpup(list++);
+        if ( phandle )
+        {
+            /*
+             * Find the provider node and parse the #*-cells
+             * property to determine the argument length.
+             *
+             * This is not needed if the cell count is hard-coded
+             * (i.e. cells_name not set, but cell_count is set),
+             * except when we're going to return the found node
+             * below.
+             */
+            if ( cells_name || cur_index == index )
+            {
+                node = dt_find_node_by_phandle(phandle);
+                if ( !node )
+                {
+                    dt_printk(XENLOG_ERR "%s: could not find phandle\n",
+                              np->full_name);
+                    goto err;
+                }
+            }
+
+            if ( cells_name )
+            {
+                if ( !dt_property_read_u32(node, cells_name, &count) )
+                {
+                    dt_printk("%s: could not get %s for %s\n",
+                              np->full_name, cells_name, node->full_name);
+                    goto err;
+                }
+            }
+            else
+                count = cell_count;
+
+            /*
+             * Make sure that the arguments actually fit in the
+             * remaining property data length
+             */
+            if ( list + count > list_end )
+            {
+                dt_printk(XENLOG_ERR "%s: arguments longer than property\n",
+                          np->full_name);
+                goto err;
+            }
+        }
+
+        /*
+         * All of the error cases above bail out of the loop, so at
+         * this point, the parsing is successful. If the requested
+         * index matches, then fill the out_args structure and return,
+         * or return -ENOENT for an empty entry.
+         */
+        rc = -ENOENT;
+        if ( cur_index == index )
+        {
+            if (!phandle)
+                goto err;
+
+            if ( out_args )
+            {
+                int i;
+
+                WARN_ON(count > MAX_PHANDLE_ARGS);
+                if (count > MAX_PHANDLE_ARGS)
+                    count = MAX_PHANDLE_ARGS;
+                out_args->np = node;
+                out_args->args_count = count;
+                for ( i = 0; i < count; i++ )
+                    out_args->args[i] = be32_to_cpup(list++);
+            }
+
+            /* Found it! return success */
+            return 0;
+        }
+
+        node = NULL;
+        list += count;
+        cur_index++;
+    }
+
+    /*
+     * Returning result will be one of:
+     * -ENOENT : index is for empty phandle
+     * -EINVAL : parsing error on data
+     * [1..n]  : Number of phandle (count mode; when index = -1)
+     */
+    rc = index < 0 ? cur_index : -ENOENT;
+err:
+    return rc;
+}
+
+struct dt_device_node *dt_parse_phandle(const struct dt_device_node *np,
+                                        const char *phandle_name, int index)
+{
+    struct dt_phandle_args args;
+
+    if (index < 0)
+        return NULL;
+
+    if (__dt_parse_phandle_with_args(np, phandle_name, NULL, 0,
+                                     index, &args))
+        return NULL;
+
+    return args.np;
+}
+
+
+int dt_parse_phandle_with_args(const struct dt_device_node *np,
+                               const char *list_name,
+                               const char *cells_name, int index,
+                               struct dt_phandle_args *out_args)
+{
+    if ( index < 0 )
+        return -EINVAL;
+    return __dt_parse_phandle_with_args(np, list_name, cells_name, 0,
+                                        index, out_args);
+}
+
 /**
  * unflatten_dt_node - Alloc and populate a device_node from the flat tree
  * @fdt: The parent device tree blob
diff --git a/xen/include/xen/device_tree.h b/xen/include/xen/device_tree.h
index 7c075d9..d429e60 100644
--- a/xen/include/xen/device_tree.h
+++ b/xen/include/xen/device_tree.h
@@ -112,6 +112,13 @@ struct dt_device_node {
 
 };
 
+#define MAX_PHANDLE_ARGS 16
+struct dt_phandle_args {
+    struct dt_device_node *np;
+    int args_count;
+    uint32_t args[MAX_PHANDLE_ARGS];
+};
+
 /**
  * IRQ line type.
  *
@@ -621,6 +628,53 @@ void dt_set_range(__be32 **cellp, const struct dt_device_node *np,
 void dt_get_range(const __be32 **cellp, const struct dt_device_node *np,
                   u64 *address, u64 *size);
 
+/**
+ * dt_parse_phandle - Resolve a phandle property to a device_node pointer
+ * @np: Pointer to device node holding phandle property
+ * @phandle_name: Name of property holding a phandle value
+ * @index: For properties holding a table of phandles, this is the index into
+ *         the table
+ *
+ * Returns the device_node pointer.
+ */
+struct dt_device_node *dt_parse_phandle(const struct dt_device_node *np,
+				                        const char *phandle_name,
+                                        int index);
+
+/**
+ * dt_parse_phandle_with_args() - Find a node pointed by phandle in a list
+ * @np:	pointer to a device tree node containing a list
+ * @list_name: property name that contains a list
+ * @cells_name: property name that specifies phandles' arguments count
+ * @index: index of a phandle to parse out
+ * @out_args: optional pointer to output arguments structure (will be filled)
+ *
+ * This function is useful to parse lists of phandles and their arguments.
+ * Returns 0 on success and fills out_args, on error returns appropriate
+ * errno value.
+ *
+ * Example:
+ *
+ * phandle1: node1 {
+ * 	#list-cells = <2>;
+ * }
+ *
+ * phandle2: node2 {
+ * 	#list-cells = <1>;
+ * }
+ *
+ * node3 {
+ * 	list = <&phandle1 1 2 &phandle2 3>;
+ * }
+ *
+ * To get a device_node of the `node2' node you may call this:
+ * dt_parse_phandle_with_args(node3, "list", "#list-cells", 1, &args);
+ */
+int dt_parse_phandle_with_args(const struct dt_device_node *np,
+                               const char *list_name,
+                               const char *cells_name, int index,
+                               struct dt_phandle_args *out_args);
+
 #endif /* __XEN_DEVICE_TREE_H */
 
 /*
-- 
1.7.10.4