[PATCH 3/3] phy: tegra: Add lane margin support

[Manikanta Maddireddy]

Per PCIe r6.0.1, section 4.2.18, Lane Margining at Receiver is mandatory
for all Ports supporting a data rate of 16.0 GT/s or higher. Tegra234
supports Gen4 and Receiver Lane Margining with per lane PIPE2UPHY instance
acting as a relay between PCIe controller and Universal PHY (UPHY).

P2U driver enables MARGIN_SW_READY and MARGIN_READY bits to start snooping
on changes in lane margin control register in PCIe configuration space.
P2U HW generates MARGIN_START or MARGIN_CHANGE interrupt after it copied
margin control data to P2U_RX_MARGIN_CTRL register. On MARGIN_START or
MARGIN_CHANGE interrupt, P2U driver copies margin control data to UPHY
via mailbox communication. UPHY HW performs margining operation and
P2U driver copies margin status from UPHY to P2U_RX_MARGIN_STATUS
register. P2U driver repeats this until PCIe controller issues margin
stop command.

Signed-off-by: Manikanta Maddireddy <mmaddireddy@xxxxxxxxxx>
---
 drivers/phy/tegra/phy-tegra194-p2u.c | 272 +++++++++++++++++++++++++++
 1 file changed, 272 insertions(+)

diff --git a/drivers/phy/tegra/phy-tegra194-p2u.c b/drivers/phy/tegra/phy-tegra194-p2u.c
index 633e6b747275..a86b4af70ab9 100644
--- a/drivers/phy/tegra/phy-tegra194-p2u.c
+++ b/drivers/phy/tegra/phy-tegra194-p2u.c
@@ -7,12 +7,15 @@
  * Author: Vidya Sagar <vidyas@xxxxxxxxxx>
  */
 
+#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/phy/phy.h>
+#include <soc/tegra/bpmp.h>
+#include <soc/tegra/bpmp-abi.h>
 
 #define P2U_CONTROL_CMN			0x74
 #define P2U_CONTROL_CMN_ENABLE_L2_EXIT_RATE_CHANGE		BIT(13)
@@ -31,14 +34,57 @@
 #define P2U_DIR_SEARCH_CTRL				0xd4
 #define P2U_DIR_SEARCH_CTRL_GEN4_FINE_GRAIN_SEARCH_TWICE	BIT(18)
 
+#define P2U_RX_MARGIN_SW_INT_EN		0xe0
+#define P2U_RX_MARGIN_SW_INT_EN_READINESS		BIT(0)
+#define P2U_RX_MARGIN_SW_INT_EN_MARGIN_START		BIT(1)
+#define P2U_RX_MARGIN_SW_INT_EN_MARGIN_CHANGE		BIT(2)
+#define P2U_RX_MARGIN_SW_INT_EN_MARGIN_STOP		BIT(3)
+
+#define P2U_RX_MARGIN_SW_INT		0xe4
+#define P2U_RX_MARGIN_SW_INT_MASK			0xf
+#define P2U_RX_MARGIN_SW_INT_READINESS			BIT(0)
+#define P2U_RX_MARGIN_SW_INT_MARGIN_START		BIT(1)
+#define P2U_RX_MARGIN_SW_INT_MARGIN_CHANGE		BIT(2)
+#define P2U_RX_MARGIN_SW_INT_MARGIN_STOP		BIT(3)
+
+#define P2U_RX_MARGIN_SW_STATUS		0xe8
+#define P2U_RX_MARGIN_SW_STATUS_MARGIN_SW_READY		BIT(0)
+#define P2U_RX_MARGIN_SW_STATUS_MARGIN_READY		BIT(1)
+#define P2U_RX_MARGIN_SW_STATUS_PHY_MARGIN_STATUS	BIT(2)
+#define P2U_RX_MARGIN_SW_STATUS_PHY_MARGIN_ERROR_STATUS	BIT(3)
+
+#define P2U_RX_MARGIN_CTRL		0xec
+
+#define P2U_RX_MARGIN_STATUS		0xf0
+#define P2U_RX_MARGIN_STATUS_ERRORS_MASK		0xffff
+
+enum margin_state {
+	RX_MARGIN_START_CHANGE = 1,
+	RX_MARGIN_STOP,
+	RX_MARGIN_GET_MARGIN,
+};
+
 struct tegra_p2u_of_data {
 	bool one_dir_search;
+	bool lane_margin;
 };
 
 struct tegra_p2u {
 	void __iomem *base;
 	bool skip_sz_protection_en; /* Needed to support two retimers */
 	struct tegra_p2u_of_data *of_data;
+	struct device *dev;
+	struct tegra_bpmp *bpmp;
+	u32 id;
+	atomic_t margin_state;
+};
+
+struct margin_ctrl {
+	u32 en:1;
+	u32 clr:1;
+	u32 x:7;
+	u32 y:6;
+	u32 n_blks:8;
 };
 
 static inline void p2u_writel(struct tegra_p2u *phy, const u32 value,
@@ -83,6 +129,14 @@ static int tegra_p2u_power_on(struct phy *x)
 		p2u_writel(phy, val, P2U_DIR_SEARCH_CTRL);
 	}
 
+	if (phy->of_data->lane_margin) {
+		p2u_writel(phy, P2U_RX_MARGIN_SW_INT_EN_READINESS |
+			   P2U_RX_MARGIN_SW_INT_EN_MARGIN_START |
+			   P2U_RX_MARGIN_SW_INT_EN_MARGIN_CHANGE |
+			   P2U_RX_MARGIN_SW_INT_EN_MARGIN_STOP,
+			   P2U_RX_MARGIN_SW_INT_EN);
+	}
+
 	return 0;
 }
 
@@ -104,17 +158,195 @@ static const struct phy_ops ops = {
 	.owner = THIS_MODULE,
 };
 
+static int tegra_p2u_set_margin_control(struct tegra_p2u *phy, u32 ctrl_data)
+{
+	struct tegra_bpmp_message msg;
+	struct mrq_uphy_response resp;
+	struct mrq_uphy_request req;
+	struct margin_ctrl ctrl;
+	int err;
+
+	memcpy(&ctrl, &ctrl_data, sizeof(ctrl_data));
+
+	memset(&req, 0, sizeof(req));
+	memset(&resp, 0, sizeof(resp));
+
+	req.lane = phy->id;
+	req.cmd = CMD_UPHY_PCIE_LANE_MARGIN_CONTROL;
+	req.uphy_set_margin_control.en = ctrl.en;
+	req.uphy_set_margin_control.clr = ctrl.clr;
+	req.uphy_set_margin_control.x = ctrl.x;
+	req.uphy_set_margin_control.y = ctrl.y;
+	req.uphy_set_margin_control.nblks = ctrl.n_blks;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_UPHY;
+	msg.tx.data = &req;
+	msg.tx.size = sizeof(req);
+	msg.rx.data = &resp;
+	msg.rx.size = sizeof(resp);
+
+	err = tegra_bpmp_transfer(phy->bpmp, &msg);
+	if (err)
+		return err;
+	if (msg.rx.ret)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int tegra_p2u_get_margin_status(struct tegra_p2u *phy, u32 *val)
+{
+	struct tegra_bpmp_message msg;
+	struct mrq_uphy_response resp;
+	struct mrq_uphy_request req;
+	int rc;
+
+	req.lane = phy->id;
+	req.cmd = CMD_UPHY_PCIE_LANE_MARGIN_STATUS;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_UPHY;
+	msg.tx.data = &req;
+	msg.tx.size = sizeof(req);
+	msg.rx.data = &resp;
+	msg.rx.size = sizeof(resp);
+
+	rc = tegra_bpmp_transfer(phy->bpmp, &msg);
+	if (rc)
+		return rc;
+	if (msg.rx.ret)
+		return -EINVAL;
+
+	*val = resp.uphy_get_margin_status.status;
+
+	return 0;
+}
+
+static irqreturn_t tegra_p2u_irq_thread(int irq, void *arg)
+{
+	struct tegra_p2u *phy = arg;
+	struct device *dev = phy->dev;
+	u32 val;
+	int state, ret;
+
+	do {
+		state = atomic_read(&phy->margin_state);
+		switch (state) {
+		case RX_MARGIN_START_CHANGE:
+		case RX_MARGIN_STOP:
+			/* Read margin control data and copy it to UPHY. */
+			val = p2u_readl(phy, P2U_RX_MARGIN_CTRL);
+			ret = tegra_p2u_set_margin_control(phy, val);
+			if (ret) {
+				dev_err(dev, "failed to set margin control: %d\n", ret);
+				break;
+			}
+
+			p2u_writel(phy, P2U_RX_MARGIN_SW_STATUS_MARGIN_SW_READY |
+				   P2U_RX_MARGIN_SW_STATUS_MARGIN_READY |
+				   P2U_RX_MARGIN_SW_STATUS_PHY_MARGIN_STATUS |
+				   P2U_RX_MARGIN_SW_STATUS_PHY_MARGIN_ERROR_STATUS,
+				   P2U_RX_MARGIN_SW_STATUS);
+
+			usleep_range(10, 11);
+
+			if (state == RX_MARGIN_STOP) {
+				/* Return from the loop if PCIe ctrl issues margin stop cmd. */
+				p2u_writel(phy, P2U_RX_MARGIN_SW_STATUS_MARGIN_SW_READY |
+					   P2U_RX_MARGIN_SW_STATUS_MARGIN_READY |
+					   P2U_RX_MARGIN_SW_STATUS_PHY_MARGIN_ERROR_STATUS,
+					   P2U_RX_MARGIN_SW_STATUS);
+
+				return IRQ_HANDLED;
+			}
+
+			atomic_set(&phy->margin_state, RX_MARGIN_GET_MARGIN);
+			break;
+
+		case RX_MARGIN_GET_MARGIN:
+			/*
+			 * Read margin status from UPHY and program it in P2U_RX_MARGIN_STATUS
+			 * register. This data will reflect in PCIe controller's margining lane
+			 * status register.
+			 */
+			ret = tegra_p2u_get_margin_status(phy, &val);
+			if (ret) {
+				dev_err(dev, "failed to get margin status: %d\n", ret);
+				break;
+			}
+			p2u_writel(phy, val & P2U_RX_MARGIN_STATUS_ERRORS_MASK,
+				   P2U_RX_MARGIN_STATUS);
+
+			p2u_writel(phy, P2U_RX_MARGIN_SW_STATUS_MARGIN_SW_READY |
+				   P2U_RX_MARGIN_SW_STATUS_MARGIN_READY |
+				   P2U_RX_MARGIN_SW_STATUS_PHY_MARGIN_ERROR_STATUS,
+				   P2U_RX_MARGIN_SW_STATUS);
+
+			msleep(20);
+			break;
+
+		default:
+			dev_err(dev, "Invalid margin state: %d\n", state);
+			return IRQ_HANDLED;
+		};
+	} while (1);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t tegra_p2u_irq_handler(int irq, void *arg)
+{
+	struct tegra_p2u *phy = (struct tegra_p2u *)arg;
+	u32 val = 0;
+	irqreturn_t ret = IRQ_HANDLED;
+
+	val = p2u_readl(phy, P2U_RX_MARGIN_SW_INT);
+	p2u_writel(phy, val, P2U_RX_MARGIN_SW_INT);
+
+	/*
+	 * When PCIe link trains to Gen4, P2U HW generate READINESS interrupt. Set MARGIN_SW_READY
+	 * and MARGIN_READY bits to enable P2U HW sample lane margin control data from PCIe
+	 * controller's configuration space.
+	 */
+	if (val & P2U_RX_MARGIN_SW_INT_READINESS)
+		p2u_writel(phy, P2U_RX_MARGIN_SW_STATUS_MARGIN_SW_READY |
+			   P2U_RX_MARGIN_SW_STATUS_MARGIN_READY,
+			   P2U_RX_MARGIN_SW_STATUS);
+
+	/*
+	 * P2U HW generates MARGIN_START or MARGIN_CHANGE interrupt after it copied margin control
+	 * data to P2U_RX_MARGIN_CTRL register.
+	 */
+	if ((val & P2U_RX_MARGIN_SW_INT_MARGIN_START) ||
+	    (val & P2U_RX_MARGIN_SW_INT_MARGIN_CHANGE)) {
+		atomic_set(&phy->margin_state, RX_MARGIN_START_CHANGE);
+		ret = IRQ_WAKE_THREAD;
+	}
+
+	/* P2U HW generates MARGIN_STOP interrupt when PCIe controller issues margin stop cmd. */
+	if (val & P2U_RX_MARGIN_SW_INT_MARGIN_STOP)
+		atomic_set(&phy->margin_state, RX_MARGIN_STOP);
+
+	return ret;
+}
+
 static int tegra_p2u_probe(struct platform_device *pdev)
 {
 	struct phy_provider *phy_provider;
 	struct device *dev = &pdev->dev;
 	struct phy *generic_phy;
 	struct tegra_p2u *phy;
+	int ret;
+	u32 irq;
 
 	phy = devm_kzalloc(dev, sizeof(*phy), GFP_KERNEL);
 	if (!phy)
 		return -ENOMEM;
 
+	phy->dev = dev;
+	platform_set_drvdata(pdev, phy);
+
 	phy->of_data =
 		(struct tegra_p2u_of_data *)of_device_get_match_data(dev);
 	if (!phy->of_data)
@@ -140,15 +372,54 @@ static int tegra_p2u_probe(struct platform_device *pdev)
 	if (IS_ERR(phy_provider))
 		return PTR_ERR(phy_provider);
 
+	if (phy->of_data->lane_margin) {
+		irq = platform_get_irq_byname(pdev, "intr");
+		if (irq < 0) {
+			dev_err(dev, "failed to get intr interrupt\n");
+			return irq;
+		}
+
+		ret = devm_request_threaded_irq(dev, irq, tegra_p2u_irq_handler,
+						tegra_p2u_irq_thread, 0,
+						"tegra-p2u-intr", phy);
+		if (ret) {
+			dev_err(dev, "failed to request intr irq\n");
+			return ret;
+		}
+
+		ret = of_property_read_u32_index(dev->of_node, "nvidia,bpmp",
+						 1, &phy->id);
+		if (ret) {
+			dev_err(dev, "failed to read P2U id: %d\n", ret);
+			return ret;
+		}
+
+		phy->bpmp = tegra_bpmp_get(dev);
+		if (IS_ERR(phy->bpmp))
+			return PTR_ERR(phy->bpmp);
+	}
+
+	return 0;
+}
+
+static int tegra_p2u_remove(struct platform_device *pdev)
+{
+	struct tegra_p2u *phy = platform_get_drvdata(pdev);
+
+	if (phy->of_data->lane_margin)
+		tegra_bpmp_put(phy->bpmp);
+
 	return 0;
 }
 
 static const struct tegra_p2u_of_data tegra194_p2u_of_data = {
 	.one_dir_search = false,
+	.lane_margin = false,
 };
 
 static const struct tegra_p2u_of_data tegra234_p2u_of_data = {
 	.one_dir_search = true,
+	.lane_margin = true,
 };
 
 static const struct of_device_id tegra_p2u_id_table[] = {
@@ -166,6 +437,7 @@ MODULE_DEVICE_TABLE(of, tegra_p2u_id_table);
 
 static struct platform_driver tegra_p2u_driver = {
 	.probe = tegra_p2u_probe,
+	.remove = tegra_p2u_remove,
 	.driver = {
 		.name = "tegra194-p2u",
 		.of_match_table = tegra_p2u_id_table,
-- 
2.25.1