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.skvort...@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
