1 files changed, 53 insertions, 4 deletions

diff --git a/drivers/gpu/drm/i915/intel_lspcon.c b/drivers/gpu/drm/i915/intel_lspcon.c
index 632149c6b3ad..daa523410953 100644
--- a/drivers/gpu/drm/i915/intel_lspcon.c
+++ b/drivers/gpu/drm/i915/intel_lspcon.c
@@ -27,10 +27,18 @@
 #include <drm/drm_dp_dual_mode_helper.h>
 #include "intel_drv.h"
 
+static struct intel_dp *lspcon_to_intel_dp(struct intel_lspcon *lspcon)
+{
+	struct intel_digital_port *dig_port =
+		container_of(lspcon, struct intel_digital_port, lspcon);
+
+	return &dig_port->dp;
+}
+
 static enum drm_lspcon_mode lspcon_get_current_mode(struct intel_lspcon *lspcon)
 {
 	enum drm_lspcon_mode current_mode = DRM_LSPCON_MODE_INVALID;
-	struct i2c_adapter *adapter = &lspcon->aux->ddc;
+	struct i2c_adapter *adapter = &lspcon_to_intel_dp(lspcon)->aux.ddc;
 
 	if (drm_lspcon_get_mode(adapter, &current_mode))
 		DRM_ERROR("Error reading LSPCON mode\n");
@@ -45,7 +53,7 @@ static int lspcon_change_mode(struct intel_lspcon *lspcon,
 {
 	int err;
 	enum drm_lspcon_mode current_mode;
-	struct i2c_adapter *adapter = &lspcon->aux->ddc;
+	struct i2c_adapter *adapter = &lspcon_to_intel_dp(lspcon)->aux.ddc;
 
 	err = drm_lspcon_get_mode(adapter, &current_mode);
 	if (err) {
@@ -72,7 +80,7 @@ static int lspcon_change_mode(struct intel_lspcon *lspcon,
 static bool lspcon_probe(struct intel_lspcon *lspcon)
 {
 	enum drm_dp_dual_mode_type adaptor_type;
-	struct i2c_adapter *adapter = &lspcon->aux->ddc;
+	struct i2c_adapter *adapter = &lspcon_to_intel_dp(lspcon)->aux.ddc;
 
 	/* Lets probe the adaptor and check its type */
 	adaptor_type = drm_dp_dual_mode_detect(adapter);
@@ -89,8 +97,43 @@ static bool lspcon_probe(struct intel_lspcon *lspcon)
 	return true;
 }
 
+static void lspcon_resume_in_pcon_wa(struct intel_lspcon *lspcon)
+{
+	struct intel_dp *intel_dp = lspcon_to_intel_dp(lspcon);
+	unsigned long start = jiffies;
+
+	if (!lspcon->desc_valid)
+		return;
+
+	while (1) {
+		struct intel_dp_desc desc;
+
+		/*
+		 * The w/a only applies in PCON mode and we don't expect any
+		 * AUX errors.
+		 */
+		if (!__intel_dp_read_desc(intel_dp, &desc))
+			return;
+
+		if (!memcmp(&intel_dp->desc, &desc, sizeof(desc))) {
+			DRM_DEBUG_KMS("LSPCON recovering in PCON mode after %u ms\n",
+				      jiffies_to_msecs(jiffies - start));
+			return;
+		}
+
+		if (time_after(jiffies, start + msecs_to_jiffies(1000)))
+			break;
+
+		usleep_range(10000, 15000);
+	}
+
+	DRM_DEBUG_KMS("LSPCON DP descriptor mismatch after resume\n");
+}
+
 void lspcon_resume(struct intel_lspcon *lspcon)
 {
+	lspcon_resume_in_pcon_wa(lspcon);
+
 	if (lspcon_change_mode(lspcon, DRM_LSPCON_MODE_PCON, true))
 		DRM_ERROR("LSPCON resume failed\n");
 	else
@@ -111,7 +154,6 @@ bool lspcon_init(struct intel_digital_port *intel_dig_port)
 
 	lspcon->active = false;
 	lspcon->mode = DRM_LSPCON_MODE_INVALID;
-	lspcon->aux = &dp->aux;
 
 	if (!lspcon_probe(lspcon)) {
 		DRM_ERROR("Failed to probe lspcon\n");
@@ -131,6 +173,13 @@ bool lspcon_init(struct intel_digital_port *intel_dig_port)
 		}
 	}
 
+	if (!intel_dp_read_dpcd(dp)) {
+		DRM_ERROR("LSPCON DPCD read failed\n");
+		return false;
+	}
+
+	lspcon->desc_valid = intel_dp_read_desc(dp);
+
 	DRM_DEBUG_KMS("Success: LSPCON init\n");
 	return true;
 }