drm/omapdrm: gamma table support

index fe79498..8d0fe45 100644
@@ -264,24 +264,24 @@ cleanup:
 	return r;
 }
 
-static int omap_modeset_create_crtc(struct drm_device *dev, int id,
-				    enum omap_channel channel)
+static int omap_modeset_create_crtc(struct drm_device *dev, int crtc_id,
+				    int plane_id, enum omap_channel channel)
 {
 	struct omap_drm_private *priv = dev->dev_private;
 	struct drm_plane *plane;
 	struct drm_crtc *crtc;
 
-	plane = omap_plane_init(dev, id, DRM_PLANE_TYPE_PRIMARY);
+	plane = omap_plane_init(dev, plane_id, DRM_PLANE_TYPE_PRIMARY);
 	if (IS_ERR(plane))
 		return PTR_ERR(plane);
 
-	crtc = omap_crtc_init(dev, plane, channel, id);
+	crtc = omap_crtc_init(dev, plane, channel, crtc_id);
 
 	BUG_ON(priv->num_crtcs >= ARRAY_SIZE(priv->crtcs));
-	priv->crtcs[id] = crtc;
+	priv->crtcs[crtc_id] = crtc;
 	priv->num_crtcs++;
 
-	priv->planes[id] = plane;
+	priv->planes[plane_id] = plane;
 	priv->num_planes++;
 
 	return 0;

@@ -315,7 +315,7 @@ static int omap_modeset_init(struct drm_device *dev)
 	int num_ovls = dss_feat_get_num_ovls();
 	int num_mgrs = dss_feat_get_num_mgrs();
 	int num_crtcs;
-	int i, id = 0;
+	int i, crtc_id = 0, plane_id = 0;
 	int ret;
 
 	drm_mode_config_init(dev);

@@ -377,7 +377,7 @@ static int omap_modeset_init(struct drm_device *dev)
 		 * the possible_crtcs field for all the encoders with the final
 		 * set of crtcs we create
 		 */
-		if (id == num_crtcs)
+		if (crtc_id == num_crtcs)
 			continue;
 
 		/*

@@ -396,7 +396,8 @@ static int omap_modeset_init(struct drm_device *dev)
 		 * allocated crtc, we create a new crtc for it
 		 */
 		if (!channel_used(dev, channel)) {
-			ret = omap_modeset_create_crtc(dev, id, channel);
+			ret = omap_modeset_create_crtc(dev, crtc_id, plane_id,
+						       channel);
 			if (ret < 0) {
 				dev_err(dev->dev,
 					"could not create CRTC (channel %u)\n",

@@ -404,7 +405,7 @@ static int omap_modeset_init(struct drm_device *dev)
 				return ret;
 			}
 
-			id++;
+			crtc_id++; plane_id++;
 		}
 	}
 

@@ -412,7 +413,7 @@ static int omap_modeset_init(struct drm_device *dev)
 	 * we have allocated crtcs according to the need of the panels/encoders,
 	 * adding more crtcs here if needed
 	 */
-	for (; id < num_crtcs; id++) {
+	for (; crtc_id < num_crtcs; crtc_id++) {
 
 		/* find a free manager for this crtc */
 		for (i = 0; i < num_mgrs; i++) {

@@ -426,21 +427,22 @@ static int omap_modeset_init(struct drm_device *dev)
 			return -ENOMEM;
 		}
 
-		ret = omap_modeset_create_crtc(dev, id, i);
+		ret = omap_modeset_create_crtc(dev, crtc_id, plane_id, i);
 		if (ret < 0) {
 			dev_err(dev->dev,
 				"could not create CRTC (channel %u)\n", i);
 			return ret;
 		}
+		plane_id++;
 	}
 
 	/*
 	 * Create normal planes for the remaining overlays:
 	 */
-	for (; id < num_ovls; id++) {
+	for (; plane_id < num_ovls; plane_id++) {
 		struct drm_plane *plane;
 
-		plane = omap_plane_init(dev, id, DRM_PLANE_TYPE_OVERLAY);
+		plane = omap_plane_init(dev, plane_id, DRM_PLANE_TYPE_OVERLAY);
 		if (IS_ERR(plane))
 			return PTR_ERR(plane);
 

@@ -453,6 +455,7 @@ static int omap_modeset_init(struct drm_device *dev)
 		struct omap_dss_device *dssdev =
 					omap_encoder_get_dssdev(encoder);
 		struct omap_dss_device *output;
+		int id;
 
 		output = omapdss_find_output_from_display(dssdev);
 

index f83608b..87dde05 100644
@@ -116,6 +116,7 @@ struct dispc_features {
 };
 
 #define DISPC_MAX_NR_FIFOS 5
+#define DISPC_MAX_CHANNEL_GAMMA 4
 
 static struct {
 	struct platform_device *pdev;

@@ -135,6 +136,8 @@ static struct {
 	bool		ctx_valid;
 	u32		ctx[DISPC_SZ_REGS / sizeof(u32)];
 
+	u32 *gamma_table[DISPC_MAX_CHANNEL_GAMMA];
+
 	const struct dispc_features *feat;
 
 	bool is_enabled;

@@ -178,11 +181,19 @@ struct dispc_reg_field {
 	u8 low;
 };
 
+struct dispc_gamma_desc {
+	u32 len;
+	u32 bits;
+	u16 reg;
+	bool has_index;
+};
+
 static const struct {
 	const char *name;
 	u32 vsync_irq;
 	u32 framedone_irq;
 	u32 sync_lost_irq;
+	struct dispc_gamma_desc gamma;
 	struct dispc_reg_field reg_desc[DISPC_MGR_FLD_NUM];
 } mgr_desc[] = {
 	[OMAP_DSS_CHANNEL_LCD] = {

@@ -190,6 +201,12 @@ static const struct {
 		.vsync_irq	= DISPC_IRQ_VSYNC,
 		.framedone_irq	= DISPC_IRQ_FRAMEDONE,
 		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST,
+		.gamma		= {
+			.len	= 256,
+			.bits	= 8,
+			.reg	= DISPC_GAMMA_TABLE0,
+			.has_index = true,
+		},
 		.reg_desc	= {
 			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL,  0,  0 },
 			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL,  3,  3 },

@@ -207,6 +224,12 @@ static const struct {
 		.vsync_irq	= DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN,
 		.framedone_irq	= DISPC_IRQ_FRAMEDONETV,
 		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST_DIGIT,
+		.gamma		= {
+			.len	= 1024,
+			.bits	= 10,
+			.reg	= DISPC_GAMMA_TABLE2,
+			.has_index = false,
+		},
 		.reg_desc	= {
 			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL,  1,  1 },
 			[DISPC_MGR_FLD_STNTFT]		= { },

@@ -224,6 +247,12 @@ static const struct {
 		.vsync_irq	= DISPC_IRQ_VSYNC2,
 		.framedone_irq	= DISPC_IRQ_FRAMEDONE2,
 		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST2,
+		.gamma		= {
+			.len	= 256,
+			.bits	= 8,
+			.reg	= DISPC_GAMMA_TABLE1,
+			.has_index = true,
+		},
 		.reg_desc	= {
 			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL2,  0,  0 },
 			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL2,  3,  3 },

@@ -241,6 +270,12 @@ static const struct {
 		.vsync_irq	= DISPC_IRQ_VSYNC3,
 		.framedone_irq	= DISPC_IRQ_FRAMEDONE3,
 		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST3,
+		.gamma		= {
+			.len	= 256,
+			.bits	= 8,
+			.reg	= DISPC_GAMMA_TABLE3,
+			.has_index = true,
+		},
 		.reg_desc	= {
 			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL3,  0,  0 },
 			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL3,  3,  3 },

@@ -1084,20 +1119,6 @@ static u32 dispc_ovl_get_burst_size(enum omap_plane plane)
 	return unit * 8;
 }
 
-void dispc_enable_gamma_table(bool enable)
-{
-	/*
-	 * This is partially implemented to support only disabling of
-	 * the gamma table.
-	 */
-	if (enable) {
-		DSSWARN("Gamma table enabling for TV not yet supported");
-		return;
-	}
-
-	REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
-}
-
 static void dispc_mgr_enable_cpr(enum omap_channel channel, bool enable)
 {
 	if (channel == OMAP_DSS_CHANNEL_DIGIT)

@@ -3814,6 +3835,128 @@ void dispc_disable_sidle(void)
 	REG_FLD_MOD(DISPC_SYSCONFIG, 1, 4, 3);	/* SIDLEMODE: no idle */
 }
 
+u32 dispc_mgr_gamma_size(enum omap_channel channel)
+{
+	const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+
+	if (!dss_has_feature(FEAT_GAMMA_TABLE))
+		return 0;
+
+	return gdesc->len;
+}
+EXPORT_SYMBOL(dispc_mgr_gamma_size);
+
+static void dispc_mgr_write_gamma_table(enum omap_channel channel)
+{
+	const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+	u32 *table = dispc.gamma_table[channel];
+	unsigned int i;
+
+	DSSDBG("%s: channel %d\n", __func__, channel);
+
+	for (i = 0; i < gdesc->len; ++i) {
+		u32 v = table[i];
+
+		if (gdesc->has_index)
+			v |= i << 24;
+		else if (i == 0)
+			v |= 1 << 31;
+
+		dispc_write_reg(gdesc->reg, v);
+	}
+}
+
+static void dispc_restore_gamma_tables(void)
+{
+	DSSDBG("%s()\n", __func__);
+
+	if (!dss_has_feature(FEAT_GAMMA_TABLE))
+		return;
+
+	dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_LCD);
+
+	dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_DIGIT);
+
+	if (dss_has_feature(FEAT_MGR_LCD2))
+		dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_LCD2);
+
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_LCD3);
+}
+
+void dispc_mgr_set_gamma(enum omap_channel channel,
+			 const struct drm_color_lut *lut,
+			 unsigned int length)
+{
+	const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+	u32 *table = dispc.gamma_table[channel];
+	int i;
+
+	DSSDBG("%s: channel %d, lut len %u, hw len %u\n", __func__,
+	       channel, length, gdesc->len);
+
+	if (!dss_has_feature(FEAT_GAMMA_TABLE))
+		return;
+
+	for (i = 0; i < length; ++i) {
+		int first = i * (gdesc->len - 1) / (length - 1);
+		int last = (i + 1) * (gdesc->len - 1) / (length - 1);
+		int w = last - first;
+		u16 r, g, b;
+		int j;
+
+		for (j = 0; j <= w; j++) {
+			r = (lut[i].red * (w - j) + lut[i+1].red * j) / w;
+			g = (lut[i].green * (w - j) + lut[i+1].green * j) / w;
+			b = (lut[i].blue * (w - j) + lut[i+1].blue * j) / w;
+
+			r >>= (16 - gdesc->bits);
+			g >>= (16 - gdesc->bits);
+			b >>= (16 - gdesc->bits);
+
+			table[first + j] = (r << (gdesc->bits * 2)) |
+				(g << gdesc->bits) | b;
+		}
+	}
+
+	if (dispc.is_enabled)
+		dispc_mgr_write_gamma_table(channel);
+}
+EXPORT_SYMBOL(dispc_mgr_set_gamma);
+
+static int dispc_init_gamma_tables(void)
+{
+	int channel;
+
+	if (!dss_has_feature(FEAT_GAMMA_TABLE))
+		return;
+
+	for (channel = 0; channel < ARRAY_SIZE(dispc.gamma_table); channel++) {
+		const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+		u32 *table;
+		int i;
+
+		if (channel == OMAP_DSS_CHANNEL_LCD2 &&
+		    !dss_has_feature(FEAT_MGR_LCD2))
+			continue;
+
+		if (channel == OMAP_DSS_CHANNEL_LCD3 &&
+		    !dss_has_feature(FEAT_MGR_LCD3))
+			continue;
+
+		table = devm_kmalloc_array(&dispc.pdev->dev, gdesc->len,
+					   sizeof(table[i]), GFP_KERNEL);
+		if (!table)
+			return -ENOMEM;
+
+		for (i = 0; i < gdesc->len; ++i)
+			table[i] =
+				(i << 2*gdesc->bits) | (i << gdesc->bits) | i;
+		dispc.gamma_table[channel] = table;
+	}
+	return 0;
+}
+
 static void _omap_dispc_initial_config(void)
 {
 	u32 l;

@@ -3829,8 +3972,16 @@ static void _omap_dispc_initial_config(void)
 		dispc.core_clk_rate = dispc_fclk_rate();
 	}
 
-	/* FUNCGATED */
-	if (dss_has_feature(FEAT_FUNCGATED))
+	/* Use gamma table */
+	if (dss_has_feature(FEAT_GAMMA_TABLE))
+		REG_FLD_MOD(DISPC_CONFIG, 1, 3, 3);
+
+	/* For older DSS versions (FEAT_FUNCGATED) this enables
+	 * func-clock auto-gating. For newer versions
+	 * (FEAT_GAMMA_TABLE) this enables tv-out gamma tables.
+	 */
+	if (dss_has_feature(FEAT_FUNCGATED) ||
+	    dss_has_feature(FEAT_GAMMA_TABLE))
 		REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
 
 	dispc_setup_color_conv_coef();

@@ -4100,6 +4251,10 @@ static int dispc_bind(struct device *dev, struct device *master, void *data)
 		}
 	}
 
+	r = dispc_init_gamma_tables();
+	if (r)
+		return r;
+
 	pm_runtime_enable(&pdev->dev);
 
 	r = dispc_runtime_get();

@@ -4170,6 +4325,8 @@ static int dispc_runtime_resume(struct device *dev)
 		_omap_dispc_initial_config();
 
 		dispc_restore_context();
+
+		dispc_restore_gamma_tables();
 	}
 
 	dispc.is_enabled = true;

index 4837442..bc1d812 100644
@@ -42,6 +42,11 @@
 #define DISPC_MSTANDBY_CTRL		0x0858
 #define DISPC_GLOBAL_MFLAG_ATTRIBUTE	0x085C
 
+#define DISPC_GAMMA_TABLE0		0x0630
+#define DISPC_GAMMA_TABLE1		0x0634
+#define DISPC_GAMMA_TABLE2		0x0638
+#define DISPC_GAMMA_TABLE3		0x0850
+
 /* DISPC overlay registers */
 #define DISPC_OVL_BA0(n)		(DISPC_OVL_BASE(n) + \
 					DISPC_BA0_OFFSET(n))

index c886a29..f77b1c5 100644
@@ -593,6 +593,7 @@ static const enum dss_feat_id omap4_dss_feat_list[] = {
 	FEAT_ALPHA_FREE_ZORDER,
 	FEAT_FIFO_MERGE,
 	FEAT_BURST_2D,
+	FEAT_GAMMA_TABLE,
 };
 
 static const enum dss_feat_id omap5_dss_feat_list[] = {

@@ -615,6 +616,7 @@ static const enum dss_feat_id omap5_dss_feat_list[] = {
 	FEAT_BURST_2D,
 	FEAT_DSI_PHY_DCC,
 	FEAT_MFLAG,
+	FEAT_GAMMA_TABLE,
 };
 
 /* OMAP2 DSS Features */

index 3d67d39..d4d14db 100644
@@ -62,6 +62,7 @@ enum dss_feat_id {
 	FEAT_BURST_2D,
 	FEAT_DSI_PHY_DCC,
 	FEAT_MFLAG,
+	FEAT_GAMMA_TABLE,
 };
 
 /* DSS register field id */

index f892ae15..12bef2d 100644
@@ -213,9 +213,6 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 
 	hdmi4_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
 
-	/* bypass TV gamma table */
-	dispc_enable_gamma_table(0);
-
 	/* tv size */
 	dss_mgr_set_timings(channel, p);
 

index a43f7b1..10530c5 100644
@@ -230,9 +230,6 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 
 	hdmi5_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
 
-	/* bypass TV gamma table */
-	dispc_enable_gamma_table(0);
-
 	/* tv size */
 	dss_mgr_set_timings(channel, p);
 

index d7e7c90..adcd315 100644
@@ -19,6 +19,7 @@
 #define __OMAP_DRM_DSS_H
 
 #include <video/omapdss.h>
+#include <uapi/drm/drm_mode.h>
 
 u32 dispc_read_irqstatus(void);
 void dispc_clear_irqstatus(u32 mask);

@@ -44,6 +45,10 @@ void dispc_mgr_set_timings(enum omap_channel channel,
 		const struct omap_video_timings *timings);
 void dispc_mgr_setup(enum omap_channel channel,
 		const struct omap_overlay_manager_info *info);
+u32 dispc_mgr_gamma_size(enum omap_channel channel);
+void dispc_mgr_set_gamma(enum omap_channel channel,
+			 const struct drm_color_lut *lut,
+			 unsigned int length);
 
 int dispc_ovl_enable(enum omap_plane plane, bool enable);
 bool dispc_ovl_enabled(enum omap_plane plane);

index 87dde05..3608cf2 100644
@@ -4201,6 +4201,164 @@ void dispc_free_irq(void *dev_id)
 }
 EXPORT_SYMBOL(dispc_free_irq);
 
+/*
+ * Work-a-round for errata i734 in DSS dispc
+ *  - LCD1 Gamma Correction Is Not Working When GFX Pipe Is Disabled
+ *
+ * For gamma tables to work on LCD1 the GFX plane has to be used at
+ * least once after DSS HW has come out of reset. The work-a-round
+ * sets up a minimal LCD setup with GFX plane and waits for one
+ * vertical sync irq before disabling the setup and continuing with
+ * the context restore. The physical outputs are gated during the
+ * operation. This work-a-round requires that gamma table's LOADMODE
+ * is set to 0x2 in DISPC_CONTROL1 register.
+ *
+ * For details see:
+ * OMAP543x Multimedia Device Silicon Revision 2.0 Silicon Errata
+ * Literature Number: SWPZ037E
+ * Or some other relevant errata document for the DSS IP version.
+ */
+
+static struct dispc_errata_i734_data {
+	struct omap_video_timings timings;
+	struct omap_overlay_info ovli;
+	struct omap_overlay_manager_info mgri;
+	struct dss_lcd_mgr_config lcd_conf;
+	struct i734_buf {
+		dma_addr_t paddr;
+		void *vaddr;
+	} buf;
+} i734 = {
+	.timings = {
+		.x_res = 8, .y_res = 1,
+		.pixelclock = 16000000,
+		.hsw = 8, .hfp = 4, .hbp = 4,
+		.vsw = 1, .vfp = 1, .vbp = 1,
+		.vsync_level = OMAPDSS_SIG_ACTIVE_LOW,
+		.hsync_level = OMAPDSS_SIG_ACTIVE_LOW,
+		.interlace = false,
+		.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+		.de_level = OMAPDSS_SIG_ACTIVE_HIGH,
+		.sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+		.double_pixel = false,
+	},
+	.ovli = {
+		.screen_width = 1,
+		.width = 1, .height = 1,
+		.color_mode = OMAP_DSS_COLOR_RGB24U,
+		.rotation = OMAP_DSS_ROT_0,
+		.rotation_type = OMAP_DSS_ROT_DMA,
+		.mirror = 0,
+		.pos_x = 0, .pos_y = 0,
+		.out_width = 0, .out_height = 0,
+		.global_alpha = 0xff,
+		.pre_mult_alpha = 0,
+		.zorder = 0,
+	},
+	.mgri = {
+		.default_color = 0,
+		.trans_enabled = false,
+		.partial_alpha_enabled = false,
+		.cpr_enable = false,
+	},
+	.lcd_conf = {
+		.io_pad_mode = DSS_IO_PAD_MODE_BYPASS,
+		.stallmode = false,
+		.fifohandcheck = false,
+		.clock_info = {
+			.lck_div = 1,
+			.pck_div = 2,
+		},
+		.video_port_width = 24,
+		.lcden_sig_polarity = 0,
+	},
+};
+
+static int dispc_errata_i734_wa_init(void)
+{
+	size_t buff_size = i734.ovli.width * i734.ovli.height *
+		color_mode_to_bpp(i734.ovli.color_mode) / 8;
+
+	if (!dss_has_feature(FEAT_GAMMA_I734_BUG))
+		return 0;
+
+	i734.buf.vaddr = dma_alloc_writecombine(&dispc.pdev->dev, buff_size,
+						&i734.buf.paddr, GFP_KERNEL);
+	if (WARN_ON(IS_ERR(i734.buf.vaddr)))
+		return PTR_ERR(i734.buf.vaddr);
+
+	i734.ovli.paddr = i734.buf.paddr;
+
+	return 0;
+}
+
+static void dispc_errata_i734_wa_fini(void)
+{
+	size_t buff_size = i734.ovli.width * i734.ovli.height *
+		color_mode_to_bpp(i734.ovli.color_mode) / 8;
+
+	if (!dss_has_feature(FEAT_GAMMA_I734_BUG))
+		return;
+
+	dma_free_writecombine(&dispc.pdev->dev, buff_size, i734.buf.vaddr,
+			      i734.buf.paddr);
+}
+
+static void dispc_errata_i734_wa(void)
+{
+	u32 vsync_irq = dispc_mgr_get_vsync_irq(OMAP_DSS_CHANNEL_LCD);
+	u32 framedone_irq = dispc_mgr_get_framedone_irq(OMAP_DSS_CHANNEL_LCD);
+	u32 gatestate = REG_GET(DISPC_CONTROL, 8, 4);
+	unsigned int count;
+
+	if (!dss_has_feature(FEAT_GAMMA_I734_BUG))
+		return;
+
+	/* Gate all LCD1 outputs */
+	REG_FLD_MOD(DISPC_CONTROL, 0, 8, 4);
+
+	/* Setup and enable GFX plane */
+	dispc_ovl_set_channel_out(OMAP_DSS_GFX, OMAP_DSS_CHANNEL_LCD);
+	dispc_ovl_setup(OMAP_DSS_GFX, &i734.ovli, false, &i734.timings, false);
+	dispc_ovl_enable(OMAP_DSS_GFX, true);
+
+	/* Set up and enable display manager for LCD1 */
+	dispc_mgr_setup(OMAP_DSS_CHANNEL_LCD, &i734.mgri);
+	dispc_calc_clock_rates(dss_get_dispc_clk_rate(),
+			       &i734.lcd_conf.clock_info);
+	dispc_mgr_set_lcd_config(OMAP_DSS_CHANNEL_LCD, &i734.lcd_conf);
+	dispc_mgr_set_timings(OMAP_DSS_CHANNEL_LCD, &i734.timings);
+	dispc_mgr_enable(OMAP_DSS_CHANNEL_LCD, true);
+
+	dispc_clear_irqstatus(vsync_irq | framedone_irq);
+
+	/* Busy wait for vsync (we can't fiddle with irq handlers in resume) */
+	count = 0;
+	while (!(dispc_read_irqstatus() & vsync_irq))
+		if (count++ > 10000) {
+			dev_err(&dispc.pdev->dev, "%s: vsync timeout\n",
+				__func__);
+			break;
+		}
+
+	/* Shut down the WA setup */
+	dispc_mgr_enable(OMAP_DSS_CHANNEL_LCD, false);
+	dispc_ovl_enable(OMAP_DSS_GFX, false);
+	count = 0;
+	while (!(dispc_read_irqstatus() & framedone_irq))
+		if (count++ > 10000) {
+			dev_err(&dispc.pdev->dev, "%s: framedone timeout\n",
+				__func__);
+			break;
+		}
+
+	/* Clear all irq bits before continuing */
+	dispc_clear_irqstatus(0xffffffff);
+
+	/* Restore the original state to LCD1 output gates */
+	REG_FLD_MOD(DISPC_CONTROL, gatestate, 8, 4);
+}
+
 /* DISPC HW IP initialisation */
 static int dispc_bind(struct device *dev, struct device *master, void *data)
 {

@@ -4214,6 +4372,10 @@ static int dispc_bind(struct device *dev, struct device *master, void *data)
 
 	spin_lock_init(&dispc.control_lock);
 
+	r = dispc_errata_i734_wa_init();
+	if (r)
+		return r;
+
 	r = dispc_init_features(dispc.pdev);
 	if (r)
 		return r;

@@ -4282,6 +4444,8 @@ static void dispc_unbind(struct device *dev, struct device *master,
 			       void *data)
 {
 	pm_runtime_disable(dev);
+
+	dispc_errata_i734_wa_fini();
 }
 
 static const struct component_ops dispc_component_ops = {

@@ -4324,6 +4488,8 @@ static int dispc_runtime_resume(struct device *dev)
 	if (REG_GET(DISPC_CONFIG, 2, 1) != OMAP_DSS_LOAD_FRAME_ONLY) {
 		_omap_dispc_initial_config();
 
+		dispc_errata_i734_wa();
+
 		dispc_restore_context();
 
 		dispc_restore_gamma_tables();

index f77b1c5..3127bd6 100644
@@ -594,6 +594,7 @@ static const enum dss_feat_id omap4_dss_feat_list[] = {
 	FEAT_FIFO_MERGE,
 	FEAT_BURST_2D,
 	FEAT_GAMMA_TABLE,
+	FEAT_GAMMA_I734_BUG,
 };
 
 static const enum dss_feat_id omap5_dss_feat_list[] = {

@@ -617,6 +618,7 @@ static const enum dss_feat_id omap5_dss_feat_list[] = {
 	FEAT_DSI_PHY_DCC,
 	FEAT_MFLAG,
 	FEAT_GAMMA_TABLE,
+	FEAT_GAMMA_I734_BUG,
 };
 
 /* OMAP2 DSS Features */

index d4d14db..0f74083 100644
@@ -63,6 +63,7 @@ enum dss_feat_id {
 	FEAT_DSI_PHY_DCC,
 	FEAT_MFLAG,
 	FEAT_GAMMA_TABLE,
+	FEAT_GAMMA_I734_BUG,
 };
 
 /* DSS register field id */

index 075f2bb..d5210fe 100644
@@ -384,6 +384,15 @@ static void omap_crtc_atomic_flush(struct drm_crtc *crtc,
 
 	WARN_ON(omap_crtc->vblank_irq.registered);
 
+	if (crtc->state->color_mgmt_changed) {
+		struct drm_color_lut *lut = (struct drm_color_lut *)
+			crtc->state->gamma_lut->data;
+		unsigned int length = crtc->state->gamma_lut->length /
+			sizeof(*lut);
+
+		dispc_mgr_set_gamma(omap_crtc->channel, lut, length);
+	}
+
 	if (dispc_mgr_is_enabled(omap_crtc->channel)) {
 
 		DBG("%s: GO", omap_crtc->name);

@@ -460,6 +469,7 @@ static const struct drm_crtc_funcs omap_crtc_funcs = {
 	.set_config = drm_atomic_helper_set_config,
 	.destroy = omap_crtc_destroy,
 	.page_flip = drm_atomic_helper_page_flip,
+	.gamma_set = drm_atomic_helper_legacy_gamma_set,
 	.set_property = drm_atomic_helper_crtc_set_property,
 	.atomic_duplicate_state = drm_atomic_helper_crtc_duplicate_state,
 	.atomic_destroy_state = drm_atomic_helper_crtc_destroy_state,

@@ -534,6 +544,16 @@ struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 
 	drm_crtc_helper_add(crtc, &omap_crtc_helper_funcs);
 
+	/* The dispc API adapts to what ever size, but the HW supports
+	 * 256 element gamma table for LCDs and 1024 element table for
+	 * OMAP_DSS_CHANNEL_DIGIT. X server assumes 256 element gamma
+	 * tables so lets use that. Size of HW gamma table can be
+	 * extracted with dispc_mgr_gamma_size(). If it returns 0
+	 * gamma table is not supprted.
+	 */
+	if (dispc_mgr_gamma_size(channel))
+		drm_mode_crtc_set_gamma_size(crtc, 256);
+
 	omap_plane_install_properties(crtc->primary, &crtc->base);
 
 	omap_crtcs[channel] = omap_crtc;