From 9f779fa4105f463920e5a857d775672fd0890c41 Mon Sep 17 00:00:00 2001
From: Peng Fan <peng.fan@nxp.com>
Date: Mon, 26 Aug 2019 08:11:56 +0000
Subject: [PATCH] imx8: disable node when the resource is not owned

When resource is not assigned to non-secure Linux, if linux continue
to use the node, linux may crash or hang. So need to set the node
status to disabled for not owned resources.

The resource id is in the power-domains property in device tree,
so parse the power-domains property to get the resource id and
use scfw api to check whether it is owned by current partition.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
---
 arch/arm/mach-imx/imx8/Makefile |   1 +
 arch/arm/mach-imx/imx8/fdt.c    | 108 ++++++++++++++++++++++++++++++++
 2 files changed, 109 insertions(+)
 create mode 100644 arch/arm/mach-imx/imx8/fdt.c

diff --git a/arch/arm/mach-imx/imx8/Makefile b/arch/arm/mach-imx/imx8/Makefile
index 5f62b2826e..7fc34762a2 100644
--- a/arch/arm/mach-imx/imx8/Makefile
+++ b/arch/arm/mach-imx/imx8/Makefile
@@ -5,6 +5,7 @@
 #
 
 obj-y += cpu.o iomux.o misc.o
+obj-$(CONFIG_OF_SYSTEM_SETUP) += fdt.o
 
 ifdef CONFIG_SPL_BUILD
 obj-$(CONFIG_SPL_LOAD_IMX_CONTAINER) += parse-container.o
diff --git a/arch/arm/mach-imx/imx8/fdt.c b/arch/arm/mach-imx/imx8/fdt.c
new file mode 100644
index 0000000000..d20cb6973a
--- /dev/null
+++ b/arch/arm/mach-imx/imx8/fdt.c
@@ -0,0 +1,108 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2019 NXP
+ */
+
+#include <common.h>
+#include <asm/arch/sci/sci.h>
+#include <dm/ofnode.h>
+#include <fdt_support.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static bool check_owned_resource(sc_rsrc_t rsrc_id)
+{
+	bool owned;
+
+	owned = sc_rm_is_resource_owned(-1, rsrc_id);
+
+	return owned;
+}
+
+static int disable_fdt_node(void *blob, int nodeoffset)
+{
+	int rc, ret;
+	const char *status = "disabled";
+
+	do {
+		rc = fdt_setprop(blob, nodeoffset, "status", status,
+				 strlen(status) + 1);
+		if (rc) {
+			if (rc == -FDT_ERR_NOSPACE) {
+				ret = fdt_increase_size(blob, 512);
+				if (ret)
+					return ret;
+			}
+		}
+	} while (rc == -FDT_ERR_NOSPACE);
+
+	return rc;
+}
+
+static void update_fdt_with_owned_resources(void *blob)
+{
+	/*
+	 * Traverses the fdt nodes, check its power domain and use
+	 * the resource id in the power domain for checking whether
+	 * it is owned by current partition
+	 */
+	struct fdtdec_phandle_args args;
+	int offset = 0, depth = 0;
+	u32 rsrc_id;
+	int rc, i;
+
+	for (offset = fdt_next_node(blob, offset, &depth); offset > 0;
+	     offset = fdt_next_node(blob, offset, &depth)) {
+		debug("Node name: %s, depth %d\n",
+		      fdt_get_name(blob, offset, NULL), depth);
+
+		if (!fdt_get_property(blob, offset, "power-domains", NULL)) {
+			debug("   - ignoring node %s\n",
+			      fdt_get_name(blob, offset, NULL));
+			continue;
+		}
+
+		if (!fdtdec_get_is_enabled(blob, offset)) {
+			debug("   - ignoring node %s\n",
+			      fdt_get_name(blob, offset, NULL));
+			continue;
+		}
+
+		i = 0;
+		while (true) {
+			rc = fdtdec_parse_phandle_with_args(blob, offset,
+							    "power-domains",
+							    "#power-domain-cells",
+							    0, i++, &args);
+			if (rc == -ENOENT) {
+				break;
+			} else if (rc) {
+				printf("Parse power-domains of %s wrong: %d\n",
+				       fdt_get_name(blob, offset, NULL), rc);
+				continue;
+			}
+
+			rsrc_id = args.args[0];
+
+			if (!check_owned_resource(rsrc_id)) {
+				rc = disable_fdt_node(blob, offset);
+				if (!rc) {
+					printf("Disable %s rsrc %u not owned\n",
+					       fdt_get_name(blob, offset, NULL),
+					       rsrc_id);
+				} else {
+					printf("Unable to disable %s, err=%s\n",
+					       fdt_get_name(blob, offset, NULL),
+					       fdt_strerror(rc));
+				}
+			}
+		}
+	}
+}
+
+int ft_system_setup(void *blob, bd_t *bd)
+{
+	update_fdt_with_owned_resources(blob);
+
+	return 0;
+}
-- 
2.39.5