1 /* $OpenBSD: fdt.c,v 1.10 2023/02/13 16:16:03 kettenis Exp $ */
2
3 /*
4 * Copyright (c) 2009 Dariusz Swiderski <sfires@sfires.net>
5 * Copyright (c) 2009, 2016 Mark Kettenis <kettenis@openbsd.org>
6 *
7 * Permission to use, copy, modify, and distribute this software for any
8 * purpose with or without fee is hereby granted, provided that the above
9 * copyright notice and this permission notice appear in all copies.
10 *
11 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
12 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
13 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
14 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
15 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
16 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
17 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
18 */
19
20 #include <sys/param.h>
21 #include <sys/systm.h>
22
23 #include <lib/libkern/libkern.h>
24
25 #include "fdt.h"
26
27 unsigned int fdt_check_head(void *);
28 char *fdt_get_str(uint32_t);
29 void *skip_property(uint32_t *);
30 void *skip_props(uint32_t *);
31 void *skip_node_name(uint32_t *);
32 void *skip_node(void *);
33 void *skip_nops(uint32_t *);
34 void *fdt_parent_node_recurse(void *, void *);
35
36 static int tree_inited = 0;
37 static struct fdt tree;
38
39 unsigned int
fdt_check_head(void * fdt)40 fdt_check_head(void *fdt)
41 {
42 struct fdt_head *fh;
43 uint32_t *ptr, *tok;
44
45 fh = fdt;
46 ptr = (uint32_t *)fdt;
47
48 if (betoh32(fh->fh_magic) != FDT_MAGIC)
49 return 0;
50
51 if (betoh32(fh->fh_version) > FDT_CODE_VERSION)
52 return 0;
53
54 tok = skip_nops(ptr + (betoh32(fh->fh_struct_off) / 4));
55 if (betoh32(*tok) != FDT_NODE_BEGIN)
56 return 0;
57
58 /* check for end signature on version 17 blob */
59 if ((betoh32(fh->fh_version) >= 17) &&
60 (betoh32(*(ptr + (betoh32(fh->fh_struct_off) / 4) +
61 (betoh32(fh->fh_struct_size) / 4) - 1)) != FDT_END))
62 return 0;
63
64 return betoh32(fh->fh_version);
65 }
66
67 /*
68 * Initializes internal structures of module.
69 * Has to be called once.
70 */
71 int
fdt_init(void * fdt)72 fdt_init(void *fdt)
73 {
74 int version;
75
76 memset(&tree, 0, sizeof(struct fdt));
77 tree_inited = 0;
78
79 if (!fdt)
80 return 0;
81
82 if (!(version = fdt_check_head(fdt)))
83 return 0;
84
85 tree.header = (struct fdt_head *)fdt;
86 tree.tree = (char *)fdt + betoh32(tree.header->fh_struct_off);
87 tree.strings = (char *)fdt + betoh32(tree.header->fh_strings_off);
88 tree.memory = (char *)fdt + betoh32(tree.header->fh_reserve_off);
89 tree.end = (char *)fdt + betoh32(tree.header->fh_size);
90 tree.version = version;
91 tree.strings_size = betoh32(tree.header->fh_strings_size);
92 if (tree.version >= 17)
93 tree.struct_size = betoh32(tree.header->fh_struct_size);
94 tree_inited = 1;
95
96 return version;
97 }
98
99 void
fdt_finalize(void)100 fdt_finalize(void)
101 {
102 char *start = (char *)tree.header;
103
104 tree.header->fh_size = htobe32(tree.end - start);
105 tree.header->fh_struct_off = htobe32(tree.tree - start);
106 tree.header->fh_strings_off = htobe32(tree.strings - start);
107 tree.header->fh_reserve_off = htobe32(tree.memory - start);
108 tree.header->fh_strings_size = htobe32(tree.strings_size);
109 if (tree.version >= 17)
110 tree.header->fh_struct_size = htobe32(tree.struct_size);
111 }
112
113 /*
114 * Return the size of the FDT.
115 */
116 size_t
fdt_get_size(void * fdt)117 fdt_get_size(void *fdt)
118 {
119 if (!fdt)
120 return 0;
121
122 if (!fdt_check_head(fdt))
123 return 0;
124
125 return betoh32(((struct fdt_head *)fdt)->fh_size);
126 }
127
128 /*
129 * Retrieve string pointer from strings table.
130 */
131 char *
fdt_get_str(uint32_t num)132 fdt_get_str(uint32_t num)
133 {
134 if (num > tree.strings_size)
135 return NULL;
136 return (tree.strings) ? (tree.strings + num) : NULL;
137 }
138
139 int
fdt_add_str(char * name)140 fdt_add_str(char *name)
141 {
142 size_t len = roundup(strlen(name) + 1, sizeof(uint32_t));
143 char *end = tree.strings + tree.strings_size;
144
145 if (end + len > tree.end)
146 panic("FDT overflow");
147
148 tree.strings_size += len;
149 memset(end, 0, len);
150 memcpy(end, name, strlen(name));
151
152 return (end - tree.strings);
153 }
154
155 /*
156 * Utility functions for skipping parts of tree.
157 */
158
159 void *
skip_nops(uint32_t * ptr)160 skip_nops(uint32_t *ptr)
161 {
162 while (betoh32(*ptr) == FDT_NOP)
163 ptr++;
164
165 return ptr;
166 }
167
168 void *
skip_property(uint32_t * ptr)169 skip_property(uint32_t *ptr)
170 {
171 uint32_t size;
172
173 size = betoh32(*(ptr + 1));
174 /* move forward by magic + size + nameid + rounded up property size */
175 ptr += 3 + roundup(size, sizeof(uint32_t)) / sizeof(uint32_t);
176
177 return skip_nops(ptr);
178 }
179
180 void *
skip_props(uint32_t * ptr)181 skip_props(uint32_t *ptr)
182 {
183 while (betoh32(*ptr) == FDT_PROPERTY) {
184 ptr = skip_property(ptr);
185 }
186 return ptr;
187 }
188
189 void *
skip_node_name(uint32_t * ptr)190 skip_node_name(uint32_t *ptr)
191 {
192 /* skip name, aligned to 4 bytes, this is NULL term., so must add 1 */
193 ptr += roundup(strlen((char *)ptr) + 1,
194 sizeof(uint32_t)) / sizeof(uint32_t);
195
196 return skip_nops(ptr);
197 }
198
199 /*
200 * Retrieves node property, the returned pointer is inside the fdt tree,
201 * so we should not modify content pointed by it directly.
202 */
203 int
fdt_node_property(void * node,char * name,char ** out)204 fdt_node_property(void *node, char *name, char **out)
205 {
206 uint32_t *ptr;
207 uint32_t nameid;
208 char *tmp;
209
210 if (!tree_inited)
211 return 0;
212
213 ptr = (uint32_t *)node;
214
215 if (betoh32(*ptr) != FDT_NODE_BEGIN)
216 return 0;
217
218 ptr = skip_node_name(ptr + 1);
219
220 while (betoh32(*ptr) == FDT_PROPERTY) {
221 nameid = betoh32(*(ptr + 2)); /* id of name in strings table */
222 tmp = fdt_get_str(nameid);
223 if (!strcmp(name, tmp)) {
224 *out = (char *)(ptr + 3); /* beginning of the value */
225 return betoh32(*(ptr + 1)); /* size of value */
226 }
227 ptr = skip_property(ptr);
228 }
229 return 0;
230 }
231
232 int
fdt_node_set_property(void * node,char * name,void * data,int len)233 fdt_node_set_property(void *node, char *name, void *data, int len)
234 {
235 char *end = tree.strings + tree.strings_size;
236 uint32_t *ptr, *next;
237 uint32_t nameid;
238 uint32_t curlen;
239 size_t delta;
240 char *tmp;
241
242 if (!tree_inited)
243 return 0;
244
245 ptr = (uint32_t *)node;
246
247 if (betoh32(*ptr) != FDT_NODE_BEGIN)
248 return 0;
249
250 ptr = skip_node_name(ptr + 1);
251
252 while (betoh32(*ptr) == FDT_PROPERTY) {
253 nameid = betoh32(*(ptr + 2)); /* id of name in strings table */
254 tmp = fdt_get_str(nameid);
255 next = skip_property(ptr);
256 if (!strcmp(name, tmp)) {
257 curlen = betoh32(*(ptr + 1));
258 delta = roundup(len, sizeof(uint32_t)) -
259 roundup(curlen, sizeof(uint32_t));
260 if (end + delta > tree.end)
261 panic("FDT overflow");
262
263 memmove((char *)next + delta, next,
264 end - (char *)next);
265 tree.struct_size += delta;
266 tree.strings += delta;
267 *(ptr + 1) = htobe32(len);
268 memcpy(ptr + 3, data, len);
269 return 1;
270 }
271 ptr = next;
272 }
273 return 0;
274 }
275
276 int
fdt_node_add_property(void * node,char * name,void * data,int len)277 fdt_node_add_property(void *node, char *name, void *data, int len)
278 {
279 char *end = tree.strings + tree.strings_size;
280 char *dummy;
281
282 if (!tree_inited)
283 return 0;
284
285 if (!fdt_node_property(node, name, &dummy)) {
286 uint32_t *ptr = (uint32_t *)node;
287
288 if (betoh32(*ptr) != FDT_NODE_BEGIN)
289 return 0;
290
291 if (end + 3 * sizeof(uint32_t) > tree.end)
292 panic("FDT overflow");
293
294 ptr = skip_node_name(ptr + 1);
295
296 memmove(ptr + 3, ptr, end - (char *)ptr);
297 tree.struct_size += 3 * sizeof(uint32_t);
298 tree.strings += 3 * sizeof(uint32_t);
299 *ptr++ = htobe32(FDT_PROPERTY);
300 *ptr++ = htobe32(0);
301 *ptr++ = htobe32(fdt_add_str(name));
302 }
303
304 return fdt_node_set_property(node, name, data, len);
305 }
306
307 int
fdt_node_add_node(void * node,char * name,void ** child)308 fdt_node_add_node(void *node, char *name, void **child)
309 {
310 size_t len = roundup(strlen(name) + 1, sizeof(uint32_t)) + 8;
311 char *end = tree.strings + tree.strings_size;
312 uint32_t *ptr = (uint32_t *)node;
313
314 if (!tree_inited)
315 return 0;
316
317 if (betoh32(*ptr) != FDT_NODE_BEGIN)
318 return 0;
319
320 if (end + len > tree.end)
321 panic("FDT overflow");
322
323 ptr = skip_node_name(ptr + 1);
324 ptr = skip_props(ptr);
325
326 /* skip children */
327 while (betoh32(*ptr) == FDT_NODE_BEGIN)
328 ptr = skip_node(ptr);
329
330 memmove((char *)ptr + len, ptr, end - (char *)ptr);
331 tree.struct_size += len;
332 tree.strings += len;
333
334 *child = ptr;
335 *ptr++ = htobe32(FDT_NODE_BEGIN);
336 memset(ptr, 0, len - 8);
337 memcpy(ptr, name, strlen(name));
338 ptr += (len - 8) / sizeof(uint32_t);
339 *ptr++ = htobe32(FDT_NODE_END);
340
341 return 1;
342 }
343
344 /*
345 * Retrieves next node, skipping all the children nodes of the pointed node,
346 * returns pointer to next node, no matter if it exists or not.
347 */
348 void *
skip_node(void * node)349 skip_node(void *node)
350 {
351 uint32_t *ptr = node;
352
353 ptr++;
354
355 ptr = skip_node_name(ptr);
356 ptr = skip_props(ptr);
357
358 /* skip children */
359 while (betoh32(*ptr) == FDT_NODE_BEGIN)
360 ptr = skip_node(ptr);
361
362 return skip_nops(ptr + 1);
363 }
364
365 /*
366 * Retrieves next node, skipping all the children nodes of the pointed node,
367 * returns pointer to next node if exists, otherwise returns NULL.
368 * If passed 0 will return first node of the tree (root).
369 */
370 void *
fdt_next_node(void * node)371 fdt_next_node(void *node)
372 {
373 uint32_t *ptr;
374
375 if (!tree_inited)
376 return NULL;
377
378 ptr = node;
379
380 if (node == NULL) {
381 ptr = skip_nops((uint32_t *)tree.tree);
382 return (betoh32(*ptr) == FDT_NODE_BEGIN) ? ptr : NULL;
383 }
384
385 if (betoh32(*ptr) != FDT_NODE_BEGIN)
386 return NULL;
387
388 ptr++;
389
390 ptr = skip_node_name(ptr);
391 ptr = skip_props(ptr);
392
393 /* skip children */
394 while (betoh32(*ptr) == FDT_NODE_BEGIN)
395 ptr = skip_node(ptr);
396
397 if (betoh32(*ptr) != FDT_NODE_END)
398 return NULL;
399
400 ptr = skip_nops(ptr + 1);
401
402 if (betoh32(*ptr) != FDT_NODE_BEGIN)
403 return NULL;
404
405 return ptr;
406 }
407
408 /*
409 * Retrieves node property as integers and puts them in the given
410 * integer array.
411 */
412 int
fdt_node_property_ints(void * node,char * name,int * out,int outlen)413 fdt_node_property_ints(void *node, char *name, int *out, int outlen)
414 {
415 int *data;
416 int i, inlen;
417
418 inlen = fdt_node_property(node, name, (char **)&data) / sizeof(int);
419 if (inlen <= 0)
420 return -1;
421
422 for (i = 0; i < inlen && i < outlen; i++)
423 out[i] = betoh32(data[i]);
424
425 return i;
426 }
427
428 /*
429 * Retrieves node property as an integer.
430 */
431 int
fdt_node_property_int(void * node,char * name,int * out)432 fdt_node_property_int(void *node, char *name, int *out)
433 {
434 return fdt_node_property_ints(node, name, out, 1);
435 }
436
437 /*
438 * Retrieves next node, skipping all the children nodes of the pointed node
439 */
440 void *
fdt_child_node(void * node)441 fdt_child_node(void *node)
442 {
443 uint32_t *ptr;
444
445 if (!tree_inited)
446 return NULL;
447
448 ptr = node;
449
450 if (betoh32(*ptr) != FDT_NODE_BEGIN)
451 return NULL;
452
453 ptr++;
454
455 ptr = skip_node_name(ptr);
456 ptr = skip_props(ptr);
457 /* check if there is a child node */
458 return (betoh32(*ptr) == FDT_NODE_BEGIN) ? (ptr) : NULL;
459 }
460
461 /*
462 * Retrieves node name.
463 */
464 char *
fdt_node_name(void * node)465 fdt_node_name(void *node)
466 {
467 uint32_t *ptr;
468
469 if (!tree_inited)
470 return NULL;
471
472 ptr = node;
473
474 if (betoh32(*ptr) != FDT_NODE_BEGIN)
475 return NULL;
476
477 return (char *)(ptr + 1);
478 }
479
480 void *
fdt_find_node(char * name)481 fdt_find_node(char *name)
482 {
483 void *node = fdt_next_node(0);
484 const char *p = name;
485
486 if (!tree_inited)
487 return NULL;
488
489 if (*p != '/')
490 return NULL;
491
492 while (*p) {
493 void *child;
494 const char *q;
495
496 while (*p == '/')
497 p++;
498 if (*p == 0)
499 return node;
500 q = strchr(p, '/');
501 if (q == NULL)
502 q = p + strlen(p);
503
504 for (child = fdt_child_node(node); child;
505 child = fdt_next_node(child)) {
506 if (strncmp(p, fdt_node_name(child), q - p) == 0) {
507 node = child;
508 break;
509 }
510 }
511
512 if (child == NULL)
513 return NULL; /* No match found. */
514
515 p = q;
516 }
517
518 return node;
519 }
520
521 void *
fdt_parent_node_recurse(void * pnode,void * child)522 fdt_parent_node_recurse(void *pnode, void *child)
523 {
524 void *node = fdt_child_node(pnode);
525 void *tmp;
526
527 while (node && (node != child)) {
528 if ((tmp = fdt_parent_node_recurse(node, child)))
529 return tmp;
530 node = fdt_next_node(node);
531 }
532 return (node) ? pnode : NULL;
533 }
534
535 void *
fdt_parent_node(void * node)536 fdt_parent_node(void *node)
537 {
538 void *pnode = fdt_next_node(0);
539
540 if (!tree_inited)
541 return NULL;
542
543 if (node == pnode)
544 return NULL;
545
546 return fdt_parent_node_recurse(pnode, node);
547 }
548
549 int
fdt_node_is_compatible(void * node,const char * name)550 fdt_node_is_compatible(void *node, const char *name)
551 {
552 char *data;
553 int len;
554
555 len = fdt_node_property(node, "compatible", &data);
556 while (len > 0) {
557 if (strcmp(data, name) == 0)
558 return 1;
559 len -= strlen(data) + 1;
560 data += strlen(data) + 1;
561 }
562
563 return 0;
564 }
565
566 #ifdef DEBUG
567 /*
568 * Debug methods for printing whole tree, particular nodes and properties
569 */
570 void *
fdt_print_property(void * node,int level)571 fdt_print_property(void *node, int level)
572 {
573 uint32_t *ptr;
574 char *tmp, *value;
575 int cnt;
576 uint32_t nameid, size;
577
578 ptr = (uint32_t *)node;
579
580 if (!tree_inited)
581 return NULL;
582
583 if (betoh32(*ptr) != FDT_PROPERTY)
584 return ptr; /* should never happen */
585
586 /* extract property name_id and size */
587 size = betoh32(*++ptr);
588 nameid = betoh32(*++ptr);
589
590 for (cnt = 0; cnt < level; cnt++)
591 printf("\t");
592
593 tmp = fdt_get_str(nameid);
594 printf("\t%s : ", tmp ? tmp : "NO_NAME");
595
596 ptr++;
597 value = (char *)ptr;
598
599 if (!strcmp(tmp, "device_type") || !strcmp(tmp, "compatible") ||
600 !strcmp(tmp, "model") || !strcmp(tmp, "bootargs") ||
601 !strcmp(tmp, "linux,stdout-path")) {
602 printf("%s", value);
603 } else if (!strcmp(tmp, "clock-frequency") ||
604 !strcmp(tmp, "timebase-frequency")) {
605 printf("%d", betoh32(*((unsigned int *)value)));
606 } else {
607 for (cnt = 0; cnt < size; cnt++) {
608 if ((cnt % sizeof(uint32_t)) == 0)
609 printf(" ");
610 printf("%x%x", value[cnt] >> 4, value[cnt] & 0xf);
611 }
612 }
613 ptr += roundup(size, sizeof(uint32_t)) / sizeof(uint32_t);
614 printf("\n");
615
616 return ptr;
617 }
618
619 void
fdt_print_node(void * node,int level)620 fdt_print_node(void *node, int level)
621 {
622 uint32_t *ptr;
623 int cnt;
624
625 ptr = (uint32_t *)node;
626
627 if (betoh32(*ptr) != FDT_NODE_BEGIN)
628 return;
629
630 ptr++;
631
632 for (cnt = 0; cnt < level; cnt++)
633 printf("\t");
634 printf("%s :\n", fdt_node_name(node));
635 ptr = skip_node_name(ptr);
636
637 while (betoh32(*ptr) == FDT_PROPERTY)
638 ptr = fdt_print_property(ptr, level);
639 }
640
641 void
fdt_print_node_recurse(void * node,int level)642 fdt_print_node_recurse(void *node, int level)
643 {
644 void *child;
645
646 fdt_print_node(node, level);
647 for (child = fdt_child_node(node); child; child = fdt_next_node(child))
648 fdt_print_node_recurse(child, level + 1);
649 }
650
651 void
fdt_print_tree(void)652 fdt_print_tree(void)
653 {
654 fdt_print_node_recurse(fdt_next_node(0), 0);
655 }
656 #endif
657