common/dtb_patch.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

/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * dtb_patch.c - rewrite cpu-release-addr values in a flattened
 * devicetree, in place, no libfdt, no structural change. the walk
 * follows the devicetree specification structure, FDT_BEGIN_NODE
 * then name then properties then children then FDT_END_NODE, all
 * tokens and lengths big endian, everything 4 byte aligned.
 *
 * The bootloader owns the spin gates, the dtb names them, this
 * writes the real addresses over the build time placeholders.
 * The enable-method conversion and the placeholder properties are
 * done at build time on the host, a firmware dtb is a fixed blob,
 * only the gate addresses depend on where the image actually landed.
 *
 * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org>
 */

#include <string.h>
#include <endian.h>
#include <dtb_patch.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 void put_be64(void *p, uint64_t v)
{
	uint8_t *b = p;
	b[0] = (uint8_t)(v >> 56);
	b[1] = (uint8_t)(v >> 48);
	b[2] = (uint8_t)(v >> 40);
	b[3] = (uint8_t)(v >> 32);
	b[4] = (uint8_t)(v >> 24);
	b[5] = (uint8_t)(v >> 16);
	b[6] = (uint8_t)(v >> 8);
	b[7] = (uint8_t)v;
}

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

/*
 * walk and rewrite. returns the number of cpu-release-addr values
 * written, negative on a malformed blob.
 */
int tb_dtb_patch_spin_table(uintptr_t dtb, uintptr_t *gates, int ngates)
{
	uint8_t *base = (uint8_t *)dtb;
	uint32_t off_struct = be32(base + 8);
	uint32_t off_strings = be32(base + 12);
	uint8_t *p = base + off_struct;
	uint8_t *strings = base + off_strings;
	const char *cur_cpu = NULL;
	int in_cpus = 0;
	int written = 0;
	int depth = 0;

	if (be32(base) != 0xd00dfeed)
		return -1;

	while (p < base + be32(base + 4)) {
		uint32_t token = be32(p);

		switch (token) {
		case FDT_BEGIN_NODE: {
			char *name = (char *)(p + 4);
			size_t len = strlen(name) + 1;

			p += 4 + ((len + 3) & ~3);
			depth++;

			if (depth == 2 && name_eq(name, "cpus")) {
				in_cpus = 1;
			} else if (depth == 2) {
				in_cpus = 0;
			} else if (in_cpus && depth == 3) {
				cur_cpu = name;
			}
			break;
		}
		case FDT_END_NODE:
			depth--;
			p += 4;
			break;
		case FDT_PROP: {
			uint32_t plen = be32(p + 4);
			const char *pname = (char *)strings + be32(p + 8);
			uint8_t *val = p + 12;

			p += 12 + ((plen + 3) & ~3);

			if (in_cpus && depth == 3 &&
			    strcmp(pname, "cpu-release-addr") == 0 &&
			    plen == 8 && cur_cpu) {
				long idx = -1;
				const char *at = strchr(cur_cpu, '@');

				if (at) {
					idx = 0;
					while (*at >= '0' && *at <= '9') {
						at++;
					}
					at = strchr(cur_cpu, '@') + 1;
					while (*at >= '0' && *at <= '9') {
						idx = idx * 10 + (*at - '0');
						at++;
					}
				}
				if (idx >= 0 && idx < ngates) {
					put_be64(val, (uint64_t)gates[idx]);
					written++;
				}
			}
			break;
		}
		case FDT_NOP:
			p += 4;
			break;
		case FDT_END:
			return written;
		}
	}

	return written;
}