U8500: Generic display driver

This patch introduces combined DSI and DPI display support for u-boot.
The code is also similar to the kernel code for easy maintenance.

ST-Ericsson ID: ER319241
ST-Ericsson FOSS-OUT ID: STETL-FOSS-OUT-10069

Change-Id: Ic232b6f738348cbedb67e27418678ddd223d7800
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/11038
Reviewed-by: Jimmy RUBIN <jimmy.rubin@stericsson.com>
Reviewed-by: Michael BRANDT <michael.brandt@stericsson.com>
Tested-by: Torbjorn SVENSSON <torbjorn.x.svensson@stericsson.com>

11 files changed, 2013 insertions, 593 deletions

diff --git a/board/st/u8500/Makefile b/board/st/u8500/Makefile
index 9c6da5a12..26af32e76 100644
--- a/board/st/u8500/Makefile
+++ b/board/st/u8500/Makefile
@@ -28,7 +28,8 @@ LIB	= $(obj)lib$(BOARD).a
 COBJS	:= u8500.o u8500_i2c.o mmc_host.o mmc_utils.o ab8500vibra.o cmd_cdump.o
 SOBJS	:= mmc_fifo.o
 
-COBJS-$(CONFIG_VIDEO_LOGO) += mcde_display.o mcde_hw.o mcde_display_image.o
+COBJS-$(CONFIG_VIDEO_LOGO) += mcde_display.o mcde_display_dsi.o mcde_display_dpi.o
+COBJS-$(CONFIG_VIDEO_LOGO) += mcde_hw.o mcde_display_image.o
 
 COBJS	+= $(COBJS-y)
 
diff --git a/board/st/u8500/dsilink_regs.h b/board/st/u8500/dsilink_regs.h
index 69b08718b..f921150f7 100644
--- a/board/st/u8500/dsilink_regs.h
+++ b/board/st/u8500/dsilink_regs.h
@@ -81,6 +81,10 @@
 #define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN_MASK 0x00002000
 #define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN(__x) \
 	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_EOT_GEN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN_SHIFT 14
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN_MASK 0x00004000
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DLX_REMAP_EN, __x)
 #define DSI_MCTL_MAIN_PHY_CTL 0x00000008
 #define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN_SHIFT 0
 #define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN_MASK 0x00000001
diff --git a/board/st/u8500/mcde.h b/board/st/u8500/mcde.h
index 237e7deb3..c13a2c935 100644
--- a/board/st/u8500/mcde.h
+++ b/board/st/u8500/mcde.h
@@ -14,11 +14,13 @@
 /* Physical interface types */
 enum mcde_port_type {
 	MCDE_PORTTYPE_DSI = 0,
+	MCDE_PORTTYPE_DPI = 1,
 };
 
 /* Interface mode */
 enum mcde_port_mode {
 	MCDE_PORTMODE_CMD = 0,
+	MCDE_PORTMODE_VID = 1,
 };
 
 /* MCDE fifos */
@@ -37,22 +39,136 @@ enum mcde_chnl {
 	MCDE_CHNL_C1 = 3,
 };
 
+/* Display power modes */
+enum mcde_display_power_mode {
+	MCDE_DISPLAY_PM_OFF     = 0, /* Power off */
+	MCDE_DISPLAY_PM_STANDBY = 1, /* DCS sleep mode */
+	MCDE_DISPLAY_PM_ON      = 2, /* DCS normal mode, display on */
+};
+
+/* Update sync mode */
+#define MCDE_CHNLPATH(__chnl, __fifo, __type, __ifc, __link) \
+	(((__chnl) << 16) | ((__fifo) << 12) | \
+	 ((__type) << 8) | ((__ifc) << 4) | ((__link) << 0))
+enum mcde_chnl_path {
+	/* Channel A */
+	MCDE_CHNLPATH_CHNLA_FIFOA_DPI_0 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DPI, 0, 0),
+	MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLA_FIFOC0_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLA_FIFOA_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_A,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 1, 2),
+	/* Channel B */
+	MCDE_CHNLPATH_CHNLB_FIFOB_DPI_1 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DPI, 0, 1),
+	MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLB_FIFOC1_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLB_FIFOB_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_B,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 1, 2),
+	/* Channel C0 */
+	MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLC0_FIFOC0_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_C0, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLC0_FIFOA_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_C0,
+		MCDE_FIFO_A, MCDE_PORTTYPE_DSI, 1, 2),
+	/* Channel C1 */
+	MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC0_0 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 0),
+	MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC0_1 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 0, 1),
+	MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC0_2 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 0, 2),
+	MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC1_0 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 0),
+	MCDE_CHNLPATH_CHNLC1_FIFOC1_DSI_IFC1_1 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_C1, MCDE_PORTTYPE_DSI, 1, 1),
+	MCDE_CHNLPATH_CHNLC1_FIFOB_DSI_IFC1_2 = MCDE_CHNLPATH(MCDE_CHNL_C1,
+		MCDE_FIFO_B, MCDE_PORTTYPE_DSI, 1, 2),
+};
+
 /* Update sync mode */
 enum mcde_sync_src {
+	MCDE_SYNCSRC_OFF = 0, /* No sync */
+	MCDE_SYNCSRC_TE0 = 1, /* MCDE ext TE0 */
+	MCDE_SYNCSRC_TE1 = 2, /* MCDE ext TE1 */
 	MCDE_SYNCSRC_BTA = 3, /* DSI BTA */
 };
 
+/* Interface pixel formats (output) */
+enum mcde_port_pix_fmt {
+	/* MIPI standard formats */
+
+	MCDE_PORTPIXFMT_DPI_16BPP_C1 =     0x21,
+	MCDE_PORTPIXFMT_DPI_16BPP_C2 =     0x22,
+	MCDE_PORTPIXFMT_DPI_16BPP_C3 =     0x23,
+	MCDE_PORTPIXFMT_DPI_18BPP_C1 =     0x24,
+	MCDE_PORTPIXFMT_DPI_18BPP_C2 =     0x25,
+	MCDE_PORTPIXFMT_DPI_24BPP =        0x26,
+
+	MCDE_PORTPIXFMT_DSI_16BPP =        0x31,
+	MCDE_PORTPIXFMT_DSI_18BPP =        0x32,
+	MCDE_PORTPIXFMT_DSI_18BPP_PACKED = 0x33,
+	MCDE_PORTPIXFMT_DSI_24BPP =        0x34,
+
+	/* Custom formats */
+	MCDE_PORTPIXFMT_DSI_YCBCR422 =     0x40,
+};
+
+#define MCDE_PORT_DPI_NO_CLOCK_DIV	0
+
+#define DPI_ACT_HIGH_ALL	0 /* all signals are active high	  */
+#define DPI_ACT_LOW_HSYNC	1 /* horizontal sync signal is active low */
+#define DPI_ACT_LOW_VSYNC	2 /* vertical sync signal is active low	  */
+#define DPI_ACT_LOW_DATA_ENABLE	4 /* data enable signal is active low	  */
+#define DPI_ACT_ON_FALLING_EDGE	8 /* drive data on the falling edge of the
+				   * pixel clock
+				   */
+
 struct mcde_port {
 	enum mcde_port_type type;
 	enum mcde_port_mode mode;
+	enum mcde_port_pix_fmt pixel_format;
 	u8 ifc;
 	u8 link;
 	enum mcde_sync_src sync_src;
+	u8 update_auto_trig;
 	union {
 		struct {
 			u8 virt_id;
 			u8 num_data_lanes;
+			u8 ui;
+			u8 clk_cont;
+			u8 data_lanes_swap;
 		} dsi;
+		struct {
+			u8 bus_width;
+			u8 tv_mode;
+			u16 clock_div;	/* use 0 or 1 for no clock divider */
+			u32 polarity;	/* see DPI_ACT_LOW_* definitions */
+		} dpi;
 	} phy;
 };
 
@@ -62,17 +178,56 @@ enum mcde_ovly_pix_fmt {
 	MCDE_OVLYPIXFMT_RGB888   = 4,
 };
 
-/* Interface pixel formats (output) */
-enum mcde_port_pix_fmt {
-	/* MIPI standard formats */
-	MCDE_PORTPIXFMT_DSI_24BPP =        0x34,
+#define MCDE_FIFO_AB_SIZE 640
+#define MCDE_FIFO_C0C1_SIZE 160
 
-};
+#define MCDE_PIXFETCH_LARGE_WTRMRKLVL 128
+#define MCDE_PIXFETCH_MEDIUM_WTRMRKLVL 32
+#define MCDE_PIXFETCH_SMALL_WTRMRKLVL 16
+
+/* Tv-out defines */
+#define MCDE_CONFIG_TVOUT_HBORDER 2
+#define MCDE_CONFIG_TVOUT_VBORDER 2
+#define MCDE_CONFIG_TVOUT_BACKGROUND_LUMINANCE		0x83
+#define MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CB	0x9C
+#define MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CR	0x2C
+
+/* In seconds */
+#define MCDE_AUTO_SYNC_WATCHDOG 5
+
+/* Hardware versions */
+#define MCDE_CHIP_VERSION_3_0_8 2
+#define MCDE_CHIP_VERSION_3_0_5 1
+#define MCDE_CHIP_VERSION_3	0
 
 /* DSI modes */
 #define DSI_VIDEO_MODE	0
 #define DSI_CMD_MODE	1
 
+/* Video mode descriptor */
+struct mcde_video_mode {
+	u32 xres;
+	u32 yres;
+	u32 pixclock;	/* pixel clock in ps (pico seconds) */
+	u32 hbp;	/* hor back porch = left_margin */
+	u32 hfp;	/* hor front porch equals to right_margin */
+	u32 hsw;	/* horizontal sync width */
+	u32 vbp1;	/* field 1: vert back porch equals to upper_margin */
+	u32 vfp1;	/* field 1: vert front porch equals to lower_margin */
+	u32 vbp2;	/* field 2: vert back porch equals to upper_margin */
+	u32 vfp2;	/* field 2: vert front porch equals to lower_margin */
+	u32 vsw;	/* vertical sync width*/
+	u8 interlaced;
+	u8 bckcol[3];	/* background color */
+};
+
+struct mcde_rectangle {
+	u16 x;
+	u16 y;
+	u16 w;
+	u16 h;
+};
+
 struct mcde_chnl_state;
 
 struct mcde_chnl_state *mcde_chnl_get(enum mcde_chnl chnl_id,
@@ -82,7 +237,8 @@ void mcde_chnl_set_update_area(struct mcde_chnl_state *chnl,
 void mcde_chnl_set_pixel_format(struct mcde_chnl_state *chnl,
 	enum mcde_port_pix_fmt pix_fmt);
 void mcde_chnl_apply(struct mcde_chnl_state *chnl);
-void mcde_chnl_update(struct mcde_chnl_state *chnl);
+int mcde_chnl_update(struct mcde_chnl_state *chnl,
+					struct mcde_rectangle *update_area);
 
 void mcde_enable_dss(void);
 
@@ -108,10 +264,32 @@ void mcde_ovly_apply(struct mcde_ovly_state *ovly);
 
 int mcde_dsi_dcs_write(struct mcde_port *port, u8 cmd, u8* data, int len);
 
+struct mcde_platform_data {
+	/* DSI */
+	int num_dsilinks;
+
+	/* DPI */
+	u8 outmux[5]; /* MCDE_CONF0.OUTMUXx */
+	u8 syncmux;   /* MCDE_CONF0.SYNCMUXx */
+};
+
+
 /* MCDE */
 
-int mcde_turn_on_display(void);
-int mcde_init(u8 num_data_lanes);
+extern int dpi_display_platform_enable(void);
+extern void mcde_get_hardware_version(void);
+extern void update_mcde_registers(struct mcde_platform_data *pdata);
+extern int mcde_chnl_set_video_mode(struct mcde_chnl_state *chnl,
+				struct mcde_video_mode *vmode);
+
+extern int mcde_chnl_set_power_mode(struct mcde_chnl_state *chnl,
+	enum mcde_display_power_mode power_mode);
+extern int mcde_chnl_set_video_mode(struct mcde_chnl_state *chnl,
+				struct mcde_video_mode *vmode);
+int mcde_turn_on_display_dpi(void);
+int mcde_turn_on_display_dsi(void);
+int mcde_probe(u8 num_data_lanes, struct mcde_platform_data *pdata);
+void mcde_init(void);
 void mcde_exit(void);
 
 #endif /* __MCDE__H__ */
diff --git a/board/st/u8500/mcde_display.c b/board/st/u8500/mcde_display.c
index 76ded82e6..9c738fac4 100644
--- a/board/st/u8500/mcde_display.c
+++ b/board/st/u8500/mcde_display.c
@@ -7,38 +7,32 @@
 * License terms: GNU General Public License (GPL), version 2.
 */
 
-
 #include <common.h>
 #include <command.h>
-#include "mcde_display.h"
-#include "dsilink_regs.h"
-#include <tc35892.h>
-#include "mcde_regs.h"
 #include <malloc.h>
-#include "mcde.h"
-#include <linux/err.h>
-#include <asm/arch/ab8500.h>
 #include <asm/arch/common.h>
-#include <part.h>
-#include <mmc.h>
-
-#define DEBUG 0
-#define dbg_printk(format, arg...)			\
-	if (DEBUG)					\
-		printf("mcde: " format, ##arg)		\
+#include <asm/arch/ab8500.h>
+#include <linux/err.h>
+#include <tc35892.h>	/* Needed for DSI, to be removed */
+#include "mcde.h"
+#include "mcde_regs.h"
+#include "mcde_display.h"
+#include "dsilink_regs.h"
 
 static struct mcde_chnl_state *chnl;
 
+#if CONFIG_SYS_DISPLAY_DSI
 static struct mcde_port port0 = {
 	.type = MCDE_PORTTYPE_DSI,
 	.mode = MCDE_PORTMODE_CMD,
-	.ifc = 0,
+	.ifc = 1,
 	.link = 0,
 	.sync_src = MCDE_SYNCSRC_BTA,
 	.phy = {
 		.dsi = {
 			.virt_id = 0,
 			.num_data_lanes = 2,
+			.ui = 9,
 		},
 	},
 };
@@ -48,266 +42,182 @@ struct mcde_display_generic_platform_data main_display_data = {
 	.reset_delay = 10,
 };
 
+struct mcde_platform_data platform_data = {
+	0,
+};
+
+struct mcde_video_mode video_mode = {
+	.xres = CONFIG_SYS_DISPLAY_NATIVE_X_RES,
+	.yres = CONFIG_SYS_DISPLAY_NATIVE_Y_RES,
+	.pixclock = 37037,	/* from kernel */
+	.interlaced = 0,
+	.bckcol = {255, 255, 255}, /* R, G, B */
+};
+
 struct mcde_display_device main_display = {
 	.port = &port0,
-	.chnl_id = MCDE_CHNL_C0,
-	.fifo = MCDE_FIFO_A,
+	.chnl_id = MCDE_CHNL_A,
+	.fifo = MCDE_FIFO_C0,
 	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
 	.port_pixel_format = MCDE_PORTPIXFMT_DSI_24BPP,
 	.native_x_res = CONFIG_SYS_DISPLAY_NATIVE_X_RES,
 	.native_y_res = CONFIG_SYS_DISPLAY_NATIVE_Y_RES,
 };
+#endif
 
-static int mcde_enable_gpio(void)
-{
-	int ret;
-	dbg_printk("Enable GPIO pins!\n");
-
-	/* Only main display should be initialized */
-	ret = tc35892_gpio_dir(CONFIG_SYS_I2C_GPIOE_ADDR,
-				main_display_data.reset_gpio, 1);
-	if (ret) {
-		printf("%s:Could not set direction for gpio \n", __func__);
-		return -EINVAL;
-	}
-	ret = tc35892_gpio_set(CONFIG_SYS_I2C_GPIOE_ADDR,
-				main_display_data.reset_gpio, 0);
-	if (ret) {
-		printf("%s:Could reset gpio \n", __func__);
-		return -EINVAL;
-	}
-	mdelay(main_display_data.reset_delay);
-	ret = tc35892_gpio_set(CONFIG_SYS_I2C_GPIOE_ADDR,
-				main_display_data.reset_gpio, 1);
-	if (ret) {
-		printf("%s:Could set gpior\n", __func__);
-		return -EINVAL;
-	}
-	mdelay(main_display_data.reset_delay);
-
-	dbg_printk("All needed GPIOS enabled!\n");
-	return 0;
-}
-
-#define DCS_CMD_EXIT_SLEEP_MODE       0x11
-#define DCS_CMD_SET_DISPLAY_ON        0x29
-
-int mcde_turn_on_display(void)
-{
-	int ret = 0;
-	dbg_printk("Turn on display!\n");
-	ret = mcde_dsi_dcs_write(main_display.port,
-				DCS_CMD_EXIT_SLEEP_MODE, NULL, 0);
-	if (!ret) {
-		dbg_printk("mcde_dsi_dcs_write "
-				"DCS_CMD_EXIT_SLEEP_MODE success!\n");
-		ret = mcde_dsi_dcs_write(main_display.port,
-				DCS_CMD_SET_DISPLAY_ON,  NULL, 0);
-		if (!ret)
-			dbg_printk("mcde_dsi_dcs_write "
-				"DCS_CMD_SET_DISPLAY_ON success!\n");
-	}
-
-	return ret;
-}
-
-/* aux supplies */
-#define MASK_LDO_VAUX1		(0x3)
-#define MASK_LDO_VAUX1_SHIFT	(0x0)
-#define VAUXSEL_VOLTAGE_MASK	(0xf)
-
-#define VANA_ENABLE_IN_HP_MODE	0x05
-
-#define ENABLE_PWM1		0x01
-#define PWM_DUTY_LOW_1024_1024	0xFF
-#define PWM_DUTY_HI_1024_1024	0x03
-
+#if CONFIG_SYS_DISPLAY_DPI
 /*
- * regulator layout
- * @voltage: supported voltage
- * @regval: register value to be written
+ * STE VUIB card
  */
-struct regulator_voltage {
-	int voltage;
-	int regval;
+static struct mcde_port port0 = {
+	.type = MCDE_PORTTYPE_DPI,
+	.mode = MCDE_PORTMODE_VID,
+	.ifc = 0,
+	.link = 1,		/* DPI channel B can only be on link 1 */
+	.sync_src = MCDE_SYNCSRC_OFF,   /* sync from output formatter  */
+	.update_auto_trig = TRUE,
+	.phy = {
+		.dpi = {
+			.tv_mode = FALSE,
+			.clock_div = 2,
+			.polarity = DPI_ACT_LOW_VSYNC | DPI_ACT_LOW_HSYNC,
+		},
+	},
 };
 
-/* voltage table for VAUXn regulators */
-static struct regulator_voltage vauxn_table[] = {
-	{ .voltage = 1100000, .regval  = 0x0, },
-	{ .voltage = 1200000, .regval  = 0x1, },
-	{ .voltage = 1300000, .regval  = 0x2, },
-	{ .voltage = 1400000, .regval  = 0x3, },
-	{ .voltage = 1500000, .regval  = 0x4, },
-	{ .voltage = 1800000, .regval  = 0x5, },
-	{ .voltage = 1850000, .regval  = 0x6, },
-	{ .voltage = 1900000, .regval  = 0x7, },
-	{ .voltage = 2500000, .regval  = 0x8, },
-	{ .voltage = 2650000, .regval  = 0x9, },
-	{ .voltage = 2700000, .regval  = 0xa, },
-	{ .voltage = 2750000, .regval  = 0xb, },
-	{ .voltage = 2800000, .regval  = 0xc, },
-	{ .voltage = 2900000, .regval  = 0xd, },
-	{ .voltage = 3000000, .regval  = 0xe, },
-	{ .voltage = 3300000, .regval  = 0xf, },
+struct mcde_display_generic_platform_data main_display_data = {
+	0,
 };
 
-
-/*
- * This code is derived from the handling of AB8500_LDO_VAUX1 in
- * ab8500_ldo_is_enabled in Linux.
- */
-static int mcde_is_vaux1_enabled(void)
-{
-	int val;
-	val = ab8500_read(AB8500_REGU_CTRL2,
-			AB8500_REGU_VAUX12_REGU_REG);
-	if (val & MASK_LDO_VAUX1)
-		return TRUE;
-	return FALSE;
-}
-
-/*
- * This code is derived from the handling of AB8500_LDO_VAUX1 in
- * ab8500_ldo_get_voltage in Linux.
- */
-static int mcde_get_vaux1_voltage(void)
-{
-	int val;
-	val = ab8500_read(AB8500_REGU_CTRL2,
-		AB8500_REGU_VAUX1_SEL_REG);
-	return vauxn_table[val & VAUXSEL_VOLTAGE_MASK].voltage;
-}
-
-static int mcde_display_power_init(void)
-{
-	int val;
-	int i;
-
+struct mcde_platform_data platform_data = {
+	/* DPI */
 	/*
-	 * On v1.1 HREF boards (HREF+) and V2 boards
-	 * Vaux1 needs to be enabled for the
-	 * display to work.  This is done by enabling the regulators in the
-	 * AB8500 via PRCMU I2C transactions.
-	 *
-	 * This code is derived from the handling of AB8500_LDO_VAUX1 in
-	 * ab8500_ldo_enable(), ab8500_ldo_disable() and
-	 * ab8500_get_best_voltage in Linux.
+	 * [0] = 3: 24 bits DPI: connect LSB Ch B to D[0:7]
+	 * [3] = 4: 24 bits DPI: connect MID Ch B to D[24:31]
+	 * [4] = 5: 24 bits DPI: connect MSB Ch B to D[32:39]
 	 *
-	 * Turn off and delay is required to have it work across soft reboots.
+	 * [1] = 3: TV out     : connect LSB Ch B to D[8:15]
 	 */
+#define DONT_CARE 0
+	.outmux = { 3, 3, DONT_CARE, 4, 5 },	/* vuib500 */
+#undef DONT_CARE
+	.syncmux = 0x01,			/* vuib500 */
+};
 
-	val = ab8500_read(AB8500_REGU_CTRL2,
-		AB8500_REGU_VAUX12_REGU_REG);
-	if (val < 0) {
-		printf("Read vaux1 status failed\n");
-		return -EINVAL;
-	}
-
-	/* Turn off */
-	if (ab8500_write(AB8500_REGU_CTRL2, AB8500_REGU_VAUX12_REGU_REG,
-			   val & ~MASK_LDO_VAUX1) < 0) {
-		printf("Turn off Vaux1 failed\n");
-		return -EINVAL;
-	}
-
-	udelay(10 * 1000);
-
-
-	/* Find voltage from vauxn table */
-	for (i = 0; i < ARRAY_SIZE(vauxn_table) ; i++) {
-		if (vauxn_table[i].voltage == CONFIG_SYS_DISPLAY_VOLTAGE) {
-			if (ab8500_write(AB8500_REGU_CTRL2,
-				AB8500_REGU_VAUX1_SEL_REG,
-				vauxn_table[i].regval) < 0) {
-				printf("AB8500_REGU_VAUX1_SEL_REG failed\n");
-				return -EINVAL;
-			}
-			break;
-		}
-	}
-
-	val = val & ~MASK_LDO_VAUX1;
-	val = val | (1 << MASK_LDO_VAUX1_SHIFT);
-
-	/* Turn on the supply */
-	if (ab8500_write(AB8500_REGU_CTRL2,
-			AB8500_REGU_VAUX12_REGU_REG, val) < 0) {
-		printf("Turn on Vaux1 failed\n");
-		return -EINVAL;
-	}
-
-	/*  DCI & CSI (DSI / PLL / Camera) */ /* Vana & Vpll HP mode */
-	if (ab8500_write(AB8500_REGU_CTRL2, AB8500_REGU_VPLLVANA_REGU_REG,
-						VANA_ENABLE_IN_HP_MODE) < 0) {
-		printf("Turn on Vana failed\n");
-		return -EINVAL;
-	}
+#define DPI_PIXCLK_FREQ	25000000
+
+struct mcde_video_mode video_mode = {
+	.hbp = 40,	/* Horizontal Back Porch */
+	.hfp = 8,	/* Horizontal Front Porch */
+	.hsw = 96,	/* Horizontal Synchronization pulse Width */
+	.vbp1 = 25,	/* Vertical Back Porch 1 */
+	.vfp1 = 2,	/* Vertical Front Porch 1 */
+	.vbp2 = 0,	/* Vertical Back Porch 2 */
+	.vfp2 = 0,	/* Vertical Front Porch 2 */
+	.vsw = 2,	/* Vertical Synchronization pulse Width */
+	.pixclock = (int)(1e+12 * (1.0 / DPI_PIXCLK_FREQ)),
+	.interlaced = 0,
+	.bckcol = {255, 255, 255}, /* R, G, B */
+};
 
-	/* Enable the PWM control for the backlight Main display */
-	if (ab8500_write(AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-							ENABLE_PWM1) < 0) {
-		printf("Enable PWM1 failed\n");
-		return -EINVAL;
-	}
-	if (ab8500_write(AB8500_MISC, AB8500_PWM_OUT_CTRL1_REG,
-						PWM_DUTY_LOW_1024_1024) < 0) {
-		printf("PWM_DUTY_LOW_1024_1024 failed\n");
-		return -EINVAL;
-	}
-	if (ab8500_write(AB8500_MISC, AB8500_PWM_OUT_CTRL2_REG,
-						PWM_DUTY_HI_1024_1024) < 0) {
-		printf("PWM_DUTY_HI_1024_1024 failed\n");
-		return -EINVAL;
-	}
+struct mcde_display_device main_display = {
+	.port = &port0,
+	.chnl_id = MCDE_CHNL_A,
+	.fifo = MCDE_FIFO_A,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGB565,
+	.port_pixel_format = MCDE_PORTPIXFMT_DPI_24BPP,
+	.native_x_res = CONFIG_SYS_DISPLAY_NATIVE_X_RES,
+	.native_y_res = CONFIG_SYS_DISPLAY_NATIVE_Y_RES,
+};
+#endif
 
-	if (!mcde_is_vaux1_enabled() || mcde_get_vaux1_voltage()
-					!= CONFIG_SYS_DISPLAY_VOLTAGE) {
-		printf("VAUX is %d and is set to %d V should be %d\n"
-			, mcde_is_vaux1_enabled(), mcde_get_vaux1_voltage()
-			, CONFIG_SYS_DISPLAY_VOLTAGE);
-		return -EINVAL;
-	}
+int mcde_turn_on_display(void)
+{
+	debug("%s: Enter\n", __func__);
 
-	return 0;
+	if (main_display.port->type == MCDE_PORTTYPE_DSI)
+		return mcde_turn_on_display_dsi();
+	else if (main_display.port->type == MCDE_PORTTYPE_DPI)
+		return mcde_turn_on_display_dpi();
+	return -ENODEV;	/* Should never occur */
 }
 
 int mcde_splash_image(void)
 {
-	u8 num_dsilinks;
-	int ret;
-
-	num_dsilinks = main_display.port->phy.dsi.num_data_lanes;
-	mcde_init(num_dsilinks);
-	ret = mcde_display_power_init();
-	if (ret)
-		goto display_power_failed;
-	mcde_enable_dss();
-
-	ret = mcde_enable_gpio();
-	if (ret)
-		goto enable_gpio_failed;
+	int ret = -ENODEV;
+	struct mcde_rectangle rect;
+
+	debug("%s: enter\n", __func__);
+
+	if (main_display.port->type == MCDE_PORTTYPE_DSI)
+		ret = mcde_startup_dsi(&platform_data);
+	else if (main_display.port->type == MCDE_PORTTYPE_DPI)
+		ret = mcde_startup_dpi(&platform_data);
+	if (ret != 0) {
+		printf("%s: mcde_startup... -> %d\n", __func__, ret);
+		return ret;
+	}
 
+	/* dss enable display */
 	chnl = mcde_chnl_get(main_display.chnl_id, main_display.fifo,
 							main_display.port);
 	if (IS_ERR(chnl)) {
 		ret = PTR_ERR(chnl);
-		printf("%s:Failed to acquire MCDE channel\n", __func__);
+		printf("%s: Failed to acquire MCDE channel ret=%d\n",
+				__func__, ret);
 		goto get_chnl_failed;
 	}
+
+	ret = mcde_chnl_set_power_mode(chnl, MCDE_DISPLAY_PM_STANDBY);
+	if (ret) {
+		printf("%s: mcde_chnl_set_power_mode() -> %d\n",
+				__func__, ret);
+		goto get_chnl_failed;
+	}
+
+	/* dss set video mode */
+	ret = mcde_chnl_set_video_mode(chnl, &video_mode);
+	if (ret < 0) {
+		printf("%s:Failed to set video mode on channel ret=%d\n",
+			__func__, ret);
+		goto set_video_mode_failed;
+	}
+
+	mcde_chnl_set_pixel_format(chnl, main_display.port_pixel_format);
 	mcde_chnl_set_update_area(chnl, 0, 0, main_display.native_x_res,
 						main_display.native_y_res);
-	mcde_chnl_set_pixel_format(chnl, main_display.port_pixel_format);
 	mcde_chnl_apply(chnl);
 
-	/* Everything setup ok, display image */
+	/* chnl setup ok, display image */
 	ret = mcde_display_image(chnl);
+	if (ret != 0) {
+		debug("%s: mcde_display_image() -> %d\n",
+			__func__, ret);
+	}
+
+	mcde_chnl_apply(chnl);
+	rect.x = 0;
+	rect.y = 0;
+	rect.w = main_display.native_x_res;
+	rect.h = main_display.native_y_res;
+	mcde_chnl_update(chnl, &rect);
+
+	ret = mcde_turn_on_display();
+	if (ret) {
+		printf("%s: mcde_turn_on_display() -> %d\n", __func__, ret);
+		goto get_chnl_failed;
+	}
+
+	ret = mcde_chnl_set_power_mode(chnl, MCDE_DISPLAY_PM_ON);
+	if (ret) {
+		printf("%s: mcde_chnl_set_power_mode() -> %d\n", __func__, ret);
+		goto get_chnl_failed;
+	}
 
 	return ret;
 
 get_chnl_failed:
-display_power_failed:
-enable_gpio_failed:
+set_video_mode_failed:
 	mcde_exit();
 	return ret;
 }
diff --git a/board/st/u8500/mcde_display.h b/board/st/u8500/mcde_display.h
index ff0e8f847..25f74eb1e 100644
--- a/board/st/u8500/mcde_display.h
+++ b/board/st/u8500/mcde_display.h
@@ -30,7 +30,14 @@ struct mcde_display_device {
 	u16 native_y_res;
 };
 
+extern struct mcde_display_device main_display;
+extern struct mcde_display_generic_platform_data main_display_data;
+extern struct mcde_video_mode video_mode;
+extern struct mcde_platform_data platform_data;
+
 int mcde_splash_image(void);
+int mcde_startup_dpi(struct mcde_platform_data *pdata);
+int mcde_startup_dsi(struct mcde_platform_data *pdata);
 int mcde_display_image(struct mcde_chnl_state *chnl);
 
 #endif /* !defined(__MCDE_UTILS_H) */
diff --git a/board/st/u8500/mcde_display_dpi.c b/board/st/u8500/mcde_display_dpi.c
new file mode 100644
index 000000000..c2122c672
--- /dev/null
+++ b/board/st/u8500/mcde_display_dpi.c
@@ -0,0 +1,110 @@
+/*
+* Copyright (C) ST-Ericsson SA 2010
+*
+* Author: Torbjorn Svensson <torbjorn.x.svensson@stericsson.com>
+* for ST-Ericsson.
+*
+* License terms: GNU General Public License (GPL), version 2.
+*/
+
+
+#include <common.h>
+#include <command.h>
+#include <asm/arch/common.h>
+#include "mcde_display.h"
+#include "mcde_regs.h"
+#include <malloc.h>
+#include "mcde.h"
+#include <linux/err.h>
+#include <asm/arch/ab8500.h>
+#include "common.h"
+
+static int mcde_enable_gpio(void)
+{
+	debug("%s: enter\n", __func__);
+	/* placeholder */
+
+	return 0;
+}
+
+int mcde_turn_on_display_dpi(void)
+{
+	debug("Turn on display (dpi)!\n");
+	/* placeholder */
+
+	return 0;
+}
+
+static int mcde_display_power_init(void)
+{
+	debug("%s: Entering\n", __func__);
+	/* placeholder */
+
+	return 0;
+}
+
+int dpi_display_platform_enable(void)
+{
+	int res = 0;
+	debug("%s: Entering\n", __func__);
+
+	/* the gpios used to be enabled here */
+
+	return res;
+}
+
+void print_vmode(struct mcde_video_mode *vmode)
+{
+	if (!vmode) {
+		printf("%s: vmode = NULL\n", __func__);
+		return;
+	}
+	debug("resolution: %dx%d\n", vmode->xres, vmode->yres);
+	debug("  pixclock: %d\n",    vmode->pixclock);
+	debug("       hbp: %d\n",    vmode->hbp);
+	debug("       hfp: %d\n",    vmode->hfp);
+	debug("       hsw: %d\n",    vmode->hsw);
+	debug("       vbp: %d\n",    vmode->vbp1);
+	debug("       vfp: %d\n",    vmode->vfp1);
+	debug("       vsw: %d\n",    vmode->vsw);
+	debug("interlaced: %s\n",    vmode->interlaced ? "true" : "false");
+	debug("    bckcol: (%d,%d,%d)\n",
+		vmode->bckcol[0], vmode->bckcol[1], vmode->bckcol[2]);
+}
+
+int mcde_startup_dpi(struct mcde_platform_data *pdata)
+{
+	int ret;
+
+	debug("%s: Entering\n", __func__);
+	if (main_display.port->mode != MCDE_PORTMODE_VID) {
+		printf("%s: Only VIDEO mode supported\n", __func__);
+		return -ENODEV;
+	}
+
+	mcde_init();
+	ret = mcde_probe(1, pdata);
+	if (ret != 0) {
+		printf("%s: mcde_probe() -> %d\n", __func__, ret);
+		return ret;
+	}
+	ret = mcde_display_power_init();
+	if (ret != 0) {
+		printf("%s: mcde_display_power_init() -> %d\n", __func__, ret);
+		return ret;
+	}
+	ret = mcde_enable_gpio();
+	if (ret != 0) {
+		printf("%s: mcde_enable_gpio() -> %d\n", __func__, ret);
+		return ret;
+	}
+	ret =  dpi_display_platform_enable();
+	if (ret != 0) {
+		printf("%s: dpi_display_platform_enable() -> %d\n",
+				__func__, ret);
+		return ret;
+	}
+	print_vmode(&video_mode);
+
+	return 0;
+}
diff --git a/board/st/u8500/mcde_display_dsi.c b/board/st/u8500/mcde_display_dsi.c
new file mode 100644
index 000000000..30186eb2b
--- /dev/null
+++ b/board/st/u8500/mcde_display_dsi.c
@@ -0,0 +1,269 @@
+/*
+* Copyright (C) ST-Ericsson SA 2010
+*
+* Author: Jimmy Rubin <jimmy.rubin@stericsson.com>
+* for ST-Ericsson.
+*
+* License terms: GNU General Public License (GPL), version 2.
+*/
+
+
+#include <common.h>
+#include <command.h>
+#include <asm/arch/common.h>
+#include "mcde_display.h"
+#include "dsilink_regs.h"
+#include <tc35892.h>
+#include "mcde_regs.h"
+#include <malloc.h>
+#include "mcde.h"
+#include <linux/err.h>
+#include <asm/arch/ab8500.h>
+
+static int mcde_enable_gpio(void)
+{
+	int ret;
+
+	debug("%s: enter\n", __func__);
+
+	/* Only main display should be initialized */
+	ret = tc35892_gpio_dir(CONFIG_SYS_I2C_GPIOE_ADDR,
+				main_display_data.reset_gpio, 1);
+	if (ret) {
+		printf("%s:Could not set direction for gpio\n", __func__);
+		return -EINVAL;
+	}
+	ret = tc35892_gpio_set(CONFIG_SYS_I2C_GPIOE_ADDR,
+				main_display_data.reset_gpio, 0);
+	if (ret) {
+		printf("%s:Could reset gpio\n", __func__);
+		return -EINVAL;
+	}
+	mdelay(main_display_data.reset_delay);
+	ret = tc35892_gpio_set(CONFIG_SYS_I2C_GPIOE_ADDR,
+				main_display_data.reset_gpio, 1);
+	if (ret) {
+		printf("%s:Could set gpio\n", __func__);
+		return -EINVAL;
+	}
+	mdelay(main_display_data.reset_delay);
+	return 0;
+}
+
+#define DCS_CMD_EXIT_SLEEP_MODE       0x11
+#define DCS_CMD_SET_DISPLAY_ON        0x29
+
+int mcde_turn_on_display_dsi(void)
+{
+	int ret = 0;
+
+	debug("%s: enter\n", __func__);
+	ret = mcde_dsi_dcs_write(main_display.port,
+				DCS_CMD_EXIT_SLEEP_MODE, NULL, 0);
+	if (!ret) {
+		debug("mcde_dsi_dcs_write "
+				"DCS_CMD_EXIT_SLEEP_MODE success!\n");
+		ret = mcde_dsi_dcs_write(main_display.port,
+				DCS_CMD_SET_DISPLAY_ON,  NULL, 0);
+		if (!ret)
+			debug("mcde_dsi_dcs_write "
+				"DCS_CMD_SET_DISPLAY_ON success!\n");
+	}
+	return ret;
+}
+
+/* aux supplies */
+#define MASK_LDO_VAUX1		0x3
+#define MASK_LDO_VAUX1_SHIFT	0x0
+#define VAUXSEL_VOLTAGE_MASK	0xf
+
+#define VANA_ENABLE_IN_HP_MODE	0x05
+
+#define ENABLE_PWM1		0x01
+#define PWM_DUTY_LOW_1024_1024	0xFF
+#define PWM_DUTY_HI_1024_1024	0x03
+
+/*
+ * regulator layout
+ * @voltage: supported voltage
+ * @regval: register value to be written
+ */
+struct regulator_voltage {
+	int voltage;
+	int regval;
+};
+
+/* voltage table for VAUXn regulators */
+static struct regulator_voltage vauxn_table[] = {
+	{ .voltage = 1100000, .regval  = 0x0, },
+	{ .voltage = 1200000, .regval  = 0x1, },
+	{ .voltage = 1300000, .regval  = 0x2, },
+	{ .voltage = 1400000, .regval  = 0x3, },
+	{ .voltage = 1500000, .regval  = 0x4, },
+	{ .voltage = 1800000, .regval  = 0x5, },
+	{ .voltage = 1850000, .regval  = 0x6, },
+	{ .voltage = 1900000, .regval  = 0x7, },
+	{ .voltage = 2500000, .regval  = 0x8, },
+	{ .voltage = 2650000, .regval  = 0x9, },
+	{ .voltage = 2700000, .regval  = 0xa, },
+	{ .voltage = 2750000, .regval  = 0xb, },
+	{ .voltage = 2800000, .regval  = 0xc, },
+	{ .voltage = 2900000, .regval  = 0xd, },
+	{ .voltage = 3000000, .regval  = 0xe, },
+	{ .voltage = 3300000, .regval  = 0xf, },
+};
+
+
+/*
+ * This code is derived from the handling of AB8500_LDO_VAUX1 in
+ * ab8500_ldo_is_enabled in Linux.
+ */
+static int mcde_is_vaux1_enabled(void)
+{
+	int val;
+	val = ab8500_read(AB8500_REGU_CTRL2,
+			AB8500_REGU_VAUX12_REGU_REG);
+	if (val & MASK_LDO_VAUX1)
+		return TRUE;
+	return FALSE;
+}
+
+/*
+ * This code is derived from the handling of AB8500_LDO_VAUX1 in
+ * ab8500_ldo_get_voltage in Linux.
+ */
+static int mcde_get_vaux1_voltage(void)
+{
+	int val;
+	val = ab8500_read(AB8500_REGU_CTRL2,
+		AB8500_REGU_VAUX1_SEL_REG);
+	return vauxn_table[val & VAUXSEL_VOLTAGE_MASK].voltage;
+}
+
+static int mcde_display_power_init(void)
+{
+	int val;
+	int i;
+
+	debug("%s: enter\n", __func__);
+	/*
+	 * On v1.1 HREF boards (HREF+) and V2 boards
+	 * Vaux1 needs to be enabled for the
+	 * display to work.  This is done by enabling the regulators in the
+	 * AB8500 via PRCMU I2C transactions.
+	 *
+	 * This code is derived from the handling of AB8500_LDO_VAUX1 in
+	 * ab8500_ldo_enable(), ab8500_ldo_disable() and
+	 * ab8500_get_best_voltage in Linux.
+	 *
+	 * Turn off and delay is required to have it work across soft reboots.
+	 */
+
+	val = ab8500_read(AB8500_REGU_CTRL2,
+		AB8500_REGU_VAUX12_REGU_REG);
+	if (val < 0) {
+		printf("Read vaux1 status failed\n");
+		return -EINVAL;
+	}
+
+	/* Turn off */
+	if (ab8500_write(AB8500_REGU_CTRL2, AB8500_REGU_VAUX12_REGU_REG,
+			   val & ~MASK_LDO_VAUX1) < 0) {
+		printf("Turn off Vaux1 failed\n");
+		return -EINVAL;
+	}
+
+	udelay(10 * 1000);
+
+
+	/* Find voltage from vauxn table */
+	for (i = 0; i < ARRAY_SIZE(vauxn_table) ; i++) {
+		if (vauxn_table[i].voltage == CONFIG_SYS_DISPLAY_VOLTAGE) {
+			if (ab8500_write(AB8500_REGU_CTRL2,
+				AB8500_REGU_VAUX1_SEL_REG,
+				vauxn_table[i].regval) < 0) {
+				printf("AB8500_REGU_VAUX1_SEL_REG failed\n");
+				return -EINVAL;
+			}
+			break;
+		}
+	}
+
+	val = val & ~MASK_LDO_VAUX1;
+	val = val | (1 << MASK_LDO_VAUX1_SHIFT);
+
+	/* Turn on the supply */
+	if (ab8500_write(AB8500_REGU_CTRL2,
+			AB8500_REGU_VAUX12_REGU_REG, val) < 0) {
+		printf("Turn on Vaux1 failed\n");
+		return -EINVAL;
+	}
+
+	/*  DCI & CSI (DSI / PLL / Camera) */ /* Vana & Vpll HP mode */
+	if (ab8500_write(AB8500_REGU_CTRL2, AB8500_REGU_VPLLVANA_REGU_REG,
+						VANA_ENABLE_IN_HP_MODE) < 0) {
+		printf("Turn on Vana failed\n");
+		return -EINVAL;
+	}
+
+	/* Enable the PWM control for the backlight Main display */
+	if (ab8500_write(AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+							ENABLE_PWM1) < 0) {
+		printf("Enable PWM1 failed\n");
+		return -EINVAL;
+	}
+	if (ab8500_write(AB8500_MISC, AB8500_PWM_OUT_CTRL1_REG,
+						PWM_DUTY_LOW_1024_1024) < 0) {
+		printf("PWM_DUTY_LOW_1024_1024 failed\n");
+		return -EINVAL;
+	}
+	if (ab8500_write(AB8500_MISC, AB8500_PWM_OUT_CTRL2_REG,
+						PWM_DUTY_HI_1024_1024) < 0) {
+		printf("PWM_DUTY_HI_1024_1024 failed\n");
+		return -EINVAL;
+	}
+
+	if (!mcde_is_vaux1_enabled() || mcde_get_vaux1_voltage()
+					!= CONFIG_SYS_DISPLAY_VOLTAGE) {
+		printf("VAUX is %d and is set to %d V should be %d\n"
+			, mcde_is_vaux1_enabled(), mcde_get_vaux1_voltage()
+			, CONFIG_SYS_DISPLAY_VOLTAGE);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+int mcde_startup_dsi(struct mcde_platform_data *pdata)
+{
+	u8 num_dsilinks;
+	int ret;
+
+	debug("%s: enter\n", __func__);
+
+	if (main_display.port->mode != MCDE_PORTMODE_CMD) {
+		printf("%s: only CMD mode supported\n", __func__);
+		return -ENODEV;
+	}
+	num_dsilinks = main_display.port->phy.dsi.num_data_lanes;
+	mcde_init();
+	ret = mcde_probe(num_dsilinks, pdata);
+	if (ret) {
+		printf("%s: mcde_probe() -> %d\n", __func__, ret);
+		return ret;
+	}
+	ret = mcde_display_power_init();
+	if (ret) {
+		printf("%s: mcde_display_power_init() -> %d\n", __func__, ret);
+		return ret;
+	}
+
+	ret = mcde_enable_gpio();
+	if (ret) {
+		printf("%s: mcde_enable_gpio() -> %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
diff --git a/board/st/u8500/mcde_display_image.c b/board/st/u8500/mcde_display_image.c
index 483b09cc1..fb05ce98f 100644
--- a/board/st/u8500/mcde_display_image.c
+++ b/board/st/u8500/mcde_display_image.c
@@ -13,14 +13,10 @@
 #include <part.h>
 #include <mmc.h>
 #include <bmp_layout.h>
+#include <asm/arch/common.h>
 #include "mcde.h"
 #include "mcde_display.h"
 
-#define DEBUG_THIS_FILE 0
-#define dbg_printk(format, arg...)			\
-	if (DEBUG_THIS_FILE)				\
-		printf("mcde: " format, ##arg)
-
 /* bmp compression constants */
 #define BI_RGB		0
 #define BI_RLE8		1	/* RLE 8-bit/pixel */
@@ -156,8 +152,7 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 	u32 xpos = 0;
 	u32 ypos = 0;
 
-	dbg_printk("%s: Enter\n", __func__);
-
+	debug("%s: Enter\n", __func__);
 	emmc_dev = find_mmc_device(CONFIG_EMMC_DEV_NUM);
 	if (emmc_dev == NULL) {
 		printf("mcde_display_image: emmc not found.\n");
@@ -172,7 +167,7 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 
 	/* get bmp_image */
 	bmp_start = (u8 *)address;
-	dbg_printk("%s: bmp start = 0x%p\n", __func__, (void *)bmp_start);
+	debug("%s: bmp start = 0x%p\n", __func__, (void *)bmp_start);
 
 	/* check BMP magic */
 	bmp_header = (struct bmp_header *)bmp_start;
@@ -184,32 +179,32 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 
 	/* get offset to bitmap-data from the BMP header */
 	bmp_offset = read_unaligned32(&bmp_header->data_offset);
-	dbg_printk("bmp filesz = %d\n",
+	debug("bmp filesz = %d\n",
 		read_unaligned32(&bmp_header->file_size));
-	dbg_printk("bmp offset = %d\n", bmp_offset);
+	debug("bmp offset = %d\n", bmp_offset);
 
 	dib_width = read_unaligned32((uint32_t *)&bmp_header->width);
 	dib_height = read_unaligned32((uint32_t *)&bmp_header->height);
 	dib_bytesz = read_unaligned32(&bmp_header->image_size);
 	dib_bitspp = bmp_header->bit_count;
 	dib_header_size = read_unaligned32(&bmp_header->size);
-	dbg_printk("dib header_sz = %d\n", dib_header_size);
-	dbg_printk("dib width = %d\n", dib_width);
-	dbg_printk("dib height = %d\n", dib_height);
-	dbg_printk("dib nplanes = %d\n", bmp_header->planes);
-	dbg_printk("dib bitspp = %d\n", dib_bitspp);
+	debug("dib header_sz = %d\n", dib_header_size);
+	debug("dib width = %d\n", dib_width);
+	debug("dib height = %d\n", dib_height);
+	debug("dib nplanes = %d\n", bmp_header->planes);
+	debug("dib bitspp = %d\n", dib_bitspp);
 	dib_compression = read_unaligned32(&bmp_header->compression);
-	dbg_printk("dib compress_type = %d\n", dib_compression);
-	dbg_printk("dib dib_bytesz = %d\n", dib_bytesz);
+	debug("dib compress_type = %d\n", dib_compression);
+	debug("dib dib_bytesz = %d\n", dib_bytesz);
 
 	/* calculate palette address */
 	palette = ((u8 *)&bmp_header->size + dib_header_size);
 	palette_size = ((bmp_start + bmp_offset) - palette);
-	dbg_printk("palette size = %d\n", palette_size);
+	debug("palette size = %d\n", palette_size);
 	/* if same as image start: no palette */
 	if (palette_size == 0)
 		palette = NULL;
-	dbg_printk("palette = 0x%08x\n", (u32)palette);
+	debug("palette = 0x%08x\n", (u32)palette);
 
 	/* check validity */
 	if ((dib_width > main_display.native_x_res) ||
@@ -227,7 +222,7 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 	src_pitch = dib_width * dib_bitspp / 8;
 	src_pitch = (src_pitch + 3) >> 2;	/* pad to 32-bit boundary */
 	src_pitch <<= 2;
-	dbg_printk("src_pitch=%d\n", src_pitch);
+	debug("src_pitch=%d\n", src_pitch);
 
 	if (dib_compression != BI_RGB && dib_compression != BI_BITFIELDS)
 		err++;
@@ -239,12 +234,12 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 	dst_pitch = dib_width * 16 / 8;		/* dest is 16 bpp */
 	dst_pitch = (dst_pitch + 7) >> 3;	/* pad to 64-bit boundary */
 	dst_pitch <<= 3;
-	dbg_printk("dst_pitch=%d\n", dst_pitch);
+	debug("dst_pitch=%d\n", dst_pitch);
 
 	/* image is stored upside-down in the file */
 	bmp_dst = ovly_mem;
 	bmp_src = (u8 *)(bmp_start + bmp_offset);
-	dbg_printk("bmp copy dst=0x%08x, src=0x%08x, len=%d\n",
+	debug("bmp copy dst=0x%08x, src=0x%08x, len=%d\n",
 		(uint32_t)bmp_dst, (uint32_t)bmp_src, dib_bytesz);
 
 	switch (dib_bitspp) {
@@ -265,20 +260,20 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 		printf("%s: unsupported bitspp=%d\n", __func__, dib_bitspp);
 		return -EINVAL;
 	}
-	dbg_printk("%s: image OK\n", __func__);
+	debug("%s: image OK\n", __func__);
 
+	/* dss_enable_overlay */
 	ovly = mcde_ovly_get(chnl);
 	if (IS_ERR(ovly)) {
 		err = PTR_ERR(ovly);
 		printf("%s: Failed to get channel\n", __func__);
 		return -err;
 	}
-	dbg_printk("ovly=%p ovly_mem=%p\n", (void *)ovly, (void *)ovly_mem);
+	debug("ovly=%p ovly_mem=%p\n", (void *)ovly, (void *)ovly_mem);
 	mcde_ovly_set_source_buf(ovly, (u32)ovly_mem);
 	mcde_ovly_set_source_info(ovly, dst_pitch,
 					main_display.default_pixel_format);
 	mcde_ovly_set_source_area(ovly, 0, 0, dib_width, dib_height);
-
 	if (dib_width == main_display.native_x_res)
 		xpos = 0;
 	else
@@ -288,11 +283,8 @@ int mcde_display_image(struct mcde_chnl_state *chnl)
 		ypos = 0;
 	else
 		ypos = (main_display.native_y_res - dib_height) / 2;
-
 	mcde_ovly_set_dest_pos(ovly, xpos, ypos, 0);
-
 	mcde_ovly_apply(ovly);
-	mcde_chnl_update(chnl);
 
 	return mcde_turn_on_display();
 }
diff --git a/board/st/u8500/mcde_hw.c b/board/st/u8500/mcde_hw.c
index d89a8eb52..72e4596ae 100644
--- a/board/st/u8500/mcde_hw.c
+++ b/board/st/u8500/mcde_hw.c
@@ -1,6 +1,8 @@
 /*
 * Copyright (C) ST-Ericsson SA 2010
 *
+ * ST-Ericsson MCDE driver for u-boot
+ *
 * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
 * for ST-Ericsson.
 *
@@ -20,14 +22,29 @@
 #include "mcde_regs.h"
 #include "mcde.h"
 #include <asm/arch/hardware.h>
+#define SCREEN_PPL_HIGH 1920
 
-#define DEBUG 0
-#define dbg_printk(format, arg...)			\
-	if (DEBUG)					\
-		printf("mcde: " format, ##arg)
+#define false	(0)
+#define true	(1)
 
-u8 *mcdeio;
-u8 **dsiio;
+/* For compatability with kernel code */
+#define dev_info(dev, format, arg...)			\
+		debug("mcde: " format, ##arg)
+#define dev_warn(dev, format, arg...)			\
+		debug("mcde: " format, ##arg)
+#define dev_dbg(dev, format, arg...)			\
+		debug("mcde: " format, ##arg)
+#define dev_vdbg(dev, format, arg...)			\
+		debug("mcde: " format, ##arg)
+
+#define msleep(m) mdelay(m)
+
+extern void print_vmode(struct mcde_video_mode *vmode);	/* for debugging */
+
+static u8 *mcdeio;
+static u8 **dsiio;
+
+static u8 hardware_version;
 
 static inline u32 dsi_rreg(int i, u32 reg)
 {
@@ -63,19 +80,19 @@ static inline void mcde_wreg(u32 reg, u32 val)
 
 struct ovly_regs {
 	u8   ch_id;
-	t_bool enabled;
+	u8   enabled;
 	u32  baseaddress0;
 	u32  baseaddress1;
-	t_bool buf_id;
+	u8   reset_buf_id;
 	u8   bits_per_pixel;
 	u8   bpp;
-	t_bool bgr;
-	t_bool bebo;
-	t_bool opq;
+	u8   bgr;
+	u8   bebo;
+	u8   opq;
+	u8   col_conv;
 	u8   pixoff;
 	u16  ppl;
 	u16  lpf;
-	u32  ljinc;
 	u16  cropx;
 	u16  cropy;
 	u16  xpos;
@@ -84,7 +101,7 @@ struct ovly_regs {
 };
 
 struct mcde_ovly_state {
-	t_bool inuse;
+	u8 inuse;
 	u8 idx; /* MCDE overlay index */
 	struct mcde_chnl_state *chnl; /* Owner channel */
 	u32 transactionid; /* Apply time stamp */
@@ -116,29 +133,78 @@ static struct mcde_ovly_state overlays[] = {
 };
 
 struct chnl_regs {
-	t_bool floen;
+	u8  floen;
 	u16 x;
 	u16 y;
 	u16 ppl;
 	u16 lpf;
 	u8  bpp;
+	u8  internal_clk;
+	u16 pcd;
+	u8  clksel;
+	u8  cdwin;
+	u8  bcd;
+	u8  synchronized_update;
+	u8  roten;
+
 	/* DSI */
-	u8 dsipacking;
+	u8  dsipacking;
+};
+
+struct col_regs {
+	u16 y_red;
+	u16 y_green;
+	u16 y_blue;
+	u16 cb_red;
+	u16 cb_green;
+	u16 cb_blue;
+	u16 cr_red;
+	u16 cr_green;
+	u16 cr_blue;
+	u16 off_red;
+	u16 off_green;
+	u16 off_blue;
+};
+
+struct tv_regs {
+	u16 dho; /* TV mode: left border width; destination horizontal offset */
+		 /* LCD MODE: horizontal back porch */
+	u16 alw; /* TV mode: right border width */
+		 /* LCD mode: horizontal front porch */
+	u16 hsw; /* horizontal synch width */
+	u16 dvo; /* TV mode: top border width; destination horizontal offset */
+		 /* LCD MODE: vertical back porch */
+	u16 bsl; /* TV mode: bottom border width; blanking start line */
+		 /* LCD MODE: vertical front porch */
+	/* field 1 */
+	u16 bel1; /* TV mode: field total vertical blanking lines */
+		 /* LCD mode: vertical sync width */
+	u16 fsl1; /* field vbp */
+	/* field 2 */
+	u16 bel2;
+	u16 fsl2;
+	u8 tv_mode;
+	u8 sel_mode_tv;
+	u8 interlaced_en;
+	u32 lcdtim1;
 };
 
 struct mcde_chnl_state {
-	t_bool inuse;
+	u8 inuse;
 	enum mcde_chnl id;
 	enum mcde_fifo fifo;
 	struct mcde_port port;
 	struct mcde_ovly_state *ovly0;
 	struct mcde_ovly_state *ovly1;
+	const struct chnl_config *cfg;
 	u32 transactionid;
 	u32 transactionid_regs;
 
+	enum mcde_display_power_mode power_mode;
 	/* Staged settings */
-	t_bool synchronized_update;
+	u8 synchronized_update;
 	enum mcde_port_pix_fmt pix_fmt;
+	struct mcde_video_mode vmode;
 	u16 update_x;
 	u16 update_y;
 	u16 update_w;
@@ -146,36 +212,289 @@ struct mcde_chnl_state {
 
 	/* Applied settings */
 	struct chnl_regs regs;
+	struct col_regs  col_regs;
+	struct tv_regs   tv_regs;
+
+	u8 continous_running;
 };
 
 static struct mcde_chnl_state channels[] = {
 	{
+		.id = MCDE_CHNL_A,
+		.ovly0 = &overlays[0],
+		.ovly1 = &overlays[1],
+	},
+	{
+		.id = MCDE_CHNL_B,
+		.ovly0 = &overlays[2],
+		.ovly1 = &overlays[3],
+	},
+	{
 		.id = MCDE_CHNL_C0,
 		.ovly0 = &overlays[4],
 		.ovly1 = NULL,
 	},
+	{
+		.id = MCDE_CHNL_C1,
+		.ovly0 = &overlays[5],
+		.ovly1 = NULL,
+	},
+};
+
+struct chnl_config {
+	/* Key */
+	enum mcde_chnl_path path;
+
+	/* Value */
+	u8 swap_a_c0;
+	u8 swap_a_c0_set;
+	u8 swap_b_c1;
+	u8 swap_b_c1_set;
+	u8 fabmux;
+	u8 fabmux_set;
+	u8 f01mux;
+	u8 f01mux_set;
 };
 
+int mcde_chnl_set_video_mode(struct mcde_chnl_state *chnl,
+				struct mcde_video_mode *vmode)
+{
+	if (chnl == NULL || vmode == NULL)
+		return -EINVAL;
+
+	chnl->vmode = *vmode;
+
+	return 0;
+}
+
+static void dpi_video_mode_apply(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	chnl->tv_regs.interlaced_en = chnl->vmode.interlaced;
+
+	chnl->tv_regs.sel_mode_tv = chnl->port.phy.dpi.tv_mode;
+	if (chnl->tv_regs.sel_mode_tv) {
+		/* TV mode */
+		u32 bel = chnl->vmode.vbp1 + chnl->vmode.vfp1
+				+ chnl->vmode.vbp2 + chnl->vmode.vfp2;
+		/* -4 since hsw is excluding SAV/EAV, 2 bytes each */
+		chnl->tv_regs.hsw  = chnl->vmode.hbp + chnl->vmode.hfp - 4;
+		chnl->tv_regs.dho  = MCDE_CONFIG_TVOUT_HBORDER;
+		chnl->tv_regs.alw  = MCDE_CONFIG_TVOUT_HBORDER;
+		/* in TV mode: bel1 = bel2 + 1 */
+		chnl->tv_regs.bel2 = bel / 2;
+		chnl->tv_regs.bel1 = bel - chnl->tv_regs.bel2;
+		chnl->tv_regs.dvo  = MCDE_CONFIG_TVOUT_VBORDER;
+		chnl->tv_regs.bsl  = MCDE_CONFIG_TVOUT_VBORDER;
+		chnl->tv_regs.fsl1 = chnl->vmode.vbp1;
+		chnl->tv_regs.fsl2 = chnl->vmode.vbp2;
+		if (chnl->port.phy.dpi.bus_width == 4)
+			chnl->tv_regs.tv_mode = MCDE_TVCRA_TVMODE_SDTV_656P_BE;
+		else
+			chnl->tv_regs.tv_mode = MCDE_TVCRA_TVMODE_SDTV_656P;
+	} else {
+		/* LCD mode */
+		u32 polarity;
+		chnl->tv_regs.hsw  = chnl->vmode.hsw;
+		chnl->tv_regs.dho  = chnl->vmode.hbp;
+		chnl->tv_regs.alw  = chnl->vmode.hfp;
+		chnl->tv_regs.bel1 = chnl->vmode.vsw;
+		chnl->tv_regs.bel2 = chnl->tv_regs.bel1;
+		chnl->tv_regs.dvo  = chnl->vmode.vbp1 + chnl->vmode.vbp2;
+		chnl->tv_regs.bsl  = chnl->vmode.vfp1 + chnl->vmode.vfp2;
+		chnl->tv_regs.fsl1 = 0;
+		chnl->tv_regs.fsl2 = 0;
+		polarity = chnl->port.phy.dpi.polarity;
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IPC(
+				(polarity & DPI_ACT_ON_FALLING_EDGE) != 0);
+		chnl->tv_regs.lcdtim1 = MCDE_LCDTIM1A_IHS(
+				(polarity & DPI_ACT_LOW_HSYNC) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IVS(
+				(polarity & DPI_ACT_LOW_VSYNC) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IOE(
+				(polarity & DPI_ACT_LOW_DATA_ENABLE) != 0);
+	}
+	debug("%s: Leaving\n", __func__);
+}
+
+static void update_dpi_registers(enum mcde_chnl chnl_id, struct tv_regs *regs)
+{
+	u8 idx = chnl_id;
+
+	dev_dbg(&mcde_dev->dev, "%s\n", __func__);
+	mcde_wreg(MCDE_TVCRA + idx * MCDE_TVCRA_GROUPOFFSET,
+			MCDE_TVCRA_SEL_MOD(regs->sel_mode_tv)             |
+			MCDE_TVCRA_INTEREN(regs->interlaced_en)           |
+			MCDE_TVCRA_IFIELD(1)                              |
+			MCDE_TVCRA_TVMODE(regs->tv_mode)                  |
+			MCDE_TVCRA_SDTVMODE(MCDE_TVCRA_SDTVMODE_Y0CBY1CR) |
+			MCDE_TVCRA_AVRGEN(0));
+	mcde_wreg(MCDE_TVBLUA + idx * MCDE_TVBLUA_GROUPOFFSET,
+		MCDE_TVBLUA_TVBLU(MCDE_CONFIG_TVOUT_BACKGROUND_LUMINANCE) |
+		MCDE_TVBLUA_TVBCB(MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CB)|
+		MCDE_TVBLUA_TVBCR(MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CR));
+
+	/* Vertical timing registers */
+	mcde_wreg(MCDE_TVDVOA + idx * MCDE_TVDVOA_GROUPOFFSET,
+					MCDE_TVDVOA_DVO1(regs->dvo) |
+					MCDE_TVDVOA_DVO2(regs->dvo));
+	mcde_wreg(MCDE_TVBL1A + idx * MCDE_TVBL1A_GROUPOFFSET,
+					MCDE_TVBL1A_BEL1(regs->bel1) |
+					MCDE_TVBL1A_BSL1(regs->bsl));
+	mcde_wreg(MCDE_TVBL2A + idx * MCDE_TVBL1A_GROUPOFFSET,
+					MCDE_TVBL2A_BEL2(regs->bel2) |
+					MCDE_TVBL2A_BSL2(regs->bsl));
+	mcde_wreg(MCDE_TVISLA + idx * MCDE_TVISLA_GROUPOFFSET,
+					MCDE_TVISLA_FSL1(regs->fsl1) |
+					MCDE_TVISLA_FSL2(regs->fsl2));
+
+	/* Horizontal timing registers */
+	if (!regs->sel_mode_tv ||
+			hardware_version == MCDE_CHIP_VERSION_3_0_8) {
+		mcde_wreg(MCDE_TVLBALWA + idx * MCDE_TVLBALWA_GROUPOFFSET,
+					MCDE_TVLBALWA_LBW(regs->hsw) |
+					MCDE_TVLBALWA_ALW(regs->alw));
+		mcde_wreg(MCDE_TVTIM1A + idx * MCDE_TVTIM1A_GROUPOFFSET,
+					MCDE_TVTIM1A_DHO(regs->dho));
+	} else {
+		/*
+		 * in earlier versions the LBW and DHO fields are swapped
+		 * TV mode only
+		 */
+		mcde_wreg(MCDE_TVLBALWA + idx * MCDE_TVLBALWA_GROUPOFFSET,
+					MCDE_TVLBALWA_LBW(regs->dho) |
+					MCDE_TVLBALWA_ALW(regs->alw));
+		mcde_wreg(MCDE_TVTIM1A + idx * MCDE_TVTIM1A_GROUPOFFSET,
+					MCDE_TVTIM1A_DHO(regs->hsw));
+	}
+	if (!regs->sel_mode_tv)
+		mcde_wreg(MCDE_LCDTIM1A + idx * MCDE_LCDTIM1A_GROUPOFFSET,
+								regs->lcdtim1);
+	debug("%s: Leaving\n", __func__);
+}
+
+static void update_col_registers(enum mcde_chnl chnl_id, struct col_regs *regs)
+{
+	u8 idx = chnl_id;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	mcde_wreg(MCDE_RGBCONV1A + idx * MCDE_RGBCONV1A_GROUPOFFSET,
+				MCDE_RGBCONV1A_YR_RED(regs->y_red) |
+				MCDE_RGBCONV1A_YR_GREEN(regs->y_green));
+	mcde_wreg(MCDE_RGBCONV2A + idx * MCDE_RGBCONV2A_GROUPOFFSET,
+				MCDE_RGBCONV2A_YR_BLUE(regs->y_blue) |
+				MCDE_RGBCONV2A_CR_RED(regs->cr_red));
+	mcde_wreg(MCDE_RGBCONV3A + idx * MCDE_RGBCONV3A_GROUPOFFSET,
+				MCDE_RGBCONV3A_CR_GREEN(regs->cr_green) |
+				MCDE_RGBCONV3A_CR_BLUE(regs->cr_blue));
+	mcde_wreg(MCDE_RGBCONV4A + idx * MCDE_RGBCONV4A_GROUPOFFSET,
+				MCDE_RGBCONV4A_CB_RED(regs->cb_red) |
+				MCDE_RGBCONV4A_CB_GREEN(regs->cb_green));
+	mcde_wreg(MCDE_RGBCONV5A + idx * MCDE_RGBCONV5A_GROUPOFFSET,
+				MCDE_RGBCONV5A_CB_BLUE(regs->cb_blue) |
+				MCDE_RGBCONV5A_OFF_RED(regs->off_red));
+	mcde_wreg(MCDE_RGBCONV6A + idx * MCDE_RGBCONV6A_GROUPOFFSET,
+				MCDE_RGBCONV6A_OFF_GREEN(regs->off_green) |
+				MCDE_RGBCONV6A_OFF_BLUE(regs->off_blue));
+}
+
 /* MCDE internal helpers */
 static u8 portfmt2dsipacking(enum mcde_port_pix_fmt pix_fmt)
 {
 	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+		return MCDE_DSIVID0CONF0_PACKING_RGB565;
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		return MCDE_DSIVID0CONF0_PACKING_RGB666;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
 	case MCDE_PORTPIXFMT_DSI_24BPP:
 	default:
 		return MCDE_DSIVID0CONF0_PACKING_RGB888;
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		return MCDE_DSIVID0CONF0_PACKING_HDTV;
 	}
 }
 
 static u8 portfmt2bpp(enum mcde_port_pix_fmt pix_fmt)
 {
 	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DPI_16BPP_C1:
+	case MCDE_PORTPIXFMT_DPI_16BPP_C2:
+	case MCDE_PORTPIXFMT_DPI_16BPP_C3:
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		return 16;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C1:
+	case MCDE_PORTPIXFMT_DPI_18BPP_C2:
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		return 18;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+	case MCDE_PORTPIXFMT_DPI_24BPP:
 	case MCDE_PORTPIXFMT_DSI_24BPP:
 		return 24;
 	default:
+		return 1;
+	}
+}
+
+static u8 bpp2outbpp(u8 bpp)
+{
+	switch (bpp) {
+	case 16:
+		return MCDE_CRA1_OUTBPP_16BPP;
+	case 18:
+		return MCDE_CRA1_OUTBPP_18BPP;
+	case 24:
+		return MCDE_CRA1_OUTBPP_24BPP;
+	default:
 		return 0;
 	}
 }
 
+static u8 portfmt2cdwin(enum mcde_port_pix_fmt pix_fmt)
+{
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DPI_16BPP_C1:
+		return MCDE_CRA1_CDWIN_16BBP_C1;
+	case MCDE_PORTPIXFMT_DPI_16BPP_C2:
+		return MCDE_CRA1_CDWIN_16BBP_C2;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C1:
+		return MCDE_CRA1_CDWIN_18BBP_C1;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C2:
+		return MCDE_CRA1_CDWIN_18BBP_C2;
+	case MCDE_PORTPIXFMT_DPI_24BPP:
+		return MCDE_CRA1_CDWIN_24BBP;
+
+	case MCDE_PORTPIXFMT_DPI_16BPP_C3:
+		/* 16 BPP C3 not supported by HW */
+		dev_warn(&mcde_dev->dev, "DPI 16 BPP C3 not supported\n");
+		/* intentional fall through */
+	default:
+		/* only DPI formats are relevant */
+		return 0;
+	}
+}
+
+static u32 get_output_fifo_size(enum mcde_fifo fifo)
+{
+	u32 ret = 1; /* Avoid div by zero */
+
+	switch (fifo) {
+	case MCDE_FIFO_A:
+	case MCDE_FIFO_B:
+		ret = MCDE_FIFO_AB_SIZE;
+		break;
+	case MCDE_FIFO_C0:
+	case MCDE_FIFO_C1:
+		ret = MCDE_FIFO_C0C1_SIZE;
+		break;
+	default:
+		dev_vdbg(&mcde_dev->dev, "Unsupported fifo");
+		break;
+	}
+	return ret;
+}
+
 static u8 get_dsi_formid(const struct mcde_port *port)
 {
 	if (port->ifc == DSI_VIDEO_MODE && port->link == 0)
@@ -193,73 +512,131 @@ static u8 get_dsi_formid(const struct mcde_port *port)
 	return 0;
 }
 
-#define DSI_UNIT_INTERVAL_0	0x9
+
+void wait_for_overlay(struct mcde_ovly_state *ovly)
+{
+	/* ovly not used in u-boot */
+}
+
+void wait_for_channel(struct mcde_chnl_state *chnl)
+{
+	/* ovly not used in u-boot */
+}
 
 static int update_channel_static_registers(struct mcde_chnl_state *chnl)
 {
+	const struct chnl_config *cfg = chnl->cfg;
 	const struct mcde_port *port = &chnl->port;
-	int i = 0;
-	u8 idx = 2 * port->link + port->ifc;
-	u8 lnk = port->link;
 
-	if (!cpu_is_u8500v2()) {
+	if (hardware_version == MCDE_CHIP_VERSION_3_0_5) {
 		/* Fifo & muxing */
-		mcde_wfld(MCDE_CONF0, SWAP_A_C0_V1, TRUE);
-		mcde_wfld(MCDE_CR, FABMUX_V1, FALSE);
-
-		if (port->ifc == DSI_VIDEO_MODE && port->link == 0)
-			mcde_wfld(MCDE_CR, DSIVID0_EN_V1, TRUE);
-		else if (port->ifc == DSI_VIDEO_MODE && port->link == 1)
-			mcde_wfld(MCDE_CR, DSIVID1_EN_V1, TRUE);
-		else if (port->ifc == DSI_VIDEO_MODE && port->link == 2)
-			mcde_wfld(MCDE_CR, DSIVID2_EN_V1, TRUE);
-		else if (port->ifc == DSI_CMD_MODE && port->link == 0)
-			mcde_wfld(MCDE_CR, DSICMD0_EN_V1, TRUE);
-		else if (port->ifc == DSI_CMD_MODE && port->link == 1)
-			mcde_wfld(MCDE_CR, DSICMD1_EN_V1, TRUE);
-		else if (port->ifc == DSI_CMD_MODE && port->link == 2)
-			mcde_wfld(MCDE_CR, DSICMD2_EN_V1, TRUE);
-
-		mcde_wreg(MCDE_CTRLC0, MCDE_CTRLC0_FIFOWTRMRK(0xa0));
+		if (cfg->swap_a_c0_set)
+			mcde_wfld(MCDE_CONF0, SWAP_A_C0_V1, cfg->swap_a_c0);
+		if (cfg->swap_b_c1_set)
+			mcde_wfld(MCDE_CONF0, SWAP_B_C1_V1, cfg->swap_b_c1);
+		if (cfg->fabmux_set)
+			mcde_wfld(MCDE_CR, FABMUX_V1, cfg->fabmux);
+		if (cfg->f01mux_set)
+			mcde_wfld(MCDE_CR, F01MUX_V1, cfg->f01mux);
+
+		if (port->type == MCDE_PORTTYPE_DPI) {
+			if (port->link == 0)
+				mcde_wfld(MCDE_CR, DPIA_EN_V1, true);
+			else if (port->link == 1)
+				mcde_wfld(MCDE_CR, DPIB_EN_V1, true);
+		} else if (port->type == MCDE_PORTTYPE_DSI) {
+			if (port->ifc == DSI_VIDEO_MODE && port->link == 0)
+				mcde_wfld(MCDE_CR, DSIVID0_EN_V1, true);
+			else if (port->ifc == DSI_VIDEO_MODE && port->link == 1)
+				mcde_wfld(MCDE_CR, DSIVID1_EN_V1, true);
+			else if (port->ifc == DSI_VIDEO_MODE && port->link == 2)
+				mcde_wfld(MCDE_CR, DSIVID2_EN_V1, true);
+			else if (port->ifc == DSI_CMD_MODE && port->link == 0)
+				mcde_wfld(MCDE_CR, DSICMD0_EN_V1, true);
+			else if (port->ifc == DSI_CMD_MODE && port->link == 1)
+				mcde_wfld(MCDE_CR, DSICMD1_EN_V1, true);
+			else if (port->ifc == DSI_CMD_MODE && port->link == 2)
+				mcde_wfld(MCDE_CR, DSICMD2_EN_V1, true);
+		}
+
+		if (chnl->fifo == MCDE_FIFO_C0)
+			mcde_wreg(MCDE_CTRLC0, MCDE_CTRLC0_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C0)));
+		else if (chnl->fifo == MCDE_FIFO_C1)
+			mcde_wreg(MCDE_CTRLC1, MCDE_CTRLC1_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C1)));
+		else if (port->update_auto_trig &&
+					(port->sync_src == MCDE_SYNCSRC_TE0))
+			mcde_wreg(MCDE_CTRLC0, MCDE_CTRLC0_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C0)));
+		else if (port->update_auto_trig &&
+					(port->sync_src == MCDE_SYNCSRC_TE1))
+			mcde_wreg(MCDE_CTRLC1, MCDE_CTRLC1_FIFOWTRMRK(
+					get_output_fifo_size(MCDE_FIFO_C1)));
 	} else {
 		switch (chnl->fifo) {
 		case MCDE_FIFO_A:
 			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
-			MCDE_CHNL0MUXING_V2_GROUPOFFSET,
-			MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_A));
-			mcde_wfld(MCDE_CTRLA, FORMTYPE,
-					MCDE_CTRLA_FORMTYPE_DSI);
-			mcde_wfld(MCDE_CTRLA, FORMID,
-						get_dsi_formid(port));
-			mcde_wfld(MCDE_CTRLA, FIFOWTRMRK, 0x280);
+				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
+				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_A));
+			if (port->type == MCDE_PORTTYPE_DPI) {
+				mcde_wfld(MCDE_CTRLA, FORMTYPE,
+						MCDE_CTRLA_FORMTYPE_DPITV);
+				mcde_wfld(MCDE_CTRLA, FORMID, port->link);
+				mcde_wfld(MCDE_CTRLA, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_A));
+			} else if (port->type == MCDE_PORTTYPE_DSI) {
+				mcde_wfld(MCDE_CTRLA, FORMTYPE,
+						MCDE_CTRLA_FORMTYPE_DSI);
+				mcde_wfld(MCDE_CTRLA, FORMID,
+							get_dsi_formid(port));
+				mcde_wfld(MCDE_CTRLA, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_A));
+			}
 			break;
 		case MCDE_FIFO_B:
 			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
-			MCDE_CHNL0MUXING_V2_GROUPOFFSET,
-			MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_B));
-			mcde_wfld(MCDE_CTRLB, FORMTYPE,
-					MCDE_CTRLB_FORMTYPE_DSI);
-			mcde_wfld(MCDE_CTRLB, FORMID,
-						get_dsi_formid(port));
-			mcde_wfld(MCDE_CTRLB, FIFOWTRMRK, 0x280);
+				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
+				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_B));
+			if (port->type == MCDE_PORTTYPE_DPI) {
+				mcde_wfld(MCDE_CTRLB, FORMTYPE,
+						MCDE_CTRLB_FORMTYPE_DPITV);
+				mcde_wfld(MCDE_CTRLB, FORMID, port->link);
+				mcde_wfld(MCDE_CTRLB, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_B));
+			} else if (port->type == MCDE_PORTTYPE_DSI) {
+				mcde_wfld(MCDE_CTRLB, FORMTYPE,
+						MCDE_CTRLB_FORMTYPE_DSI);
+				mcde_wfld(MCDE_CTRLB, FORMID,
+							get_dsi_formid(port));
+				mcde_wfld(MCDE_CTRLB, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_B));
+			}
+
 			break;
 		case MCDE_FIFO_C0:
 			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
 				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
 				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_C0));
+			if (port->type == MCDE_PORTTYPE_DPI)
+				return -EINVAL;
 			mcde_wfld(MCDE_CTRLC0, FORMTYPE,
 						MCDE_CTRLC0_FORMTYPE_DSI);
 			mcde_wfld(MCDE_CTRLC0, FORMID, get_dsi_formid(port));
-			mcde_wfld(MCDE_CTRLC0, FIFOWTRMRK, 0xa0);
+			mcde_wfld(MCDE_CTRLC0, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_C0));
 			break;
 		case MCDE_FIFO_C1:
 			mcde_wreg(MCDE_CHNL0MUXING_V2 + chnl->id *
 				MCDE_CHNL0MUXING_V2_GROUPOFFSET,
 				MCDE_CHNL0MUXING_V2_FIFO_ID_ENUM(FIFO_C1));
+			if (port->type == MCDE_PORTTYPE_DPI)
+				return -EINVAL;
 			mcde_wfld(MCDE_CTRLC1, FORMTYPE,
 						MCDE_CTRLC1_FORMTYPE_DSI);
 			mcde_wfld(MCDE_CTRLC1, FORMID, get_dsi_formid(port));
-			mcde_wfld(MCDE_CTRLC1, FIFOWTRMRK, 0xa0);
+			mcde_wfld(MCDE_CTRLC1, FIFOWTRMRK,
+					get_output_fifo_size(MCDE_FIFO_C1));
 			break;
 		default:
 			return -EINVAL;
@@ -267,115 +644,189 @@ static int update_channel_static_registers(struct mcde_chnl_state *chnl)
 	}
 
 	/* Formatter */
-	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, LINK_EN, TRUE);
-	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, TRUE);
-	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, READ_EN, TRUE);
-	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, TRUE);
-	dsi_wreg(lnk, DSI_MCTL_DPHY_STATIC,
-		DSI_MCTL_DPHY_STATIC_UI_X4(DSI_UNIT_INTERVAL_0));
-	dsi_wreg(lnk, DSI_DPHY_LANES_TRIM,
-		DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_ENUM(0_90));
-	dsi_wreg(lnk, DSI_MCTL_DPHY_TIMEOUT,
-		DSI_MCTL_DPHY_TIMEOUT_CLK_DIV(0xf) |
-		DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL(0x3fff) |
-		DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL(0x3fff));
-	dsi_wreg(lnk, DSI_MCTL_MAIN_PHY_CTL,
-		DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME(0xf) |
-		DSI_MCTL_MAIN_PHY_CTL_LANE2_EN(TRUE) |
-		DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS(FALSE));
-	dsi_wreg(lnk, DSI_MCTL_ULPOUT_TIME,
-		DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME(1) |
-		DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME(1));
-	dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_MODE, FALSE);
-	dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_PRI, port->ifc == DSI_CMD_MODE);
-	dsi_wfld(lnk, DSI_CMD_MODE_CTL, TE_TIMEOUT, 0x3ff);
-	dsi_wreg(lnk, DSI_MCTL_MAIN_EN,
-		DSI_MCTL_MAIN_EN_PLL_START(TRUE) |
-		DSI_MCTL_MAIN_EN_CKLANE_EN(TRUE) |
-		DSI_MCTL_MAIN_EN_DAT1_EN(TRUE) |
-		DSI_MCTL_MAIN_EN_DAT2_EN(port->phy.dsi.num_data_lanes
-			== 2) |
-		DSI_MCTL_MAIN_EN_IF1_EN(port->ifc == DSI_VIDEO_MODE) |
-		DSI_MCTL_MAIN_EN_IF2_EN(port->ifc == DSI_CMD_MODE));
-	while (dsi_rfld(lnk, DSI_MCTL_MAIN_STS, CLKLANE_READY) == 0 ||
-	       dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT1_READY) == 0 ||
-	       dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT2_READY) == 0) {
-		mdelay(1);
-		if (i++ == 10) {
-			printf("DSI lane not ready (link=%d)!\n", lnk);
-			return -EINVAL;
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		int i = 0;
+		u8 idx = 2 * port->link + port->ifc;
+		u8 lnk = port->link;
+
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, LINK_EN, true);
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, true);
+
+		if (hardware_version == MCDE_CHIP_VERSION_3_0_5) {
+			if (port->phy.dsi.data_lanes_swap) {
+				dev_warn(&mcde_dev->dev,
+				"DSI %d data lane remap not available!\n",
+				lnk);
+				return -EINVAL;
+			}
+		} else
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, DLX_REMAP_EN,
+					port->phy.dsi.data_lanes_swap);
+
+		dsi_wreg(lnk, DSI_MCTL_DPHY_STATIC,
+			DSI_MCTL_DPHY_STATIC_UI_X4(port->phy.dsi.ui));
+		dsi_wreg(lnk, DSI_DPHY_LANES_TRIM,
+			DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_ENUM(0_90));
+		dsi_wreg(lnk, DSI_MCTL_DPHY_TIMEOUT,
+			DSI_MCTL_DPHY_TIMEOUT_CLK_DIV(0xf) |
+			DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL(0x3fff) |
+			DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL(0x3fff));
+		dsi_wreg(lnk, DSI_MCTL_MAIN_PHY_CTL,
+			DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME(0xf) |
+			DSI_MCTL_MAIN_PHY_CTL_LANE2_EN(true) |
+			DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS(
+				port->phy.dsi.clk_cont));
+		dsi_wreg(lnk, DSI_MCTL_ULPOUT_TIME,
+			DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME(1) |
+			DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME(1));
+		dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_MODE, false);
+		dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_PRI, port->ifc == 1);
+		dsi_wreg(lnk, DSI_MCTL_MAIN_EN,
+			DSI_MCTL_MAIN_EN_PLL_START(true) |
+			DSI_MCTL_MAIN_EN_CKLANE_EN(true) |
+			DSI_MCTL_MAIN_EN_DAT1_EN(true) |
+			DSI_MCTL_MAIN_EN_DAT2_EN(port->phy.dsi.num_data_lanes
+				== 2) |
+			DSI_MCTL_MAIN_EN_IF1_EN(port->ifc == DSI_VIDEO_MODE) |
+			DSI_MCTL_MAIN_EN_IF2_EN(port->ifc == DSI_CMD_MODE));
+		while (dsi_rfld(lnk, DSI_MCTL_MAIN_STS, CLKLANE_READY) == 0 ||
+		       dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT1_READY) == 0 ||
+		       dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT2_READY) == 0) {
+			mdelay(1);
+			if (i++ == 10) {
+				dev_warn(&mcde_dev->dev,
+					"DSI lane not ready (link=%d)!\n", lnk);
+				return -EINVAL;
+			}
 		}
-	}
-
-	mcde_wreg(MCDE_DSIVID0CONF0 +
-		idx * MCDE_DSIVID0CONF0_GROUPOFFSET,
-		MCDE_DSIVID0CONF0_BLANKING(0) |
-		MCDE_DSIVID0CONF0_VID_MODE(0) |
-		MCDE_DSIVID0CONF0_CMD8(TRUE) |
-		MCDE_DSIVID0CONF0_BIT_SWAP(FALSE) |
-		MCDE_DSIVID0CONF0_BYTE_SWAP(FALSE) |
-		MCDE_DSIVID0CONF0_DCSVID_NOTGEN(TRUE));
 
-	if (port->ifc == DSI_VIDEO_MODE)
-		dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF1_ID,
-			port->phy.dsi.virt_id);
-	else if (port->ifc == DSI_CMD_MODE)
-		dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF2_ID,
-			port->phy.dsi.virt_id);
+		mcde_wreg(MCDE_DSIVID0CONF0 +
+			idx * MCDE_DSIVID0CONF0_GROUPOFFSET,
+			MCDE_DSIVID0CONF0_BLANKING(0) |
+			MCDE_DSIVID0CONF0_VID_MODE(
+				port->mode == MCDE_PORTMODE_VID) |
+			MCDE_DSIVID0CONF0_CMD8(true) |
+			MCDE_DSIVID0CONF0_BIT_SWAP(false) |
+			MCDE_DSIVID0CONF0_BYTE_SWAP(false) |
+			MCDE_DSIVID0CONF0_DCSVID_NOTGEN(true));
+
+		if (port->mode == MCDE_PORTMODE_CMD) {
+			if (port->ifc == DSI_VIDEO_MODE)
+				dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF1_ID,
+					port->phy.dsi.virt_id);
+			else if (port->ifc == DSI_CMD_MODE)
+				dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF2_ID,
+					port->phy.dsi.virt_id);
+		}
+	}
 
-	mcde_wfld(MCDE_CRC, SYCEN0, TRUE);
-	mcde_wreg(MCDE_VSCRC0,
-		MCDE_VSCRC0_VSPMIN(1) |
-		MCDE_VSCRC0_VSPMAX(0xff));
+	mcde_wfld(MCDE_CR, MCDEEN, true);
 
-	mcde_wfld(MCDE_CR, MCDEEN, TRUE);
-	dbg_printk("Static registers setup, chnl=%d\n", chnl->id);
+	dev_vdbg(&mcde_dev->dev, "Static registers setup, chnl=%d\n", chnl->id);
 
 	return 0;
 }
 
 static void update_overlay_registers(u8 idx, struct ovly_regs *regs,
-	u16 update_x, u16 update_y, u16 update_w, u16 update_h)
+			struct mcde_port *port, enum mcde_fifo fifo,
+			u16 update_x, u16 update_y, u16 update_w,
+			u16 update_h, u16 stride, u8 interlaced)
 {
 	u32 lmrgn = (regs->cropx + update_x) * regs->bits_per_pixel;
-	u32 tmrgn = (regs->cropy + update_y) * regs->ljinc;
+	u32 tmrgn = (regs->cropy + update_y) * stride;
 	u32 ppl = regs->ppl - update_x;
 	u32 lpf = regs->lpf - update_y;
+	u32 ljinc = stride;
+	u32 pixelfetchwtrmrklevel;
+	u8  nr_of_bufs = 1;
+	u32 fifo_size;
 
+	/* TODO: disable if everything clipped */
 	if (!regs->enabled) {
 		u32 temp;
 		temp = mcde_rreg(MCDE_OVL0CR + idx * MCDE_OVL0CR_GROUPOFFSET);
 		mcde_wreg(MCDE_OVL0CR + idx * MCDE_OVL0CR_GROUPOFFSET,
 			(temp & ~MCDE_OVL0CR_OVLEN_MASK) |
-			MCDE_OVL0CR_OVLEN(FALSE));
+			MCDE_OVL0CR_OVLEN(false));
 		return;
 	}
 
-	mcde_wreg(MCDE_EXTSRC0A0 + idx * MCDE_EXTSRC0A0_GROUPOFFSET,
-		regs->baseaddress0);
-	mcde_wreg(MCDE_EXTSRC0A1 + idx * MCDE_EXTSRC0A1_GROUPOFFSET,
-		regs->baseaddress1);
+	/*
+	* TODO: Preferably most of this is done in some apply function instead
+	* of every update. Problem is however that at overlay apply
+	* there is no port type info available (and the question is
+	* whether it is appropriate to add a port type there).
+	* Note that lpf has a dependency on update_y.
+	*/
+	if (port->type == MCDE_PORTTYPE_DPI && port->phy.dpi.tv_mode)
+		/* REVIEW: Why not for DSI? enable in regs? */
+		regs->col_conv = MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT;
+	else if (port->type == MCDE_PORTTYPE_DSI) {
+		if (port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422)
+			regs->col_conv = MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT;
+		else
+			regs->col_conv = MCDE_OVL0CR_COLCCTRL_DISABLED;
+		if (interlaced) {
+			nr_of_bufs = 2;
+			lpf = lpf / 2;
+			ljinc *= 2;
+		}
+	}
+
+	fifo_size = get_output_fifo_size(fifo);
+#ifdef CONFIG_AV8100_SDTV
+	/* TODO: check if these watermark levels work for HDMI as well. */
+	pixelfetchwtrmrklevel = MCDE_PIXFETCH_SMALL_WTRMRKLVL;
+#else
+	if ((fifo == MCDE_FIFO_A || fifo == MCDE_FIFO_B) &&
+					regs->ppl >= fifo_size * 2)
+		pixelfetchwtrmrklevel = MCDE_PIXFETCH_LARGE_WTRMRKLVL;
+	else
+		pixelfetchwtrmrklevel = MCDE_PIXFETCH_MEDIUM_WTRMRKLVL;
+#endif /* CONFIG_AV8100_SDTV */
+
+	if (regs->reset_buf_id) {
+		u32 sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+		if (port->update_auto_trig && port->type == MCDE_PORTTYPE_DSI) {
+			switch (port->sync_src) {
+			case MCDE_SYNCSRC_OFF:
+				sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+				break;
+			case MCDE_SYNCSRC_TE0:
+			case MCDE_SYNCSRC_TE1:
+			default:
+				sel_mod = MCDE_EXTSRC0CR_SEL_MOD_AUTO_TOGGLE;
+			}
+		} else if (port->type == MCDE_PORTTYPE_DPI) {
+			sel_mod = port->update_auto_trig ?
+					MCDE_EXTSRC0CR_SEL_MOD_AUTO_TOGGLE :
+					MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+		}
+
+		regs->reset_buf_id = false;
 	mcde_wreg(MCDE_EXTSRC0CONF + idx * MCDE_EXTSRC0CONF_GROUPOFFSET,
-		MCDE_EXTSRC0CONF_BUF_ID(regs->buf_id) |
-		MCDE_EXTSRC0CONF_BUF_NB(2) |
+		MCDE_EXTSRC0CONF_BUF_ID(0) |
+			MCDE_EXTSRC0CONF_BUF_NB(nr_of_bufs) |
 		MCDE_EXTSRC0CONF_PRI_OVLID(idx) |
 		MCDE_EXTSRC0CONF_BPP(regs->bpp) |
 		MCDE_EXTSRC0CONF_BGR(regs->bgr) |
 		MCDE_EXTSRC0CONF_BEBO(regs->bebo) |
-		MCDE_EXTSRC0CONF_BEPO(FALSE));
+		MCDE_EXTSRC0CONF_BEPO(false));
 	mcde_wreg(MCDE_EXTSRC0CR + idx * MCDE_EXTSRC0CR_GROUPOFFSET,
-		MCDE_EXTSRC0CR_SEL_MOD_ENUM(SOFTWARE_SEL) |
+			MCDE_EXTSRC0CR_SEL_MOD(sel_mod) |
 		MCDE_EXTSRC0CR_MULTIOVL_CTRL_ENUM(PRIMARY) |
-		MCDE_EXTSRC0CR_FS_DIV_DISABLE(FALSE) |
-		MCDE_EXTSRC0CR_FORCE_FS_DIV(FALSE));
+		MCDE_EXTSRC0CR_FS_DIV_DISABLE(false) |
+		MCDE_EXTSRC0CR_FORCE_FS_DIV(false));
 	mcde_wreg(MCDE_OVL0CR + idx * MCDE_OVL0CR_GROUPOFFSET,
-		MCDE_OVL0CR_OVLEN(TRUE) |
-		MCDE_OVL0CR_COLCCTRL_ENUM(DISABLED) |
-		MCDE_OVL0CR_CKEYGEN(FALSE) |
-		MCDE_OVL0CR_ALPHAPMEN(TRUE) |
-		MCDE_OVL0CR_OVLF(FALSE) |
-		MCDE_OVL0CR_OVLR(FALSE) |
-		MCDE_OVL0CR_OVLB(FALSE) |
+		MCDE_OVL0CR_OVLEN(true) |
+		MCDE_OVL0CR_COLCCTRL(regs->col_conv) |
+		MCDE_OVL0CR_CKEYGEN(false) |
+		MCDE_OVL0CR_ALPHAPMEN(true) |
+		MCDE_OVL0CR_OVLF(false) |
+		MCDE_OVL0CR_OVLR(false) |
+		MCDE_OVL0CR_OVLB(false) |
 		MCDE_OVL0CR_FETCH_ROPC(0) |
 		MCDE_OVL0CR_STBPRIO(0) |
 		MCDE_OVL0CR_BURSTSIZE_ENUM(HW_8W) |
@@ -387,12 +838,12 @@ static void update_overlay_registers(u8 idx, struct ovly_regs *regs,
 		MCDE_OVL0CONF_LPF(lpf));
 	mcde_wreg(MCDE_OVL0CONF2 + idx * MCDE_OVL0CONF2_GROUPOFFSET,
 		MCDE_OVL0CONF2_BP_ENUM(PER_PIXEL_ALPHA) |
-		MCDE_OVL0CONF2_ALPHAVALUE(128) | /* TODO: Allow setting? */
+			MCDE_OVL0CONF2_ALPHAVALUE(0x80) |
 		MCDE_OVL0CONF2_OPQ(regs->opq) |
 		MCDE_OVL0CONF2_PIXOFF(lmrgn & 63) |
 		MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL(32));
 	mcde_wreg(MCDE_OVL0LJINC + idx * MCDE_OVL0LJINC_GROUPOFFSET,
-		regs->ljinc);
+			ljinc);
 	mcde_wreg(MCDE_OVL0CROP + idx * MCDE_OVL0CROP_GROUPOFFSET,
 		MCDE_OVL0CROP_TMRGN(tmrgn) |
 		MCDE_OVL0CROP_LMRGN(lmrgn >> 6));
@@ -401,52 +852,225 @@ static void update_overlay_registers(u8 idx, struct ovly_regs *regs,
 		MCDE_OVL0COMP_CH_ID(regs->ch_id) |
 		MCDE_OVL0COMP_YPOS(regs->ypos) |
 		MCDE_OVL0COMP_Z(regs->z));
-	dbg_printk("Overlay registers setup, idx=%d\n", idx);
+	}
+	dev_vdbg(&mcde_dev->dev, "Overlay registers setup, idx=%d\n", idx);
 }
 
-void update_channel_registers(enum mcde_chnl chnl_id, struct chnl_regs *regs,
-	struct mcde_port *port)
+static void update_overlay_address_registers(u8 idx, struct ovly_regs *regs)
 {
-	u8 idx = chnl_id;
-	if (!regs->floen) {
-		mcde_wfld(MCDE_CRC, C1EN, FALSE);
+	mcde_wreg(MCDE_EXTSRC0A0 + idx * MCDE_EXTSRC0A0_GROUPOFFSET,
+		regs->baseaddress0);
+	mcde_wreg(MCDE_EXTSRC0A1 + idx * MCDE_EXTSRC0A1_GROUPOFFSET,
+		regs->baseaddress1);
+}
+
+#define MCDE_FLOWEN_MAX_TRIAL	6
+
+static void disable_channel(struct mcde_chnl_state *chnl)
+{
+	int i;
+	const struct mcde_port *port = &chnl->port;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (port->type == MCDE_PORTTYPE_DSI)
+		dsi_wfld(port->link, DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS,
+			false);
+
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		mcde_wfld(MCDE_CRA0, FLOEN, false);
+		wait_for_channel(chnl);
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			msleep(1);
+			if (!mcde_rfld(MCDE_CRA0, FLOEN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (A) disable after >= %d ms\n", i);
+				goto break_switch;
+			}
+		}
+		dev_warn(&mcde_dev->dev, "%s: channel A timeout\n", __func__);
+		break;
+	case MCDE_CHNL_B:
+		mcde_wfld(MCDE_CRB0, FLOEN, false);
+		wait_for_channel(chnl);
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			msleep(1);
+			if (!mcde_rfld(MCDE_CRB0, FLOEN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (B) disable after >= %d ms\n", i);
+				goto break_switch;
+			}
+		}
+		dev_warn(&mcde_dev->dev, "%s: channel B timeout\n", __func__);
+		break;
+	case MCDE_CHNL_C0:
+		mcde_wfld(MCDE_CRC, C1EN, false);
 		if (!mcde_rfld(MCDE_CRC, C2EN))
-			mcde_wfld(MCDE_CRC, FLOEN, FALSE);
+			mcde_wfld(MCDE_CRC, FLOEN, false);
+		wait_for_channel(chnl);
+		break;
+	case MCDE_CHNL_C1:
+		mcde_wfld(MCDE_CRC, C2EN, false);
+		if (!mcde_rfld(MCDE_CRC, C1EN))
+			mcde_wfld(MCDE_CRC, FLOEN, false);
+		wait_for_channel(chnl);
+		break;
+	}
+break_switch:
+	chnl->continous_running = false;
+#undef MCDE_FLOWEN_MAX_TRIAL
+}
+
+static void enable_channel(struct mcde_chnl_state *chnl)
+{
+	const struct mcde_port *port = &chnl->port;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (port->type == MCDE_PORTTYPE_DSI)
+		dsi_wfld(port->link, DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS,
+				port->phy.dsi.clk_cont);
+
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		mcde_wfld(MCDE_CRA0, FLOEN, true);
+		break;
+	case MCDE_CHNL_B:
+		mcde_wfld(MCDE_CRB0, FLOEN, true);
+		break;
+	case MCDE_CHNL_C0:
+		mcde_wfld(MCDE_CRC, POWEREN, true);
+		mcde_wfld(MCDE_CRC, FLOEN, true);
+		mcde_wfld(MCDE_CRC, C1EN, true);
+		break;
+	case MCDE_CHNL_C1:
+		mcde_wfld(MCDE_CRC, POWEREN, true);
+		mcde_wfld(MCDE_CRC, FLOEN, true);
+		mcde_wfld(MCDE_CRC, C2EN, true);
+		break;
 	}
+}
+
+/* TODO get from register */
+#define MCDE_CLK_FREQ_MHZ 160
+
+void update_channel_registers(enum mcde_chnl chnl_id, struct chnl_regs *regs,
+				struct mcde_port *port, enum mcde_fifo fifo,
+				struct mcde_video_mode *video_mode)
+{
+	u8 idx = chnl_id;
+	u32 out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+	u32 src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	print_vmode(video_mode);
 
 	/* Channel */
+	if (port->update_auto_trig && port->type == MCDE_PORTTYPE_DSI) {
+		switch (port->sync_src) {
+		case MCDE_SYNCSRC_TE0:
+			out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_VSYNC0;
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT;
+			break;
+		case MCDE_SYNCSRC_OFF:
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+			break;
+		case MCDE_SYNCSRC_TE1:
+		default:
+			out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_VSYNC1;
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI) {
+		src_synch = port->update_auto_trig ?
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_OUTPUT :
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+	}
+
 	mcde_wreg(MCDE_CHNL0CONF + idx * MCDE_CHNL0CONF_GROUPOFFSET,
 		MCDE_CHNL0CONF_PPL(regs->ppl-1) |
 		MCDE_CHNL0CONF_LPF(regs->lpf-1));
 	mcde_wreg(MCDE_CHNL0STAT + idx * MCDE_CHNL0STAT_GROUPOFFSET,
-		MCDE_CHNL0STAT_CHNLBLBCKGND_EN(FALSE) |
-		MCDE_CHNL0STAT_CHNLRD(TRUE));
-	mcde_wreg(MCDE_CHNL0SYNCHMOD + idx * MCDE_CHNL0SYNCHMOD_GROUPOFFSET,
-		MCDE_CHNL0SYNCHMOD_SRC_SYNCH_ENUM(SOFTWARE) | /* TODO: */
-		MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_ENUM(FORMATTER));
+		MCDE_CHNL0STAT_CHNLBLBCKGND_EN(true) |
+		MCDE_CHNL0STAT_CHNLRD(true));
+	mcde_wreg(MCDE_CHNL0SYNCHMOD +
+		idx * MCDE_CHNL0SYNCHMOD_GROUPOFFSET,
+		MCDE_CHNL0SYNCHMOD_SRC_SYNCH(src_synch) |
+		MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC(out_synch_src));
 	mcde_wreg(MCDE_CHNL0BCKGNDCOL + idx * MCDE_CHNL0BCKGNDCOL_GROUPOFFSET,
-		MCDE_CHNL0BCKGNDCOL_B(255) | /* TODO: Temp */
-		MCDE_CHNL0BCKGNDCOL_G(255) |
-		MCDE_CHNL0BCKGNDCOL_R(255));
-
-	mcde_wfld(MCDE_CRC, POWEREN, TRUE);
-	mcde_wfld(MCDE_CRC, FLOEN, TRUE);
-	mcde_wfld(MCDE_CRC, C1EN, TRUE);
+		MCDE_CHNL0BCKGNDCOL_R(video_mode->bckcol[0]) |
+		MCDE_CHNL0BCKGNDCOL_G(video_mode->bckcol[1]) |
+		MCDE_CHNL0BCKGNDCOL_B(video_mode->bckcol[2]));
+
+	switch (chnl_id) {
+	case MCDE_CHNL_A:
+		mcde_wreg(MCDE_CRA1,
+			MCDE_CRA1_PCD(regs->pcd) |
+			MCDE_CRA1_CLKSEL(regs->clksel) |
+			MCDE_CRA1_CDWIN(regs->cdwin) |
+			MCDE_CRA1_OUTBPP(bpp2outbpp(regs->bpp)) |
+			MCDE_CRA1_BCD(regs->bcd) |
+			MCDE_CRA1_CLKTYPE(regs->internal_clk)
+		);
+		break;
+	case MCDE_CHNL_B:
+		mcde_wreg(MCDE_CRB1,
+			MCDE_CRB1_PCD(regs->pcd) |
+			MCDE_CRB1_CLKSEL(regs->clksel) |
+			MCDE_CRB1_CDWIN(regs->cdwin) |
+			MCDE_CRB1_OUTBPP(bpp2outbpp(regs->bpp)) |
+			MCDE_CRB1_BCD(regs->bcd) |
+			MCDE_CRB1_CLKTYPE(regs->internal_clk)
+		);
+		break;
+	default:
+		break;
+	}
 
 	/* Formatter */
-	{
+	if (port->type == MCDE_PORTTYPE_DSI) {
 		u8 fidx = 2 * port->link + port->ifc;
 		u32 temp, packet;
+		/*
+		 * pkt_div is used to avoid underflow in output fifo for
+		 * large packets
+		 */
+		u32 pkt_div = 1;
+		u32 dsi_delay0 = 0;
+		u32 screen_ppl, screen_lpf;
+
+		screen_ppl = video_mode->xres;
+		screen_lpf = video_mode->yres;
+
+		if (screen_ppl == SCREEN_PPL_HIGH) {
+			pkt_div = (screen_ppl - 1) /
+					get_output_fifo_size(fifo) + 1;
+		} else {
+			pkt_div = screen_ppl /
+					(get_output_fifo_size(fifo) * 2) + 1;
+		}
+
+		if (video_mode->interlaced)
+			screen_lpf /= 2;
+
+		/* pkt_delay_progressive = pixelclock * htot /
+		 * (1E12 / 160E6) / pkt_div */
+		dsi_delay0 = (video_mode->pixclock + 1) *
+			(video_mode->xres + video_mode->hbp +
+				video_mode->hfp) /
+			(1000000 / MCDE_CLK_FREQ_MHZ) / pkt_div;
 		temp = mcde_rreg(MCDE_DSIVID0CONF0 +
 			fidx * MCDE_DSIVID0CONF0_GROUPOFFSET);
 		mcde_wreg(MCDE_DSIVID0CONF0 +
 			fidx * MCDE_DSIVID0CONF0_GROUPOFFSET,
 			(temp & ~MCDE_DSIVID0CONF0_PACKING_MASK) |
 			MCDE_DSIVID0CONF0_PACKING(regs->dsipacking));
-		packet = ((regs->ppl * regs->bpp) >> 3) + 1; /* 1==CMD8 */
+		/* 1==CMD8 */
+		packet = ((screen_ppl / pkt_div * regs->bpp) >> 3) + 1;
 		mcde_wreg(MCDE_DSIVID0FRAME +
 			fidx * MCDE_DSIVID0FRAME_GROUPOFFSET,
-			MCDE_DSIVID0FRAME_FRAME(packet * regs->lpf));
+			MCDE_DSIVID0FRAME_FRAME(packet * pkt_div * screen_lpf));
 		mcde_wreg(MCDE_DSIVID0PKT + fidx * MCDE_DSIVID0PKT_GROUPOFFSET,
 			MCDE_DSIVID0PKT_PACKET(packet));
 		mcde_wreg(MCDE_DSIVID0SYNC +
@@ -459,13 +1083,78 @@ void update_channel_registers(enum mcde_chnl chnl_id, struct chnl_regs *regs,
 			MCDE_DSIVID0CMDW_CMDW_CONTINUE(DCS_CMD_WRITE_CONTINUE));
 		mcde_wreg(MCDE_DSIVID0DELAY0 +
 			fidx * MCDE_DSIVID0DELAY0_GROUPOFFSET,
-			MCDE_DSIVID0DELAY0_INTPKTDEL(0));
+			MCDE_DSIVID0DELAY0_INTPKTDEL(dsi_delay0));
 		mcde_wreg(MCDE_DSIVID0DELAY1 +
 			fidx * MCDE_DSIVID0DELAY1_GROUPOFFSET,
 			MCDE_DSIVID0DELAY1_TEREQDEL(0) |
 			MCDE_DSIVID0DELAY1_FRAMESTARTDEL(0));
 	}
-	dbg_printk("Channel registers setup, chnl=%d\n", chnl_id);
+	debug("Channel registers setup, chnl=%d\n", chnl_id);
+}
+
+void mcde_chnl_set_update_area(struct mcde_chnl_state *chnl,
+	u16 x, u16 y, u16 w, u16 h)
+{
+	if (!chnl->inuse) {
+		debug("%s: channel in use chnl=%p\n", __func__, chnl);
+		return;
+	}
+
+	chnl->update_x = x;
+	chnl->update_y = y;
+	chnl->update_w = w;
+	chnl->update_h = h;
+}
+
+/* DSI */
+
+int mcde_dsi_dcs_write(struct mcde_port *port, u8 cmd, u8* data, int len)
+{
+	int i;
+	u32 wrdat[4] = { 0, 0, 0, 0 };
+	u32 settings;
+	u8 link = port->link;
+	u8 virt_id = port->phy.dsi.virt_id;
+
+	debug("Entering %s\n", __func__);
+	if (len > MCDE_MAX_DCS_WRITE)
+		return -EINVAL;
+
+	wrdat[0] = cmd;
+	for (i = 1; i <= len; i++)
+		wrdat[i>>2] |= ((u32)data[i-1] << ((i & 3) * 8));
+
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(WRITE) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(len > 1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(len+1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true);
+	if (len == 0)
+		settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+			DCS_SHORT_WRITE_0);
+	else if (len == 1)
+		settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+			DCS_SHORT_WRITE_1);
+	else
+		settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+			DCS_LONG_WRITE);
+
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, wrdat[0]);
+	if (len >  3)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT1, wrdat[1]);
+	if (len >  7)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT2, wrdat[2]);
+	if (len > 11)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT3, wrdat[3]);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+	mdelay(10);
+
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+
+	return 0;
 }
 
 /* MCDE channels */
@@ -475,109 +1164,251 @@ struct mcde_chnl_state *mcde_chnl_get(enum mcde_chnl chnl_id,
 	int i;
 	struct mcde_chnl_state *chnl = NULL;
 
+	debug("%s: Entering chnl_id=%d\n", __func__, chnl_id);
 	/* Allocate channel */
 	for (i = 0; i < ARRAY_SIZE(channels); i++) {
 		if (chnl_id == channels[i].id)
 			chnl = &channels[i];
 	}
 	if (!chnl) {
-		printf("Invalid channel, chnl=%d\n", chnl_id);
+		dev_dbg(&mcde_dev->dev, "Invalid channel, chnl=%d\n", chnl_id);
 		return ERR_PTR(-EINVAL);
 	}
 	if (chnl->inuse) {
-		printf("Channel in use, chnl=%d\n", chnl_id);
+		dev_dbg(&mcde_dev->dev, "Channel in use, chnl=%d\n", chnl_id);
 		return ERR_PTR(-EBUSY);
 	}
 
 	chnl->port = *port;
 	chnl->fifo = fifo;
-
 	if (update_channel_static_registers(chnl) < 0)
 		return ERR_PTR(-EINVAL);
 
-	chnl->synchronized_update = FALSE;
+	chnl->synchronized_update = false;
+	chnl->pix_fmt = port->pixel_format;
 	chnl->update_x = 0;
 	chnl->update_y = 0;
 	chnl->update_w = 0;
 	chnl->update_h = 0;
 	mcde_chnl_apply(chnl);
-	chnl->inuse = TRUE;
+	chnl->inuse = true;
 
+	debug("%s: Leaving chnl=%p\n", __func__, (void *)chnl);
 	return chnl;
 }
 
-void mcde_chnl_set_update_area(struct mcde_chnl_state *chnl,
-	u16 x, u16 y, u16 w, u16 h)
+void mcde_chnl_set_pixel_format(struct mcde_chnl_state *chnl,
+	enum mcde_port_pix_fmt pix_fmt)
 {
-	if (!chnl->inuse)
+	if (!chnl->inuse) {
+		debug("%s: channel in use ovly=%p\n", __func__, chnl);
 		return;
+	}
 
-	chnl->update_x = x;
-	chnl->update_y = y;
-	chnl->update_w = w;
-	chnl->update_h = h;
+	chnl->pix_fmt = pix_fmt;
 }
 
-void mcde_chnl_set_pixel_format(struct mcde_chnl_state *chnl,
-	enum mcde_port_pix_fmt pix_fmt)
+int mcde_chnl_enable_synchronized_update(struct mcde_chnl_state *chnl,
+	u8 enable)
 {
 	if (!chnl->inuse)
-		return;
+		return -EINVAL;
+	chnl->synchronized_update = enable;
+	return 0;
+}
 
-	chnl->pix_fmt = pix_fmt;
+int mcde_chnl_set_power_mode(struct mcde_chnl_state *chnl,
+	enum mcde_display_power_mode power_mode)
+{
+	if (!chnl->inuse)
+		return -EINVAL;
+
+	chnl->power_mode = power_mode;
+	return 0;
 }
 
 void mcde_chnl_apply(struct mcde_chnl_state *chnl)
 {
-	t_bool enable;
-	if (!chnl->inuse)
+	u8 enable;
+	debug("Entering %s\n", __func__);
+	if (!chnl->inuse) {
+		debug("%s: channel not in use chnl=%p\n", __func__, chnl);
 		return;
+	}
 
-	enable = chnl->update_w > 0 && chnl->update_h > 0;
+	enable = (chnl->update_w > 0) && (chnl->update_h > 0);
 	chnl->regs.floen = enable;
 	chnl->regs.ppl = chnl->update_w;
 	chnl->regs.lpf = chnl->update_h;
 	chnl->regs.bpp = portfmt2bpp(chnl->pix_fmt);
-	chnl->regs.dsipacking = portfmt2dsipacking(chnl->pix_fmt);
+	chnl->regs.synchronized_update = chnl->synchronized_update;
+	if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+		chnl->regs.clksel = MCDE_CRA1_CLKSEL_166MHZ;
+		chnl->regs.dsipacking = portfmt2dsipacking(chnl->pix_fmt);
+	} else if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		if (chnl->port.phy.dpi.tv_mode) {
+			chnl->regs.internal_clk = false;
+			if (chnl->id == MCDE_CHNL_A)
+				chnl->regs.clksel = MCDE_CRA1_CLKSEL_EXT_TV1;
+			else
+				chnl->regs.clksel = MCDE_CRA1_CLKSEL_EXT_TV2;
+		} else {
+			chnl->regs.internal_clk = true;
+			chnl->regs.clksel = MCDE_CRA1_CLKSEL_LCD;
+			chnl->regs.cdwin = portfmt2cdwin(chnl->pix_fmt);
+			chnl->regs.bcd = (chnl->port.phy.dpi.clock_div < 2);
+			if (!chnl->regs.bcd)
+				chnl->regs.pcd =
+					chnl->port.phy.dpi.clock_div - 2;
+		}
+		dpi_video_mode_apply(chnl);
+	}
 	chnl->transactionid++;
-	dbg_printk("Channel applied, chnl=%d\n", chnl->id);
+
+	dev_vdbg(&mcde_dev->dev, "Channel applied, chnl=%d\n", chnl->id);
+	return;
 }
 
-void mcde_chnl_update(struct mcde_chnl_state *chnl)
+static void chnl_update_registers(struct mcde_chnl_state *chnl)
 {
-	struct mcde_ovly_state *ovly;
+	/* REVIEW: Move content to update_channel_register */
+	/* and remove this one */
+	if (chnl->port.type == MCDE_PORTTYPE_DPI)
+		update_dpi_registers(chnl->id, &chnl->tv_regs);
+	if (chnl->id == MCDE_CHNL_A || chnl->id == MCDE_CHNL_B)
+		update_col_registers(chnl->id, &chnl->col_regs);
+	update_channel_registers(chnl->id, &chnl->regs, &chnl->port,
+						chnl->fifo, &chnl->vmode);
+
+	chnl->transactionid_regs = chnl->transactionid;
+}
 
-	if (!chnl->inuse || !chnl->regs.floen)
-		return;
+static void chnl_update_continous(struct mcde_chnl_state *chnl)
+{
+	debug("%s: Entering\n", __func__);
+	if (!chnl->continous_running) {
+		if (chnl->transactionid_regs < chnl->transactionid)
+			chnl_update_registers(chnl);
+		if (chnl->port.sync_src == MCDE_SYNCSRC_TE0)
+			mcde_wfld(MCDE_CRC, SYCEN0, true);
+		else if (chnl->port.sync_src == MCDE_SYNCSRC_TE1)
+			mcde_wfld(MCDE_CRC, SYCEN1, true);
+		chnl->continous_running = true;
+		/*
+		* For main and secondary display,
+		* FLOWEN has to be set before a SOFTWARE TRIG
+		* Otherwise not overlay interrupt is triggerd
+		*/
+		enable_channel(chnl);
+		if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+				chnl->port.sync_src == MCDE_SYNCSRC_OFF) {
+			/*
+			 * not used in u-boot
+			mod_timer(&chnl->auto_sync_timer,
+					jiffies +
+			msecs_to_jiffies(MCDE_AUTO_SYNC_WATCHDOG * 1000));
+			*/
+		}
+	}
+}
 
+static void chnl_update_non_continous(struct mcde_chnl_state *chnl)
+{
+	debug("%s\n", __func__);
 	/* Commit settings to registers */
-	ovly = chnl->ovly0;
-	if (ovly->transactionid_regs < ovly->transactionid ||
-		chnl->transactionid_regs < chnl->transactionid) {
-		update_overlay_registers(ovly->idx, &ovly->regs,
-			chnl->regs.x, chnl->regs.y,
-			chnl->regs.ppl, chnl->regs.lpf);
-		ovly->transactionid_regs = ovly->transactionid;
+	wait_for_channel(chnl);
+	chnl_update_registers(chnl);
+	/*
+	* For main and secondary display,
+	* FLOWEN has to be set before a SOFTWARE TRIG
+	* Otherwise not overlay interrupt is triggerd
+	* However FLOWEN must not be triggered before SOFTWARE TRIG
+	* if rotation is enabled
+	*/
+	enable_channel(chnl);
+	/* TODO: look at port sync source and synched_update */
+	if (chnl->regs.synchronized_update &&
+				chnl->power_mode == MCDE_DISPLAY_PM_ON) {
+		if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+			chnl->port.sync_src == MCDE_SYNCSRC_BTA) {
+			while (dsi_rfld(chnl->port.link, DSI_CMD_MODE_STS,
+				CSM_RUNNING))
+				udelay(100);
+			/*dsi_te_request(chnl);	not for u-boot */
+		}
+	} else {
+		mcde_wreg(MCDE_CHNL0SYNCHSW +
+			chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+			MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+		dev_vdbg(&mcde_dev->dev, "Channel update (no sync), chnl=%d\n",
+			chnl->id);
 	}
-	ovly = chnl->ovly1;
-	if (ovly && (
-		ovly->transactionid_regs < ovly->transactionid ||
-		chnl->transactionid_regs < chnl->transactionid)) {
-		update_overlay_registers(ovly->idx, &ovly->regs,
-			chnl->regs.x, chnl->regs.y,
-			chnl->regs.ppl, chnl->regs.lpf);
+	if (chnl->power_mode == MCDE_DISPLAY_PM_ON)
+		enable_channel(chnl);
+}
+
+static void chnl_update_overlay(struct mcde_chnl_state *chnl,
+						struct mcde_ovly_state *ovly)
+{
+	debug("%s: chnl=%p ovly=%p\n", __func__, chnl, ovly);
+	if (!ovly)
+		return;
+	if (!ovly || (ovly->transactionid_regs >= ovly->transactionid &&
+			chnl->transactionid_regs >= chnl->transactionid))
+		return;
+
+	update_overlay_address_registers(ovly->idx, &ovly->regs);
+	if (ovly->regs.reset_buf_id) {
+		if (!chnl->continous_running)
+			wait_for_overlay(ovly);
+
+		update_overlay_registers(ovly->idx, &ovly->regs, &chnl->port,
+			chnl->fifo, chnl->regs.x, chnl->regs.y,
+			chnl->regs.ppl, chnl->regs.lpf, ovly->stride,
+			chnl->vmode.interlaced);
+		ovly->transactionid_regs = ovly->transactionid;
+	} else if (chnl->continous_running) {
 		ovly->transactionid_regs = ovly->transactionid;
+		wait_for_overlay(ovly);
 	}
-	if (chnl->transactionid_regs < chnl->transactionid) {
-		update_channel_registers(chnl->id, &chnl->regs, &chnl->port);
-		chnl->transactionid_regs = chnl->transactionid;
+}
+
+int mcde_chnl_update(struct mcde_chnl_state *chnl,
+					struct mcde_rectangle *update_area)
+{
+	debug("%s: chnl=%p rect=(%d, %d, %d, %d)\n", __func__, chnl,
+		update_area->x, update_area->y, update_area->w, update_area->h);
+
+	/* TODO: lock & make wait->trig async */
+	if (!chnl->inuse || !update_area
+			|| (update_area->w == 0 && update_area->h == 0)) {
+		return -EINVAL;
 	}
 
-	if (chnl->regs.floen)
-		mcde_wreg(MCDE_CHNL0SYNCHSW +
-			chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
-			MCDE_CHNL0SYNCHSW_SW_TRIG(TRUE));
-	dbg_printk("Channel updated, chnl=%d\n", chnl->id);
+	chnl->regs.x   = update_area->x;
+	chnl->regs.y   = update_area->y;
+	/* TODO Crop against video_mode.xres and video_mode.yres */
+	chnl->regs.ppl = update_area->w;
+	chnl->regs.lpf = update_area->h;
+	if (chnl->port.type == MCDE_PORTTYPE_DPI &&
+						chnl->port.phy.dpi.tv_mode) {
+		chnl->regs.ppl -= 2 * MCDE_CONFIG_TVOUT_HBORDER;
+		/* subtract double borders, ie. per field */
+		chnl->regs.lpf -= 4 * MCDE_CONFIG_TVOUT_VBORDER;
+	} else if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+			chnl->vmode.interlaced)
+		chnl->regs.lpf /= 2;
+
+	chnl_update_overlay(chnl, chnl->ovly0);
+	chnl_update_overlay(chnl, chnl->ovly1);
+
+	if (chnl->port.update_auto_trig)
+		chnl_update_continous(chnl);
+	else
+		chnl_update_non_continous(chnl);
+
+	dev_vdbg(&mcde_dev->dev, "Channel updated, chnl=%d\n", chnl->id);
+	return 0;
 }
 
 /* MCDE overlays */
@@ -585,18 +1416,23 @@ struct mcde_ovly_state *mcde_ovly_get(struct mcde_chnl_state *chnl)
 {
 	struct mcde_ovly_state *ovly;
 
-	if (!chnl->inuse)
+	if (!chnl->inuse) {
+		debug("%s: channel in use ovly=%p\n", __func__, chnl);
 		return ERR_PTR(-EINVAL);
+	}
 
 	if (!chnl->ovly0->inuse)
 		ovly = chnl->ovly0;
 	else if (chnl->ovly1 && !chnl->ovly1->inuse)
 		ovly = chnl->ovly1;
-	else
+	else {
+		debug("%s: overlay in use ovly=%p\n", __func__, ovly);
 		ovly = ERR_PTR(-EBUSY);
+	}
+
 
 	if (!IS_ERR(ovly)) {
-		ovly->inuse = TRUE;
+		ovly->inuse = true;
 		ovly->paddr = 0;
 		ovly->stride = 0;
 		ovly->pix_fmt = MCDE_OVLYPIXFMT_RGB565;
@@ -615,8 +1451,10 @@ struct mcde_ovly_state *mcde_ovly_get(struct mcde_chnl_state *chnl)
 
 void mcde_ovly_set_source_buf(struct mcde_ovly_state *ovly, u32 paddr)
 {
-	if (!ovly->inuse)
+	if (!ovly->inuse) {
+		debug("%s: overlay not in use ovly=%p\n", __func__, ovly);
 		return;
+	}
 
 	ovly->paddr = paddr;
 }
@@ -624,8 +1462,10 @@ void mcde_ovly_set_source_buf(struct mcde_ovly_state *ovly, u32 paddr)
 void mcde_ovly_set_source_info(struct mcde_ovly_state *ovly,
 	u32 stride, enum mcde_ovly_pix_fmt pix_fmt)
 {
-	if (!ovly->inuse)
+	if (!ovly->inuse) {
+		debug("%s: overlay not in use ovly=%p\n", __func__, ovly);
 		return;
+	}
 
 	ovly->stride = stride;
 	ovly->pix_fmt = pix_fmt;
@@ -634,8 +1474,10 @@ void mcde_ovly_set_source_info(struct mcde_ovly_state *ovly,
 void mcde_ovly_set_source_area(struct mcde_ovly_state *ovly,
 	u16 x, u16 y, u16 w, u16 h)
 {
-	if (!ovly->inuse)
+	if (!ovly->inuse) {
+		debug("%s: overlay not in use ovly=%p\n", __func__, ovly);
 		return;
+	}
 
 	ovly->src_x = x;
 	ovly->src_y = y;
@@ -645,116 +1487,76 @@ void mcde_ovly_set_source_area(struct mcde_ovly_state *ovly,
 
 void mcde_ovly_set_dest_pos(struct mcde_ovly_state *ovly, u16 x, u16 y, u8 z)
 {
-	if (!ovly->inuse)
+	if (!ovly->inuse) {
+		debug("%s: overlay not in use ovly=%p\n", __func__, ovly);
 		return;
+	}
 
 	ovly->dst_x = x;
 	ovly->dst_y = y;
 	ovly->dst_z = z;
 }
 
-void mcde_ovly_apply(struct mcde_ovly_state *ovly)
+void mcde_ovly_set_pixfmt(struct mcde_ovly_state *ovly,
+				enum mcde_ovly_pix_fmt fmt)
 {
-	if (!ovly->inuse)
+	if (!ovly->inuse) {
+		debug("%s: overlay in use ovly=%p\n", __func__, ovly);
 		return;
+	}
+
+	ovly->pix_fmt = fmt;
+}
 
+void mcde_ovly_apply(struct mcde_ovly_state *ovly)
+{
+	if (!ovly->inuse) {
+		debug("%s: overlay not in use ovly=%p\n", __func__, ovly);
+		return;
+	}
 	ovly->regs.ch_id = ovly->chnl->id;
 	ovly->regs.enabled = ovly->paddr != 0;
-	if (ovly->regs.buf_id)
-		ovly->regs.baseaddress0 = ovly->paddr;
-	else
-		ovly->regs.baseaddress1 = ovly->paddr;
-	ovly->regs.buf_id = !ovly->regs.buf_id;
+	ovly->regs.baseaddress0 = ovly->paddr;
+	ovly->regs.baseaddress1 = ovly->paddr + ovly->stride;
+	/* TODO set to true if interlaced */
+	ovly->regs.reset_buf_id = !ovly->chnl->continous_running;
 	switch (ovly->pix_fmt) {
 	case MCDE_OVLYPIXFMT_RGB565:
 		ovly->regs.bits_per_pixel = 16;
 		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_RGB565;
-		ovly->regs.bgr = FALSE;
-		ovly->regs.bebo = FALSE;
-		ovly->regs.opq = TRUE;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
 		break;
 	case MCDE_OVLYPIXFMT_RGB888:
 		ovly->regs.bits_per_pixel = 24;
 		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_RGB888;
-		ovly->regs.bgr = FALSE;
-		ovly->regs.bebo = FALSE;
-		ovly->regs.opq = TRUE;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
 		break;
 	default:
 		break;
 	}
 	ovly->regs.ppl = ovly->w;
 	ovly->regs.lpf = ovly->h;
-	ovly->regs.ljinc = ovly->stride;
 	ovly->regs.cropx = ovly->src_x;
 	ovly->regs.cropy = ovly->src_y;
 	ovly->regs.xpos = ovly->dst_x;
 	ovly->regs.ypos = ovly->dst_y;
-	ovly->regs.z = ovly->dst_z > 0; /* 0 or 1 */
+	ovly->regs.z = ovly->dst_z > 0;
 
 	ovly->transactionid = ++ovly->chnl->transactionid;
-	dbg_printk("Overlay applied, chnl=%d\n", ovly->chnl->id);
+	dev_vdbg(&mcde_dev->dev, "Overlay applied, chnl=%d\n", ovly->chnl->id);
 }
 
-/* DSI */
-int mcde_dsi_dcs_write(struct mcde_port *port, u8 cmd, u8* data, int len)
-{
-	int i;
-	u32 wrdat[4] = { 0, 0, 0, 0 };
-	u32 settings;
-	u8 link = port->link;
-	u8 virt_id = port->phy.dsi.virt_id;
-
-	if (len > MCDE_MAX_DCS_WRITE)
-		return -EINVAL;
-
-	wrdat[0] = cmd;
-	for (i = 1; i <= len; i++)
-		wrdat[i>>2] |= (data[i-1] << (i & 3));
-
-	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(WRITE) |
-		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(len > 1) |
-		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
-		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(len+1) |
-		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(TRUE);
-	if (len == 0)
-		settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
-			DCS_SHORT_WRITE_0);
-	else if (len == 1)
-		settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
-			DCS_SHORT_WRITE_1);
-	else
-		settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
-			DCS_LONG_WRITE);
-
-	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
-	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, wrdat[0]);
-	if (len >  3)
-		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT1, wrdat[1]);
-	if (len >  7)
-		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT2, wrdat[2]);
-	if (len > 11)
-		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT3, wrdat[3]);
-	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
-	dsi_wreg(link, DSI_DIRECT_CMD_SEND, TRUE);
-	mdelay(10);
-
-	dsi_wreg(link, DSI_CMD_MODE_STS_CLR, ~0);
-	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
-	return 0;
-}
-
-/*
-* Used by MCDE to setup all necessary PRCMU registers
-*/
-
 /* Level shifter and clamp control registers */
 #define PRCM_MMIP_LS_CLAMP_SET     (U8500_PRCMU_BASE + 0x420)
 #define PRCM_MMIP_LS_CLAMP_CLR     (U8500_PRCMU_BASE + 0x424)
 
-/* PRCMU clock/PLL/reset registers */
 #define PRCM_PLLDSI_FREQ           (U8500_PRCMU_BASE + 0x500)
 #define PRCM_PLLDSI_ENABLE         (U8500_PRCMU_BASE + 0x504)
+#define PRCM_YYCLKEN0_MGT_SET      (U8500_PRCMU_BASE + 0x510)
 #define PRCM_LCDCLK_MGT            (U8500_PRCMU_BASE + 0x044)
 #define PRCM_MCDECLK_MGT           (U8500_PRCMU_BASE + 0x064)
 #define PRCM_HDMICLK_MGT           (U8500_PRCMU_BASE + 0x058)
@@ -774,6 +1576,10 @@ int mcde_dsi_dcs_write(struct mcde_port *port, u8 cmd, u8* data, int len)
 /* Miscellaneous unit registers */
 #define PRCM_DSI_SW_RESET          (U8500_PRCMU_BASE + 0x324)
 
+/*
+ * Used by MCDE to setup all necessary PRCMU registers
+ */
+
 #define PRCMU_CLAMP_DSS_DSIPLL		0x00600C00
 #define PRCMU_RESET_DSS			0x0000000C
 #define PRCMU_ENABLE_DSS_MEM		0x00200000
@@ -782,22 +1588,38 @@ int mcde_dsi_dcs_write(struct mcde_port *port, u8 cmd, u8* data, int len)
 #define PRCMU_UNCLAMP_DSS_DSIPLL	0x00600C00
 #define PRCMU_POWER_ON_DSI		0x00008000
 
+/* PRCMU clock/PLL/reset registers */
+#define PRCMU_CLK_PLL_DIV_SHIFT		0
+#define PRCMU_CLK_PLL_SW_SHIFT		5
+#define PRCMU_CLK_EN			(1 << 8)
+
+#define PRCMU_DSI_CLOCK_SETTING		0x00000148
+#define PRCMU_LCDCLKEN			0x20000		/* bit 17 */
+
 #define PRCMU_MCDE_CLOCK_SETTING	0x00000125
+/*
+ * from linux prcmu-db8500.c:
+ * Set DPI clock to 50000000 Hz
+ */
+#define PRCMU_DPI_CLOCK_SETTING		(PRCMU_CLK_EN | \
+					(2 << PRCMU_CLK_PLL_SW_SHIFT) | \
+					(8 << PRCMU_CLK_PLL_DIV_SHIFT))
+
+#define PRCMU_DSI_LP_CLOCK_SETTING	0x00000F00
+#define PRCMU_PLLDSI_FREQ_SETTING	0x00020123
 #define PRCMU_ENABLE_PLLDSI		0x00000001
 #define PRCMU_RELEASE_RESET_DSS		0x0000400C
 #define PRCMU_DSI_PLLOUT_SEL_SETTING	0x00000202
+#define PRCMU_ENABLE_ESCAPE_CLOCK	0x07030101
 #define PRCMU_DSI_RESET_SW		0x00000007
 
-#define PRCMU_DSI_CLOCK_SETTING		0x00000148
-#define PRCMU_PLLDSI_FREQ_SETTING	0x00020123
-#define PRCMU_DSI_LP_CLOCK_SETTING	0x00000F00
-#define PRCMU_ENABLE_ESCAPE_CLOCK	0x07030101
+#define PRCMU_MCDE_DELAY		2
 
-#define PRCMU_MCDE_DELAY			2
 
 void mcde_enable_dss(void)
 {
 	u32 temp;
+	debug("Entering %s\n", __func__);
 	/* Clamp DSS out, DSIPLL in/out, (why not DSS input?) */
 	writel(PRCMU_CLAMP_DSS_DSIPLL, PRCM_MMIP_LS_CLAMP_SET);
 	mdelay(PRCMU_MCDE_DELAY);
@@ -829,6 +1651,8 @@ void mcde_enable_dss(void)
 	/* PLLDIV=14, PLLSW=2, CLKEN=1 */
 	writel(PRCMU_DSI_LP_CLOCK_SETTING, PRCM_TVCLK_MGT);
 	mdelay(PRCMU_MCDE_DELAY);
+	writel(PRCMU_DPI_CLOCK_SETTING, PRCM_LCDCLK_MGT);
+	mdelay(PRCMU_MCDE_DELAY);
 
 	/* D=43, N=1, R=4, SELDIV2=0 */
 	writel(PRCMU_PLLDSI_FREQ_SETTING, PRCM_PLLDSI_FREQ);
@@ -850,28 +1674,142 @@ void mcde_enable_dss(void)
 	mdelay(PRCMU_MCDE_DELAY);
 }
 
-int mcde_init(u8 num_data_lanes)
+void update_mcde_registers(struct mcde_platform_data *pdata)
+{
+	/* Setup output muxing */
+	mcde_wreg(MCDE_CONF0,
+		MCDE_CONF0_IFIFOCTRLWTRMRKLVL(7) |
+		MCDE_CONF0_OUTMUX0(pdata->outmux[0]) |
+		MCDE_CONF0_OUTMUX1(pdata->outmux[1]) |
+		MCDE_CONF0_OUTMUX2(pdata->outmux[2]) |
+		MCDE_CONF0_OUTMUX3(pdata->outmux[3]) |
+		MCDE_CONF0_OUTMUX4(pdata->outmux[4]) |
+		pdata->syncmux);
+
+	/* Enable channel VCMP interrupts */
+	mcde_wreg(MCDE_IMSCPP,
+		MCDE_IMSCPP_VCMPAIM(true) |
+		MCDE_IMSCPP_VCMPBIM(true) |
+		MCDE_IMSCPP_VCMPC0IM(true) |
+		MCDE_IMSCPP_VCMPC1IM(true));
+
+	/* Enable overlay fetch done interrupts */
+	mcde_wfld(MCDE_IMSCOVL, OVLFDIM, 0x3f);
+
+	/* Setup sync pulse length */
+	mcde_wreg(MCDE_VSCRC0,
+		MCDE_VSCRC0_VSPMIN(1) |
+		MCDE_VSCRC0_VSPMAX(0xff));
+	mcde_wreg(MCDE_VSCRC1,
+		MCDE_VSCRC1_VSPMIN(1) |
+		MCDE_VSCRC1_VSPMAX(0xff));
+}
+
+int mcde_probe(u8 num_dsilinks, struct mcde_platform_data *pdata)
 {
+	int ret = 0;
 	int i;
-	for (i = 0; i < ARRAY_SIZE(channels); i++) {
-		channels[i].ovly0->chnl = &channels[i];
-		if (channels[i].ovly1)
-			channels[i].ovly1->chnl = &channels[i];
-	}
+	u8 major_version;
+	u8 minor_version;
+	u8 development_version;
+
+	printf("MCDE subsystem init begin\n");
 
-	dsiio = malloc(num_data_lanes * sizeof(*dsiio));
-	if (!dsiio) {
-		printf("%s: Failed to malloc dsiio\n", __func__);
+	if (!pdata) {
+		dev_dbg(&pdev->dev, "No platform data\n");
 		return -EINVAL;
 	}
+	if (num_dsilinks > 0) {
+		dsiio = malloc(num_dsilinks * sizeof(*dsiio));
+		if (!dsiio) {
+			ret = -ENOMEM;
+			printf("%s: Failed to malloc dsiio\n", __func__);
+			goto failed_dsi_alloc;
+		}
+
+		mcdeio = (u8 *)CFG_MCDE_BASE;
+		debug("MCDE iomap: 0x%.8X\n", (u32)mcdeio);
+		for (i = 0; i < num_dsilinks; i++) {
+			dsiio[i] = (u8 *)(CFG_DSI_BASE + i*0x1000);
+			debug("MCDE DSI%d iomap: 0x%.8X\n", i, (u32)dsiio[i]);
+		}
+	}
 
-	mcdeio = (u8 *)CFG_MCDE_BASE;
-	dbg_printk("MCDE iomap: 0x%.8X\n", (u32)mcdeio);
-	for (i = 0; i < num_data_lanes; i++) {
-		dsiio[i] = (u8 *)(CFG_DSI_BASE + i*0x1000);
-		dbg_printk("MCDE DSI%d iomap: 0x%.8X\n", i, (u32)dsiio[i]);
+	mcde_enable_dss();
+
+	update_mcde_registers(pdata);
+
+	major_version = MCDE_REG2VAL(MCDE_PID, MAJOR_VERSION,
+							mcde_rreg(MCDE_PID));
+	minor_version = MCDE_REG2VAL(MCDE_PID, MINOR_VERSION,
+							mcde_rreg(MCDE_PID));
+	development_version = MCDE_REG2VAL(MCDE_PID, DEVELOPMENT_VERSION,
+							mcde_rreg(MCDE_PID));
+
+	dev_info(&mcde_dev->dev, "MCDE HW revision %u.%u.%u.%u\n",
+			major_version, minor_version, development_version,
+					mcde_rfld(MCDE_PID, METALFIX_VERSION));
+
+	if (major_version == 3 && minor_version == 0 &&
+					development_version >= 8) {
+		hardware_version = MCDE_CHIP_VERSION_3_0_8;
+		dev_info(&mcde_dev->dev, "V2 HW\n");
+	} else if (major_version == 3 && minor_version == 0 &&
+					development_version >= 5) {
+		hardware_version = MCDE_CHIP_VERSION_3_0_5;
+		dev_info(&mcde_dev->dev, "V1 HW\n");
+	} else {
+		debug("Unsupported HW version\n");
+		ret = ENODEV;
+		goto failed_hardware_version;
 	}
+
+	printf("MCDE subsystem init done\n");
 	return 0;
+
+failed_hardware_version:
+	free(dsiio);
+	dsiio = NULL;
+
+failed_dsi_alloc:
+	return ret;
+}
+
+void mcde_init(void)
+{
+	int i;
+	int j;
+
+	debug("%s\n", __func__);
+	/*
+	 * clear and init static data
+	 * link channel->overlay
+	 */
+	for (i = 0, j = 0; i < ARRAY_SIZE(channels); i++) {
+		memset(&channels[i], 0, sizeof(struct mcde_chnl_state));
+		if (j < 6)
+			channels[i].ovly0 = &overlays[j++];
+		if (j < 6)
+			channels[i].ovly1 = &overlays[j++];
+	}
+	channels[0].id = MCDE_CHNL_A;
+	channels[1].id = MCDE_CHNL_B;
+	channels[2].id = MCDE_CHNL_C0;
+	channels[3].id = MCDE_CHNL_C1;
+
+	for (i = 0; i < ARRAY_SIZE(overlays); i++) {
+		memset(&overlays[i], 0, sizeof(struct mcde_ovly_state));
+		overlays[i].idx = i;
+	}
+
+	/* link overlay->channel */
+	for (i = 0; i < ARRAY_SIZE(channels); i++) {
+		channels[i].ovly0->chnl = &channels[i];
+		if (channels[i].ovly1)
+			channels[i].ovly1->chnl = &channels[i];
+	}
+
+	return;
 }
 
 void mcde_exit(void)
@@ -881,3 +1819,4 @@ void mcde_exit(void)
 		dsiio = NULL;
 	}
 }
+
diff --git a/board/st/u8500/mcde_regs.h b/board/st/u8500/mcde_regs.h
index d4682b860..9e25f2dfe 100644
--- a/board/st/u8500/mcde_regs.h
+++ b/board/st/u8500/mcde_regs.h
@@ -3055,9 +3055,13 @@
 #define MCDE_CRB1_CDWIN_12BBP_C2 2
 #define MCDE_CRB1_CDWIN_16BBP_C1 3
 #define MCDE_CRB1_CDWIN_16BBP_C2 4
-#define MCDE_CRB1_CDWIN_18BBP_C1 5
-#define MCDE_CRB1_CDWIN_18BBP_C2 6
-#define MCDE_CRB1_CDWIN_24BBP 7
+#define MCDE_CRB1_CDWIN_V1_18BBP_C1 5
+#define MCDE_CRB1_CDWIN_V1_18BBP_C2 6
+#define MCDE_CRB1_CDWIN_V1_24BBP 7
+#define MCDE_CRB1_CDWIN_V2_16BBP_C3 5
+#define MCDE_CRB1_CDWIN_V2_18BBP_C1 6
+#define MCDE_CRB1_CDWIN_V2_18BBP_C2 7
+#define MCDE_CRB1_CDWIN_V2_24BBP 8
 #define MCDE_CRB1_CDWIN_ENUM(__x) \
 	MCDE_VAL2REG(MCDE_CRB1, CDWIN, MCDE_CRB1_CDWIN_##__x)
 #define MCDE_CRB1_CDWIN(__x) \
@@ -3507,23 +3511,23 @@
 	MCDE_VAL2REG(MCDE_TVTIM1B, DHO, __x)
 #define MCDE_TVLBALWA 0x00000850
 #define MCDE_TVLBALWA_GROUPOFFSET 0x200
-#define MCDE_TVLBALWA_ALW_SHIFT 0
-#define MCDE_TVLBALWA_ALW_MASK 0x000007FF
-#define MCDE_TVLBALWA_ALW(__x) \
-	MCDE_VAL2REG(MCDE_TVLBALWA, ALW, __x)
 #define MCDE_TVLBALWA_LBW_SHIFT 16
 #define MCDE_TVLBALWA_LBW_MASK 0x07FF0000
 #define MCDE_TVLBALWA_LBW(__x) \
 	MCDE_VAL2REG(MCDE_TVLBALWA, LBW, __x)
+#define MCDE_TVLBALWA_ALW_SHIFT 0
+#define MCDE_TVLBALWA_ALW_MASK 0x000007FF
+#define MCDE_TVLBALWA_ALW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWA, ALW, __x)
 #define MCDE_TVLBALWB 0x00000A50
-#define MCDE_TVLBALWB_ALW_SHIFT 0
-#define MCDE_TVLBALWB_ALW_MASK 0x000007FF
-#define MCDE_TVLBALWB_ALW(__x) \
-	MCDE_VAL2REG(MCDE_TVLBALWB, ALW, __x)
 #define MCDE_TVLBALWB_LBW_SHIFT 16
 #define MCDE_TVLBALWB_LBW_MASK 0x07FF0000
 #define MCDE_TVLBALWB_LBW(__x) \
 	MCDE_VAL2REG(MCDE_TVLBALWB, LBW, __x)
+#define MCDE_TVLBALWB_ALW_SHIFT 0
+#define MCDE_TVLBALWB_ALW_MASK 0x000007FF
+#define MCDE_TVLBALWB_ALW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWB, ALW, __x)
 #define MCDE_TVBL2A 0x00000854
 #define MCDE_TVBL2A_GROUPOFFSET 0x200
 #define MCDE_TVBL2A_BEL2_SHIFT 0
@@ -3962,9 +3966,9 @@
 #define MCDE_CTRLA_FORMID_DSI0VID 0
 #define MCDE_CTRLA_FORMID_DSI0CMD 1
 #define MCDE_CTRLA_FORMID_DSI1VID 2
-#define MCDE_CTRLA_FORMID_DSI1CMD 0
-#define MCDE_CTRLA_FORMID_DSI2VID 1
-#define MCDE_CTRLA_FORMID_DSI2CMD 2
+#define MCDE_CTRLA_FORMID_DSI1CMD 3
+#define MCDE_CTRLA_FORMID_DSI2VID 4
+#define MCDE_CTRLA_FORMID_DSI2CMD 5
 #define MCDE_CTRLA_FORMID_DPIA 0
 #define MCDE_CTRLA_FORMID_DPIB 1
 #define MCDE_CTRLA_FORMID_ENUM(__x) \
@@ -3998,9 +4002,9 @@
 #define MCDE_CTRLB_FORMID_DSI0VID 0
 #define MCDE_CTRLB_FORMID_DSI0CMD 1
 #define MCDE_CTRLB_FORMID_DSI1VID 2
-#define MCDE_CTRLB_FORMID_DSI1CMD 0
-#define MCDE_CTRLB_FORMID_DSI2VID 1
-#define MCDE_CTRLB_FORMID_DSI2CMD 2
+#define MCDE_CTRLB_FORMID_DSI1CMD 3
+#define MCDE_CTRLB_FORMID_DSI2VID 4
+#define MCDE_CTRLB_FORMID_DSI2CMD 5
 #define MCDE_CTRLB_FORMID_DPIA 0
 #define MCDE_CTRLB_FORMID_DPIB 1
 #define MCDE_CTRLB_FORMID_ENUM(__x) \
diff --git a/include/configs/u8500.h b/include/configs/u8500.h
index fb3614fd3..db48a51de 100644
--- a/include/configs/u8500.h
+++ b/include/configs/u8500.h
@@ -284,9 +284,15 @@
  * Video Logo Related configs
  */
 #define CONFIG_VIDEO_LOGO	/* Enable startup logo */
+#define CONFIG_SYS_DISPLAY_DSI		1
+#define CONFIG_SYS_DISPLAY_DPI		0
 #define CONFIG_SYS_VIDEO_FB_ADRS	0x14000000
-#define CONFIG_SYS_DISPLAY_NATIVE_X_RES	864
-#define CONFIG_SYS_DISPLAY_NATIVE_Y_RES	480
+#if CONFIG_SYS_DISPLAY_DPI
+#define CONFIG_SYS_DISPLAY_NATIVE_X_RES	640	/* VGA */
+#else
+#define CONFIG_SYS_DISPLAY_NATIVE_X_RES	864	/* HREF */
+#endif
+#define CONFIG_SYS_DISPLAY_NATIVE_Y_RES	480	/* VGA + HREF */
 /* 2.5V */
 #define CONFIG_SYS_DISPLAY_VOLTAGE	2500000
 #define MCDE_TOC_SPLASH_NAME		"SPLASH"