1 files changed, 95 insertions, 0 deletions
diff --git a/plat/qemu/dt.c b/plat/qemu/dt.c
new file mode 100644
index 00000000..c544d9f4
--- /dev/null
+++ b/plat/qemu/dt.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <console.h>
+#include <debug.h>
+#include <libfdt.h>
+#include <psci.h>
+#include <string.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;
+}