Skip to content
Snippets Groups Projects
fdt_overlay.c 8.24 KiB
Newer Older
  • Learn to ignore specific revisions
  • #include "libfdt_env.h"
    
    #include <fdt.h>
    #include <libfdt.h>
    
    #include "libfdt_internal.h"
    
    static uint32_t overlay_get_target_phandle(const void *fdto, int fragment)
    {
    	const uint32_t *val;
    	int len;
    
    	val = fdt_getprop(fdto, fragment, "target", &len);
    	if (!val)
    		return 0;
    
    	if ((*val == 0xffffffff) || (len != sizeof(*val)))
    		return 0xffffffff;
    
    	return fdt32_to_cpu(*val);
    }
    
    static int overlay_get_target(const void *fdt, const void *fdto,
    			      int fragment)
    {
    	uint32_t phandle;
    	const char *path;
    
    	/* Try first to do a phandle based lookup */
    	phandle = overlay_get_target_phandle(fdto, fragment);
    	if (phandle == -1)
    		return -FDT_ERR_BADPHANDLE;
    
    	if (phandle)
    		return fdt_node_offset_by_phandle(fdt, phandle);
    
    	/* And then a path based lookup */
    	path = fdt_getprop(fdto, fragment, "target-path", NULL);
    	if (!path)
    		return -FDT_ERR_NOTFOUND;
    
    	return fdt_path_offset(fdt, path);
    }
    
    static int overlay_phandle_add_offset(void *fdt, int node,
    				      const char *name, uint32_t delta)
    {
    	const uint32_t *val;
    	uint32_t adj_val;
    	int len;
    
    	val = fdt_getprop(fdt, node, name, &len);
    	if (!val)
    		return len;
    
    	if (len != sizeof(*val))
    		return -FDT_ERR_BADSTRUCTURE;
    
    	adj_val = fdt32_to_cpu(*val);
    	if ((adj_val + delta) < adj_val)
    		return -FDT_ERR_BADPHANDLE;
    
    	adj_val += delta;
    	return fdt_setprop_inplace_u32(fdt, node, name, adj_val);
    }
    
    static int overlay_adjust_node_phandles(void *fdto, int node,
    					uint32_t delta)
    {
    	bool found = false;
    	int child;
    	int ret;
    
    	ret = overlay_phandle_add_offset(fdto, node, "phandle", delta);
    	if (ret && ret != -FDT_ERR_NOTFOUND)
    		return ret;
    
    	if (!ret)
    		found = true;
    
    	ret = overlay_phandle_add_offset(fdto, node, "linux,phandle", delta);
    	if (ret && ret != -FDT_ERR_NOTFOUND)
    		return ret;
    
    	/*
    	 * If neither phandle nor linux,phandle have been found return
    	 * an error.
    	 */
    	if (!found && !ret)
    		return ret;
    
    	fdt_for_each_subnode(fdto, child, node)
    		overlay_adjust_node_phandles(fdto, child, delta);
    
    	return 0;
    }
    
    static int overlay_adjust_local_phandles(void *fdto, uint32_t delta)
    {
    	/*
    	 * Start adjusting the phandles from the overlay root
    	 */
    	return overlay_adjust_node_phandles(fdto, 0, delta);
    }
    
    static int overlay_update_local_node_references(void *fdto,
    						int tree_node,
    						int fixup_node,
    						uint32_t delta)
    {
    	int fixup_prop;
    	int fixup_child;
    	int ret;
    
    	fdt_for_each_property_offset(fixup_prop, fdto, fixup_node) {
    		const uint32_t *val = NULL;
    		uint32_t adj_val, index;
    		const char *name;
    		int fixup_len;
    		int tree_len;
    
    		val = fdt_getprop_by_offset(fdto, fixup_prop,
    					    &name, &fixup_len);
    		if (!val)
    			return fixup_len;
    		index = fdt32_to_cpu(*val);
    
    		val = fdt_getprop(fdto, tree_node, name, &tree_len);
    		if (!val)
    			return tree_len;
    
    		/*
    		 * The index can be unaligned.
    		 *
    		 * Use a memcpy for the architectures that do not
    		 * support unaligned accesses.
    		 */
    		memcpy(&adj_val, (unsigned char *)val + index,
    		       sizeof(uint32_t));
    
    		adj_val = fdt32_to_cpu(adj_val);
    		adj_val += delta;
    		adj_val = cpu_to_fdt32(adj_val);
    
    
    		ret = fdt_setprop_inplace_namelen_partial(fdto, tree_node,
    							  name, strlen(name),
    							  index, &adj_val,
    							  sizeof(adj_val));
    
    		if (ret)
    			return ret;
    	}
    
    	fdt_for_each_subnode(fdto, fixup_child, fixup_node) {
    		const char *fixup_child_name = fdt_get_name(fdto, fixup_child,
    							    NULL);
    		int tree_child;
    
    		tree_child = fdt_subnode_offset(fdto, tree_node,
    						fixup_child_name);
    		if (tree_child < 0)
    			return tree_child;
    
    		ret = overlay_update_local_node_references(fdto,
    							   tree_child,
    							   fixup_child,
    							   delta);
    		if (ret)
    			return ret;
    	}
    
    	return 0;
    }
    
    static int overlay_update_local_references(void *dto, uint32_t delta)
    {
    	int fixups;
    
    	fixups = fdt_path_offset(dto, "/__local_fixups__");
    	if (fixups < 0) {
    		/* There's no local phandles to adjust, bail out */
    		if (fixups == -FDT_ERR_NOTFOUND)
    			return 0;
    
    		return fixups;
    	}
    
    	/*
    	 * Update our local references from the root of the tree
    	 */
    	return overlay_update_local_node_references(dto, 0, fixups,
    						    delta);
    }
    
    static int overlay_fixup_one_phandle(void *fdt, void *fdto,
    				     int symbols_off,
    				     const char *path, uint32_t path_len,
    				     const char *name, uint32_t name_len,
    				     int index, const char *label)
    {
    	const char *symbol_path;
    	uint32_t phandle;
    	int symbol_off, fixup_off;
    	int prop_len;
    
    	symbol_path = fdt_getprop(fdt, symbols_off, label,
    				  &prop_len);
    	if (!symbol_path)
    		return -FDT_ERR_NOTFOUND;
    
    	symbol_off = fdt_path_offset(fdt, symbol_path);
    	if (symbol_off < 0)
    		return symbol_off;
    
    	phandle = fdt_get_phandle(fdt, symbol_off);
    	if (!phandle)
    		return -FDT_ERR_NOTFOUND;
    
    	fixup_off = fdt_path_offset_namelen(fdto, path, path_len);
    	if (fixup_off < 0)
    		return fixup_off;
    
    	phandle = cpu_to_fdt32(phandle);
    
    	return fdt_setprop_inplace_namelen_partial(fdto, fixup_off,
    						   name, name_len, index,
    						   &phandle, sizeof(phandle));
    
    };
    
    static int overlay_fixup_phandle(void *fdt, void *fdto, int symbols_off,
    				 int property)
    {
    	const char *value;
    	const char *label;
    	int len;
    
    	value = fdt_getprop_by_offset(fdto, property,
    				      &label, &len);
    	if (!value)
    		return len;
    
    	do {
    		const char *prop_string = value;
    		const char *path, *name;
    		uint32_t prop_len = strlen(value);
    		uint32_t path_len, name_len;
    		char *sep, *endptr;
    		int index;
    		int ret;
    
    		path = prop_string;
    		sep = memchr(prop_string, ':', prop_len);
    		if (*sep != ':')
    			return -FDT_ERR_BADSTRUCTURE;
    		path_len = sep - path;
    
    		name = sep + 1;
    		sep = memchr(name, ':', prop_len);
    		if (*sep != ':')
    			return -FDT_ERR_BADSTRUCTURE;
    		name_len = sep - name;
    
    		index = strtoul(sep + 1, &endptr, 10);
    		if ((*endptr != '\0') || (endptr <= (sep + 1)))
    			return -FDT_ERR_BADSTRUCTURE;
    
    		len -= prop_len + 1;
    		value += prop_len + 1;
    
    		ret = overlay_fixup_one_phandle(fdt, fdto, symbols_off,
    						path, path_len, name, name_len,
    						index, label);
    		if (ret)
    			return ret;
    	} while (len > 0);
    
    	return 0;
    }
    
    static int overlay_fixup_phandles(void *dt, void *dto)
    {
    	int fixups_off, symbols_off;
    	int property;
    
    	symbols_off = fdt_path_offset(dt, "/__symbols__");
    	fixups_off = fdt_path_offset(dto, "/__fixups__");
    
    	fdt_for_each_property_offset(property, dto, fixups_off)
    		overlay_fixup_phandle(dt, dto, symbols_off, property);
    
    	return 0;
    }
    
    static int apply_overlay_node(void *dt, int target,
    			      void *dto, int fragment)
    {
    	int property;
    	int node;
    
    	fdt_for_each_property_offset(property, dto, fragment) {
    		const char *name;
    		const void *prop;
    		int prop_len;
    		int ret;
    
    		prop = fdt_getprop_by_offset(dto, property, &name,
    					     &prop_len);
    		if (prop_len == -FDT_ERR_NOTFOUND)
    			return -FDT_ERR_INTERNAL;
    		if (prop_len < 0)
    			return prop_len;
    
    		ret = fdt_setprop(dt, target, name, prop, prop_len);
    		if (ret)
    			return ret;
    	}
    
    	fdt_for_each_subnode(dto, node, fragment) {
    		const char *name = fdt_get_name(dto, node, NULL);
    		int nnode;
    		int ret;
    
    		nnode = fdt_add_subnode(dt, target, name);
    		if (nnode == -FDT_ERR_EXISTS)
    			nnode = fdt_subnode_offset(dt, target, name);
    
    		if (nnode < 0)
    			return nnode;
    
    		ret = apply_overlay_node(dt, nnode, dto, node);
    		if (ret)
    			return ret;
    	}
    
    	return 0;
    }
    
    static int overlay_merge(void *dt, void *dto)
    {
    	int fragment;
    
    	fdt_for_each_subnode(dto, fragment, 0) {
    		int overlay;
    		int target;
    		int ret;
    
    		target = overlay_get_target(dt, dto, fragment);
    		if (target < 0)
    			continue;
    
    		overlay = fdt_subnode_offset(dto, fragment, "__overlay__");
    		if (overlay < 0)
    			return overlay;
    
    		ret = apply_overlay_node(dt, target, dto, overlay);
    		if (ret)
    			return ret;
    	}
    
    	return 0;
    }
    
    int fdt_overlay_apply(void *fdt, void *fdto)
    {
    	uint32_t delta = fdt_get_max_phandle(fdt) + 1;
    	int ret;
    
    	FDT_CHECK_HEADER(fdt);
    	FDT_CHECK_HEADER(fdto);
    
    	ret = overlay_adjust_local_phandles(fdto, delta);
    	if (ret)
    		goto err;
    
    	ret = overlay_update_local_references(fdto, delta);
    	if (ret)
    		goto err;
    
    	ret = overlay_fixup_phandles(fdt, fdto);
    	if (ret)
    		goto err;
    
    	ret = overlay_merge(fdt, fdto);
    	if (ret)
    		goto err;
    
    	/*
    	 * The overlay has been damaged, erase its magic.
    	 */
    	fdt_set_magic(fdto, ~0);
    
    	return 0;
    
    err:
    	/*
    	 * The overlay might have been damaged, erase its magic.
    	 */
    	fdt_set_magic(fdto, ~0);
    
    	/*
    	 * The base device tree might have been damaged, erase its
    	 * magic.
    	 */
    	fdt_set_magic(fdt, ~0);
    
    	return ret;
    }