From 201a37432303687d25d7a071ea22b1c20dca886e Mon Sep 17 00:00:00 2001
From: Thomas Makin <halorocker89@gmail.com>
Date: Tue, 2 Dec 2025 02:06:18 +0000
Subject: [PATCH] pci: tegra: update lanes when incorrect

---
 drivers/pci/controller/pci-tegra.c | 32 +++++++++++++++++++++++++++---
 1 file changed, 29 insertions(+), 3 deletions(-)

diff --git a/drivers/pci/controller/pci-tegra.c b/drivers/pci/controller/pci-tegra.c
index e6935c1a4861..c21d6e20946e 100644
--- a/drivers/pci/controller/pci-tegra.c
+++ b/drivers/pci/controller/pci-tegra.c
@@ -2171,6 +2171,15 @@ static void tegra_pcie_disable_interrupts(struct tegra_pcie *pcie)
 	afi_writel(pcie, value, AFI_INTR_MASK);
 }
 
+static void update_rp_lanes(struct tegra_pcie *pcie, u32 lanes)
+{
+	struct tegra_pcie_port *port = NULL;
+
+	list_for_each_entry(port, &pcie->ports, list)
+		port->lanes = (lanes >> (port->index << 3)) & 0xFF;
+
+}
+
 static int tegra_pcie_get_xbar_config(struct tegra_pcie *pcie, u32 lanes,
 				      u32 *xbar)
 {
@@ -2199,12 +2208,22 @@ static int tegra_pcie_get_xbar_config(struct tegra_pcie *pcie, u32 lanes,
 				 "switching to default 2x1, 1x1, 1x1 "
 				 "configuration\n");
 			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_211;
+			update_rp_lanes(pcie, 0x010102);
 			return 0;
 		}
 	} else if (of_device_is_compatible(np, "nvidia,tegra210b01-pcie")) {
-		dev_info(dev, "4x1, 1x1 configuration\n");
-		*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X4_X1;
-		return 0;
+		switch (lanes) {
+		case 0x0104:
+			dev_info(dev, "4x1, 1x1 configuration\n");
+			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X4_X1;
+			return 0;
+		default:
+			dev_info(dev, "wrong configuration updated in DT, "
+				"switching to default 4x1, 1x1 configuration\n");
+			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X4_X1;
+			update_rp_lanes(pcie, 0x0104);
+			return 0;
+		}
 	} else if (of_device_is_compatible(np, "nvidia,tegra124-pcie") ||
 		   of_device_is_compatible(np, "nvidia,tegra210-pcie")) {
 		switch (lanes) {
@@ -2217,6 +2236,13 @@ static int tegra_pcie_get_xbar_config(struct tegra_pcie *pcie, u32 lanes,
 			dev_info(dev, "2x1, 1x1 configuration\n");
 			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X2_X1;
 			return 0;
+
+		default:
+			dev_info(dev, "wrong configuration updated in DT, "
+				"switching to default 4x1, 1x1 configuration\n");
+			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X4_X1;
+			update_rp_lanes(pcie, 0x0104);
+			return 0;
 		}
 	} else if (of_device_is_compatible(np, "nvidia,tegra30-pcie")) {
 		switch (lanes) {