[RFC PATCH] drm/bridge: add support for FEC in mhdp8546 driver

[Damian Kos <dkos@cadence.com>]

From: Przemyslaw Gaj <pgaj@cadence.com>

This patch enables FEC (Forward Error Correction) on Cadence DisplayPort
controller if it's supported by the Sink.

This patch is an addition to patch with mhdp8546 driver @
patchwork.kernel.org/cover/10632065/

Signed-off-by: Przemyslaw Gaj <pgaj@cadence.com>
Signed-off-by: Damian Kos <dkos@cadence.com>
---
 drivers/gpu/drm/bridge/cdns-mhdp.c    | 138 ++++++++++++++++++++++++++
 drivers/gpu/drm/bridge/cdns-mhdp.h    |   4 +-
 include/drm/bridge/cdns-mhdp-common.h |   2 +
 3 files changed, 143 insertions(+), 1 deletion(-)

diff --git a/drivers/gpu/drm/bridge/cdns-mhdp.c b/drivers/gpu/drm/bridge/cdns-mhdp.c
index caaf3c17d74f..3bfe1eda1468 100644
--- a/drivers/gpu/drm/bridge/cdns-mhdp.c
+++ b/drivers/gpu/drm/bridge/cdns-mhdp.c
@@ -392,6 +392,127 @@ static void mhdp_adjust_requested_eq(struct cdns_mhdp_device *mhdp,
 	}
 }
 
+static int cdns_mhdp_wait_for_fec(struct cdns_mhdp_device *mhdp,
+				  bool expected_status)
+{
+	u32 fec_status;
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	cdns_mhdp_reg_read(mhdp, CDNS_DP_FEC_STATUS, &fec_status);
+	while (((fec_status & CDNS_DP_FEC_BUSY) != expected_status) &&
+		 time_before(jiffies, timeout)) {
+		cdns_mhdp_reg_read(mhdp, CDNS_DP_FEC_STATUS, &fec_status);
+		cpu_relax();
+	}
+
+	if (time_after_eq(jiffies, timeout)) {
+		DRM_DEV_ERROR(mhdp->dev, "Timeout while waiting for FEC\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int cdns_mhdp_fec_sink_support(struct cdns_mhdp_device *mhdp)
+{
+	int ret;
+	u16 dpcd_buffer;
+
+	ret = drm_dp_dpcd_read(&mhdp->aux, DP_FEC_CAPABILITY, &dpcd_buffer, 1);
+	if (ret)
+		return ret;
+
+	if (!(dpcd_buffer & DP_FEC_CAPABLE)) {
+		ret = -ENOTSUPP;
+		DRM_DEV_ERROR(mhdp->dev, "sink does not support FEC: %d\n",
+			      ret);
+		goto err;
+	}
+
+	ret = 0;
+
+err:
+	return ret;
+}
+
+static int cdns_mhdp_fec_sink_set_ready(struct cdns_mhdp_device *mhdp,
+					bool enable)
+{
+	int ret;
+	u16 dpcd_buffer;
+
+	ret = drm_dp_dpcd_read(&mhdp->aux, DP_FEC_CONFIGURATION,
+			       &dpcd_buffer, 1);
+	if (ret)
+		goto err;
+
+	if (enable)
+		dpcd_buffer |= DP_FEC_READY;
+	else
+		dpcd_buffer &= ~DP_FEC_READY;
+
+	ret = drm_dp_dpcd_write(&mhdp->aux, DP_FEC_CONFIGURATION,
+				&dpcd_buffer, 1);
+	if (ret)
+		return 0;
+
+err:
+	DRM_DEV_ERROR(mhdp->dev, "cannot set sink FEC ready: %d\n", ret);
+	return -EIO;
+}
+
+static int cdns_mhdp_fec_set_ready(struct cdns_mhdp_device *mhdp, bool enable)
+{
+	int ret;
+
+	ret = cdns_mhdp_fec_sink_support(mhdp);
+	if (ret)
+		goto err;
+
+	ret = cdns_mhdp_fec_sink_set_ready(mhdp, enable);
+	if (ret)
+		goto err;
+
+	ret = cdns_mhdp_reg_write_bit(mhdp, CDNS_DP_FEC_CTRL, 1, 1, enable);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	return ret;
+}
+
+static int cdns_mhdp_fec_enable(struct cdns_mhdp_device *mhdp, bool enable)
+{
+	int ret;
+	u32 resp;
+	unsigned int dp_framer_global_config;
+
+	if (mhdp->fec_enabled == enable) {
+		ret = -EEXIST;
+		goto err;
+	}
+
+	ret = cdns_mhdp_reg_read(mhdp, DP_FRAMER_GLOBAL_CONFIG, &resp);
+	if (ret)
+		goto err;
+
+	dp_framer_global_config = be32_to_cpu(resp);
+	pr_err("0x%.8x\n", dp_framer_global_config);
+	if (!(dp_framer_global_config & CDNS_DP_NO_VIDEO_MODE)) {
+		ret = -EIO;
+		goto err;
+	}
+
+	ret = cdns_mhdp_reg_write_bit(mhdp, CDNS_DP_FEC_CTRL, 0, 1, enable);
+
+	return cdns_mhdp_wait_for_fec(mhdp, enable);
+err:
+	DRM_DEV_ERROR(mhdp->dev, "set fec enable failed: %d\n", ret);
+	return ret;
+}
+
+
 static bool mhdp_link_training_channel_eq(struct cdns_mhdp_device *mhdp,
 					  u8 eq_tps,
 					  unsigned int training_interval)
@@ -769,6 +890,13 @@ static int cdns_mhdp_link_up(struct cdns_mhdp_device *mhdp)
 	amp[1] = DP_SET_ANSI_8B10B;
 	drm_dp_dpcd_write(&mhdp->aux, DP_DOWNSPREAD_CTRL, amp, 2);
 
+	if (cdns_mhdp_fec_set_ready(mhdp, true)) {
+		mhdp->fec_enabled = false;
+		dev_info(mhdp->dev, "Cannot set FEC ready.\n");
+	} else {
+		mhdp->fec_enabled = true;
+	}
+
 	if (mhdp->host.fast_link & mhdp->sink.fast_link) {
 		/* FIXME: implement fastlink */
 		dev_dbg(mhdp->dev, "fastlink\n");
@@ -807,6 +935,14 @@ static int cdns_mhdp_sst_enable(struct drm_bridge *bridge)
 	pxlfmt = cdns_mhdp_get_pxlfmt(disp_info->color_formats);
 	bpp = cdns_mhdp_get_bpp(disp_info->bpc, pxlfmt);
 
+	if (mhdp->fec_enabled && cdns_mhdp_fec_enable(mhdp, true)) {
+		mhdp->fec_enabled = false;
+		dev_info(mhdp->dev, "Cannot enable FEC.\n");
+	} else {
+		mhdp->fec_enabled = true;
+	}
+
+
 	/* find optimal tu_size */
 	required_bandwidth = pxlclock * bpp / 8;
 	available_bandwidth = mhdp->link.num_lanes * rate;
@@ -814,6 +950,8 @@ static int cdns_mhdp_sst_enable(struct drm_bridge *bridge)
 		tu_size += 2;
 
 		vs_f = tu_size * required_bandwidth / available_bandwidth;
+		if (mhdp->fec_enabled)
+			vs_f = (vs_f * 1024) / 1000; // 102.4 percent
 		vs = vs_f / 1000;
 		vs_f = vs_f % 1000;
 		/*
diff --git a/drivers/gpu/drm/bridge/cdns-mhdp.h b/drivers/gpu/drm/bridge/cdns-mhdp.h
index abc1fa3f51a6..ba233adf9fe7 100644
--- a/drivers/gpu/drm/bridge/cdns-mhdp.h
+++ b/drivers/gpu/drm/bridge/cdns-mhdp.h
@@ -179,7 +179,9 @@
 #define CDNS_DP_LANE_EN				(CDNS_DPTX_GLOBAL + 0x00)
 #define CDNS_DP_LANE_EN_LANES(x)		GENMASK(x - 1, 0)
 #define CDNS_DP_ENHNCD				(CDNS_DPTX_GLOBAL + 0x04)
-
+#define CDNS_DP_FEC_CTRL			(CDNS_DPTX_GLOBAL + 0x10)
+#define CDNS_DP_FEC_STATUS			(CDNS_DPTX_GLOBAL + 0x14)
+#define CDNS_DP_FEC_BUSY			BIT(0)
 
 #define to_mhdp_connector(x) container_of(x, struct cdns_mhdp_connector, base)
 #define to_mhdp_bridge(x) container_of(x, struct cdns_mhdp_bridge, base)
diff --git a/include/drm/bridge/cdns-mhdp-common.h b/include/drm/bridge/cdns-mhdp-common.h
index 1e8a44138ce2..52feeca3a914 100644
--- a/include/drm/bridge/cdns-mhdp-common.h
+++ b/include/drm/bridge/cdns-mhdp-common.h
@@ -566,6 +566,8 @@ struct cdns_mhdp_device {
 	bool can_mst;
 	bool link_up;
 	bool plugged;
+
+	bool fec_enabled;
 };
 
 void cdns_mhdp_clock_reset(struct cdns_mhdp_device *mhdp);
-- 
2.17.1