From 2024e43f6794a367c891faef9ab101dc6a9d0f25 Mon Sep 17 00:00:00 2001
From: Peter Seiderer <ps.report@gmx.net>
Date: Fri, 29 Nov 2013 17:04:22 +0100
Subject: [PATCH] imx-drm: gamma correction for imx-ldb
To: dri-devel@lists.freedesktop.org
Cc: Greg Kroah-Hartman <gregkh@linuxfoundation.org>,
    Sascha Hauer <kernel@pengutronix.de>

Signed-off-by: Peter Seiderer <ps.report@gmx.net>
---
 arch/arm/boot/dts/imx6q-sabrelite.dts       |  3 +++
 drivers/staging/imx-drm/imx-drm-core.c      | 27 +++++++++++++++++++++++
 drivers/staging/imx-drm/imx-drm.h           |  4 ++++
 drivers/staging/imx-drm/imx-ldb.c           | 18 ++++++++++++++++
 drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h |  2 ++
 drivers/staging/imx-drm/ipu-v3/ipu-dp.c     | 33 +++++++++++++++++++++++++++++
 drivers/staging/imx-drm/ipuv3-crtc.c        |  9 ++++++++
 7 files changed, 96 insertions(+)

diff --git a/arch/arm/boot/dts/imx6q-sabrelite.dts b/arch/arm/boot/dts/imx6q-sabrelite.dts
index fca8f220..5dabc45 100644
--- a/arch/arm/boot/dts/imx6q-sabrelite.dts
+++ b/arch/arm/boot/dts/imx6q-sabrelite.dts
@@ -171,6 +171,9 @@
 	lvds-channel@0 {
 		fsl,data-mapping = "spwg";
 		fsl,data-width = <18>;
+		/* gamma = 0.6 */
+		fsl,gamma-constk = /bits/ 16 <0x000 0x000 0x000 0x000 0x1ff 0x001 0x009 0x015 0x025 0x037 0x04d 0x064 0x07f 0x09c 0x0bb 0x0dc>;
+		fsl,gamma-slopek = /bits/ 16 <0x000 0x000 0x000 0x000 0x002 0x008 0x00c 0x010 0x012 0x016 0x017 0x01b 0x01d 0x01f 0x021 0x022>;
 		status = "okay";
 
 		display-timings {
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/staging/imx-drm/imx-drm-core.c
index 0507b66..6e205fb 100644
--- a/drivers/staging/imx-drm/imx-drm-core.c
+++ b/drivers/staging/imx-drm/imx-drm-core.c
@@ -139,6 +139,33 @@ int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
 }
 EXPORT_SYMBOL_GPL(imx_drm_crtc_panel_format);
 
+int imx_drm_crtc_panel_gamma(struct drm_crtc *crtc,
+		u16 *gamma_constk, u16 *gamma_slopek)
+{
+	struct imx_drm_device *imxdrm = crtc->dev->dev_private;
+	struct imx_drm_crtc *imx_crtc;
+	struct imx_drm_crtc_helper_funcs *helper;
+
+	mutex_lock(&imxdrm->mutex);
+
+	list_for_each_entry(imx_crtc, &imxdrm->crtc_list, list)
+		if (imx_crtc->crtc == crtc)
+			goto found;
+
+	mutex_unlock(&imxdrm->mutex);
+
+	return -EINVAL;
+found:
+	mutex_unlock(&imxdrm->mutex);
+
+	helper = &imx_crtc->imx_drm_helper_funcs;
+	if (helper->set_interface_gamma)
+		return helper->set_interface_gamma(crtc,
+				gamma_constk, gamma_slopek);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_drm_crtc_panel_gamma);
+
 int imx_drm_crtc_vblank_get(struct imx_drm_crtc *imx_drm_crtc)
 {
 	return drm_vblank_get(imx_drm_crtc->imxdrm->drm, imx_drm_crtc->pipe);
diff --git a/drivers/staging/imx-drm/imx-drm.h b/drivers/staging/imx-drm/imx-drm.h
index ae90c9c..995e29f 100644
--- a/drivers/staging/imx-drm/imx-drm.h
+++ b/drivers/staging/imx-drm/imx-drm.h
@@ -21,6 +21,8 @@ struct imx_drm_crtc_helper_funcs {
 	void (*disable_vblank)(struct drm_crtc *crtc);
 	int (*set_interface_pix_fmt)(struct drm_crtc *crtc, u32 encoder_type,
 			u32 pix_fmt, int hsync_pin, int vsync_pin);
+	int (*set_interface_gamma)(struct drm_crtc *crtc,
+			u16 *gamma_constk, u16 *gamma_slopek);
 	const struct drm_crtc_helper_funcs *crtc_helper_funcs;
 	const struct drm_crtc_funcs *crtc_funcs;
 };
@@ -60,6 +62,8 @@ int imx_drm_crtc_panel_format_pins(struct drm_crtc *crtc, u32 encoder_type,
 		u32 interface_pix_fmt, int hsync_pin, int vsync_pin);
 int imx_drm_crtc_panel_format(struct drm_crtc *crtc, u32 encoder_type,
 		u32 interface_pix_fmt);
+int imx_drm_crtc_panel_gamma(struct drm_crtc *crtc,
+		u16 *gamma_constk, u16 *gamma_slopek);
 void imx_drm_fb_helper_set(struct drm_fbdev_cma *fbdev_helper);
 
 struct device_node;
diff --git a/drivers/staging/imx-drm/imx-ldb.c b/drivers/staging/imx-drm/imx-ldb.c
index 7e59329..5dc9d6c 100644
--- a/drivers/staging/imx-drm/imx-ldb.c
+++ b/drivers/staging/imx-drm/imx-ldb.c
@@ -64,6 +64,8 @@ struct imx_ldb_channel {
 	int chno;
 	void *edid;
 	int edid_len;
+	u16 *gamma_constk;
+	u16 *gamma_slopek;
 	struct drm_display_mode mode;
 	int mode_valid;
 };
@@ -209,6 +211,10 @@ static void imx_ldb_encoder_prepare(struct drm_encoder *encoder)
 
 	imx_drm_crtc_panel_format(encoder->crtc, DRM_MODE_ENCODER_LVDS,
 			pixel_fmt);
+
+	if (imx_ldb_ch->gamma_constk && imx_ldb_ch->gamma_slopek)
+		imx_drm_crtc_panel_gamma(encoder->crtc, imx_ldb_ch->gamma_constk,
+				imx_ldb_ch->gamma_slopek);
 }
 
 static void imx_ldb_encoder_commit(struct drm_encoder *encoder)
@@ -468,6 +474,7 @@ static int imx_ldb_probe(struct platform_device *pdev)
 	const u8 *edidp;
 	struct imx_ldb *imx_ldb;
 	int datawidth;
+	u16 gamma_val[16];
 	int mapping;
 	int dual;
 	int ret;
@@ -548,6 +555,14 @@ static int imx_ldb_probe(struct platform_device *pdev)
 		else if (datawidth != 18 && datawidth != 24)
 			return -EINVAL;
 
+		ret = of_property_read_u16_array(child, "fsl,gamma-constk", gamma_val, 16);
+		if (!ret)
+			channel->gamma_constk = kmemdup(gamma_val, sizeof(u16) * 16, GFP_KERNEL);
+
+		ret = of_property_read_u16_array(child, "fsl,gamma-slopek", gamma_val, 16);
+		if (!ret)
+			channel->gamma_slopek = kmemdup(gamma_val, sizeof(u16) * 16, GFP_KERNEL);
+
 		mapping = of_get_data_mapping(child);
 		switch (mapping) {
 		case LVDS_BIT_MAP_SPWG:
@@ -599,6 +614,9 @@ static int imx_ldb_remove(struct platform_device *pdev)
 
 		imx_drm_remove_connector(channel->imx_drm_connector);
 		imx_drm_remove_encoder(channel->imx_drm_encoder);
+
+		kfree(channel->gamma_constk);
+		kfree(channel->gamma_slopek);
 	}
 
 	return 0;
diff --git a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
index 4826b5c..6f821dc 100644
--- a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
+++ b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
@@ -154,6 +154,8 @@ int ipu_dp_enable_channel(struct ipu_dp *dp);
 void ipu_dp_disable_channel(struct ipu_dp *dp);
 int ipu_dp_setup_channel(struct ipu_dp *dp,
 		enum ipu_color_space in, enum ipu_color_space out);
+int ipu_dp_set_gamma(struct ipu_dp *dp,
+                u16 *gamma_constk, u16 *gamma_slopek);
 int ipu_dp_set_window_pos(struct ipu_dp *, u16 x_pos, u16 y_pos);
 int ipu_dp_set_global_alpha(struct ipu_dp *dp, bool enable, u8 alpha,
 		bool bg_chan);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
index 58f87c8..56ab92e 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
@@ -29,6 +29,8 @@
 #define DP_COM_CONF		0x0
 #define DP_GRAPH_WIND_CTRL	0x0004
 #define DP_FG_POS		0x0008
+#define DP_GAMMA_C_SYNC		0x0014
+#define DP_GAMMA_S_SYNC		0x0034
 #define DP_CSC_A_0		0x0044
 #define DP_CSC_A_1		0x0048
 #define DP_CSC_A_2		0x004C
@@ -45,6 +47,7 @@
 #define DP_COM_CONF_CSC_DEF_FG		(3 << 8)
 #define DP_COM_CONF_CSC_DEF_BG		(2 << 8)
 #define DP_COM_CONF_CSC_DEF_BOTH	(1 << 8)
+#define DP_COM_CONF_GAMMA_EN		(1 << 12)
 
 #define IPUV3_NUM_FLOWS		3
 
@@ -215,6 +218,36 @@ int ipu_dp_setup_channel(struct ipu_dp *dp,
 }
 EXPORT_SYMBOL_GPL(ipu_dp_setup_channel);
 
+int ipu_dp_set_gamma(struct ipu_dp *dp,
+		u16 *gamma_constk, u16 *gamma_slopek)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	u32 reg;
+	int i;
+
+	/* set DP_GAMMA_C_SYNC registers */
+	for(i = 0; i < 8 ; i++) {
+		reg = (gamma_constk[i*2+1] << 16) | (gamma_constk[i*2]);
+		writel(reg, flow->base + DP_GAMMA_C_SYNC + 4 * i);
+	}
+	/* set DP_GAMMA_S_SYNC registers */
+	for(i = 0; i < 4 ; i++) {
+		reg = (gamma_slopek[i*4+3] << 24)|
+		      (gamma_slopek[i*4+2] << 16) |
+		      (gamma_slopek[i*4+1] << 8) |
+		      gamma_slopek[i*4];
+		writel(reg, flow->base + DP_GAMMA_S_SYNC + 4 * i);
+	}
+
+	/* enable gammma correction */
+	reg = readl(flow->base + DP_COM_CONF);
+	reg |= DP_COM_CONF_GAMMA_EN;
+	writel(reg, flow->base + DP_COM_CONF);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_set_gamma);
+
 int ipu_dp_enable_channel(struct ipu_dp *dp)
 {
 	struct ipu_flow *flow = to_flow(dp);
diff --git a/drivers/staging/imx-drm/ipuv3-crtc.c b/drivers/staging/imx-drm/ipuv3-crtc.c
index ce6ba98..b227ace 100644
--- a/drivers/staging/imx-drm/ipuv3-crtc.c
+++ b/drivers/staging/imx-drm/ipuv3-crtc.c
@@ -291,10 +291,19 @@ static int ipu_set_interface_pix_fmt(struct drm_crtc *crtc, u32 encoder_type,
 	return 0;
 }
 
+static int ipu_set_interface_gamma(struct drm_crtc *crtc,
+		u16 *gamma_constk, u16 *gamma_slopek)
+{
+	struct ipu_crtc *ipu_crtc = to_ipu_crtc(crtc);
+
+	return ipu_dp_set_gamma(ipu_crtc->plane[0]->dp, gamma_constk, gamma_slopek);
+}
+
 static const struct imx_drm_crtc_helper_funcs ipu_crtc_helper_funcs = {
 	.enable_vblank = ipu_enable_vblank,
 	.disable_vblank = ipu_disable_vblank,
 	.set_interface_pix_fmt = ipu_set_interface_pix_fmt,
+	.set_interface_gamma = ipu_set_interface_gamma,
 	.crtc_funcs = &ipu_crtc_funcs,
 	.crtc_helper_funcs = &ipu_helper_funcs,
 };
-- 
1.8.1.4

