of/flattree: Merge unflatten_dt_node

Merge common code between PowerPC and MicroBlaze

Signed-off-by: Grant Likely <grant.likely@secretlab.ca>
Reviewed-by: Wolfram Sang <w.sang@pengutronix.de>
Tested-by: Michal Simek <monstr@monstr.eu>

1 files changed, 200 insertions, 0 deletions

diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index 5cdd958db9af..6852ecf6d1e1 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -166,3 +166,203 @@ int __init of_flat_dt_is_compatible(unsigned long node, const char *compat)
         return 0;
 }
 
+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;
+}
+
+/**
+ * unflatten_dt_node - Alloc and populate a device_node from the flat tree
+ * @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.
+ */
+unsigned long __init unflatten_dt_node(unsigned long mem,
+                                        unsigned long *p,
+                                        struct device_node *dad,
+                                        struct device_node ***allnextpp,
+                                        unsigned long fpsize)
+{
+        struct device_node *np;
+        struct property *pp, **prev_pp = NULL;
+        char *pathp;
+        u32 tag;
+        unsigned int l, allocl;
+        int has_name = 0;
+        int new_format = 0;
+
+        tag = *((u32 *)(*p));
+        if (tag != OF_DT_BEGIN_NODE) {
+                pr_err("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 device_node) + allocl,
+                                __alignof__(struct device_node));
+        if (allnextpp) {
+                memset(np, 0, sizeof(*np));
+                np->full_name = ((char *)np) + sizeof(struct device_node);
+                if (new_format) {
+                        char *fn = np->full_name;
+                        /* rebuild full path for new format */
+                        if (dad && dad->parent) {
+                                strcpy(fn, dad->full_name);
+#ifdef DEBUG
+                                if ((strlen(fn) + l + 1) != allocl) {
+                                        pr_debug("%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;
+                }
+                kref_init(&np->kref);
+        }
+        while (1) {
+                u32 sz, noff;
+                char *pname;
+
+                tag = *((u32 *)(*p));
+                if (tag == OF_DT_NOP) {
+                        *p += 4;
+                        continue;
+                }
+                if (tag != OF_DT_PROP)
+                        break;
+                *p += 4;
+                sz = *((u32 *)(*p));
+                noff = *((u32 *)((*p) + 4));
+                *p += 8;
+                if (initial_boot_params->version < 0x10)
+                        *p = _ALIGN(*p, sz >= 8 ? 8 : 4);
+
+                pname = find_flat_dt_string(noff);
+                if (pname == NULL) {
+                        pr_info("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 property),
+                                        __alignof__(struct property));
+                if (allnextpp) {
+                        if (strcmp(pname, "linux,phandle") == 0) {
+                                np->node = *((u32 *)*p);
+                                if (np->linux_phandle == 0)
+                                        np->linux_phandle = np->node;
+                        }
+                        if (strcmp(pname, "ibm,phandle") == 0)
+                                np->linux_phandle = *((u32 *)*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 property) + sz,
+                                        __alignof__(struct 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;
+                        pr_debug("fixed up name for %s -> %s\n", pathp,
+                                (char *)pp->value);
+                }
+        }
+        if (allnextpp) {
+                *prev_pp = NULL;
+                np->name = of_get_property(np, "name", NULL);
+                np->type = of_get_property(np, "device_type", NULL);
+
+                if (!np->name)
+                        np->name = "<NULL>";
+                if (!np->type)
+                        np->type = "<NULL>";
+        }
+        while (tag == OF_DT_BEGIN_NODE) {
+                mem = unflatten_dt_node(mem, p, np, allnextpp, fpsize);
+                tag = *((u32 *)(*p));
+        }
+        if (tag != OF_DT_END_NODE) {
+                pr_err("Weird tag at end of node: %x\n", tag);
+                return mem;
+        }
+        *p += 4;
+        return mem;
+}