[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/lib/devicetree/devicetree.c b/src/bsp/lk/lib/devicetree/devicetree.c
new file mode 100644
index 0000000..c43d805
--- /dev/null
+++ b/src/bsp/lk/lib/devicetree/devicetree.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "devicetree.h"
+
+#define DT_MAGIC 0xD00DFEED
+#define DT_NODE_BEGIN 1
+#define DT_NODE_END 2
+#define DT_PROP 3
+#define DT_END 9
+
+typedef struct dt_slice slice_t;
+
+u32 dt_rd32(u8 *data) {
+ return (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
+}
+
+void dt_wr32(u32 n, u8 *data) {
+ *data++ = n >> 24;
+ *data++ = n >> 16;
+ *data++ = n >> 8;
+ *data = n;
+}
+
+/* init subslice from slice, returning 0 if successful */
+static int sslice(slice_t *src, slice_t *dst, u32 off, u32 len) {
+ if (off >= src->size)
+ return -1;
+ if (len >= src->size)
+ return -1;
+ if ((off + len) > src->size)
+ return -1;
+ dst->data = src->data + off;
+ dst->size = len;
+ return 0;
+}
+
+/* return nonzero if slice is empty */
+static inline int sempty(slice_t *s) {
+ return s->size == 0;
+}
+
+/* read be32 from slice,
+ * or 0 (and make slice empty) if slice is too small
+ */
+static u32 su32(slice_t *s) {
+ if (s->size < 4) {
+ s->size = 0;
+ return 0;
+ } else {
+ u32 n = (s->data[0] << 24) | (s->data[1] << 16) | (s->data[2] << 8) | s->data[3];
+ s->size -= 4;
+ s->data += 4;
+ return n;
+ }
+}
+
+/* return pointer to data in slice,
+ * or 0 (and make slice empty) if slice is too small
+ */
+static void *sdata(slice_t *s, u32 len) {
+ if (len > s->size) {
+ s->size = 0;
+ return 0;
+ } else {
+ void *data = s->data;
+ s->size -= len;
+ s->data += len;
+ while (len & 3) {
+ if (s->size) {
+ s->size++;
+ s->data++;
+ }
+ len++;
+ }
+ return data;
+ }
+}
+
+/* return pointer to string in slice,
+ * or "" (and make slice empty) if slice is too small
+ */
+static const char *sstring(slice_t *s) {
+ u32 sz = s->size;
+ u8 *end = s->data;
+ const char *data;
+ while (sz-- > 0) {
+ if (*end++ == 0) {
+ while (((end - s->data) & 3) && (sz > 0)) {
+ end++;
+ sz--;
+ }
+ data = (const char*) s->data;
+ s->size = sz;
+ s->data = end;
+ return data;
+ }
+ }
+ s->size = 0;
+ return "";
+}
+
+static int oops(devicetree_t *dt, const char *msg) {
+ if (dt->error)
+ dt->error(msg);
+ return -1;
+}
+
+int dt_init(devicetree_t *dt, void *data, u32 len) {
+ slice_t s;
+
+ dt->top.data = data;
+ dt->top.size = len;
+
+ s = dt->top;
+
+ dt->hdr.magic = su32(&s);
+ dt->hdr.size = su32(&s);
+ dt->hdr.off_struct = su32(&s);
+ dt->hdr.off_strings = su32(&s);
+ dt->hdr.off_reserve = su32(&s);
+ dt->hdr.version = su32(&s);
+ dt->hdr.version_compat = su32(&s);
+ dt->hdr.boot_cpuid = su32(&s);
+ dt->hdr.sz_strings = su32(&s);
+ dt->hdr.sz_struct = su32(&s);
+
+ if (dt->hdr.magic != DT_MAGIC)
+ return oops(dt, "bad magic");
+ if (dt->hdr.size > dt->top.size)
+ return oops(dt, "bogus size field");
+ if (dt->hdr.version != 17)
+ return oops(dt, "version != 17");
+ if (sslice(&dt->top, &dt->dt, dt->hdr.off_struct, dt->hdr.sz_struct))
+ return oops(dt, "invalid structure off/len");
+ if (sslice(&dt->top, &dt->ds, dt->hdr.off_strings, dt->hdr.sz_strings))
+ return oops(dt, "invalid strings off/len");
+
+ return 0;
+}
+
+int dt_walk(devicetree_t *dtree, dt_node_cb ncb, dt_prop_cb pcb, void *cookie) {
+ const char *p;
+ void *data;
+ u32 depth = 0;
+ slice_t dt, ds;
+ u32 sz, str;
+
+ dt = dtree->dt;
+ ds = dtree->ds;
+
+ while (!sempty(&dt)) {
+ u32 type = su32(&dt);
+ switch (type) {
+ case DT_END:
+ if (depth)
+ return oops(dtree, "unexpected DT_END");
+ return 0;
+ case DT_NODE_BEGIN:
+ depth++;
+ p = sstring(&dt);
+ if (ncb(depth, p, cookie))
+ return 0;
+ break;
+ case DT_NODE_END:
+ if (depth == 0)
+ return oops(dtree, "unexpected NODE_END");
+ depth--;
+ break;
+ case DT_PROP:
+ if (depth == 0)
+ return oops(dtree, "PROP outside of NODE");
+ sz = su32(&dt);
+ str = su32(&dt);
+ data = sdata(&dt, sz);
+ if (pcb((const char*) (ds.data + str), data, sz, cookie))
+ return 0;
+ break;
+ default:
+ return oops(dtree, "invalid node type");
+
+ }
+ }
+
+ if (depth != 0)
+ return oops(dtree, "incomplete tree");
+
+ return 0;
+}