mtk-lk/lib/libfdt/fdt_overlay.c

/*-
 * Copyright (c) 2015 Oleksandr Tymoshenko <gonzo@FreeBSD.org>
 * All rights reserved.
 *
 * This software was developed by Semihalf under sponsorship from
 * the FreeBSD Foundation.
 *
 * 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 "fdt_overlay.h"

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

// CJP #include <stand.h>
#include <stdio.h>
#include <stdlib.h>             // for strtoul()


// CJP from BSD/sys/boot/common/commands.c
char *command_errmsg;
char command_errbuf[256];
#ifdef DEBUG
#define debugf(fmt, args...) do { printf("%s(): ", __func__);   \
    printf(fmt,##args); } while (0)
#else
#define debugf(fmt, args...)
#endif



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

	// TODO: Keep track of substring lengths so we don't have to malloc
	// a copy and split it up.
	path = strdup(fixup);
	if (!path)
		return NULL;

	prop = strchr(path, ':');
	if (prop == NULL) {
		printf("missing property part in \"%s\"\n", path);
		free(path);
		return NULL;
	}

	*prop = 0;
	prop++;

	offsetp = strchr(prop, ':');
	if (offsetp == NULL) {
		printf("missing offset part in \"%s\"\n", path);
		free(path);
		return NULL;
	}

	*offsetp = 0;
	offsetp++;

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

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

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

	if (proplen < prop_offset + (int)sizeof(uint32_t)) {
		printf("%s: property length is too small for fixup\n", path);
		free(path);
		return NULL;
	}

	return (char*)prop_w + prop_offset;
}

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

	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 deviced 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) {
			int ret;
			ret = fdt_setprop(main_fdtp, target_o, name, val, len);
			if (ret != 0)
				return;
		}
	}

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

	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) {
		printf("Bad main_symbols in fodf\n");
		return -1;
	}
// Jim
#if 1
	if (overlay_fixups_o < 0) {
		printf("Bad overlay_fixups in fodf\n");
		return -1;
	}
#endif

	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) {
			printf("Failed one fixup in fodf\n");
			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;
	const char *fixups;
	uint32_t phandle_offset;

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

	if (overlay_local_fixups_o < 0) {
		return 0;
	}

	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) {
		if (fdt_do_local_fixup(overlay_fdtp, fixups, len, phandle_offset) < 0) {
			printf("Failed a local fixup in fodlf\n");
			return -1;
		}
	}

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

static int
fdt_overlay_apply(void *main_fdtp, void *overlay_fdtp, size_t overlay_length)
{
	if (overlay_length < sizeof(struct fdt_header)) {
		printf("Overlay_length %zu smaller than header size %zu\n",
		       overlay_length, sizeof(struct fdt_header));
		return -1;
	}

	if (fdt_overlay_do_fixups(main_fdtp, overlay_fdtp) < 0) {
		printf("failed to perform fixups in overlay\n");
		return -1;
	}

	if (fdt_overlay_do_local_fixups(main_fdtp, overlay_fdtp) < 0) {
		printf("failed to perform local fixups in overlay\n");
		return -1;
	}

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

	return 0;
}


// From fdt_loader_cmd.c

struct fdt_header *
fdt_install_blob(void *blob, size_t blob_size)
{
	struct fdt_header *pHeader;
	int err;
	struct fdt_header *fdtp = NULL;

	debugf("fdt_load_dtb(0x%08x)\n", (unsigned int)blob);

	if (blob_size < sizeof(struct fdt_header)) {
		printf("Blob_size %zu smaller than header size %zu\n",
		       blob_size, sizeof(struct fdt_header));
		return NULL;
	}

	pHeader = (struct fdt_header *)blob;
	err = fdt_check_header(pHeader);
	if (err < 0) {
		if (err == -FDT_ERR_BADVERSION) {
			if (fdtp)
				snprintf(command_errbuf, sizeof(command_errbuf),
			        "incompatible blob version: %d, should be: %d",
			        fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION);

		} else {
			snprintf(command_errbuf, sizeof(command_errbuf), "error validating blob: %s",
			        fdt_strerror(err));
		}
		return NULL;
	}

	return pHeader;
}


// From Google, based on fdt_overlay_apply() logic
// Will malloc a new fdt blob and return it. Will not free parameters.
int apply_overlay(struct fdt_header *fdtp, size_t fdt_size, void *overlay, size_t overlay_size, struct fdt_header *new_fdtp)
{
	size_t new_fdtp_size;
	int rv;
	int ret;

	if (fdtp == NULL) {
		return -1;
	}

	if (overlay_size < 8 || overlay_size != fdt_totalsize(overlay)) {
		printf("Bad overlay size!\n");
		//exit(1);
		return -1;
	}
	if (fdt_size < 8 || fdt_size != fdt_totalsize(fdtp)) {
		printf("Bad fdt size!\n");
		//exit(1);
		return -1;
	}

	new_fdtp_size = fdt_totalsize(fdtp) + overlay_size;
	/* It's actually more than enough */
	rv = fdt_open_into(fdtp, new_fdtp, new_fdtp_size);
	if (rv != 0) {
		printf("failed to open DTB blob for applying overlays\n");
		return -1;
	}

	ret = fdt_overlay_apply(new_fdtp, overlay, overlay_size);
	return ret;
}