blob: 482359b8a93b43273c758a0fbcc283f792c3878e (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
|
#include <early/dtb.h>
#include <sys/mem.h>
struct dt_node *dt_parse(void *fdt) {
struct fdt_header *hdr = fdt;
if (hdr->magic != FDT_MAGIC) {
return NULL;
}
uint32_t *p = (uint32_t *)(fdt + btohi(hdr->off_dt_struct));
char *strings = fdt + btohi(hdr->off_dt_strings);
struct dt_node *root = NULL;
struct dt_node *stack[DTB_MAX_NODES];
int sp = 0;
while (1) {
uint32_t token = btohi(*p++);
switch (token) {
case FDT_BEGIN_NODE: {
char *name = (char *)p;
p += (strlen(name) + 3) / 4;
struct dt_node *node = dt_node_alloc(name);
if (sp == 0) {
root = node;
} else {
node->next = stack[sp-1]->children;
stack[sp-1]->children = node;
}
stack[sp++] = node;
break;
}
case FDT_PROP: {
uint32_t len = btohi(*p++);
uint32_t nameoff = btohi(*p++);
void *value = p;
p += (len + 3) / 4;
struct dt_prop *prop = dt_prop_alloc(strings + nameoff, value, len);
prop->next = stack[sp-1]->props;
stack[sp-1]->props = prop;
break;
}
case FDT_END_NODE:
sp--;
break;
case FDT_END:
return root;
}
}
}
struct dt_node *dt_find_device(struct dt_node *head, const char *name) {
struct dt_node *child = head->children;
while (child) {
struct dt_node *result = dt_find_device(child, name);
if (result) {
return result;
} else {
child = child->next;
}
}
struct dt_prop *props = head->props;
while (props) {
if (strcmp(props->name, "device_type") == 0 &&
strcmp(props->value, name) == 0) {
return head;
}
}
return NULL;
}
void *dt_get_prop(struct dt_node *node, char *name, size_t *len) {
for (struct dt_prop *p = node->props; p; p = p->next) {
if (strcmp(p->name, name) == 0) {
if (len) *len = p->len;
return p->value;
}
}
return NULL;
}
struct dt_node *dt_node_alloc(char *name) {
struct dt_node *node = kemalloc(sizeof(struct dt_node));
node->name = name;
node->props = NULL;
node->children = NULL;
node->next = NULL;
return node;
}
struct dt_prop *dt_prop_alloc(char *name, void *value, size_t len) {
struct dt_prop *prop = kemalloc(sizeof(struct dt_prop));
prop->name = name;
prop->value = value;
prop->len = len;
prop->next = NULL;
return prop;
}
|
