path: root/common/dtb_patch.c

/* 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_be32(void *p, uint32_t v)
{
	uint8_t *b = p;
	b[0] = (uint8_t)(v >> 24);
	b[1] = (uint8_t)(v >> 16);
	b[2] = (uint8_t)(v >> 8);
	b[3] = (uint8_t)v;
}

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';
}

/*
 * rewrite /memory reg with the RAM the bootloader actually sees.
 * the value is two u32 cells, base and size, addresses above 4GB
 * need the parent #address-cells respected, virt is below 4GB and
 * 2 cells for size. returns 0 on success.
 */
int tb_dtb_patch_memory(uintptr_t dtb, uint64_t base, uint64_t size)
{
	uint8_t *basep = (uint8_t *)dtb;
	uint32_t off_struct = be32(basep + 8);
	uint32_t off_strings = be32(basep + 12);
	uint8_t *p = basep + off_struct;
	uint8_t *strings = basep + off_strings;
	const char *cur_node = NULL;
	int depth = 0;

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

	while (p < basep + be32(basep + 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++;
			cur_node = 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 (depth == 2 && name_eq(cur_node, "memory") &&
			    strcmp(pname, "reg") == 0 && plen >= 16) {
				/*
				 * #address-cells 2, #size-cells 2, the
				 * reg is four cells, base hi lo and
				 * size hi lo, below 4GB the hi cells
				 * are zero.
				 */
				put_be32(val, (uint32_t)(base >> 32));
				put_be32(val + 4, (uint32_t)base);
				put_be32(val + 8, (uint32_t)(size >> 32));
				put_be32(val + 12, (uint32_t)size);
				return 0;
			}
			break;
		}
		case FDT_NOP:
			p += 4;
			break;
		case FDT_END:
			return -2;
		}
	}

	return -3;
}

/*
 * 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;
}

/*
 * tell the kernel where the initrd landed. /chosen is created by
 * the machine firmware, the two cells exist when an initrd was
 * already staged, we overwrite them in place. depth 2 under the
 * root, node name "chosen".
 */
int tb_dtb_patch_initrd(uintptr_t dtb, uint64_t start, uint64_t end)
{
	uint8_t *basep = (uint8_t *)dtb;
	uint32_t off_struct = be32(basep + 8);
	uint32_t off_strings = be32(basep + 12);
	uint8_t *p = basep + off_struct;
	uint8_t *strings = basep + off_strings;
	const char *cur_node = NULL;
	int depth = 0;

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

	while (p < basep + be32(basep + 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++;
			cur_node = 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 (depth == 2 && name_eq(cur_node, "chosen") &&
			    strcmp(pname, "linux,initrd-start") == 0 &&
			    plen >= 8) {
				put_be64(val, start);
			}
			if (depth == 2 && name_eq(cur_node, "chosen") &&
			    strcmp(pname, "linux,initrd-end") == 0 &&
			    plen >= 8) {
				put_be64(val, end);
				return 0;
			}
			break;
		}
		case FDT_NOP:
			p += 4;
			break;
		case FDT_END:
			return -2;
		}
	}

	return -2;
}