common/dtb_find.c


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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178

/*
 * dtb_find.c - locate nodes and read reg by walking the flat
 * devicetree. the machine tells the firmware where its devices
 * live, a bootloader that hardcodes the gic address breaks on
 * the first board with a different map.
 *
 * the walk is the standard token scan, FDT_BEGIN_NODE with a
 * matching name at any depth, then the reg property inside,
 * the first address/size pair decoded per the parent's cell
 * counts, which the root carries in #address-cells and
 * #size-cells.
 *
 * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
 */

#include <string.h>
#include <stdint.h>
#include <boot.h>

#define FDT_BEGIN_NODE	1
#define FDT_END_NODE	2
#define FDT_PROP	3
#define FDT_NOP		4
#define FDT_END		9

static uint32_t be32(const void *p)
{
	const uint8_t *b = p;

	return ((uint32_t)b[0] << 24) | ((uint32_t)b[1] << 16) |
	       ((uint32_t)b[2] << 8) | (uint32_t)b[3];
}

static int name_eq(const char *a, const char *b)
{
	while (*a && *a != '@') {
		if (*a != *b)
			return 0;
		a++;
		b++;
	}
	return *b == '\0' || *b == '@';
}

/*
 * find the first node whose name matches, at any depth. returns
 * the offset of its FDT_BEGIN_NODE token or 0 when absent.
 */
static uint32_t fdt_find_node(uintptr_t dtb, const char *name)
{
	uint8_t *basep = (uint8_t *)dtb;
	uint32_t off_struct = be32(basep + 8);
	uint32_t totalsize = be32(basep + 4);
	uint8_t *p = basep + off_struct;

	if (be32(basep) != 0xd00dfeed)
		return 0;

	while (p < basep + totalsize) {
		uint32_t token = be32(p);

		if (token == FDT_BEGIN_NODE) {
			char *n = (char *)(p + 4);
			size_t nlen = strlen(n) + 1;

			if (name_eq(n, name))
				return (uint32_t)(p - basep);
			p += 4 + ((nlen + 3) & ~3);
		} else if (token == FDT_PROP) {
			uint32_t plen = be32(p + 4);

			p += 12 + ((plen + 3) & ~3);
		} else if (token == FDT_END_NODE ||
			   token == FDT_NOP) {
			p += 4;
		} else if (token == FDT_END) {
			break;
		} else {
			return 0;
		}
	}

	return 0;
}

/*
 * read the first reg pair of a node at the given token offset,
 * honoring the root cell counts. pairs of 2 or 4 cells are the
 * ones machines carry, anything else fails. the caller reads
 * more pairs off the returned cursor if it needs them.
 */
int tb_dtb_reg0(uintptr_t dtb, uint32_t node_off, uintptr_t *addr,
		size_t *size)
{
	uint8_t *basep = (uint8_t *)dtb;
	uint32_t off_strings = be32(basep + 12);
	uint8_t *p = basep + node_off;
	uint32_t totalsize = be32(basep + 4);
	uint32_t ac = 2;
	uint32_t sc = 2;
	/*
	 * zero, the node's own FDT_BEGIN_NODE below brings it to
	 * one and the props inside sit at depth one. starting at
	 * one instead skips every prop in the node.
	 */
	int depth_open = 0;

	while (p < basep + totalsize) {
		uint32_t token = be32(p);

		if (token == FDT_BEGIN_NODE) {
			char *n = (char *)(p + 4);
			size_t nlen = strlen(n) + 1;

			depth_open++;
			p += 4 + ((nlen + 3) & ~3);
		} else if (token == FDT_END_NODE) {
			depth_open--;
			if (!depth_open)
				return -1;
			p += 4;
		} else if (token == FDT_PROP) {
			uint32_t plen = be32(p + 4);
			const char *pname =
				(char *)basep + off_strings + be32(p + 8);
			uint8_t *val = p + 12;

			if (depth_open == 1 &&
			    strcmp(pname, "#address-cells") == 0)
				ac = be32(val);
			if (depth_open == 1 &&
			    strcmp(pname, "#size-cells") == 0)
				sc = be32(val);
			if (depth_open == 1 && strcmp(pname, "reg") == 0) {
				if (plen >= (ac + sc) * 4) {
					uint64_t a = 0;
					uint64_t s = 0;

					for (uint32_t i = 0; i < ac; i++)
						a = (a << 32) |
						    be32(val + i * 4);
					for (uint32_t i = 0; i < sc; i++)
						s = (s << 32) |
						    be32(val + (ac + i) * 4);
					*addr = (uintptr_t)a;
					if (size)
						*size = (size_t)s;
					return 0;
				}
				return -1;
			}
			p += 12 + ((plen + 3) & ~3);
		} else if (token == FDT_NOP) {
			p += 4;
		} else if (token == FDT_END) {
			return -1;
		} else {
			return -1;
		}
	}

	return -1;
}

/*
 * the whole lookup in one call: find the node, read its first
 * reg pair.
 */
int tb_dtb_find_reg0(uintptr_t dtb, const char *name, uintptr_t *addr,
		     size_t *size)
{
	uint32_t off = fdt_find_node(dtb, name);

	if (!off)
		return -1;

	return tb_dtb_reg0(dtb, off, addr, size);
}