[PATCH] sunxi: board: provide CPU idle states to loaded OS

[Andrey Skvortsov]

When using SCPI as the PSCI backend, firmware can wake up the CPUs and
cluster from sleep, so CPU idle states are available for loaded OS to
use. TF-A modifies DTB to advertise available CPU idle states, when
SCPI is detected. This change copies nodes added by TF-A to any new
dtb that is used for loaded OS.

Signed-off-by: Andrey Skvortsov <andrej.skvortzov at gmail.com>
---
 board/sunxi/board.c | 120 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 120 insertions(+)

diff --git a/board/sunxi/board.c b/board/sunxi/board.c
index f321cd58a6..e88bd11a99 100644
--- a/board/sunxi/board.c
+++ b/board/sunxi/board.c
@@ -870,6 +870,125 @@ int board_late_init(void)
 	return 0;
 }
 
+static int cpuidle_dt_fixup_copy_node(ofnode src, int dst_offset, void *dst_blob,
+				      u32 *count, u32 *phandle)
+{
+	int offs, len, ret;
+	struct ofprop prop;
+	ofnode subnode;
+
+	ofnode_for_each_prop(prop, src) {
+		const char *name;
+		const char *val;
+
+		val = ofprop_get_property(&prop, &name, &len);
+		if (!val)
+			return -EINVAL;
+		if (!strcmp(name, "phandle")) {
+			if (ofnode_device_is_compatible(src, "arm,idle-state")) {
+				fdt_setprop_u32(dst_blob, dst_offset, name, *phandle);
+				(*phandle)++;
+				(*count)++;
+			} else {
+				log_err("Unexpected phandle node: %s\n", name);
+				return -EINVAL;
+			}
+		} else {
+			ret = fdt_setprop(dst_blob, dst_offset, name, val, len);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	ofnode_for_each_subnode(subnode, src) {
+		const char *name = ofnode_get_name(subnode);
+
+		if (!name)
+			return -EINVAL;
+		offs = fdt_add_subnode(dst_blob, dst_offset, name);
+		if (offs < 0)
+			return offs;
+		ret = cpuidle_dt_fixup_copy_node(subnode, offs, dst_blob, count, phandle);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int cpuidle_dt_fixup(void *blob)
+{
+	ofnode idle_states_node;
+	ofnode subnode;
+	u32 count, phandle;
+	const struct property *prop;
+	int offs, len, ret;
+
+	idle_states_node = ofnode_path("/cpus/idle-states");
+	if (!ofnode_valid(idle_states_node)) {
+		log_err("No idle-states node in old fdt, nothing to do");
+		return 0;
+	}
+
+	/*
+	 * Do not proceed if the target dt already has an idle-states node.
+	 * In this case assume that the system knows better somehow,
+	 * so do not interfere.
+	 */
+	if (fdt_path_offset(blob, "/cpus/idle-states") >= 0) {
+		log_err("idle-states node already exists in target");
+		return 0;
+	}
+
+	offs = fdt_path_offset(blob, "/cpus");
+	if (offs < 0)
+		return offs;
+
+	offs = fdt_add_subnode(blob, offs, "idle-states");
+	if (offs < 0)
+		return offs;
+
+	/* copy "/cpus/idle-states" node to destination fdt */
+	ret = fdt_find_max_phandle(blob, &phandle);
+	if (ret < 0)
+		return ret;
+	phandle++;
+	count = 0;
+	ret =  cpuidle_dt_fixup_copy_node(idle_states_node, offs, blob, &count, &phandle);
+	if (ret < 0)
+		return ret;
+
+	/* copy "cpu-idle-state" property for all cpus */
+	ofnode_for_each_subnode(subnode, ofnode_path("/cpus")) {
+		char path[32];
+		fdt32_t *value;
+
+		prop = ofnode_get_property(subnode, "cpu-idle-states", &len);
+		if (!prop)
+			continue;
+
+		/* find the same node in a new device-tree */
+		ret = ofnode_get_path(subnode, path, sizeof(path));
+		if (ret)
+			return ret;
+
+		offs = fdt_path_offset(blob, path);
+		if (offs < 0)
+			return offs;
+
+		/* Allocate space for the list of phandles. */
+		ret = fdt_setprop_placeholder(blob, offs, "cpu-idle-states",
+					      count * sizeof(phandle),
+					      (void **)&value);
+		if (ret < 0)
+			return ret;
+
+		/* Fill in the phandles of the idle state nodes. */
+		for (u32 i = 0U; i < count; ++i)
+			value[i] = cpu_to_fdt32(phandle - count + i);
+	}
+	return 0;
+}
+
 static void bluetooth_dt_fixup(void *blob)
 {
 	/* Some devices ship with a Bluetooth controller default address.
@@ -914,6 +1033,7 @@ int ft_board_setup(void *blob, struct bd_info *bd)
 	setup_environment(blob);
 	fdt_fixup_ethernet(blob);
 
+	cpuidle_dt_fixup(blob);
 	bluetooth_dt_fixup(blob);
 
 #ifdef CONFIG_VIDEO_DT_SIMPLEFB
-- 
2.40.1