drm/bridge: synopsys: dw-hdmi-qp: Add flt thread for frl cts

Added flt state machine thread to ensure that frl cts can pass:

1.Support txFFE Level switch.
2.Support LTS4.
3.Polling sink frl status after flt pass.

Change-Id: I0d2aa1e8fb5ae39ff3493daf4f2036dffe0817a2
Signed-off-by: Algea Cao <algea.cao@rock-chips.com>

drivers/gpu/drm/bridge/synopsys/dw-hdmi-qp.c

@@ -290,7 +290,7 @@ struct dw_hdmi_qp {
 	bool sink_is_hdmi;
 	bool sink_has_audio;
 	bool dclk_en;
-	bool frl_switch;
+	bool frl_switch;		/* when frl mode switch color and freq is equal set true */
 	bool cec_enable;
 	bool allm_enable;
 	bool support_hdmi;
@@ -313,6 +313,7 @@ struct dw_hdmi_qp {
 
 	bool update;
 	bool hdr2sdr;
+	bool flt_no_timeout;
 
 	u32 scdc_intr;
 	u32 flt_intr;
@@ -347,6 +348,9 @@ struct dw_hdmi_qp {
 	hdmi_codec_plugged_cb plugged_cb;
 	struct device *codec_dev;
 	enum drm_connector_status last_connector_result;
+
+	struct work_struct flt_work;
+	struct workqueue_struct *workqueue;
 };
 
 static inline void hdmi_writel(struct dw_hdmi_qp *hdmi, u32 val, int offset)
@@ -1841,19 +1845,90 @@ static int hdmi_set_frl_mask(int frl_rate)
 	return 0;
 }
 
-static int hdmi_start_flt(struct dw_hdmi_qp *hdmi, u8 rate)
+static int hdmi_set_frl_actual(int frl_level)
 {
-	u8 val;
+	switch (frl_level) {
-	u32 value;
+	case FRL_12GBPS_4LANE:
-	u8 ffe_lv = 0;
+		return 48;
-	int i = 0;
+	case FRL_10GBPS_4LANE:
-	bool ltsp = false;
+		return 40;
+	case FRL_8GBPS_4LANE:
+		return 32;
+	case FRL_6GBPS_4LANE:
+		return 24;
+	case FRL_6GBPS_3LANE:
+		return 18;
+	case FRL_3GBPS_3LANE:
+		return 9;
+	}
 
-	hdmi_modb(hdmi, AVP_DATAPATH_VIDEO_SWDISABLE,
+	return 0;
-		  AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
+}
 
-	/* reset avp data path */
+enum flt_state {
-	hdmi_writel(hdmi, BIT(6), GLOBAL_SWRESET_REQUEST);
+	LTS1 = 0,	/* Read edid */
+	LTS2,		/* Prepare for frl */
+	LTS3,		/* Training in progress */
+	LTS4,		/* Update frl_rate */
+	LTSP,		/* Training passed */
+	LTSL,		/* Exit frl mode */
+};
+
+static bool dw_hdmi_qp_is_disabled(struct dw_hdmi_qp *hdmi)
+{
+	if (hdmi->phy.ops->read_hpd(hdmi, hdmi->phy.data) == connector_status_disconnected)
+		return true;
+
+	mutex_lock(&hdmi->mutex);
+	if (hdmi->disabled) {
+		mutex_unlock(&hdmi->mutex);
+		return true;
+	}
+	mutex_unlock(&hdmi->mutex);
+
+	return false;
+}
+
+/* check sink version and if flt no timeout mode */
+static int dw_hdmi_qp_flt_lts1(struct dw_hdmi_qp *hdmi)
+{
+	u8 val = 0;
+
+	mutex_lock(&hdmi->audio_mutex);
+	if (hdmi->dclk_en) {
+		hdmi_modb(hdmi, AVP_DATAPATH_VIDEO_SWDISABLE,
+			  AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
+
+		/* reset avp data path */
+		hdmi_writel(hdmi, BIT(6), GLOBAL_SWRESET_REQUEST);
+	} else {
+		dev_err(hdmi->dev, "hdmi dclk is disabled, lts1 failed\n");
+		mutex_unlock(&hdmi->audio_mutex);
+		return LTSL;
+	}
+	mutex_unlock(&hdmi->audio_mutex);
+
+	drm_scdc_readb(hdmi->ddc, SCDC_SINK_VERSION, &val);
+	if (!val) {
+		dev_err(hdmi->dev, "scdc sink version is zero, lts1 failed\n");
+		return LTSL;
+	}
+	drm_scdc_writeb(hdmi->ddc, SCDC_SOURCE_VERSION, 1);
+	drm_scdc_readb(hdmi->ddc, SCDC_SOURCE_TEST_CONFIG, &val);
+	if (val & BIT(5))
+		hdmi->flt_no_timeout = true;
+	else
+		hdmi->flt_no_timeout = false;
+
+	return LTS2;
+}
+
+/* check if sink is ready to training and set source output frl rate/max ffe level */
+static int dw_hdmi_qp_flt_lts2(struct dw_hdmi_qp *hdmi, u8 rate)
+{
+	int i;
+	u8 val = 0;
+	u8 flt_rate = hdmi_set_frl_mask(rate);
 
 	/* FLT_READY & FFE_LEVELS read */
 	for (i = 0; i < 20; i++) {
@@ -1864,75 +1939,296 @@ static int hdmi_start_flt(struct dw_hdmi_qp *hdmi, u8 rate)
 	}
 
 	if (i == 20) {
-		dev_err(hdmi->dev, "sink flt isn't ready\n");
+		dev_err(hdmi->dev, "sink flt isn't ready,SCDC_STATUS_FLAGS_0:0x%x\n", val);
-		return -EINVAL;
+		return LTSL;
 	}
 
-	/* clear flt flags */
-	drm_scdc_readb(hdmi->ddc, 0x10, &val);
-	if (val & BIT(5))
-		drm_scdc_writeb(hdmi->ddc, 0x10, BIT(5));
-
 	/* max ffe level 3 */
-	val = 0 << 4 | hdmi_set_frl_mask(rate);
+	val = 3 << 4 | flt_rate;
-	drm_scdc_writeb(hdmi->ddc, 0x31, val);
+	drm_scdc_writeb(hdmi->ddc, SCDC_CONFIG_1, val);
+	drm_scdc_writeb(hdmi->ddc, SCDC_CONFIG_0, 0);
 
-	/* select FRL_RATE & FFE_LEVELS */
+	return LTS3;
-	hdmi_writel(hdmi, ffe_lv, FLT_CONFIG0);
+}
+
+static void dw_hdmi_qp_set_ltp(struct dw_hdmi_qp *hdmi, u32 value, bool flt_no_timeout)
+{
+	/* support hfr1-10, send old ltp when all lane is 3 */
+	if (!flt_no_timeout && value == 0x3333f)
+		value = hdmi_readl(hdmi, FLT_CONFIG1);
+
+	hdmi_writel(hdmi, value, FLT_CONFIG1);
+}
+
+/*
+ * conducts link training for the specified frl rate
+ * send sink request ltp or change ffe level
+ */
+static int dw_hdmi_qp_flt_lts3(struct dw_hdmi_qp *hdmi, u8 rate)
+{
+	u8 val;
+	int i = 0, ret = 0;
+	u8 src_test_cfg = 0;
+	u32 value;
+	u8 ffe_lv = 0;
 
 	/* we set max 2s timeout */
 	i = 4000;
-	while (i--) {
+	while (i > 0 || hdmi->flt_no_timeout) {
+		if (dw_hdmi_qp_is_disabled(hdmi)) {
+			dev_info(hdmi->dev, "hdmi dclk is disabled, stop flt\n");
+			break;
+		}
+
+		i--;
 		/* source should poll update flag every 2ms or less */
 		usleep_range(400, 500);
-		drm_scdc_readb(hdmi->ddc, 0x10, &val);
 
-		if (!(val & 0x30))
+		drm_scdc_readb(hdmi->ddc, SCDC_UPDATE_0, &val);
-			continue;
 
-		if (val & BIT(5)) {
+		/* SOURCE_TEST_UPDATE */
-			u8 reg_val, ln0, ln1, ln2, ln3;
+		if (val & BIT(3)) {
-
+			/* quit test mode */
-			drm_scdc_readb(hdmi->ddc, 0x41, &reg_val);
+			drm_scdc_readb(hdmi->ddc, SCDC_SOURCE_TEST_CONFIG, &src_test_cfg);
-			ln0 = reg_val & 0xf;
+			if (hdmi->flt_no_timeout && !(src_test_cfg & BIT(5))) {
-			ln1 = (reg_val >> 4) & 0xf;
+				DRM_DEV_DEBUG_DRIVER(hdmi->dev, "flt get out of test mode\n");
-
+				hdmi->flt_no_timeout = false;
-			drm_scdc_readb(hdmi->ddc, 0x42, &reg_val);
+			} else if (!hdmi->flt_no_timeout && (src_test_cfg & BIT(5))) {
-			ln2 = reg_val & 0xf;
+				DRM_DEV_DEBUG_DRIVER(hdmi->dev, "flt go into test mode\n");
-			ln3 = (reg_val >> 4) & 0xf;
+				hdmi->flt_no_timeout = true;
-
-			if (!ln0 && !ln1 && !ln2 && !ln3) {
-				dev_info(hdmi->dev, "goto ltsp\n");
-				ltsp = true;
-				hdmi_writel(hdmi, 0, FLT_CONFIG1);
-			} else if ((ln0 == 0xf) | (ln1 == 0xf) | (ln2 == 0xf) | (ln3 == 0xf)) {
-				dev_err(hdmi->dev, "goto lts4\n");
-				break;
-			} else if ((ln0 == 0xe) | (ln1 == 0xe) | (ln2 == 0xe) | (ln3 == 0xe)) {
-				dev_info(hdmi->dev, "goto ffe\n");
-				break;
-			} else {
-				value = (ln3 << 16) | (ln2 << 12) | (ln1 << 8) | (ln0 << 4) | 0xf;
-				hdmi_writel(hdmi, value, FLT_CONFIG1);
 			}
 		}
 
-		/* only clear frl_start and flt_update */
+		if (!(val & SCDC_CONFIG_0)) {
-		drm_scdc_writeb(hdmi->ddc, 0x10, val & 0x30);
+			/* clear SOURCE_TEST_UPDATE flag */
-
+			if (val & BIT(3))
-		if ((val & BIT(4)) && ltsp) {
+				drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, val);
-			hdmi_modb(hdmi, 0, AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
+			continue;
-			dev_info(hdmi->dev, "flt success\n");
-			break;
 		}
+
+		/* flt_update */
+		if (val & BIT(5)) {
+			u8 reg_val, ln0, ln1, ln2, ln3;
+
+			drm_scdc_readb(hdmi->ddc, SCDC_STATUS_FLAGS_1, &reg_val);
+			ln0 = reg_val & 0xf;
+			ln1 = (reg_val >> 4) & 0xf;
+
+			drm_scdc_readb(hdmi->ddc, SCDC_STATUS_FLAGS_2, &reg_val);
+			ln2 = reg_val & 0xf;
+			ln3 = (reg_val >> 4) & 0xf;
+
+			DRM_DEV_DEBUG_DRIVER(hdmi->dev, "ln0:0x%x,ln1:0x%x,ln2:0x%x,ln3:0x%x\n",
+					     ln0, ln1, ln2, ln3);
+
+			if (!ln0 && !ln1 && !ln2 && !ln3) {
+				dev_info(hdmi->dev, "Training finish, go to ltsp\n");
+				mutex_lock(&hdmi->audio_mutex);
+				if (hdmi->dclk_en) {
+					hdmi_writel(hdmi, 0, FLT_CONFIG1);
+					ret = LTSP;
+				} else {
+					dev_err(hdmi->dev, "hdmi dclk is disabled, goto ltsp failed\n");
+					ret = LTSL;
+				}
+				mutex_unlock(&hdmi->audio_mutex);
+			} else if ((ln0 == 0xf) | (ln1 == 0xf) | (ln2 == 0xf) | (ln3 == 0xf)) {
+				dev_err(hdmi->dev, "goto lts4\n");
+				ret = LTS4;
+			} else if ((ln0 == 0xe) | (ln1 == 0xe) | (ln2 == 0xe) | (ln3 == 0xe)) {
+				dev_info(hdmi->dev, "goto ffe\n");
+				if (ffe_lv < 3) {
+					hdmi->phy.ops->set_ffe(hdmi, hdmi->phy.data, ++ffe_lv);
+				} else {
+					dev_err(hdmi->dev, "ffe level out of range\n");
+					ret = LTSL;
+				}
+			} else {
+				mutex_lock(&hdmi->audio_mutex);
+				if (hdmi->dclk_en) {
+					value = (ln3 << 16) | (ln2 << 12) | (ln1 << 8) |
+						(ln0 << 4) | 0xf;
+
+					dw_hdmi_qp_set_ltp(hdmi, value, hdmi->flt_no_timeout);
+				} else {
+					dev_err(hdmi->dev, "hdmi dclk is disabled, set ltp failed\n");
+					ret = LTSL;
+				}
+				mutex_unlock(&hdmi->audio_mutex);
+			}
+
+			/* only clear flt_update */
+			drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, val);
+		}
+
+		if (ret)
+			break;
+	}
+
+	if (!ret) {
+		ret = LTSL;
+		dev_err(hdmi->dev, "lts3 time out, goto ltsl\n");
+	}
+
+	return ret;
+}
+
+/* sink request frl rate change, start training for a new rate. */
+static int dw_hdmi_qp_flt_lts4(struct dw_hdmi_qp *hdmi, u8 *rate)
+{
+	u8 actual_rate;
+	void *data = hdmi->plat_data->phy_data;
+	u8 flt_rate = hdmi_set_frl_mask(*rate);
+
+	/* we don't use frl rate below 24G */
+	if (flt_rate == FRL_8GBPS_4LANE) {
+		dev_err(hdmi->dev, "goto ltsl\n");
+		return LTSL;
+	}
+
+	/* disable phy */
+	hdmi->phy.ops->disable(hdmi, hdmi->phy.data);
+	if (hdmi->plat_data->link_clk_set)
+		hdmi->plat_data->link_clk_set(data, false);
+
+	/* set lower frl rate */
+	flt_rate--;
+	actual_rate = hdmi_set_frl_actual(flt_rate);
+	if (hdmi->plat_data->force_frl_rate)
+		hdmi->plat_data->force_frl_rate(data, actual_rate);
+
+	if (hdmi->plat_data->link_clk_set)
+		hdmi->plat_data->link_clk_set(data, true);
+
+	/* enable phy */
+	hdmi->phy.ops->init(hdmi, hdmi->phy.data, &hdmi->previous_mode);
+
+	*rate = actual_rate;
+	/* set new rate */
+	drm_scdc_writeb(hdmi->ddc, SCDC_CONFIG_1, (3 << 4 | flt_rate));
+	drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, BIT(5));
+
+	dev_info(hdmi->dev, "from lts4 go to lts3\n");
+	return LTS3;
+}
+
+/* training is passed, start poll sink check if sink want to change rate or exit frl mode */
+static int dw_hdmi_qp_flt_ltsp(struct dw_hdmi_qp *hdmi)
+{
+	u8 val = 0;
+	int i = 4000;
+
+	/* wait frl start */
+	while (i--) {
+		if (dw_hdmi_qp_is_disabled(hdmi)) {
+			dev_info(hdmi->dev, "hdmi dclk is disabled, quit ltsp\n");
+			return LTSL;
+		}
+
+		/* source should poll update flag every 2ms or less */
+		usleep_range(400, 500);
+
+		drm_scdc_readb(hdmi->ddc, SCDC_UPDATE_0, &val);
+
+		if (!(val & SCDC_CONFIG_0))
+			continue;
+
+		mutex_lock(&hdmi->audio_mutex);
+		if (hdmi->dclk_en) {
+			/* flt_start */
+			if (val & BIT(4)) {
+				hdmi_modb(hdmi, 0, AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
+				/* clear flt_start */
+				drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, BIT(4));
+				hdmi_writel(hdmi, 2, PKTSCHED_PKT_CONTROL0);
+				hdmi_modb(hdmi, PKTSCHED_GCP_TX_EN, PKTSCHED_GCP_TX_EN,
+					  PKTSCHED_PKT_EN);
+				dev_info(hdmi->dev, "flt success\n");
+				mutex_unlock(&hdmi->audio_mutex);
+				break;
+			} else if (val & BIT(5)) {
+				hdmi_modb(hdmi, AVP_DATAPATH_VIDEO_SWDISABLE,
+					  AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
+				drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, BIT(5));
+				mutex_unlock(&hdmi->audio_mutex);
+				return LTS3;
+			}
+		} else {
+			mutex_unlock(&hdmi->audio_mutex);
+			dev_err(hdmi->dev, "hdmi dclk is disabled, wait frl start failed\n");
+			return LTSL;
+		}
+		mutex_unlock(&hdmi->audio_mutex);
 	}
 
 	if (i < 0) {
-		dev_err(hdmi->dev, "flt time out\n");
+		dev_err(hdmi->dev, "wait flt_start or flt_update time out, SCDC_UPDATE_0:0x%x\n",
-		return -ETIMEDOUT;
+			val);
+		return LTSL;
 	}
 
-	return 0;
+	/*
+	 * For compatibility with tv changhong d8k no signal issue,
+	 * not hdmi protocol requirements.
+	 */
+	for (i = 0; i < 200; i++) {
+		hdmi_modb(hdmi, PKTSCHED_NULL_TX_EN, PKTSCHED_NULL_TX_EN, PKTSCHED_PKT_EN);
+		usleep_range(50, 60);
+		hdmi_modb(hdmi, 0, PKTSCHED_NULL_TX_EN, PKTSCHED_PKT_EN);
+		usleep_range(50, 60);
+	}
+
+	i = 5;
+	/* flt success poll flt_update */
+	while (1) {
+		if (dw_hdmi_qp_is_disabled(hdmi)) {
+			dev_info(hdmi->dev, "hdmi dclk is disabled, stop poll flt_update\n");
+			return LTSL;
+		}
+
+		if (!i) {
+			i = 5;
+			drm_scdc_readb(hdmi->ddc, SCDC_UPDATE_0, &val);
+
+			mutex_lock(&hdmi->audio_mutex);
+			if (hdmi->dclk_en) {
+				if (val & BIT(5)) {
+					hdmi_writel(hdmi, 1, PKTSCHED_PKT_CONTROL0);
+					hdmi_modb(hdmi, PKTSCHED_GCP_TX_EN, PKTSCHED_GCP_TX_EN,
+						  PKTSCHED_PKT_EN);
+					msleep(50);
+					hdmi_modb(hdmi, AVP_DATAPATH_VIDEO_SWDISABLE,
+						AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
+					drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, BIT(5));
+					mutex_unlock(&hdmi->audio_mutex);
+					return LTS2;
+				}
+			} else {
+				mutex_unlock(&hdmi->audio_mutex);
+				dev_info(hdmi->dev,
+					 "hdmi is disconnected, stop poll flt update flag\n");
+				return LTSL;
+			}
+			mutex_unlock(&hdmi->audio_mutex);
+		}
+		/* after flt success source should poll update_flag at least once per 250ms */
+		msleep(20);
+		i--;
+	}
+
+	return LTSL;
+}
+
+/* exit frl mode, maybe it was a training failure or hdmi was disabled */
+static int dw_hdmi_qp_flt_ltsl(struct dw_hdmi_qp *hdmi)
+{
+	if (hdmi->frl_switch)
+		return -EINVAL;
+
+	drm_scdc_writeb(hdmi->ddc, SCDC_CONFIG_1, 0);
+	drm_scdc_writeb(hdmi->ddc, SCDC_UPDATE_0, BIT(5));
+
+	return -EINVAL;
 }
 
 #define HDMI_MODE_FRL_MASK     BIT(30)
@@ -1941,8 +2237,7 @@ static int hdmi_set_op_mode(struct dw_hdmi_qp *hdmi,
 			    struct dw_hdmi_link_config *link_cfg,
 			    const struct drm_connector *connector)
 {
-	int frl_rate;
+	int ret = 0;
-	int i, ret = 0;
 
 	if (hdmi->frl_switch)
 		return 0;
@@ -1971,31 +2266,14 @@ static int hdmi_set_op_mode(struct dw_hdmi_qp *hdmi,
 
 	hdmi_modb(hdmi, 1, OPMODE_FRL, LINK_CONFIG0);
 
-	frl_rate = link_cfg->frl_lanes * link_cfg->rate_per_lane;
-
 	ret = hdmi->phy.ops->init(hdmi, hdmi->phy.data, &hdmi->previous_mode);
-	if (ret)
+	if (!ret) {
-		return ret;
+		hdmi->disabled = false;
-	hdmi->disabled = false;
+		/* wait phy output stable then start flt */
-
+		msleep(50);
-	msleep(50);
-
-	ret = hdmi_start_flt(hdmi, frl_rate);
-	if (ret) {
-		hdmi_writel(hdmi, 0, FLT_CONFIG0);
-		drm_scdc_writeb(hdmi->ddc, 0x31, 0);
-		hdmi_modb(hdmi, 0, AVP_DATAPATH_VIDEO_SWDISABLE, GLOBAL_SWDISABLE);
-		return ret;
 	}
 
-	for (i = 0; i < 200; i++) {
+	return ret;
-		hdmi_modb(hdmi, PKTSCHED_NULL_TX_EN, PKTSCHED_NULL_TX_EN, PKTSCHED_PKT_EN);
-		usleep_range(50, 60);
-		hdmi_modb(hdmi, 0, PKTSCHED_NULL_TX_EN, PKTSCHED_PKT_EN);
-		usleep_range(50, 60);
-	}
-
-	return 0;
 }
 
 static unsigned long
@@ -2102,6 +2380,48 @@ static void dw_hdmi_qp_hdcp_enable(struct dw_hdmi_qp *hdmi,
 	}
 }
 
+static void dw_hdmi_qp_flt_work(struct work_struct *p_work)
+{
+	struct dw_hdmi_qp *hdmi = container_of(p_work, struct dw_hdmi_qp, flt_work);
+	void *data = hdmi->plat_data->phy_data;
+	struct dw_hdmi_link_config *link_cfg = hdmi->plat_data->get_link_cfg(data);
+	u8 frl_rate;
+	int state = LTS1;
+
+	if (hdmi->frl_switch)
+		return;
+
+	frl_rate = link_cfg->frl_lanes * link_cfg->rate_per_lane;
+
+	while (1) {
+		switch (state) {
+		case LTS1:
+			state = dw_hdmi_qp_flt_lts1(hdmi);
+			break;
+		case LTS2:
+			state = dw_hdmi_qp_flt_lts2(hdmi, frl_rate);
+			break;
+		case LTS3:
+			state = dw_hdmi_qp_flt_lts3(hdmi, frl_rate);
+			break;
+		case LTS4:
+			state = dw_hdmi_qp_flt_lts4(hdmi, &frl_rate);
+			break;
+		case LTSP:
+			state = dw_hdmi_qp_flt_ltsp(hdmi);
+			break;
+		case LTSL:
+			state = dw_hdmi_qp_flt_ltsl(hdmi);
+			break;
+		default:
+			dev_err(hdmi->dev, "flt failed\n");
+		}
+
+		if (state <= 0)
+			break;
+	}
+}
+
 static int dw_hdmi_qp_setup(struct dw_hdmi_qp *hdmi,
 			    const struct drm_connector *connector,
 			    struct drm_display_mode *mode)
@@ -3106,9 +3426,9 @@ static void dw_hdmi_qp_bridge_atomic_disable(struct drm_bridge *bridge,
 	if (hdmi->phy.ops->disable && !hdmi->frl_switch) {
 		hdmi_writel(hdmi, 0, FLT_CONFIG0);
 		hdmi_writel(hdmi, 0, SCRAMB_CONFIG0);
-		/* set sink frl mode disable */
+
-		if (dw_hdmi_support_scdc(hdmi, &hdmi->curr_conn->display_info))
+		if (hdmi->plat_data->force_frl_rate)
-			drm_scdc_writeb(hdmi->ddc, 0x31, 0);
+			hdmi->plat_data->force_frl_rate(data, 0);
 
 		hdmi->phy.ops->disable(hdmi, hdmi->phy.data);
 		hdmi->disabled = true;
@@ -3119,6 +3439,9 @@ static void dw_hdmi_qp_bridge_atomic_disable(struct drm_bridge *bridge,
 	hdmi->curr_conn = NULL;
 	mutex_unlock(&hdmi->mutex);
 
+	cancel_work_sync(&hdmi->flt_work);
+	flush_workqueue(hdmi->workqueue);
+
 	if (hdmi->panel)
 		drm_panel_unprepare(hdmi->panel);
 }
@@ -3130,6 +3453,10 @@ static void dw_hdmi_qp_bridge_atomic_enable(struct drm_bridge *bridge,
 	struct drm_atomic_state *state = old_state->base.state;
 	struct drm_connector *connector;
 	void *data = hdmi->plat_data->phy_data;
+	struct dw_hdmi_link_config *link_cfg = NULL;
+
+	if (hdmi->plat_data->get_link_cfg)
+		link_cfg = hdmi->plat_data->get_link_cfg(data);
 
 	if (hdmi->panel)
 		drm_panel_prepare(hdmi->panel);
@@ -3141,8 +3468,11 @@ static void dw_hdmi_qp_bridge_atomic_enable(struct drm_bridge *bridge,
 	hdmi->curr_conn = connector;
 
 	dw_hdmi_qp_setup(hdmi, hdmi->curr_conn, &hdmi->previous_mode);
-	hdmi_writel(hdmi, 2, PKTSCHED_PKT_CONTROL0);
+
-	hdmi_modb(hdmi, PKTSCHED_GCP_TX_EN, PKTSCHED_GCP_TX_EN, PKTSCHED_PKT_EN);
+	if ((link_cfg && !link_cfg->frl_mode) || hdmi->frl_switch) {
+		hdmi_writel(hdmi, 2, PKTSCHED_PKT_CONTROL0);
+		hdmi_modb(hdmi, PKTSCHED_GCP_TX_EN, PKTSCHED_GCP_TX_EN, PKTSCHED_PKT_EN);
+	}
 	mutex_unlock(&hdmi->mutex);
 
 	if (!hdmi->dclk_en) {
@@ -3152,6 +3482,10 @@ static void dw_hdmi_qp_bridge_atomic_enable(struct drm_bridge *bridge,
 		hdmi->dclk_en = true;
 		mutex_unlock(&hdmi->audio_mutex);
 	}
+
+	if (link_cfg && link_cfg->frl_mode)
+		queue_work(hdmi->workqueue, &hdmi->flt_work);
+
 	dw_hdmi_qp_init_audio_infoframe(hdmi);
 	dw_hdmi_qp_audio_enable(hdmi);
 	hdmi_clk_regenerator_update_pixel_clock(hdmi);
@@ -4108,6 +4442,9 @@ static struct dw_hdmi_qp *dw_hdmi_qp_probe(struct platform_device *pdev,
 		}
 	}
 
+	hdmi->workqueue = create_workqueue("dw_hdmi_flt_queue");
+	INIT_WORK(&hdmi->flt_work, dw_hdmi_qp_flt_work);
+
 	return hdmi;
 
 err_ddc:
@@ -4129,6 +4466,10 @@ err_ddc:
 
 static void dw_hdmi_qp_remove(struct dw_hdmi_qp *hdmi)
 {
+	cancel_work_sync(&hdmi->flt_work);
+	flush_workqueue(hdmi->workqueue);
+	destroy_workqueue(hdmi->workqueue);
+
 	if (hdmi->avp_irq)
 		disable_irq(hdmi->avp_irq);
 
@@ -4241,7 +4582,8 @@ void dw_hdmi_qp_suspend(struct device *dev, struct dw_hdmi_qp *hdmi)
 		disable_irq(hdmi->earc_irq);
 
 	pinctrl_pm_select_sleep_state(dev);
-	drm_connector_update_edid_property(&hdmi->connector, NULL);
+	if (!hdmi->next_bridge)
+		drm_connector_update_edid_property(&hdmi->connector, NULL);
 }
 EXPORT_SYMBOL_GPL(dw_hdmi_qp_suspend);
 

drivers/gpu/drm/bridge/synopsys/dw-hdmi-qp.h

@@ -853,4 +853,8 @@
 #define HDMI_HDCP14_MEM_M0_1				0x5960
 #define HDMI_HDCP14_MEM_M0_7				0x597c
 
+#define SCDC_CONFIG_1					0x31
+#define SCDC_SOURCE_TEST_CONFIG				0x35
+#define SCDC_STATUS_FLAGS_2				0x42
+
 #endif /* __DW_HDMI_QP_H__ */

drivers/gpu/drm/rockchip/dw_hdmi-rockchip.c

@@ -298,6 +298,7 @@ struct rockchip_hdmi {
 	enum rk_if_color_format hdmi_output;
 	struct rockchip_drm_sub_dev sub_dev;
 
+	u64 force_frl_rate;
 	u8 max_frl_rate_per_lane;
 	u8 max_lanes;
 	u8 add_func;
@@ -1415,7 +1416,12 @@ static irqreturn_t rk3576_hdmi_thread(int irq, void *dev_id)
 
 	if (stat) {
 		hdmi->hpd_stat = true;
-		msecs = 150;
+		/*
+		 * Responding to hpd too early will cause the hdmi frl
+		 * hfr1-10 test to fail. The ln3 value of TE should have
+		 * been 3 but changed to 0.
+		 */
+		msecs = 450;
 	} else {
 		hdmi->hpd_stat = false;
 		msecs = 20;
@@ -2565,6 +2571,45 @@ dw_hdmi_rockchip_check_color(struct drm_connector_state *conn_state,
 		return false;
 }
 
+static int
+dw_hdmi_get_lane_cfg_by_frl_rate(struct rockchip_hdmi *hdmi, u64 rate,
+				 u8 *rate_per_lane, u8 *lanes)
+{
+	switch (rate) {
+	case 48000000:
+		*rate_per_lane = 12;
+		*lanes = 4;
+		break;
+	case 40000000:
+		*rate_per_lane = 10;
+		*lanes = 4;
+		break;
+	case 32000000:
+		*rate_per_lane = 8;
+		*lanes = 4;
+		break;
+	case 24000000:
+		*rate_per_lane = 6;
+		*lanes = 4;
+		break;
+	case 18000000:
+		*rate_per_lane = 6;
+		*lanes = 3;
+		break;
+	case 9000000:
+		*rate_per_lane = 3;
+		*lanes = 3;
+		break;
+	default:
+		dev_err(hdmi->dev, "%s frl rate is out of range, set to 40G\n", __func__);
+		*rate_per_lane = 10;
+		*lanes = 4;
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int
 dw_hdmi_rockchip_encoder_atomic_check(struct drm_encoder *encoder,
 				      struct drm_crtc_state *crtc_state,
@@ -2603,10 +2648,22 @@ secondary:
 		hdmi_select_link_config(hdmi, crtc_state, tmdsclk);
 
 		if (hdmi->link_cfg.frl_mode) {
-			/* in the current version, support max 40G frl */
+			/* if current frl training failed, set lower frl rate */
-			if (hdmi->link_cfg.rate_per_lane >= 10) {
+			if (hdmi->force_frl_rate) {
+				u8 rate_per_lane, frl_lanes;
+
+				dw_hdmi_get_lane_cfg_by_frl_rate(hdmi, hdmi->force_frl_rate,
+								 &rate_per_lane, &frl_lanes);
+
+				if (hdmi->link_cfg.rate_per_lane > rate_per_lane)
+					hdmi->link_cfg.rate_per_lane = rate_per_lane;
+
+				if (hdmi->link_cfg.frl_lanes > frl_lanes)
+					hdmi->link_cfg.frl_lanes = frl_lanes;
+			/* 40G is preferred */
+			} else if (hdmi->link_cfg.rate_per_lane >= 12) {
 				hdmi->link_cfg.frl_lanes = 4;
-				hdmi->link_cfg.rate_per_lane = 10;
+				hdmi->link_cfg.rate_per_lane = 12;
 			}
 			bus_width = hdmi->link_cfg.frl_lanes *
 				hdmi->link_cfg.rate_per_lane * 1000000;
@@ -2985,6 +3042,22 @@ static u32 dw_hdmi_rockchip_get_refclk_rate(void *data)
 	return clk_get_rate(hdmi->hdmitx_ref);
 }
 
+static void dw_hdmi_rockchip_force_frl_rate(void *data, u8 rate)
+{
+	struct rockchip_hdmi *hdmi = (struct rockchip_hdmi *)data;
+	u64 bus_width = rate * 1000000;
+
+	if (!bus_width) {
+		hdmi->force_frl_rate = 0;
+		return;
+	}
+	hdmi->phy_bus_width &= ~DATA_RATE_MASK;
+	hdmi->phy_bus_width |= bus_width;
+	hdmi->force_frl_rate = bus_width;
+
+	phy_set_bus_width(hdmi->phy, hdmi->phy_bus_width);
+}
+
 static const struct drm_prop_enum_list color_depth_enum_list[] = {
 	{ 0, "Automatic" }, /* Prefer highest color depth */
 	{ 8, "24bit" },
@@ -3666,6 +3739,7 @@ static void dw_hdmi_rk3576_setup_hpd(struct dw_hdmi_qp *dw_hdmi, void *data)
 	      HIWORD_UPDATE(0, RK3576_HDMITX_HPD_INT_MSK);
 
 	regmap_write(hdmi->regmap, RK3576_IOC_MISC_CON0, val);
+	regmap_write(hdmi->regmap, 0xa404, 0xffff0102);
 }
 
 static void rk3588_io_path_init(struct rockchip_hdmi *hdmi)
@@ -3771,6 +3845,16 @@ static void dw_hdmi_rk3588_phy_set_mode(struct dw_hdmi_qp *dw_hdmi, void *data,
 	phy_set_bus_width(hdmi->phy, hdmi->phy_bus_width);
 }
 
+static void dw_hdmi_qp_rockchip_phy_set_ffe(struct dw_hdmi_qp *dw_hdmi, void *data, u8 ffe)
+{
+	struct rockchip_hdmi *hdmi = (struct rockchip_hdmi *)data;
+
+	if (!hdmi->phy)
+		return;
+
+	phy_set_mode_ext(hdmi->phy, 0, ffe);
+}
+
 static const struct dw_hdmi_phy_ops rk3228_hdmi_phy_ops = {
 	.init		= dw_hdmi_rockchip_genphy_init,
 	.disable	= dw_hdmi_rockchip_genphy_disable,
@@ -3926,6 +4010,7 @@ static const struct dw_hdmi_qp_phy_ops rk3576_hdmi_phy_ops = {
 	.read_hpd	= dw_hdmi_rk3576_read_hpd,
 	.setup_hpd	= dw_hdmi_rk3576_setup_hpd,
 	.set_mode       = dw_hdmi_rk3588_phy_set_mode,
+	.set_ffe        = dw_hdmi_qp_rockchip_phy_set_ffe,
 };
 
 static const struct rockchip_hdmi_chip_ops rk3576_hdmi_chip_ops = {
@@ -3962,6 +4047,7 @@ static const struct dw_hdmi_qp_phy_ops rk3588_hdmi_phy_ops = {
 	.read_hpd	= dw_hdmi_rk3588_read_hpd,
 	.setup_hpd	= dw_hdmi_rk3588_setup_hpd,
 	.set_mode       = dw_hdmi_rk3588_phy_set_mode,
+	.set_ffe        = dw_hdmi_qp_rockchip_phy_set_ffe,
 };
 
 static const struct rockchip_hdmi_chip_ops rk3588_hdmi_chip_ops = {
@@ -4096,6 +4182,8 @@ static int dw_hdmi_rockchip_bind(struct device *dev, struct device *master,
 		dw_hdmi_rockchip_get_force_timing;
 	plat_data->get_refclk_rate =
 		dw_hdmi_rockchip_get_refclk_rate;
+	plat_data->force_frl_rate =
+		dw_hdmi_rockchip_force_frl_rate;
 	plat_data->property_ops = &dw_hdmi_rockchip_property_ops;
 
 	secondary = rockchip_hdmi_find_by_id(dev->driver, !hdmi->id);

include/drm/bridge/dw_hdmi.h

@@ -164,6 +164,7 @@ struct dw_hdmi_qp_phy_ops {
 	void (*setup_hpd)(struct dw_hdmi_qp *hdmi, void *data);
 	void (*set_mode)(struct dw_hdmi_qp *dw_hdmi, void *data,
 			 u32 mode_mask, bool enable);
+	void (*set_ffe)(struct dw_hdmi_qp *dw_hdmi, void *data, u8 ffe);
 };
 
 struct dw_hdmi_property_ops {
@@ -271,6 +272,7 @@ struct dw_hdmi_plat_data {
 	void (*set_hdcp14_mem)(void *data, bool enable);
 	struct drm_display_mode *(*get_force_timing)(void *data);
 	u32 (*get_refclk_rate)(void *data);
+	void (*force_frl_rate)(void *data, u8 rate);
 
 	/* Vendor Property support */
 	const struct dw_hdmi_property_ops *property_ops;