From 4c230d74d15f63d73b49216f357f865843f55b0a Mon Sep 17 00:00:00 2001
From: Wasim Khan <wasim.khan@nxp.com>
Date: Thu, 17 Jun 2021 09:12:59 +0200
Subject: [PATCH] armv8: lx2160ardb: Add thermal node fixup for revc board

lx2160ardb Rev-C board has i2c node for thermal monitors
connected to different chip offset.
Add device tree fixup to use lx2160ardb dts and apply
thermal node fixups for lx2160ardb Rev-C board.

Signed-off-by: Wasim Khan <wasim.khan@nxp.com>
[sp:thernal->thermal]
Reviewed-by: Priyanka Jain <priyanka.jain@nxp.com>
---
 board/freescale/lx2160a/lx2160a.c | 121 +++++++++++++++++++++++++++++-
 1 file changed, 120 insertions(+), 1 deletion(-)

diff --git a/board/freescale/lx2160a/lx2160a.c b/board/freescale/lx2160a/lx2160a.c
index db00ad867d..f505e82fb9 100644
--- a/board/freescale/lx2160a/lx2160a.c
+++ b/board/freescale/lx2160a/lx2160a.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0+
 /*
- * Copyright 2018-2020 NXP
+ * Copyright 2018-2021 NXP
  */
 
 #include <common.h>
@@ -707,6 +707,116 @@ void board_quiesce_devices(void)
 }
 #endif
 
+#if CONFIG_IS_ENABLED(TARGET_LX2160ARDB)
+int fdt_fixup_add_thermal(void *blob, int mux_node, int channel, int reg)
+{
+	int err;
+	int noff;
+	int offset;
+	char channel_node_name[50];
+	char thermal_node_name[50];
+	u32 phandle;
+
+	snprintf(channel_node_name, sizeof(channel_node_name),
+		 "i2c@%x", channel);
+	debug("channel_node_name = %s\n", channel_node_name);
+
+	snprintf(thermal_node_name, sizeof(thermal_node_name),
+		 "temperature-sensor@%x", reg);
+	debug("thermal_node_name = %s\n", thermal_node_name);
+
+	err = fdt_increase_size(blob, 200);
+	if (err) {
+		printf("fdt_increase_size: err=%s\n", fdt_strerror(err));
+		return err;
+	}
+
+	noff = fdt_subnode_offset(blob, mux_node, (const char *)
+				  channel_node_name);
+	if (noff < 0) {
+		/* channel node not found - create it */
+		noff = fdt_add_subnode(blob, mux_node, channel_node_name);
+		if (noff < 0) {
+			printf("fdt_add_subnode: err=%s\n", fdt_strerror(err));
+			return err;
+		}
+		fdt_setprop_u32 (blob, noff, "#address-cells", 1);
+		fdt_setprop_u32 (blob, noff, "#size-cells", 0);
+		fdt_setprop_u32 (blob, noff, "reg", channel);
+	}
+
+	/* Create thermal node*/
+	offset = fdt_add_subnode(blob, noff, thermal_node_name);
+	fdt_setprop(blob, offset, "compatible", "nxp,sa56004",
+		    strlen("nxp,sa56004") + 1);
+	fdt_setprop_u32 (blob, offset, "reg", reg);
+
+	/* fixup phandle*/
+	noff = fdt_node_offset_by_compatible(blob, -1, "regulator-fixed");
+	if (noff < 0) {
+		printf("%s : failed to get phandle\n", __func__);
+		return noff;
+	}
+	phandle = fdt_get_phandle(blob, noff);
+	fdt_setprop_u32 (blob, offset, "vcc-supply", phandle);
+
+	return 0;
+}
+
+void fdt_fixup_delete_thermal(void *blob, int mux_node, int channel, int reg)
+{
+	int node;
+	int value;
+	int err;
+	int subnode;
+
+	fdt_for_each_subnode(subnode, blob, mux_node) {
+		value = fdtdec_get_uint(blob, subnode, "reg", -1);
+		if (value == channel) {
+			/* delete thermal node */
+			fdt_for_each_subnode(node, blob, subnode) {
+				value = fdtdec_get_uint(blob, node, "reg", -1);
+				err = fdt_node_check_compatible(blob, node,
+								"nxp,sa56004");
+				if (!err && value == reg) {
+					fdt_del_node(blob, node);
+					break;
+				}
+			}
+		}
+	}
+}
+
+void fdt_fixup_i2c_thermal_node(void *blob)
+{
+	int i2coffset;
+	int mux_node;
+	int reg;
+	int err;
+
+	i2coffset = fdt_node_offset_by_compat_reg(blob, "fsl,vf610-i2c",
+						  0x2000000);
+	if (i2coffset != -FDT_ERR_NOTFOUND) {
+		fdt_for_each_subnode(mux_node, blob, i2coffset) {
+			reg = fdtdec_get_uint(blob, mux_node, "reg", -1);
+			err = fdt_node_check_compatible(blob, mux_node,
+							"nxp,pca9547");
+			if (!err && reg == 0x77) {
+				fdt_fixup_delete_thermal(blob, mux_node,
+							 0x3, 0x4d);
+				err = fdt_fixup_add_thermal(blob, mux_node,
+							    0x3, 0x48);
+				if (err)
+					printf("%s: Add thermal node failed\n",
+					       __func__);
+			}
+		}
+	} else {
+		printf("%s: i2c node not found\n", __func__);
+	}
+}
+#endif
+
 #ifdef CONFIG_OF_BOARD_SETUP
 int ft_board_setup(void *blob, struct bd_info *bd)
 {
@@ -718,6 +828,9 @@ int ft_board_setup(void *blob, struct bd_info *bd)
 	u64 mc_memory_base = 0;
 	u64 mc_memory_size = 0;
 	u16 total_memory_banks;
+#if CONFIG_IS_ENABLED(TARGET_LX2160ARDB)
+	u8 board_rev;
+#endif
 
 	ft_cpu_setup(blob, bd);
 
@@ -772,6 +885,12 @@ int ft_board_setup(void *blob, struct bd_info *bd)
 #endif
 	fdt_fixup_icid(blob);
 
+#if CONFIG_IS_ENABLED(TARGET_LX2160ARDB)
+	board_rev = (QIXIS_READ(arch) & 0xf) - 1 + 'A';
+	if (board_rev == 'C')
+		fdt_fixup_i2c_thermal_node(blob);
+#endif
+
 	return 0;
 }
 #endif
-- 
2.39.5