freebsd-skq/stand/fdt/fdt_overlay.c

/*-
 * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo@FreeBSD.org>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <sys/cdefs.h>
__FBSDID("$FreeBSD$");

#include <stand.h>
#include <libfdt.h>

#include "fdt_overlay.h"

/*
 * Get max phandle
 */
static uint32_t
fdt_max_phandle(void *fdtp)
{
	int o, depth;
	uint32_t max_phandle, phandle;

	depth = 1;
	o = fdt_path_offset(fdtp, "/");
	max_phandle = fdt_get_phandle(fdtp, o);
	for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(fdtp, o, &depth)) {
		phandle = fdt_get_phandle(fdtp, o);
		if (max_phandle < phandle)
			max_phandle = phandle;
	}

	return max_phandle;
}

/*
 * Returns exact memory location specified by fixup in format
 * /path/to/node:property:offset
 */
static void *
fdt_get_fixup_location(void *fdtp, const char *fixup)
{
	char *path, *prop, *offsetp, *endp;
	int prop_offset, o, proplen;
	void  *result;

	result = NULL;

	path = strdup(fixup);
	prop = strchr(path, ':');
	if (prop == NULL) {
		printf("missing property part in \"%s\"\n", fixup);
		result = NULL;
		goto out;
	}

	*prop = 0;
	prop++;

	offsetp = strchr(prop, ':');
	if (offsetp == NULL) {
		printf("missing offset part in \"%s\"\n", fixup);
		result = NULL;
		goto out;
	}

	*offsetp = 0;
	offsetp++;

	prop_offset = strtoul(offsetp, &endp, 10);
	if (*endp != '\0') {
		printf("\"%s\" is not valid number\n", offsetp);
		result = NULL;
		goto out;
	}

	o = fdt_path_offset(fdtp, path);
	if (o < 0) {
		printf("path \"%s\" not found\n", path);
		result = NULL;
		goto out;
	}

	result = fdt_getprop_w(fdtp, o, prop, &proplen);
	if (result == NULL){
		printf("property \"%s\" not found in  \"%s\" node\n", prop, path);
		result = NULL;
		goto out;
	}

	if (proplen < prop_offset + sizeof(uint32_t)) {
		printf("%s: property length is too small for fixup\n", fixup);
		result = NULL;
		goto out;
	}

	result = (char*)result + prop_offset;

out:
	free(path);
	return (result);
}

/*
 * Process one entry in __fixups__ { } node
 * @fixups is property value, array of NUL-terminated strings
 *   with fixup locations
 * @fixups_len length of the fixups array in bytes
 * @phandle is value for these locations
 */
static int
fdt_do_one_fixup(void *fdtp, const char *fixups, int fixups_len, int phandle)
{
	void *fixup_pos;
	uint32_t val;

	val = cpu_to_fdt32(phandle);

	while (fixups_len > 0) {
		fixup_pos = fdt_get_fixup_location(fdtp, fixups);
		if (fixup_pos != NULL)
			memcpy(fixup_pos, &val, sizeof(val));

		fixups_len -= strlen(fixups) + 1;
		fixups += strlen(fixups) + 1;
	}

	return (0);
}

/*
 * Increase u32 value at pos by offset
 */
static void
fdt_increase_u32(void *pos, uint32_t offset)
{
	uint32_t val;

	memcpy(&val, pos,  sizeof(val));
	val = cpu_to_fdt32(fdt32_to_cpu(val) + offset);
	memcpy(pos, &val, sizeof(val));
}

/*
 * Process local fixups
 * @fixups is property value, array of NUL-terminated strings
 *   with fixup locations
 * @fixups_len length of the fixups array in bytes
 * @offset value these locations should be increased by
 */
static int
fdt_do_local_fixup(void *fdtp, const char *fixups, int fixups_len, int offset)
{
	void *fixup_pos;

	while (fixups_len > 0) {
		fixup_pos = fdt_get_fixup_location(fdtp, fixups);
		if (fixup_pos != NULL)
			fdt_increase_u32(fixup_pos, offset);

		fixups_len -= strlen(fixups) + 1;
		fixups += strlen(fixups) + 1;
	}

	return (0);
}

/*
 * Increase node phandle by phandle_offset
 */
static void
fdt_increase_phandle(void *fdtp, int node_offset, uint32_t phandle_offset)
{
	int proplen;
	void *phandle_pos, *node_pos;

	node_pos = (char*)fdtp + node_offset;

	phandle_pos = fdt_getprop_w(fdtp, node_offset, "phandle", &proplen);
	if (phandle_pos)
		fdt_increase_u32(phandle_pos, phandle_offset);
	phandle_pos = fdt_getprop_w(fdtp, node_offset, "linux,phandle", &proplen);
	if (phandle_pos)
		fdt_increase_u32(phandle_pos, phandle_offset);
}

/*
 * Increase all phandles by offset
 */
static void
fdt_increase_phandles(void *fdtp, uint32_t offset)
{
	int o, depth;

	o = fdt_path_offset(fdtp, "/");
	for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(fdtp, o, &depth)) {
		fdt_increase_phandle(fdtp, o, offset);
	}
}

/*
 * Overlay one node defined by <overlay_fdtp, overlay_o> over <main_fdtp, target_o>
 */
static void
fdt_overlay_node(void *main_fdtp, int target_o, void *overlay_fdtp, int overlay_o)
{
	int len, o, depth;
	const char *name;
	const void *val;
	int target_subnode_o;

	/* Overlay properties */
	for (o = fdt_first_property_offset(overlay_fdtp, overlay_o);
	    o >= 0; o = fdt_next_property_offset(overlay_fdtp, o)) {
		val = fdt_getprop_by_offset(overlay_fdtp, o, &name, &len);
		if (val)
			fdt_setprop(main_fdtp, target_o, name, val, len);
	}

	/* Now overlay nodes */
	o = overlay_o;
        for (depth = 0; (o >= 0) && (depth >= 0);
	    o = fdt_next_node(overlay_fdtp, o, &depth)) {
		if (depth != 1)
			continue;
		/* Check if there is node with the same name */
		name = fdt_get_name(overlay_fdtp, o, NULL);
		target_subnode_o = fdt_subnode_offset(main_fdtp, target_o, name);
		if (target_subnode_o < 0) {
			/* create new subnode and run merge recursively */
			target_subnode_o = fdt_add_subnode(main_fdtp, target_o, name);
			if (target_subnode_o < 0) {
				printf("failed to create subnode \"%s\": %d\n",
				    name, target_subnode_o);
				return;
			}
		}

		fdt_overlay_node(main_fdtp, target_subnode_o,
		    overlay_fdtp, o);
	}
}

/*
 * Apply one overlay fragment
 */
static void
fdt_apply_fragment(void *main_fdtp, void *overlay_fdtp, int fragment_o)
{
	uint32_t target;
	const char *target_path;
	const void *val;
	int target_node_o, overlay_node_o;

	target_node_o = -1;
	val = fdt_getprop(overlay_fdtp, fragment_o, "target", NULL);
	if (val) {
		memcpy(&target, val, sizeof(target));
		target = fdt32_to_cpu(target);
		target_node_o = fdt_node_offset_by_phandle(main_fdtp, target);
		if (target_node_o < 0) {
			printf("failed to find target %04x\n", target);
			return;
		}
	}

	if (target_node_o < 0) {
		target_path = fdt_getprop(overlay_fdtp, fragment_o, "target-path", NULL);
		if (target_path == NULL)
			return;

		target_node_o = fdt_path_offset(main_fdtp, target_path);
		if (target_node_o < 0) {
			printf("failed to find target-path %s\n", target_path);
			return;
		}
	}

	if (target_node_o < 0)
		return;

	overlay_node_o = fdt_subnode_offset(overlay_fdtp, fragment_o, "__overlay__");
	if (overlay_node_o < 0) {
		printf("missing __overlay__ sub-node\n");
		return;
	}

	fdt_overlay_node(main_fdtp, target_node_o, overlay_fdtp, overlay_node_o);
}

/*
 * Handle __fixups__ node in overlay DTB
 */
static int
fdt_overlay_do_fixups(void *main_fdtp, void *overlay_fdtp)
{
	int main_symbols_o, symbol_o, overlay_fixups_o;
	int fixup_prop_o;
	int len;
	const char *fixups, *name;
	const char *symbol_path;
	uint32_t phandle;

	main_symbols_o = fdt_path_offset(main_fdtp, "/__symbols__");
	overlay_fixups_o = fdt_path_offset(overlay_fdtp, "/__fixups__");

	if (main_symbols_o < 0)
		return (-1);
	if (overlay_fixups_o < 0)
		return (-1);

	for (fixup_prop_o = fdt_first_property_offset(overlay_fdtp, overlay_fixups_o);
	    fixup_prop_o >= 0;
	    fixup_prop_o = fdt_next_property_offset(overlay_fdtp, fixup_prop_o)) {
		fixups = fdt_getprop_by_offset(overlay_fdtp, fixup_prop_o, &name, &len);
		symbol_path = fdt_getprop(main_fdtp, main_symbols_o, name, NULL);
		if (symbol_path == NULL) {
			printf("couldn't find \"%s\" symbol in main dtb\n", name);
			return (-1);
		}
		symbol_o = fdt_path_offset(main_fdtp, symbol_path);
		if (symbol_o < 0) {
			printf("couldn't find \"%s\" path in main dtb\n", symbol_path);
			return (-1);
		}
		phandle = fdt_get_phandle(main_fdtp, symbol_o);
		if (fdt_do_one_fixup(overlay_fdtp, fixups, len, phandle) < 0)
			return (-1);
	}

	return (0);
}

/*
 * Handle __local_fixups__ node in overlay DTB
 */
static int
fdt_overlay_do_local_fixups(void *main_fdtp, void *overlay_fdtp)
{
	int overlay_local_fixups_o;
	int len, rv;
	const char *fixups;
	uint32_t phandle_offset;

	overlay_local_fixups_o = fdt_path_offset(overlay_fdtp, "/__local_fixups__");

	if (overlay_local_fixups_o < 0) {
		/* If not local fixups - do nothing */
		if (overlay_local_fixups_o == -FDT_ERR_NOTFOUND)
			return (0);
		return (overlay_local_fixups_o);
	}

	phandle_offset = fdt_max_phandle(main_fdtp);
	fdt_increase_phandles(overlay_fdtp, phandle_offset);
	fixups = fdt_getprop_w(overlay_fdtp, overlay_local_fixups_o, "fixup", &len);
	if (fixups) {
		rv = fdt_do_local_fixup(overlay_fdtp, fixups, len, phandle_offset);
		if (rv < 0)
			return (rv);
	}

	return (0);
}

/*
 * Apply all fragments to main DTB
 */
static int
fdt_overlay_apply_fragments(void *main_fdtp, void *overlay_fdtp)
{
	int o, depth;

	o = fdt_path_offset(overlay_fdtp, "/");
	for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(overlay_fdtp, o, &depth)) {
		if (depth != 1)
			continue;

		fdt_apply_fragment(main_fdtp, overlay_fdtp, o);
	}

	return (0);
}

int
fdt_overlay_apply(void *main_fdtp, void *overlay_fdtp, size_t overlay_length)
{
	void *overlay_copy;
	int rv;

	rv = 0;

	/* We modify overlay in-place, so we need writable copy */
	overlay_copy = malloc(overlay_length);
	if (overlay_copy == NULL) {
		printf("failed to allocate memory for overlay copy\n");
		return (-1);
	}

	memcpy(overlay_copy, overlay_fdtp, overlay_length);

	if (fdt_overlay_do_fixups(main_fdtp, overlay_copy) < 0) {
		printf("failed to perform fixups in overlay\n");
		rv = -1;
		goto out;
	}

	if (fdt_overlay_do_local_fixups(main_fdtp, overlay_copy) < 0) {
		printf("failed to perform local fixups in overlay\n");
		rv = -1;
		goto out;
	}

	if (fdt_overlay_apply_fragments(main_fdtp, overlay_copy) < 0) {
		printf("failed to apply fragments\n");
		rv = -1;
	}

out:
	free(overlay_copy);

	return (rv);
}