[PATCH] drm/fsl-dcu: Add gamma set for crtc

[Meng Yi]

Gamma correction is optional and can be used to adjust the color
output values to match the gamut of a particular TFT LCD panel

Signed-off-by: Meng Yi <meng.yi at nxp.com>
---
 drivers/gpu/drm/fsl-dcu/Kconfig            |  6 +++
 drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c | 63 ++++++++++++++++++++++++++++++
 drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.h  |  7 ++++
 3 files changed, 76 insertions(+)

diff --git a/drivers/gpu/drm/fsl-dcu/Kconfig b/drivers/gpu/drm/fsl-dcu/Kconfig
index b9c714d..ddfe3c4 100644
--- a/drivers/gpu/drm/fsl-dcu/Kconfig
+++ b/drivers/gpu/drm/fsl-dcu/Kconfig
@@ -16,3 +16,9 @@ config DRM_FSL_DCU
 	help
 	  Choose this option if you have an Freescale DCU chipset.
 	  If M is selected the module will be called fsl-dcu-drm.
+
+config DRM_FSL_DCU_GAMMA
+	bool "Gamma Correction Support for NXP/Freescale DCU"
+	depends on DRM_FSL_DCU
+	help
+	  Enable support for gamma correction.
diff --git a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
index 3371635..d8426b1 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
@@ -46,6 +46,11 @@ static void fsl_dcu_drm_disable_crtc(struct drm_crtc *crtc)
 
 	drm_crtc_vblank_off(crtc);
 
+#ifdef CONFIG_DRM_FSL_DCU_GAMMA
+	regmap_update_bits(fsl_dev->regmap, DCU_DCU_MODE,
+			   DCU_MODE_EN_GAMMA_MASK,
+			   DCU_MODE_GAMMA_DISABLE);
+#endif
 	regmap_update_bits(fsl_dev->regmap, DCU_DCU_MODE,
 			   DCU_MODE_DCU_MODE_MASK,
 			   DCU_MODE_DCU_MODE(DCU_MODE_OFF));
@@ -58,6 +63,11 @@ static void fsl_dcu_drm_crtc_enable(struct drm_crtc *crtc)
 	struct drm_device *dev = crtc->dev;
 	struct fsl_dcu_drm_device *fsl_dev = dev->dev_private;
 
+#ifdef CONFIG_DRM_FSL_DCU_GAMMA
+	regmap_update_bits(fsl_dev->regmap, DCU_DCU_MODE,
+			   DCU_MODE_EN_GAMMA_MASK,
+			   DCU_MODE_GAMMA_ENABLE);
+#endif
 	regmap_update_bits(fsl_dev->regmap, DCU_DCU_MODE,
 			   DCU_MODE_DCU_MODE_MASK,
 			   DCU_MODE_DCU_MODE(DCU_MODE_NORMAL));
@@ -128,6 +138,58 @@ static const struct drm_crtc_helper_funcs fsl_dcu_drm_crtc_helper_funcs = {
 	.mode_set_nofb = fsl_dcu_drm_crtc_mode_set_nofb,
 };
 
+/*
+ * Gamma_R, Gamma_G and Gamma_B registers are little-endian registers while
+ * the rest of the address-space in 2D-ACE is big-endian. 2D-ACE Gamma_R,
+ * Gamma_G and Gamma_B registers are 32 bit registers, where the first 24
+ * bits are reserved and last 8 bits denote the gamma value. Because of a
+ * connection issue in the device, the first 8-bit [31:24] is connected and
+ * the rest of the 24-bits[23:0] are reserved.
+ * Workaround: Perform the byte_swapping for Gamma_[R/G/B]_registers.
+ * For example: While writing 0000_00ABh to any of the gamma registers, byte
+ * swap the data so it results in AB00_0000h. Write this value to the gamma
+ * register.
+ */
+static u32 swap_bytes(u16 var)
+{
+	union res {
+		char byte[4];
+		u32 val;
+	};
+	union res in, out;
+	unsigned int i = 0;
+
+	in.val = var;
+	for (i = 0; i < 4; i++)
+		out.byte[i] = in.byte[3-i];
+
+	return out.val;
+}
+
+static int fsl_crtc_gamma_set(struct drm_crtc *crtc, u16 *r, u16 *g, u16 *b,
+			uint32_t size)
+{
+	struct rgb {
+		u32 r[size], g[size], b[size];
+	};
+
+	struct drm_device *dev = crtc->dev;
+	struct fsl_dcu_drm_device *fsl_dev = dev->dev_private;
+	unsigned int i;
+	struct rgb glut;
+
+	for (i = 0; i < size; i++) {
+		glut.r[i] = swap_bytes(r[i]);
+		glut.g[i] = swap_bytes(g[i]);
+		glut.b[i] = swap_bytes(b[i]);
+		regmap_write(fsl_dev->regmap, FSL_GAMMA_R + 4 * i, glut.r[i]);
+		regmap_write(fsl_dev->regmap, FSL_GAMMA_G + 4 * i, glut.g[i]);
+		regmap_write(fsl_dev->regmap, FSL_GAMMA_B + 4 * i, glut.b[i]);
+	}
+
+	return 0;
+}
+
 static const struct drm_crtc_funcs fsl_dcu_drm_crtc_funcs = {
 	.atomic_duplicate_state = drm_atomic_helper_crtc_duplicate_state,
 	.atomic_destroy_state = drm_atomic_helper_crtc_destroy_state,
@@ -135,6 +197,7 @@ static const struct drm_crtc_funcs fsl_dcu_drm_crtc_funcs = {
 	.page_flip = drm_atomic_helper_page_flip,
 	.reset = drm_atomic_helper_crtc_reset,
 	.set_config = drm_atomic_helper_set_config,
+	.gamma_set = fsl_crtc_gamma_set,
 };
 
 int fsl_dcu_drm_crtc_create(struct fsl_dcu_drm_device *fsl_dev)
diff --git a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.h b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.h
index 3b371fe7..d3bc540 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.h
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.h
@@ -25,6 +25,9 @@
 #define DCU_MODE_NORMAL			1
 #define DCU_MODE_TEST			2
 #define DCU_MODE_COLORBAR		3
+#define DCU_MODE_EN_GAMMA_MASK		0x04
+#define DCU_MODE_GAMMA_ENABLE		BIT(2)
+#define DCU_MODE_GAMMA_DISABLE		0
 
 #define DCU_BGND			0x0014
 #define DCU_BGND_R(x)			((x) << 16)
@@ -165,6 +168,10 @@
 #define VF610_LAYER_REG_NUM		9
 #define LS1021A_LAYER_REG_NUM		10
 
+#define FSL_GAMMA_R			0x4000
+#define FSL_GAMMA_G			0x4400
+#define FSL_GAMMA_B			0x4800
+
 struct clk;
 struct device;
 struct drm_device;
-- 
2.1.0.27.g96db324