[V2,07/33] xen/arm: Create a hierarchical device tree

diff --git a/xen/arch/arm/setup.c b/xen/arch/arm/setup.c
index 59646d6..07c0444 100644
--- a/xen/arch/arm/setup.c
+++ b/xen/arch/arm/setup.c
@@ -429,12 +429,17 @@  void __init start_xen(unsigned long boot_phys_offset,
     setup_pagetables(boot_phys_offset, get_xen_paddr());
     setup_mm(fdt_paddr, fdt_size);
 
+    dt_unflatten_host_device_tree();
+
 #ifdef EARLY_UART_ADDRESS
     /* TODO Need to get device tree or command line for UART address */
     pl011_init(0, FIXMAP_ADDR(FIXMAP_CONSOLE));
     console_init_preirq();
 #endif
 
+    /* FIXME: Do something smarter */
+    dt_switch_to_printk();
+
     processor_id();
 
     init_xen_time();
diff --git a/xen/common/device_tree.c b/xen/common/device_tree.c
index 7997f41..bdf8871 100644
--- a/xen/common/device_tree.c
+++ b/xen/common/device_tree.c
@@ -2,6 +2,8 @@ 
  * Device Tree
  *
  * Copyright (C) 2012 Citrix Systems, Inc.
+ * Copyright 2009 Benjamin Herrenschmidt, IBM Corp
+ * benh@kernel.crashing.org
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -19,15 +21,51 @@ 
 #include <xen/stdarg.h>
 #include <xen/string.h>
 #include <xen/cpumask.h>
+#include <xen/ctype.h>
+#include <xen/lib.h>
 #include <asm/early_printk.h>
 
 struct dt_early_info __initdata early_info;
 void *device_tree_flattened;
+/* Host device tree */
+struct dt_device_node *dt_host;
+
+/**
+ * struct dt_alias_prop - Alias property in 'aliases' node
+ * @link: List node to link the structure in aliases_lookup list
+ * @alias: Alias property name
+ * @np: Pointer to device_node that the alias stands for
+ * @id: Index value from end of alias name
+ * @stem: Alias string without the index
+ *
+ * The structure represents one alias property of 'aliases' node as
+ * an entry in aliases_lookup list.
+ */
+struct dt_alias_prop {
+    struct list_head link;
+    const char *alias;
+    struct dt_device_node *np;
+    int id;
+    char stem[0];
+};
+
+static LIST_HEAD(aliases_lookup);
+
 
 /* Some device tree functions may be called both before and after the
    console is initialized. */
 static void (*dt_printk)(const char *fmt, ...) = early_printk;
 
+#define ALIGN(x, a) ((x + (a) - 1) & ~((a) - 1));
+
+// #define DEBUG_DT
+
+#ifdef DEBUG_DT
+# define dt_dprintk(fmt, args...) dt_printk(XENLOG_DEBUG fmt, ##args)
+#else
+# define dt_dprintk(fmt, args...) do {} while ( 0 )
+#endif
+
 bool_t device_tree_node_matches(const void *fdt, int node, const char *match)
 {
     const char *name;
@@ -263,7 +301,7 @@  static int dump_node(const void *fdt, int node, const char *name, int depth,
 
     if ( name[0] == '\0' )
         name = "/";
-    printk("%s%s:\n", prefix, name);
+    dt_printk("%s%s:\n", prefix, name);
 
     for ( prop = fdt_first_property_offset(fdt, node);
           prop >= 0;
@@ -273,7 +311,7 @@  static int dump_node(const void *fdt, int node, const char *name, int depth,
 
         p = fdt_get_property_by_offset(fdt, prop, NULL);
 
-        printk("%s  %s\n", prefix, fdt_string(fdt, fdt32_to_cpu(p->nameoff)));
+        dt_printk("%s  %s\n", prefix, fdt_string(fdt, fdt32_to_cpu(p->nameoff)));
     }
 
     return 0;
@@ -488,11 +526,417 @@  size_t __init device_tree_early_init(const void *fdt)
     device_tree_for_each_node((void *)fdt, early_scan_node, NULL);
     early_print_info();
 
-    dt_printk = printk;
-
     return fdt_totalsize(fdt);
 }
 
+static void __init *unflatten_dt_alloc(unsigned long *mem, unsigned long size,
+                                       unsigned long align)
+{
+    void *res;
+
+    *mem = ALIGN(*mem, align);
+    res = (void *)*mem;
+    *mem += size;
+
+    return res;
+}
+
+/* Find a property with a given name for a given node and return it. */
+static const struct dt_property *
+dt_find_property(const struct dt_device_node *np,
+                 const char *name,
+                 u32 *lenp)
+{
+    const struct dt_property *pp;
+
+    if ( !np )
+        return NULL;
+
+    for ( pp = np->properties; pp; pp = pp->next )
+    {
+        if ( strcmp(pp->name, name) == 0 )
+        {
+            if ( lenp )
+                *lenp = pp->length;
+            break;
+        }
+    }
+
+    return pp;
+}
+
+const void *dt_get_property(const struct dt_device_node *np,
+                            const char *name, u32 *lenp)
+{
+    const struct dt_property *pp = dt_find_property(np, name, lenp);
+
+    return pp ? pp->value : NULL;
+}
+
+struct dt_device_node *dt_find_node_by_path(const char *path)
+{
+    struct dt_device_node *np;
+
+    for_each_device_node(dt_host, np)
+        if ( np->full_name && (dt_node_cmp(np->full_name, path) == 0) )
+            break;
+
+    return np;
+}
+
+/**
+ * unflatten_dt_node - Alloc and populate a device_node from the flat tree
+ * @fdt: The parent device tree blob
+ * @mem: Memory chunk to use for allocating device nodes and properties
+ * @p: pointer to node in flat tree
+ * @dad: Parent struct device_node
+ * @allnextpp: pointer to ->allnext from last allocated device_node
+ * @fpsize: Size of the node path up at the current depth.
+ */
+static unsigned long __init unflatten_dt_node(const void *fdt,
+                                              unsigned long mem,
+                                              unsigned long *p,
+                                              struct dt_device_node *dad,
+                                              struct dt_device_node ***allnextpp,
+                                              unsigned long fpsize)
+{
+    struct dt_device_node *np;
+    struct dt_property *pp, **prev_pp = NULL;
+    char *pathp;
+    u32 tag;
+    unsigned int l, allocl;
+    int has_name = 0;
+    int new_format = 0;
+
+    tag = be32_to_cpup((__be32 *)(*p));
+    if ( tag != FDT_BEGIN_NODE )
+    {
+        dt_printk(XENLOG_WARNING "Weird tag at start of node: %x\n", tag);
+        return mem;
+    }
+    *p += 4;
+    pathp = (char *)*p;
+    l = allocl = strlen(pathp) + 1;
+    *p = ALIGN(*p + l, 4);
+
+    /* version 0x10 has a more compact unit name here instead of the full
+     * path. we accumulate the full path size using "fpsize", we'll rebuild
+     * it later. We detect this because the first character of the name is
+     * not '/'.
+     */
+    if ( (*pathp) != '/' )
+    {
+        new_format = 1;
+        if ( fpsize == 0 )
+        {
+            /* root node: special case. fpsize accounts for path
+             * plus terminating zero. root node only has '/', so
+             * fpsize should be 2, but we want to avoid the first
+             * level nodes to have two '/' so we use fpsize 1 here
+             */
+            fpsize = 1;
+            allocl = 2;
+        }
+        else
+        {
+            /* account for '/' and path size minus terminal 0
+             * already in 'l'
+             */
+            fpsize += l;
+            allocl = fpsize;
+        }
+    }
+
+    np = unflatten_dt_alloc(&mem, sizeof(struct dt_device_node) + allocl,
+                            __alignof__(struct dt_device_node));
+    if ( allnextpp )
+    {
+        memset(np, 0, sizeof(*np));
+        np->full_name = ((char *)np) + sizeof(struct dt_device_node);
+        /* By default dom0 owns the device */
+        np->used_by = 0;
+        if ( new_format )
+        {
+            char *fn = np->full_name;
+            /* rebuild full path for new format */
+            if ( dad && dad->parent )
+            {
+                strlcpy(fn, dad->full_name, allocl);
+#ifdef DEBUG_DT
+                if ( (strlen(fn) + l + 1) != allocl )
+                {
+                    dt_dprintk("%s: p: %d, l: %d, a: %d\n",
+                               pathp, (int)strlen(fn),
+                               l, allocl);
+                }
+#endif
+                fn += strlen(fn);
+            }
+            *(fn++) = '/';
+            memcpy(fn, pathp, l);
+        }
+        else
+            memcpy(np->full_name, pathp, l);
+        prev_pp = &np->properties;
+        **allnextpp = np;
+        *allnextpp = &np->allnext;
+        if ( dad != NULL )
+        {
+            np->parent = dad;
+            /* we temporarily use the next field as `last_child'*/
+            if ( dad->next == NULL )
+                dad->child = np;
+            else
+                dad->next->sibling = np;
+            dad->next = np;
+        }
+    }
+    /* process properties */
+    while ( 1 )
+    {
+        u32 sz, noff;
+        const char *pname;
+
+        tag = be32_to_cpup((__be32 *)(*p));
+        if ( tag == FDT_NOP )
+        {
+            *p += 4;
+            continue;
+        }
+        if ( tag != FDT_PROP )
+            break;
+        *p += 4;
+        sz = be32_to_cpup((__be32 *)(*p));
+        noff = be32_to_cpup((__be32 *)((*p) + 4));
+        *p += 8;
+        if ( fdt_version(fdt) < 0x10 )
+            *p = ALIGN(*p, sz >= 8 ? 8 : 4);
+
+        pname = fdt_string(fdt, noff);
+        if ( pname == NULL )
+        {
+            dt_dprintk("Can't find property name in list!\n");
+            break;
+        }
+        if ( strcmp(pname, "name") == 0 )
+            has_name = 1;
+        l = strlen(pname) + 1;
+        pp = unflatten_dt_alloc(&mem, sizeof(struct dt_property),
+                                __alignof__(struct dt_property));
+        if ( allnextpp )
+        {
+            /* We accept flattened tree phandles either in
+             * ePAPR-style "phandle" properties, or the
+             * legacy "linux,phandle" properties.  If both
+             * appear and have different values, things
+             * will get weird.  Don't do that. */
+            if ( (strcmp(pname, "phandle") == 0) ||
+                 (strcmp(pname, "linux,phandle") == 0) )
+            {
+                if ( np->phandle == 0 )
+                    np->phandle = be32_to_cpup((__be32*)*p);
+            }
+            /* And we process the "ibm,phandle" property
+             * used in pSeries dynamic device tree
+             * stuff */
+            if ( strcmp(pname, "ibm,phandle") == 0 )
+                np->phandle = be32_to_cpup((__be32 *)*p);
+            pp->name = pname;
+            pp->length = sz;
+            pp->value = (void *)*p;
+            *prev_pp = pp;
+            prev_pp = &pp->next;
+        }
+        *p = ALIGN((*p) + sz, 4);
+    }
+    /* with version 0x10 we may not have the name property, recreate
+     * it here from the unit name if absent
+     */
+    if ( !has_name )
+    {
+        char *p1 = pathp, *ps = pathp, *pa = NULL;
+        int sz;
+
+        while ( *p1 )
+        {
+            if ( (*p1) == '@' )
+                pa = p1;
+            if ( (*p1) == '/' )
+                ps = p1 + 1;
+            p1++;
+        }
+        if ( pa < ps )
+            pa = p1;
+        sz = (pa - ps) + 1;
+        pp = unflatten_dt_alloc(&mem, sizeof(struct dt_property) + sz,
+                                __alignof__(struct dt_property));
+        if ( allnextpp )
+        {
+            pp->name = "name";
+            pp->length = sz;
+            pp->value = pp + 1;
+            *prev_pp = pp;
+            prev_pp = &pp->next;
+            memcpy(pp->value, ps, sz - 1);
+            ((char *)pp->value)[sz - 1] = 0;
+            dt_dprintk("fixed up name for %s -> %s\n", pathp,
+                       (char *)pp->value);
+        }
+    }
+    if ( allnextpp )
+    {
+        *prev_pp = NULL;
+        np->name = dt_get_property(np, "name", NULL);
+        np->type = dt_get_property(np, "device_type", NULL);
+
+        if ( !np->name )
+            np->name = "<NULL>";
+        if ( !np->type )
+            np->type = "<NULL>";
+    }
+    while ( tag == FDT_BEGIN_NODE || tag == FDT_NOP )
+    {
+        if ( tag == FDT_NOP )
+            *p += 4;
+        else
+            mem = unflatten_dt_node(fdt, mem, p, np, allnextpp, fpsize);
+        tag = be32_to_cpup((__be32 *)(*p));
+    }
+    if ( tag != FDT_END_NODE )
+    {
+        dt_printk(XENLOG_WARNING "Weird tag at end of node: %x\n", tag);
+        return mem;
+    }
+
+    *p += 4;
+    return mem;
+}
+
+/**
+ * __unflatten_device_tree - create tree of device_nodes from flat blob
+ *
+ * unflattens a device-tree, creating the
+ * tree of struct device_node. It also fills the "name" and "type"
+ * pointers of the nodes so the normal device-tree walking functions
+ * can be used.
+ * @fdt: The fdt to expand
+ * @mynodes: The device_node tree created by the call
+ */
+static void __init __unflatten_device_tree(const void *fdt,
+                                           struct dt_device_node **mynodes)
+{
+    unsigned long start, mem, size;
+    struct dt_device_node **allnextp = mynodes;
+
+    dt_dprintk(" -> unflatten_device_tree()\n");
+
+    dt_dprintk("Unflattening device tree:\n");
+    dt_dprintk("magic: %#08x\n", fdt_magic(fdt));
+    dt_dprintk("size: %#08x\n", fdt_totalsize(fdt));
+    dt_dprintk("version: %#08x\n", fdt_version(fdt));
+
+    /* First pass, scan for size */
+    start = ((unsigned long)fdt) + fdt_off_dt_struct(fdt);
+    size = unflatten_dt_node(fdt, 0, &start, NULL, NULL, 0);
+    size = (size | 3) + 1;
+
+    dt_dprintk("  size is %#lx allocating...\n", size);
+
+    /* Allocate memory for the expanded device tree */
+    mem = (unsigned long)_xmalloc (size + 4, __alignof__(struct dt_device_node));
+
+    ((__be32 *)mem)[size / 4] = cpu_to_be32(0xdeadbeef);
+
+    dt_dprintk("  unflattening %lx...\n", mem);
+
+    /* Second pass, do actual unflattening */
+    start = ((unsigned long)fdt) + fdt_off_dt_struct(fdt);
+    unflatten_dt_node(fdt, mem, &start, NULL, &allnextp, 0);
+    if ( be32_to_cpup((__be32 *)start) != FDT_END )
+        dt_printk(XENLOG_WARNING "Weird tag at end of tree: %08x\n",
+                  *((u32 *)start));
+    if ( be32_to_cpu(((__be32 *)mem)[size / 4]) != 0xdeadbeef )
+        dt_printk(XENLOG_WARNING "End of tree marker overwritten: %08x\n",
+                  be32_to_cpu(((__be32 *)mem)[size / 4]));
+    *allnextp = NULL;
+
+    dt_dprintk(" <- unflatten_device_tree()\n");
+}
+
+static void dt_alias_add(struct dt_alias_prop *ap,
+                         struct dt_device_node *np,
+                         int id, const char *stem, int stem_len)
+{
+    ap->np = np;
+    ap->id = id;
+    strlcpy(ap->stem, stem, stem_len + 1);
+    list_add_tail(&ap->link, &aliases_lookup);
+    dt_dprintk("adding DT alias:%s: stem=%s id=%d node=%s\n",
+               ap->alias, ap->stem, ap->id, dt_node_full_name(np));
+}
+
+/**
+ * dt_alias_scan - Scan all properties of 'aliases' node
+ *
+ * The function scans all the properties of 'aliases' node and populate
+ * the the global lookup table with the properties.  It returns the
+ * number of alias_prop found, or error code in error case.
+ */
+static void __init dt_alias_scan(void)
+{
+    const struct dt_property *pp;
+    const struct dt_device_node *aliases;
+
+    aliases = dt_find_node_by_path("/aliases");
+    if ( !aliases )
+        return;
+
+    for_each_property_of_node( aliases, pp )
+    {
+        const char *start = pp->name;
+        const char *end = start + strlen(start);
+        struct dt_device_node *np;
+        struct dt_alias_prop *ap;
+        int id, len;
+
+        /* Skip those we do not want to proceed */
+        if ( !strcmp(pp->name, "name") ||
+             !strcmp(pp->name, "phandle") ||
+             !strcmp(pp->name, "linux,phandle") )
+            continue;
+
+        np = dt_find_node_by_path(pp->value);
+        if ( !np )
+            continue;
+
+        /* walk the alias backwards to extract the id and work out
+         * the 'stem' string */
+        while ( isdigit(*(end-1)) && end > start )
+            end--;
+        len = end - start;
+
+        id = simple_strtoll(end, NULL, 10);
+
+        /* Allocate an alias_prop with enough space for the stem */
+        ap = _xmalloc(sizeof(*ap) + len + 1, 4);
+        if ( !ap )
+            continue;
+        ap->alias = start;
+        dt_alias_add(ap, np, id, start, len);
+    }
+}
+
+void __init dt_unflatten_host_device_tree(void)
+{
+    __unflatten_device_tree(device_tree_flattened, &dt_host);
+    dt_alias_scan();
+}
+
+void __init dt_switch_to_printk(void)
+{
+    dt_printk = printk;
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/xen/include/xen/device_tree.h b/xen/include/xen/device_tree.h
index 19bda98..8f526d1 100644
--- a/xen/include/xen/device_tree.h
+++ b/xen/include/xen/device_tree.h
@@ -10,6 +10,10 @@ 
 #ifndef __XEN_DEVICE_TREE_H__
 #define __XEN_DEVICE_TREE_H__
 
+#include <asm/byteorder.h>
+#include <public/xen.h>
+#include <xen/init.h>
+#include <xen/string.h>
 #include <xen/types.h>
 
 #define DEVICE_TREE_MAX_DEPTH 16
@@ -52,6 +56,51 @@  struct dt_early_info {
     struct dt_module_info modules;
 };
 
+typedef u32 dt_phandle;
+
+/**
+ * dt_property - describe a property for a device
+ * @name: name of the property
+ * @length: size of the value
+ * @value: pointer to data contained in the property
+ * @next: pointer to the next property of a specific node
+ */
+struct dt_property {
+    const char *name;
+    u32 length;
+    void *value;
+    struct dt_property *next;
+};
+
+#define DT_USED_BY_XEN DOMID_XEN
+
+/**
+ * dt_device_node - describe a node in the device tree
+ * @name: name of the node
+ * @type: type of the node (ie: memory, cpu, ...)
+ * @full_name: full name, it's composed of all the ascendant name separate by /
+ * @used_by: who owns the node? (ie: xen, dom0...)
+ * @properties: list of properties for the node
+ * @child: pointer to the first child
+ * @sibling: pointer to the next sibling
+ * @allnext: pointer to the next in list of all nodes
+ */
+struct dt_device_node {
+    const char *name;
+    const char *type;
+    dt_phandle phandle;
+    char *full_name;
+    domid_t used_by; /* By default it's used by dom0 */
+
+    struct dt_property *properties;
+    struct dt_device_node *parent;
+    struct dt_device_node *child;
+    struct dt_device_node *sibling;
+    struct dt_device_node *next; /* TODO: Remove it. Only use to know the last children */
+    struct dt_device_node *allnext;
+
+};
+
 typedef int (*device_tree_node_func)(const void *fdt,
                                      int node, const char *name, int depth,
                                      u32 address_cells, u32 size_cells,
@@ -77,4 +126,53 @@  int device_tree_for_each_node(const void *fdt,
 const char *device_tree_bootargs(const void *fdt);
 void device_tree_dump(const void *fdt);
 
+/**
+ * dt_unflatten_host_device_tree - Unflatten the host device tree
+ *
+ * Create a hierarchical device tree for the host DTB to be able
+ * to retrieve parents.
+ */
+void __init dt_unflatten_host_device_tree(void);
+
+/**
+ * Dump device tree message with printk
+ * TODO: Find another way to switch between early_printk and printk
+ * int the device tree code
+ */
+void __init dt_switch_to_printk(void);
+
+/**
+ * Host device tree
+ * DO NOT modify it!
+ */
+extern struct dt_device_node *dt_host;
+
+#define dt_node_cmp(s1, s2) strcmp((s1), (s2))
+#define dt_compat_cmp(s1, s2, l) strnicmp((s1), (s2), l)
+
+#define for_each_property_of_node(dn, pp)                   \
+    for ( pp = dn->properties; pp != NULL; pp = pp->next )
+
+#define for_each_device_node(dt, dn)                         \
+    for ( dn = dt; dn != NULL; dn = dn->allnext )
+
+static inline const char *dt_node_full_name(const struct dt_device_node *np)
+{
+    return (np && np->full_name) ? np->full_name : "<no-node>";
+}
+
+/**
+ * Find a property with a given name for a given node
+ * and return the value.
+ */
+const void *dt_get_property(const struct dt_device_node *np,
+                            const char *name, u32 *lenp);
+
+/**
+ * dt_find_node_by_path - Find a node matching a full DT path
+ * @path: The full path to match
+ *
+ * Returns a node pointer.
+ */
+struct dt_device_node *dt_find_node_by_path(const char *path);
 #endif