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