[PATCH 1/6] drivers: pci: pcie_dw_common: Add dw_pcie_link_set_max_link_width()

[Marek Vasut]

Add dw_pcie_link_set_max_link_width() implementation ported from Linux kernel
as of commit 89db0793c9f2 ("PCI: dwc: Add missing PCI_EXP_LNKCAP_MLW handling").
This is common code which is already duplicated in multiple drivers.

Signed-off-by: Marek Vasut <marek.vasut+renesas at mailbox.org>
---
Cc: Casey Connolly <casey.connolly at linaro.org>
Cc: Christian Marangi <ansuelsmth at gmail.com>
Cc: Daniel Schwierzeck <daniel.schwierzeck at gmail.com>
Cc: Jiaxun Yang <jiaxun.yang at flygoat.com>
Cc: John Crispin <john at phrozen.org>
Cc: Kever Yang <kever.yang at rock-chips.com>
Cc: Neil Armstrong <neil.armstrong at linaro.org>
Cc: Nobuhiro Iwamatsu <iwamatsu at nigauri.org>
Cc: Philipp Tomsich <philipp.tomsich at vrull.eu>
Cc: Siddharth Vadapalli <s-vadapalli at ti.com>
Cc: Simon Glass <sjg at chromium.org>
Cc: Sumit Garg <sumit.garg at kernel.org>
Cc: Tom Rini <trini at konsulko.com>
Cc: u-boot-amlogic at groups.io
Cc: u-boot-qcom at groups.io
Cc: u-boot at lists.denx.de
---
 drivers/pci/pcie_dw_common.c | 45 ++++++++++++++++++++++++++++++++++++
 drivers/pci/pcie_dw_common.h |  2 ++
 2 files changed, 47 insertions(+)

diff --git a/drivers/pci/pcie_dw_common.c b/drivers/pci/pcie_dw_common.c
index 78961271a8e..c4cad019373 100644
--- a/drivers/pci/pcie_dw_common.c
+++ b/drivers/pci/pcie_dw_common.c
@@ -13,6 +13,7 @@
 #include <pci.h>
 #include <dm/device_compat.h>
 #include <asm/io.h>
+#include <linux/bitfield.h>
 #include <linux/delay.h>
 #include "pcie_dw_common.h"
 
@@ -28,6 +29,50 @@ int pcie_dw_get_link_width(struct pcie_dw *pci)
 		PCIE_LINK_STATUS_WIDTH_MASK) >> PCIE_LINK_STATUS_WIDTH_OFF;
 }
 
+void dw_pcie_link_set_max_link_width(struct pcie_dw *pci, u32 num_lanes)
+{
+	u32 lnkcap, lwsc, plc;
+	u8 cap;
+
+	if (!num_lanes)
+		return;
+
+	/* Set the number of lanes */
+	plc = readl(pci->dbi_base + PCIE_PORT_LINK_CONTROL);
+	plc &= ~PORT_LINK_FAST_LINK_MODE;
+	plc &= ~PORT_LINK_MODE_MASK;
+
+	/* Set link width speed control register */
+	lwsc = readl(pci->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
+	lwsc &= ~PORT_LOGIC_LINK_WIDTH_MASK;
+	lwsc |= PORT_LOGIC_LINK_WIDTH_1_LANES;
+	switch (num_lanes) {
+	case 1:
+		plc |= PORT_LINK_MODE_1_LANES;
+		break;
+	case 2:
+		plc |= PORT_LINK_MODE_2_LANES;
+		break;
+	case 4:
+		plc |= PORT_LINK_MODE_4_LANES;
+		break;
+	case 8:
+		plc |= PORT_LINK_MODE_8_LANES;
+		break;
+	default:
+		dev_err(pci->dev, "num-lanes %u: invalid value\n", num_lanes);
+		return;
+	}
+	writel(plc, pci->dbi_base + PCIE_PORT_LINK_CONTROL);
+	writel(lwsc, pci->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
+
+	cap = pcie_dw_find_capability(pci, PCI_CAP_ID_EXP);
+	lnkcap = readl(pci->dbi_base + cap + PCI_EXP_LNKCAP);
+	lnkcap &= ~PCI_EXP_LNKCAP_MLW;
+	lnkcap |= FIELD_PREP(PCI_EXP_LNKCAP_MLW, num_lanes);
+	writel(lnkcap, pci->dbi_base + cap + PCI_EXP_LNKCAP);
+}
+
 static void dw_pcie_writel_ob_unroll(struct pcie_dw *pci, u32 index, u32 reg,
 				     u32 val)
 {
diff --git a/drivers/pci/pcie_dw_common.h b/drivers/pci/pcie_dw_common.h
index 8cb99a12ea1..ccd423081eb 100644
--- a/drivers/pci/pcie_dw_common.h
+++ b/drivers/pci/pcie_dw_common.h
@@ -130,6 +130,8 @@ int pcie_dw_get_link_speed(struct pcie_dw *pci);
 
 int pcie_dw_get_link_width(struct pcie_dw *pci);
 
+void dw_pcie_link_set_max_link_width(struct pcie_dw *pci, u32 num_lanes);
+
 int pcie_dw_prog_outbound_atu_unroll(struct pcie_dw *pci, int index, int type, u64 cpu_addr,
 				     u64 pci_addr, u32 size);
 
-- 
2.47.2