diff --git a/lib/libfdt/libfdt_env.h b/lib/libfdt/libfdt_env.h
index 213d7fb..a45ff00 100644
--- a/lib/libfdt/libfdt_env.h
+++ b/lib/libfdt/libfdt_env.h
@@ -4,6 +4,9 @@
 #include <stddef.h>
 #include <stdint.h>
 #include <string.h>
+#include <km_main.h>
+
+typedef uint32_t fdt32_t;
 
 #define EXTRACT_BYTE(n)	((unsigned long long)((uint8_t *)&x)[n])
 static inline uint16_t fdt16_to_cpu(uint16_t x)
@@ -26,4 +29,106 @@
 #define cpu_to_fdt64(x) fdt64_to_cpu(x)
 #undef EXTRACT_BYTE
 
+#define toupper(a)  ((((a) >= 'a') && ((a) <= 'z')) ? ((a) - 'a' + 'A') : (a))
+#define isalpha(chr) (('a' <= chr && chr <= 'z') || ('A' <= chr && chr <= 'Z'))
+
+static bool isalnum(unsigned char Chr)
+{
+        return (('0' <= Chr && Chr <= '9') ||
+                ('A' <= Chr && Chr <= 'Z') ||
+                ('a' <= Chr && Chr <= 'z')
+                );
+}
+
+static int Digit2Val( int c)
+{
+        if(isalpha(c)) {  /* If c is one of [A-Za-z]... */
+                c = toupper(c) - 7;   // Adjust so 'A' is ('9' + 1)
+        }
+        return c - '0';   // Value returned is between 0 and 35, inclusive.
+}
+
+
+static inline int isspace (int c)
+{
+  //
+  // <space> ::= [ ]
+  //
+        return ((c) == ' ');
+}
+
+/** The strtoul function converts the initial portion of the string pointed to
+    by nptr to unsigned long int representation.
+
+    See the description for strtol for more information.
+
+  @return   The strtoul function returns the converted value, if any. If no
+            conversion could be performed, zero is returned. If the correct
+            value is outside the range of representable values, ULONG_MAX is
+            returned and the value of the macro ERANGE is stored in errno.
+**/
+static inline unsigned long
+strtoul(const char * __restrict nptr, char ** __restrict endptr, int base)
+{
+        const char     *pEnd;
+        unsigned long   Result = 0;
+        unsigned long   Previous;
+        int             temp;
+
+        pEnd = nptr;
+
+        if((base < 0) || (base == 1) || (base > 36)) {
+                if(endptr != NULL) {
+                        *endptr = NULL;
+                }
+                return 0;
+        }
+// Skip leading spaces.
+        while(isspace(*nptr))   ++nptr;
+
+// Process Subject sequence: optional + sign followed by digits.
+        if(*nptr == '+') {
+                ++nptr;
+        }
+
+        if(*nptr == '0') {  /* Might be Octal or Hex */
+                if(toupper(nptr[1]) == 'X') {   /* Looks like Hex */
+                        if((base == 0) || (base == 16)) {
+                                nptr += 2;  /* Skip the "0X"      */
+                                base = 16;  /* In case base was 0 */
+                        }
+                }
+                else {    /* Looks like Octal */
+                        if((base == 0) || (base == 8)) {
+                                ++nptr;     /* Skip the leading "0" */
+                                base = 8;   /* In case base was 0   */
+                        }
+                }
+        }
+        if(base == 0) {   /* If still zero then must be decimal */
+                base = 10;
+
+	}
+        if(*nptr  == '0') {
+                for( ; *nptr == '0'; ++nptr);  /* Skip any remaining leading zeros */
+                pEnd = nptr;
+	}
+
+        while( isalnum(*nptr) && ((temp = Digit2Val(*nptr)) < base)) {
+                Previous = Result;
+                Result = (Result * base) + (unsigned long)temp;
+                if( Result < Previous)  {   // If we overflowed
+                        Result = UINT32_MAX;
+                        //errno = -1;
+                        break;
+                }
+                pEnd = ++nptr;
+        }
+// Save pointer to final sequence
+        if(endptr != NULL) {
+                *endptr = (char *)pEnd;
+        }
+        return Result;
+}
+
 #endif /* _LIBFDT_ENV_H */
diff --git a/lib/libufdt/fdt_internal.h b/lib/libufdt/fdt_internal.h
new file mode 100644
index 0000000..1ac9d68
--- /dev/null
+++ b/lib/libufdt/fdt_internal.h
@@ -0,0 +1,145 @@
+#ifndef FDT_INTERNAL_H
+#define FDT_INTERNAL_H
+
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *     1. Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *     2. Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ *     THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *     CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *     INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *     MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *     DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ *     CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *     SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ *     NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ *     HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ *     CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ *     OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "libfdt.h"
+#include "libufdt_sysdeps.h"
+
+
+/*
+ * Most of the codes below are copied from external/dtc/libfdt/libfdt_internal.h
+ * and external/dtc/libfdt/fdt_sw.c .
+ */
+
+#define FDT_ALIGN(x, a) (((x) + (a)-1) & ~((a)-1))
+#define FDT_TAGALIGN(x) (FDT_ALIGN((x), FDT_TAGSIZE))
+
+static inline const void *_fdt_offset_ptr(const void *fdt, int offset) {
+  return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
+}
+
+static inline void *_fdt_offset_ptr_w(void *fdt, int offset) {
+  return (void *)(uintptr_t)_fdt_offset_ptr(fdt, offset);
+}
+
+#define FDT_SW_MAGIC (~FDT_MAGIC)
+
+static int _fdt_sw_check_header(void *fdt) {
+  if (fdt_magic(fdt) != FDT_SW_MAGIC) return -FDT_ERR_BADMAGIC;
+  /* FIXME: should check more details about the header state */
+  return 0;
+}
+
+#define FDT_SW_CHECK_HEADER(fdt)                            \
+  {                                                         \
+    int err;                                                \
+    if ((err = _fdt_sw_check_header(fdt)) != 0) return err; \
+  }
+
+static void *_fdt_grab_space(void *fdt, size_t len) {
+  int offset = fdt_size_dt_struct(fdt);
+  int spaceleft;
+
+  spaceleft =
+      fdt_totalsize(fdt) - fdt_off_dt_struct(fdt) - fdt_size_dt_strings(fdt);
+
+  if ((offset + (int)len < offset) || (offset + (int)len > spaceleft))
+    return NULL;
+
+  fdt_set_size_dt_struct(fdt, offset + len);
+  return _fdt_offset_ptr_w(fdt, offset);
+}
+
+static int _fdt_add_string(void *fdt, const char *s) {
+  char *strtab = (char *)fdt + fdt_totalsize(fdt);
+  int strtabsize = fdt_size_dt_strings(fdt);
+  int len = dto_strlen(s) + 1;
+  int struct_top, offset;
+
+  /* Add it */
+  offset = -strtabsize - len;
+  struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+  if (fdt_totalsize(fdt) + offset < (size_t)struct_top)
+    return 0; /* no more room :( */
+
+  dto_memcpy(strtab + offset, s, len);
+  fdt_set_size_dt_strings(fdt, strtabsize + len);
+  return offset;
+}
+
+/*
+ * From Google, speed up fdt_property() by passing nameoff to the function.
+ * Adds new string to fdt if *pnameoff is 0.
+ * Otherwise, use the nameoff provided.
+ */
+static int fast_fdt_sw_property(void *fdt, const char *name, const void *val,
+                                int len, int *pnameoff,
+                                struct fdt_property **prop_ptr) {
+  FDT_SW_CHECK_HEADER(fdt);
+
+  if (*pnameoff == 0) {
+    *pnameoff = _fdt_add_string(fdt, name);
+    if (*pnameoff == 0) return -FDT_ERR_NOSPACE;
+  }
+
+  *prop_ptr = _fdt_grab_space(fdt, sizeof(**prop_ptr) + FDT_TAGALIGN(len));
+  if (*prop_ptr == NULL) return -FDT_ERR_NOSPACE;
+
+  (*prop_ptr)->tag = cpu_to_fdt32(FDT_PROP);
+  (*prop_ptr)->nameoff = cpu_to_fdt32(*pnameoff);
+  (*prop_ptr)->len = cpu_to_fdt32(len);
+  dto_memcpy((*prop_ptr)->data, val, len);
+  return 0;
+}
+
+#endif /* FDT_INTERNAL_H */
diff --git a/lib/libufdt/include/libufdt.h b/lib/libufdt/include/libufdt.h
new file mode 100644
index 0000000..d966afb
--- /dev/null
+++ b/lib/libufdt/include/libufdt.h
@@ -0,0 +1,434 @@
+
+#ifndef LIBUFDT_H
+#define LIBUFDT_H
+
+#include "libufdt_sysdeps.h"
+#include "ufdt_types.h"
+#include <libfdt_env.h>
+
+#define false 0
+#define true 1
+
+/*
+ * BEGIN of ufdt_node_dict methods
+ * Since in the current implementation, it's actually a hash table.
+ * So most of operation's time complexity are O(1) with high probability
+ * (w.h.p.).
+ */
+
+/*
+ * Allocates some new spaces and creates a new ufdt_node_dict.
+ *
+ * @return: a pointer to the newly created ufdt_node_dict or
+ *          NULL if dto_malloc failed
+ */
+struct ufdt_node_dict ufdt_node_dict_construct();
+
+/*
+ * Frees all space dto_malloced, not including ufdt_nodes in the table.
+ */
+void ufdt_node_dict_destruct(struct ufdt_node_dict *dict);
+
+/*
+ * Adds a ufdt_node (as pointer) to the ufdt_node_dict.
+ * @return: 0 if success
+ *          < 0 otherwise
+ *
+ * @Time: O(length of node->name) w.h.p.
+ */
+int ufdt_node_dict_add(struct ufdt_node_dict *dict, struct ufdt_node *node);
+
+/*
+ * Returns the pointer to the entry in ufdt_node_dict with node->name =
+ * name[0..len-1], for direct modification of the entry.
+ * e.g., Delete an entry from the ufdt_node_dict.
+ *
+ * @return: a pointer to the entry in ufdt_node_dict or
+ *          NULL if no such entry.
+ *
+ * @Time: O(len = |name|) w.h.p.
+ */
+struct ufdt_node **ufdt_node_dict_find_len(struct ufdt_node_dict *dict,
+                                           const char *name, int len);
+struct ufdt_node **ufdt_node_dict_find(struct ufdt_node_dict *dict,
+                                       const char *name);
+
+/*
+ * Returns the pointer to the ufdt_node with node->name =
+ * name[0..len-1], for direct modification of the node.
+ *
+ * @return: a pointer to the node or
+ *          NULL if no such node in ufdt_node_dict with same name.
+ *
+ * @Time: O(len = |name|) w.h.p.
+ */
+struct ufdt_node *ufdt_node_dict_find_node_len(struct ufdt_node_dict *dict,
+                                               const char *name, int len);
+struct ufdt_node *ufdt_node_dict_find_node(struct ufdt_node_dict *dict,
+                                           const char *name);
+
+/*
+ * Prints the each (index, node->name) pair in the dict to stdout in the
+ * following format:
+ * ```
+ * [idx0] [name0]
+ * [idx1] [name1]
+ * ...
+ * ```
+ */
+void ufdt_node_dict_print(struct ufdt_node_dict *dict);
+
+/*
+ * END of ufdt_node_dict methods
+ */
+
+/*
+ * BEGIN of ufdt_node methods
+ */
+
+/*
+ * Allocates spaces for new ufdt_node who represents a fdt node at fdt_tag_ptr.
+ * In order to get name pointer, it's neccassary to give the pointer to the
+ * entire fdt it belongs to.
+ *
+ *
+ * @return: a pointer to the newly created ufdt_node or
+ *          NULL if dto_malloc failed
+ */
+struct ufdt_node *ufdt_node_construct(void *fdtp, fdt32_t *fdt_tag_ptr);
+
+/*
+ * Frees all nodes in the subtree rooted at *node.
+ * Also dto_frees those ufdt_node_dicts in each node.
+ */
+void ufdt_node_destruct(struct ufdt_node *node);
+
+/*
+ * Adds the child as a subnode of the parent.
+ * It's been done by add entries in parent->prop_list or node_list depending on
+ * the tag type of child.
+ *
+ * @return: 0 if success
+ *          < 0 otherwise
+ *
+ * @Time: O(1) w.h.p.
+ */
+int ufdt_node_add_child(struct ufdt_node *parent, struct ufdt_node *child);
+
+/* BEGIN of FDT_PROP related functions .*/
+
+/*
+ * Gets pointer to FDT_PROP subnode of node with name equals to name[0..len-1]
+ *
+ * @return: a pointer to the subnode or
+ *          NULL if no such subnode.
+ *
+ * @Time: O(len = length of name) w.h.p.
+ */
+struct ufdt_node *ufdt_node_get_property_by_name_len(
+    const struct ufdt_node *node, const char *name, int len);
+struct ufdt_node *ufdt_node_get_property_by_name(const struct ufdt_node *node,
+                                                 const char *name);
+
+/*
+ * Gets the pointer to the FDT_PROP node's data in the corresponding fdt.
+ * Also writes the length of such data to *out_len if out_len is not NULL.
+ *
+ * @return: a pointer to some data located in fdt or
+ *          NULL if *node is not a FDT_PROP
+ */
+char *ufdt_node_get_fdt_prop_data(const struct ufdt_node *node, int *out_len);
+
+/*
+ * Gets pointer to FDT_PROP node's data in fdt with name equals to
+ * name[0..len-1], which is a subnode of *node.
+ * It's actually a composition of ufdt_node_get_property_by_name and
+ * ufdt_node_get_fdt_prop_data
+ *
+ * @return: a pointer to some data located in fdt or
+ *          NULL if no such subnode.
+ *
+ * @Time: O(len = length of name) w.h.p.
+ */
+char *ufdt_node_get_fdt_prop_data_by_name_len(const struct ufdt_node *node,
+                                              const char *name, int len,
+                                              int *out_len);
+char *ufdt_node_get_fdt_prop_data_by_name(const struct ufdt_node *node,
+                                          const char *name, int *out_len);
+
+/* END of FDT_PROP related functions .*/
+
+/*
+ * Gets pointer to FDT_BEGIN_NODE subnode of node with name equals to
+ * name[0..len-1].
+ *
+ * @return: a pointer to the subnode or
+ *          NULL if no such subnode.
+ *
+ * @Time: O(len = length of name) w.h.p.
+ */
+
+struct ufdt_node *ufdt_node_get_subnode_by_name_len(const struct ufdt_node *node,
+                                                    const char *name, int len);
+struct ufdt_node *ufdt_node_get_subnode_by_name(const struct ufdt_node *node,
+                                                const char *name);
+
+/*
+ * Gets the pointer to FDT_NODE node whose relative path to *node is
+ * path[0..len-1].
+ * Note that the relative path doesn't support parent node like:
+ * "../path/to/node".
+ *
+ * @return: a pointer to the node or
+ *          NULL if no such node.
+ *
+ * @Time: O(len = length of path) w.h.p.
+ */
+struct ufdt_node *ufdt_node_get_node_by_path_len(const struct ufdt_node *node,
+                                                 const char *path, int len);
+struct ufdt_node *ufdt_node_get_node_by_path(const struct ufdt_node *node,
+                                             const char *path);
+
+/*
+ * Gets the phandle value of the node if it has.
+ *
+ * @return: phandle value of that node or
+ *          0 if *node is not FDT_NODE or there's no "phandle"/"linux,phandle"
+ *          property.
+ *
+ * @Time: O(1) w.h.p.
+ */
+uint32_t ufdt_node_get_phandle(const struct ufdt_node *node);
+
+/*
+ * END of ufdt_node methods
+ */
+
+/*
+ * BEGIN of ufdt methods.
+ */
+
+/*
+ * Constructs a ufdt whose base fdt is fdtp.
+ * Note that this function doesn't construct the entire tree.
+ * To get the whole tree please call `fdt_to_ufdt(fdtp, fdt_size)`
+ *
+ * @return: an empty ufdt with base fdtp = fdtp
+ */
+struct ufdt *ufdt_construct(void *fdtp);
+
+/*
+ * Frees the space occupied by the ufdt, including all ufdt_nodes and
+ * ufdt_node_dicts along
+ * with static_phandle_table.
+ */
+void ufdt_destruct(struct ufdt *tree);
+
+/*
+ * Gets the pointer to the ufdt_node in tree with phandle = phandle.
+ * The function do a binary search in tree->phandle_table.
+ *
+ * @return: a pointer to the target ufdt_node
+ *          NULL if no ufdt_node has phandle = phandle
+ *
+ * @Time: O(log(# of nodes in tree)) = O(log(size of underlying fdt))
+ */
+struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree, uint32_t phandle);
+
+/*
+ * Gets the pointer to the ufdt_node in tree with absoulte path =
+ * path[0..len-1].
+ * Absolute path has form "/path/to/node" or "some_alias/to/node".
+ * In later example, some_alias is a property in "/aliases" with data is a path
+ * to some node X. Then the funcion will return node with relative
+ * path = "to/node" w.r.t. X.
+ *
+ * @return: a pointer to the target ufdt_node or
+ *          NULL if such dnt doesn't exist.
+ *
+ * @Time: O(len = length of path) w.h.p.
+ */
+struct ufdt_node *ufdt_get_node_by_path_len(struct ufdt *tree, const char *path,
+                                            int len);
+struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path);
+
+/*
+ * END of ufdt methods.
+ */
+
+/*
+ * Compares function between 2 nodes, compare by name of each node.
+ *
+ * @return: x < 0  if a's name is lexicographically smaller
+ *          x == 0 if a, b has same name
+ *          x > 0  if a's name is lexicographically bigger
+ */
+int node_cmp(const void *a, const void *b);
+
+/*
+ * Determines whether node->name equals to name[0..len-1]
+ *
+ * @return: true if they're equal.
+ *          false otherwise
+ */
+bool node_name_eq(const struct ufdt_node *node, const char *name, int len);
+
+/*
+ * Merges tree_b into tree_a with tree_b has all nodes except root disappeared.
+ * Overwrite property in tree_a if there's one with same name in tree_b.
+ * Otherwise add the property to tree_a.
+ * For subnodes with the same name, recursively run this function.
+ *
+ * Ex:
+ * tree_a : ta {
+ *  b = "b";
+ *  c = "c";
+ *  d {
+ *    e = "g";
+ *  };
+ * };
+ *
+ * tree_b : tb {
+ *  c = "C";
+ *  g = "G";
+ *  d {
+ *    da = "dad";
+ *  };
+ *  h {
+ *    hh = "HH";
+ *  };
+ * };
+ *
+ * The resulting trees will be:
+ *
+ * tree_a : ta {
+ *  b = "b";
+ *  c = "C";
+ *  g = "G";
+ *  d {
+ *    da = "dad";
+ *    e = "g";
+ *  };
+ *  h {
+ *    hh = "HH";
+ *  };
+ * };
+ *
+ * tree_b : tb {
+ * };
+ *
+ *
+ * @return: 0 if merge success
+ *          < 0 otherwise
+ *
+ * @Time: O(# of nodes in tree_b + total length of all names in tree_b) w.h.p.
+ */
+int merge_ufdt_into(struct ufdt_node *tree_a, struct ufdt_node *tree_b);
+
+/*
+ * BEGIN of ufdt output functions
+ */
+
+/*
+ * Builds the ufdt for FDT pointed by fdtp.
+ * This including build all ufdt_nodes and ufdt_node_dicts, and builds the
+ * phandle table as
+ * well.
+ *
+ * @return: the ufdt T representing fdtp or
+ *          T with T.fdtp == NULL if fdtp is unvalid.
+ *
+ * @Time: O(fdt_size + nlogn) where n = # of nodes in fdt.
+ */
+struct ufdt *fdt_to_ufdt(void *fdtp, size_t fdt_size);
+
+/*
+ * Sequentially dumps the tree rooted at *node to FDT buffer fdtp.
+ * Basically it calls functions provided by libfdt/fdt_sw.c.
+ *
+ * All those functions works fast.
+ * But when it comes to dump property node to fdt, the function they
+ * provide(fdt_property()) is really slow. Since it runs through all strings
+ * stored in fdt to find the right `nameoff` for the property node.
+ *
+ * So we implement our own fdt_property() called `output_property_to_fdt()`, the
+ * basic
+ * idea is that we keep a hash table that we can search for the nameoff of the
+ * string in constant time instead of O(total length of strings) search.
+ *
+ * @return: 0 if successfully dump or
+ *          < 0 otherwise
+ *
+ * @Time: O(total length of all names + # of nodes in subtree rooted at *root)
+ */
+int output_ufdt_node_to_fdt(struct ufdt_node *node, void *fdtp,
+                            struct ufdt_node_dict *all_props);
+
+/*
+ * Sequentially dumps the whole ufdt to FDT buffer fdtp with buffer size
+ * buf_size.
+ * Basically it calls functions provided by libfdt/fdt_sw.c.
+ * The main overhead here is to dump the tree to fdtp by calling
+ * output_ufdt_node_to_fdt().
+ *
+ *
+ * @return: 0 if successfully dump or
+ *          < 0 otherwise
+ *
+ * @Time: O(total length of all names + # of nodes in tree)
+ */
+int ufdt_to_fdt(struct ufdt *tree, void *buf, int buf_size);
+
+/*
+ * prints the entire subtree rooted at *node in form:
+ * NODE :[node name]:
+ *   PROP :[prop name]:
+ *   ...
+ *   NODE :[subnode1 name]:
+ *     ...
+ *   NODE :[subnode1 name]:
+ *     ...
+ *   ...
+ * There're (depth * TAB_SIZE) spaces in front of each line.
+ */
+void ufdt_node_print(const struct ufdt_node *node, int depth);
+
+/*
+ * It's just ufdt_node_print(tree->root, 0).
+ */
+void ufdt_print(struct ufdt *tree);
+
+/*
+ * END of ufdt output functions
+ */
+
+/*
+ * Runs closure.func(node, closure.env) for all nodes in subtree rooted at
+ * *node.
+ * The order of each node being applied by the function is depth first.
+ * Basically it's the same order as the order printed in ufdt_node_print().
+ *
+ * Example:
+ *
+ * void print_name(struct ufdt_node *node, void *env) {
+ *   printf("%s\n", node->name);
+ * }
+ *
+ * struct ufdt_node_closure clos;
+ * clos.func = print_name;
+ * clos.env = NULL;
+ * ufdt_map(tree, clos);
+ *
+ * Then you can print all names of nodes in tree.
+ *
+ * @Time: O((# of nodes in subtree rooted at *node) * avg. running time of the
+ * function closure.func)
+ */
+void ufdt_node_map(struct ufdt_node *node, struct ufdt_node_closure closure);
+
+/*
+ * It's just ufdt_node_map(tree->root, closure);
+ */
+void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure);
+
+#endif /* LIBUFDT_H */
diff --git a/lib/libufdt/include/ufdt_overlay.h b/lib/libufdt/include/ufdt_overlay.h
new file mode 100644
index 0000000..c2069a7
--- /dev/null
+++ b/lib/libufdt/include/ufdt_overlay.h
@@ -0,0 +1,25 @@
+#ifndef UFDT_OVERLAY_H
+#define UFDT_OVERLAY_H
+
+#include <libfdt.h>
+/* Given a buffer in RAM containing the contents of a .dtb file,
+ * it initializes an FDT in-place and returns a pointer to the
+ * given buffer, or NULL in case of error.
+ * In case of error, it may printf() diagnostic messages.
+ */
+struct fdt_header *ufdt_install_blob(void *blob, size_t blob_size);
+
+/* Given a main_fdt_header buffer and an overlay_fdtp buffer containing the
+ * contents of a .dtbo file, it creates a new FDT containing the applied
+ * overlay_fdtp in a dto_malloc'd buffer and returns it, or NULL in case of
+ * error.
+ * It is allowed to modify the buffers (both main_fdt_header and overlay_fdtp
+ * buffer) passed in.
+ * It does not dto_free main_fdt_header and overlay_fdtp buffer passed in.
+ */
+struct fdt_header *ufdt_apply_overlay(struct fdt_header *main_fdt_header,
+                                      size_t main_fdt_size,
+                                      void *overlay_fdtp,
+                                      size_t overlay_size);
+
+#endif /* UFDT_OVERLAY_H */
diff --git a/lib/libufdt/include/ufdt_types.h b/lib/libufdt/include/ufdt_types.h
new file mode 100644
index 0000000..a1d8118
--- /dev/null
+++ b/lib/libufdt/include/ufdt_types.h
@@ -0,0 +1,97 @@
+#ifndef UFDT_TYPES_H
+#define UFDT_TYPES_H
+
+#include <libfdt.h>
+
+#define ASCII_PRINT_S (32)
+#define ASCII_PRINT_E (128)
+#define ASCII_PRINT_SZ (ASCII_PRINT_E - ASCII_PRINT_S)
+
+#define FDT_PROP_DELI ':'
+#define FDT_NODE_DELI '/'
+
+#define DTNL_INIT_SZ 4
+
+/* Empirical values for hash functions. */
+#define HASH_BASE 13131
+
+/* it has type : struct ufdt_node** */
+#define for_each(it, node_dict)                                  \
+  if ((node_dict) != NULL)                                       \
+    for (it = (node_dict)->nodes;                                \
+         it != (node_dict)->nodes + (node_dict)->mem_size; ++it) \
+      if (*it)
+
+#define for_each_child(it, node)                                    \
+  if (tag_of(node) == FDT_BEGIN_NODE)                               \
+    for ((it) = &(((struct fdt_node_ufdt_node *)node)->child); *it; \
+         it = &((*it)->sibling))
+
+#define for_each_prop(it, node) \
+  for_each_child(it, node) if (tag_of(*it) == FDT_PROP)
+
+#define for_each_node(it, node) \
+  for_each_child(it, node) if (tag_of(*it) == FDT_BEGIN_NODE)
+
+/*
+ * Gets prop name from FDT requires complicated manipulation.
+ * To avoid the manipulation, the *name pointer in fdt_prop_ufdt_node
+ * is pointed to the final destination of the prop name in FDT.
+ * For the FDT_BEGIN_NODE name, it can be obtained from FDT directly.
+ */
+#define name_of(node)                                                    \
+  ((tag_of(node) == FDT_BEGIN_NODE)                                      \
+       ? (((const struct fdt_node_header *)((node)->fdt_tag_ptr))->name) \
+       : (((const struct fdt_prop_ufdt_node *)node)->name))
+
+struct ufdt_node {
+  fdt32_t *fdt_tag_ptr;
+  struct ufdt_node *sibling;
+};
+
+struct ufdt_node_dict {
+  int mem_size;
+  int num_used;
+  struct ufdt_node **nodes;
+};
+
+struct fdt_prop_ufdt_node {
+  struct ufdt_node parent;
+  const char *name;
+};
+
+struct fdt_node_ufdt_node {
+  struct ufdt_node parent;
+  struct ufdt_node *child;
+  struct ufdt_node **last_child_p;
+};
+
+struct phandle_table_entry {
+  uint32_t phandle;
+  struct ufdt_node *node;
+};
+
+struct static_phandle_table {
+  int len;
+  struct phandle_table_entry *data;
+};
+
+struct ufdt {
+  void *fdtp;
+  struct ufdt_node *root;
+  struct static_phandle_table phandle_table;
+};
+
+typedef void func_on_ufdt_node(struct ufdt_node *, void *);
+
+struct ufdt_node_closure {
+  func_on_ufdt_node *func;
+  void *env;
+};
+
+static uint32_t tag_of(const struct ufdt_node *node) {
+  if (!node) return FDT_END;
+  return fdt32_to_cpu(*node->fdt_tag_ptr);
+}
+
+#endif /* UFDT_TYPES_H */
diff --git a/lib/libufdt/rules.mk b/lib/libufdt/rules.mk
new file mode 100644
index 0000000..849b537
--- /dev/null
+++ b/lib/libufdt/rules.mk
@@ -0,0 +1,9 @@
+LOCAL_PATH := $(GET_LOCAL_DIR)
+
+LIBFDT_INCLUDES = ufdt_util.h fdt_internal.h ufdt_types.h ufdt_overlay.h libufdt.h
+LIBFDT_SRCS =  ufdt_overlay.c ufdt_node_dict.c ufdt_node.c ufdt_convert.c sysdeps/libufdt_sysdeps_vendor.c
+LIBFDT_OBJS = $(LIBFDT_SRCS:%.c=%.o)
+
+INCLUDES += -I$(LOCAL_PATH) -I$(LOCAL_PATH)/include -I$(LOCAL_PATH)/sysdeps/include
+
+OBJS += $(addprefix $(LOCAL_PATH)/, $(LIBFDT_OBJS))
diff --git a/lib/libufdt/sysdeps/include/libufdt_sysdeps.h b/lib/libufdt/sysdeps/include/libufdt_sysdeps.h
new file mode 100644
index 0000000..8efa43c
--- /dev/null
+++ b/lib/libufdt/sysdeps/include/libufdt_sysdeps.h
@@ -0,0 +1,65 @@
+#ifndef LIBDTOVERLAY_SYSDEPS_H
+#define LIBDTOVERLAY_SYSDEPS_H
+
+#ifdef INCLUDE_PLATFORM_HDRS
+/* Change these includes to match your platform to bring in the
+ * equivalent types available in a normal C runtime. At least things
+ * like uint8_t, uint64_t, and bool (with |false|, |true| keywords)
+ * must be present.
+ */
+#include <inttypes.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#else
+#include <libfdt_env.h>
+#endif
+
+#ifdef DTO_ENABLE_DEBUG
+/* Print functions, used for diagnostics.
+ *
+ * These have no effect unless FDT_ENABLE_DEBUG is defined.
+ */
+#define dto_debug(...)                   \
+  do {                                   \
+    dto_print("DEBUG: %s():", __func__); \
+    dto_print(__VA_ARGS__);              \
+  } while (0)
+#else
+#define dto_debug(...)
+#endif
+
+#define dto_error(...)                   \
+  do {                                   \
+    dto_print("ERROR: %s():", __func__); \
+    dto_print(__VA_ARGS__);              \
+  } while (0)
+
+int dto_print(const char *fmt, ...);
+
+void dto_qsort(void *base, size_t nmemb, size_t size,
+               int (*compar)(const void *, const void *));
+
+void *dto_malloc(size_t size);
+
+void dto_free(void *ptr);
+
+char *dto_strdup(const char *s);
+
+char *dto_strchr(const char *s, int c);
+
+unsigned long int dto_strtoul(const char *nptr, char **endptr, int base);
+
+size_t dto_strlen(const char *s);
+
+void *dto_memcpy(void *dest, const void *src, size_t n);
+
+int dto_strcmp(const char *s1, const char *s2);
+
+int dto_strncmp(const char *s1, const char *s2, size_t n);
+
+void *dto_memchr(const void *s, int c, size_t n);
+
+void *dto_memset(void *s, int c, size_t n);
+
+#endif /* LIBDTOVERLAY_SYSDEPS_H */
diff --git a/lib/libufdt/sysdeps/libufdt_sysdeps_vendor.c b/lib/libufdt/sysdeps/libufdt_sysdeps_vendor.c
new file mode 100644
index 0000000..44a26d2
--- /dev/null
+++ b/lib/libufdt/sysdeps/libufdt_sysdeps_vendor.c
@@ -0,0 +1,223 @@
+#include <malloc.h>
+#include <stdlib.h>
+
+#include "libufdt_sysdeps.h"
+#define LK_DTBO_ERROR -1
+
+#if INCLUDE_PLATFORM_HDRS
+#include <debug.h>
+#include <stdio.h>
+//#include <stdlib.h>
+#include <sys/types.h>
+int dto_print(const char *fmt, ...) {
+  int err;
+
+  va_list ap;
+  va_start(ap, fmt);
+  err = _dvprintf(fmt, ap);
+  va_end(ap);
+
+  return err;
+}
+#endif
+
+int dto_print(const char *fmt, ...) {
+  return LK_DTBO_ERROR;
+}
+/* Codes from
+ * https://android.googlesource.com/platform/bionic.git/+/eclair-release/libc/stdlib/qsort.c
+ * Start
+ */
+
+/* $OpenBSD: qsort.c,v 1.10 2005/08/08 08:05:37 espie Exp $ */
+/*-
+ * Copyright (c) 1992, 1993
+ * The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+static __inline char *med3(char *, char *, char *,
+                           int (*)(const void *, const void *));
+static __inline void swapfunc(char *, char *, int, int);
+#define min(a, b) (a) < (b) ? a : b
+
+/*
+ * Qsort routine from Bentley & McIlroy's "Engineering a Sort Function".
+ */
+#define swapcode(TYPE, parmi, parmj, n) \
+  {                                     \
+    long i = (n) / sizeof(TYPE);        \
+    TYPE *pi = (TYPE *)(parmi);         \
+    TYPE *pj = (TYPE *)(parmj);         \
+    do {                                \
+      TYPE t = *pi;                     \
+      *pi++ = *pj;                      \
+      *pj++ = t;                        \
+    } while (--i > 0);                  \
+  }
+#define SWAPINIT(a, es)                                                  \
+  swaptype = ((char *)a - (char *)0) % sizeof(long) || es % sizeof(long) \
+                 ? 2                                                     \
+                 : es == sizeof(long) ? 0 : 1;
+
+static __inline void swapfunc(char *a, char *b, int n, int swaptype) {
+  if (swaptype <= 1) swapcode(long, a, b, n) else swapcode(char, a, b, n)
+}
+
+#define swap(a, b)               \
+  if (swaptype == 0) {           \
+    long t = *(long *)(a);       \
+    *(long *)(a) = *(long *)(b); \
+    *(long *)(b) = t;            \
+  } else                         \
+    swapfunc(a, b, es, swaptype)
+#define vecswap(a, b, n) \
+  if ((n) > 0) swapfunc(a, b, n, swaptype)
+
+static __inline char *med3(char *a, char *b, char *c,
+                           int (*cmp)(const void *, const void *)) {
+  return cmp(a, b) < 0 ? (cmp(b, c) < 0 ? b : (cmp(a, c) < 0 ? c : a))
+                       : (cmp(b, c) > 0 ? b : (cmp(a, c) < 0 ? a : c));
+}
+
+void qsort(void *aa, size_t n, size_t es,
+           int (*cmp)(const void *, const void *)) {
+  char *pa, *pb, *pc, *pd, *pl, *pm, *pn;
+  int d, r, swaptype, swap_cnt;
+  char *a = aa;
+loop:
+  SWAPINIT(a, es);
+  swap_cnt = 0;
+  if (n < 7) {
+    for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es)
+      for (pl = pm; pl > (char *)a && cmp(pl - es, pl) > 0; pl -= es)
+        swap(pl, pl - es);
+    return;
+  }
+  pm = (char *)a + (n / 2) * es;
+  if (n > 7) {
+    pl = (char *)a;
+    pn = (char *)a + (n - 1) * es;
+    if (n > 40) {
+      d = (n / 8) * es;
+      pl = med3(pl, pl + d, pl + 2 * d, cmp);
+      pm = med3(pm - d, pm, pm + d, cmp);
+      pn = med3(pn - 2 * d, pn - d, pn, cmp);
+    }
+    pm = med3(pl, pm, pn, cmp);
+  }
+  swap(a, pm);
+  pa = pb = (char *)a + es;
+
+  pc = pd = (char *)a + (n - 1) * es;
+  for (;;) {
+    while (pb <= pc && (r = cmp(pb, a)) <= 0) {
+      if (r == 0) {
+        swap_cnt = 1;
+        swap(pa, pb);
+        pa += es;
+      }
+      pb += es;
+    }
+    while (pb <= pc && (r = cmp(pc, a)) >= 0) {
+      if (r == 0) {
+        swap_cnt = 1;
+        swap(pc, pd);
+        pd -= es;
+      }
+      pc -= es;
+    }
+    if (pb > pc) break;
+    swap(pb, pc);
+    swap_cnt = 1;
+    pb += es;
+    pc -= es;
+  }
+  if (swap_cnt == 0) { /* Switch to insertion sort */
+    for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es)
+      for (pl = pm; pl > (char *)a && cmp(pl - es, pl) > 0; pl -= es)
+        swap(pl, pl - es);
+    return;
+  }
+  pn = (char *)a + n * es;
+  r = min(pa - (char *)a, pb - pa);
+  vecswap(a, pb - r, r);
+  r = min(pd - pc, pn - pd - (int)es);
+  vecswap(pb, pn - r, r);
+  if ((r = pb - pa) > (int)es) qsort(a, r / es, es, cmp);
+  if ((r = pd - pc) > (int)es) {
+    /* Iterate rather than recurse to save stack space */
+    a = pn - r;
+    n = r / es;
+    goto loop;
+  }
+  /* qsort(pn - r, r / es, es, cmp); */
+}
+
+/* End of the copied qsort. */
+
+void dto_qsort(void *base, size_t nmemb, size_t size,
+               int (*compar)(const void *, const void *)) {
+  qsort(base, nmemb, size, compar);
+}
+
+/* Assuming the following functions are already defined in the
+ * bootloader source with the names conforming to POSIX.
+ */
+
+void *dto_malloc(size_t size) {
+	return malloc(size);
+}
+
+void dto_free(void *ptr) {
+	return free(ptr);
+}
+
+char *dto_strdup(const char *s) {
+	return strdup(s);
+}
+
+char *dto_strchr(const char *s, int c) { return strchr(s, c); }
+
+unsigned long int dto_strtoul(const char *nptr, char **endptr, int base) {
+  return strtoul(nptr, endptr, base);
+}
+
+size_t dto_strlen(const char *s) { return strlen(s); }
+
+void *dto_memcpy(void *dest, const void *src, size_t n) {
+  return memcpy(dest, src, n);
+}
+
+int dto_strcmp(const char *s1, const char *s2) { return strcmp(s1, s2); }
+
+int dto_strncmp(const char *s1, const char *s2, size_t n) {
+  return strncmp(s1, s2, n);
+}
+
+void *dto_memchr(const void *s, int c, size_t n) { return memchr(s, c, n); }
+
+void *dto_memset(void *s, int c, size_t n) { return memset(s, c, n); }
diff --git a/lib/libufdt/tests/README b/lib/libufdt/tests/README
new file mode 100644
index 0000000..645470d
--- /dev/null
+++ b/lib/libufdt/tests/README
@@ -0,0 +1,51 @@
+This folder contains scripts and test data to test libufdt.
+
+# Test scripts
+
+* run_tests.sh: The main entry to run test cases. Using different
+  test cases under testdata/*.
+* gen_test.sh: The script to run a single test case.
+* common.sh: A common lib containing several useful functions.
+
+# Test data
+
+testdata/${my_test_case}.base_dts
+ - Base device tree source.
+ - Sample format:
+   ```
+   /dts-v1/;
+   / {
+     a: a{};
+   };
+   ```
+
+testdata/${my_test_case}.add_dts
+ - Additional device tree source.
+ - Sample format:
+   ```
+   &a{ name = "a"; };
+   ```
+
+testdata/${my_test_case}.add_ov_dts (optional)
+ - Additional device tree fragment source.
+ - Sample format:
+   ```
+   /dts-v1/ /plugin/;
+   / {
+     fragment@0{
+        target = <&a>;
+        __overlay__ {
+          name = "a";
+        };
+     };
+   };
+   ```
+
+# Steps to run the test
+
+Suppose you are at the root directory of your Android source.
+
+1. `source build/envsetup.sh`
+2. `lunch`
+3. `mmma system/libufdt`
+4. `system/libufdt/tests/run_tests.sh`
diff --git a/lib/libufdt/tests/apply_overlay.sh b/lib/libufdt/tests/apply_overlay.sh
new file mode 100755
index 0000000..943613f
--- /dev/null
+++ b/lib/libufdt/tests/apply_overlay.sh
@@ -0,0 +1,60 @@
+#!/bin/bash
+
+PROG_NAME=`basename $0`
+
+function usage() {
+  echo "Usage:"
+  echo "  $PROG_NAME (--fdt|--ufdt) <Base DTS> <Overlay DTS> <Output DTS>"
+}
+
+function on_exit() {
+  rm -rf "$TEMP_DIR"
+}
+
+#
+# Start
+#
+
+# Setup OVERLAY
+if [ "$1" == "--fdt" ]; then
+  shift
+  OVERLAY="fdt_apply_overlay"
+elif [ "$1" == "--ufdt" ]; then
+  shift
+  OVERLAY="ufdt_apply_overlay"
+else
+  usage
+  exit 1
+fi
+
+if [[ $# -lt 3 ]]; then
+  usage
+  exit 1
+fi
+
+BASE_DTS=$1
+OVERLAY_DTS=$2
+OUT_DTS=$3
+
+TEMP_DIR=`mktemp -d`
+# The script will exit directly if any command fails.
+set -e
+trap on_exit EXIT
+
+# Compile the *-base.dts to make *-base.dtb
+BASE_DTS_NAME=`basename "$BASE_DTS"`
+BASE_DTB="$TEMP_DIR/${BASE_DTS_NAME}-base.dtb"
+dtc -@ -qq -O dtb -o "$BASE_DTB" "$BASE_DTS"
+
+# Compile the *-overlay.dts to make *-overlay.dtb
+OVERLAY_DTS_NAME=`basename "$OVERLAY_DTS"`
+OVERLAY_DTB="$TEMP_DIR/${OVERLAY_DTS_NAME}-overlay.dtb"
+dtc -@ -qq -O dtb -o "$OVERLAY_DTB" "$OVERLAY_DTS"
+
+# Run ufdt_apply_overlay to combine *-base.dtb and *-overlay.dtb
+# into *-merged.dtb
+MERGED_DTB="$TEMP_DIR/${BASE_DTS_NAME}-merged.dtb"
+"$OVERLAY" "$BASE_DTB" "$OVERLAY_DTB" "$MERGED_DTB"
+
+# Dump
+dtc -s -O dts -o "$OUT_DTS" "$MERGED_DTB"
diff --git a/lib/libufdt/tests/common.sh b/lib/libufdt/tests/common.sh
new file mode 100755
index 0000000..7fcc4c4
--- /dev/null
+++ b/lib/libufdt/tests/common.sh
@@ -0,0 +1,27 @@
+#!/bin/bash
+
+alert() {
+  echo "$*" >&2
+}
+
+die() {
+  echo "ERROR: $@"
+  exit 1
+}
+
+command_exists () {
+  type "$1" &> /dev/null;
+}
+
+remove_local_fixups() {
+  sed '/__local_fixups__/ {s/^\s*__local_fixups__\s*//; :again;N; s/{[^{}]*};//; /^$/ !b again; d}' $1
+}
+
+remove_overlay_stuff() {
+  # remove __symbols__, phandle, "linux,phandle" and __local_fixups__
+  sed "/__symbols__/,/[}];/d" $1 | sed "/\(^[ \t]*phandle\)/d" | sed "/\(^[ \t]*linux,phandle\)/d" | sed '/^\s*$/d' | remove_local_fixups
+}
+
+dts_diff () {
+  diff -u <(cat "$1" | remove_overlay_stuff) <(cat "$2" | remove_overlay_stuff)
+}
diff --git a/lib/libufdt/tests/gen_test.sh b/lib/libufdt/tests/gen_test.sh
new file mode 100755
index 0000000..9b6d06c
--- /dev/null
+++ b/lib/libufdt/tests/gen_test.sh
@@ -0,0 +1,42 @@
+#!/bin/bash
+
+# We want to generate two device tree blob (.dtb) files by combining
+# the "base" and "add" device tree source (.dts) files in two
+# different ways.
+#
+# 1) Use /include/ and compile via dtc to make the "gold standard"
+#
+# 2) Compile them separately dtc, and join them with the
+#    ufdt_apply_overlay program
+#
+# Then, compare 1) and 2) with dts_diff (diff merged nodes) and return 0
+# iff they are identical.
+
+SCRIPT_DIR="$(dirname "$(readlink -f "$0")")"
+source ${SCRIPT_DIR}/common.sh
+
+on_exit() {
+  rm -rf "$TEMP_DIR"
+}
+
+# Constants
+IN_DATA_DIR="testdata"
+
+TEMP_DIR=`mktemp -d`
+# The script will exit directly if any command fails.
+set -e
+trap on_exit EXIT
+
+# Global variables
+TESTCASE_NAME=$1
+BASE_DTS="$IN_DATA_DIR/${TESTCASE_NAME}-base.dts"
+OVERLAY_DTS="$IN_DATA_DIR/${TESTCASE_NAME}-overlay.dts"
+REF_MERGED_DTS="$TEMP_DIR/${TESTCASE_NAME}-ref-merged.dts"
+OVL_MERGED_DTS="$TEMP_DIR/${TESTCASE_NAME}-ovl-merged.dts"
+
+#
+# Complie and diff
+#
+$SCRIPT_DIR/apply_overlay.sh --fdt "$BASE_DTS" "$OVERLAY_DTS" "$REF_MERGED_DTS"
+$SCRIPT_DIR/apply_overlay.sh --ufdt "$BASE_DTS" "$OVERLAY_DTS" "$OVL_MERGED_DTS"
+dts_diff "$REF_MERGED_DTS" "$OVL_MERGED_DTS"
diff --git a/lib/libufdt/tests/run_tests.sh b/lib/libufdt/tests/run_tests.sh
new file mode 100755
index 0000000..eda6d5c
--- /dev/null
+++ b/lib/libufdt/tests/run_tests.sh
@@ -0,0 +1,70 @@
+#!/bin/bash
+
+# Include some functions from common.sh.
+SCRIPT_DIR="$(dirname "$(readlink -f "$0")")"
+source ${SCRIPT_DIR}/common.sh
+
+# Usage: run_test_case <filename> <description>
+# Args:
+#   filename: The file name for ./gen_test.sh to generate and run the
+#     test case. Several files under ./testdata subfolder are required:
+#       - ./testdata/${filename}.base_dts
+#       - ./testdata/${filename}.add_dts
+#       - ./testdata/${filename}.add_ov_dts (optional)
+#     For more details, check ./gen_test.sh.
+#   description: a description message to be displayed in the terminal
+run_test_case() {
+  local filename="$1"
+  local description="$2"
+
+  alert "${description}"
+  ./gen_test.sh "${filename}" >&2 ||
+    die "Test case: ${filename} failed!!"
+}
+
+main() {
+  alert "========== Running Tests of libufdt =========="
+
+  if [ -z "${ANDROID_BUILD_TOP}" ]; then
+    die "Run envsetup.sh / lunch yet?"
+  fi
+
+  if ! command_exists dtc ||
+     ! command_exists fdt_apply_overlay ||
+     ! command_exists ufdt_apply_overlay; then
+    die "Run mmma $(dirname ${SCRIPT_DIR}) yet?"
+  fi
+
+  (
+
+  # cd to ${SCRIPT_DIR} in a subshell because gen_test.sh uses relative
+  # paths for dependent files.
+  cd "${SCRIPT_DIR}"
+
+  run_test_case \
+    "no_local_fixup" \
+    "Run test about fdt_apply_fragment with no local fixup"
+  run_test_case \
+    "apply_fragment" \
+    "Run test about fdt_apply_fragment with phandle update"
+  run_test_case \
+    "local_fixup" \
+    "Run test about fdt_overlay_do_local_fixups"
+  run_test_case \
+    "local_fixup_with_offset" \
+    "Run test about dealing with local fixup with offset > 0"
+  run_test_case \
+    "overlay_2_layers" \
+    "Run test about dealing with overlay deep tree"
+  # looks that libfdt doesn't promise the order, the order isn't matched.
+  run_test_case \
+    "node_ordering" \
+    "Run test about node ordering"
+  )
+
+  if [ $? -ne 0 ]; then
+    die "Some test cases failed, please check error message..."
+  fi
+}
+
+main "$@"
diff --git a/lib/libufdt/tests/src/fdt_overlay_test_app.c b/lib/libufdt/tests/src/fdt_overlay_test_app.c
new file mode 100644
index 0000000..b023452
--- /dev/null
+++ b/lib/libufdt/tests/src/fdt_overlay_test_app.c
@@ -0,0 +1,70 @@
+#include <stdlib.h>
+#include <time.h>
+
+#include "libfdt.h"
+#include "libufdt_sysdeps.h"
+
+#include "util.h"
+
+
+int apply_ovleray_files(const char *out_filename,
+                        const char *base_filename,
+                        const char *overlay_filename) {
+  int ret = 1;
+  char *base_buf = NULL;
+  char *overlay_buf = NULL;
+  char *merged_buf = NULL;
+
+  size_t base_len;
+  base_buf = load_file(base_filename, &base_len);
+  if (!base_buf) {
+    fprintf(stderr, "Can not load base file: %s\n", base_filename);
+    goto end;
+  }
+
+  size_t overlay_len;
+  overlay_buf = load_file(overlay_filename, &overlay_len);
+  if (!overlay_buf) {
+    fprintf(stderr, "Can not load overlay file: %s\n", overlay_filename);
+    goto end;
+  }
+
+  size_t merged_buf_len = base_len + overlay_len;
+  merged_buf = dto_malloc(merged_buf_len);
+  fdt_open_into(base_buf, merged_buf, merged_buf_len);
+
+  clock_t start = clock();
+  fdt_overlay_apply(merged_buf, overlay_buf);
+  clock_t end = clock();
+
+  if (write_fdt_to_file(out_filename, merged_buf) != 0) {
+    fprintf(stderr, "Write file error: %s\n", out_filename);
+    goto end;
+  }
+
+  // Outputs the used time.
+  double cpu_time_used = ((double)(end - start)) / CLOCKS_PER_SEC;
+  printf(" fdt_apply_overlay: took %.9f secs\n", cpu_time_used);
+  ret = 0;
+
+end:
+  if (merged_buf) dto_free(merged_buf);
+  if (overlay_buf) dto_free(overlay_buf);
+  if (base_buf) dto_free(base_buf);
+
+  return ret;
+}
+
+int main(int argc, char **argv) {
+  if (argc < 4) {
+    fprintf(stderr, "Usage: %s <base_file> <overlay_file> <out_file>\n", argv[0]);
+    return 1;
+  }
+
+  const char *base_file = argv[1];
+  const char *overlay_file = argv[2];
+  const char *out_file = argv[3];
+  int ret = apply_ovleray_files(out_file, base_file, overlay_file);
+
+  return ret;
+}
diff --git a/lib/libufdt/tests/src/ufdt_overlay_test_app.c b/lib/libufdt/tests/src/ufdt_overlay_test_app.c
new file mode 100644
index 0000000..587328c
--- /dev/null
+++ b/lib/libufdt/tests/src/ufdt_overlay_test_app.c
@@ -0,0 +1,75 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+
+#include "ufdt_overlay.h"
+#include "libufdt_sysdeps.h"
+
+#include "util.h"
+
+
+int apply_ovleray_files(const char *out_filename,
+                        const char *base_filename,
+                        const char *overlay_filename) {
+  int ret = 1;
+  char *base_buf = NULL;
+  char *overlay_buf = NULL;
+  struct fdt_header *new_blob = NULL;
+
+  size_t blob_len;
+  base_buf = load_file(base_filename, &blob_len);
+  if (!base_buf) {
+    fprintf(stderr, "Can not load base file: %s\n", base_filename);
+    goto end;
+  }
+
+  size_t overlay_len;
+  overlay_buf = load_file(overlay_filename, &overlay_len);
+  if (!overlay_buf) {
+    fprintf(stderr, "Can not load overlay file: %s\n", overlay_filename);
+    goto end;
+  }
+
+  struct fdt_header *blob = ufdt_install_blob(base_buf, blob_len);
+  if (!blob) {
+    fprintf(stderr, "ufdt_install_blob() returns null\n");
+    goto end;
+  }
+
+  clock_t start = clock();
+  new_blob = ufdt_apply_overlay(blob, blob_len, overlay_buf, overlay_len);
+  clock_t end = clock();
+
+  if (write_fdt_to_file(out_filename, new_blob) != 0) {
+    fprintf(stderr, "Write file error: %s\n", out_filename);
+    goto end;
+  }
+
+  // Outputs the used time.
+  double cpu_time_used = ((double)(end - start)) / CLOCKS_PER_SEC;
+  printf("ufdt_apply_overlay: took %.9f secs\n", cpu_time_used);
+  ret = 0;
+
+end:
+  // Do not dto_free(blob) - it's the same as base_buf.
+
+  if (new_blob) dto_free(new_blob);
+  if (overlay_buf) dto_free(overlay_buf);
+  if (base_buf) dto_free(base_buf);
+
+  return ret;
+}
+
+int main(int argc, char **argv) {
+  if (argc < 4) {
+    fprintf(stderr, "Usage: %s <base_file> <overlay_file> <out_file>\n", argv[0]);
+    return 1;
+  }
+
+  const char *base_file = argv[1];
+  const char *overlay_file = argv[2];
+  const char *out_file = argv[3];
+  int ret = apply_ovleray_files(out_file, base_file, overlay_file);
+
+  return ret;
+}
diff --git a/lib/libufdt/tests/src/util.c b/lib/libufdt/tests/src/util.c
new file mode 100644
index 0000000..69f5c34
--- /dev/null
+++ b/lib/libufdt/tests/src/util.c
@@ -0,0 +1,50 @@
+#include "util.h"
+
+#include <stdio.h>
+
+#include "libfdt.h"
+#include "libufdt_sysdeps.h"
+
+
+char *load_file(const char *filename, size_t *pLen) {
+  FILE *fp = fopen(filename, "r");
+  if (!fp) {
+    return NULL;
+  }
+
+  // Gets the file size.
+  fseek(fp, 0, SEEK_END);
+  size_t len = ftell(fp);
+  fseek(fp, 0, SEEK_SET);
+
+  char *buf = dto_malloc(len);
+  if (fread(buf, len, 1, fp) != 1) {
+    dto_free(buf);
+    return NULL;
+  }
+
+  if (pLen) {
+    *pLen = len;
+  }
+  return buf;
+}
+
+int write_fdt_to_file(const char *filename, void *fdt) {
+  int ret = 0;
+  FILE *fout = NULL;
+
+  fout = fopen(filename, "wb");
+  if (!fout) {
+    ret = 1;
+    goto end;
+  }
+  if (fwrite(fdt, 1, fdt_totalsize(fdt), fout) < 1) {
+    ret = 2;
+    goto end;
+  }
+
+end:
+  if (fout) fclose(fout);
+
+  return ret;
+}
diff --git a/lib/libufdt/tests/src/util.h b/lib/libufdt/tests/src/util.h
new file mode 100644
index 0000000..f77d541
--- /dev/null
+++ b/lib/libufdt/tests/src/util.h
@@ -0,0 +1,10 @@
+#ifndef UTIL_H
+#define UTIL_H
+
+#include <stdio.h>
+
+
+char *load_file(const char *filename, size_t *pLen);
+int write_fdt_to_file(const char *filename, void *fdt);
+
+#endif
diff --git a/lib/libufdt/tests/testdata/apply_fragment-base.dts b/lib/libufdt/tests/testdata/apply_fragment-base.dts
new file mode 100644
index 0000000..30880bb
--- /dev/null
+++ b/lib/libufdt/tests/testdata/apply_fragment-base.dts
@@ -0,0 +1,7 @@
+/dts-v1/;
+
+/ {
+  a: a {};
+  b: b {};
+  c: c {};
+};
diff --git a/lib/libufdt/tests/testdata/apply_fragment-overlay.dts b/lib/libufdt/tests/testdata/apply_fragment-overlay.dts
new file mode 100644
index 0000000..7fe4f4e
--- /dev/null
+++ b/lib/libufdt/tests/testdata/apply_fragment-overlay.dts
@@ -0,0 +1,5 @@
+/dts-v1/;
+/plugin/;
+
+&a { d: d{}; };
+&b { e{}; };
diff --git a/lib/libufdt/tests/testdata/local_fixup-base.dts b/lib/libufdt/tests/testdata/local_fixup-base.dts
new file mode 100644
index 0000000..bc2d837
--- /dev/null
+++ b/lib/libufdt/tests/testdata/local_fixup-base.dts
@@ -0,0 +1,7 @@
+/dts-v1/;
+
+/ {
+a: a {
+    interrupt_parent = <&a>;
+  };
+};
diff --git a/lib/libufdt/tests/testdata/local_fixup-overlay.dts b/lib/libufdt/tests/testdata/local_fixup-overlay.dts
new file mode 100644
index 0000000..1f33aa0
--- /dev/null
+++ b/lib/libufdt/tests/testdata/local_fixup-overlay.dts
@@ -0,0 +1,8 @@
+/dts-v1/;
+/plugin/;
+
+&a {
+  c: c{
+    d{ interrupt_parent = <&c>; };
+  };
+};
diff --git a/lib/libufdt/tests/testdata/local_fixup_with_offset-base.dts b/lib/libufdt/tests/testdata/local_fixup_with_offset-base.dts
new file mode 100644
index 0000000..bc2d837
--- /dev/null
+++ b/lib/libufdt/tests/testdata/local_fixup_with_offset-base.dts
@@ -0,0 +1,7 @@
+/dts-v1/;
+
+/ {
+a: a {
+    interrupt_parent = <&a>;
+  };
+};
diff --git a/lib/libufdt/tests/testdata/local_fixup_with_offset-overlay.dts b/lib/libufdt/tests/testdata/local_fixup_with_offset-overlay.dts
new file mode 100644
index 0000000..2c453eb
--- /dev/null
+++ b/lib/libufdt/tests/testdata/local_fixup_with_offset-overlay.dts
@@ -0,0 +1,11 @@
+/dts-v1/;
+/plugin/;
+
+&a {
+  c: c{
+    d{ interrupt_parent = <0 1 &c 2 3>; };
+  };
+  e: e{
+    f{ interrupt_parent = <&c 5 &e>; };
+  };
+};
diff --git a/lib/libufdt/tests/testdata/no_local_fixup-base.dts b/lib/libufdt/tests/testdata/no_local_fixup-base.dts
new file mode 100644
index 0000000..01bad36
--- /dev/null
+++ b/lib/libufdt/tests/testdata/no_local_fixup-base.dts
@@ -0,0 +1,7 @@
+/dts-v1/;
+/ {
+    a: a {
+       b = "b";
+       c = "c";
+    };
+  };
diff --git a/lib/libufdt/tests/testdata/no_local_fixup-overlay.dts b/lib/libufdt/tests/testdata/no_local_fixup-overlay.dts
new file mode 100644
index 0000000..c076a6a
--- /dev/null
+++ b/lib/libufdt/tests/testdata/no_local_fixup-overlay.dts
@@ -0,0 +1,6 @@
+/dts-v1/;
+/plugin/;
+
+&a { c = "new-c";
+  d: d = "d";
+};
diff --git a/lib/libufdt/tests/testdata/node_ordering-base.dts b/lib/libufdt/tests/testdata/node_ordering-base.dts
new file mode 100644
index 0000000..ef32906
--- /dev/null
+++ b/lib/libufdt/tests/testdata/node_ordering-base.dts
@@ -0,0 +1,8 @@
+/dts-v1/;
+
+/ {
+  a: a {
+    b1 {};
+    b2 {};
+  };
+};
diff --git a/lib/libufdt/tests/testdata/node_ordering-overlay.dts b/lib/libufdt/tests/testdata/node_ordering-overlay.dts
new file mode 100644
index 0000000..4614848
--- /dev/null
+++ b/lib/libufdt/tests/testdata/node_ordering-overlay.dts
@@ -0,0 +1,12 @@
+/dts-v1/;
+/plugin/;
+
+&a {
+  c1 {};
+  c2 {
+    c2x {};
+    c2y {};
+    c2z {};
+  };
+  c3 {};
+};
diff --git a/lib/libufdt/tests/testdata/overlay_2_layers-base.dts b/lib/libufdt/tests/testdata/overlay_2_layers-base.dts
new file mode 100644
index 0000000..75ff810
--- /dev/null
+++ b/lib/libufdt/tests/testdata/overlay_2_layers-base.dts
@@ -0,0 +1,10 @@
+/dts-v1/;
+/ {
+  a: a {
+     b = "b";
+     c = "c";
+     d {
+       e = "e";
+     };
+  };
+};
diff --git a/lib/libufdt/tests/testdata/overlay_2_layers-overlay.dts b/lib/libufdt/tests/testdata/overlay_2_layers-overlay.dts
new file mode 100644
index 0000000..67592cd
--- /dev/null
+++ b/lib/libufdt/tests/testdata/overlay_2_layers-overlay.dts
@@ -0,0 +1,9 @@
+/dts-v1/;
+/plugin/;
+
+&a {
+  g = "g";
+  d {
+    f = "f";
+  };
+};
diff --git a/lib/libufdt/tests/verify_dto_inc.sh b/lib/libufdt/tests/verify_dto_inc.sh
new file mode 100755
index 0000000..ed258a0
--- /dev/null
+++ b/lib/libufdt/tests/verify_dto_inc.sh
@@ -0,0 +1,51 @@
+#!/bin/bash
+
+PROG_NAME=`basename $0`
+
+function usage() {
+  echo "Usage:"
+  echo "  $PROG_NAME <Base DTS> <Overlay DTS> <Output DTS>"
+}
+
+function on_exit() {
+  rm -rf "$TEMP_DIR"
+}
+
+#
+# Start
+#
+
+if [[ $# -lt 3 ]]; then
+  usage
+  exit 1
+fi
+
+BASE_DTS=$1
+OVERLAY_DTS=$2
+OUT_DTS=$3
+
+TEMP_DIR=`mktemp -d`
+# The script will exit directly if any command fails.
+set -e
+trap on_exit EXIT
+
+# Finds '/dts-v1/; and /plugin/;' then replace them with '/* REMOVED */'
+OVERLAY_DTS_DIR=`dirname "$OVERLAY_DTS"`
+OVERLAY_DTS_NAME=`basename "$OVERLAY_DTS"`
+OVERLAY_DT_WO_HEADER_DTS="$TEMP_DIR/$OVERLAY_DTS_NAME"
+sed "s/\\(\\/dts-v1\\/\\s*;\\|\\/plugin\\/\\s*;\\)/\\/\\* REMOVED \\*\\//g" \
+  "$OVERLAY_DTS" > "$OVERLAY_DT_WO_HEADER_DTS"
+
+# Appends /include/ ...;
+BASE_DTS_DIR=`dirname "$BASE_DTS"`
+BASE_DTS_NAME=`basename "$BASE_DTS"`
+BASE_DT_WITH_INC_DTS="$TEMP_DIR/$BASE_DTS_NAME"
+cp "$BASE_DTS" "$BASE_DT_WITH_INC_DTS"
+echo "/include/ \"$OVERLAY_DT_WO_HEADER_DTS\"" >> "$BASE_DT_WITH_INC_DTS"
+
+# Simulate device tree overlay
+MERGED_DTB="$BASE_DT_WITH_INC_DTS.dtb"
+dtc -@ -i "$BASE_DTS_DIR" -i "$OVERLAY_DTS_DIR" -O dtb -o "$MERGED_DTB" "$BASE_DT_WITH_INC_DTS"
+
+# Dump
+dtc -s -O dts -o "$OUT_DTS" "$MERGED_DTB"
diff --git a/lib/libufdt/ufdt_convert.c b/lib/libufdt/ufdt_convert.c
new file mode 100644
index 0000000..caa3ce3
--- /dev/null
+++ b/lib/libufdt/ufdt_convert.c
@@ -0,0 +1,294 @@
+#include "libufdt.h"
+
+#include "fdt_internal.h"
+#include "ufdt_util.h"
+
+
+struct ufdt *ufdt_construct(void *fdtp) {
+  struct ufdt *res_ufdt = dto_malloc(sizeof(struct ufdt));
+  res_ufdt->fdtp = fdtp;
+  res_ufdt->root = NULL;
+
+  return res_ufdt;
+}
+
+void ufdt_destruct(struct ufdt *tree) {
+  ufdt_node_destruct(tree->root);
+  dto_free(tree->phandle_table.data);
+}
+
+static struct ufdt_node *ufdt_new_node(void *fdtp, int node_offset) {
+  if (fdtp == NULL) {
+    dto_error("Failed to get new_node because tree is NULL\n");
+    return NULL;
+  }
+
+  fdt32_t *fdt_tag_ptr =
+      (fdt32_t *)fdt_offset_ptr(fdtp, node_offset, sizeof(fdt32_t));
+  struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr);
+  return res;
+}
+
+static struct ufdt_node *fdt_to_ufdt_tree(void *fdtp, int cur_fdt_tag_offset,
+                                          int *next_fdt_tag_offset,
+                                          int cur_tag) {
+  if (fdtp == NULL) {
+    return NULL;
+  }
+  uint32_t tag;
+  struct ufdt_node *res, *child_node;
+
+  res = NULL;
+  child_node = NULL;
+  tag = cur_tag;
+
+  switch (tag) {
+    case FDT_END_NODE:
+    case FDT_NOP:
+    case FDT_END:
+      break;
+
+    case FDT_PROP:
+      res = ufdt_new_node(fdtp, cur_fdt_tag_offset);
+      break;
+
+    case FDT_BEGIN_NODE:
+      res = ufdt_new_node(fdtp, cur_fdt_tag_offset);
+
+      do {
+        cur_fdt_tag_offset = *next_fdt_tag_offset;
+        tag = fdt_next_tag(fdtp, cur_fdt_tag_offset, next_fdt_tag_offset);
+        child_node = fdt_to_ufdt_tree(fdtp, cur_fdt_tag_offset,
+                                      next_fdt_tag_offset, tag);
+        ufdt_node_add_child(res, child_node);
+      } while (tag != FDT_END_NODE);
+      break;
+
+    default:
+      break;
+  }
+
+  return res;
+}
+
+void ufdt_print(struct ufdt *tree) { ufdt_node_print(tree->root, 0); }
+
+struct ufdt_node *ufdt_get_node_by_path_len(struct ufdt *tree, const char *path,
+                                            int len) {
+  /*
+   * RARE: aliases
+   * In device tree, we can assign some alias to specific nodes by defining
+   * these relation in "/aliases" node.
+   * The node has the form:
+   * {
+   *   a = "/a_for_apple";
+   *   b = "/b_for_banana";
+   * };
+   * So the path "a/subnode_1" should be expanded to "/a_for_apple/subnode_1".
+   */
+  if (*path != '/') {
+    const char *end = path + len;
+
+    const char *next_slash;
+    next_slash = dto_memchr(path, '/', end - path);
+    if (!next_slash) next_slash = end;
+
+    struct ufdt_node *aliases_node =
+        ufdt_node_get_node_by_path(tree->root, "/aliases");
+    aliases_node = ufdt_node_get_property_by_name_len(aliases_node, path,
+                                                      next_slash - path);
+
+    int path_len = 0;
+    const char *alias_path =
+        ufdt_node_get_fdt_prop_data(aliases_node, &path_len);
+
+    if (alias_path == NULL) {
+      dto_error("Failed to find alias %s\n", path);
+      return NULL;
+    }
+
+    struct ufdt_node *target_node =
+        ufdt_node_get_node_by_path_len(tree->root, alias_path, path_len);
+
+    return ufdt_node_get_node_by_path_len(target_node, next_slash,
+                                          end - next_slash);
+  }
+  return ufdt_node_get_node_by_path_len(tree->root, path, len);
+}
+
+struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path) {
+  return ufdt_get_node_by_path_len(tree, path, dto_strlen(path));
+}
+
+struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree,
+                                           uint32_t phandle) {
+  struct ufdt_node *res = NULL;
+  /*
+   * Do binary search in phandle_table.data.
+   * [s, e) means the possible range which contains target node.
+   */
+  int s = 0, e = tree->phandle_table.len;
+  while (e - s > 1) {
+    int mid = s + ((e - s) >> 1);
+    uint32_t mid_phandle = tree->phandle_table.data[mid].phandle;
+    if (phandle < mid_phandle)
+      e = mid;
+    else
+      s = mid;
+  }
+  if (e - s > 0) {
+    res = tree->phandle_table.data[s].node;
+  }
+  return res;
+}
+
+int merge_children(struct ufdt_node *node_a, struct ufdt_node *node_b) {
+  int err = 0;
+  struct ufdt_node *it;
+  for (it = ((struct fdt_node_ufdt_node *)node_b)->child; it;) {
+    struct ufdt_node *cur_node = it;
+    it = it->sibling;
+    cur_node->sibling = NULL;
+    struct ufdt_node *target_node = NULL;
+    if (tag_of(cur_node) == FDT_BEGIN_NODE) {
+      target_node = ufdt_node_get_subnode_by_name(node_a, name_of(cur_node));
+    } else {
+      target_node = ufdt_node_get_property_by_name(node_a, name_of(cur_node));
+    }
+    if (target_node == NULL) {
+      err = ufdt_node_add_child(node_a, cur_node);
+    } else {
+      err = merge_ufdt_into(target_node, cur_node);
+    }
+    if (err < 0) return -1;
+  }
+  /*
+   * The ufdt_node* in node_b will be copied to node_a.
+   * To prevent the ufdt_node from being freed twice
+   * (main_tree and overlay_tree) at the end of function
+   * ufdt_apply_overlay(), set this node in node_b
+   * (overlay_tree) to NULL.
+   */
+  ((struct fdt_node_ufdt_node *)node_b)->child = NULL;
+
+  return 0;
+}
+
+int merge_ufdt_into(struct ufdt_node *node_a, struct ufdt_node *node_b) {
+  if (tag_of(node_a) == FDT_PROP) {
+    node_a->fdt_tag_ptr = node_b->fdt_tag_ptr;
+    return 0;
+  }
+
+  int err = 0;
+  err = merge_children(node_a, node_b);
+  if (err < 0) return -1;
+
+  return 0;
+}
+
+void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure) {
+  ufdt_node_map(tree->root, closure);
+}
+
+static int count_phandle_node(struct ufdt_node *node) {
+  if (node == NULL) return 0;
+  if (tag_of(node) != FDT_BEGIN_NODE) return 0;
+  int res = 0;
+  if (ufdt_node_get_phandle(node) > 0) res++;
+  struct ufdt_node **it;
+  for_each_child(it, node) { res += count_phandle_node(*it); }
+  return res;
+}
+
+static void set_phandle_table_entry(struct ufdt_node *node,
+                                    struct phandle_table_entry *data,
+                                    int *cur) {
+  if (node == NULL || tag_of(node) != FDT_BEGIN_NODE) return;
+  int ph = ufdt_node_get_phandle(node);
+  if (ph > 0) {
+    data[*cur].phandle = ph;
+    data[*cur].node = node;
+    (*cur)++;
+  }
+  struct ufdt_node **it;
+  for_each_node(it, node) set_phandle_table_entry(*it, data, cur);
+  return;
+}
+
+int phandle_table_entry_cmp(const void *pa, const void *pb) {
+  uint32_t ph_a = ((const struct phandle_table_entry *)pa)->phandle;
+  uint32_t ph_b = ((const struct phandle_table_entry *)pb)->phandle;
+  if (ph_a < ph_b)
+    return -1;
+  else if (ph_a == ph_b)
+    return 0;
+  else
+    return 1;
+}
+
+struct static_phandle_table build_phandle_table(struct ufdt *tree) {
+  struct static_phandle_table res;
+  res.len = count_phandle_node(tree->root);
+  res.data = dto_malloc(sizeof(struct phandle_table_entry) * res.len);
+  int cur = 0;
+  set_phandle_table_entry(tree->root, res.data, &cur);
+  dto_qsort(res.data, res.len, sizeof(struct phandle_table_entry),
+            phandle_table_entry_cmp);
+  return res;
+}
+
+struct ufdt *fdt_to_ufdt(void *fdtp, size_t fdt_size) {
+  (void)(fdt_size); // unused parameter
+
+  struct ufdt *res_tree = ufdt_construct(fdtp);
+
+  int start_offset = fdt_path_offset(fdtp, "/");
+  if (start_offset < 0) {
+    res_tree->fdtp = NULL;
+    return res_tree;
+  }
+
+  int end_offset;
+  int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset);
+  res_tree->root = fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag);
+
+  res_tree->phandle_table = build_phandle_table(res_tree);
+
+  return res_tree;
+}
+
+int ufdt_to_fdt(struct ufdt *tree, void *buf, int buf_size) {
+  int err;
+  err = fdt_create(buf, buf_size);
+  if (err < 0) return -1;
+
+  int n_mem_rsv = fdt_num_mem_rsv(tree->fdtp);
+  for (int i = 0; i < n_mem_rsv; i++) {
+    uint64_t addr, size;
+    fdt_get_mem_rsv(tree->fdtp, i, &addr, &size);
+    fdt_add_reservemap_entry(buf, addr, size);
+  }
+
+  err = fdt_finish_reservemap(buf);
+  if (err < 0) return -1;
+
+  /*
+   * Obtains all props for later use because getting them from
+   * FDT requires complicated manipulation.
+  */
+  struct ufdt_node_dict all_props = ufdt_node_dict_construct();
+  err = output_ufdt_node_to_fdt(tree->root, buf, &all_props);
+  if (err < 0) return -1;
+
+  ufdt_node_dict_destruct(&all_props);
+
+  err = fdt_finish(buf);
+  if (err < 0) return -1;
+
+  /*
+   * IMPORTANT: fdt_totalsize(buf) might be less than buf_size
+   * so this is needed to make use of remain spaces.
+   */
+  return fdt_open_into(buf, buf, buf_size);
+}
diff --git a/lib/libufdt/ufdt_node.c b/lib/libufdt/ufdt_node.c
new file mode 100644
index 0000000..6ff4dc1
--- /dev/null
+++ b/lib/libufdt/ufdt_node.c
@@ -0,0 +1,304 @@
+#include "libufdt.h"
+#include "ufdt_util.h"
+
+
+int node_cmp(const void *a, const void *b) {
+  const struct ufdt_node *na = *(struct ufdt_node **)a;
+  const struct ufdt_node *nb = *(struct ufdt_node **)b;
+  return dto_strcmp(name_of(na), name_of(nb));
+}
+
+bool node_name_eq(const struct ufdt_node *node, const char *name, int len) {
+  if (!node) return false;
+  if (!name) return false;
+  if (dto_strncmp(name_of(node), name, len) != 0) return false;
+  if (name_of(node)[len] != '\0') return false;
+  return true;
+}
+
+/*
+ * ufdt_node methods.
+ */
+
+struct ufdt_node *ufdt_node_construct(void *fdtp, fdt32_t *fdt_tag_ptr) {
+  uint32_t tag = fdt32_to_cpu(*fdt_tag_ptr);
+  if (tag == FDT_PROP) {
+    struct fdt_prop_ufdt_node *res = dto_malloc(sizeof(struct fdt_prop_ufdt_node));
+    if (res == NULL) return NULL;
+    res->parent.fdt_tag_ptr = fdt_tag_ptr;
+    res->parent.sibling = NULL;
+    res->name = get_name(fdtp, (struct ufdt_node *)res);
+    return (struct ufdt_node *)res;
+  } else {
+    struct fdt_node_ufdt_node *res = dto_malloc(sizeof(struct fdt_node_ufdt_node));
+    if (res == NULL) return NULL;
+    res->parent.fdt_tag_ptr = fdt_tag_ptr;
+    res->parent.sibling = NULL;
+    res->child = NULL;
+    res->last_child_p = &res->child;
+    return (struct ufdt_node *)res;
+  }
+}
+
+void ufdt_node_destruct(struct ufdt_node *node) {
+  if (node == NULL) return;
+
+  if (tag_of(node) == FDT_BEGIN_NODE) {
+    ufdt_node_destruct(((struct fdt_node_ufdt_node *)node)->child);
+  }
+
+  ufdt_node_destruct(node->sibling);
+  dto_free(node);
+
+  return;
+}
+
+int ufdt_node_add_child(struct ufdt_node *parent, struct ufdt_node *child) {
+  if (!parent || !child) return -1;
+  if (tag_of(parent) != FDT_BEGIN_NODE) return -1;
+
+  int err = 0;
+  uint32_t child_tag = tag_of(child);
+
+  switch (child_tag) {
+    case FDT_PROP:
+    case FDT_BEGIN_NODE:
+      // Append the child node to the last child of parant node
+      *((struct fdt_node_ufdt_node *)parent)->last_child_p = child;
+      ((struct fdt_node_ufdt_node *)parent)->last_child_p = &child->sibling;
+      break;
+
+    default:
+      err = -1;
+      dto_error("invalid children tag type\n");
+  }
+
+  return err;
+}
+
+/*
+ * BEGIN of FDT_PROP related methods.
+ */
+
+struct ufdt_node *ufdt_node_get_subnode_by_name_len(const struct ufdt_node *node,
+                                                  const char *name, int len) {
+  struct ufdt_node **it = NULL;
+  for_each_node(it, node) {
+    if (node_name_eq(*it, name, len)) return *it;
+  }
+  return NULL;
+}
+
+struct ufdt_node *ufdt_node_get_subnode_by_name(const struct ufdt_node *node,
+                                              const char *name) {
+  return ufdt_node_get_subnode_by_name_len(node, name, strlen(name));
+}
+
+struct ufdt_node *ufdt_node_get_property_by_name_len(
+    const struct ufdt_node *node, const char *name, int len) {
+  if (!node) return NULL;
+
+  struct ufdt_node **it = NULL;
+  for_each_prop(it, node) {
+    if (node_name_eq(*it, name, len)) return *it;
+  }
+  return NULL;
+}
+
+struct ufdt_node *ufdt_node_get_property_by_name(const struct ufdt_node *node,
+                                                 const char *name) {
+  return ufdt_node_get_property_by_name_len(node, name, dto_strlen(name));
+}
+
+char *ufdt_node_get_fdt_prop_data(const struct ufdt_node *node, int *out_len) {
+  if (!node || tag_of(node) != FDT_PROP) {
+    return NULL;
+  }
+  const struct fdt_property *prop = (struct fdt_property *)node->fdt_tag_ptr;
+  if (out_len != NULL) {
+    *out_len = fdt32_to_cpu(prop->len);
+  }
+  return (char *)prop->data;
+}
+
+char *ufdt_node_get_fdt_prop_data_by_name_len(const struct ufdt_node *node,
+                                              const char *name, int len,
+                                              int *out_len) {
+  return ufdt_node_get_fdt_prop_data(
+      ufdt_node_get_property_by_name_len(node, name, len), out_len);
+}
+
+char *ufdt_node_get_fdt_prop_data_by_name(const struct ufdt_node *node,
+                                          const char *name, int *out_len) {
+  return ufdt_node_get_fdt_prop_data(ufdt_node_get_property_by_name(node, name),
+                                     out_len);
+}
+
+/*
+ * END of FDT_PROP related methods.
+ */
+
+/*
+ * BEGIN of searching-in-ufdt_node methods.
+ */
+
+uint32_t ufdt_node_get_phandle(const struct ufdt_node *node) {
+  if (!node || tag_of(node) != FDT_BEGIN_NODE) {
+    return 0;
+  }
+  int len = 0;
+  void *ptr = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len);
+  if (!ptr || len != sizeof(fdt32_t)) {
+    ptr = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len);
+    if (!ptr || len != sizeof(fdt32_t)) {
+      return 0;
+    }
+  }
+  return fdt32_to_cpu(*((fdt32_t *)ptr));
+}
+
+struct ufdt_node *ufdt_node_get_node_by_path_len(const struct ufdt_node *node,
+                                                 const char *path, int len) {
+  const char *end = path + len;
+
+  struct ufdt_node *cur = (struct ufdt_node *)node;
+
+  while (path < end) {
+    while (path[0] == '/') path++;
+    if (path == end) return cur;
+
+    const char *next_slash;
+    next_slash = dto_memchr(path, '/', end - path);
+    if (!next_slash) next_slash = end;
+
+    struct ufdt_node *next = NULL;
+
+    next = ufdt_node_get_subnode_by_name_len(cur, path, next_slash - path);
+
+    cur = next;
+    path = next_slash;
+    if (!cur) return cur;
+  }
+
+  return cur;
+}
+
+struct ufdt_node *ufdt_node_get_node_by_path(const struct ufdt_node *node,
+                                             const char *path) {
+  return ufdt_node_get_node_by_path_len(node, path, dto_strlen(path));
+}
+
+/*
+ * END of searching-in-ufdt_node methods.
+ */
+
+int output_property_to_fdt(struct ufdt_node *prop_node, void *fdtp,
+                           struct ufdt_node_dict *props_dict) {
+  int err = 0;
+  int len = 0;
+  void *data = ufdt_node_get_fdt_prop_data(prop_node, &len);
+  int nameoff = 0;
+  struct ufdt_node *same_name_prop =
+      ufdt_node_dict_find_node(props_dict, name_of(prop_node));
+
+  /*
+   * There's already a property with same name, take its nameoff instead.
+   */
+  if (same_name_prop != NULL) {
+    const struct fdt_property *prop =
+        (const struct fdt_property *)same_name_prop->fdt_tag_ptr;
+    nameoff = fdt32_to_cpu(prop->nameoff);
+  }
+  /*
+   * Modifies prop_node->fdt_tag_ptr to point to the node in new fdtp.
+   */
+  err = fast_fdt_sw_property(fdtp, name_of(prop_node), data, len, &nameoff,
+                             (struct fdt_property **)&(prop_node->fdt_tag_ptr));
+
+  if (err < 0) {
+    dto_error("Not enough space for the string space: %d\n",
+              fdt_size_dt_strings(fdtp));
+  }
+  ufdt_node_dict_add(props_dict, prop_node);
+  return err;
+}
+
+int output_ufdt_node_to_fdt(struct ufdt_node *node, void *fdtp,
+                            struct ufdt_node_dict *props_dict) {
+  uint32_t tag = tag_of(node);
+
+  if (tag == FDT_PROP) {
+    int err = output_property_to_fdt(node, fdtp, props_dict);
+    return err;
+  }
+
+  int err = fdt_begin_node(fdtp, name_of(node));
+  if (err < 0) return -1;
+
+  struct ufdt_node **it;
+  for_each_prop(it, node) {
+    err = output_ufdt_node_to_fdt(*it, fdtp, props_dict);
+    if (err < 0) return -1;
+  }
+
+  for_each_node(it, node) {
+    err = output_ufdt_node_to_fdt(*it, fdtp, props_dict);
+    if (err < 0) return -1;
+  }
+
+  err = fdt_end_node(fdtp);
+  if (err < 0) return -1;
+
+  return 0;
+}
+
+#define TAB_SIZE 2
+
+void ufdt_node_print(const struct ufdt_node *node, int depth) {
+  if (!node) return;
+
+  int i;
+  for (i = 0; i < depth * TAB_SIZE; i++) dto_print(" ");
+
+  uint32_t tag;
+  tag = tag_of(node);
+
+  switch (tag) {
+    case FDT_BEGIN_NODE:
+      dto_print("NODE ");
+      break;
+    case FDT_PROP:
+      dto_print("PROP ");
+      break;
+    default:
+      dto_print("UNKNOWN ");
+      break;
+  }
+
+  if (name_of(node)) {
+    dto_print(":%s:\n", name_of(node));
+  } else {
+    dto_print("node name is NULL.\n");
+  }
+
+  if (tag_of(node) == FDT_BEGIN_NODE) {
+    struct ufdt_node **it;
+
+    for_each_prop(it, node) ufdt_node_print(*it, depth + 1);
+
+    for_each_node(it, node) ufdt_node_print(*it, depth + 1);
+  }
+
+  return;
+}
+
+void ufdt_node_map(struct ufdt_node *node, struct ufdt_node_closure closure) {
+  if (node == NULL) return;
+  closure.func(node, closure.env);
+  if (tag_of(node) == FDT_BEGIN_NODE) {
+    struct ufdt_node **it;
+    for_each_prop(it, node) ufdt_node_map(*it, closure);
+    for_each_node(it, node) ufdt_node_map(*it, closure);
+  }
+  return;
+}
diff --git a/lib/libufdt/ufdt_node_dict.c b/lib/libufdt/ufdt_node_dict.c
new file mode 100644
index 0000000..448bc49
--- /dev/null
+++ b/lib/libufdt/ufdt_node_dict.c
@@ -0,0 +1,178 @@
+
+#include "libufdt.h"
+#include "ufdt_util.h"
+
+/*
+ * BEGIN of Hash table internal implementations.
+ */
+
+#define DICT_LIMIT_NUM 2
+#define DICT_LIMIT_DEN 3
+
+static bool _ufdt_node_dict_is_too_full(struct ufdt_node_dict *dict) {
+  /*
+   * We say a dict is too full if it's DICT_LIMIT_NUM / DICT_LIMIT_DEN full.
+   */
+  if (dict->num_used * DICT_LIMIT_DEN > dict->mem_size * DICT_LIMIT_NUM)
+    return true;
+  return false;
+}
+
+/*
+ * If collision happened, use linear probing to find idx in the hash table.
+ */
+static int _ufdt_node_dict_find_index_by_name_len(struct ufdt_node **hash_table,
+                                                  int size, const char *name,
+                                                  int len) {
+  if (!name || !size) return -1;
+  /*
+   * All size should be 2^k for some k
+   */
+  int hsh = get_hash_len(name, len);
+  int idx = hsh & (size - 1);
+  for (int cnt = 0; cnt < size; ++cnt) {
+    if (hash_table[idx] == NULL) return idx;
+    if (node_name_eq(hash_table[idx], name, len) == 1) return idx;
+    ++idx;
+    idx &= (size - 1);
+  }
+  return -1;
+}
+
+static int _ufdt_node_dict_find_index_by_name(struct ufdt_node **hash_table,
+                                              int size, const char *name) {
+  return _ufdt_node_dict_find_index_by_name_len(hash_table, size, name,
+                                                dto_strlen(name));
+}
+
+static int _ufdt_node_dict_find_index_in_ht(struct ufdt_node **hash_table,
+                                            int size, struct ufdt_node *x) {
+  if (x == NULL) return -1;
+  return _ufdt_node_dict_find_index_by_name(hash_table, size, name_of(x));
+}
+
+/*
+ * END of Hash table internal implementations.
+ */
+
+/*
+ * ufdt_node_dict methods.
+ */
+
+struct ufdt_node_dict ufdt_node_dict_construct() {
+  struct ufdt_node_dict res;
+  res.mem_size = DTNL_INIT_SZ;
+  res.num_used = 0;
+  res.nodes = dto_malloc(DTNL_INIT_SZ * sizeof(struct ufdt_node *));
+  if (res.nodes == NULL) {
+    res.mem_size = 0;
+    return res;
+  }
+  dto_memset(res.nodes, 0, DTNL_INIT_SZ * sizeof(struct ufdt_node *));
+  return res;
+}
+
+void ufdt_node_dict_destruct(struct ufdt_node_dict *dict) {
+  if (dict == NULL) return;
+  dto_free(dict->nodes);
+  dict->mem_size = dict->num_used = 0;
+}
+
+static int ufdt_node_dict_resize(struct ufdt_node_dict *dict) {
+  if (dict == NULL) return -1;
+
+  int new_size = dict->mem_size << 1;
+
+  struct ufdt_node **new_nodes =
+      dto_malloc(new_size * sizeof(struct ufdt_node *));
+
+  dto_memset(new_nodes, 0, new_size * sizeof(struct ufdt_node *));
+
+  for (int i = 0; i < dict->mem_size; i++) {
+    struct ufdt_node *node = dict->nodes[i];
+    if (node == NULL) continue;
+    int idx = _ufdt_node_dict_find_index_in_ht(new_nodes, new_size, node);
+    if (idx < 0) {
+      dto_error(
+          "failed to find new index in ufdt_node_dict resize for entry :%s:\n",
+          name_of(node));
+      dto_free(new_nodes);
+      return -1;
+    }
+    new_nodes[idx] = node;
+  }
+
+  dto_free(dict->nodes);
+
+  dict->mem_size = new_size;
+  dict->nodes = new_nodes;
+  return 0;
+}
+
+int ufdt_node_dict_add(struct ufdt_node_dict *dict, struct ufdt_node *node) {
+  if (node == NULL) return -1;
+  if (dict == NULL) return -1;
+
+  int idx = _ufdt_node_dict_find_index_in_ht(dict->nodes, dict->mem_size, node);
+  if (idx < 0) {
+    dto_error("failed to find new index in ufdt_node_dict add for entry :%s:\n",
+              name_of(node));
+    return -1;
+  }
+
+  if (dict->nodes[idx] == NULL) ++dict->num_used;
+  dict->nodes[idx] = node;
+
+  /*
+   * When the hash table is too full, double the size and rehashing.
+   */
+  int err = 0;
+  if (_ufdt_node_dict_is_too_full(dict)) {
+    err = ufdt_node_dict_resize(dict);
+  }
+
+  return err;
+}
+
+/*
+ * BEGIN of ufdt_dict searching related methods.
+ */
+
+/*
+ * return a pointer to the hash table entry
+ */
+struct ufdt_node **ufdt_node_dict_find_len(struct ufdt_node_dict *dict,
+                                           const char *name, int len) {
+  if (dict == NULL) return NULL;
+  int idx = _ufdt_node_dict_find_index_by_name_len(dict->nodes, dict->mem_size,
+                                                   name, len);
+  if (idx < 0) return NULL;
+  if (dict->nodes[idx] == NULL) return NULL;
+  return dict->nodes + idx;
+}
+
+struct ufdt_node **ufdt_node_dict_find(struct ufdt_node_dict *dict,
+                                       const char *name) {
+  return ufdt_node_dict_find_len(dict, name, dto_strlen(name));
+}
+
+struct ufdt_node *ufdt_node_dict_find_node_len(struct ufdt_node_dict *dict,
+                                               const char *name, int len) {
+  struct ufdt_node **res = ufdt_node_dict_find_len(dict, name, len);
+  if (res == NULL) return NULL;
+  return *res;
+}
+
+struct ufdt_node *ufdt_node_dict_find_node(struct ufdt_node_dict *dict,
+                                           const char *name) {
+  return ufdt_node_dict_find_node_len(dict, name, dto_strlen(name));
+}
+
+/*
+ * END of ufdt_dict searching related methods.
+ */
+
+void ufdt_node_dict_print(struct ufdt_node_dict *dict) {
+  struct ufdt_node **it;
+  for_each(it, dict) dto_print("%ld -> %s\n", it - dict->nodes, name_of(*it));
+}
diff --git a/lib/libufdt/ufdt_overlay.c b/lib/libufdt/ufdt_overlay.c
new file mode 100644
index 0000000..4a909a0
--- /dev/null
+++ b/lib/libufdt/ufdt_overlay.c
@@ -0,0 +1,675 @@
+/*-
+ * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo@FreeBSD.org>
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under sponsorship from
+ * the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include "ufdt_overlay.h"
+
+#include "libufdt.h"
+
+
+/*
+ * The original version of fdt_overlay.c is slow in searching for particular
+ * nodes and adding subnodes/properties due to the operations on flattened
+ * device tree (FDT).
+ *
+ * Here we introduce `libufdt` which builds a real tree structure (named
+ * ufdt -- unflattned device tree) from FDT. In the real tree, we can perform
+ * certain operations (e.g., merge 2 subtrees, search for a node by path) in
+ * almost optimal time complexity with acceptable additional memory usage.
+ *
+ * This file is the improved version of fdt_overlay.c by using the real tree
+ * structure defined in libufdt.
+ *
+ * How the device tree overlay works and some
+ * special terms (e.g., fixups, local fixups, fragment, etc)
+ * are described in the document
+ * external/dtc/Documentation/dt-object-internal.txt.
+ */
+
+/* BEGIN of operations about phandles in ufdt. */
+
+/*
+ * Increases u32 value at pos by offset.
+ */
+static void fdt_increase_u32(void *pos, uint32_t offset) {
+  uint32_t val;
+
+  dto_memcpy(&val, pos, sizeof(val));
+  val = cpu_to_fdt32(fdt32_to_cpu(val) + offset);
+  dto_memcpy(pos, &val, sizeof(val));
+}
+
+/*
+ * Gets the max phandle of a given ufdt.
+ */
+static uint32_t ufdt_get_max_phandle(struct ufdt *tree) {
+  struct static_phandle_table sorted_table = tree->phandle_table;
+  if (sorted_table.len > 0)
+    return sorted_table.data[sorted_table.len - 1].phandle;
+  else
+    return 0;
+}
+
+/*
+ * Tries to increase the phandle value of a node
+ * if the phandle exists.
+ */
+static void ufdt_node_try_increase_phandle(struct ufdt_node *node,
+                                           uint32_t offset) {
+  int len = 0;
+  char *prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len);
+  if (prop_data != NULL && len == sizeof(fdt32_t)) {
+    fdt_increase_u32(prop_data, offset);
+  }
+  prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len);
+  if (prop_data != NULL && len == sizeof(fdt32_t)) {
+    fdt_increase_u32(prop_data, offset);
+  }
+}
+
+/*
+ * Increases all phandles by offset in a ufdt
+ * in O(n) time.
+ */
+static void ufdt_try_increase_phandle(struct ufdt *tree, uint32_t offset) {
+  struct static_phandle_table sorted_table = tree->phandle_table;
+  int i;
+
+  for (i = 0; i < sorted_table.len; i++) {
+    struct ufdt_node *target_node = sorted_table.data[i].node;
+
+    ufdt_node_try_increase_phandle(target_node, offset);
+  }
+}
+
+/* END of operations about phandles in ufdt. */
+
+/*
+ * In the overlay_tree, there are some references (phandle)
+ * pointing to somewhere in the main_tree.
+ * Fix-up operations is to resolve the right address
+ * in the overlay_tree.
+ */
+
+/* BEGIN of doing fixup in the overlay ufdt. */
+
+/*
+ * Returns exact memory location specified by fixup in format
+ * /path/to/node:property:offset.
+ * A property might contain multiple values and the offset is used to locate a
+ * reference inside the property.
+ * e.g.,
+ * "property"=<1, 2, &ref, 4>, we can use /path/to/node:property:8 to get ref,
+ * where 8 is sizeof(uint32) + sizeof(unit32).
+ */
+static void *ufdt_get_fixup_location(struct ufdt *tree, const char *fixup) {
+  char *path, *prop_ptr, *offset_ptr, *end_ptr;
+  int prop_offset, prop_len;
+  const char *prop_data;
+
+  /*
+   * TODO(akaineko): Keep track of substring lengths so we don't have to
+   * dto_malloc a copy and split it up.
+   */
+  path = dto_strdup(fixup);
+  prop_ptr = dto_strchr(path, ':');
+  if (prop_ptr == NULL) {
+    dto_error("Missing property part in '%s'\n", path);
+    goto fail;
+  }
+
+  *prop_ptr = '\0';
+  prop_ptr++;
+
+  offset_ptr = dto_strchr(prop_ptr, ':');
+  if (offset_ptr == NULL) {
+    dto_error("Missing offset part in '%s'\n", path);
+    goto fail;
+  }
+
+  *offset_ptr = '\0';
+  offset_ptr++;
+
+  prop_offset = dto_strtoul(offset_ptr, &end_ptr, 10 /* base */);
+  if (*end_ptr != '\0') {
+    dto_error("'%s' is not valid number\n", offset_ptr);
+    goto fail;
+  }
+
+  struct ufdt_node *target_node;
+  target_node = ufdt_get_node_by_path(tree, path);
+  if (target_node == NULL) {
+    dto_error("Path '%s' not found\n", path);
+    goto fail;
+  }
+
+  prop_data =
+      ufdt_node_get_fdt_prop_data_by_name(target_node, prop_ptr, &prop_len);
+  if (prop_data == NULL) {
+    dto_error("Property '%s' not found in  '%s' node\n", prop_ptr, path);
+    goto fail;
+  }
+  /*
+   * Note that prop_offset is the offset inside the property data.
+   */
+  if (prop_len < prop_offset + (int)sizeof(uint32_t)) {
+    dto_error("%s: property length is too small for fixup\n", path);
+    goto fail;
+  }
+
+  dto_free(path);
+  return (char *)prop_data + prop_offset;
+
+fail:
+  dto_free(path);
+  return NULL;
+}
+
+/*
+ * Process one entry in __fixups__ { } node.
+ * @fixups is property value, array of NUL-terminated strings
+ *   with fixup locations.
+ * @fixups_len length of the fixups array in bytes.
+ * @phandle is value for these locations.
+ */
+static int ufdt_do_one_fixup(struct ufdt *tree, const char *fixups,
+                             int fixups_len, int phandle) {
+  void *fixup_pos;
+  uint32_t val;
+
+  val = cpu_to_fdt32(phandle);
+
+  while (fixups_len > 0) {
+    fixup_pos = ufdt_get_fixup_location(tree, fixups);
+    if (fixup_pos != NULL) {
+      dto_memcpy(fixup_pos, &val, sizeof(val));
+    } else {
+      return -1;
+    }
+
+    fixups_len -= dto_strlen(fixups) + 1;
+    fixups += dto_strlen(fixups) + 1;
+  }
+
+  return 0;
+}
+
+/*
+ * Handle __fixups__ node in overlay tree.
+ */
+
+static int ufdt_overlay_do_fixups(struct ufdt *main_tree,
+                                  struct ufdt *overlay_tree) {
+  int len = 0;
+  struct ufdt_node *main_symbols_node, *overlay_fixups_node;
+
+  main_symbols_node = ufdt_get_node_by_path(main_tree, "/__symbols__");
+  overlay_fixups_node = ufdt_get_node_by_path(overlay_tree, "/__fixups__");
+
+  if (!main_symbols_node) {
+    dto_error("Bad main_symbols in ufdt_overlay_do_fixups\n");
+    return -1;
+  }
+
+  if (!overlay_fixups_node) {
+    dto_error("Bad overlay_fixups in ufdt_overlay_do_fixups\n");
+    return -1;
+  }
+
+  struct ufdt_node **it;
+  for_each_prop(it, overlay_fixups_node) {
+    /*
+     * A property in __fixups__ looks like:
+     * symbol_name =
+     * "/path/to/node:prop:offset0\x00/path/to/node:prop:offset1..."
+     * So we firstly find the node "symbol_name" and obtain its phandle in
+     * __symbols__ of the main_tree.
+     */
+
+    struct ufdt_node *fixups = *it;
+    char *symbol_path = ufdt_node_get_fdt_prop_data_by_name(
+        main_symbols_node, name_of(fixups), &len);
+
+    if (!symbol_path) {
+      dto_error("Couldn't find '%s' symbol in main dtb\n", name_of(fixups));
+      return -1;
+    }
+
+    struct ufdt_node *symbol_node;
+    symbol_node = ufdt_get_node_by_path(main_tree, symbol_path);
+
+    if (!symbol_node) {
+      dto_error("Couldn't find '%s' path in main dtb\n", symbol_path);
+      return -1;
+    }
+
+    uint32_t phandle = ufdt_node_get_phandle(symbol_node);
+
+    const char *fixups_paths = ufdt_node_get_fdt_prop_data(fixups, &len);
+
+    if (ufdt_do_one_fixup(overlay_tree, fixups_paths, len, phandle) < 0) {
+      dto_error("Failed one fixup in ufdt_do_one_fixup\n");
+      return -1;
+    }
+  }
+
+  return 0;
+}
+
+/* END of doing fixup in the overlay ufdt. */
+
+/*
+ * Here is to overlay all fragments in the overlay_tree to the main_tree.
+ * What is "overlay fragment"? The main purpose is to add some subtrees to the
+ * main_tree in order to complete the entire device tree.
+ *
+ * A frgament consists of two parts: 1. the subtree to be added 2. where it
+ * should be added.
+ *
+ * Overlaying a fragment requires: 1. find the node in the main_tree 2. merge
+ * the subtree into that node in the main_tree.
+ */
+
+/* BEGIN of applying fragments. */
+
+/*
+ * Overlay the overlay_node over target_node.
+ */
+static int ufdt_overlay_node(struct ufdt_node *target_node,
+                             struct ufdt_node *overlay_node) {
+  return merge_ufdt_into(target_node, overlay_node);
+}
+
+/*
+ * Return value of ufdt_apply_fragment().
+ */
+
+enum overlay_result {
+  OVERLAY_RESULT_OK,
+  OVERLAY_RESULT_MISSING_TARGET,
+  OVERLAY_RESULT_MISSING_OVERLAY,
+  OVERLAY_RESULT_TARGET_PATH_INVALID,
+  OVERLAY_RESULT_TARGET_INVALID,
+  OVERLAY_RESULT_MERGE_FAIL,
+};
+
+/*
+ * Apply one overlay fragment (subtree).
+ */
+static enum overlay_result ufdt_apply_fragment(struct ufdt *tree,
+                                               struct ufdt_node *frag_node) {
+  uint32_t target;
+  const char *target_path;
+  const void *val;
+  struct ufdt_node *target_node = NULL;
+  struct ufdt_node *overlay_node = NULL;
+
+  val = ufdt_node_get_fdt_prop_data_by_name(frag_node, "target", NULL);
+  if (val) {
+    dto_memcpy(&target, val, sizeof(target));
+    target = fdt32_to_cpu(target);
+    target_node = ufdt_get_node_by_phandle(tree, target);
+    if (target_node == NULL) {
+      dto_error("failed to find target %04x\n", target);
+      return OVERLAY_RESULT_TARGET_INVALID;
+    }
+  }
+
+  if (target_node == NULL) {
+    target_path =
+        ufdt_node_get_fdt_prop_data_by_name(frag_node, "target-path", NULL);
+    if (target_path == NULL) {
+      return OVERLAY_RESULT_MISSING_TARGET;
+    }
+
+    target_node = ufdt_get_node_by_path(tree, target_path);
+    if (target_node == NULL) {
+      dto_error("failed to find target-path %s\n", target_path);
+      return OVERLAY_RESULT_TARGET_PATH_INVALID;
+    }
+  }
+
+  overlay_node = ufdt_node_get_node_by_path(frag_node, "__overlay__");
+  if (overlay_node == NULL) {
+    dto_error("missing __overlay__ sub-node\n");
+    return OVERLAY_RESULT_MISSING_OVERLAY;
+  }
+
+  int err = ufdt_overlay_node(target_node, overlay_node);
+
+  if (err < 0) {
+    dto_error("failed to overlay node %s to target %s\n", name_of(overlay_node),
+              name_of(target_node));
+    return OVERLAY_RESULT_MERGE_FAIL;
+  }
+
+  return OVERLAY_RESULT_OK;
+}
+
+/*
+ * Applies all fragments to the main_tree.
+ */
+static int ufdt_overlay_apply_fragments(struct ufdt *main_tree,
+                                        struct ufdt *overlay_tree) {
+  enum overlay_result err;
+  struct ufdt_node **it;
+  /*
+   * This loop may iterate to subnodes that's not a fragment node.
+   * In such case, ufdt_apply_fragment would fail with return value = -1.
+   */
+  for_each_node(it, overlay_tree->root) {
+    err = ufdt_apply_fragment(main_tree, *it);
+    if (err == OVERLAY_RESULT_MERGE_FAIL) {
+      return -1;
+    }
+  }
+  return 0;
+}
+
+/* END of applying fragments. */
+
+/*
+ * Since the overlay_tree will be "merged" into the main_tree, some
+ * references (e.g., phandle values that acts as an unique ID) need to be
+ * updated so it won't lead to collision that different nodes have the same
+ * phandle value.
+ *
+ * Two things need to be done:
+ *
+ * 1. ufdt_try_increase_phandle()
+ * Update phandle (an unique integer ID of a node in the device tree) of each
+ * node in the overlay_tree. To achieve this, we simply increase each phandle
+ * values in the overlay_tree by the max phandle value of the main_tree.
+ *
+ * 2. ufdt_overlay_do_local_fixups()
+ * If there are some reference in the overlay_tree that references nodes
+ * inside the overlay_tree, we have to modify the reference value (address of
+ * the referenced node: phandle) so that it corresponds to the right node inside
+ * the overlay_tree. Where the reference exists is kept in __local_fixups__ node
+ * in the overlay_tree.
+ */
+
+/* BEGIN of updating local references (phandle values) in the overlay ufdt. */
+
+/*
+ * local fixups
+ */
+static int ufdt_local_fixup_prop(struct ufdt_node *target_prop_node,
+                                 struct ufdt_node *local_fixup_prop_node,
+                                 uint32_t phandle_offset) {
+  /*
+   * prop_offsets_ptr should be a list of fdt32_t.
+   * <offset0 offset1 offset2 ...>
+   */
+  char *prop_offsets_ptr;
+  int len = 0;
+  prop_offsets_ptr = ufdt_node_get_fdt_prop_data(local_fixup_prop_node, &len);
+
+  char *prop_data;
+  int target_length = 0;
+
+  prop_data = ufdt_node_get_fdt_prop_data(target_prop_node, &target_length);
+
+  if (prop_offsets_ptr == NULL || prop_data == NULL) return -1;
+
+  int i;
+  for (i = 0; i < len; i += sizeof(fdt32_t)) {
+    int offset = fdt32_to_cpu(*(fdt32_t *)(prop_offsets_ptr + i));
+    if (offset + sizeof(fdt32_t) > (size_t)target_length) return -1;
+    fdt_increase_u32((prop_data + offset), phandle_offset);
+  }
+  return 0;
+}
+
+static int ufdt_local_fixup_node(struct ufdt_node *target_node,
+                                 struct ufdt_node *local_fixups_node,
+                                 uint32_t phandle_offset) {
+  if (local_fixups_node == NULL) return 0;
+
+  struct ufdt_node **it_local_fixups;
+  struct ufdt_node *sub_target_node;
+
+  for_each_prop(it_local_fixups, local_fixups_node) {
+    sub_target_node =
+        ufdt_node_get_property_by_name(target_node, name_of(*it_local_fixups));
+
+    if (sub_target_node != NULL) {
+      int err = ufdt_local_fixup_prop(sub_target_node, *it_local_fixups,
+                                      phandle_offset);
+      if (err < 0) return -1;
+    } else {
+      return -1;
+    }
+  }
+
+  for_each_node(it_local_fixups, local_fixups_node) {
+    sub_target_node =
+        ufdt_node_get_node_by_path(target_node, name_of(*it_local_fixups));
+    if (sub_target_node != NULL) {
+      int err = ufdt_local_fixup_node(sub_target_node, *it_local_fixups,
+                                      phandle_offset);
+      if (err < 0) return -1;
+    } else {
+      return -1;
+    }
+  }
+
+  return 0;
+}
+
+static int ufdt_overlay_root_node(struct ufdt *tree,
+                                     struct ufdt *overlay_tree) {
+     struct ufdt_node *target_node = ufdt_get_node_by_path(tree, "/");
+     struct ufdt_node *overlay_node = ufdt_get_node_by_path(overlay_tree, "/");
+     struct ufdt_node **it_prop;
+     struct ufdt_node *target_prop;
+
+     if(!target_node)
+          return 0;
+
+     for_each_prop(it_prop, overlay_node) {
+          target_prop =
+               ufdt_node_get_property_by_name(target_node, name_of(*it_prop));
+
+          if (target_prop) {
+               if(merge_ufdt_into(target_prop, *it_prop))
+                    return -1;
+          }
+     }
+
+     return 0;
+}
+
+/*
+ * Handle __local_fixups__ node in overlay DTB
+ * The __local_fixups__ format we expect is
+ * __local_fixups__ {
+ *   path {
+ *    to {
+ *      local_ref1 = <offset>;
+ *    };
+ *   };
+ *   path2 {
+ *    to2 {
+ *      local_ref2 = <offset1 offset2 ...>;
+ *    };
+ *   };
+ * };
+ *
+ * which follows the dtc patch from:
+ * https://marc.info/?l=devicetree&m=144061468601974&w=4
+ */
+static int ufdt_overlay_do_local_fixups(struct ufdt *tree,
+                                        uint32_t phandle_offset) {
+  struct ufdt_node *overlay_node = ufdt_get_node_by_path(tree, "/");
+  struct ufdt_node *local_fixups_node =
+      ufdt_get_node_by_path(tree, "/__local_fixups__");
+
+  int err =
+      ufdt_local_fixup_node(overlay_node, local_fixups_node, phandle_offset);
+
+  if (err < 0) return -1;
+
+  return 0;
+}
+
+static int ufdt_overlay_local_ref_update(struct ufdt *main_tree,
+                                         struct ufdt *overlay_tree) {
+  uint32_t phandle_offset = 0;
+
+  phandle_offset = ufdt_get_max_phandle(main_tree);
+  if (phandle_offset > 0) {
+    ufdt_try_increase_phandle(overlay_tree, phandle_offset);
+  }
+
+  int err = ufdt_overlay_do_local_fixups(overlay_tree, phandle_offset);
+  if (err < 0) {
+    dto_error("failed to perform local fixups in overlay\n");
+    return -1;
+  }
+  return 0;
+}
+
+/* END of updating local references (phandle values) in the overlay ufdt. */
+
+static int ufdt_overlay_apply(struct ufdt *main_tree, struct ufdt *overlay_tree,
+                              size_t overlay_length) {
+  if (overlay_length < sizeof(struct fdt_header)) {
+    dto_error("Overlay_length %zu smaller than header size %zu\n",
+              overlay_length, sizeof(struct fdt_header));
+    return -1;
+  }
+
+  if(ufdt_overlay_root_node(main_tree, overlay_tree))
+    return -1;
+
+  if (ufdt_overlay_local_ref_update(main_tree, overlay_tree) < 0) {
+    dto_error("failed to perform local fixups in overlay\n");
+    return -1;
+  }
+
+  if (ufdt_overlay_do_fixups(main_tree, overlay_tree) < 0) {
+    dto_error("failed to perform fixups in overlay\n");
+    return -1;
+  }
+  if (ufdt_overlay_apply_fragments(main_tree, overlay_tree) < 0) {
+    dto_error("failed to apply fragments\n");
+    return -1;
+  }
+
+  return 0;
+}
+
+struct fdt_header *ufdt_install_blob(void *blob, size_t blob_size) {
+  struct fdt_header *pHeader;
+  int err;
+
+  dto_debug("ufdt_install_blob (0x%08jx)\n", (uintmax_t)blob);
+
+  if (blob_size < sizeof(struct fdt_header)) {
+    dto_error("Blob_size %zu smaller than the header size %zu\n", blob_size,
+              sizeof(struct fdt_header));
+    return NULL;
+  }
+
+  pHeader = (struct fdt_header *)blob;
+  err = fdt_check_header(pHeader);
+  if (err < 0) {
+    if (err == -FDT_ERR_BADVERSION) {
+      dto_error("incompatible blob version: %d, should be: %d",
+                fdt_version(pHeader), FDT_LAST_SUPPORTED_VERSION);
+
+    } else {
+      dto_error("error validating blob: %s", fdt_strerror(err));
+    }
+    return NULL;
+  }
+
+  return pHeader;
+}
+
+/*
+* From Google, based on dt_overlay_apply() logic
+* Will dto_malloc a new fdt blob and return it. Will not dto_free parameters.
+*/
+struct fdt_header *ufdt_apply_overlay(struct fdt_header *main_fdt_header,
+                                 size_t main_fdt_size,
+                                 void *overlay_fdtp,
+                                 size_t overlay_size) {
+  size_t out_fdt_size;
+
+  if (main_fdt_header == NULL) {
+    return NULL;
+  }
+
+  if (overlay_size < 8 || overlay_size != fdt_totalsize(overlay_fdtp)) {
+    dto_error("Bad overlay size!\n");
+    return NULL;
+  }
+
+  if (main_fdt_size < 8 || main_fdt_size != fdt_totalsize(main_fdt_header)) {
+    dto_error("Bad fdt size!\n");
+    return NULL;
+  }
+
+  out_fdt_size = fdt_totalsize(main_fdt_header) + overlay_size;
+  /* It's actually more than enough */
+  struct fdt_header *out_fdt_header = dto_malloc(out_fdt_size);
+
+  if (out_fdt_header == NULL) {
+    dto_error("failed to allocate memory for DTB blob with overlays\n");
+    return NULL;
+  }
+
+  struct ufdt *main_tree, *overlay_tree;
+
+  main_tree = fdt_to_ufdt(main_fdt_header, main_fdt_size);
+
+  overlay_tree = fdt_to_ufdt(overlay_fdtp, overlay_size);
+
+  int err = ufdt_overlay_apply(main_tree, overlay_tree, overlay_size);
+  if (err < 0) {
+    goto fail;
+  }
+
+  err = ufdt_to_fdt(main_tree, out_fdt_header, out_fdt_size);
+  if (err < 0) {
+    dto_error("Failed to dump the device tree to out_fdt_header\n");
+    goto fail;
+  }
+  ufdt_destruct(main_tree);
+  ufdt_destruct(overlay_tree);
+
+  return out_fdt_header;
+
+fail:
+  ufdt_destruct(main_tree);
+  ufdt_destruct(overlay_tree);
+  dto_free(out_fdt_header);
+  return NULL;
+}
diff --git a/lib/libufdt/ufdt_util.h b/lib/libufdt/ufdt_util.h
new file mode 100644
index 0000000..ccd7f76
--- /dev/null
+++ b/lib/libufdt/ufdt_util.h
@@ -0,0 +1,60 @@
+
+#ifndef UFDT_UTIL_H
+#define UFDT_UTIL_H
+
+#include "fdt_internal.h"
+#include "ufdt_types.h"
+
+static const char *tag_name(uint32_t tag) {
+  switch (tag) {
+    case FDT_BEGIN_NODE:
+      return "FDT_BEGIN_NODE";
+    case FDT_END_NODE:
+      return "FDT_END_NODE";
+    case FDT_PROP:
+      return "FDT_PROP";
+    case FDT_END:
+      return "FDT_END";
+  }
+  return "";
+}
+
+static const char *get_name(void *fdtp, struct ufdt_node *node) {
+  if (!fdtp || !node) return NULL;
+
+  const struct fdt_node_header *nh;
+  const struct fdt_property *prop;
+
+  uint32_t tag = tag_of(node);
+
+  switch (tag) {
+    case FDT_BEGIN_NODE:
+      nh = (const struct fdt_node_header *)node->fdt_tag_ptr;
+      return nh->name;
+    case FDT_PROP:
+      prop = (const struct fdt_property *)node->fdt_tag_ptr;
+      return fdt_string(fdtp, fdt32_to_cpu(prop->nameoff));
+    default:
+      return "";
+  }
+}
+
+static const void *value_of(const struct ufdt_node *node) {
+  if (!node) {
+    dto_error( "Failed to get value since tree or node is NULL\n");
+    return NULL;
+  }
+  return node->fdt_tag_ptr;
+}
+
+static int get_hash_len(const char *str, int len) {
+  int res = 0;
+  for (int i = 0; i < len; i++) {
+    res = res * HASH_BASE;
+    res = res + str[i];
+  }
+  return res;
+}
+static int get_hash(const char *str) { return get_hash_len(str, dto_strlen(str)); }
+
+#endif /* UFDT_UTIL_H */
diff --git a/platform/msm8953/platform.c b/platform/msm8953/platform.c
index 6f85204..5f75b70 100644
--- a/platform/msm8953/platform.c
+++ b/platform/msm8953/platform.c
@@ -65,7 +65,7 @@
 	{    MSM_IOMAP_BASE,        MSM_IOMAP_BASE,          MSM_IOMAP_SIZE,   IOMAP_MEMORY},
 	{    APPS_SS_BASE,          APPS_SS_BASE,            APPS_SS_SIZE,      IOMAP_MEMORY},
 	{    MSM_SHARED_IMEM_BASE,  MSM_SHARED_IMEM_BASE,    1,                COMMON_MEMORY},
-	{    SCRATCH_ADDR,          SCRATCH_ADDR,            512,              SCRATCH_MEMORY},
+	{    SCRATCH_ADDR,          SCRATCH_ADDR,            SCRATCH_SIZE,     SCRATCH_MEMORY},
 	{    MIPI_FB_ADDR,          MIPI_FB_ADDR,            20,                     COMMON_MEMORY},
 	{    RPMB_SND_RCV_BUF,      RPMB_SND_RCV_BUF,        RPMB_SND_RCV_BUF_SZ,    IOMAP_MEMORY},
 };
diff --git a/platform/msm_shared/dev_tree.c b/platform/msm_shared/dev_tree.c
index ecff926..58603e6 100644
--- a/platform/msm_shared/dev_tree.c
+++ b/platform/msm_shared/dev_tree.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2015,2017 The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2015,2017-2018 The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -37,14 +37,39 @@
 #include <board.h>
 #include <list.h>
 #include <kernel/thread.h>
+#include <kernel/event.h>
 #include <target.h>
 #include <partial_goods.h>
 #include <boot_device.h>
 #include <platform.h>
 #include <scm.h>
+#include <partition_parser.h>
+#include <libufdt_sysdeps.h>
+#include <ufdt_overlay.h>
 
 #define BOOT_DEV_MAX_LEN        64
 #define NODE_PROPERTY_MAX_LEN   64
+#define ADD_OF(a, b) (UINT_MAX - b > a) ? (a + b) : UINT_MAX
+#define ADDR_ALIGNMENT 16
+/** 512KB stack **/
+#define DTBO_STACK_SIZE (524288)
+#define MAX_DTBO_SZ 2097152
+
+static bool  dtbo_needed = true;
+static void *board_dtb = NULL;
+static void *soc_dtb = NULL;
+static void *soc_dtb_hdr = NULL;
+static void *final_dtb_hdr = NULL;
+static event_t dtbo_event;
+
+typedef enum dtbo_error
+{
+	DTBO_ERROR = 0,
+	DTBO_NOT_SUPPORTED = 1,
+	DTBO_SUCCESS = 2
+}dtbo_error;
+
+dtbo_error ret = DTBO_SUCCESS;
 
 struct dt_entry_v1
 {
@@ -134,6 +159,512 @@
 	return dt_node_member;
 }
 
+/*
+ * Function to validate dtbo image.
+ * return: TRUE or FALSE.
+ */
+dtbo_error load_validate_dtbo_image(void **dtbo_buf)
+{
+	uint64_t dtbo_total_size = 0;
+	void *dtbo_image_buf = NULL;
+	unsigned int dtbo_image_buf_size;
+	unsigned int dtbo_partition_size;
+	unsigned long long ptn, ptn_size, boot_img_sz;
+	int index = INVALID_PTN;
+	int page_size = mmc_page_size();
+	struct dtbo_table_hdr *dtbo_table_header = NULL;
+	dtbo_error ret = DTBO_SUCCESS;
+
+	/* Immediately return if dtbo is not supported */
+	index = partition_get_index("dtbo");
+	if (index == INVALID_PTN)
+	{
+		ret = DTBO_NOT_SUPPORTED;
+		goto out;
+	}
+
+	ptn = partition_get_offset(index);
+	if(!ptn)
+	{
+		dprintf(CRITICAL, "ERROR: dtbo parition failed to get offset. \n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	ptn_size = partition_get_size(index);
+	if (ptn_size > DTBO_IMG_BUF)
+	{
+		dprintf(CRITICAL, "ERROR: dtbo parition size is greater than supported.\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+/*
+	Read dtbo image into scratch region after kernel image.
+	dtbo_image_buf_size = total_scratch_region_size - boot_img_sz
+*/
+	boot_img_sz = partition_get_size(partition_get_index("boot"));
+	if (!boot_img_sz)
+	{
+		dprintf(CRITICAL, "ERROR: Unable to get boot partition size\n");
+		ret = DTBO_NOT_SUPPORTED;
+		goto out;
+	}
+
+	dtbo_image_buf_size = target_get_max_flash_size() - boot_img_sz;
+	dtbo_partition_size = ptn_size + ADDR_ALIGNMENT;
+	dtbo_partition_size = ROUND_TO_PAGE(dtbo_partition_size, (page_size - 1)); /* Maximum dtbo size possible */
+	if (dtbo_partition_size == UINT_MAX ||
+		dtbo_image_buf_size < dtbo_partition_size)
+	{
+		dprintf(CRITICAL, "ERROR: Invalid DTBO partition size\n");
+		ret = DTBO_NOT_SUPPORTED;
+		goto out;
+	}
+
+	mmc_set_lun(partition_get_lun(index));
+	dtbo_image_buf = target_get_scratch_address() + boot_img_sz; /* read dtbo after boot.img */
+	dtbo_image_buf = (void *)ROUND_TO_PAGE((addr_t)dtbo_image_buf, (ADDR_ALIGNMENT-1) );
+	if(dtbo_image_buf == (void *)UINT_MAX)
+	{
+		dprintf(CRITICAL, "ERROR: Invalid DTBO image buf addr\n");
+		ret = DTBO_NOT_SUPPORTED;
+		goto out;
+	}
+
+	/* Read dtbo partition with header */
+	if (mmc_read(ptn, (uint32_t *)(dtbo_image_buf), dtbo_partition_size))
+	{
+		dprintf(CRITICAL, "ERROR: dtbo partition mmc read failure \n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/* validate the dtbo image, before reading complete image */
+	dtbo_table_header = (struct dtbo_table_hdr *)dtbo_image_buf;
+
+	/*Check for dtbo magic*/
+	if (fdt32_to_cpu(dtbo_table_header->magic) != DTBO_TABLE_MAGIC)
+	{
+		dprintf(CRITICAL, "dtbo header magic mismatch %x, with %x\n",
+			fdt32_to_cpu(dtbo_table_header->magic), DTBO_TABLE_MAGIC);
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/*Check for dtbo image table size*/
+	if (fdt32_to_cpu(dtbo_table_header->hdr_size) != sizeof(struct dtbo_table_hdr))
+	{
+		dprintf(CRITICAL, "dtbo table header size got corrupted\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/*Check for dt entry size of dtbo image*/
+	if (fdt32_to_cpu(dtbo_table_header->dt_entry_size) != sizeof(struct dtbo_table_entry))
+	{
+		dprintf(CRITICAL, "dtbo table dt entry size got corrupted\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/*Total size of dtbo */
+	dtbo_total_size = fdt32_to_cpu(dtbo_table_header->total_size);
+	if (dtbo_total_size > dtbo_partition_size || dtbo_total_size == 0)
+	{
+		dprintf(CRITICAL, "dtbo table total size exceeded the dtbo buffer allocated\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/* Total size of dtbo entries */
+	dtbo_total_size = fdt32_to_cpu(dtbo_table_header->dt_entry_count) *
+				fdt32_to_cpu(dtbo_table_header->dt_entry_size);
+	if (dtbo_total_size > dtbo_partition_size || dtbo_total_size == 0)
+	{
+		dprintf(CRITICAL, "dtbo table total size exceeded the dtbo buffer allocated\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/* Offset should be less than image size */
+	if (fdt32_to_cpu(dtbo_table_header->dt_entry_offset) > dtbo_partition_size)
+	{
+		dprintf(CRITICAL, "dtbo offset exceeds the dtbo buffer allocated\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+
+	/* Offset + total size is less than image size */
+	if (dtbo_total_size + fdt32_to_cpu(dtbo_table_header->dt_entry_offset) > dtbo_partition_size)
+	{
+		dprintf(CRITICAL, "dtbo size exceeded the dtbo buffer allocated\n");
+		ret = DTBO_ERROR;
+	}
+
+out:
+	*dtbo_buf = dtbo_image_buf;
+	return ret;
+}
+
+static bool check_all_bits_set(uint32_t matchdt_value)
+{
+	return (matchdt_value & ALL_BITS_SET) == (ALL_BITS_SET);
+}
+
+/* Dt selection table for quick reference
+  | SNO | Dt Property   | CDT Property    | Exact | Best | Default |
+  |-----+---------------+-----------------+-------+------+---------+
+  |     | qcom, msm-id  |                 |       |      |         |
+  |     |               | PlatformId      | Y     | N    | N       |
+  |     |               | SocRev          | N     | Y    | N       |
+  |     |               | FoundryId       | Y     | N    | 0       |
+  |     | qcom,board-id |                 |       |      |         |
+  |     |               | VariantId       | Y     | N    | N       |
+  |     |               | VariantMajor    | N     | Y    | N       |
+  |     |               | VariantMinor    | N     | Y    | N       |
+  |     |               | PlatformSubtype | Y     | N    | 0       |
+  |     | qcom,pmic-id  |                 |       |      |         |
+  |     |               | PmicModelId     | Y     | N    | 0       |
+  |     |               | PmicMetalRev    | N     | Y    | N       |
+  |     |               | PmicLayerRev    | N     | Y    | N       |
+  |     |               | PmicVariantRev  | N     | Y    | N       |
+*/
+
+static void dtb_read_find_match(dt_info *current_dtb_info, dt_info *best_dtb_info, uint32_t exact_match)
+{
+	int board_id_len;
+	int platform_id_len = 0;
+	int pmic_id_len;
+	int root_offset = 0;
+	void *dtb = current_dtb_info->dtb;
+	uint32_t idx;
+	const char *platform_prop = NULL;
+	const char *board_prop = NULL;
+	const char *pmic_prop = NULL;
+
+	current_dtb_info->dt_match_val = 0;
+	root_offset = fdt_path_offset(dtb, "/");
+	if (root_offset < 0) {
+		dprintf(CRITICAL, "ERROR: Unable to locate root node\n");
+		return;
+	}
+
+	/* Get the msm-id prop from DTB and find best match */
+	platform_prop = (const char *)fdt_getprop(dtb, root_offset, "qcom,msm-id", &platform_id_len);
+	if (platform_prop && (platform_id_len > 0) && (!(platform_id_len % PLAT_ID_SIZE))) {
+		/*Compare msm-id of the dtb vs board*/
+		current_dtb_info->dt_platform_id = fdt32_to_cpu(((struct plat_id *)platform_prop)->platform_id);
+		dprintf(SPEW, "Board SOC ID = %x | DT SOC ID = %x\n", (board_platform_id() & SOC_MASK),
+								(current_dtb_info->dt_platform_id & SOC_MASK));
+		if ((board_platform_id() & SOC_MASK) == (current_dtb_info->dt_platform_id & SOC_MASK)) {
+			current_dtb_info->dt_match_val |= BIT(SOC_MATCH);
+		} else {
+			dprintf(SPEW, "qcom,msm-id does not match\n");
+			 /* If soc doesn't match, don't select dtb */
+			current_dtb_info->dt_match_val = BIT(NONE_MATCH);
+			goto cleanup;
+		}
+		/*Compare soc rev of the dtb vs board*/
+		current_dtb_info->dt_soc_rev = fdt32_to_cpu(((struct plat_id *)platform_prop)->soc_rev);
+		dprintf(SPEW, "Board SOC Rev = %x | DT SOC Rev =%x\n",  board_soc_version(),
+									current_dtb_info->dt_soc_rev);
+		if (current_dtb_info->dt_soc_rev ==  board_soc_version()) {
+			current_dtb_info->dt_match_val |= BIT(VERSION_EXACT_MATCH);
+		} else if (current_dtb_info->dt_soc_rev <  board_soc_version()) {
+			current_dtb_info->dt_match_val |= BIT(VERSION_BEST_MATCH);
+		} else if (current_dtb_info->dt_soc_rev) {
+			dprintf(SPEW, "SOC version does not match\n");
+		}
+		/*Compare Foundry Id of the dtb vs Board*/
+		current_dtb_info->dt_foundry_id = fdt32_to_cpu(((struct plat_id *)platform_prop)->platform_id) & FOUNDRY_ID_MASK;
+		dprintf (SPEW, "Board Foundry id = %x | DT Foundry id = %x\n", (board_foundry_id() << PLATFORM_FOUNDRY_SHIFT), current_dtb_info->dt_foundry_id);
+		if (current_dtb_info->dt_foundry_id == (board_foundry_id() << PLATFORM_FOUNDRY_SHIFT)) {
+			current_dtb_info->dt_match_val |= BIT (FOUNDRYID_EXACT_MATCH);
+		} else if (current_dtb_info->dt_foundry_id == 0 ){
+			current_dtb_info->dt_match_val |= BIT (FOUNDRYID_DEFAULT_MATCH);
+		} else {
+			dprintf(SPEW, "soc foundry does not match\n");
+			/* If soc doesn't match, don't select dtb */
+			current_dtb_info->dt_match_val = BIT(NONE_MATCH);
+			goto cleanup;
+		}
+	} else {
+		dprintf(SPEW, "qcom,msm-id does not exist (or) is (%d) not a multiple of (%d)\n", platform_id_len, PLAT_ID_SIZE);
+	}
+
+	/* Get the properties like variant id, subtype from DTB and compare the DTB vs Board */
+	board_prop = (const char *)fdt_getprop(dtb, root_offset, "qcom,board-id", &board_id_len);
+	if (board_prop && (board_id_len > 0) && (!(board_id_len % BOARD_ID_SIZE))) {
+		current_dtb_info->dt_variant_id = fdt32_to_cpu(((struct board_id *)board_prop)->variant_id);
+		current_dtb_info->dt_platform_subtype = fdt32_to_cpu(((struct board_id *)board_prop)->platform_subtype);
+		if (current_dtb_info->dt_platform_subtype == 0)
+			current_dtb_info->dt_platform_subtype = fdt32_to_cpu(((struct board_id *)board_prop)->variant_id) >> PLATFORM_SUBTYPE_SHIFT_ID;
+
+		dprintf(SPEW, "Board variant id = %x | DT variant id = %x\n",board_hardware_id(), current_dtb_info->dt_variant_id);
+
+		current_dtb_info->dt_variant_major = current_dtb_info->dt_variant_id & VARIANT_MAJOR_MASK;
+		current_dtb_info->dt_variant_minor = current_dtb_info->dt_variant_id & VARIANT_MINOR_MASK;
+		current_dtb_info->dt_variant_id = current_dtb_info->dt_variant_id & VARIANT_MASK;
+
+		if (current_dtb_info->dt_variant_id == board_hardware_id()) {
+			current_dtb_info->dt_match_val |= BIT(VARIANT_MATCH);
+		} else if (current_dtb_info->dt_variant_id) {
+			dprintf(SPEW, "qcom,board-id doesnot match\n");
+			/* If board variant doesn't match, don't select dtb */
+			current_dtb_info->dt_match_val = BIT(NONE_MATCH);
+			goto cleanup;
+		}
+
+		if (current_dtb_info->dt_variant_major == (board_target_id() & VARIANT_MAJOR_MASK)) {
+			current_dtb_info->dt_match_val |= BIT(VARIANT_MAJOR_EXACT_MATCH);
+		} else if (current_dtb_info->dt_variant_major < (board_target_id() & VARIANT_MAJOR_MASK)) {
+			current_dtb_info->dt_match_val |= BIT(VARIANT_MAJOR_BEST_MATCH);
+		} else if (current_dtb_info->dt_variant_major) {
+			dprintf(SPEW, "qcom,board-id major version doesnot match\n");
+		}
+
+		if (current_dtb_info->dt_variant_minor == (board_target_id() & VARIANT_MINOR_MASK)) {
+			current_dtb_info->dt_match_val |= BIT(VARIANT_MINOR_EXACT_MATCH);
+		} else if (current_dtb_info->dt_variant_minor < (board_target_id() & VARIANT_MINOR_MASK)) {
+			current_dtb_info->dt_match_val |= BIT(VARIANT_MINOR_BEST_MATCH);
+		} else if (current_dtb_info->dt_variant_minor) {
+			dprintf(SPEW, "qcom,board-id minor version doesnot match\n");
+		}
+
+		dprintf(SPEW, "Board platform subtype = %x | DT platform subtype = %x\n",
+					board_hardware_subtype(), current_dtb_info->dt_platform_subtype);
+		if (current_dtb_info->dt_platform_subtype == board_hardware_subtype()) {
+			current_dtb_info->dt_match_val |= BIT(SUBTYPE_EXACT_MATCH);
+		} else if (current_dtb_info->dt_platform_subtype == 0) {
+			current_dtb_info->dt_match_val |= BIT(SUBTYPE_DEFAULT_MATCH);
+		} else if (current_dtb_info->dt_platform_subtype) {
+			dprintf(SPEW, "subtype id doesnot match\n");
+			/* If board platform doesn't match, don't select dtb */
+			current_dtb_info->dt_match_val = BIT(NONE_MATCH);
+			goto cleanup;
+		}
+	} else {
+		dprintf(SPEW, "qcom,board-id does not exist (or)(%d) is not a multiple of (%d)\n",
+									board_id_len, BOARD_ID_SIZE);
+	}
+
+	/* Get the pmic property from DTB then compare the DTB vs Board */
+	pmic_prop = (const char *)fdt_getprop(dtb, root_offset, "qcom,pmic-id", &pmic_id_len);
+	if (pmic_prop && (pmic_id_len > 0) && (!(pmic_id_len % sizeof(struct pmic_id))))
+	{
+		pmic_info curr_pmic_info;
+		pmic_info best_pmic_info;
+		unsigned int pmic_entry_indx;
+		unsigned int pmic_entries_count;
+
+		/* Find all pmic entries count */
+		pmic_entries_count = pmic_id_len/sizeof(struct pmic_id);
+
+		memset(&best_pmic_info, 0, sizeof(pmic_info));
+		for (pmic_entry_indx = 0; pmic_entry_indx < pmic_entries_count; pmic_entry_indx++)
+		{
+			memset(&curr_pmic_info, 0, sizeof(pmic_info));
+
+			/* Read all pmic info */
+			/* Compare with board pmic */
+			for (idx = 0; idx < MAX_PMIC_IDX; idx++)
+			{
+				curr_pmic_info.dt_pmic_model[idx] =
+					fdt32_to_cpu (((struct pmic_id *)pmic_prop)->pmic_version[idx]);
+				dprintf(SPEW, "pmic_data[%u]:%x\n", idx, curr_pmic_info.dt_pmic_model[idx]);
+				curr_pmic_info.dt_pmic_rev[idx] =
+					curr_pmic_info.dt_pmic_model[idx] & PMIC_REV_MASK;
+				curr_pmic_info.dt_pmic_model[idx] =
+					curr_pmic_info.dt_pmic_model[idx] & PMIC_MODEL_MASK;
+
+				/* Compare with board pmic information & Update bit mask */
+				if (curr_pmic_info.dt_pmic_model[idx] == (board_pmic_target(idx) & PMIC_MODEL_MASK))
+					curr_pmic_info.dt_match_val |= BIT (PMIC_MATCH_EXACT_MODEL_IDX0 + idx * PMIC_SHIFT_IDX);
+				else if (curr_pmic_info.dt_pmic_model[idx] == 0)
+					curr_pmic_info.dt_match_val |= BIT (PMIC_MATCH_DEFAULT_MODEL_IDX0 + idx * PMIC_SHIFT_IDX);
+				else
+				{
+					curr_pmic_info.dt_match_val |= BIT (NONE_MATCH);
+					dprintf(SPEW, "PMIC Model doesn't match\n");
+					break; /* go to next pmic entry */
+				}
+
+				if (curr_pmic_info.dt_pmic_rev[idx] == (board_pmic_target(idx) & PMIC_REV_MASK))
+					curr_pmic_info.dt_match_val |= BIT(PMIC_MATCH_EXACT_REV_IDX0 + idx * PMIC_SHIFT_IDX);
+				else if (curr_pmic_info.dt_pmic_rev[idx] < (board_pmic_target(idx) & PMIC_REV_MASK))
+					curr_pmic_info.dt_match_val |= BIT(PMIC_MATCH_BEST_REV_IDX0 + idx * PMIC_SHIFT_IDX);
+				else
+					dprintf(SPEW, "PMIC revision doesn't match\n");
+					break; /* go to next pmic entry */
+			}
+
+			dprintf(SPEW, "Bestpmicinfo.dtmatchval : %x | cur_pmic_info.dtmatchval: %x\n",
+				best_pmic_info.dt_match_val, curr_pmic_info.dt_match_val);
+
+			/* Update best pmic info, if required */
+			if(best_pmic_info.dt_match_val < curr_pmic_info.dt_match_val)
+				memscpy(&best_pmic_info, sizeof(pmic_info), &curr_pmic_info, sizeof(pmic_info));
+			else if (best_pmic_info.dt_match_val == curr_pmic_info.dt_match_val)
+			{
+				if (best_pmic_info.dt_pmic_rev[PMIC_IDX0] < curr_pmic_info.dt_pmic_rev[PMIC_IDX0])
+					memscpy(&best_pmic_info, sizeof(pmic_info), &curr_pmic_info, sizeof(pmic_info));
+				else if (best_pmic_info.dt_pmic_rev[PMIC_IDX1] < curr_pmic_info.dt_pmic_rev[PMIC_IDX1])
+					memscpy(&best_pmic_info, sizeof(pmic_info), &curr_pmic_info, sizeof(pmic_info));
+				else if (best_pmic_info.dt_pmic_rev[PMIC_IDX2] < curr_pmic_info.dt_pmic_rev[PMIC_IDX2])
+					memscpy(&best_pmic_info, sizeof(pmic_info), &curr_pmic_info, sizeof(pmic_info));
+				else if (best_pmic_info.dt_pmic_rev[PMIC_IDX3] < curr_pmic_info.dt_pmic_rev[PMIC_IDX3])
+					memscpy(&best_pmic_info, sizeof(pmic_info), &curr_pmic_info, sizeof(pmic_info));
+			}
+
+			/* Increment to next pmic entry */
+			pmic_prop += sizeof (struct pmic_id);
+		}
+
+		dprintf(SPEW, "Best pmic info 0x%0x/0x%x/0x%x/0x%0x for current dt\n",
+					best_pmic_info.dt_pmic_model[PMIC_IDX0],
+					best_pmic_info.dt_pmic_model[PMIC_IDX1],
+					best_pmic_info.dt_pmic_model[PMIC_IDX2],
+					best_pmic_info.dt_pmic_model[PMIC_IDX3]);
+
+		current_dtb_info->dt_match_val |= best_pmic_info.dt_match_val;
+		current_dtb_info->dt_pmic_rev[PMIC_IDX0] = best_pmic_info.dt_pmic_rev[PMIC_IDX0];
+		current_dtb_info->dt_pmic_model[PMIC_IDX0] = best_pmic_info.dt_pmic_model[PMIC_IDX0];
+		current_dtb_info->dt_pmic_rev[PMIC_IDX1] = best_pmic_info.dt_pmic_rev[PMIC_IDX1];
+		current_dtb_info->dt_pmic_model[PMIC_IDX1] = best_pmic_info.dt_pmic_model[PMIC_IDX1];
+		current_dtb_info->dt_pmic_rev[PMIC_IDX2] = best_pmic_info.dt_pmic_rev[PMIC_IDX2];
+		current_dtb_info->dt_pmic_model[PMIC_IDX2] = best_pmic_info.dt_pmic_model[PMIC_IDX2];
+		current_dtb_info->dt_pmic_rev[PMIC_IDX3] = best_pmic_info.dt_pmic_rev[PMIC_IDX3];
+		current_dtb_info->dt_pmic_model[PMIC_IDX3] = best_pmic_info.dt_pmic_model[PMIC_IDX3];
+	}
+	else
+	{
+		dprintf(SPEW, "qcom,pmic-id does not exit (or) is (%d) not a multiple of (%d)\n",
+										pmic_id_len, PMIC_ID_SIZE);
+	}
+
+cleanup:
+	if (current_dtb_info->dt_match_val & BIT(exact_match)) {
+		if (best_dtb_info->dt_match_val < current_dtb_info->dt_match_val)
+			memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+		else if (best_dtb_info->dt_match_val == current_dtb_info->dt_match_val) {
+			if (best_dtb_info->dt_soc_rev < current_dtb_info->dt_soc_rev)
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+			else if (best_dtb_info->dt_variant_major < current_dtb_info->dt_variant_major)
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+			else if (best_dtb_info->dt_variant_minor < current_dtb_info->dt_variant_minor)
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+			else if (best_dtb_info->dt_pmic_rev[0] < current_dtb_info->dt_pmic_rev[0])
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+			else if (best_dtb_info->dt_pmic_rev[1] < current_dtb_info->dt_pmic_rev[1])
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+			else if (best_dtb_info->dt_pmic_rev[2] < current_dtb_info->dt_pmic_rev[2])
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+			else if (best_dtb_info->dt_pmic_rev[3] < current_dtb_info->dt_pmic_rev[3])
+				memscpy(best_dtb_info, sizeof(dt_info), current_dtb_info, sizeof(dt_info));
+		}
+	}
+}
+
+void *get_soc_dtb(void *kernel, uint32_t kernel_size, uint32_t dtb_offset)
+{
+	uintptr_t kernel_end_offset = (uintptr_t)kernel + kernel_size;
+	void *dtb = NULL;
+	struct fdt_header dtb_header;
+	uint32_t dtb_size = 0;
+	dt_info cur_dtb_info = {0};
+	dt_info best_dtb_info = {0};
+
+	if (!dtb_offset){
+		dprintf(CRITICAL, "DTB offset is NULL\n");
+		return NULL;
+	}
+	if (((uintptr_t)kernel + (uintptr_t)dtb_offset) < (uintptr_t)kernel) {
+		return NULL;
+	}
+	dtb = kernel + dtb_offset;
+	while (((uintptr_t)dtb + sizeof(struct fdt_header)) < (uintptr_t)kernel_end_offset) {
+		/* the DTB could be unaligned, so extract the header,
+		 * and operate on it separately */
+		memscpy(&dtb_header, sizeof(struct fdt_header), dtb, sizeof(struct fdt_header));
+		dtb_size = fdt_totalsize((const void *)&dtb_header);
+		if (fdt_check_header((const void *)&dtb_header) != 0 ||
+		fdt_check_header_ext((void *)&dtb_header) != 0 ||
+		((uintptr_t)dtb + dtb_size < (uintptr_t)dtb) ||
+		((uintptr_t)dtb + dtb_size > (uintptr_t)kernel_end_offset))
+		break;
+
+		cur_dtb_info.dtb = dtb;
+		dtb_read_find_match(&cur_dtb_info, &best_dtb_info, SOC_MATCH);
+		if (cur_dtb_info.dt_match_val) {
+			if (cur_dtb_info.dt_match_val & BIT(SOC_MATCH)) {
+				if (check_all_bits_set(cur_dtb_info.dt_match_val)) {
+					dprintf(CRITICAL, "Exact DTB match found. dtbo search is not required\n");
+					dtbo_needed = false;
+				}
+			}
+		}
+		dprintf(SPEW, "Best Match DTB VAL = %x\n", best_dtb_info.dt_match_val);
+		dtb += dtb_size;
+	}
+	if (!best_dtb_info.dtb) {
+		dprintf(CRITICAL, "No match found for soc dtb type\n");
+		return NULL;
+	}
+	return best_dtb_info.dtb;
+}
+
+void *get_board_dtb(void *dtbo_image_buf)
+{
+	struct dtbo_table_hdr *dtbo_table_header = dtbo_image_buf;
+	struct dtbo_table_entry *dtb_table_entry = NULL;
+	uint32_t dtbo_count = 0;
+	void *board_dtb = NULL;
+	uint32_t dtbo_table_entries_count = 0;
+	uint32_t first_dtbo_table_entry_offset = 0;
+	struct fdt_header dtb_header;
+	uint32_t dtb_size = 0;
+	dt_info cur_dtb_info = {0};
+	dt_info best_dtb_info = {0};
+
+	if (!dtbo_image_buf) {
+		dprintf(CRITICAL, "dtbo image buffer is NULL\n");
+		return NULL;
+	}
+
+	first_dtbo_table_entry_offset = fdt32_to_cpu(dtbo_table_header->dt_entry_offset);
+	if ((uintptr_t)dtbo_image_buf > ((uintptr_t)dtbo_image_buf + (uintptr_t)first_dtbo_table_entry_offset))
+	{
+		dprintf(CRITICAL, "dtbo table entry offset is invalid\n");
+		return NULL;
+	}
+	dtb_table_entry = (struct dtbo_table_entry *)(dtbo_image_buf + first_dtbo_table_entry_offset);
+	dtbo_table_entries_count = fdt32_to_cpu(dtbo_table_header->dt_entry_count);
+	for (dtbo_count = 0; dtbo_count < dtbo_table_entries_count; dtbo_count++) {
+		board_dtb = dtbo_image_buf + fdt32_to_cpu(dtb_table_entry->dt_offset);
+		/* The DTB could be unaligned, so extract the header,
+                 * and operate on it separately */
+		memscpy(&dtb_header, sizeof(struct fdt_header), board_dtb, sizeof(struct fdt_header));
+		dtb_size = fdt_totalsize((const void *)&dtb_header);
+		if (fdt_check_header((const void *)&dtb_header) != 0 ||
+			fdt_check_header_ext((void *)&dtb_header) != 0 ||
+			((uintptr_t)board_dtb + dtb_size < (uintptr_t)board_dtb)) {
+			dprintf(CRITICAL, "No valid board dtb found\n");
+	                break;
+		}
+		dprintf(SPEW, "Valid board dtb is found\n");
+		cur_dtb_info.dtb = board_dtb;
+		dtb_read_find_match(&cur_dtb_info, &best_dtb_info, VARIANT_MATCH);
+		dprintf(SPEW, "dtbo count = %u local_board_dt_match =%x\n",dtbo_count, cur_dtb_info.dt_match_val);
+		dtb_table_entry++;
+	}
+	if (!best_dtb_info.dtb) {
+		dprintf(CRITICAL, "Unable to find the board dtb\n");
+		return NULL;
+	}
+	return best_dtb_info.dtb;
+}
+
 static void insert_dt_entry_in_queue(struct dt_entry_node *dt_list, struct dt_entry_node *dt_node_member)
 {
 	list_add_tail(&dt_list->node, &dt_node_member->node);
@@ -452,6 +983,109 @@
 	return true;
 }
 
+/* function to handle the overlay in independent thread */
+static int dtb_overlay_handler(void *args)
+{
+	dprintf(SPEW, "thread %s() started\n", __func__);
+
+	soc_dtb_hdr = ufdt_install_blob(soc_dtb, fdt_totalsize(soc_dtb));
+	if(!soc_dtb_hdr)
+	{
+		dprintf(CRITICAL, "ERROR: Install Blob failed\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+	final_dtb_hdr = ufdt_apply_overlay(soc_dtb_hdr, fdt_totalsize(soc_dtb_hdr),board_dtb,
+							fdt_totalsize(board_dtb));
+	if (!final_dtb_hdr)
+	{
+		dprintf(CRITICAL, "ERROR: UFDT apply overlay failed\n");
+		ret = DTBO_ERROR;
+		goto out;
+	}
+out:
+	/* This flag can only be updated here, hence it is not protected */
+	event_signal(&dtbo_event, true);
+	thread_exit(0);
+	return 0;
+}
+
+/*
+ * Function to check and form new dtb with Overlay support.
+*/
+dtbo_error dev_tree_appended_with_dtbo(void *kernel, uint32_t kernel_size,
+					uint32_t dtb_offset, void *tags)
+{
+	void *dtbo_image_buf = NULL;
+
+	ret = load_validate_dtbo_image(&dtbo_image_buf);
+	if (ret == DTBO_SUCCESS)
+	{
+		final_dtb_hdr = soc_dtb = get_soc_dtb(kernel,
+						kernel_size, dtb_offset);
+		if(!soc_dtb)
+		{
+			ret = DTBO_ERROR;
+			goto out;
+		}
+
+		if (dtbo_needed)
+		{
+			board_dtb = get_board_dtb(dtbo_image_buf);
+			if(!board_dtb)
+			{
+				ret = DTBO_ERROR;
+				goto out;
+			}
+
+			if((ADD_OF((fdt_totalsize(soc_dtb)), (fdt_totalsize(board_dtb)))) > MAX_DTBO_SZ)
+			{
+				dprintf(CRITICAL, "ERROR: dtb greater than max supported.\n");
+				ret = DTBO_ERROR;
+				goto out;
+			}
+
+			/*
+			spawn a seperate thread for dtbo overlay with indpendent,
+			stack to avoid issues with stack corruption seen during flattening,
+			of dtb in overlay functionality
+			*/
+			{
+				thread_t *thr = NULL;
+				event_init(&dtbo_event, 0, EVENT_FLAG_AUTOUNSIGNAL);
+				thr = thread_create("dtb_overlay", dtb_overlay_handler, 0,
+							DEFAULT_PRIORITY, DTBO_STACK_SIZE);
+				if (!thr)
+				{
+					dprintf(CRITICAL, "ERROR: Failed to create DTBO thread.\n");
+					ret = DTBO_ERROR;
+					goto out;
+				}
+				thread_resume(thr);
+
+				/* block current thread, untill woken up by dtb_overlay_handler. */
+				event_wait(&dtbo_event);
+
+				/* ret is updated by dtb overlay thread, in case of error */
+				if(ret == DTBO_ERROR)
+				{
+					goto out;
+				}
+			} /* dtbo_overlay_handler exited */
+
+		}
+		memscpy(tags, fdt_totalsize(final_dtb_hdr), final_dtb_hdr,
+						fdt_totalsize(final_dtb_hdr));
+		dprintf(CRITICAL, "DTB overlay is successful\n");
+	}
+	else
+	{
+		dprintf(CRITICAL, "ERROR: DTBO read is not valid\n DTB Overlay failed.\n");
+		ret = DTBO_NOT_SUPPORTED;
+	}
+out:
+	return ret;
+}
 /*
  * Will relocate the DTB to the tags addr if the device tree is found and return
  * its address
@@ -474,6 +1108,14 @@
 	struct dt_entry_node *dt_entry_queue = NULL;
 	struct dt_entry_node *dt_node_tmp1 = NULL;
 	struct dt_entry_node *dt_node_tmp2 = NULL;
+	dtbo_error ret = DTBO_NOT_SUPPORTED;
+
+	/* Check for dtbo support */
+	ret = dev_tree_appended_with_dtbo(kernel, kernel_size, dtb_offset, tags);
+	if (ret == DTBO_SUCCESS)
+		return tags;
+	else if (ret == DTBO_ERROR)
+		return NULL;
 
 	/* Initialize the dtb entry node*/
 	dt_entry_queue = (struct dt_entry_node *)
diff --git a/platform/msm_shared/include/dev_tree.h b/platform/msm_shared/include/dev_tree.h
index 1d2bd44..6457525 100755
--- a/platform/msm_shared/include/dev_tree.h
+++ b/platform/msm_shared/include/dev_tree.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2014,2017-2018 The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -28,6 +28,7 @@
 
 #include <debug.h>
 #include <list.h>
+#include <bits.h>
 
 #ifndef __DEVICE_TREE__
 #define __DEVICE_TREE__
@@ -45,7 +46,19 @@
 #define DTB_OFFSET              0x2C
 
 #define DTB_PAD_SIZE            1024
-
+#define DTBO_TABLE_MAGIC        0xD7B7AB1E
+#define DTBO_CUSTOM_MAX         4
+#define PLATFORM_FOUNDRY_SHIFT  16
+#define SOC_MASK                  (0xffff)
+#define VARIANT_MASK              (0x000000ff)
+#define VARIANT_MINOR_MASK        (0x0000ff00)
+#define VARIANT_MAJOR_MASK        (0x00ff0000)
+#define PMIC_MODEL_MASK           (0x000000ff)
+#define PMIC_REV_MASK             (0xffffff00)
+#define PMIC_SHIFT_IDX            (2)
+#define PLATFORM_SUBTYPE_SHIFT_ID (0x18)
+#define FOUNDRY_ID_MASK           (0x00ff0000)
+#define DTBO_IMG_BUF             (8388608) /* 8MB 8 * 1024 * 1024 */
 /*
  * For DTB V1: The DTB entries would be of the format
  * qcom,msm-id = <msm8974, CDP, rev_1>; (3 * sizeof(uint32_t))
@@ -59,6 +72,75 @@
 #define BOARD_ID_SIZE           0x8
 #define PMIC_ID_SIZE           0x8
 
+typedef enum {
+	NONE_MATCH,
+	PMIC_MATCH_BEST_REV_IDX0,
+	PMIC_MATCH_EXACT_REV_IDX0,
+	PMIC_MATCH_BEST_REV_IDX1,
+	PMIC_MATCH_EXACT_REV_IDX1,
+	PMIC_MATCH_BEST_REV_IDX2,
+	PMIC_MATCH_EXACT_REV_IDX2,
+	PMIC_MATCH_BEST_REV_IDX3,
+	PMIC_MATCH_EXACT_REV_IDX3,
+	VARIANT_MINOR_BEST_MATCH,
+	VARIANT_MINOR_EXACT_MATCH,
+	VARIANT_MAJOR_BEST_MATCH,
+	VARIANT_MAJOR_EXACT_MATCH,
+	VERSION_BEST_MATCH,
+	VERSION_EXACT_MATCH,
+	FOUNDRYID_DEFAULT_MATCH,
+	FOUNDRYID_EXACT_MATCH,
+	PMIC_MATCH_DEFAULT_MODEL_IDX0,
+	PMIC_MATCH_EXACT_MODEL_IDX0,
+	PMIC_MATCH_DEFAULT_MODEL_IDX1,
+	PMIC_MATCH_EXACT_MODEL_IDX1,
+	PMIC_MATCH_DEFAULT_MODEL_IDX2,
+	PMIC_MATCH_EXACT_MODEL_IDX2,
+	PMIC_MATCH_DEFAULT_MODEL_IDX3,
+	PMIC_MATCH_EXACT_MODEL_IDX3,
+	SUBTYPE_DEFAULT_MATCH,
+	SUBTYPE_EXACT_MATCH,
+	VARIANT_MATCH,
+	SOC_MATCH,
+	MAX_MATCH,
+}dt_match_params;
+
+#define TOTAL_MATCH_BITS 6
+#define ALL_BITS_SET  (BIT (SOC_MATCH) | BIT (VARIANT_MATCH) | \
+                       BIT (SUBTYPE_EXACT_MATCH) | BIT (FOUNDRYID_EXACT_MATCH) \
+                       | BIT (PMIC_MATCH_EXACT_MODEL_IDX0) | \
+                       BIT (PMIC_MATCH_EXACT_MODEL_IDX1))
+
+typedef enum {
+	PMIC_IDX0,
+	PMIC_IDX1,
+	PMIC_IDX2,
+	PMIC_IDX3,
+	MAX_PMIC_IDX,
+}pmic_indexes;
+
+
+typedef struct dt_info
+{
+	uint32_t dt_platform_id;
+	uint32_t dt_soc_rev;
+	uint32_t dt_foundry_id;
+	uint32_t dt_variant_id;
+	uint32_t dt_variant_major;
+	uint32_t dt_variant_minor;
+	uint32_t dt_platform_subtype;
+	uint32_t dt_pmic_model[MAX_PMIC_IDX];
+	uint32_t dt_pmic_rev[MAX_PMIC_IDX];
+	uint32_t dt_match_val;
+	void *dtb;
+}dt_info;
+
+typedef struct pmic_info
+{
+	uint32_t dt_pmic_model[MAX_PMIC_IDX];
+	uint32_t dt_pmic_rev[MAX_PMIC_IDX];
+	uint32_t dt_match_val;
+}pmic_info;
 
 struct dt_entry_v2
 {
@@ -138,6 +220,25 @@
 	struct dt_entry * dt_entry_m;
 }dt_node;
 
+struct dtbo_table_hdr {
+	uint32_t magic;           //dtb table magic
+	uint32_t total_size;       //Includes dt_table_hdr + all dt_table_entry and all dtb/dtbo
+	uint32_t hdr_size;      //sizeof(dt_table_hdr)
+	uint32_t dt_entry_size;     //sizeof(dt_table_entry)
+	uint32_t dt_entry_count;    //number of dt_table_entry
+	uint32_t dt_entry_offset;   //offset to the first dt_table_entry
+	uint32_t page_size;        //flash pagesize we assume
+	uint32_t reserved[1];     //must zeros
+};
+
+struct dtbo_table_entry {
+	uint32_t dt_size;
+	uint32_t dt_offset;     //offset from head of dt_table_hdr
+	uint32_t id;           //optional, must zero if unused
+	uint32_t revision;          //optional, must zero if unused
+	uint32_t custom[DTBO_CUSTOM_MAX];    //optional, must zero if unused
+};
+
 int dev_tree_validate(struct dt_table *table, unsigned int page_size, uint32_t *dt_hdr_size);
 int dev_tree_get_entry_info(struct dt_table *table, struct dt_entry *dt_entry_info);
 int update_device_tree(void *fdt, const char *, void *, unsigned);
diff --git a/target/rules.mk b/target/rules.mk
index 32095ae..571177d 100644
--- a/target/rules.mk
+++ b/target/rules.mk
@@ -4,3 +4,4 @@
 	$(LOCAL_DIR)/init.o \
 	$(LOCAL_DIR)/target_display.o
 
+MODULES +=  lib/libufdt