From c4ceb8934dbfa51bc966b927b17394c4b622712c Mon Sep 17 00:00:00 2001
From: Tomi Valkeinen <tomi.valkeinen@ti.com>
Date: Fri, 30 Jun 2017 11:39:53 +0300
Subject: [PATCH] drm/omap: hack error worker

---
 drivers/gpu/drm/omapdrm/omap_crtc.c | 69 ++++++++++++++++++++++++++++++++-----
 1 file changed, 61 insertions(+), 8 deletions(-)

diff --git a/drivers/gpu/drm/omapdrm/omap_crtc.c b/drivers/gpu/drm/omapdrm/omap_crtc.c
index dccd03726796..eb36b35f5eb8 100644
--- a/drivers/gpu/drm/omapdrm/omap_crtc.c
+++ b/drivers/gpu/drm/omapdrm/omap_crtc.c
@@ -36,12 +36,15 @@ struct omap_crtc {
 
 	struct videomode vm;
 
-	bool ignore_digit_sync_lost;
+	bool ignore_sync_lost;
 
 	bool enabled;
 	bool pending;
 	wait_queue_head_t pending_wait;
 	struct drm_pending_vblank_event *event;
+
+	struct work_struct error_work;
+	u32 error_channels;
 };
 
 /* -----------------------------------------------------------------------------
@@ -157,7 +160,7 @@ static void omap_crtc_set_enabled(struct drm_crtc *crtc, bool enable)
 		 * Digit output produces some sync lost interrupts during the
 		 * first frame when enabling, so we need to ignore those.
 		 */
-		omap_crtc->ignore_digit_sync_lost = true;
+		omap_crtc->ignore_sync_lost = true;
 	}
 
 	framedone_irq = priv->dispc_ops->mgr_get_framedone_irq(channel);
@@ -191,7 +194,7 @@ static void omap_crtc_set_enabled(struct drm_crtc *crtc, bool enable)
 	}
 
 	if (omap_crtc->channel == OMAP_DSS_CHANNEL_DIGIT) {
-		omap_crtc->ignore_digit_sync_lost = false;
+		omap_crtc->ignore_sync_lost = false;
 		/* make sure the irq handler sees the value above */
 		mb();
 	}
@@ -263,17 +266,65 @@ static const struct dss_mgr_ops mgr_ops = {
  * Setup, Flush and Page Flip
  */
 
+static void omap_crtc_error_worker(struct work_struct *work)
+{
+	struct omap_crtc *omap_crtc = container_of(work, struct omap_crtc, error_work);
+	struct drm_crtc *crtc = &omap_crtc->base;
+	struct drm_device *dev = omap_crtc->base.dev;
+	struct omap_drm_private *priv = dev->dev_private;
+
+	drm_modeset_lock(&crtc->mutex, NULL);
+
+	dev_warn(dev->dev, "sync lost on %s, enabling & disabling...\n",
+		omap_crtc->name);
+
+	priv->dispc_ops->mgr_enable(omap_crtc->channel, false);
+
+	msleep(50);
+	dev_warn(dev->dev, "sync lost enabling %s\n",
+			omap_crtc->name);
+
+	priv->dispc_ops->mgr_enable(omap_crtc->channel, true);
+
+	msleep(50);
+
+	dev_warn(dev->dev, "sync lost recovery done on on %s\n",
+		omap_crtc->name);
+
+	omap_crtc->ignore_sync_lost = false;
+	/* make sure the irq handler sees the value above */
+	mb();
+
+	drm_modeset_unlock(&crtc->mutex);
+}
+
 void omap_crtc_error_irq(struct drm_crtc *crtc, uint32_t irqstatus)
 {
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	struct drm_device *dev = omap_crtc->base.dev;
+	struct omap_drm_private *priv = dev->dev_private;
+	enum omap_channel channel = omap_crtc_channel(crtc);
+	u32 sync_lost_irq;
+	bool sync_lost;
+
+	sync_lost_irq = priv->dispc_ops->mgr_get_sync_lost_irq(channel);
 
-	if (omap_crtc->ignore_digit_sync_lost) {
-		irqstatus &= ~DISPC_IRQ_SYNC_LOST_DIGIT;
-		if (!irqstatus)
-			return;
+	sync_lost = irqstatus & sync_lost_irq;
+
+	if (sync_lost) {
+		if (omap_crtc->ignore_sync_lost) {
+			irqstatus &= ~sync_lost_irq;
+		} else {
+			/* error worker will set this to false */
+			omap_crtc->ignore_sync_lost = true;
+			schedule_work(&omap_crtc->error_work);
+		}
 	}
 
-	DRM_ERROR_RATELIMITED("%s: errors: %08x\n", omap_crtc->name, irqstatus);
+	if (!irqstatus)
+		return;
+
+	printk("%s: errors: %08x\n", omap_crtc->name, irqstatus);
 }
 
 void omap_crtc_vblank_irq(struct drm_crtc *crtc)
@@ -612,6 +663,8 @@ struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 
 	init_waitqueue_head(&omap_crtc->pending_wait);
 
+	INIT_WORK(&omap_crtc->error_work, omap_crtc_error_worker);
+
 	omap_crtc->channel = channel;
 	omap_crtc->name = channel_names[channel];
 
-- 
2.7.4