1// Copyright 2018 The Fuchsia Authors. All rights reserved.
2// Use of this source code is governed by a BSD-style license that can be
3// found in the LICENSE file.
4
5#include "devicetree.h"
6
7#define DT_MAGIC        0xD00DFEED
8#define DT_NODE_BEGIN   1
9#define DT_NODE_END     2
10#define DT_PROP         3
11#define DT_END          9
12
13typedef struct dt_slice slice_t;
14
15uint32_t dt_rd32(uint8_t *data) {
16    return (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
17}
18
19void dt_wr32(uint32_t n, uint8_t *data) {
20    *data++ = n >> 24;
21    *data++ = n >> 16;
22    *data++ = n >> 8;
23    *data = n;
24}
25
26/* init subslice from slice, returning 0 if successful */
27static int sslice(slice_t *src, slice_t *dst, uint32_t off, uint32_t len) {
28    if (off >= src->size)
29        return -1;
30    if (len >= src->size)
31        return -1;
32    if ((off + len) > src->size)
33        return -1;
34    dst->data = src->data + off;
35    dst->size = len;
36    return 0;
37}
38
39/* return nonzero if slice is empty */
40static inline int sempty(slice_t *s) {
41    return s->size == 0;
42}
43
44/* read be32 from slice,
45 * or 0 (and make slice empty) if slice is too small
46 */
47static uint32_t suint32_t(slice_t *s) {
48    if (s->size < 4) {
49        s->size = 0;
50        return 0;
51    } else {
52        uint32_t n = (s->data[0] << 24) | (s->data[1] << 16) | (s->data[2] << 8) | s->data[3];
53        s->size -= 4;
54        s->data += 4;
55        return n;
56    }
57}
58
59/* return pointer to data in slice,
60 * or 0 (and make slice empty) if slice is too small
61 */
62static void *sdata(slice_t *s, uint32_t len) {
63    if (len > s->size) {
64        s->size = 0;
65        return 0;
66    } else {
67        void *data = s->data;
68        s->size -= len;
69        s->data += len;
70        while (len & 3) {
71            if (s->size) {
72                s->size++;
73                s->data++;
74            }
75            len++;
76        }
77        return data;
78    }
79}
80
81/* return pointer to string in slice,
82 * or "" (and make slice empty) if slice is too small
83 */
84static const char *sstring(slice_t *s) {
85    uint32_t sz = s->size;
86    uint8_t  *end = s->data;
87    const char *data;
88    while (sz-- > 0) {
89        if (*end++ == 0) {
90            while (((end - s->data) & 3) && (sz > 0)) {
91                end++;
92                sz--;
93            }
94            data = (const char*) s->data;
95            s->size = sz;
96            s->data = end;
97            return data;
98        }
99    }
100    s->size = 0;
101    return "";
102}
103
104static int oops(devicetree_t *dt, const char *msg) {
105    if (dt->error)
106        dt->error(msg);
107    return -1;
108}
109
110int dt_init(devicetree_t *dt, void *data, uint32_t len) {
111    slice_t s;
112
113    dt->top.data = data;
114    dt->top.size = len;
115
116    s = dt->top;
117
118    dt->hdr.magic = suint32_t(&s);
119    dt->hdr.size = suint32_t(&s);
120    dt->hdr.off_struct = suint32_t(&s);
121    dt->hdr.off_strings = suint32_t(&s);
122    dt->hdr.off_reserve = suint32_t(&s);
123    dt->hdr.version = suint32_t(&s);
124    dt->hdr.version_compat = suint32_t(&s);
125    dt->hdr.boot_cpuid = suint32_t(&s);
126    dt->hdr.sz_strings = suint32_t(&s);
127    dt->hdr.sz_struct = suint32_t(&s);
128
129    if (dt->hdr.magic != DT_MAGIC)
130        return oops(dt, "bad magic");
131    if (dt->hdr.size > dt->top.size)
132        return oops(dt, "bogus size field");
133    if (dt->hdr.version != 17)
134        return oops(dt, "version != 17");
135    if (sslice(&dt->top, &dt->dt, dt->hdr.off_struct, dt->hdr.sz_struct))
136        return oops(dt, "invalid structure off/len");
137    if (sslice(&dt->top, &dt->ds, dt->hdr.off_strings, dt->hdr.sz_strings))
138        return oops(dt, "invalid strings off/len");
139
140    return 0;
141}
142
143int dt_walk(devicetree_t *dtree, dt_node_cb ncb, dt_prop_cb pcb, void *cookie) {
144    const char *p;
145    void *data;
146    uint32_t depth = 0;
147    slice_t dt, ds;
148    uint32_t sz, str;
149
150    dt = dtree->dt;
151    ds = dtree->ds;
152
153    while (!sempty(&dt)) {
154        uint32_t type = suint32_t(&dt);
155        switch (type) {
156        case DT_END:
157            if (depth)
158                return oops(dtree, "unexpected DT_END");
159            return 0;
160        case DT_NODE_BEGIN:
161            depth++;
162            p = sstring(&dt);
163            if (ncb(depth, p, cookie))
164                return 0;
165            break;
166        case DT_NODE_END:
167            if (depth == 0)
168                return oops(dtree, "unexpected NODE_END");
169            depth--;
170            break;
171        case DT_PROP:
172            if (depth == 0)
173                return oops(dtree, "PROP outside of NODE");
174            sz = suint32_t(&dt);
175            str = suint32_t(&dt);
176            data = sdata(&dt, sz);
177            if (pcb((const char*) (ds.data + str), data, sz, cookie))
178                return 0;
179            break;
180        default:
181            return oops(dtree, "invalid node type");
182
183        }
184    }
185
186    if (depth != 0)
187        return oops(dtree, "incomplete tree");
188
189    return 0;
190}
191