MLK-19526-2 imx8mq: Update kernel DTB for iMX8MD and iMX8MQLite

Since VPU/DCSS/HDMI are disabled on iMX8QLite, the CPU core 2/3 are disabled
on iMX8MD, we have to update kernel DTB to disable relevant nodes. The MIPI-DSI
can input from DCSS or LCDIF, so we need to check the input in DTB and only

Signed-off-by: Ye Li <ye.li@nxp.com>

1 files changed, 149 insertions, 30 deletions

diff --git a/arch/arm/mach-imx/imx8m/soc.c b/arch/arm/mach-imx/imx8m/soc.c
index 3e125630e8..0b3ac1a168 100644
--- a/arch/arm/mach-imx/imx8m/soc.c
+++ b/arch/arm/mach-imx/imx8m/soc.c
@@ -17,6 +17,7 @@
 #include <asm/armv8/mmu.h>
 #include <errno.h>
 #include <fdt_support.h>
+#include <fdtdec.h>
 #include <fsl_wdog.h>
 #include <imx_sip.h>
 #include <generated/version_autogenerated.h>
@@ -303,46 +304,154 @@ static int ft_add_optee_node(void *fdt, bd_t *bd)
 	return 0;
 }
 
-int ft_system_setup(void *blob, bd_t *bd)
-{
 #ifdef CONFIG_IMX8MQ
+static int disable_fdt_nodes(void *blob, const char *nodes_path[], int size_array)
+{
 	int i = 0;
 	int rc;
 	int nodeoff;
+	const char *status = "disabled";
 
-	if (get_boot_device() == USB_BOOT) {
-		static const char *nodes_path[] = {
-			"/dcss@32e00000",
-			"/hdmi@32c00000",
-			"/hdmi_cec@32c33800",
-			"/hdmi_drm@32c00000",
-			"/display-subsystem",
-			"/sound-hdmi"
-		};
-		const char *status = "disabled";
-
-		for (i = 0; i < ARRAY_SIZE(nodes_path); i++) {
-			nodeoff = fdt_path_offset(blob, nodes_path[i]);
-			if (nodeoff < 0)
-				continue; /* Not found, skip it */
+	for (i = 0; i < size_array; i++) {
+		nodeoff = fdt_path_offset(blob, nodes_path[i]);
+		if (nodeoff < 0)
+			continue; /* Not found, skip it */
 
-			printf("Found %s node\n", nodes_path[i]);
+		printf("Found %s node\n", nodes_path[i]);
 
 add_status:
-			rc = fdt_setprop(blob, nodeoff, "status", status, strlen(status) + 1);
-			if (rc) {
-				if (rc == -FDT_ERR_NOSPACE) {
-					rc = fdt_increase_size(blob, 512);
-					if (!rc)
-						goto add_status;
-				}
-				printf("Unable to update property %s:%s, err=%s\n",
-					nodes_path[i], "status", fdt_strerror(rc));
-			} else {
-				printf("Modify %s:%s disabled\n",
-					nodes_path[i], "status");
+		rc = fdt_setprop(blob, nodeoff, "status", status, strlen(status) + 1);
+		if (rc) {
+			if (rc == -FDT_ERR_NOSPACE) {
+				rc = fdt_increase_size(blob, 512);
+				if (!rc)
+					goto add_status;
 			}
+			printf("Unable to update property %s:%s, err=%s\n",
+				nodes_path[i], "status", fdt_strerror(rc));
+		} else {
+			printf("Modify %s:%s disabled\n",
+				nodes_path[i], "status");
 		}
+	}
+
+	return 0;
+}
+
+static int disable_mipi_dsi_nodes(void *blob)
+{
+	const char *nodes_path[] = {
+		"/mipi_dsi@30A00000",
+		"/mipi_dsi_bridge@30A00000",
+		"/dsi_phy@30A00300"
+	};
+
+	return disable_fdt_nodes(blob, nodes_path, ARRAY_SIZE(nodes_path));
+}
+
+static int disable_dcss_nodes(void *blob)
+{
+	const char *nodes_path[] = {
+		"/dcss@0x32e00000",
+		"/dcss@32e00000",
+		"/hdmi@32c00000",
+		"/hdmi_cec@32c33800",
+		"/hdmi_drm@32c00000",
+		"/display-subsystem",
+		"/sound-hdmi"
+	};
+
+	return disable_fdt_nodes(blob, nodes_path, ARRAY_SIZE(nodes_path));
+}
+
+static int disable_vpu_nodes(void *blob)
+{
+	const char *nodes_path[] = {
+		"/vpu@38300000"
+	};
+
+	return disable_fdt_nodes(blob, nodes_path, ARRAY_SIZE(nodes_path));
+}
+
+static int disable_cpu_nodes(void *blob)
+{
+	const char *nodes_path[] = {
+			"/cpus/cpu@2",
+			"/cpus/cpu@3",
+	};
+
+	int i = 0;
+	int rc;
+	int nodeoff;
+
+	for (i = 0; i < ARRAY_SIZE(nodes_path); i++) {
+		nodeoff = fdt_path_offset(blob, nodes_path[i]);
+		if (nodeoff < 0)
+			continue; /* Not found, skip it */
+
+		printf("Found %s node\n", nodes_path[i]);
+
+		rc = fdt_del_node(blob, nodeoff);
+		if (rc < 0) {
+			printf("Unable to delete node %s, err=%s\n",
+				nodes_path[i], fdt_strerror(rc));
+		} else {
+			printf("Delete node %s\n", nodes_path[i]);
+		}
+	}
+
+	return 0;
+}
+
+static int check_mipi_dsi_nodes(void *blob)
+{
+	const char *lcdif_path = "/lcdif@30320000";
+	const char *mipi_dsi_path = "/mipi_dsi@30A00000";
+
+	const char *lcdif_ep_path = "/lcdif@30320000/port@0/mipi-dsi-endpoint";
+	const char *mipi_dsi_ep_path = "/mipi_dsi@30A00000/port@1/endpoint";
+
+	int nodeoff;
+	nodeoff = fdt_path_offset(blob, lcdif_path);
+	if (nodeoff < 0 || !fdtdec_get_is_enabled(blob, nodeoff)) {
+		/* If can't find lcdif node or lcdif node is disabled, then disable all mipi dsi,
+		    since they only can input from DCSS */
+		return disable_mipi_dsi_nodes(blob);
+	}
+
+	nodeoff = fdt_path_offset(blob, mipi_dsi_path);
+	if (nodeoff < 0 || !fdtdec_get_is_enabled(blob, nodeoff))
+		return 0;
+
+	nodeoff = fdt_path_offset(blob, lcdif_ep_path);
+	if (nodeoff < 0) {
+		/* If can't find lcdif endpoint, then disable all mipi dsi,
+		    since they only can input from DCSS */
+		return disable_mipi_dsi_nodes(blob);
+	} else {
+		int lookup_node;
+		lookup_node = fdtdec_lookup_phandle(blob, nodeoff, "remote-endpoint");
+		nodeoff = fdt_path_offset(blob, mipi_dsi_ep_path);
+
+		if (nodeoff >0 && nodeoff == lookup_node)
+			return 0;
+
+		return disable_mipi_dsi_nodes(blob);
+	}
+
+}
+#endif
+
+int ft_system_setup(void *blob, bd_t *bd)
+{
+#ifdef CONFIG_IMX8MQ
+	int i = 0;
+	int rc;
+	int nodeoff;
+
+	if (get_boot_device() == USB_BOOT) {
+
+		disable_dcss_nodes(blob);
 
 		const char *usb_dwc3_path = "/usb@38100000/dwc3";
 		nodeoff = fdt_path_offset(blob, usb_dwc3_path);
@@ -397,6 +506,16 @@ usb_modify_speed:
 			       "cpu-idle-states");
 		}
 	}
+
+	if (is_imx8mql()) {
+		disable_vpu_nodes(blob);
+		disable_dcss_nodes(blob);
+		check_mipi_dsi_nodes(blob);
+	}
+
+	if (is_imx8md())
+		disable_cpu_nodes(blob);
+
 #endif
 
 	return ft_add_optee_node(blob, bd);