qemu: Move and generalise FDT PSCI fixup

The QEMU platform port scans its device tree to advertise PSCI as the
CPU enable method. It does this by scanning *every* node in the DT and
check whether its compatible string starts with "arm,cortex-a". Then it
sets the enable-method to PSCI, if it doesn't already have one.

Other platforms might want to use this functionality as well, so let's
move it out of the QEMU platform directory and make it more robust by
fixing some shortcomings:
- A compatible string starting with a certain prefix is not a good way
to find the CPU nodes. For instance a "arm,cortex-a72-pmu" node will
match as well and is in turn favoured with an enable-method.
- If the DT already has an enable-method, we won't change this to PSCI.

Those two issues will for instance fail on the Raspberry Pi 4 DT.
To fix those problems, we adjust the scanning method:
The DT spec says that all CPU nodes are subnodes of the mandatory
/cpus node, which is a subnode of the root node. Also each CPU node has
to have a device_type = "cpu" property. So we find the /cpus node, then
scan for a subnode with the proper device_type, forcing the
enable-method to "psci".
We have to restart this search after a property has been patched, as the
node offsets might have changed meanwhile.

This allows this routine to be reused for the Raspberry Pi 4 later.

Change-Id: I00cae16cc923d9f8bb96a9b2a2933b9a79b06139
Signed-off-by: Andre Przywara <andre.przywara@arm.com>

6 files changed, 142 insertions, 104 deletions

diff --git a/common/fdt_fixup.c b/common/fdt_fixup.c
new file mode 100644
index 00000000..0ae0050c
--- /dev/null
+++ b/common/fdt_fixup.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Contains generic routines to fix up the device tree blob passed on to
+ * payloads like BL32 and BL33 (and further down the boot chain).
+ * This allows to easily add PSCI nodes, when the original DT does not have
+ * it or advertises another method.
+ */
+
+#include <string.h>
+
+#include <libfdt.h>
+
+#include <common/debug.h>
+#include <drivers/console.h>
+#include <lib/psci/psci.h>
+
+#include <common/fdt_fixup.h>
+
+static int append_psci_compatible(void *fdt, int offs, const char *str)
+{
+	return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1);
+}
+
+int dt_add_psci_node(void *fdt)
+{
+	int offs;
+
+	if (fdt_path_offset(fdt, "/psci") >= 0) {
+		WARN("PSCI Device Tree node already exists!\n");
+		return 0;
+	}
+
+	offs = fdt_path_offset(fdt, "/");
+	if (offs < 0)
+		return -1;
+	offs = fdt_add_subnode(fdt, offs, "psci");
+	if (offs < 0)
+		return -1;
+	if (append_psci_compatible(fdt, offs, "arm,psci-1.0"))
+		return -1;
+	if (append_psci_compatible(fdt, offs, "arm,psci-0.2"))
+		return -1;
+	if (append_psci_compatible(fdt, offs, "arm,psci"))
+		return -1;
+	if (fdt_setprop_string(fdt, offs, "method", "smc"))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_AARCH64))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_AARCH64))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "sys_poweroff", PSCI_SYSTEM_OFF))
+		return -1;
+	if (fdt_setprop_u32(fdt, offs, "sys_reset", PSCI_SYSTEM_RESET))
+		return -1;
+	return 0;
+}
+
+/*
+ * Find the first subnode that has a "device_type" property with the value
+ * "cpu" and which's enable-method is not "psci" (yet).
+ * Returns 0 if no such subnode is found, so all have already been patched
+ * or none have to be patched in the first place.
+ * Returns 1 if *one* such subnode has been found and successfully changed
+ * to "psci".
+ * Returns -1 on error.
+ *
+ * Call in a loop until it returns 0. Recalculate the node offset after
+ * it has returned 1.
+ */
+static int dt_update_one_cpu_node(void *fdt, int offset)
+{
+	int offs;
+
+	/* Iterate over all subnodes to find those with device_type = "cpu". */
+	for (offs = fdt_first_subnode(fdt, offset); offs >= 0;
+	     offs = fdt_next_subnode(fdt, offs)) {
+		const char *prop;
+		int len;
+
+		prop = fdt_getprop(fdt, offs, "device_type", &len);
+		if (!prop)
+			continue;
+		if (memcmp(prop, "cpu", 4) != 0 || len != 4)
+			continue;
+
+		/* Ignore any nodes which already use "psci". */
+		prop = fdt_getprop(fdt, offs, "enable-method", &len);
+		if (prop && memcmp(prop, "psci", 5) == 0 && len == 5)
+			continue;
+
+		if (fdt_setprop_string(fdt, offs, "enable-method", "psci"))
+			return -1;
+		/*
+		 * Subnode found and patched.
+		 * Restart to accommodate potentially changed offsets.
+		 */
+		return 1;
+	}
+
+	if (offs == -FDT_ERR_NOTFOUND)
+		return 0;
+
+	return offs;
+}
+
+int dt_add_psci_cpu_enable_methods(void *fdt)
+{
+	int offs, ret;
+
+	do {
+		offs = fdt_path_offset(fdt, "/cpus");
+		if (offs < 0)
+			return offs;
+
+		ret = dt_update_one_cpu_node(fdt, offs);
+	} while (ret > 0);
+
+	return ret;
+}
diff --git a/include/common/fdt_fixup.h b/include/common/fdt_fixup.h
new file mode 100644
index 00000000..bb05bf5d
--- /dev/null
+++ b/include/common/fdt_fixup.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FDT_FIXUP_H
+#define FDT_FIXUP_H
+
+int dt_add_psci_node(void *fdt);
+int dt_add_psci_cpu_enable_methods(void *fdt);
+
+#endif /* FDT_FIXUP_H */
diff --git a/plat/qemu/dt.c b/plat/qemu/dt.c
deleted file mode 100644
index b1cd368c..00000000
--- a/plat/qemu/dt.c
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <string.h>
-
-#include <libfdt.h>
-
-#include <common/debug.h>
-#include <drivers/console.h>
-#include <lib/psci/psci.h>
-
-#include "qemu_private.h"
-
-static int append_psci_compatible(void *fdt, int offs, const char *str)
-{
-	return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1);
-}
-
-int dt_add_psci_node(void *fdt)
-{
-	int offs;
-
-	if (fdt_path_offset(fdt, "/psci") >= 0) {
-		WARN("PSCI Device Tree node already exists!\n");
-		return 0;
-	}
-
-	offs = fdt_path_offset(fdt, "/");
-	if (offs < 0)
-		return -1;
-	offs = fdt_add_subnode(fdt, offs, "psci");
-	if (offs < 0)
-		return -1;
-	if (append_psci_compatible(fdt, offs, "arm,psci-1.0"))
-		return -1;
-	if (append_psci_compatible(fdt, offs, "arm,psci-0.2"))
-		return -1;
-	if (append_psci_compatible(fdt, offs, "arm,psci"))
-		return -1;
-	if (fdt_setprop_string(fdt, offs, "method", "smc"))
-		return -1;
-	if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_AARCH64))
-		return -1;
-	if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF))
-		return -1;
-	if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_AARCH64))
-		return -1;
-	if (fdt_setprop_u32(fdt, offs, "sys_poweroff", PSCI_SYSTEM_OFF))
-		return -1;
-	if (fdt_setprop_u32(fdt, offs, "sys_reset", PSCI_SYSTEM_RESET))
-		return -1;
-	return 0;
-}
-
-static int check_node_compat_prefix(void *fdt, int offs, const char *prefix)
-{
-	const size_t prefix_len = strlen(prefix);
-	size_t l;
-	int plen;
-	const char *prop;
-
-	prop = fdt_getprop(fdt, offs, "compatible", &plen);
-	if (!prop)
-		return -1;
-
-	while (plen > 0) {
-		if (memcmp(prop, prefix, prefix_len) == 0)
-			return 0; /* match */
-
-		l = strlen(prop) + 1;
-		prop += l;
-		plen -= l;
-	}
-
-	return -1;
-}
-
-int dt_add_psci_cpu_enable_methods(void *fdt)
-{
-	int offs = 0;
-
-	while (1) {
-		offs = fdt_next_node(fdt, offs, NULL);
-		if (offs < 0)
-			break;
-		if (fdt_getprop(fdt, offs, "enable-method", NULL))
-			continue; /* already set */
-		if (check_node_compat_prefix(fdt, offs, "arm,cortex-a"))
-			continue; /* no compatible */
-		if (fdt_setprop_string(fdt, offs, "enable-method", "psci"))
-			return -1;
-		/* Need to restart scanning as offsets may have changed */
-		offs = 0;
-	}
-	return 0;
-}
diff --git a/plat/qemu/platform.mk b/plat/qemu/platform.mk
index 6b9749c7..bc4a21bc 100644
--- a/plat/qemu/platform.mk
+++ b/plat/qemu/platform.mk
@@ -114,7 +114,7 @@ BL2_SOURCES		+=	drivers/io/io_semihosting.c		\
 				plat/qemu/qemu_io_storage.c		\
 				plat/qemu/${ARCH}/plat_helpers.S	\
 				plat/qemu/qemu_bl2_setup.c		\
-				plat/qemu/dt.c				\
+				common/fdt_fixup.c			\
 				plat/qemu/qemu_bl2_mem_params_desc.c	\
 				plat/qemu/qemu_image_load.c		\
 				common/desc_image_load.c
diff --git a/plat/qemu/qemu_bl2_setup.c b/plat/qemu/qemu_bl2_setup.c
index 4c97c8dd..166d2454 100644
--- a/plat/qemu/qemu_bl2_setup.c
+++ b/plat/qemu/qemu_bl2_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,7 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
+#include <common/fdt_fixup.h>
 #include <lib/optee_utils.h>
 #include <lib/utils.h>
 #include <plat/common/platform.h>
diff --git a/plat/qemu/qemu_private.h b/plat/qemu/qemu_private.h
index 46b1ca1e..71ea4de1 100644
--- a/plat/qemu/qemu_private.h
+++ b/plat/qemu/qemu_private.h
@@ -28,9 +28,6 @@ void qemu_configure_mmu_el3(unsigned long total_base, unsigned long total_size,
 void plat_qemu_io_setup(void);
 unsigned int plat_qemu_calc_core_pos(u_register_t mpidr);
 
-int dt_add_psci_node(void *fdt);
-int dt_add_psci_cpu_enable_methods(void *fdt);
-
 void qemu_console_init(void);
 
 void plat_qemu_gic_init(void);