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