From 8521d1baa2940c22c9eede1517ee7ff85738f751 Mon Sep 17 00:00:00 2001
From: Vasily Khoruzhick <anarsoul@gmail.com>
Date: Wed, 13 Mar 2019 19:50:17 -0700
Subject: drm/sun4i: Implement gamma correction

Add support for gamma corretion to sun4i TCON driver. Its LUT has 256
entries and can be updated only when gamma correction is disabled.

Signed-off-by: Vasily Khoruzhick <anarsoul@gmail.com>
---
 drivers/gpu/drm/sun4i/sun4i_crtc.c | 14 +++++++++++++
 drivers/gpu/drm/sun4i/sun4i_tcon.c | 33 ++++++++++++++++++++++++++++++
 drivers/gpu/drm/sun4i/sun4i_tcon.h | 12 ++++++++++-
 3 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/drivers/gpu/drm/sun4i/sun4i_crtc.c b/drivers/gpu/drm/sun4i/sun4i_crtc.c
index 18e74047b0f5..a09944a49e89 100644
--- a/drivers/gpu/drm/sun4i/sun4i_crtc.c
+++ b/drivers/gpu/drm/sun4i/sun4i_crtc.c
@@ -103,6 +103,20 @@ static void sun4i_crtc_atomic_flush(struct drm_crtc *crtc,
 			drm_crtc_send_vblank_event(crtc, event);
 		spin_unlock_irq(&crtc->dev->event_lock);
 	}
+
+	if (crtc->state->color_mgmt_changed) {
+		if (crtc->state->gamma_lut) {
+			/* LUT can be only updated when gamma correction is
+			 * disabled
+			 */
+			sun4i_tcon_enable_gamma(scrtc->tcon, false);
+			sun4i_tcon_load_gamma_lut(scrtc->tcon,
+						  crtc->state->gamma_lut->data);
+			sun4i_tcon_enable_gamma(scrtc->tcon, true);
+		} else
+			sun4i_tcon_enable_gamma(scrtc->tcon, false);
+	}
+
 }
 
 static void sun4i_crtc_atomic_disable(struct drm_crtc *crtc,
diff --git a/drivers/gpu/drm/sun4i/sun4i_tcon.c b/drivers/gpu/drm/sun4i/sun4i_tcon.c
index fca95b76e258..ac6ad2ed1cc0 100644
--- a/drivers/gpu/drm/sun4i/sun4i_tcon.c
+++ b/drivers/gpu/drm/sun4i/sun4i_tcon.c
@@ -240,6 +240,34 @@ void sun4i_tcon_enable_vblank(struct sun4i_tcon *tcon, bool enable)
 }
 EXPORT_SYMBOL(sun4i_tcon_enable_vblank);
 
+void sun4i_tcon_load_gamma_lut(struct sun4i_tcon *tcon,
+			       struct drm_color_lut *lut)
+{
+	int i;
+
+	for (i = 0; i < SUN4I_TCON_GAMMA_LUT_SIZE; i++) {
+		u32 r, g, b;
+
+		r = drm_color_lut_extract(lut[i].red, 8);
+		g = drm_color_lut_extract(lut[i].green, 8);
+		b = drm_color_lut_extract(lut[i].blue, 8);
+
+		regmap_write(tcon->regs, SUN4I_TCON_GAMMA_TABLE_REG + 4 * i,
+			     SUN4I_TCON_GAMMA_TABLE_R(r) |
+			     SUN4I_TCON_GAMMA_TABLE_G(g) |
+			     SUN4I_TCON_GAMMA_TABLE_B(b));
+	}
+}
+EXPORT_SYMBOL(sun4i_tcon_load_gamma_lut);
+
+void sun4i_tcon_enable_gamma(struct sun4i_tcon *tcon, bool enable)
+{
+	regmap_update_bits(tcon->regs, SUN4I_TCON_GCTL_REG,
+			   SUN4I_TCON_GCTL_GAMMA_ENABLE,
+			   enable ? SUN4I_TCON_GCTL_GAMMA_ENABLE : 0);
+}
+EXPORT_SYMBOL(sun4i_tcon_enable_gamma);
+
 /*
  * This function is a helper for TCON output muxing. The TCON output
  * muxing control register in earlier SoCs (without the TCON TOP block)
@@ -1288,6 +1316,11 @@ static int sun4i_tcon_bind(struct device *dev, struct device *master,
 
 	list_add_tail(&tcon->list, &drv->tcon_list);
 
+	drm_mode_crtc_set_gamma_size(&tcon->crtc->crtc,
+				     SUN4I_TCON_GAMMA_LUT_SIZE);
+	drm_crtc_enable_color_mgmt(&tcon->crtc->crtc, 0, false,
+				   tcon->crtc->crtc.gamma_size);
+
 	return 0;
 
 err_free_dclk:
diff --git a/drivers/gpu/drm/sun4i/sun4i_tcon.h b/drivers/gpu/drm/sun4i/sun4i_tcon.h
index fa23aa23fe4a..97df39db2a31 100644
--- a/drivers/gpu/drm/sun4i/sun4i_tcon.h
+++ b/drivers/gpu/drm/sun4i/sun4i_tcon.h
@@ -19,6 +19,7 @@
 
 #define SUN4I_TCON_GCTL_REG			0x0
 #define SUN4I_TCON_GCTL_TCON_ENABLE			BIT(31)
+#define SUN4I_TCON_GCTL_GAMMA_ENABLE			BIT(30)
 #define SUN4I_TCON_GCTL_IOMAP_MASK			BIT(0)
 #define SUN4I_TCON_GCTL_IOMAP_TCON1			(1 << 0)
 #define SUN4I_TCON_GCTL_IOMAP_TCON0			(0 << 0)
@@ -229,7 +230,13 @@
 #define SUN4I_TCON1_FILL_BEG2_REG		0x31c
 #define SUN4I_TCON1_FILL_END2_REG		0x320
 #define SUN4I_TCON1_FILL_DATA2_REG		0x324
-#define SUN4I_TCON1_GAMMA_TABLE_REG		0x400
+
+#define SUN4I_TCON_GAMMA_TABLE_REG		0x400
+#define SUN4I_TCON_GAMMA_TABLE_B(x)		((x) & 0xff)
+#define SUN4I_TCON_GAMMA_TABLE_G(x)		(((x) & 0xff) << 8)
+#define SUN4I_TCON_GAMMA_TABLE_R(x)		(((x) & 0xff) << 16)
+
+#define SUN4I_TCON_GAMMA_LUT_SIZE		256
 
 #define SUN4I_TCON_MAX_CHANNELS		2
 
@@ -297,6 +304,9 @@ void sun4i_tcon_mode_set(struct sun4i_tcon *tcon,
 			 const struct drm_display_mode *mode);
 void sun4i_tcon_set_status(struct sun4i_tcon *crtc,
 			   const struct drm_encoder *encoder, bool enable);
+void sun4i_tcon_load_gamma_lut(struct sun4i_tcon *tcon,
+			       struct drm_color_lut *lut);
+void sun4i_tcon_enable_gamma(struct sun4i_tcon *tcon, bool enable);
 
 extern const struct of_device_id sun4i_tcon_of_table[];
 
-- 
2.35.3

