From a6870706640aa4e775a4f05f3c85e47e0d3e4b85 Mon Sep 17 00:00:00 2001
From: Wadim Egorov <w.egorov@phytec.de>
Date: Wed, 30 Oct 2024 17:48:15 +0100
Subject: [PATCH] board: phytec: common: k3: Apply SoM-specific overlays to OS
 device tree

Our SoMs are available in multiple configurations, managed via device
tree overlays. To determine the specific variant in use, we read the
EEPROM and apply the appropriate overlays during boot to the device tree
used by the OS.

Signed-off-by: Wadim Egorov <w.egorov@phytec.de>
Acked-by: Neha Malcom Francis <n-francis@ti.com>
---
 board/phytec/common/k3/board.c | 73 ++++++++++++++++++++++++++++++++++
 1 file changed, 73 insertions(+)

diff --git a/board/phytec/common/k3/board.c b/board/phytec/common/k3/board.c
index 3d7e090cca..346b2b6491 100644
--- a/board/phytec/common/k3/board.c
+++ b/board/phytec/common/k3/board.c
@@ -6,7 +6,9 @@
 
 #include <env_internal.h>
 #include <fdt_support.h>
+#include <dm/ofnode.h>
 #include <spl.h>
+#include <malloc.h>
 #include <asm/arch/hardware.h>
 
 #include "../am6_som_detection.h"
@@ -97,8 +99,79 @@ int board_late_init(void)
 #endif
 
 #if IS_ENABLED(CONFIG_OF_LIBFDT) && IS_ENABLED(CONFIG_OF_BOARD_SETUP)
+static int fdt_apply_overlay_from_fit(const char *overlay_path, void *fdt)
+{
+	u64 loadaddr;
+	ofnode node;
+	int ret;
+
+	node = ofnode_path(overlay_path);
+	if (!ofnode_valid(node))
+		return -FDT_ERR_NOTFOUND;
+
+	ret = ofnode_read_u64(node, "load", &loadaddr);
+	if (ret)
+		return ret;
+
+	return fdt_overlay_apply_verbose(fdt, (void *)loadaddr);
+}
+
+static void fdt_apply_som_overlays(void *blob)
+{
+	void *fdt_copy;
+	u32 fdt_size;
+	struct phytec_eeprom_data data;
+	int err;
+
+	fdt_size = fdt_totalsize(blob);
+	fdt_copy = malloc(fdt_size);
+	if (!fdt_copy)
+		goto fixup_error;
+
+	memcpy(fdt_copy, blob, fdt_size);
+
+	err = phytec_eeprom_data_setup(&data, 0, EEPROM_ADDR);
+	if (err)
+		goto fixup_error;
+
+	if (phytec_get_am6_rtc(&data) == 0) {
+		err = fdt_apply_overlay_from_fit("/fit-images/som-no-rtc", fdt_copy);
+		if (err)
+			goto fixup_error;
+	}
+
+	if (phytec_get_am6_spi(&data) == PHYTEC_EEPROM_VALUE_X) {
+		err = fdt_apply_overlay_from_fit("/fit-images/som-no-spi", fdt_copy);
+		if (err)
+			goto fixup_error;
+	}
+
+	if (phytec_get_am6_eth(&data) == 0) {
+		err = fdt_apply_overlay_from_fit("/fit-images/som-no-eth", fdt_copy);
+		if (err)
+			goto fixup_error;
+	}
+
+	if (phytec_am6_is_qspi(&data)) {
+		err = fdt_apply_overlay_from_fit("/fit-images/som-qspi-nor", fdt_copy);
+		if (err)
+			goto fixup_error;
+	}
+
+	memcpy(blob, fdt_copy, fdt_size);
+
+cleanup:
+	free(fdt_copy);
+	return;
+
+fixup_error:
+	pr_err("Failed to apply SoM overlays\n");
+	goto cleanup;
+}
+
 int ft_board_setup(void *blob, struct bd_info *bd)
 {
+	fdt_apply_som_overlays(blob);
 	fdt_copy_fixed_partitions(blob);
 
 	return 0;
-- 
2.39.5