Implement proper MST fallback retraining in i915

index 9a4a51e79fa1..4dd1b2287dd6 100644
@@ -43,7 +43,6 @@
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
 
-#define DP_LINK_CHECK_TIMEOUT	(10 * 1000)
 #define DP_DPRX_ESI_LEN 14
 
 /* Compliance test status bits  */

index f424fff477f6..85d5af4e4f5b 100644
@@ -15394,16 +15394,35 @@ static void intel_hpd_poll_fini(struct drm_device *dev)
 {
 	struct intel_connector *connector;
 	struct drm_connector_list_iter conn_iter;
+	struct work_struct *work;
 
 	/* Kill all the work that may have been queued by hpd. */
 	drm_connector_list_iter_begin(dev, &conn_iter);
 	for_each_intel_connector_iter(connector, &conn_iter) {
-		if (connector->modeset_retry_work.func)
-			cancel_work_sync(&connector->modeset_retry_work);
 		if (connector->hdcp_shim) {
 			cancel_delayed_work_sync(&connector->hdcp_check_work);
 			cancel_work_sync(&connector->hdcp_prop_work);
 		}
+
+		if (connector->base.connector_type !=
+		    DRM_MODE_CONNECTOR_DisplayPort)
+			continue;
+
+		if (connector->mst_port) {
+			work = &connector->mst_port->modeset_retry_work;
+		} else {
+			struct intel_encoder *intel_encoder =
+				connector->encoder;
+			struct intel_dp *intel_dp;
+
+			if (!intel_encoder)
+				continue;
+
+			intel_dp = enc_to_intel_dp(&intel_encoder->base);
+			work = &intel_dp->modeset_retry_work;
+		}
+
+		cancel_work_sync(work);
 	}
 	drm_connector_list_iter_end(&conn_iter);
 }

index 4dd1b2287dd6..5abf0c95725a 100644
@@ -6261,12 +6261,10 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 
 static void intel_dp_modeset_retry_work_fn(struct work_struct *work)
 {
-	struct intel_connector *intel_connector;
-	struct drm_connector *connector;
+	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
+						 modeset_retry_work);
+	struct drm_connector *connector = &intel_dp->attached_connector->base;
 
-	intel_connector = container_of(work, typeof(*intel_connector),
-				       modeset_retry_work);
-	connector = &intel_connector->base;
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
 		      connector->name);
 

@@ -6295,7 +6293,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	int type;
 
 	/* Initialize the work for modeset in case of link train failure */
-	INIT_WORK(&intel_connector->modeset_retry_work,
+	INIT_WORK(&intel_dp->modeset_retry_work,
 		  intel_dp_modeset_retry_work_fn);
 
 	if (WARN(intel_dig_port->max_lanes < 1,

index f59b59bb0a21..2cfa58ce1f95 100644
@@ -340,7 +340,7 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
 							     intel_dp->link_rate,
 							     intel_dp->lane_count))
 			/* Schedule a Hotplug Uevent to userspace to start modeset */
-			schedule_work(&intel_connector->modeset_retry_work);
+			schedule_work(&intel_dp->modeset_retry_work);
 	} else {
 		DRM_ERROR("[CONNECTOR:%d:%s] Link Training failed at link rate = %d, lane count = %d",
 			  intel_connector->base.base.id,

index 83e5ca889d9c..3700fcfddb1f 100644
@@ -406,14 +406,14 @@ struct intel_connector {
 
 	struct intel_dp *mst_port;
 
-	/* Work struct to schedule a uevent on link train failure */
-	struct work_struct modeset_retry_work;
-
 	const struct intel_hdcp_shim *hdcp_shim;
 	struct mutex hdcp_mutex;
 	uint64_t hdcp_value; /* protected by hdcp_mutex */
 	struct delayed_work hdcp_check_work;
 	struct work_struct hdcp_prop_work;
+
+	/* Work struct to schedule a uevent on link train failure */
+	struct work_struct modeset_retry_work;
 };
 
 struct intel_digital_connector_state {

@@ -1135,6 +1135,9 @@ struct intel_dp {
 
 	/* Displayport compliance testing */
 	struct intel_dp_compliance compliance;
+
+	/* Work struct to schedule a uevent on link train failure */
+	struct work_struct modeset_retry_work;
 };
 
 struct intel_lspcon {

index f424fff477f6..85d5af4e4f5b 100644
@@ -15394,16 +15394,35 @@ static void intel_hpd_poll_fini(struct drm_device *dev)
 {
 	struct intel_connector *connector;
 	struct drm_connector_list_iter conn_iter;
+	struct work_struct *work;
 
 	/* Kill all the work that may have been queued by hpd. */
 	drm_connector_list_iter_begin(dev, &conn_iter);
 	for_each_intel_connector_iter(connector, &conn_iter) {
-		if (connector->modeset_retry_work.func)
-			cancel_work_sync(&connector->modeset_retry_work);
 		if (connector->hdcp_shim) {
 			cancel_delayed_work_sync(&connector->hdcp_check_work);
 			cancel_work_sync(&connector->hdcp_prop_work);
 		}
+
+		if (connector->base.connector_type !=
+		    DRM_MODE_CONNECTOR_DisplayPort)
+			continue;
+
+		if (connector->mst_port) {
+			work = &connector->mst_port->modeset_retry_work;
+		} else {
+			struct intel_encoder *intel_encoder =
+				connector->encoder;
+			struct intel_dp *intel_dp;
+
+			if (!intel_encoder)
+				continue;
+
+			intel_dp = enc_to_intel_dp(&intel_encoder->base);
+			work = &intel_dp->modeset_retry_work;
+		}
+
+		cancel_work_sync(work);
 	}
 	drm_connector_list_iter_end(&conn_iter);
 }

index 4dd1b2287dd6..5abf0c95725a 100644
@@ -6261,12 +6261,10 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 
 static void intel_dp_modeset_retry_work_fn(struct work_struct *work)
 {
-	struct intel_connector *intel_connector;
-	struct drm_connector *connector;
+	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
+						 modeset_retry_work);
+	struct drm_connector *connector = &intel_dp->attached_connector->base;
 
-	intel_connector = container_of(work, typeof(*intel_connector),
-				       modeset_retry_work);
-	connector = &intel_connector->base;
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
 		      connector->name);
 

@@ -6295,7 +6293,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	int type;
 
 	/* Initialize the work for modeset in case of link train failure */
-	INIT_WORK(&intel_connector->modeset_retry_work,
+	INIT_WORK(&intel_dp->modeset_retry_work,
 		  intel_dp_modeset_retry_work_fn);
 
 	if (WARN(intel_dig_port->max_lanes < 1,

index f59b59bb0a21..2cfa58ce1f95 100644
@@ -340,7 +340,7 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
 							     intel_dp->link_rate,
 							     intel_dp->lane_count))
 			/* Schedule a Hotplug Uevent to userspace to start modeset */
-			schedule_work(&intel_connector->modeset_retry_work);
+			schedule_work(&intel_dp->modeset_retry_work);
 	} else {
 		DRM_ERROR("[CONNECTOR:%d:%s] Link Training failed at link rate = %d, lane count = %d",
 			  intel_connector->base.base.id,

index 83e5ca889d9c..3f19dc80997f 100644
@@ -406,9 +406,6 @@ struct intel_connector {
 
 	struct intel_dp *mst_port;
 
-	/* Work struct to schedule a uevent on link train failure */
-	struct work_struct modeset_retry_work;
-
 	const struct intel_hdcp_shim *hdcp_shim;
 	struct mutex hdcp_mutex;
 	uint64_t hdcp_value; /* protected by hdcp_mutex */

@@ -1135,6 +1132,9 @@ struct intel_dp {
 
 	/* Displayport compliance testing */
 	struct intel_dp_compliance compliance;
+
+	/* Work struct to schedule a uevent on link train failure */
+	struct work_struct modeset_retry_work;
 };
 
 struct intel_lspcon {

index 5abf0c95725a..5645a194de92 100644
@@ -3871,18 +3871,25 @@ intel_dp_can_mst(struct intel_dp *intel_dp)
 static void
 intel_dp_configure_mst(struct intel_dp *intel_dp)
 {
+	bool was_mst;
+
 	if (!i915_modparams.enable_dp_mst)
 		return;
 
 	if (!intel_dp->can_mst)
 		return;
 
+	was_mst = intel_dp->is_mst;
 	intel_dp->is_mst = intel_dp_can_mst(intel_dp);
 
-	if (intel_dp->is_mst)
+	if (intel_dp->is_mst) {
 		DRM_DEBUG_KMS("Sink is MST capable\n");
-	else
+
+		if (!was_mst)
+			intel_dp->mst_bw_locked = false;
+	} else {
 		DRM_DEBUG_KMS("Sink is not MST capable\n");
+	}
 
 	drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr,
 					intel_dp->is_mst);

index c3de0918ee13..c0553456b18e 100644
@@ -42,7 +42,7 @@ static bool intel_dp_mst_compute_config(struct intel_encoder *encoder,
 		to_intel_connector(conn_state->connector);
 	struct drm_atomic_state *state = pipe_config->base.state;
 	int bpp;
-	int lane_count, slots;
+	int lane_count, link_rate, slots;
 	const struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
 	int mst_pbn;
 	bool reduce_m_n = drm_dp_has_quirk(&intel_dp->desc,

@@ -56,16 +56,22 @@ static bool intel_dp_mst_compute_config(struct intel_encoder *encoder,
 			      bpp);
 	}
 	/*
-	 * for MST we always configure max link bw - the spec doesn't
-	 * seem to suggest we should do otherwise.
+	 * for MST we always configure max link bw if we don't know better -
+	 * the spec doesn't seem to suggest we should do otherwise. But,
+	 * ensure it always stays consistent with the rest of this hub's
+	 * state.
 	 */
-	lane_count = intel_dp_max_lane_count(intel_dp);
+	if (intel_dp->mst_bw_locked) {
+		lane_count = intel_dp->lane_count;
+		link_rate = intel_dp->link_rate;
+	} else {
+		lane_count = intel_dp_max_lane_count(intel_dp);
+		link_rate = intel_dp_max_link_rate(intel_dp);
+	}
 
 	pipe_config->lane_count = lane_count;
-
 	pipe_config->pipe_bpp = bpp;
-
-	pipe_config->port_clock = intel_dp_max_link_rate(intel_dp);
+	pipe_config->port_clock = link_rate;
 
 	if (drm_dp_mst_port_has_audio(&intel_dp->mst_mgr, connector->port))
 		pipe_config->has_audio = true;

@@ -221,6 +227,8 @@ static void intel_mst_pre_enable_dp(struct intel_encoder *encoder,
 	connector->encoder = encoder;
 	intel_mst->connector = connector;
 
+	intel_dp->mst_bw_locked = true;
+
 	DRM_DEBUG_KMS("active links %d\n", intel_dp->active_mst_links);
 
 	drm_dp_send_power_updown_phy(&intel_dp->mst_mgr, connector->port, true);

index 3700fcfddb1f..e5d3ef6754a5 100644
@@ -1110,6 +1110,12 @@ struct intel_dp {
 	bool can_mst; /* this port supports mst */
 	bool is_mst;
 	int active_mst_links;
+	/* Set when we've already decided on a link bw for mst, to prevent us
+	 * from setting different link bandwiths if the hub tries to confuse
+	 * us by changing it later
+	 */
+	bool mst_bw_locked;
+
 	/* connector directly attached - won't be use for modeset in mst world */
 	struct intel_connector *attached_connector;
 

index 6fac4129e6a2..0d6604500b29 100644
@@ -2076,7 +2076,7 @@ static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
 {
 	switch (dp_link_bw) {
 	default:
-		DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
+		DRM_DEBUG_KMS("invalid link bandwidth: %x (link count: %d)\n",
 			      dp_link_bw, dp_link_count);
 		return false;
 

@@ -2096,6 +2096,77 @@ static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
 	return true;
 }
 
+static void drm_dp_set_mstb_link_status(struct drm_dp_mst_branch *mstb,
+					enum drm_link_status status)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			drm_dp_set_mstb_link_status(rmstb, status);
+			drm_dp_put_mst_branch_device(rmstb);
+		}
+
+		if (port->connector)
+			port->connector->state->link_status = status;
+	}
+}
+
+/**
+ * drm_dp_mst_topology_mgr_lower_link_rate() - Override the DP link bw/count
+ * for all connectors in a given MST topology
+ * @mgr: manager to set state for
+ * @dp_link_bw: The new DP link bandwidth
+ * @dp_link_count: The new DP link count
+ *
+ * This is called by the driver when it detects that the current DP link for
+ * the given topology manager is unstable, and needs to be retrained at a
+ * lower link rate.
+ *
+ * This takes care of updating the link status on all downstream connectors
+ * along with recalculating the VC payloads. The driver should send a hotplug
+ * event after calling this function to notify userspace of the link status
+ * change.
+ *
+ * RETURNS:
+ *
+ * True for success, or negative error code on failure.
+ */
+int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
+					    int dp_link_bw, int dp_link_count)
+{
+	struct drm_device *dev = mgr->dev;
+	struct drm_dp_mst_branch *mst_primary;
+	int new_pbn_div;
+	int ret = 0;
+
+	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
+
+	if (!drm_dp_get_vc_payload_bw(drm_dp_link_rate_to_bw_code(dp_link_bw),
+				      dp_link_count, &new_pbn_div)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	mst_primary = drm_dp_get_validated_mstb_ref(mgr, mgr->mst_primary);
+	if (!mst_primary)
+		goto out;
+
+	DRM_DEBUG_KMS("MST link failed to retrain, lowering pbn_div to %d\n",
+		      new_pbn_div);
+	mgr->pbn_div = new_pbn_div;
+
+	drm_dp_set_mstb_link_status(mst_primary, DRM_MODE_LINK_STATUS_BAD);
+
+	drm_dp_put_mst_branch_device(mst_primary);
+out:
+	drm_modeset_unlock(&dev->mode_config.connection_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(drm_dp_mst_topology_mgr_lower_link_rate);
+
 /**
  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
  * @mgr: manager to set state for

index 7f78d26a0766..6261ec43a2c0 100644
@@ -634,4 +634,7 @@ int drm_dp_atomic_release_vcpi_slots(struct drm_atomic_state *state,
 int drm_dp_send_power_updown_phy(struct drm_dp_mst_topology_mgr *mgr,
 				 struct drm_dp_mst_port *port, bool power_up);
 
+int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
+					    int dp_link_bw, int dp_link_count);
+
 #endif

index 0d6604500b29..c4a91b1ba61b 100644
@@ -2167,6 +2167,229 @@ int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
 }
 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_lower_link_rate);
 
+static bool drm_atomic_dp_mst_state_only_disables_mstbs(struct drm_atomic_state *state,
+							struct drm_dp_mst_topology_mgr *mgr,
+							struct drm_dp_mst_branch *mstb)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *crtc_state;
+	int ret;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			ret = drm_atomic_dp_mst_state_only_disables_mstbs(
+			    state, mgr, rmstb);
+			drm_dp_put_mst_branch_device(rmstb);
+			if (!ret)
+				return false;
+		}
+
+		connector = port->connector;
+		if (!connector)
+			continue;
+
+		conn_state = drm_atomic_get_new_connector_state(
+		    state, connector);
+		if (!conn_state)
+			continue;
+
+		crtc = conn_state->crtc;
+		if (!crtc)
+			continue;
+
+		crtc_state = drm_atomic_get_new_crtc_state(state, crtc);
+		if (!crtc_state)
+			continue;
+
+		if (drm_atomic_crtc_needs_modeset(crtc_state))
+			return false;
+	}
+
+	return true;
+}
+
+static int drm_atomic_dp_mst_all_mstbs_disabled(struct drm_atomic_state *state,
+						struct drm_dp_mst_topology_mgr *mgr,
+						struct drm_dp_mst_branch *mstb)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	int ret;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			ret = drm_atomic_dp_mst_all_mstbs_disabled(
+			    state, mgr, rmstb);
+			drm_dp_put_mst_branch_device(rmstb);
+			if (ret <= 0)
+				return ret;
+		}
+
+		connector = port->connector;
+		if (!connector)
+			continue;
+
+		conn_state = drm_atomic_get_connector_state(
+		    state, connector);
+		if (IS_ERR(conn_state))
+			return PTR_ERR(conn_state);
+
+		if (conn_state->crtc)
+			return false;
+	}
+
+	/* No enabled CRTCs found */
+	return true;
+}
+
+static int drm_atomic_dp_mst_retrain_mstb(struct drm_atomic_state *state,
+					  struct drm_dp_mst_topology_mgr *mgr,
+					  struct drm_dp_mst_branch *mstb,
+					  bool full_modeset)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *crtc_state;
+	int ret;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			ret = drm_atomic_dp_mst_retrain_mstb(
+			    state, mgr, rmstb, full_modeset);
+			drm_dp_put_mst_branch_device(rmstb);
+			if (ret)
+				return ret;
+		}
+
+		connector = port->connector;
+		if (!connector)
+			continue;
+
+		conn_state = drm_atomic_get_connector_state(state, connector);
+		if (IS_ERR(conn_state))
+			return PTR_ERR(conn_state);
+
+		if (conn_state->link_status != DRM_MODE_LINK_STATUS_GOOD) {
+			DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] link status bad -> good\n",
+					 connector->base.id, connector->name);
+			conn_state->link_status = DRM_MODE_LINK_STATUS_GOOD;
+		}
+
+		if (!full_modeset)
+			continue;
+
+		crtc = conn_state->crtc;
+		if (!crtc)
+			continue;
+
+		crtc_state = drm_atomic_get_crtc_state(state, crtc);
+		if (IS_ERR(crtc_state))
+			return PTR_ERR(crtc_state);
+
+		if (!crtc_state->mode_changed) {
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] needs full modeset\n",
+					 crtc->base.id, crtc->name);
+			crtc_state->mode_changed = true;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * drm_atomic_dp_mst_retrain_topology() - prepare a modeset that will retrain
+ * an MST topology
+ * @state: The new state that would retrain the topology link
+ * @mgr: The topology manager to use
+ *
+ * After userspace has been signaled that connectors are in need of
+ * retraining, it's expected that a modeset will be performed for any CRTCs on
+ * said connectors. Since all of the branch devices in an MST topology share a
+ * single link, they also share the same link status, lane count, and link
+ * rate.
+ *
+ * Since all of these characteristics are shared, there's only two valid
+ * solutions when fallback link training parameters need to be applied. The
+ * first is simply unassigning each mstb connector's CRTC. Since this action
+ * can only free slots on the VCPI table and not allocate them, this can be
+ * done without pulling in additional CRTCs on the MST topology. Additionally
+ * if this action would result in there no longer being any CRTCs assigned to
+ * mstb connectors, this would be enough to bring the topology back into a
+ * trained state since there would be nothing left requiring VCPI
+ * reallocations.
+ *
+ * The second solution is to commit new modes to all of the connectors on the
+ * topology. Since this action would result in VCPI reallocations with a new
+ * link rate and lane count, a modeset must also be performed on every other
+ * CRTC driving a branch device on the given topology at the same time. This
+ * is to ensure that all VCPI allocations are properly recalculated in
+ * response to the new link rate and lane count, that the new modes will fit
+ * into the recalculated VCPI table, and that the new atomic state could never
+ * result in an otherwise physically impossible configuration (e.g. different
+ * mstbs with CRTCs trained to different link rates/lane counts).
+ *
+ * Finally, any atomic commit which would result in going from an
+ * unrecoverable link state to a properly trained link state must also take
+ * care of appropriately updating the link status properties of all connectors
+ * on the topology from bad to good. This function takes care of all of these
+ * checks in @state for the topology manager @mgr including possibly pulling
+ * in additional CRTCs into the modeset, and possibly updating the link status
+ * of each mstb's DRM connector.
+ *
+ * This function should be called within the driver's atomic check callbacks
+ * whenever a modeset happens on an MST connector with it's link status set to
+ * DRM_MODE_LINK_STATUS_BAD.
+ *
+ * RETURNS:
+ *
+ * Returns 0 for success, or negative error code for failure.
+ */
+int drm_atomic_dp_mst_retrain_topology(struct drm_atomic_state *state,
+				       struct drm_dp_mst_topology_mgr *mgr)
+{
+	struct drm_dp_mst_branch *mstb =
+		drm_dp_get_validated_mstb_ref(mgr, mgr->mst_primary);
+	int ret = 0;
+	bool need_modeset = false;
+
+	if (!mstb)
+		return 0;
+
+	if (drm_atomic_dp_mst_state_only_disables_mstbs(state, mgr, mstb)) {
+		ret = drm_atomic_dp_mst_all_mstbs_disabled(state, mgr, mstb);
+		if (ret < 0)
+			goto out;
+
+		if (ret)
+			DRM_DEBUG_ATOMIC("state %p disables all CRTCs on mst mgr %p\n",
+					 state, mgr);
+		else
+			goto out; /* valid, but doesn't retrain link */
+	} else {
+		DRM_DEBUG_ATOMIC("state %p requires full modeset for CRTCs on mst mgr %p\n",
+				 state, mgr);
+		need_modeset = true;
+	}
+
+	ret = drm_atomic_dp_mst_retrain_mstb(state, mgr, mstb, need_modeset);
+out:
+	drm_dp_put_mst_branch_device(mgr->mst_primary);
+	return ret;
+}
+EXPORT_SYMBOL(drm_atomic_dp_mst_retrain_topology);
+
 /**
  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
  * @mgr: manager to set state for

index 6261ec43a2c0..24075eb2dd1b 100644
@@ -636,5 +636,7 @@ int drm_dp_send_power_updown_phy(struct drm_dp_mst_topology_mgr *mgr,
 
 int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
 					    int dp_link_bw, int dp_link_count);
+int drm_atomic_dp_mst_retrain_topology(struct drm_atomic_state *state,
+				       struct drm_dp_mst_topology_mgr *mgr);
 
 #endif

index 5645a194de92..7626652732b6 100644
@@ -45,6 +45,8 @@
 
 #define DP_DPRX_ESI_LEN 14
 
+#define DP_MST_RETRAIN_TIMEOUT (msecs_to_jiffies(100))
+
 /* Compliance test status bits  */
 #define INTEL_DP_RESOLUTION_SHIFT_MASK	0
 #define INTEL_DP_RESOLUTION_PREFERRED	(1 << INTEL_DP_RESOLUTION_SHIFT_MASK)

@@ -4224,6 +4226,118 @@ static void intel_dp_handle_test_request(struct intel_dp *intel_dp)
 		DRM_DEBUG_KMS("Could not write test response to sink\n");
 }
 
+/* Get a mask of the CRTCs that are running on the given intel_dp struct. For
+ * MST, this returns a crtc mask containing all of the CRTCs driving
+ * downstream sinks, for SST it just returns a mask of the attached
+ * connector's CRTC.
+ */
+int
+intel_dp_get_crtc_mask(struct intel_dp *intel_dp)
+{
+	struct drm_device *dev = dp_to_dig_port(intel_dp)->base.base.dev;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	struct intel_connector *intel_connector;
+	struct drm_crtc *crtc;
+	int crtc_mask = 0;
+
+	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
+
+	if (intel_dp->is_mst) {
+		struct drm_connector_list_iter conn_iter;
+
+		drm_connector_list_iter_begin(dev, &conn_iter);
+		for_each_intel_connector_iter(intel_connector, &conn_iter) {
+			if (intel_connector->mst_port != intel_dp)
+				continue;
+
+			conn_state = intel_connector->base.state;
+			if (!conn_state->crtc)
+				continue;
+
+			crtc_mask |= drm_crtc_mask(conn_state->crtc);
+		}
+		drm_connector_list_iter_end(&conn_iter);
+	} else {
+		connector = &intel_dp->attached_connector->base;
+		crtc = connector->state->crtc;
+
+		if (crtc)
+			crtc_mask |= drm_crtc_mask(crtc);
+	}
+
+	return crtc_mask;
+}
+
+static bool
+intel_dp_needs_link_retrain(struct intel_dp *intel_dp,
+			    const u8 esi[DP_DPRX_ESI_LEN])
+{
+	u8 buf[max(DP_LINK_STATUS_SIZE, DP_DPRX_ESI_LEN)];
+	const u8 *link_status = NULL;
+
+	if (intel_dp->is_mst) {
+		if (!intel_dp->active_mst_links)
+			return false;
+		if (intel_dp->mst_link_is_bad)
+			return false;
+
+		if (esi) {
+			link_status = &esi[10];
+		} else {
+			/* We're not running from the ESI handler, so wait a
+			 * little bit to see if the ESI handler lets us know
+			 * that the link status is OK
+			 */
+			if (wait_for_completion_timeout(
+				&intel_dp->mst_retrain_completion,
+				DP_MST_RETRAIN_TIMEOUT))
+				return false;
+		}
+	} else {
+		if (intel_dp->link_trained)
+			return false;
+		if (!intel_dp_get_link_status(intel_dp, buf))
+			return false;
+
+		link_status = buf;
+	}
+
+	/*
+	 * Validate the cached values of intel_dp->link_rate and
+	 * intel_dp->lane_count before attempting to retrain.
+	 */
+	if (!intel_dp_link_params_valid(intel_dp, intel_dp->link_rate,
+					intel_dp->lane_count))
+		return false;
+
+	if (link_status) {
+		return !drm_dp_channel_eq_ok(link_status,
+					     intel_dp->lane_count);
+	} else {
+		return true;
+	}
+}
+
+static inline void
+intel_dp_mst_check_link_status(struct intel_dp *intel_dp,
+			       const u8 esi[DP_DPRX_ESI_LEN])
+{
+	if (intel_dp_needs_link_retrain(intel_dp, esi)) {
+		DRM_DEBUG_KMS("Channel EQ failing\n");
+
+		if (!work_busy(&intel_dp->mst_retrain_work)) {
+			reinit_completion(&intel_dp->mst_retrain_completion);
+			schedule_work(&intel_dp->mst_retrain_work);
+			DRM_DEBUG_KMS("Retraining started\n");
+		}
+	} else if (work_busy(&intel_dp->mst_retrain_work) &&
+		   !completion_done(&intel_dp->mst_retrain_completion)) {
+		DRM_DEBUG_KMS("Channel EQ stable\n");
+		complete_all(&intel_dp->mst_retrain_completion);
+	}
+}
+
 static int
 intel_dp_check_mst_status(struct intel_dp *intel_dp)
 {

@@ -4237,14 +4351,7 @@ intel_dp_check_mst_status(struct intel_dp *intel_dp)
 		bret = intel_dp_get_sink_irq_esi(intel_dp, esi);
 go_again:
 		if (bret == true) {
-
-			/* check link status - esi[10] = 0x200c */
-			if (intel_dp->active_mst_links &&
-			    !drm_dp_channel_eq_ok(&esi[10], intel_dp->lane_count)) {
-				DRM_DEBUG_KMS("channel EQ not ok, retraining\n");
-				intel_dp_start_link_train(intel_dp);
-				intel_dp_stop_link_train(intel_dp);
-			}
+			intel_dp_mst_check_link_status(intel_dp, esi);
 
 			DRM_DEBUG_KMS("got esi %3ph\n", esi);
 			ret = drm_dp_mst_hpd_irq(&intel_dp->mst_mgr, esi, &handled);

@@ -4281,29 +4388,6 @@ intel_dp_check_mst_status(struct intel_dp *intel_dp)
 	return -EINVAL;
 }
 
-static bool
-intel_dp_needs_link_retrain(struct intel_dp *intel_dp)
-{
-	u8 link_status[DP_LINK_STATUS_SIZE];
-
-	if (!intel_dp->link_trained)
-		return false;
-
-	if (!intel_dp_get_link_status(intel_dp, link_status))
-		return false;
-
-	/*
-	 * Validate the cached values of intel_dp->link_rate and
-	 * intel_dp->lane_count before attempting to retrain.
-	 */
-	if (!intel_dp_link_params_valid(intel_dp, intel_dp->link_rate,
-					intel_dp->lane_count))
-		return false;
-
-	/* Retrain if Channel EQ or CR not ok */
-	return !drm_dp_channel_eq_ok(link_status, intel_dp->lane_count);
-}
-
 /*
  * If display is now connected check links status,
  * there has been known issues of link loss triggering

@@ -4319,64 +4403,78 @@ intel_dp_needs_link_retrain(struct intel_dp *intel_dp)
 int intel_dp_retrain_link(struct intel_encoder *encoder,
 			  struct drm_modeset_acquire_ctx *ctx)
 {
-	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
-	struct intel_connector *connector = intel_dp->attached_connector;
-	struct drm_connector_state *conn_state;
-	struct intel_crtc_state *crtc_state;
-	struct intel_crtc *crtc;
+	struct drm_crtc *crtc;
+	struct intel_crtc *intel_crtc;
+	int crtc_mask, retry_count = 0;
 	int ret;
 
-	/* FIXME handle the MST connectors as well */
-
-	if (!connector || connector->base.status != connector_status_connected)
-		return 0;
-
 	ret = drm_modeset_lock(&dev_priv->drm.mode_config.connection_mutex,
 			       ctx);
 	if (ret)
 		return ret;
 
-	conn_state = connector->base.state;
-
-	crtc = to_intel_crtc(conn_state->crtc);
-	if (!crtc)
-		return 0;
+	crtc_mask = intel_dp_get_crtc_mask(intel_dp);
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask) {
+		struct drm_crtc_state *crtc_state;
+		struct intel_crtc_state *intel_crtc_state;
 
-	ret = drm_modeset_lock(&crtc->base.mutex, ctx);
-	if (ret)
-		return ret;
+		crtc = &intel_crtc->base;
+		ret = drm_modeset_lock(&crtc->mutex, ctx);
+		if (ret)
+			return ret;
 
-	crtc_state = to_intel_crtc_state(crtc->base.state);
+		crtc_state = crtc->state;
+		intel_crtc_state = to_intel_crtc_state(crtc_state);
+		WARN_ON(!intel_crtc_has_dp_encoder(intel_crtc_state));
 
-	WARN_ON(!intel_crtc_has_dp_encoder(crtc_state));
+		if (crtc_state->commit &&
+		    !try_wait_for_completion(&crtc_state->commit->hw_done))
+			return 0;
+	}
 
-	if (!crtc_state->base.active)
+	if (!intel_dp_needs_link_retrain(intel_dp, NULL))
 		return 0;
 
-	if (conn_state->commit &&
-	    !try_wait_for_completion(&conn_state->commit->hw_done))
-		return 0;
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask) {
+		intel_set_cpu_fifo_underrun_reporting(
+		    dev_priv, intel_crtc->pipe, false);
 
-	if (!intel_dp_needs_link_retrain(intel_dp))
-		return 0;
+		if (intel_crtc->config->has_pch_encoder) {
+			intel_set_pch_fifo_underrun_reporting(
+			    dev_priv, intel_crtc_pch_transcoder(intel_crtc),
+			    false);
+		}
+	}
 
-	/* Suppress underruns caused by re-training */
-	intel_set_cpu_fifo_underrun_reporting(dev_priv, crtc->pipe, false);
-	if (crtc->config->has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev_priv,
-						      intel_crtc_pch_transcoder(crtc), false);
+	do {
+		if (++retry_count > 5) {
+			DRM_DEBUG_KMS("Too many retries, can't retrain\n");
+			return -EINVAL;
+		}
 
-	intel_dp_start_link_train(intel_dp);
-	intel_dp_stop_link_train(intel_dp);
+		intel_dp_start_link_train(intel_dp);
+		intel_dp_stop_link_train(intel_dp);
+	} while (intel_dp_needs_link_retrain(intel_dp, NULL));
+
+	/* Wait for things to become stable */
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask)
+		intel_wait_for_vblank(dev_priv, intel_crtc->pipe);
 
-	/* Keep underrun reporting disabled until things are stable */
-	intel_wait_for_vblank(dev_priv, crtc->pipe);
+	/* Now that we know everything is OK, finally re-enable underrun
+	 * reporting */
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask) {
+		intel_set_cpu_fifo_underrun_reporting(
+		    dev_priv, intel_crtc->pipe, true);
 
-	intel_set_cpu_fifo_underrun_reporting(dev_priv, crtc->pipe, true);
-	if (crtc->config->has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev_priv,
-						      intel_crtc_pch_transcoder(crtc), true);
+		if (intel_crtc->config->has_pch_encoder) {
+			intel_set_pch_fifo_underrun_reporting(
+			    dev_priv, intel_crtc_pch_transcoder(intel_crtc),
+			    true);
+		}
+	}
 
 	return 0;
 }

@@ -4402,6 +4500,10 @@ static bool intel_dp_hotplug(struct intel_encoder *encoder,
 
 	changed = intel_encoder_hotplug(encoder, connector);
 
+	/* We don't want to end up trying to retrain MST links! */
+	if (encoder && enc_to_intel_dp(&encoder->base)->is_mst)
+		return changed;
+
 	drm_modeset_acquire_init(&ctx, 0);
 
 	for (;;) {

@@ -4478,7 +4580,7 @@ intel_dp_short_pulse(struct intel_dp *intel_dp)
 	}
 
 	/* defer to the hotplug work for link retraining if needed */
-	if (intel_dp_needs_link_retrain(intel_dp))
+	if (intel_dp_needs_link_retrain(intel_dp, NULL))
 		return false;
 
 	if (intel_dp->compliance.test_type == DP_TEST_LINK_TRAINING) {

@@ -6266,25 +6368,98 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	return false;
 }
 
+static void intel_dp_mst_retrain_link_work(struct work_struct *work)
+{
+	struct drm_modeset_acquire_ctx ctx;
+	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
+						 mst_retrain_work);
+	struct intel_encoder *intel_encoder = &dp_to_dig_port(intel_dp)->base;
+	struct drm_device *dev = intel_encoder->base.dev;
+	int ret;
+	bool had_error = false;
+
+	drm_modeset_acquire_init(&ctx, 0);
+
+	for (;;) {
+		ret = intel_dp_retrain_link(intel_encoder, &ctx);
+		if (ret == -EDEADLK) {
+			drm_modeset_backoff(&ctx);
+			continue;
+		}
+
+		break;
+	}
+	if (!ret) {
+		DRM_DEBUG_KMS("Retrain complete\n");
+		goto out;
+	} else if (ret == -EIO) {
+		DRM_ERROR("IO error with sink during retrain? Aborting\n");
+		had_error = true;
+		goto out;
+	}
+
+	DRM_DEBUG_KMS("Retraining failed with %d, marking link status as bad\n",
+		      ret);
+
+	/* We ran out of retries, if the sink hasn't changed the link rate in
+	 * it's dpcd yet force us to fallback to a lower link rate/count */
+	if (ret == -EINVAL) {
+		ret = intel_dp_get_dpcd(intel_dp);
+		if (!ret) {
+			DRM_ERROR("IO error while reading dpcd from sink\n");
+			had_error = true;
+			goto out;
+		}
+
+		if (intel_dp->link_rate == intel_dp_max_link_rate(intel_dp) &&
+		    intel_dp->lane_count == intel_dp_max_lane_count(intel_dp)) {
+			intel_dp_get_link_train_fallback_values(
+			    intel_dp, intel_dp_max_link_rate(intel_dp),
+			    intel_dp_max_lane_count(intel_dp));
+		}
+	}
+
+	intel_dp->mst_link_is_bad = true;
+	intel_dp->mst_bw_locked = false;
+	schedule_work(&intel_dp->modeset_retry_work);
+out:
+	drm_modeset_drop_locks(&ctx);
+	drm_modeset_acquire_fini(&ctx);
+	if (had_error)
+		drm_kms_helper_hotplug_event(dev);
+}
+
 static void intel_dp_modeset_retry_work_fn(struct work_struct *work)
 {
 	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
 						 modeset_retry_work);
-	struct drm_connector *connector = &intel_dp->attached_connector->base;
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_connector *connector;
 
-	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
-		      connector->name);
+	mutex_lock(&dev->mode_config.mutex);
 
-	/* Grab the locks before changing connector property*/
-	mutex_lock(&connector->dev->mode_config.mutex);
-	/* Set connector link status to BAD and send a Uevent to notify
-	 * userspace to do a modeset.
+	/* Set the connector link status of all (possibly downstream) ports to
+	 * BAD and send a Uevent to notify userspace to do a modeset.
 	 */
-	drm_mode_connector_set_link_status_property(connector,
-						    DRM_MODE_LINK_STATUS_BAD);
-	mutex_unlock(&connector->dev->mode_config.mutex);
+	if (intel_dp->is_mst) {
+		drm_dp_mst_topology_mgr_lower_link_rate(
+		    &intel_dp->mst_mgr,
+		    intel_dp_max_link_rate(intel_dp),
+		    intel_dp_max_lane_count(intel_dp));
+	} else {
+		connector = &intel_dp->attached_connector->base;
+
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
+			      connector->base.id, connector->name);
+		drm_mode_connector_set_link_status_property(
+		    connector, DRM_MODE_LINK_STATUS_BAD);
+	}
+
+	mutex_unlock(&dev->mode_config.mutex);
+
 	/* Send Hotplug uevent so userspace can reprobe */
-	drm_kms_helper_hotplug_event(connector->dev);
+	drm_kms_helper_hotplug_event(dev);
 }
 
 bool

@@ -6302,6 +6477,9 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	/* Initialize the work for modeset in case of link train failure */
 	INIT_WORK(&intel_dp->modeset_retry_work,
 		  intel_dp_modeset_retry_work_fn);
+	INIT_WORK(&intel_dp->mst_retrain_work,
+		  intel_dp_mst_retrain_link_work);
+	init_completion(&intel_dp->mst_retrain_completion);
 
 	if (WARN(intel_dig_port->max_lanes < 1,
 		 "Not enough lanes (%d) for DP on port %c\n",

70 lines not on this page · raw message

index e5d3ef6754a5..c63c65923c3e 100644
@@ -1122,6 +1122,13 @@ struct intel_dp {
 	/* mst connector list */
 	struct intel_dp_mst_encoder *mst_encoders[I915_MAX_PIPES];
 	struct drm_dp_mst_topology_mgr mst_mgr;
+	/* We can't handle retraining from the dig workqueue, so... */
+	struct work_struct mst_retrain_work;
+	struct completion mst_retrain_completion;
+	/* Set when retraining the link at the current parameters is
+	 * impossible for an MST connection
+	 */
+	bool mst_link_is_bad;
 
 	uint32_t (*get_aux_clock_divider)(struct intel_dp *dp, int index);
 	/*

@@ -1689,6 +1696,7 @@ void intel_dp_compute_rate(struct intel_dp *intel_dp, int port_clock,
 bool intel_dp_source_supports_hbr2(struct intel_dp *intel_dp);
 bool
 intel_dp_get_link_status(struct intel_dp *intel_dp, uint8_t link_status[DP_LINK_STATUS_SIZE]);
+int intel_dp_get_crtc_mask(struct intel_dp *intel_dp);
 
 static inline unsigned int intel_dp_unused_lane_mask(int lane_count)
 {

index f424fff477f6..3b7fa430a84a 100644
@@ -15394,16 +15394,37 @@ static void intel_hpd_poll_fini(struct drm_device *dev)
 {
 	struct intel_connector *connector;
 	struct drm_connector_list_iter conn_iter;
+	struct work_struct *work;
+	int conn_type;
 
 	/* Kill all the work that may have been queued by hpd. */
 	drm_connector_list_iter_begin(dev, &conn_iter);
 	for_each_intel_connector_iter(connector, &conn_iter) {
-		if (connector->modeset_retry_work.func)
-			cancel_work_sync(&connector->modeset_retry_work);
 		if (connector->hdcp_shim) {
 			cancel_delayed_work_sync(&connector->hdcp_check_work);
 			cancel_work_sync(&connector->hdcp_prop_work);
 		}
+
+		conn_type = connector->base.connector_type;
+		if (conn_type != DRM_MODE_CONNECTOR_eDP &&
+		    conn_type != DRM_MODE_CONNECTOR_DisplayPort)
+			continue;
+
+		if (connector->mst_port) {
+			work = &connector->mst_port->modeset_retry_work;
+		} else {
+			struct intel_encoder *intel_encoder =
+				connector->encoder;
+			struct intel_dp *intel_dp;
+
+			if (!intel_encoder)
+				continue;
+
+			intel_dp = enc_to_intel_dp(&intel_encoder->base);
+			work = &intel_dp->modeset_retry_work;
+		}
+
+		cancel_work_sync(work);
 	}
 	drm_connector_list_iter_end(&conn_iter);
 }

index 4dd1b2287dd6..5abf0c95725a 100644
@@ -6261,12 +6261,10 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 
 static void intel_dp_modeset_retry_work_fn(struct work_struct *work)
 {
-	struct intel_connector *intel_connector;
-	struct drm_connector *connector;
+	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
+						 modeset_retry_work);
+	struct drm_connector *connector = &intel_dp->attached_connector->base;
 
-	intel_connector = container_of(work, typeof(*intel_connector),
-				       modeset_retry_work);
-	connector = &intel_connector->base;
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
 		      connector->name);
 

@@ -6295,7 +6293,7 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	int type;
 
 	/* Initialize the work for modeset in case of link train failure */
-	INIT_WORK(&intel_connector->modeset_retry_work,
+	INIT_WORK(&intel_dp->modeset_retry_work,
 		  intel_dp_modeset_retry_work_fn);
 
 	if (WARN(intel_dig_port->max_lanes < 1,

index f59b59bb0a21..2cfa58ce1f95 100644
@@ -340,7 +340,7 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
 							     intel_dp->link_rate,
 							     intel_dp->lane_count))
 			/* Schedule a Hotplug Uevent to userspace to start modeset */
-			schedule_work(&intel_connector->modeset_retry_work);
+			schedule_work(&intel_dp->modeset_retry_work);
 	} else {
 		DRM_ERROR("[CONNECTOR:%d:%s] Link Training failed at link rate = %d, lane count = %d",
 			  intel_connector->base.base.id,

index 83e5ca889d9c..3f19dc80997f 100644
@@ -406,9 +406,6 @@ struct intel_connector {
 
 	struct intel_dp *mst_port;
 
-	/* Work struct to schedule a uevent on link train failure */
-	struct work_struct modeset_retry_work;
-
 	const struct intel_hdcp_shim *hdcp_shim;
 	struct mutex hdcp_mutex;
 	uint64_t hdcp_value; /* protected by hdcp_mutex */

@@ -1135,6 +1132,9 @@ struct intel_dp {
 
 	/* Displayport compliance testing */
 	struct intel_dp_compliance compliance;
+
+	/* Work struct to schedule a uevent on link train failure */
+	struct work_struct modeset_retry_work;
 };
 
 struct intel_lspcon {

index 5abf0c95725a..5645a194de92 100644
@@ -3871,18 +3871,25 @@ intel_dp_can_mst(struct intel_dp *intel_dp)
 static void
 intel_dp_configure_mst(struct intel_dp *intel_dp)
 {
+	bool was_mst;
+
 	if (!i915_modparams.enable_dp_mst)
 		return;
 
 	if (!intel_dp->can_mst)
 		return;
 
+	was_mst = intel_dp->is_mst;
 	intel_dp->is_mst = intel_dp_can_mst(intel_dp);
 
-	if (intel_dp->is_mst)
+	if (intel_dp->is_mst) {
 		DRM_DEBUG_KMS("Sink is MST capable\n");
-	else
+
+		if (!was_mst)
+			intel_dp->mst_bw_locked = false;
+	} else {
 		DRM_DEBUG_KMS("Sink is not MST capable\n");
+	}
 
 	drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr,
 					intel_dp->is_mst);

index c3de0918ee13..c0553456b18e 100644
@@ -42,7 +42,7 @@ static bool intel_dp_mst_compute_config(struct intel_encoder *encoder,
 		to_intel_connector(conn_state->connector);
 	struct drm_atomic_state *state = pipe_config->base.state;
 	int bpp;
-	int lane_count, slots;
+	int lane_count, link_rate, slots;
 	const struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
 	int mst_pbn;
 	bool reduce_m_n = drm_dp_has_quirk(&intel_dp->desc,

@@ -56,16 +56,22 @@ static bool intel_dp_mst_compute_config(struct intel_encoder *encoder,
 			      bpp);
 	}
 	/*
-	 * for MST we always configure max link bw - the spec doesn't
-	 * seem to suggest we should do otherwise.
+	 * for MST we always configure max link bw if we don't know better -
+	 * the spec doesn't seem to suggest we should do otherwise. But,
+	 * ensure it always stays consistent with the rest of this hub's
+	 * state.
 	 */
-	lane_count = intel_dp_max_lane_count(intel_dp);
+	if (intel_dp->mst_bw_locked) {
+		lane_count = intel_dp->lane_count;
+		link_rate = intel_dp->link_rate;
+	} else {
+		lane_count = intel_dp_max_lane_count(intel_dp);
+		link_rate = intel_dp_max_link_rate(intel_dp);
+	}
 
 	pipe_config->lane_count = lane_count;
-
 	pipe_config->pipe_bpp = bpp;
-
-	pipe_config->port_clock = intel_dp_max_link_rate(intel_dp);
+	pipe_config->port_clock = link_rate;
 
 	if (drm_dp_mst_port_has_audio(&intel_dp->mst_mgr, connector->port))
 		pipe_config->has_audio = true;

@@ -221,6 +227,8 @@ static void intel_mst_pre_enable_dp(struct intel_encoder *encoder,
 	connector->encoder = encoder;
 	intel_mst->connector = connector;
 
+	intel_dp->mst_bw_locked = true;
+
 	DRM_DEBUG_KMS("active links %d\n", intel_dp->active_mst_links);
 
 	drm_dp_send_power_updown_phy(&intel_dp->mst_mgr, connector->port, true);

index 3f19dc80997f..fc338529e918 100644
@@ -1107,6 +1107,12 @@ struct intel_dp {
 	bool can_mst; /* this port supports mst */
 	bool is_mst;
 	int active_mst_links;
+	/* Set when we've already decided on a link bw for mst, to prevent us
+	 * from setting different link bandwiths if the hub tries to confuse
+	 * us by changing it later
+	 */
+	bool mst_bw_locked;
+
 	/* connector directly attached - won't be use for modeset in mst world */
 	struct intel_connector *attached_connector;
 

index 6fac4129e6a2..0d6604500b29 100644
@@ -2076,7 +2076,7 @@ static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
 {
 	switch (dp_link_bw) {
 	default:
-		DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
+		DRM_DEBUG_KMS("invalid link bandwidth: %x (link count: %d)\n",
 			      dp_link_bw, dp_link_count);
 		return false;
 

@@ -2096,6 +2096,77 @@ static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
 	return true;
 }
 
+static void drm_dp_set_mstb_link_status(struct drm_dp_mst_branch *mstb,
+					enum drm_link_status status)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			drm_dp_set_mstb_link_status(rmstb, status);
+			drm_dp_put_mst_branch_device(rmstb);
+		}
+
+		if (port->connector)
+			port->connector->state->link_status = status;
+	}
+}
+
+/**
+ * drm_dp_mst_topology_mgr_lower_link_rate() - Override the DP link bw/count
+ * for all connectors in a given MST topology
+ * @mgr: manager to set state for
+ * @dp_link_bw: The new DP link bandwidth
+ * @dp_link_count: The new DP link count
+ *
+ * This is called by the driver when it detects that the current DP link for
+ * the given topology manager is unstable, and needs to be retrained at a
+ * lower link rate.
+ *
+ * This takes care of updating the link status on all downstream connectors
+ * along with recalculating the VC payloads. The driver should send a hotplug
+ * event after calling this function to notify userspace of the link status
+ * change.
+ *
+ * RETURNS:
+ *
+ * True for success, or negative error code on failure.
+ */
+int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
+					    int dp_link_bw, int dp_link_count)
+{
+	struct drm_device *dev = mgr->dev;
+	struct drm_dp_mst_branch *mst_primary;
+	int new_pbn_div;
+	int ret = 0;
+
+	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
+
+	if (!drm_dp_get_vc_payload_bw(drm_dp_link_rate_to_bw_code(dp_link_bw),
+				      dp_link_count, &new_pbn_div)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	mst_primary = drm_dp_get_validated_mstb_ref(mgr, mgr->mst_primary);
+	if (!mst_primary)
+		goto out;
+
+	DRM_DEBUG_KMS("MST link failed to retrain, lowering pbn_div to %d\n",
+		      new_pbn_div);
+	mgr->pbn_div = new_pbn_div;
+
+	drm_dp_set_mstb_link_status(mst_primary, DRM_MODE_LINK_STATUS_BAD);
+
+	drm_dp_put_mst_branch_device(mst_primary);
+out:
+	drm_modeset_unlock(&dev->mode_config.connection_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(drm_dp_mst_topology_mgr_lower_link_rate);
+
 /**
  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
  * @mgr: manager to set state for

index 7f78d26a0766..6261ec43a2c0 100644
@@ -634,4 +634,7 @@ int drm_dp_atomic_release_vcpi_slots(struct drm_atomic_state *state,
 int drm_dp_send_power_updown_phy(struct drm_dp_mst_topology_mgr *mgr,
 				 struct drm_dp_mst_port *port, bool power_up);
 
+int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
+					    int dp_link_bw, int dp_link_count);
+
 #endif

index 0d6604500b29..c4a91b1ba61b 100644
@@ -2167,6 +2167,229 @@ int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
 }
 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_lower_link_rate);
 
+static bool drm_atomic_dp_mst_state_only_disables_mstbs(struct drm_atomic_state *state,
+							struct drm_dp_mst_topology_mgr *mgr,
+							struct drm_dp_mst_branch *mstb)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *crtc_state;
+	int ret;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			ret = drm_atomic_dp_mst_state_only_disables_mstbs(
+			    state, mgr, rmstb);
+			drm_dp_put_mst_branch_device(rmstb);
+			if (!ret)
+				return false;
+		}
+
+		connector = port->connector;
+		if (!connector)
+			continue;
+
+		conn_state = drm_atomic_get_new_connector_state(
+		    state, connector);
+		if (!conn_state)
+			continue;
+
+		crtc = conn_state->crtc;
+		if (!crtc)
+			continue;
+
+		crtc_state = drm_atomic_get_new_crtc_state(state, crtc);
+		if (!crtc_state)
+			continue;
+
+		if (drm_atomic_crtc_needs_modeset(crtc_state))
+			return false;
+	}
+
+	return true;
+}
+
+static int drm_atomic_dp_mst_all_mstbs_disabled(struct drm_atomic_state *state,
+						struct drm_dp_mst_topology_mgr *mgr,
+						struct drm_dp_mst_branch *mstb)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	int ret;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			ret = drm_atomic_dp_mst_all_mstbs_disabled(
+			    state, mgr, rmstb);
+			drm_dp_put_mst_branch_device(rmstb);
+			if (ret <= 0)
+				return ret;
+		}
+
+		connector = port->connector;
+		if (!connector)
+			continue;
+
+		conn_state = drm_atomic_get_connector_state(
+		    state, connector);
+		if (IS_ERR(conn_state))
+			return PTR_ERR(conn_state);
+
+		if (conn_state->crtc)
+			return false;
+	}
+
+	/* No enabled CRTCs found */
+	return true;
+}
+
+static int drm_atomic_dp_mst_retrain_mstb(struct drm_atomic_state *state,
+					  struct drm_dp_mst_topology_mgr *mgr,
+					  struct drm_dp_mst_branch *mstb,
+					  bool full_modeset)
+{
+	struct drm_dp_mst_branch *rmstb;
+	struct drm_dp_mst_port *port;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *crtc_state;
+	int ret;
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		rmstb = drm_dp_get_validated_mstb_ref(mstb->mgr, port->mstb);
+		if (rmstb) {
+			ret = drm_atomic_dp_mst_retrain_mstb(
+			    state, mgr, rmstb, full_modeset);
+			drm_dp_put_mst_branch_device(rmstb);
+			if (ret)
+				return ret;
+		}
+
+		connector = port->connector;
+		if (!connector)
+			continue;
+
+		conn_state = drm_atomic_get_connector_state(state, connector);
+		if (IS_ERR(conn_state))
+			return PTR_ERR(conn_state);
+
+		if (conn_state->link_status != DRM_MODE_LINK_STATUS_GOOD) {
+			DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] link status bad -> good\n",
+					 connector->base.id, connector->name);
+			conn_state->link_status = DRM_MODE_LINK_STATUS_GOOD;
+		}
+
+		if (!full_modeset)
+			continue;
+
+		crtc = conn_state->crtc;
+		if (!crtc)
+			continue;
+
+		crtc_state = drm_atomic_get_crtc_state(state, crtc);
+		if (IS_ERR(crtc_state))
+			return PTR_ERR(crtc_state);
+
+		if (!crtc_state->mode_changed) {
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] needs full modeset\n",
+					 crtc->base.id, crtc->name);
+			crtc_state->mode_changed = true;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * drm_atomic_dp_mst_retrain_topology() - prepare a modeset that will retrain
+ * an MST topology
+ * @state: The new state that would retrain the topology link
+ * @mgr: The topology manager to use
+ *
+ * After userspace has been signaled that connectors are in need of
+ * retraining, it's expected that a modeset will be performed for any CRTCs on
+ * said connectors. Since all of the branch devices in an MST topology share a
+ * single link, they also share the same link status, lane count, and link
+ * rate.
+ *
+ * Since all of these characteristics are shared, there's only two valid
+ * solutions when fallback link training parameters need to be applied. The
+ * first is simply unassigning each mstb connector's CRTC. Since this action
+ * can only free slots on the VCPI table and not allocate them, this can be
+ * done without pulling in additional CRTCs on the MST topology. Additionally
+ * if this action would result in there no longer being any CRTCs assigned to
+ * mstb connectors, this would be enough to bring the topology back into a
+ * trained state since there would be nothing left requiring VCPI
+ * reallocations.
+ *
+ * The second solution is to commit new modes to all of the connectors on the
+ * topology. Since this action would result in VCPI reallocations with a new
+ * link rate and lane count, a modeset must also be performed on every other
+ * CRTC driving a branch device on the given topology at the same time. This
+ * is to ensure that all VCPI allocations are properly recalculated in
+ * response to the new link rate and lane count, that the new modes will fit
+ * into the recalculated VCPI table, and that the new atomic state could never
+ * result in an otherwise physically impossible configuration (e.g. different
+ * mstbs with CRTCs trained to different link rates/lane counts).
+ *
+ * Finally, any atomic commit which would result in going from an
+ * unrecoverable link state to a properly trained link state must also take
+ * care of appropriately updating the link status properties of all connectors
+ * on the topology from bad to good. This function takes care of all of these
+ * checks in @state for the topology manager @mgr including possibly pulling
+ * in additional CRTCs into the modeset, and possibly updating the link status
+ * of each mstb's DRM connector.
+ *
+ * This function should be called within the driver's atomic check callbacks
+ * whenever a modeset happens on an MST connector with it's link status set to
+ * DRM_MODE_LINK_STATUS_BAD.
+ *
+ * RETURNS:
+ *
+ * Returns 0 for success, or negative error code for failure.
+ */
+int drm_atomic_dp_mst_retrain_topology(struct drm_atomic_state *state,
+				       struct drm_dp_mst_topology_mgr *mgr)
+{
+	struct drm_dp_mst_branch *mstb =
+		drm_dp_get_validated_mstb_ref(mgr, mgr->mst_primary);
+	int ret = 0;
+	bool need_modeset = false;
+
+	if (!mstb)
+		return 0;
+
+	if (drm_atomic_dp_mst_state_only_disables_mstbs(state, mgr, mstb)) {
+		ret = drm_atomic_dp_mst_all_mstbs_disabled(state, mgr, mstb);
+		if (ret < 0)
+			goto out;
+
+		if (ret)
+			DRM_DEBUG_ATOMIC("state %p disables all CRTCs on mst mgr %p\n",
+					 state, mgr);
+		else
+			goto out; /* valid, but doesn't retrain link */
+	} else {
+		DRM_DEBUG_ATOMIC("state %p requires full modeset for CRTCs on mst mgr %p\n",
+				 state, mgr);
+		need_modeset = true;
+	}
+
+	ret = drm_atomic_dp_mst_retrain_mstb(state, mgr, mstb, need_modeset);
+out:
+	drm_dp_put_mst_branch_device(mgr->mst_primary);
+	return ret;
+}
+EXPORT_SYMBOL(drm_atomic_dp_mst_retrain_topology);
+
 /**
  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
  * @mgr: manager to set state for

index 6261ec43a2c0..24075eb2dd1b 100644
@@ -636,5 +636,7 @@ int drm_dp_send_power_updown_phy(struct drm_dp_mst_topology_mgr *mgr,
 
 int drm_dp_mst_topology_mgr_lower_link_rate(struct drm_dp_mst_topology_mgr *mgr,
 					    int dp_link_bw, int dp_link_count);
+int drm_atomic_dp_mst_retrain_topology(struct drm_atomic_state *state,
+				       struct drm_dp_mst_topology_mgr *mgr);
 
 #endif

index 5645a194de92..7626652732b6 100644
@@ -45,6 +45,8 @@
 
 #define DP_DPRX_ESI_LEN 14
 
+#define DP_MST_RETRAIN_TIMEOUT (msecs_to_jiffies(100))
+
 /* Compliance test status bits  */
 #define INTEL_DP_RESOLUTION_SHIFT_MASK	0
 #define INTEL_DP_RESOLUTION_PREFERRED	(1 << INTEL_DP_RESOLUTION_SHIFT_MASK)

@@ -4224,6 +4226,118 @@ static void intel_dp_handle_test_request(struct intel_dp *intel_dp)
 		DRM_DEBUG_KMS("Could not write test response to sink\n");
 }
 
+/* Get a mask of the CRTCs that are running on the given intel_dp struct. For
+ * MST, this returns a crtc mask containing all of the CRTCs driving
+ * downstream sinks, for SST it just returns a mask of the attached
+ * connector's CRTC.
+ */
+int
+intel_dp_get_crtc_mask(struct intel_dp *intel_dp)
+{
+	struct drm_device *dev = dp_to_dig_port(intel_dp)->base.base.dev;
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	struct intel_connector *intel_connector;
+	struct drm_crtc *crtc;
+	int crtc_mask = 0;
+
+	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
+
+	if (intel_dp->is_mst) {
+		struct drm_connector_list_iter conn_iter;
+
+		drm_connector_list_iter_begin(dev, &conn_iter);
+		for_each_intel_connector_iter(intel_connector, &conn_iter) {
+			if (intel_connector->mst_port != intel_dp)
+				continue;
+
+			conn_state = intel_connector->base.state;
+			if (!conn_state->crtc)
+				continue;
+
+			crtc_mask |= drm_crtc_mask(conn_state->crtc);
+		}
+		drm_connector_list_iter_end(&conn_iter);
+	} else {
+		connector = &intel_dp->attached_connector->base;
+		crtc = connector->state->crtc;
+
+		if (crtc)
+			crtc_mask |= drm_crtc_mask(crtc);
+	}
+
+	return crtc_mask;
+}
+
+static bool
+intel_dp_needs_link_retrain(struct intel_dp *intel_dp,
+			    const u8 esi[DP_DPRX_ESI_LEN])
+{
+	u8 buf[max(DP_LINK_STATUS_SIZE, DP_DPRX_ESI_LEN)];
+	const u8 *link_status = NULL;
+
+	if (intel_dp->is_mst) {
+		if (!intel_dp->active_mst_links)
+			return false;
+		if (intel_dp->mst_link_is_bad)
+			return false;
+
+		if (esi) {
+			link_status = &esi[10];
+		} else {
+			/* We're not running from the ESI handler, so wait a
+			 * little bit to see if the ESI handler lets us know
+			 * that the link status is OK
+			 */
+			if (wait_for_completion_timeout(
+				&intel_dp->mst_retrain_completion,
+				DP_MST_RETRAIN_TIMEOUT))
+				return false;
+		}
+	} else {
+		if (intel_dp->link_trained)
+			return false;
+		if (!intel_dp_get_link_status(intel_dp, buf))
+			return false;
+
+		link_status = buf;
+	}
+
+	/*
+	 * Validate the cached values of intel_dp->link_rate and
+	 * intel_dp->lane_count before attempting to retrain.
+	 */
+	if (!intel_dp_link_params_valid(intel_dp, intel_dp->link_rate,
+					intel_dp->lane_count))
+		return false;
+
+	if (link_status) {
+		return !drm_dp_channel_eq_ok(link_status,
+					     intel_dp->lane_count);
+	} else {
+		return true;
+	}
+}
+
+static inline void
+intel_dp_mst_check_link_status(struct intel_dp *intel_dp,
+			       const u8 esi[DP_DPRX_ESI_LEN])
+{
+	if (intel_dp_needs_link_retrain(intel_dp, esi)) {
+		DRM_DEBUG_KMS("Channel EQ failing\n");
+
+		if (!work_busy(&intel_dp->mst_retrain_work)) {
+			reinit_completion(&intel_dp->mst_retrain_completion);
+			schedule_work(&intel_dp->mst_retrain_work);
+			DRM_DEBUG_KMS("Retraining started\n");
+		}
+	} else if (work_busy(&intel_dp->mst_retrain_work) &&
+		   !completion_done(&intel_dp->mst_retrain_completion)) {
+		DRM_DEBUG_KMS("Channel EQ stable\n");
+		complete_all(&intel_dp->mst_retrain_completion);
+	}
+}
+
 static int
 intel_dp_check_mst_status(struct intel_dp *intel_dp)
 {

@@ -4237,14 +4351,7 @@ intel_dp_check_mst_status(struct intel_dp *intel_dp)
 		bret = intel_dp_get_sink_irq_esi(intel_dp, esi);
 go_again:
 		if (bret == true) {
-
-			/* check link status - esi[10] = 0x200c */
-			if (intel_dp->active_mst_links &&
-			    !drm_dp_channel_eq_ok(&esi[10], intel_dp->lane_count)) {
-				DRM_DEBUG_KMS("channel EQ not ok, retraining\n");
-				intel_dp_start_link_train(intel_dp);
-				intel_dp_stop_link_train(intel_dp);
-			}
+			intel_dp_mst_check_link_status(intel_dp, esi);
 
 			DRM_DEBUG_KMS("got esi %3ph\n", esi);
 			ret = drm_dp_mst_hpd_irq(&intel_dp->mst_mgr, esi, &handled);

@@ -4281,29 +4388,6 @@ intel_dp_check_mst_status(struct intel_dp *intel_dp)
 	return -EINVAL;
 }
 
-static bool
-intel_dp_needs_link_retrain(struct intel_dp *intel_dp)
-{
-	u8 link_status[DP_LINK_STATUS_SIZE];
-
-	if (!intel_dp->link_trained)
-		return false;
-
-	if (!intel_dp_get_link_status(intel_dp, link_status))
-		return false;
-
-	/*
-	 * Validate the cached values of intel_dp->link_rate and
-	 * intel_dp->lane_count before attempting to retrain.
-	 */
-	if (!intel_dp_link_params_valid(intel_dp, intel_dp->link_rate,
-					intel_dp->lane_count))
-		return false;
-
-	/* Retrain if Channel EQ or CR not ok */
-	return !drm_dp_channel_eq_ok(link_status, intel_dp->lane_count);
-}
-
 /*
  * If display is now connected check links status,
  * there has been known issues of link loss triggering

@@ -4319,64 +4403,78 @@ intel_dp_needs_link_retrain(struct intel_dp *intel_dp)
 int intel_dp_retrain_link(struct intel_encoder *encoder,
 			  struct drm_modeset_acquire_ctx *ctx)
 {
-	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
-	struct intel_connector *connector = intel_dp->attached_connector;
-	struct drm_connector_state *conn_state;
-	struct intel_crtc_state *crtc_state;
-	struct intel_crtc *crtc;
+	struct drm_crtc *crtc;
+	struct intel_crtc *intel_crtc;
+	int crtc_mask, retry_count = 0;
 	int ret;
 
-	/* FIXME handle the MST connectors as well */
-
-	if (!connector || connector->base.status != connector_status_connected)
-		return 0;
-
 	ret = drm_modeset_lock(&dev_priv->drm.mode_config.connection_mutex,
 			       ctx);
 	if (ret)
 		return ret;
 
-	conn_state = connector->base.state;
-
-	crtc = to_intel_crtc(conn_state->crtc);
-	if (!crtc)
-		return 0;
+	crtc_mask = intel_dp_get_crtc_mask(intel_dp);
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask) {
+		struct drm_crtc_state *crtc_state;
+		struct intel_crtc_state *intel_crtc_state;
 
-	ret = drm_modeset_lock(&crtc->base.mutex, ctx);
-	if (ret)
-		return ret;
+		crtc = &intel_crtc->base;
+		ret = drm_modeset_lock(&crtc->mutex, ctx);
+		if (ret)
+			return ret;
 
-	crtc_state = to_intel_crtc_state(crtc->base.state);
+		crtc_state = crtc->state;
+		intel_crtc_state = to_intel_crtc_state(crtc_state);
+		WARN_ON(!intel_crtc_has_dp_encoder(intel_crtc_state));
 
-	WARN_ON(!intel_crtc_has_dp_encoder(crtc_state));
+		if (crtc_state->commit &&
+		    !try_wait_for_completion(&crtc_state->commit->hw_done))
+			return 0;
+	}
 
-	if (!crtc_state->base.active)
+	if (!intel_dp_needs_link_retrain(intel_dp, NULL))
 		return 0;
 
-	if (conn_state->commit &&
-	    !try_wait_for_completion(&conn_state->commit->hw_done))
-		return 0;
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask) {
+		intel_set_cpu_fifo_underrun_reporting(
+		    dev_priv, intel_crtc->pipe, false);
 
-	if (!intel_dp_needs_link_retrain(intel_dp))
-		return 0;
+		if (intel_crtc->config->has_pch_encoder) {
+			intel_set_pch_fifo_underrun_reporting(
+			    dev_priv, intel_crtc_pch_transcoder(intel_crtc),
+			    false);
+		}
+	}
 
-	/* Suppress underruns caused by re-training */
-	intel_set_cpu_fifo_underrun_reporting(dev_priv, crtc->pipe, false);
-	if (crtc->config->has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev_priv,
-						      intel_crtc_pch_transcoder(crtc), false);
+	do {
+		if (++retry_count > 5) {
+			DRM_DEBUG_KMS("Too many retries, can't retrain\n");
+			return -EINVAL;
+		}
 
-	intel_dp_start_link_train(intel_dp);
-	intel_dp_stop_link_train(intel_dp);
+		intel_dp_start_link_train(intel_dp);
+		intel_dp_stop_link_train(intel_dp);
+	} while (intel_dp_needs_link_retrain(intel_dp, NULL));
+
+	/* Wait for things to become stable */
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask)
+		intel_wait_for_vblank(dev_priv, intel_crtc->pipe);
 
-	/* Keep underrun reporting disabled until things are stable */
-	intel_wait_for_vblank(dev_priv, crtc->pipe);
+	/* Now that we know everything is OK, finally re-enable underrun
+	 * reporting */
+	for_each_intel_crtc_mask(dev, intel_crtc, crtc_mask) {
+		intel_set_cpu_fifo_underrun_reporting(
+		    dev_priv, intel_crtc->pipe, true);
 
-	intel_set_cpu_fifo_underrun_reporting(dev_priv, crtc->pipe, true);
-	if (crtc->config->has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev_priv,
-						      intel_crtc_pch_transcoder(crtc), true);
+		if (intel_crtc->config->has_pch_encoder) {
+			intel_set_pch_fifo_underrun_reporting(
+			    dev_priv, intel_crtc_pch_transcoder(intel_crtc),
+			    true);
+		}
+	}
 
 	return 0;
 }

@@ -4402,6 +4500,10 @@ static bool intel_dp_hotplug(struct intel_encoder *encoder,
 
 	changed = intel_encoder_hotplug(encoder, connector);
 
+	/* We don't want to end up trying to retrain MST links! */
+	if (encoder && enc_to_intel_dp(&encoder->base)->is_mst)
+		return changed;
+
 	drm_modeset_acquire_init(&ctx, 0);
 
 	for (;;) {

@@ -4478,7 +4580,7 @@ intel_dp_short_pulse(struct intel_dp *intel_dp)
 	}
 
 	/* defer to the hotplug work for link retraining if needed */
-	if (intel_dp_needs_link_retrain(intel_dp))
+	if (intel_dp_needs_link_retrain(intel_dp, NULL))
 		return false;
 
 	if (intel_dp->compliance.test_type == DP_TEST_LINK_TRAINING) {

@@ -6266,25 +6368,98 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	return false;
 }
 
+static void intel_dp_mst_retrain_link_work(struct work_struct *work)
+{
+	struct drm_modeset_acquire_ctx ctx;
+	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
+						 mst_retrain_work);
+	struct intel_encoder *intel_encoder = &dp_to_dig_port(intel_dp)->base;
+	struct drm_device *dev = intel_encoder->base.dev;
+	int ret;
+	bool had_error = false;
+
+	drm_modeset_acquire_init(&ctx, 0);
+
+	for (;;) {
+		ret = intel_dp_retrain_link(intel_encoder, &ctx);
+		if (ret == -EDEADLK) {
+			drm_modeset_backoff(&ctx);
+			continue;
+		}
+
+		break;
+	}
+	if (!ret) {
+		DRM_DEBUG_KMS("Retrain complete\n");
+		goto out;
+	} else if (ret == -EIO) {
+		DRM_ERROR("IO error with sink during retrain? Aborting\n");
+		had_error = true;
+		goto out;
+	}
+
+	DRM_DEBUG_KMS("Retraining failed with %d, marking link status as bad\n",
+		      ret);
+
+	/* We ran out of retries, if the sink hasn't changed the link rate in
+	 * it's dpcd yet force us to fallback to a lower link rate/count */
+	if (ret == -EINVAL) {
+		ret = intel_dp_get_dpcd(intel_dp);
+		if (!ret) {
+			DRM_ERROR("IO error while reading dpcd from sink\n");
+			had_error = true;
+			goto out;
+		}
+
+		if (intel_dp->link_rate == intel_dp_max_link_rate(intel_dp) &&
+		    intel_dp->lane_count == intel_dp_max_lane_count(intel_dp)) {
+			intel_dp_get_link_train_fallback_values(
+			    intel_dp, intel_dp_max_link_rate(intel_dp),
+			    intel_dp_max_lane_count(intel_dp));
+		}
+	}
+
+	intel_dp->mst_link_is_bad = true;
+	intel_dp->mst_bw_locked = false;
+	schedule_work(&intel_dp->modeset_retry_work);
+out:
+	drm_modeset_drop_locks(&ctx);
+	drm_modeset_acquire_fini(&ctx);
+	if (had_error)
+		drm_kms_helper_hotplug_event(dev);
+}
+
 static void intel_dp_modeset_retry_work_fn(struct work_struct *work)
 {
 	struct intel_dp *intel_dp = container_of(work, typeof(*intel_dp),
 						 modeset_retry_work);
-	struct drm_connector *connector = &intel_dp->attached_connector->base;
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_connector *connector;
 
-	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
-		      connector->name);
+	mutex_lock(&dev->mode_config.mutex);
 
-	/* Grab the locks before changing connector property*/
-	mutex_lock(&connector->dev->mode_config.mutex);
-	/* Set connector link status to BAD and send a Uevent to notify
-	 * userspace to do a modeset.
+	/* Set the connector link status of all (possibly downstream) ports to
+	 * BAD and send a Uevent to notify userspace to do a modeset.
 	 */
-	drm_mode_connector_set_link_status_property(connector,
-						    DRM_MODE_LINK_STATUS_BAD);
-	mutex_unlock(&connector->dev->mode_config.mutex);
+	if (intel_dp->is_mst) {
+		drm_dp_mst_topology_mgr_lower_link_rate(
+		    &intel_dp->mst_mgr,
+		    intel_dp_max_link_rate(intel_dp),
+		    intel_dp_max_lane_count(intel_dp));
+	} else {
+		connector = &intel_dp->attached_connector->base;
+
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
+			      connector->base.id, connector->name);
+		drm_mode_connector_set_link_status_property(
+		    connector, DRM_MODE_LINK_STATUS_BAD);
+	}
+
+	mutex_unlock(&dev->mode_config.mutex);
+
 	/* Send Hotplug uevent so userspace can reprobe */
-	drm_kms_helper_hotplug_event(connector->dev);
+	drm_kms_helper_hotplug_event(dev);
 }
 
 bool

@@ -6302,6 +6477,9 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	/* Initialize the work for modeset in case of link train failure */
 	INIT_WORK(&intel_dp->modeset_retry_work,
 		  intel_dp_modeset_retry_work_fn);
+	INIT_WORK(&intel_dp->mst_retrain_work,
+		  intel_dp_mst_retrain_link_work);
+	init_completion(&intel_dp->mst_retrain_completion);
 
 	if (WARN(intel_dig_port->max_lanes < 1,
 		 "Not enough lanes (%d) for DP on port %c\n",

70 lines not on this page · raw message

index fc338529e918..f4a5861e4dff 100644
@@ -1119,6 +1119,13 @@ struct intel_dp {
 	/* mst connector list */
 	struct intel_dp_mst_encoder *mst_encoders[I915_MAX_PIPES];
 	struct drm_dp_mst_topology_mgr mst_mgr;
+	/* We can't handle retraining from the dig workqueue, so... */
+	struct work_struct mst_retrain_work;
+	struct completion mst_retrain_completion;
+	/* Set when retraining the link at the current parameters is
+	 * impossible for an MST connection
+	 */
+	bool mst_link_is_bad;
 
 	uint32_t (*get_aux_clock_divider)(struct intel_dp *dp, int index);
 	/*

@@ -1686,6 +1693,7 @@ void intel_dp_compute_rate(struct intel_dp *intel_dp, int port_clock,
 bool intel_dp_source_supports_hbr2(struct intel_dp *intel_dp);
 bool
 intel_dp_get_link_status(struct intel_dp *intel_dp, uint8_t link_status[DP_LINK_STATUS_SIZE]);
+int intel_dp_get_crtc_mask(struct intel_dp *intel_dp);
 
 static inline unsigned int intel_dp_unused_lane_mask(int lane_count)
 {