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 
13 typedef struct dt_slice slice_t;
14 
dt_rd32(uint8_t * data)15 uint32_t dt_rd32(uint8_t *data) {
16     return (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
17 }
18 
dt_wr32(uint32_t n,uint8_t * data)19 void 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 */
sslice(slice_t * src,slice_t * dst,uint32_t off,uint32_t len)27 static 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 */
sempty(slice_t * s)40 static 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  */
suint32_t(slice_t * s)47 static 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  */
sdata(slice_t * s,uint32_t len)62 static 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  */
sstring(slice_t * s)84 static 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 
oops(devicetree_t * dt,const char * msg)104 static int oops(devicetree_t *dt, const char *msg) {
105     if (dt->error)
106         dt->error(msg);
107     return -1;
108 }
109 
dt_init(devicetree_t * dt,void * data,uint32_t len)110 int 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 
dt_walk(devicetree_t * dtree,dt_node_cb ncb,dt_prop_cb pcb,void * cookie)143 int 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