drm/msm/dp: add extra 100ms sleep when MSTM_CTRL is not cleared

Add extra sleep between SST and MST mode if MSTM_CTRL is not cleared
at hpd to make sure dongle exit from previous MST state.

CRs-Fixed: 2411911
Change-Id: I5b1785c71a91df95167bf5083e9d752a275872a8
Signed-off-by: Xiaowen Wu <[email protected]>
Signed-off-by: Fuad Hossain <[email protected]>

msm/dp/dp_display.c

@@ -635,6 +635,8 @@ static void dp_display_process_mst_hpd_high(struct dp_display_private *dp,
 {
 	bool is_mst_receiver;
 	struct dp_mst_hpd_info info;
+	const unsigned long clear_mstm_ctrl_timeout_us = 100000;
+	u8 old_mstm_ctrl;
 	int ret;
 
 	if (!dp->parser->has_mst || !dp->mst.drm_registered) {
@@ -654,8 +656,18 @@ static void dp_display_process_mst_hpd_high(struct dp_display_private *dp,
 		}
 
 		/* clear sink mst state */
+		drm_dp_dpcd_readb(dp->aux->drm_aux, DP_MSTM_CTRL,
+				&old_mstm_ctrl);
 		drm_dp_dpcd_writeb(dp->aux->drm_aux, DP_MSTM_CTRL, 0);
 
+		/* add extra delay if MST state is not cleared */
+		if (old_mstm_ctrl) {
+			DP_MST_DEBUG("MSTM_CTRL is not cleared, wait %dus\n",
+					clear_mstm_ctrl_timeout_us);
+			usleep_range(clear_mstm_ctrl_timeout_us,
+				clear_mstm_ctrl_timeout_us + 1000);
+		}
+
 		ret = drm_dp_dpcd_writeb(dp->aux->drm_aux, DP_MSTM_CTRL,
 				 DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
 		if (ret < 0) {