MLK-16087 imx8qm/qxp: Disable kernel FDT nodes for the resources are not owned

Before starting the kernel, need to check if the enabled nodes (resources) in FDT
are owned by current partition.  If it is not owned, need to disable it because A core
can't access it.
We use the node's power-domain property to get the PD node which has the SCFW resource
id in its reg property. Then we can check it with SCFW.

Signed-off-by: Ye Li <ye.li@nxp.com>
(cherry picked from downstream commit 358372674b29685788a4007b0944ab03b7fafc13)

1 files changed, 115 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/imx8/cpu.c b/arch/arm/mach-imx/imx8/cpu.c
index 72404d9eb6..7f2dd2166b 100644
--- a/arch/arm/mach-imx/imx8/cpu.c
+++ b/arch/arm/mach-imx/imx8/cpu.c
@@ -11,6 +11,8 @@
 #include <dm/lists.h>
 #include <dm/uclass.h>
 #include <errno.h>
+#include <fdt_support.h>
+#include <fdtdec.h>
 #include <thermal.h>
 #include <asm/arch/sci/sci.h>
 #include <asm/arch/sid.h>
@@ -193,6 +195,119 @@ int mmc_get_env_dev(void)
 }
 #endif
 
+#ifdef CONFIG_OF_SYSTEM_SETUP
+static bool check_owned_resource(sc_rsrc_t rsrc_id)
+{
+	sc_ipc_t ipcHndl = 0;
+	bool owned;
+
+	ipcHndl = -1;
+
+	owned = sc_rm_is_resource_owned(ipcHndl, 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
+	  */
+
+	int offset = 0, next_off, addr;
+	int depth, next_depth;
+	unsigned int rsrc_id;
+	const fdt32_t *php;
+	const char *name;
+	int rc;
+
+	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 (!fdtdec_get_is_enabled(blob, offset)) {
+			debug("   - ignoring disabled device\n");
+			continue;
+		}
+
+		if (!fdt_node_check_compatible(blob, offset, "nxp,imx8-pd")) {
+			/* Skip to next depth=1 node*/
+			next_off = offset;
+			next_depth = depth;
+			do {
+				offset = next_off;
+				depth = next_depth;
+				next_off = fdt_next_node(blob, offset, &next_depth);
+				if (next_off < 0 || next_depth < 1)
+					break;
+
+				debug("PD name: %s, offset %d, depth %d\n",
+					fdt_get_name(blob, next_off, NULL), next_off, next_depth);
+			} while (next_depth > 1);
+
+			continue;
+		}
+
+		php = fdt_getprop(blob, offset, "power-domains", NULL);
+		if (!php) {
+			debug("   - ignoring no power-domains\n");
+		} else {
+			addr = fdt_node_offset_by_phandle(blob, fdt32_to_cpu(*php));
+			rsrc_id = fdtdec_get_uint(blob, addr, "reg", 0);
+
+			if (rsrc_id == SC_R_LAST) {
+				name = fdt_get_name(blob, offset, NULL);
+				printf("%s's power domain use SC_R_LAST\n", name);
+				continue;
+			}
+
+			debug("power-domains phandle 0x%x, addr 0x%x, resource id %u\n",
+				fdt32_to_cpu(*php), addr, rsrc_id);
+
+			if (!check_owned_resource(rsrc_id)) {
+
+				/* If the resource is not owned, disable it in FDT */
+				rc = disable_fdt_node(blob, offset);
+				if (!rc)
+					printf("Disable %s, resource id %u, pd phandle 0x%x\n",
+						fdt_get_name(blob, offset, NULL), rsrc_id, fdt32_to_cpu(*php));
+				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;
+}
+#endif
+
 #define MEMSTART_ALIGNMENT  SZ_2M /* Align the memory start with 2MB */
 
 static int get_owned_memreg(sc_rm_mr_t mr, sc_faddr_t *addr_start,