Merge commit 'v3.1-rc2' into HEAD

201 files changed, 19001 insertions, 5574 deletions

diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 4923b5ec0201..549b960667c8 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -8,9 +8,6 @@ menu "Graphics support"
 config HAVE_FB_ATMEL
 	bool
 
-config HAVE_FB_IMX
-	bool
-
 config SH_MIPI_DSI
 	tristate
 	depends on (SUPERH || ARCH_SHMOBILE) && HAVE_CLK
@@ -322,69 +319,6 @@ config FB_ARMCLCD
 	  here and read <file:Documentation/kbuild/modules.txt>.  The module
 	  will be called amba-clcd.
 
-choice
-
-	depends on FB_ARMCLCD && (ARCH_LH7A40X || ARCH_LH7952X)
-	prompt "LCD Panel"
-	default FB_ARMCLCD_SHARP_LQ035Q7DB02
-
-config FB_ARMCLCD_SHARP_LQ035Q7DB02_HRTFT
-	bool "LogicPD LCD 3.5\" QVGA w/HRTFT IC"
-	help
-	  This is an implementation of the Sharp LQ035Q7DB02, a 3.5"
-	  color QVGA, HRTFT panel.  The LogicPD device includes
-	  an integrated HRTFT controller IC.
-	  The native resolution is 240x320.
-
-config FB_ARMCLCD_SHARP_LQ057Q3DC02
-	bool "LogicPD LCD 5.7\" QVGA"
-	help
-	  This is an implementation of the Sharp LQ057Q3DC02, a 5.7"
-	  color QVGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 320x240.
-
-config FB_ARMCLCD_SHARP_LQ64D343
-	bool "LogicPD LCD 6.4\" VGA"
-	help
-	  This is an implementation of the Sharp LQ64D343, a 6.4"
-	  color VGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 640x480.
-
-config FB_ARMCLCD_SHARP_LQ10D368
-	bool "LogicPD LCD 10.4\" VGA"
-	help
-	  This is an implementation of the Sharp LQ10D368, a 10.4"
-	  color VGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 640x480.
-
-
-config FB_ARMCLCD_SHARP_LQ121S1DG41
-	bool "LogicPD LCD 12.1\" SVGA"
-	help
-	  This is an implementation of the Sharp LQ121S1DG41, a 12.1"
-	  color SVGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 800x600.
-
-	  This panel requires a clock rate may be an integer fraction
-	  of the base LCDCLK frequency.  The driver will select the
-	  highest frequency available that is lower than the maximum
-	  allowed.  The panel may flicker if the clock rate is
-	  slower than the recommended minimum.
-
-config FB_ARMCLCD_AUO_A070VW01_WIDE
-	bool "AU Optronics A070VW01 LCD 7.0\" WIDE"
-	help
-	  This is an implementation of the AU Optronics, a 7.0"
-	  WIDE Color.  The native resolution is 234x480.
-
-config FB_ARMCLCD_HITACHI
-	bool "Hitachi Wide Screen 800x480"
-	help
-	  This is an implementation of the Hitachi 800x480.
-
-endchoice
-
-
 config FB_ACORN
 	bool "Acorn VIDC support"
 	depends on (FB = y) && ARM && ARCH_ACORN
@@ -422,7 +356,7 @@ config FB_SA1100
 
 config FB_IMX
 	tristate "Freescale i.MX LCD support"
-	depends on FB && (HAVE_FB_IMX || ARCH_MX1 || ARCH_MX2)
+	depends on FB && IMX_HAVE_PLATFORM_IMX_FB
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
@@ -439,6 +373,24 @@ config FB_CYBER2000
 	  Say Y if you have a NetWinder or a graphics card containing this
 	  device, otherwise say N.
 
+config FB_CYBER2000_DDC
+	bool "DDC for CyberPro support"
+	depends on FB_CYBER2000
+	select FB_DDC
+	default y
+	help
+	  Say Y here if you want DDC support for your CyberPro graphics
+	  card. This is only I2C bus support, driver does not use EDID.
+
+config FB_CYBER2000_I2C
+	bool "CyberPro 2000/2010/5000 I2C support"
+	depends on FB_CYBER2000 && I2C && ARCH_NETWINDER
+	select I2C_ALGOBIT
+	help
+	  Enable support for the I2C video decoder interface on the
+	  Integraphics CyberPro 20x0 and 5000 VGA chips.  This is used
+	  on the Netwinder machines for the SAA7111 video capture.
+
 config FB_APOLLO
 	bool
 	depends on (FB = y) && APOLLO
@@ -1508,6 +1460,14 @@ config FB_S3
 	---help---
 	  Driver for graphics boards with S3 Trio / S3 Virge chip.
 
+config FB_S3_DDC
+	bool "DDC for S3 support"
+	depends on FB_S3
+	select FB_DDC
+	default y
+	help
+	  Say Y here if you want DDC support for your S3 graphics card.
+
 config FB_SAVAGE
 	tristate "S3 Savage support"
 	depends on FB && PCI && EXPERIMENTAL
@@ -2016,6 +1976,7 @@ config FB_SH_MOBILE_LCDC
 	select FB_SYS_IMAGEBLIT
 	select FB_SYS_FOPS
 	select FB_DEFERRED_IO
+	select FB_BACKLIGHT
 	select SH_MIPI_DSI if SH_LCD_MIPI_DSI
 	---help---
 	  Frame buffer driver for the on-chip SH-Mobile LCD controller.
@@ -2030,6 +1991,18 @@ config FB_SH_MOBILE_HDMI
 	---help---
 	  Driver for the on-chip SH-Mobile HDMI controller.
 
+config FB_SH_MOBILE_MERAM
+	tristate "SuperH Mobile MERAM read ahead support for LCDC"
+	depends on FB_SH_MOBILE_LCDC
+	default y
+	---help---
+	  Enable MERAM support for the SH-Mobile LCD controller.
+
+	  This will allow for caching of the framebuffer to provide more
+	  reliable access under heavy main memory bus traffic situations.
+	  Up to 4 memory channels can be configured, allowing 4 RGB or
+	  2 YCbCr framebuffers to be configured.
+
 config FB_TMIO
 	tristate "Toshiba Mobile IO FrameBuffer support"
 	depends on FB && MFD_CORE
@@ -2293,29 +2266,43 @@ config FB_METRONOME
 config FB_MB862XX
 	tristate "Fujitsu MB862xx GDC support"
 	depends on FB
+	depends on PCI || (OF && PPC)
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
 	---help---
 	  Frame buffer driver for Fujitsu Carmine/Coral-P(A)/Lime controllers.
 
+choice
+	prompt "GDC variant"
+	depends on FB_MB862XX
+
 config FB_MB862XX_PCI_GDC
 	bool "Carmine/Coral-P(A) GDC"
-	depends on PCI && FB_MB862XX
+	depends on PCI
 	---help---
 	  This enables framebuffer support for Fujitsu Carmine/Coral-P(A)
 	  PCI graphics controller devices.
 
 config FB_MB862XX_LIME
 	bool "Lime GDC"
-	depends on FB_MB862XX
-	depends on OF && !FB_MB862XX_PCI_GDC
-	depends on PPC
+	depends on OF && PPC
 	select FB_FOREIGN_ENDIAN
 	select FB_LITTLE_ENDIAN
 	---help---
 	  Framebuffer support for Fujitsu Lime GDC on host CPU bus.
 
+endchoice
+
+config FB_MB862XX_I2C
+	bool "Support I2C bus on MB862XX GDC"
+	depends on FB_MB862XX && I2C
+	default y
+	help
+	  Selecting this option adds Coral-P(A)/Lime GDC I2C bus adapter
+	  driver to support accessing I2C devices on controller's I2C bus.
+	  These are usually some video decoder chips.
+
 config FB_EP93XX
 	tristate "EP93XX frame buffer support"
 	depends on FB && ARCH_EP93XX
@@ -2376,6 +2363,26 @@ config FB_JZ4740
 	help
 	  Framebuffer support for the JZ4740 SoC.
 
+config FB_MXS
+	tristate "MXS LCD framebuffer support"
+	depends on FB && ARCH_MXS
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  Framebuffer support for the MXS SoC.
+
+config FB_PUV3_UNIGFX
+	tristate "PKUnity v3 Unigfx framebuffer support"
+	depends on FB && UNICORE32 && ARCH_PUV3
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	help
+	  Choose this option if you want to use the Unigfx device as a
+	  framebuffer device. Without the support of PCI & AGP.
+
 source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
 
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 8c8fabdff9d0..8b83129e209c 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -130,6 +130,7 @@ obj-$(CONFIG_FB_UDL)		  += udlfb.o
 obj-$(CONFIG_FB_XILINX)           += xilinxfb.o
 obj-$(CONFIG_SH_MIPI_DSI)	  += sh_mipi_dsi.o
 obj-$(CONFIG_FB_SH_MOBILE_HDMI)	  += sh_mobile_hdmi.o
+obj-$(CONFIG_FB_SH_MOBILE_MERAM)  += sh_mobile_meram.o
 obj-$(CONFIG_FB_SH_MOBILE_LCDC)	  += sh_mobile_lcdcfb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
 obj-y                             += omap2/
@@ -139,6 +140,7 @@ obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
 obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
 obj-$(CONFIG_FB_JZ4740)		  += jz4740_fb.o
+obj-$(CONFIG_FB_PUV3_UNIGFX)      += fb-puv3.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
@@ -153,6 +155,7 @@ obj-$(CONFIG_FB_BFIN_T350MCQB)	  += bfin-t350mcqb-fb.o
 obj-$(CONFIG_FB_BFIN_7393)        += bfin_adv7393fb.o
 obj-$(CONFIG_FB_MX3)		  += mx3fb.o
 obj-$(CONFIG_FB_DA8XX)		  += da8xx-fb.o
+obj-$(CONFIG_FB_MXS)		  += mxsfb.o
 
 # the test framebuffer is last
 obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
diff --git a/drivers/video/acornfb.c b/drivers/video/acornfb.c
index 82acb8dc4aa1..6183a57eb69d 100644
--- a/drivers/video/acornfb.c
+++ b/drivers/video/acornfb.c
@@ -66,7 +66,7 @@
  * have.  Allow 1% either way on the nominal for TVs.
  */
 #define NR_MONTYPES	6
-static struct fb_monspecs monspecs[NR_MONTYPES] __initdata = {
+static struct fb_monspecs monspecs[NR_MONTYPES] __devinitdata = {
 	{	/* TV		*/
 		.hfmin	= 15469,
 		.hfmax	= 15781,
@@ -873,7 +873,7 @@ static struct fb_ops acornfb_ops = {
 /*
  * Everything after here is initialisation!!!
  */
-static struct fb_videomode modedb[] __initdata = {
+static struct fb_videomode modedb[] __devinitdata = {
 	{	/* 320x256 @ 50Hz */
 		NULL, 50,  320,  256, 125000,  92,  62,  35, 19,  38, 2,
 		FB_SYNC_COMP_HIGH_ACT,
@@ -925,8 +925,7 @@ static struct fb_videomode modedb[] __initdata = {
 	}
 };
 
-static struct fb_videomode __initdata
-acornfb_default_mode = {
+static struct fb_videomode acornfb_default_mode __devinitdata = {
 	.name =		NULL,
 	.refresh =	60,
 	.xres =		640,
@@ -942,7 +941,7 @@ acornfb_default_mode = {
 	.vmode =	FB_VMODE_NONINTERLACED
 };
 
-static void __init acornfb_init_fbinfo(void)
+static void __devinit acornfb_init_fbinfo(void)
 {
 	static int first = 1;
 
@@ -1018,8 +1017,7 @@ static void __init acornfb_init_fbinfo(void)
  *	size can optionally be followed by 'M' or 'K' for
  *	MB or KB respectively.
  */
-static void __init
-acornfb_parse_mon(char *opt)
+static void __devinit acornfb_parse_mon(char *opt)
 {
 	char *p = opt;
 
@@ -1066,8 +1064,7 @@ bad:
 	current_par.montype = -1;
 }
 
-static void __init
-acornfb_parse_montype(char *opt)
+static void __devinit acornfb_parse_montype(char *opt)
 {
 	current_par.montype = -2;
 
@@ -1108,8 +1105,7 @@ acornfb_parse_montype(char *opt)
 	}
 }
 
-static void __init
-acornfb_parse_dram(char *opt)
+static void __devinit acornfb_parse_dram(char *opt)
 {
 	unsigned int size;
 
@@ -1134,15 +1130,14 @@ acornfb_parse_dram(char *opt)
 static struct options {
 	char *name;
 	void (*parse)(char *opt);
-} opt_table[] __initdata = {
+} opt_table[] __devinitdata = {
 	{ "mon",     acornfb_parse_mon     },
 	{ "montype", acornfb_parse_montype },
 	{ "dram",    acornfb_parse_dram    },
 	{ NULL, NULL }
 };
 
-int __init
-acornfb_setup(char *options)
+static int __devinit acornfb_setup(char *options)
 {
 	struct options *optp;
 	char *opt;
@@ -1179,8 +1174,7 @@ acornfb_setup(char *options)
  * Detect type of monitor connected
  *  For now, we just assume SVGA
  */
-static int __init
-acornfb_detect_monitortype(void)
+static int __devinit acornfb_detect_monitortype(void)
 {
 	return 4;
 }
diff --git a/drivers/video/amba-clcd.c b/drivers/video/amba-clcd.c
index 1c2c68356ea7..cf03ad067147 100644
--- a/drivers/video/amba-clcd.c
+++ b/drivers/video/amba-clcd.c
@@ -120,8 +120,23 @@ static void clcdfb_enable(struct clcd_fb *fb, u32 cntl)
 static int
 clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 {
+	u32 caps;
 	int ret = 0;
 
+	if (fb->panel->caps && fb->board->caps)
+		caps = fb->panel->caps & fb->board->caps;
+	else {
+		/* Old way of specifying what can be used */
+		caps = fb->panel->cntl & CNTL_BGR ?
+			CLCD_CAP_BGR : CLCD_CAP_RGB;
+		/* But mask out 444 modes as they weren't supported */
+		caps &= ~CLCD_CAP_444;
+	}
+
+	/* Only TFT panels can do RGB888/BGR888 */
+	if (!(fb->panel->cntl & CNTL_LCDTFT))
+		caps &= ~CLCD_CAP_888;
+
 	memset(&var->transp, 0, sizeof(var->transp));
 
 	var->red.msb_right = 0;
@@ -133,6 +148,13 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 	case 2:
 	case 4:
 	case 8:
+		/* If we can't do 5551, reject */
+		caps &= CLCD_CAP_5551;
+		if (!caps) {
+			ret = -EINVAL;
+			break;
+		}
+
 		var->red.length		= var->bits_per_pixel;
 		var->red.offset		= 0;
 		var->green.length	= var->bits_per_pixel;
@@ -140,23 +162,61 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 		var->blue.length	= var->bits_per_pixel;
 		var->blue.offset	= 0;
 		break;
+
 	case 16:
-		var->red.length = 5;
-		var->blue.length = 5;
+		/* If we can't do 444, 5551 or 565, reject */
+		if (!(caps & (CLCD_CAP_444 | CLCD_CAP_5551 | CLCD_CAP_565))) {
+			ret = -EINVAL;
+			break;
+		}
+
 		/*
-		 * Green length can be 5 or 6 depending whether
-		 * we're operating in RGB555 or RGB565 mode.
+		 * Green length can be 4, 5 or 6 depending whether
+		 * we're operating in 444, 5551 or 565 mode.
 		 */
-		if (var->green.length != 5 && var->green.length != 6)
-			var->green.length = 6;
+		if (var->green.length == 4 && caps & CLCD_CAP_444)
+			caps &= CLCD_CAP_444;
+		if (var->green.length == 5 && caps & CLCD_CAP_5551)
+			caps &= CLCD_CAP_5551;
+		else if (var->green.length == 6 && caps & CLCD_CAP_565)
+			caps &= CLCD_CAP_565;
+		else {
+			/*
+			 * PL110 officially only supports RGB555,
+			 * but may be wired up to allow RGB565.
+			 */
+			if (caps & CLCD_CAP_565) {
+				var->green.length = 6;
+				caps &= CLCD_CAP_565;
+			} else if (caps & CLCD_CAP_5551) {
+				var->green.length = 5;
+				caps &= CLCD_CAP_5551;
+			} else {
+				var->green.length = 4;
+				caps &= CLCD_CAP_444;
+			}
+		}
+
+		if (var->green.length >= 5) {
+			var->red.length = 5;
+			var->blue.length = 5;
+		} else {
+			var->red.length = 4;
+			var->blue.length = 4;
+		}
 		break;
 	case 32:
-		if (fb->panel->cntl & CNTL_LCDTFT) {
-			var->red.length		= 8;
-			var->green.length	= 8;
-			var->blue.length	= 8;
+		/* If we can't do 888, reject */
+		caps &= CLCD_CAP_888;
+		if (!caps) {
+			ret = -EINVAL;
 			break;
 		}
+
+		var->red.length = 8;
+		var->green.length = 8;
+		var->blue.length = 8;
+		break;
 	default:
 		ret = -EINVAL;
 		break;
@@ -168,7 +228,20 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 	 * the bitfield length defined above.
 	 */
 	if (ret == 0 && var->bits_per_pixel >= 16) {
-		if (fb->panel->cntl & CNTL_BGR) {
+		bool bgr, rgb;
+
+		bgr = caps & CLCD_CAP_BGR && var->blue.offset == 0;
+		rgb = caps & CLCD_CAP_RGB && var->red.offset == 0;
+
+		if (!bgr && !rgb)
+			/*
+			 * The requested format was not possible, try just
+			 * our capabilities.  One of BGR or RGB must be
+			 * supported.
+			 */
+			bgr = caps & CLCD_CAP_BGR;
+
+		if (bgr) {
 			var->blue.offset = 0;
 			var->green.offset = var->blue.offset + var->blue.length;
 			var->red.offset = var->green.offset + var->green.length;
@@ -374,6 +447,8 @@ static int clcdfb_register(struct clcd_fb *fb)
 		goto out;
 	}
 
+	fb->fb.device		= &fb->dev->dev;
+
 	fb->fb.fix.mmio_start	= fb->dev->res.start;
 	fb->fb.fix.mmio_len	= resource_size(&fb->dev->res);
 
@@ -443,8 +518,8 @@ static int clcdfb_register(struct clcd_fb *fb)
 
 	fb_set_var(&fb->fb, &fb->fb.var);
 
-        printk(KERN_INFO "CLCD: %s hardware, %s display\n",
-               fb->board->name, fb->panel->mode.name);
+	dev_info(&fb->dev->dev, "%s hardware, %s display\n",
+	         fb->board->name, fb->panel->mode.name);
 
 	ret = register_framebuffer(&fb->fb);
 	if (ret == 0)
@@ -461,7 +536,7 @@ static int clcdfb_register(struct clcd_fb *fb)
 	return ret;
 }
 
-static int clcdfb_probe(struct amba_device *dev, struct amba_id *id)
+static int clcdfb_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct clcd_board *board = dev->dev.platform_data;
 	struct clcd_fb *fb;
@@ -486,6 +561,10 @@ static int clcdfb_probe(struct amba_device *dev, struct amba_id *id)
 	fb->dev = dev;
 	fb->board = board;
 
+	dev_info(&fb->dev->dev, "PL%03x rev%u at 0x%08llx\n",
+		amba_part(dev), amba_rev(dev),
+		(unsigned long long)dev->res.start);
+
 	ret = fb->board->setup(fb);
 	if (ret)
 		goto free_fb;
diff --git a/drivers/video/amifb.c b/drivers/video/amifb.c
index e5d6b56d4447..5ea6596dd824 100644
--- a/drivers/video/amifb.c
+++ b/drivers/video/amifb.c
@@ -2224,22 +2224,23 @@ static int amifb_ioctl(struct fb_info *info,
 	 * Allocate, Clear and Align a Block of Chip Memory
 	 */
 
-static u_long unaligned_chipptr = 0;
+static void *aligned_chipptr;
 
 static inline u_long __init chipalloc(u_long size)
 {
-	size += PAGE_SIZE-1;
-	if (!(unaligned_chipptr = (u_long)amiga_chip_alloc(size,
-							   "amifb [RAM]")))
-		panic("No Chip RAM for frame buffer");
-	memset((void *)unaligned_chipptr, 0, size);
-	return PAGE_ALIGN(unaligned_chipptr);
+	aligned_chipptr = amiga_chip_alloc(size, "amifb [RAM]");
+	if (!aligned_chipptr) {
+		pr_err("amifb: No Chip RAM for frame buffer");
+		return 0;
+	}
+	memset(aligned_chipptr, 0, size);
+	return (u_long)aligned_chipptr;
 }
 
 static inline void chipfree(void)
 {
-	if (unaligned_chipptr)
-		amiga_chip_free((void *)unaligned_chipptr);
+	if (aligned_chipptr)
+		amiga_chip_free(aligned_chipptr);
 }
 
 
@@ -2295,7 +2296,7 @@ default_chipset:
 			    defmode = amiga_vblank == 50 ? DEFMODE_PAL
 							 : DEFMODE_NTSC;
 			if (amiga_chip_avail()-CHIPRAM_SAFETY_LIMIT >
-			    VIDEOMEMSIZE_ECS_1M)
+			    VIDEOMEMSIZE_ECS_2M)
 				fb_info.fix.smem_len = VIDEOMEMSIZE_ECS_2M;
 			else
 				fb_info.fix.smem_len = VIDEOMEMSIZE_ECS_1M;
@@ -2312,7 +2313,7 @@ default_chipset:
 			maxfmode = TAG_FMODE_4;
 			defmode = DEFMODE_AGA;
 			if (amiga_chip_avail()-CHIPRAM_SAFETY_LIMIT >
-			    VIDEOMEMSIZE_AGA_1M)
+			    VIDEOMEMSIZE_AGA_2M)
 				fb_info.fix.smem_len = VIDEOMEMSIZE_AGA_2M;
 			else
 				fb_info.fix.smem_len = VIDEOMEMSIZE_AGA_1M;
@@ -2385,6 +2386,10 @@ default_chipset:
 	                    DUMMYSPRITEMEMSIZE+
 	                    COPINITSIZE+
 	                    4*COPLISTSIZE);
+	if (!chipptr) {
+		err = -ENOMEM;
+		goto amifb_error;
+	}
 
 	assignchunk(videomemory, u_long, chipptr, fb_info.fix.smem_len);
 	assignchunk(spritememory, u_long, chipptr, SPRITEMEMSIZE);
diff --git a/drivers/video/arcfb.c b/drivers/video/arcfb.c
index 3ec4923c2d84..c22e8d39a2cb 100644
--- a/drivers/video/arcfb.c
+++ b/drivers/video/arcfb.c
@@ -515,11 +515,10 @@ static int __devinit arcfb_probe(struct platform_device *dev)
 
 	/* We need a flat backing store for the Arc's
 	   less-flat actual paged framebuffer */
-	if (!(videomemory = vmalloc(videomemorysize)))
+	videomemory = vzalloc(videomemorysize);
+	if (!videomemory)
 		return retval;
 
-	memset(videomemory, 0, videomemorysize);
-
 	info = framebuffer_alloc(sizeof(struct arcfb_par), &dev->dev);
 	if (!info)
 		goto err;
diff --git a/drivers/video/arkfb.c b/drivers/video/arkfb.c
index 391ac939f011..8686429cbdf0 100644
--- a/drivers/video/arkfb.c
+++ b/drivers/video/arkfb.c
@@ -158,12 +158,19 @@ static void arkfb_settile(struct fb_info *info, struct fb_tilemap *map)
 	}
 }
 
+static void arkfb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+	struct arkfb_info *par = info->par;
+
+	svga_tilecursor(par->state.vgabase, info, cursor);
+}
+
 static struct fb_tile_ops arkfb_tile_ops = {
 	.fb_settile	= arkfb_settile,
 	.fb_tilecopy	= svga_tilecopy,
 	.fb_tilefill    = svga_tilefill,
 	.fb_tileblit    = svga_tileblit,
-	.fb_tilecursor  = svga_tilecursor,
+	.fb_tilecursor  = arkfb_tilecursor,
 	.fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -466,32 +473,40 @@ static unsigned short dac_regs[4] = {0x3c8, 0x3c9, 0x3c6, 0x3c7};
 
 static void ark_dac_read_regs(void *data, u8 *code, int count)
 {
-	u8 regval = vga_rseq(NULL, 0x1C);
+	struct fb_info *info = data;
+	struct arkfb_info *par;
+	u8 regval;
 
+	par = info->par;
+	regval = vga_rseq(par->state.vgabase, 0x1C);
 	while (count != 0)
 	{
-		vga_wseq(NULL, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
-		code[1] = vga_r(NULL, dac_regs[code[0] & 3]);
+		vga_wseq(par->state.vgabase, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
+		code[1] = vga_r(par->state.vgabase, dac_regs[code[0] & 3]);
 		count--;
 		code += 2;
 	}
 
-	vga_wseq(NULL, 0x1C, regval);
+	vga_wseq(par->state.vgabase, 0x1C, regval);
 }
 
 static void ark_dac_write_regs(void *data, u8 *code, int count)
 {
-	u8 regval = vga_rseq(NULL, 0x1C);
+	struct fb_info *info = data;
+	struct arkfb_info *par;
+	u8 regval;
 
+	par = info->par;
+	regval = vga_rseq(par->state.vgabase, 0x1C);
 	while (count != 0)
 	{
-		vga_wseq(NULL, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
-		vga_w(NULL, dac_regs[code[0] & 3], code[1]);
+		vga_wseq(par->state.vgabase, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
+		vga_w(par->state.vgabase, dac_regs[code[0] & 3], code[1]);
 		count--;
 		code += 2;
 	}
 
-	vga_wseq(NULL, 0x1C, regval);
+	vga_wseq(par->state.vgabase, 0x1C, regval);
 }
 
 
@@ -507,8 +522,8 @@ static void ark_set_pixclock(struct fb_info *info, u32 pixclock)
 	}
 
 	/* Set VGA misc register  */
-	regval = vga_r(NULL, VGA_MIS_R);
-	vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+	regval = vga_r(par->state.vgabase, VGA_MIS_R);
+	vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 }
 
 
@@ -520,7 +535,10 @@ static int arkfb_open(struct fb_info *info, int user)
 
 	mutex_lock(&(par->open_lock));
 	if (par->ref_count == 0) {
+		void __iomem *vgabase = par->state.vgabase;
+
 		memset(&(par->state), 0, sizeof(struct vgastate));
+		par->state.vgabase = vgabase;
 		par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
 		par->state.num_crtc = 0x60;
 		par->state.num_seq = 0x30;
@@ -646,50 +664,50 @@ static int arkfb_set_par(struct fb_info *info)
 	info->var.activate = FB_ACTIVATE_NOW;
 
 	/* Unlock registers */
-	svga_wcrt_mask(0x11, 0x00, 0x80);
+	svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
 
 	/* Blank screen and turn off sync */
-	svga_wseq_mask(0x01, 0x20, 0x20);
-	svga_wcrt_mask(0x17, 0x00, 0x80);
+	svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+	svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
 	/* Set default values */
-	svga_set_default_gfx_regs();
-	svga_set_default_atc_regs();
-	svga_set_default_seq_regs();
-	svga_set_default_crt_regs();
-	svga_wcrt_multi(ark_line_compare_regs, 0xFFFFFFFF);
-	svga_wcrt_multi(ark_start_address_regs, 0);
+	svga_set_default_gfx_regs(par->state.vgabase);
+	svga_set_default_atc_regs(par->state.vgabase);
+	svga_set_default_seq_regs(par->state.vgabase);
+	svga_set_default_crt_regs(par->state.vgabase);
+	svga_wcrt_multi(par->state.vgabase, ark_line_compare_regs, 0xFFFFFFFF);
+	svga_wcrt_multi(par->state.vgabase, ark_start_address_regs, 0);
 
 	/* ARK specific initialization */
-	svga_wseq_mask(0x10, 0x1F, 0x1F); /* enable linear framebuffer and full memory access */
-	svga_wseq_mask(0x12, 0x03, 0x03); /* 4 MB linear framebuffer size */
+	svga_wseq_mask(par->state.vgabase, 0x10, 0x1F, 0x1F); /* enable linear framebuffer and full memory access */
+	svga_wseq_mask(par->state.vgabase, 0x12, 0x03, 0x03); /* 4 MB linear framebuffer size */
 
-	vga_wseq(NULL, 0x13, info->fix.smem_start >> 16);
-	vga_wseq(NULL, 0x14, info->fix.smem_start >> 24);
-	vga_wseq(NULL, 0x15, 0);
-	vga_wseq(NULL, 0x16, 0);
+	vga_wseq(par->state.vgabase, 0x13, info->fix.smem_start >> 16);
+	vga_wseq(par->state.vgabase, 0x14, info->fix.smem_start >> 24);
+	vga_wseq(par->state.vgabase, 0x15, 0);
+	vga_wseq(par->state.vgabase, 0x16, 0);
 
 	/* Set the FIFO threshold register */
 	/* It is fascinating way to store 5-bit value in 8-bit register */
 	regval = 0x10 | ((threshold & 0x0E) >> 1) | (threshold & 0x01) << 7 | (threshold & 0x10) << 1;
-	vga_wseq(NULL, 0x18, regval);
+	vga_wseq(par->state.vgabase, 0x18, regval);
 
 	/* Set the offset register */
 	pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
-	svga_wcrt_multi(ark_offset_regs, offset_value);
+	svga_wcrt_multi(par->state.vgabase, ark_offset_regs, offset_value);
 
 	/* fix for hi-res textmode */
-	svga_wcrt_mask(0x40, 0x08, 0x08);
+	svga_wcrt_mask(par->state.vgabase, 0x40, 0x08, 0x08);
 
 	if (info->var.vmode & FB_VMODE_DOUBLE)
-		svga_wcrt_mask(0x09, 0x80, 0x80);
+		svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
 	else
-		svga_wcrt_mask(0x09, 0x00, 0x80);
+		svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
 	if (info->var.vmode & FB_VMODE_INTERLACED)
-		svga_wcrt_mask(0x44, 0x04, 0x04);
+		svga_wcrt_mask(par->state.vgabase, 0x44, 0x04, 0x04);
 	else
-		svga_wcrt_mask(0x44, 0x00, 0x04);
+		svga_wcrt_mask(par->state.vgabase, 0x44, 0x00, 0x04);
 
 	hmul = 1;
 	hdiv = 1;
@@ -699,40 +717,40 @@ static int arkfb_set_par(struct fb_info *info)
 	switch (mode) {
 	case 0:
 		pr_debug("fb%d: text mode\n", info->node);
-		svga_set_textmode_vga_regs();
+		svga_set_textmode_vga_regs(par->state.vgabase);
 
-		vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-		svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
 		dac_set_mode(par->dac, DAC_PSEUDO8_8);
 
 		break;
 	case 1:
 		pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-		vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
+		vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
 
-		vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-		svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
 		dac_set_mode(par->dac, DAC_PSEUDO8_8);
 		break;
 	case 2:
 		pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
 
-		vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-		svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
 		dac_set_mode(par->dac, DAC_PSEUDO8_8);
 		break;
 	case 3:
 		pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
 
-		vga_wseq(NULL, 0x11, 0x16); /* 8bpp accel mode */
+		vga_wseq(par->state.vgabase, 0x11, 0x16); /* 8bpp accel mode */
 
 		if (info->var.pixclock > 20000) {
 			pr_debug("fb%d: not using multiplex\n", info->node);
-			svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+			svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
 			dac_set_mode(par->dac, DAC_PSEUDO8_8);
 		} else {
 			pr_debug("fb%d: using multiplex\n", info->node);
-			svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+			svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
 			dac_set_mode(par->dac, DAC_PSEUDO8_16);
 			hdiv = 2;
 		}
@@ -740,22 +758,22 @@ static int arkfb_set_par(struct fb_info *info)
 	case 4:
 		pr_debug("fb%d: 5/5/5 truecolor\n", info->node);
 
-		vga_wseq(NULL, 0x11, 0x1A); /* 16bpp accel mode */
-		svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x1A); /* 16bpp accel mode */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
 		dac_set_mode(par->dac, DAC_RGB1555_16);
 		break;
 	case 5:
 		pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
 
-		vga_wseq(NULL, 0x11, 0x1A); /* 16bpp accel mode */
-		svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x1A); /* 16bpp accel mode */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
 		dac_set_mode(par->dac, DAC_RGB0565_16);
 		break;
 	case 6:
 		pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
 
-		vga_wseq(NULL, 0x11, 0x16); /* 8bpp accel mode ??? */
-		svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x16); /* 8bpp accel mode ??? */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
 		dac_set_mode(par->dac, DAC_RGB0888_16);
 		hmul = 3;
 		hdiv = 2;
@@ -763,8 +781,8 @@ static int arkfb_set_par(struct fb_info *info)
 	case 7:
 		pr_debug("fb%d: 8/8/8/8 truecolor\n", info->node);
 
-		vga_wseq(NULL, 0x11, 0x1E); /* 32bpp accel mode */
-		svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+		vga_wseq(par->state.vgabase, 0x11, 0x1E); /* 32bpp accel mode */
+		svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
 		dac_set_mode(par->dac, DAC_RGB8888_16);
 		hmul = 2;
 		break;
@@ -774,7 +792,7 @@ static int arkfb_set_par(struct fb_info *info)
 	}
 
 	ark_set_pixclock(info, (hdiv * info->var.pixclock) / hmul);
-	svga_set_timings(&ark_timing_regs, &(info->var), hmul, hdiv,
+	svga_set_timings(par->state.vgabase, &ark_timing_regs, &(info->var), hmul, hdiv,
 			 (info->var.vmode & FB_VMODE_DOUBLE)     ? 2 : 1,
 			 (info->var.vmode & FB_VMODE_INTERLACED) ? 2 : 1,
 			  hmul, info->node);
@@ -782,12 +800,12 @@ static int arkfb_set_par(struct fb_info *info)
 	/* Set interlaced mode start/end register */
 	value = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
 	value = ((value * hmul / hdiv) / 8) - 5;
-	vga_wcrt(NULL, 0x42, (value + 1) / 2);
+	vga_wcrt(par->state.vgabase, 0x42, (value + 1) / 2);
 
 	memset_io(info->screen_base, 0x00, screen_size);
 	/* Device and screen back on */
-	svga_wcrt_mask(0x17, 0x80, 0x80);
-	svga_wseq_mask(0x01, 0x00, 0x20);
+	svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+	svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
 	return 0;
 }
@@ -857,23 +875,25 @@ static int arkfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int arkfb_blank(int blank_mode, struct fb_info *info)
 {
+	struct arkfb_info *par = info->par;
+
 	switch (blank_mode) {
 	case FB_BLANK_UNBLANK:
 		pr_debug("fb%d: unblank\n", info->node);
-		svga_wseq_mask(0x01, 0x00, 0x20);
-		svga_wcrt_mask(0x17, 0x80, 0x80);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
 		break;
 	case FB_BLANK_NORMAL:
 		pr_debug("fb%d: blank\n", info->node);
-		svga_wseq_mask(0x01, 0x20, 0x20);
-		svga_wcrt_mask(0x17, 0x80, 0x80);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
 		break;
 	case FB_BLANK_POWERDOWN:
 	case FB_BLANK_HSYNC_SUSPEND:
 	case FB_BLANK_VSYNC_SUSPEND:
 		pr_debug("fb%d: sync down\n", info->node);
-		svga_wseq_mask(0x01, 0x20, 0x20);
-		svga_wcrt_mask(0x17, 0x00, 0x80);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 		break;
 	}
 	return 0;
@@ -884,6 +904,7 @@ static int arkfb_blank(int blank_mode, struct fb_info *info)
 
 static int arkfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+	struct arkfb_info *par = info->par;
 	unsigned int offset;
 
 	/* Calculate the offset */
@@ -897,7 +918,7 @@ static int arkfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info
 	}
 
 	/* Set the offset */
-	svga_wcrt_multi(ark_start_address_regs, offset);
+	svga_wcrt_multi(par->state.vgabase, ark_start_address_regs, offset);
 
 	return 0;
 }
@@ -930,6 +951,8 @@ static struct fb_ops arkfb_ops = {
 /* PCI probe */
 static int __devinit ark_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+	struct pci_bus_region bus_reg;
+	struct resource vga_res;
 	struct fb_info *info;
 	struct arkfb_info *par;
 	int rc;
@@ -985,8 +1008,17 @@ static int __devinit ark_pci_probe(struct pci_dev *dev, const struct pci_device_
 		goto err_iomap;
 	}
 
+	bus_reg.start = 0;
+	bus_reg.end = 64 * 1024;
+
+	vga_res.flags = IORESOURCE_IO;
+
+	pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+	par->state.vgabase = (void __iomem *) vga_res.start;
+
 	/* FIXME get memsize */
-	regval = vga_rseq(NULL, 0x10);
+	regval = vga_rseq(par->state.vgabase, 0x10);
 	info->screen_size = (1 << (regval >> 6)) << 20;
 	info->fix.smem_len = info->screen_size;
 
diff --git a/drivers/video/atafb.c b/drivers/video/atafb.c
index 5b2b5ef4edba..64e41f5448c4 100644
--- a/drivers/video/atafb.c
+++ b/drivers/video/atafb.c
@@ -3117,7 +3117,7 @@ int __init atafb_init(void)
 			atafb_ops.fb_setcolreg = &falcon_setcolreg;
 			error = request_irq(IRQ_AUTO_4, falcon_vbl_switcher,
 					    IRQ_TYPE_PRIO,
-					    "framebuffer/modeswitch",
+					    "framebuffer:modeswitch",
 					    falcon_vbl_switcher);
 			if (error)
 				return error;
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index bac163450216..817ab60f7537 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -68,7 +68,7 @@ static void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
 }
 #endif
 
-static const u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
+static u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
 		| ATMEL_LCDC_POL_POSITIVE
 		| ATMEL_LCDC_ENA_PWMENABLE;
 
@@ -127,6 +127,7 @@ static void init_backlight(struct atmel_lcdfb_info *sinfo)
 		return;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = 0xff;
 	bl = backlight_device_register("backlight", &sinfo->pdev->dev, sinfo,
 				       &atmel_lcdc_bl_ops, &props);
@@ -163,6 +164,10 @@ static void exit_backlight(struct atmel_lcdfb_info *sinfo)
 
 static void init_contrast(struct atmel_lcdfb_info *sinfo)
 {
+	/* contrast pwm can be 'inverted' */
+	if (sinfo->lcdcon_pol_negative)
+			contrast_ctr &= ~(ATMEL_LCDC_POL_POSITIVE);
+
 	/* have some default contrast/backlight settings */
 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
@@ -632,7 +637,7 @@ static inline unsigned int chan_to_field(unsigned int chan, const struct fb_bitf
  *  	magnitude which needs to be scaled in this function for the hardware.
  *	Things to take into consideration are how many color registers, if
  *	any, are supported with the current color visual. With truecolor mode
- *	no color palettes are supported. Here a psuedo palette is created
+ *	no color palettes are supported. Here a pseudo palette is created
  *	which we store the value in pseudo_palette in struct fb_info. For
  *	pseudocolor mode we have a limited color palette. To deal with this
  *	we can program what color is displayed for a particular pixel value.
@@ -710,11 +715,35 @@ static int atmel_lcdfb_pan_display(struct fb_var_screeninfo *var,
 	return 0;
 }
 
+static int atmel_lcdfb_blank(int blank_mode, struct fb_info *info)
+{
+	struct atmel_lcdfb_info *sinfo = info->par;
+
+	switch (blank_mode) {
+	case FB_BLANK_UNBLANK:
+	case FB_BLANK_NORMAL:
+		atmel_lcdfb_start(sinfo);
+		break;
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+		break;
+	case FB_BLANK_POWERDOWN:
+		atmel_lcdfb_stop(sinfo);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* let fbcon do a soft blank for us */
+	return ((blank_mode == FB_BLANK_NORMAL) ? 1 : 0);
+}
+
 static struct fb_ops atmel_lcdfb_ops = {
 	.owner		= THIS_MODULE,
 	.fb_check_var	= atmel_lcdfb_check_var,
 	.fb_set_par	= atmel_lcdfb_set_par,
 	.fb_setcolreg	= atmel_lcdfb_setcolreg,
+	.fb_blank	= atmel_lcdfb_blank,
 	.fb_pan_display	= atmel_lcdfb_pan_display,
 	.fb_fillrect	= cfb_fillrect,
 	.fb_copyarea	= cfb_copyarea,
@@ -816,6 +845,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 		sinfo->guard_time = pdata_sinfo->guard_time;
 		sinfo->smem_len = pdata_sinfo->smem_len;
 		sinfo->lcdcon_is_backlight = pdata_sinfo->lcdcon_is_backlight;
+		sinfo->lcdcon_pol_negative = pdata_sinfo->lcdcon_pol_negative;
 		sinfo->lcd_wiring_mode = pdata_sinfo->lcd_wiring_mode;
 	} else {
 		dev_err(dev, "cannot get default configuration\n");
@@ -876,7 +906,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 	if (map) {
 		/* use a pre-allocated memory buffer */
 		info->fix.smem_start = map->start;
-		info->fix.smem_len = map->end - map->start + 1;
+		info->fix.smem_len = resource_size(map);
 		if (!request_mem_region(info->fix.smem_start,
 					info->fix.smem_len, pdev->name)) {
 			ret = -EBUSY;
@@ -902,7 +932,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 
 	/* LCDC registers */
 	info->fix.mmio_start = regs->start;
-	info->fix.mmio_len = regs->end - regs->start + 1;
+	info->fix.mmio_len = resource_size(regs);
 
 	if (!request_mem_region(info->fix.mmio_start,
 				info->fix.mmio_len, pdev->name)) {
diff --git a/drivers/video/aty/aty128fb.c b/drivers/video/aty/aty128fb.c
index 4cb6a576c567..b0b2ac335347 100644
--- a/drivers/video/aty/aty128fb.c
+++ b/drivers/video/aty/aty128fb.c
@@ -1818,6 +1818,7 @@ static void aty128_bl_init(struct aty128fb_par *par)
 	snprintf(name, sizeof(name), "aty128bl%d", info->node);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
 	bd = backlight_device_register(name, info->dev, par, &aty128_bl_data,
 				       &props);
diff --git a/drivers/video/aty/atyfb_base.c b/drivers/video/aty/atyfb_base.c
index 94e293fce1d2..44bdce4242ad 100644
--- a/drivers/video/aty/atyfb_base.c
+++ b/drivers/video/aty/atyfb_base.c
@@ -248,10 +248,6 @@ static int atyfb_sync(struct fb_info *info);
 
 static int aty_init(struct fb_info *info);
 
-#ifdef CONFIG_ATARI
-static int store_video_par(char *videopar, unsigned char m64_num);
-#endif
-
 static void aty_get_crtc(const struct atyfb_par *par, struct crtc *crtc);
 
 static void aty_set_crtc(const struct atyfb_par *par, const struct crtc *crtc);
@@ -2241,6 +2237,7 @@ static void aty_bl_init(struct atyfb_par *par)
 	snprintf(name, sizeof(name), "atybl%d", info->node);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
 	bd = backlight_device_register(name, info->dev, par, &aty_bl_data,
 				       &props);
@@ -2267,11 +2264,13 @@ error:
 	return;
 }
 
+#ifdef CONFIG_PCI
 static void aty_bl_exit(struct backlight_device *bd)
 {
 	backlight_device_unregister(bd);
 	printk("aty: Backlight unloaded\n");
 }
+#endif /* CONFIG_PCI */
 
 #endif /* CONFIG_FB_ATY_BACKLIGHT */
 
@@ -2788,7 +2787,7 @@ aty_init_exit:
 	return ret;
 }
 
-#ifdef CONFIG_ATARI
+#if defined(CONFIG_ATARI) && !defined(MODULE)
 static int __devinit store_video_par(char *video_str, unsigned char m64_num)
 {
 	char *p;
@@ -2817,7 +2816,7 @@ static int __devinit store_video_par(char *video_str, unsigned char m64_num)
 	phys_vmembase[m64_num] = 0;
 	return -1;
 }
-#endif /* CONFIG_ATARI */
+#endif /* CONFIG_ATARI && !MODULE */
 
 /*
  * Blank the display.
@@ -3123,12 +3122,12 @@ static int __devinit atyfb_setup_sparc(struct pci_dev *pdev,
 		M = pll_regs[2];
 
 		/*
-		 * PLL Feedback Divider N (Dependant on CLOCK_CNTL):
+		 * PLL Feedback Divider N (Dependent on CLOCK_CNTL):
 		 */
 		N = pll_regs[7 + (clock_cntl & 3)];
 
 		/*
-		 * PLL Post Divider P (Dependant on CLOCK_CNTL):
+		 * PLL Post Divider P (Dependent on CLOCK_CNTL):
 		 */
 		P = 1 << (pll_regs[6] >> ((clock_cntl & 3) << 1));
 
@@ -3459,9 +3458,10 @@ static int __devinit atyfb_setup_generic(struct pci_dev *pdev,
 
 	raddr = addr + 0x7ff000UL;
 	rrp = &pdev->resource[2];
-	if ((rrp->flags & IORESOURCE_MEM) && request_mem_region(rrp->start, rrp->end - rrp->start + 1, "atyfb")) {
+	if ((rrp->flags & IORESOURCE_MEM) &&
+	    request_mem_region(rrp->start, resource_size(rrp), "atyfb")) {
 		par->aux_start = rrp->start;
-		par->aux_size = rrp->end - rrp->start + 1;
+		par->aux_size = resource_size(rrp);
 		raddr = rrp->start;
 		PRINTKI("using auxiliary register aperture\n");
 	}
@@ -3551,7 +3551,7 @@ static int __devinit atyfb_pci_probe(struct pci_dev *pdev,
 
 	/* Reserve space */
 	res_start = rp->start;
-	res_size = rp->end - rp->start + 1;
+	res_size = resource_size(rp);
 	if (!request_mem_region(res_start, res_size, "atyfb"))
 		return -EBUSY;
 
diff --git a/drivers/video/aty/mach64_cursor.c b/drivers/video/aty/mach64_cursor.c
index 2ba8b3c421a1..46f72ed53510 100644
--- a/drivers/video/aty/mach64_cursor.c
+++ b/drivers/video/aty/mach64_cursor.c
@@ -51,7 +51,7 @@
  * to a larger number and saturate CUR_HORZ_POSN to zero.
  *
  * if Y becomes negative, CUR_VERT_OFFSET must be adjusted to a larger number,
- * CUR_OFFSET must be adjusted to a point to the appropraite line in the cursor
+ * CUR_OFFSET must be adjusted to a point to the appropriate line in the cursor
  * definitation and CUR_VERT_POSN must be saturated to zero.
  */
 
diff --git a/drivers/video/aty/radeon_backlight.c b/drivers/video/aty/radeon_backlight.c
index 9b811ddbce83..db572df7e1ef 100644
--- a/drivers/video/aty/radeon_backlight.c
+++ b/drivers/video/aty/radeon_backlight.c
@@ -158,6 +158,7 @@ void radeonfb_bl_init(struct radeonfb_info *rinfo)
 	snprintf(name, sizeof(name), "radeonbl%d", rinfo->info->node);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
 	bd = backlight_device_register(name, rinfo->info->dev, pdata,
 				       &radeon_bl_data, &props);
diff --git a/drivers/video/aty/radeon_base.c b/drivers/video/aty/radeon_base.c
index 3c1e13ed1cba..32f8cf6200a7 100644
--- a/drivers/video/aty/radeon_base.c
+++ b/drivers/video/aty/radeon_base.c
@@ -1248,7 +1248,7 @@ static void radeon_write_pll_regs(struct radeonfb_info *rinfo, struct radeon_reg
 
 	/* Workaround from XFree */
 	if (rinfo->is_mobility) {
-	        /* A temporal workaround for the occational blanking on certain laptop
+	        /* A temporal workaround for the occasional blanking on certain laptop
 		 * panels. This appears to related to the PLL divider registers
 		 * (fail to lock?). It occurs even when all dividers are the same
 		 * with their old settings. In this case we really don't need to
diff --git a/drivers/video/aty/radeon_i2c.c b/drivers/video/aty/radeon_i2c.c
index 78d1f4cd1fe0..ab1d0fd76316 100644
--- a/drivers/video/aty/radeon_i2c.c
+++ b/drivers/video/aty/radeon_i2c.c
@@ -100,6 +100,9 @@ void radeon_create_i2c_busses(struct radeonfb_info *rinfo)
 {
 	rinfo->i2c[0].rinfo	= rinfo;
 	rinfo->i2c[0].ddc_reg	= GPIO_MONID;
+#ifndef CONFIG_PPC
+	rinfo->i2c[0].adapter.class = I2C_CLASS_HWMON;
+#endif
 	radeon_setup_i2c_bus(&rinfo->i2c[0], "monid");
 
 	rinfo->i2c[1].rinfo	= rinfo;
diff --git a/drivers/video/au1100fb.c b/drivers/video/au1100fb.c
index 34b2fc472fe8..01a8fde67f20 100644
--- a/drivers/video/au1100fb.c
+++ b/drivers/video/au1100fb.c
@@ -486,7 +486,7 @@ static int __devinit au1100fb_drv_probe(struct platform_device *dev)
 	}
 
 	au1100fb_fix.mmio_start = regs_res->start;
-	au1100fb_fix.mmio_len = regs_res->end - regs_res->start + 1;
+	au1100fb_fix.mmio_len = resource_size(regs_res);
 
 	if (!request_mem_region(au1100fb_fix.mmio_start, au1100fb_fix.mmio_len,
 				DRIVER_NAME)) {
diff --git a/drivers/video/au1200fb.c b/drivers/video/au1200fb.c
index 4ea187d93768..5dff32ac8044 100644
--- a/drivers/video/au1200fb.c
+++ b/drivers/video/au1200fb.c
@@ -1572,7 +1572,7 @@ static int au1200fb_init_fbinfo(struct au1200fb_device *fbdev)
 	/* Copy monitor specs from panel data */
 	/* fixme: we're setting up LCD controller windows, so these dont give a
 	damn as to what the monitor specs are (the panel itself does, but that
-	isnt done here...so maybe need a generic catchall monitor setting??? */
+	isn't done here...so maybe need a generic catchall monitor setting??? */
 	memcpy(&fbi->monspecs, &panel->monspecs, sizeof(struct fb_monspecs));
 
 	/* We first try the user mode passed in argument. If that failed,
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index b224396b86d5..c04b94da81f7 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -12,11 +12,11 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/fb.h>
 #include <linux/i2c.h>
 #include <linux/backlight.h>
 #include <linux/mfd/88pm860x.h>
-#include <linux/slab.h>
 
 #define MAX_BRIGHTNESS		(0xFF)
 #define MIN_BRIGHTNESS		(0)
@@ -161,29 +161,9 @@ static const struct backlight_ops pm860x_backlight_ops = {
 	.get_brightness	= pm860x_backlight_get_brightness,
 };
 
-static int __check_device(struct pm860x_backlight_pdata *pdata, char *name)
-{
-	struct pm860x_backlight_pdata *p = pdata;
-	int ret = -EINVAL;
-
-	while (p && p->id) {
-		if ((p->id != PM8606_ID_BACKLIGHT) || (p->flags < 0))
-			break;
-
-		if (!strncmp(name, pm860x_backlight_name[p->flags],
-			MFD_NAME_SIZE)) {
-			ret = (int)p->flags;
-			break;
-		}
-		p++;
-	}
-	return ret;
-}
-
 static int pm860x_backlight_probe(struct platform_device *pdev)
 {
 	struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
-	struct pm860x_platform_data *pm860x_pdata;
 	struct pm860x_backlight_pdata *pdata = NULL;
 	struct pm860x_backlight_data *data;
 	struct backlight_device *bl;
@@ -199,10 +179,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	if (pdev->dev.parent->platform_data) {
-		pm860x_pdata = pdev->dev.parent->platform_data;
-		pdata = pm860x_pdata->backlight;
-	}
+	pdata = pdev->dev.platform_data;
 	if (pdata == NULL) {
 		dev_err(&pdev->dev, "platform data isn't assigned to "
 			"backlight\n");
@@ -219,7 +196,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 	data->current_brightness = MAX_BRIGHTNESS;
 	data->pwm = pdata->pwm;
 	data->iset = pdata->iset;
-	data->port = __check_device(pdata, name);
+	data->port = pdata->flags;
 	if (data->port < 0) {
 		dev_err(&pdev->dev, "wrong platform data is assigned");
 		kfree(data);
@@ -227,6 +204,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 	}
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = MAX_BRIGHTNESS;
 	bl = backlight_device_register(name, &pdev->dev, data,
 					&pm860x_backlight_ops, &props);
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index e54a337227ea..278aeaa92505 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -109,6 +109,22 @@ config LCD_S6E63M0
 	  If you have an S6E63M0 LCD Panel, say Y to enable its
 	  LCD control driver.
 
+config LCD_LD9040
+	tristate "LD9040 AMOLED LCD Driver"
+	depends on SPI && BACKLIGHT_CLASS_DEVICE
+	default n
+	help
+	  If you have an LD9040 Panel, say Y to enable its
+	  control driver.
+
+config LCD_AMS369FG06
+	tristate "AMS369FG06 AMOLED LCD Driver"
+	depends on SPI && BACKLIGHT_CLASS_DEVICE
+	default n
+	help
+	  If you have an AMS369FG06 AMOLED Panel, say Y to enable its
+	  LCD control driver.
+
 endif # LCD_CLASS_DEVICE
 
 #
@@ -236,12 +252,12 @@ config BACKLIGHT_MAX8925
 	  If you have a LCD backlight connected to the WLED output of MAX8925
 	  WLED output, say Y here to enable this driver.
 
-config BACKLIGHT_MBP_NVIDIA
-       tristate "MacBook Pro Nvidia Backlight Driver"
-       depends on X86
+config BACKLIGHT_APPLE
+       tristate "Apple Backlight Driver"
+       depends on X86 && ACPI
        help
-         If you have an Apple Macbook Pro with Nvidia graphics hardware say Y
-	 to enable a driver for its backlight
+         If you have an Intel-based Apple say Y to enable a driver for its
+	 backlight.
 
 config BACKLIGHT_TOSA
 	tristate "Sharp SL-6000 Backlight Driver"
@@ -294,6 +310,18 @@ config BACKLIGHT_ADP8860
 	  To compile this driver as a module, choose M here: the module will
 	  be called adp8860_bl.
 
+config BACKLIGHT_ADP8870
+	tristate "Backlight Driver for ADP8870 using WLED"
+	depends on BACKLIGHT_CLASS_DEVICE && I2C
+	select NEW_LEDS
+	select LEDS_CLASS
+	help
+	  If you have a LCD backlight connected to the ADP8870,
+	  say Y here to enable this driver.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called adp8870_bl.
+
 config BACKLIGHT_88PM860X
 	tristate "Backlight Driver for 88PM8606 using WLED"
 	depends on MFD_88PM860X
@@ -307,6 +335,13 @@ config BACKLIGHT_PCF50633
 	  If you have a backlight driven by a NXP PCF50633 MFD, say Y here to
 	  enable its driver.
 
+config BACKLIGHT_AAT2870
+	tristate "AnalogicTech AAT2870 Backlight"
+	depends on BACKLIGHT_CLASS_DEVICE && MFD_AAT2870_CORE
+	help
+	  If you have a AnalogicTech AAT2870 say Y to enable the
+	  backlight driver.
+
 endif # BACKLIGHT_CLASS_DEVICE
 
 endif # BACKLIGHT_LCD_SUPPORT
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index 44c0f81ad85d..fdd1fc4b2770 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -12,6 +12,8 @@ obj-$(CONFIG_LCD_VGG2432A4)	   += vgg2432a4.o
 obj-$(CONFIG_LCD_TDO24M)	   += tdo24m.o
 obj-$(CONFIG_LCD_TOSA)		   += tosa_lcd.o
 obj-$(CONFIG_LCD_S6E63M0)	+= s6e63m0.o
+obj-$(CONFIG_LCD_LD9040)	+= ld9040.o
+obj-$(CONFIG_LCD_AMS369FG06)	+= ams369fg06.o
 
 obj-$(CONFIG_BACKLIGHT_CLASS_DEVICE) += backlight.o
 obj-$(CONFIG_BACKLIGHT_ATMEL_PWM)    += atmel-pwm-bl.o
@@ -26,13 +28,15 @@ obj-$(CONFIG_BACKLIGHT_CARILLO_RANCH) += cr_bllcd.o
 obj-$(CONFIG_BACKLIGHT_PWM)	+= pwm_bl.o
 obj-$(CONFIG_BACKLIGHT_DA903X)	+= da903x_bl.o
 obj-$(CONFIG_BACKLIGHT_MAX8925)	+= max8925_bl.o
-obj-$(CONFIG_BACKLIGHT_MBP_NVIDIA) += mbp_nvidia_bl.o
+obj-$(CONFIG_BACKLIGHT_APPLE)	+= apple_bl.o
 obj-$(CONFIG_BACKLIGHT_TOSA)	+= tosa_bl.o
 obj-$(CONFIG_BACKLIGHT_SAHARA)	+= kb3886_bl.o
 obj-$(CONFIG_BACKLIGHT_WM831X)	+= wm831x_bl.o
 obj-$(CONFIG_BACKLIGHT_ADX)    += adx_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP5520)	+= adp5520_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP8860)	+= adp8860_bl.o
+obj-$(CONFIG_BACKLIGHT_ADP8870)	+= adp8870_bl.o
 obj-$(CONFIG_BACKLIGHT_88PM860X) += 88pm860x_bl.o
 obj-$(CONFIG_BACKLIGHT_PCF50633)	+= pcf50633-backlight.o
+obj-$(CONFIG_BACKLIGHT_AAT2870) += aat2870_bl.o
 
diff --git a/drivers/video/backlight/aat2870_bl.c b/drivers/video/backlight/aat2870_bl.c
new file mode 100644
index 000000000000..331f1ef1dad5
--- /dev/null
+++ b/drivers/video/backlight/aat2870_bl.c
@@ -0,0 +1,246 @@
+/*
+ * linux/drivers/video/backlight/aat2870_bl.c
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ * Author: Jin Park <jinyoungp@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/mfd/aat2870.h>
+
+struct aat2870_bl_driver_data {
+	struct platform_device *pdev;
+	struct backlight_device *bd;
+
+	int channels;
+	int max_current;
+	int brightness; /* current brightness */
+};
+
+static inline int aat2870_brightness(struct aat2870_bl_driver_data *aat2870_bl,
+				     int brightness)
+{
+	struct backlight_device *bd = aat2870_bl->bd;
+	int val;
+
+	val = brightness * (aat2870_bl->max_current - 1);
+	val /= bd->props.max_brightness;
+
+	return val;
+}
+
+static inline int aat2870_bl_enable(struct aat2870_bl_driver_data *aat2870_bl)
+{
+	struct aat2870_data *aat2870
+			= dev_get_drvdata(aat2870_bl->pdev->dev.parent);
+
+	return aat2870->write(aat2870, AAT2870_BL_CH_EN,
+			      (u8)aat2870_bl->channels);
+}
+
+static inline int aat2870_bl_disable(struct aat2870_bl_driver_data *aat2870_bl)
+{
+	struct aat2870_data *aat2870
+			= dev_get_drvdata(aat2870_bl->pdev->dev.parent);
+
+	return aat2870->write(aat2870, AAT2870_BL_CH_EN, 0x0);
+}
+
+static int aat2870_bl_get_brightness(struct backlight_device *bd)
+{
+	return bd->props.brightness;
+}
+
+static int aat2870_bl_update_status(struct backlight_device *bd)
+{
+	struct aat2870_bl_driver_data *aat2870_bl = dev_get_drvdata(&bd->dev);
+	struct aat2870_data *aat2870 =
+			dev_get_drvdata(aat2870_bl->pdev->dev.parent);
+	int brightness = bd->props.brightness;
+	int ret;
+
+	if ((brightness < 0) || (bd->props.max_brightness < brightness)) {
+		dev_err(&bd->dev, "invalid brightness, %d\n", brightness);
+		return -EINVAL;
+	}
+
+	dev_dbg(&bd->dev, "brightness=%d, power=%d, state=%d\n",
+		 bd->props.brightness, bd->props.power, bd->props.state);
+
+	if ((bd->props.power != FB_BLANK_UNBLANK) ||
+			(bd->props.state & BL_CORE_FBBLANK) ||
+			(bd->props.state & BL_CORE_SUSPENDED))
+		brightness = 0;
+
+	ret = aat2870->write(aat2870, AAT2870_BLM,
+			     (u8)aat2870_brightness(aat2870_bl, brightness));
+	if (ret < 0)
+		return ret;
+
+	if (brightness == 0) {
+		ret = aat2870_bl_disable(aat2870_bl);
+		if (ret < 0)
+			return ret;
+	} else if (aat2870_bl->brightness == 0) {
+		ret = aat2870_bl_enable(aat2870_bl);
+		if (ret < 0)
+			return ret;
+	}
+
+	aat2870_bl->brightness = brightness;
+
+	return 0;
+}
+
+static int aat2870_bl_check_fb(struct backlight_device *bd, struct fb_info *fi)
+{
+	return 1;
+}
+
+static const struct backlight_ops aat2870_bl_ops = {
+	.options = BL_CORE_SUSPENDRESUME,
+	.get_brightness = aat2870_bl_get_brightness,
+	.update_status = aat2870_bl_update_status,
+	.check_fb = aat2870_bl_check_fb,
+};
+
+static int aat2870_bl_probe(struct platform_device *pdev)
+{
+	struct aat2870_bl_platform_data *pdata = pdev->dev.platform_data;
+	struct aat2870_bl_driver_data *aat2870_bl;
+	struct backlight_device *bd;
+	struct backlight_properties props;
+	int ret = 0;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data\n");
+		ret = -ENXIO;
+		goto out;
+	}
+
+	if (pdev->id != AAT2870_ID_BL) {
+		dev_err(&pdev->dev, "Invalid device ID, %d\n", pdev->id);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	aat2870_bl = kzalloc(sizeof(struct aat2870_bl_driver_data), GFP_KERNEL);
+	if (!aat2870_bl) {
+		dev_err(&pdev->dev,
+			"Failed to allocate memory for aat2870 backlight\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+
+	props.type = BACKLIGHT_RAW;
+	bd = backlight_device_register("aat2870-backlight", &pdev->dev,
+				       aat2870_bl, &aat2870_bl_ops, &props);
+	if (IS_ERR(bd)) {
+		dev_err(&pdev->dev,
+			"Failed allocate memory for backlight device\n");
+		ret = PTR_ERR(bd);
+		goto out_kfree;
+	}
+
+	aat2870_bl->pdev = pdev;
+	platform_set_drvdata(pdev, aat2870_bl);
+
+	aat2870_bl->bd = bd;
+
+	if (pdata->channels > 0)
+		aat2870_bl->channels = pdata->channels;
+	else
+		aat2870_bl->channels = AAT2870_BL_CH_ALL;
+
+	if (pdata->max_current > 0)
+		aat2870_bl->max_current = pdata->max_current;
+	else
+		aat2870_bl->max_current = AAT2870_CURRENT_27_9;
+
+	if (pdata->max_brightness > 0)
+		bd->props.max_brightness = pdata->max_brightness;
+	else
+		bd->props.max_brightness = 255;
+
+	aat2870_bl->brightness = 0;
+	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.brightness = bd->props.max_brightness;
+
+	ret = aat2870_bl_update_status(bd);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to initialize\n");
+		goto out_bl_dev_unregister;
+	}
+
+	return 0;
+
+out_bl_dev_unregister:
+	backlight_device_unregister(bd);
+out_kfree:
+	kfree(aat2870_bl);
+out:
+	return ret;
+}
+
+static int aat2870_bl_remove(struct platform_device *pdev)
+{
+	struct aat2870_bl_driver_data *aat2870_bl = platform_get_drvdata(pdev);
+	struct backlight_device *bd = aat2870_bl->bd;
+
+	bd->props.power = FB_BLANK_POWERDOWN;
+	bd->props.brightness = 0;
+	backlight_update_status(bd);
+
+	backlight_device_unregister(bd);
+	kfree(aat2870_bl);
+
+	return 0;
+}
+
+static struct platform_driver aat2870_bl_driver = {
+	.driver = {
+		.name	= "aat2870-backlight",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= aat2870_bl_probe,
+	.remove		= aat2870_bl_remove,
+};
+
+static int __init aat2870_bl_init(void)
+{
+	return platform_driver_register(&aat2870_bl_driver);
+}
+subsys_initcall(aat2870_bl_init);
+
+static void __exit aat2870_bl_exit(void)
+{
+	platform_driver_unregister(&aat2870_bl_driver);
+}
+module_exit(aat2870_bl_exit);
+
+MODULE_DESCRIPTION("AnalogicTech AAT2870 Backlight");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jin Park <jinyoungp@nvidia.com>");
diff --git a/drivers/video/backlight/adp5520_bl.c b/drivers/video/backlight/adp5520_bl.c
index 9f436e014f85..d1aee730d7d8 100644
--- a/drivers/video/backlight/adp5520_bl.c
+++ b/drivers/video/backlight/adp5520_bl.c
@@ -211,8 +211,12 @@ static ssize_t adp5520_bl_daylight_max_store(struct device *dev,
 			const char *buf, size_t count)
 {
 	struct adp5520_bl *data = dev_get_drvdata(dev);
+	int ret;
+
+	ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+	if (ret < 0)
+		return ret;
 
-	strict_strtoul(buf, 10, &data->cached_daylight_max);
 	return adp5520_store(dev, buf, count, ADP5520_DAYLIGHT_MAX);
 }
 static DEVICE_ATTR(daylight_max, 0664, adp5520_bl_daylight_max_show,
@@ -303,6 +307,7 @@ static int __devinit adp5520_bl_probe(struct platform_device *pdev)
 	mutex_init(&data->lock);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = ADP5020_MAX_BRIGHTNESS;
 	bl = backlight_device_register(pdev->name, data->master, data,
 				       &adp5520_bl_ops, &props);
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
index 734c650a47c4..183b6f639852 100644
--- a/drivers/video/backlight/adp8860_bl.c
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -709,6 +709,7 @@ static int __devinit adp8860_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, data);
 
 	memset(&props, 0, sizeof(props));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = ADP8860_MAX_BRIGHTNESS;
 
 	mutex_init(&data->lock);
@@ -721,8 +722,7 @@ static int __devinit adp8860_probe(struct i2c_client *client,
 		goto out2;
 	}
 
-	bl->props.max_brightness =
-		bl->props.brightness = ADP8860_MAX_BRIGHTNESS;
+	bl->props.brightness = ADP8860_MAX_BRIGHTNESS;
 
 	data->bl = bl;
 
diff --git a/drivers/video/backlight/adp8870_bl.c b/drivers/video/backlight/adp8870_bl.c
new file mode 100644
index 000000000000..05a8832bb3eb
--- /dev/null
+++ b/drivers/video/backlight/adp8870_bl.c
@@ -0,0 +1,1012 @@
+/*
+ * Backlight driver for Analog Devices ADP8870 Backlight Devices
+ *
+ * Copyright 2009-2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/leds.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+
+#include <linux/i2c/adp8870.h>
+#define ADP8870_EXT_FEATURES
+#define ADP8870_USE_LEDS
+
+
+#define ADP8870_MFDVID	0x00  /* Manufacturer and device ID */
+#define ADP8870_MDCR	0x01  /* Device mode and status */
+#define ADP8870_INT_STAT 0x02  /* Interrupts status */
+#define ADP8870_INT_EN	0x03  /* Interrupts enable */
+#define ADP8870_CFGR	0x04  /* Configuration register */
+#define ADP8870_BLSEL	0x05  /* Sink enable backlight or independent */
+#define ADP8870_PWMLED	0x06  /* PWM Enable Selection Register */
+#define ADP8870_BLOFF	0x07  /* Backlight off timeout */
+#define ADP8870_BLDIM	0x08  /* Backlight dim timeout */
+#define ADP8870_BLFR	0x09  /* Backlight fade in and out rates */
+#define ADP8870_BLMX1	0x0A  /* Backlight (Brightness Level 1-daylight) maximum current */
+#define ADP8870_BLDM1	0x0B  /* Backlight (Brightness Level 1-daylight) dim current */
+#define ADP8870_BLMX2	0x0C  /* Backlight (Brightness Level 2-bright) maximum current */
+#define ADP8870_BLDM2	0x0D  /* Backlight (Brightness Level 2-bright) dim current */
+#define ADP8870_BLMX3	0x0E  /* Backlight (Brightness Level 3-office) maximum current */
+#define ADP8870_BLDM3	0x0F  /* Backlight (Brightness Level 3-office) dim current */
+#define ADP8870_BLMX4	0x10  /* Backlight (Brightness Level 4-indoor) maximum current */
+#define ADP8870_BLDM4	0x11  /* Backlight (Brightness Level 4-indoor) dim current */
+#define ADP8870_BLMX5	0x12  /* Backlight (Brightness Level 5-dark) maximum current */
+#define ADP8870_BLDM5	0x13  /* Backlight (Brightness Level 5-dark) dim current */
+#define ADP8870_ISCLAW	0x1A  /* Independent sink current fade law register */
+#define ADP8870_ISCC	0x1B  /* Independent sink current control register */
+#define ADP8870_ISCT1	0x1C  /* Independent Sink Current Timer Register LED[7:5] */
+#define ADP8870_ISCT2	0x1D  /* Independent Sink Current Timer Register LED[4:1] */
+#define ADP8870_ISCF	0x1E  /* Independent sink current fade register */
+#define ADP8870_ISC1	0x1F  /* Independent Sink Current LED1 */
+#define ADP8870_ISC2	0x20  /* Independent Sink Current LED2 */
+#define ADP8870_ISC3	0x21  /* Independent Sink Current LED3 */
+#define ADP8870_ISC4	0x22  /* Independent Sink Current LED4 */
+#define ADP8870_ISC5	0x23  /* Independent Sink Current LED5 */
+#define ADP8870_ISC6	0x24  /* Independent Sink Current LED6 */
+#define ADP8870_ISC7	0x25  /* Independent Sink Current LED7 (Brightness Level 1-daylight) */
+#define ADP8870_ISC7_L2	0x26  /* Independent Sink Current LED7 (Brightness Level 2-bright) */
+#define ADP8870_ISC7_L3	0x27  /* Independent Sink Current LED7 (Brightness Level 3-office) */
+#define ADP8870_ISC7_L4	0x28  /* Independent Sink Current LED7 (Brightness Level 4-indoor) */
+#define ADP8870_ISC7_L5	0x29  /* Independent Sink Current LED7 (Brightness Level 5-dark) */
+#define ADP8870_CMP_CTL	0x2D  /* ALS Comparator Control Register */
+#define ADP8870_ALS1_EN	0x2E  /* Main ALS comparator level enable */
+#define ADP8870_ALS2_EN	0x2F  /* Second ALS comparator level enable */
+#define ADP8870_ALS1_STAT 0x30  /* Main ALS Comparator Status Register */
+#define ADP8870_ALS2_STAT 0x31  /* Second ALS Comparator Status Register */
+#define ADP8870_L2TRP	0x32  /* L2 comparator reference */
+#define ADP8870_L2HYS	0x33  /* L2 hysteresis */
+#define ADP8870_L3TRP	0x34  /* L3 comparator reference */
+#define ADP8870_L3HYS	0x35  /* L3 hysteresis */
+#define ADP8870_L4TRP	0x36  /* L4 comparator reference */
+#define ADP8870_L4HYS	0x37  /* L4 hysteresis */
+#define ADP8870_L5TRP	0x38  /* L5 comparator reference */
+#define ADP8870_L5HYS	0x39  /* L5 hysteresis */
+#define ADP8870_PH1LEVL	0x40  /* First phototransistor ambient light level-low byte register */
+#define ADP8870_PH1LEVH	0x41  /* First phototransistor ambient light level-high byte register */
+#define ADP8870_PH2LEVL	0x42  /* Second phototransistor ambient light level-low byte register */
+#define ADP8870_PH2LEVH	0x43  /* Second phototransistor ambient light level-high byte register */
+
+#define ADP8870_MANUFID		0x3  /* Analog Devices AD8870 Manufacturer and device ID */
+#define ADP8870_DEVID(x)	((x) & 0xF)
+#define ADP8870_MANID(x)	((x) >> 4)
+
+/* MDCR Device mode and status */
+#define D7ALSEN			(1 << 7)
+#define INT_CFG			(1 << 6)
+#define NSTBY			(1 << 5)
+#define DIM_EN			(1 << 4)
+#define GDWN_DIS		(1 << 3)
+#define SIS_EN			(1 << 2)
+#define CMP_AUTOEN		(1 << 1)
+#define BLEN			(1 << 0)
+
+/* ADP8870_ALS1_EN Main ALS comparator level enable */
+#define L5_EN			(1 << 3)
+#define L4_EN			(1 << 2)
+#define L3_EN			(1 << 1)
+#define L2_EN			(1 << 0)
+
+#define CFGR_BLV_SHIFT		3
+#define CFGR_BLV_MASK		0x7
+#define ADP8870_FLAG_LED_MASK	0xFF
+
+#define FADE_VAL(in, out)	((0xF & (in)) | ((0xF & (out)) << 4))
+#define BL_CFGR_VAL(law, blv)	((((blv) & CFGR_BLV_MASK) << CFGR_BLV_SHIFT) | ((0x3 & (law)) << 1))
+#define ALS_CMPR_CFG_VAL(filt)	((0x7 & (filt)) << 1)
+
+struct adp8870_bl {
+	struct i2c_client *client;
+	struct backlight_device *bl;
+	struct adp8870_led *led;
+	struct adp8870_backlight_platform_data *pdata;
+	struct mutex lock;
+	unsigned long cached_daylight_max;
+	int id;
+	int revid;
+	int current_brightness;
+};
+
+struct adp8870_led {
+	struct led_classdev	cdev;
+	struct work_struct	work;
+	struct i2c_client	*client;
+	enum led_brightness	new_brightness;
+	int			id;
+	int			flags;
+};
+
+static int adp8870_read(struct i2c_client *client, int reg, uint8_t *val)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, reg);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed reading at 0x%02x\n", reg);
+		return ret;
+	}
+
+	*val = ret;
+	return 0;
+}
+
+
+static int adp8870_write(struct i2c_client *client, u8 reg, u8 val)
+{
+	int ret = i2c_smbus_write_byte_data(client, reg, val);
+	if (ret)
+		dev_err(&client->dev, "failed to write\n");
+
+	return ret;
+}
+
+static int adp8870_set_bits(struct i2c_client *client, int reg, uint8_t bit_mask)
+{
+	struct adp8870_bl *data = i2c_get_clientdata(client);
+	uint8_t reg_val;
+	int ret;
+
+	mutex_lock(&data->lock);
+
+	ret = adp8870_read(client, reg, &reg_val);
+
+	if (!ret && ((reg_val & bit_mask) == 0)) {
+		reg_val |= bit_mask;
+		ret = adp8870_write(client, reg, reg_val);
+	}
+
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int adp8870_clr_bits(struct i2c_client *client, int reg, uint8_t bit_mask)
+{
+	struct adp8870_bl *data = i2c_get_clientdata(client);
+	uint8_t reg_val;
+	int ret;
+
+	mutex_lock(&data->lock);
+
+	ret = adp8870_read(client, reg, &reg_val);
+
+	if (!ret && (reg_val & bit_mask)) {
+		reg_val &= ~bit_mask;
+		ret = adp8870_write(client, reg, reg_val);
+	}
+
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+/*
+ * Independent sink / LED
+ */
+#if defined(ADP8870_USE_LEDS)
+static void adp8870_led_work(struct work_struct *work)
+{
+	struct adp8870_led *led = container_of(work, struct adp8870_led, work);
+	adp8870_write(led->client, ADP8870_ISC1 + led->id - 1,
+			 led->new_brightness >> 1);
+}
+
+static void adp8870_led_set(struct led_classdev *led_cdev,
+			   enum led_brightness value)
+{
+	struct adp8870_led *led;
+
+	led = container_of(led_cdev, struct adp8870_led, cdev);
+	led->new_brightness = value;
+	/*
+	 * Use workqueue for IO since I2C operations can sleep.
+	 */
+	schedule_work(&led->work);
+}
+
+static int adp8870_led_setup(struct adp8870_led *led)
+{
+	struct i2c_client *client = led->client;
+	int ret = 0;
+
+	ret = adp8870_write(client, ADP8870_ISC1 + led->id - 1, 0);
+	if (ret)
+		return ret;
+
+	ret = adp8870_set_bits(client, ADP8870_ISCC, 1 << (led->id - 1));
+	if (ret)
+		return ret;
+
+	if (led->id > 4)
+		ret = adp8870_set_bits(client, ADP8870_ISCT1,
+				(led->flags & 0x3) << ((led->id - 5) * 2));
+	else
+		ret = adp8870_set_bits(client, ADP8870_ISCT2,
+				(led->flags & 0x3) << ((led->id - 1) * 2));
+
+	return ret;
+}
+
+static int __devinit adp8870_led_probe(struct i2c_client *client)
+{
+	struct adp8870_backlight_platform_data *pdata =
+		client->dev.platform_data;
+	struct adp8870_bl *data = i2c_get_clientdata(client);
+	struct adp8870_led *led, *led_dat;
+	struct led_info *cur_led;
+	int ret, i;
+
+
+	led = kcalloc(pdata->num_leds, sizeof(*led), GFP_KERNEL);
+	if (led == NULL) {
+		dev_err(&client->dev, "failed to alloc memory\n");
+		return -ENOMEM;
+	}
+
+	ret = adp8870_write(client, ADP8870_ISCLAW, pdata->led_fade_law);
+	if (ret)
+		goto err_free;
+
+	ret = adp8870_write(client, ADP8870_ISCT1,
+			(pdata->led_on_time & 0x3) << 6);
+	if (ret)
+		goto err_free;
+
+	ret = adp8870_write(client, ADP8870_ISCF,
+			FADE_VAL(pdata->led_fade_in, pdata->led_fade_out));
+	if (ret)
+		goto err_free;
+
+	for (i = 0; i < pdata->num_leds; ++i) {
+		cur_led = &pdata->leds[i];
+		led_dat = &led[i];
+
+		led_dat->id = cur_led->flags & ADP8870_FLAG_LED_MASK;
+
+		if (led_dat->id > 7 || led_dat->id < 1) {
+			dev_err(&client->dev, "Invalid LED ID %d\n",
+				led_dat->id);
+			goto err;
+		}
+
+		if (pdata->bl_led_assign & (1 << (led_dat->id - 1))) {
+			dev_err(&client->dev, "LED %d used by Backlight\n",
+				led_dat->id);
+			goto err;
+		}
+
+		led_dat->cdev.name = cur_led->name;
+		led_dat->cdev.default_trigger = cur_led->default_trigger;
+		led_dat->cdev.brightness_set = adp8870_led_set;
+		led_dat->cdev.brightness = LED_OFF;
+		led_dat->flags = cur_led->flags >> FLAG_OFFT_SHIFT;
+		led_dat->client = client;
+		led_dat->new_brightness = LED_OFF;
+		INIT_WORK(&led_dat->work, adp8870_led_work);
+
+		ret = led_classdev_register(&client->dev, &led_dat->cdev);
+		if (ret) {
+			dev_err(&client->dev, "failed to register LED %d\n",
+				led_dat->id);
+			goto err;
+		}
+
+		ret = adp8870_led_setup(led_dat);
+		if (ret) {
+			dev_err(&client->dev, "failed to write\n");
+			i++;
+			goto err;
+		}
+	}
+
+	data->led = led;
+
+	return 0;
+
+ err:
+	for (i = i - 1; i >= 0; --i) {
+		led_classdev_unregister(&led[i].cdev);
+		cancel_work_sync(&led[i].work);
+	}
+
+ err_free:
+	kfree(led);
+
+	return ret;
+}
+
+static int __devexit adp8870_led_remove(struct i2c_client *client)
+{
+	struct adp8870_backlight_platform_data *pdata =
+		client->dev.platform_data;
+	struct adp8870_bl *data = i2c_get_clientdata(client);
+	int i;
+
+	for (i = 0; i < pdata->num_leds; i++) {
+		led_classdev_unregister(&data->led[i].cdev);
+		cancel_work_sync(&data->led[i].work);
+	}
+
+	kfree(data->led);
+	return 0;
+}
+#else
+static int __devinit adp8870_led_probe(struct i2c_client *client)
+{
+	return 0;
+}
+
+static int __devexit adp8870_led_remove(struct i2c_client *client)
+{
+	return 0;
+}
+#endif
+
+static int adp8870_bl_set(struct backlight_device *bl, int brightness)
+{
+	struct adp8870_bl *data = bl_get_data(bl);
+	struct i2c_client *client = data->client;
+	int ret = 0;
+
+	if (data->pdata->en_ambl_sens) {
+		if ((brightness > 0) && (brightness < ADP8870_MAX_BRIGHTNESS)) {
+			/* Disable Ambient Light auto adjust */
+			ret = adp8870_clr_bits(client, ADP8870_MDCR,
+					CMP_AUTOEN);
+			if (ret)
+				return ret;
+			ret = adp8870_write(client, ADP8870_BLMX1, brightness);
+			if (ret)
+				return ret;
+		} else {
+			/*
+			 * MAX_BRIGHTNESS -> Enable Ambient Light auto adjust
+			 * restore daylight l1 sysfs brightness
+			 */
+			ret = adp8870_write(client, ADP8870_BLMX1,
+					 data->cached_daylight_max);
+			if (ret)
+				return ret;
+
+			ret = adp8870_set_bits(client, ADP8870_MDCR,
+					 CMP_AUTOEN);
+			if (ret)
+				return ret;
+		}
+	} else {
+		ret = adp8870_write(client, ADP8870_BLMX1, brightness);
+		if (ret)
+			return ret;
+	}
+
+	if (data->current_brightness && brightness == 0)
+		ret = adp8870_set_bits(client,
+				ADP8870_MDCR, DIM_EN);
+	else if (data->current_brightness == 0 && brightness)
+		ret = adp8870_clr_bits(client,
+				ADP8870_MDCR, DIM_EN);
+
+	if (!ret)
+		data->current_brightness = brightness;
+
+	return ret;
+}
+
+static int adp8870_bl_update_status(struct backlight_device *bl)
+{
+	int brightness = bl->props.brightness;
+	if (bl->props.power != FB_BLANK_UNBLANK)
+		brightness = 0;
+
+	if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+		brightness = 0;
+
+	return adp8870_bl_set(bl, brightness);
+}
+
+static int adp8870_bl_get_brightness(struct backlight_device *bl)
+{
+	struct adp8870_bl *data = bl_get_data(bl);
+
+	return data->current_brightness;
+}
+
+static const struct backlight_ops adp8870_bl_ops = {
+	.update_status	= adp8870_bl_update_status,
+	.get_brightness	= adp8870_bl_get_brightness,
+};
+
+static int adp8870_bl_setup(struct backlight_device *bl)
+{
+	struct adp8870_bl *data = bl_get_data(bl);
+	struct i2c_client *client = data->client;
+	struct adp8870_backlight_platform_data *pdata = data->pdata;
+	int ret = 0;
+
+	ret = adp8870_write(client, ADP8870_BLSEL, ~pdata->bl_led_assign);
+	if (ret)
+		return ret;
+
+	ret = adp8870_write(client, ADP8870_PWMLED, pdata->pwm_assign);
+	if (ret)
+		return ret;
+
+	ret = adp8870_write(client, ADP8870_BLMX1, pdata->l1_daylight_max);
+	if (ret)
+		return ret;
+
+	ret = adp8870_write(client, ADP8870_BLDM1, pdata->l1_daylight_dim);
+	if (ret)
+		return ret;
+
+	if (pdata->en_ambl_sens) {
+		data->cached_daylight_max = pdata->l1_daylight_max;
+		ret = adp8870_write(client, ADP8870_BLMX2,
+						pdata->l2_bright_max);
+		if (ret)
+			return ret;
+		ret = adp8870_write(client, ADP8870_BLDM2,
+						pdata->l2_bright_dim);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_BLMX3,
+						pdata->l3_office_max);
+		if (ret)
+			return ret;
+		ret = adp8870_write(client, ADP8870_BLDM3,
+						pdata->l3_office_dim);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_BLMX4,
+						pdata->l4_indoor_max);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_BLDM4,
+						pdata->l4_indor_dim);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_BLMX5,
+						pdata->l5_dark_max);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_BLDM5,
+						pdata->l5_dark_dim);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L2TRP, pdata->l2_trip);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L2HYS, pdata->l2_hyst);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L3TRP, pdata->l3_trip);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L3HYS, pdata->l3_hyst);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L4TRP, pdata->l4_trip);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L4HYS, pdata->l4_hyst);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L5TRP, pdata->l5_trip);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_L5HYS, pdata->l5_hyst);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_ALS1_EN, L5_EN | L4_EN |
+						L3_EN | L2_EN);
+		if (ret)
+			return ret;
+
+		ret = adp8870_write(client, ADP8870_CMP_CTL,
+			ALS_CMPR_CFG_VAL(pdata->abml_filt));
+		if (ret)
+			return ret;
+	}
+
+	ret = adp8870_write(client, ADP8870_CFGR,
+			BL_CFGR_VAL(pdata->bl_fade_law, 0));
+	if (ret)
+		return ret;
+
+	ret = adp8870_write(client, ADP8870_BLFR, FADE_VAL(pdata->bl_fade_in,
+			pdata->bl_fade_out));
+	if (ret)
+		return ret;
+	/*
+	 * ADP8870 Rev0 requires GDWN_DIS bit set
+	 */
+
+	ret = adp8870_set_bits(client, ADP8870_MDCR, BLEN | DIM_EN | NSTBY |
+			(data->revid == 0 ? GDWN_DIS : 0));
+
+	return ret;
+}
+
+static ssize_t adp8870_show(struct device *dev, char *buf, int reg)
+{
+	struct adp8870_bl *data = dev_get_drvdata(dev);
+	int error;
+	uint8_t reg_val;
+
+	mutex_lock(&data->lock);
+	error = adp8870_read(data->client, reg, &reg_val);
+	mutex_unlock(&data->lock);
+
+	if (error < 0)
+		return error;
+
+	return sprintf(buf, "%u\n", reg_val);
+}
+
+static ssize_t adp8870_store(struct device *dev, const char *buf,
+			 size_t count, int reg)
+{
+	struct adp8870_bl *data = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	ret = strict_strtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	mutex_lock(&data->lock);
+	adp8870_write(data->client, reg, val);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t adp8870_bl_l5_dark_max_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLMX5);
+}
+
+static ssize_t adp8870_bl_l5_dark_max_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLMX5);
+}
+static DEVICE_ATTR(l5_dark_max, 0664, adp8870_bl_l5_dark_max_show,
+			adp8870_bl_l5_dark_max_store);
+
+
+static ssize_t adp8870_bl_l4_indoor_max_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLMX4);
+}
+
+static ssize_t adp8870_bl_l4_indoor_max_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLMX4);
+}
+static DEVICE_ATTR(l4_indoor_max, 0664, adp8870_bl_l4_indoor_max_show,
+			adp8870_bl_l4_indoor_max_store);
+
+
+static ssize_t adp8870_bl_l3_office_max_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLMX3);
+}
+
+static ssize_t adp8870_bl_l3_office_max_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLMX3);
+}
+
+static DEVICE_ATTR(l3_office_max, 0664, adp8870_bl_l3_office_max_show,
+			adp8870_bl_l3_office_max_store);
+
+static ssize_t adp8870_bl_l2_bright_max_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLMX2);
+}
+
+static ssize_t adp8870_bl_l2_bright_max_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLMX2);
+}
+static DEVICE_ATTR(l2_bright_max, 0664, adp8870_bl_l2_bright_max_show,
+			adp8870_bl_l2_bright_max_store);
+
+static ssize_t adp8870_bl_l1_daylight_max_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLMX1);
+}
+
+static ssize_t adp8870_bl_l1_daylight_max_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct adp8870_bl *data = dev_get_drvdata(dev);
+	int ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+	if (ret)
+		return ret;
+
+	return adp8870_store(dev, buf, count, ADP8870_BLMX1);
+}
+static DEVICE_ATTR(l1_daylight_max, 0664, adp8870_bl_l1_daylight_max_show,
+			adp8870_bl_l1_daylight_max_store);
+
+static ssize_t adp8870_bl_l5_dark_dim_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLDM5);
+}
+
+static ssize_t adp8870_bl_l5_dark_dim_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLDM5);
+}
+static DEVICE_ATTR(l5_dark_dim, 0664, adp8870_bl_l5_dark_dim_show,
+			adp8870_bl_l5_dark_dim_store);
+
+static ssize_t adp8870_bl_l4_indoor_dim_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLDM4);
+}
+
+static ssize_t adp8870_bl_l4_indoor_dim_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLDM4);
+}
+static DEVICE_ATTR(l4_indoor_dim, 0664, adp8870_bl_l4_indoor_dim_show,
+			adp8870_bl_l4_indoor_dim_store);
+
+
+static ssize_t adp8870_bl_l3_office_dim_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLDM3);
+}
+
+static ssize_t adp8870_bl_l3_office_dim_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLDM3);
+}
+static DEVICE_ATTR(l3_office_dim, 0664, adp8870_bl_l3_office_dim_show,
+			adp8870_bl_l3_office_dim_store);
+
+static ssize_t adp8870_bl_l2_bright_dim_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLDM2);
+}
+
+static ssize_t adp8870_bl_l2_bright_dim_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLDM2);
+}
+static DEVICE_ATTR(l2_bright_dim, 0664, adp8870_bl_l2_bright_dim_show,
+			adp8870_bl_l2_bright_dim_store);
+
+static ssize_t adp8870_bl_l1_daylight_dim_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	return adp8870_show(dev, buf, ADP8870_BLDM1);
+}
+
+static ssize_t adp8870_bl_l1_daylight_dim_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	return adp8870_store(dev, buf, count, ADP8870_BLDM1);
+}
+static DEVICE_ATTR(l1_daylight_dim, 0664, adp8870_bl_l1_daylight_dim_show,
+			adp8870_bl_l1_daylight_dim_store);
+
+#ifdef ADP8870_EXT_FEATURES
+static ssize_t adp8870_bl_ambient_light_level_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct adp8870_bl *data = dev_get_drvdata(dev);
+	int error;
+	uint8_t reg_val;
+	uint16_t ret_val;
+
+	mutex_lock(&data->lock);
+	error = adp8870_read(data->client, ADP8870_PH1LEVL, &reg_val);
+	if (error < 0) {
+		mutex_unlock(&data->lock);
+		return error;
+	}
+	ret_val = reg_val;
+	error = adp8870_read(data->client, ADP8870_PH1LEVH, &reg_val);
+	mutex_unlock(&data->lock);
+
+	if (error < 0)
+		return error;
+
+	/* Return 13-bit conversion value for the first light sensor */
+	ret_val += (reg_val & 0x1F) << 8;
+
+	return sprintf(buf, "%u\n", ret_val);
+}
+static DEVICE_ATTR(ambient_light_level, 0444,
+		adp8870_bl_ambient_light_level_show, NULL);
+
+static ssize_t adp8870_bl_ambient_light_zone_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct adp8870_bl *data = dev_get_drvdata(dev);
+	int error;
+	uint8_t reg_val;
+
+	mutex_lock(&data->lock);
+	error = adp8870_read(data->client, ADP8870_CFGR, &reg_val);
+	mutex_unlock(&data->lock);
+
+	if (error < 0)
+		return error;
+
+	return sprintf(buf, "%u\n",
+		((reg_val >> CFGR_BLV_SHIFT) & CFGR_BLV_MASK) + 1);
+}
+
+static ssize_t adp8870_bl_ambient_light_zone_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct adp8870_bl *data = dev_get_drvdata(dev);
+	unsigned long val;
+	uint8_t reg_val;
+	int ret;
+
+	ret = strict_strtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (val == 0) {
+		/* Enable automatic ambient light sensing */
+		adp8870_set_bits(data->client, ADP8870_MDCR, CMP_AUTOEN);
+	} else if ((val > 0) && (val < 6)) {
+		/* Disable automatic ambient light sensing */
+		adp8870_clr_bits(data->client, ADP8870_MDCR, CMP_AUTOEN);
+
+		/* Set user supplied ambient light zone */
+		mutex_lock(&data->lock);
+		adp8870_read(data->client, ADP8870_CFGR, &reg_val);
+		reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
+		reg_val |= (val - 1) << CFGR_BLV_SHIFT;
+		adp8870_write(data->client, ADP8870_CFGR, reg_val);
+		mutex_unlock(&data->lock);
+	}
+
+	return count;
+}
+static DEVICE_ATTR(ambient_light_zone, 0664,
+		adp8870_bl_ambient_light_zone_show,
+		adp8870_bl_ambient_light_zone_store);
+#endif
+
+static struct attribute *adp8870_bl_attributes[] = {
+	&dev_attr_l5_dark_max.attr,
+	&dev_attr_l5_dark_dim.attr,
+	&dev_attr_l4_indoor_max.attr,
+	&dev_attr_l4_indoor_dim.attr,
+	&dev_attr_l3_office_max.attr,
+	&dev_attr_l3_office_dim.attr,
+	&dev_attr_l2_bright_max.attr,
+	&dev_attr_l2_bright_dim.attr,
+	&dev_attr_l1_daylight_max.attr,
+	&dev_attr_l1_daylight_dim.attr,
+#ifdef ADP8870_EXT_FEATURES
+	&dev_attr_ambient_light_level.attr,
+	&dev_attr_ambient_light_zone.attr,
+#endif
+	NULL
+};
+
+static const struct attribute_group adp8870_bl_attr_group = {
+	.attrs = adp8870_bl_attributes,
+};
+
+static int __devinit adp8870_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct backlight_properties props;
+	struct backlight_device *bl;
+	struct adp8870_bl *data;
+	struct adp8870_backlight_platform_data *pdata =
+		client->dev.platform_data;
+	uint8_t reg_val;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter,
+					I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(&client->dev, "SMBUS Byte Data not Supported\n");
+		return -EIO;
+	}
+
+	if (!pdata) {
+		dev_err(&client->dev, "no platform data?\n");
+		return -EINVAL;
+	}
+
+	ret = adp8870_read(client, ADP8870_MFDVID, &reg_val);
+	if (ret < 0)
+		return -EIO;
+
+	if (ADP8870_MANID(reg_val) != ADP8870_MANUFID) {
+		dev_err(&client->dev, "failed to probe\n");
+		return -ENODEV;
+	}
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+
+	data->revid = ADP8870_DEVID(reg_val);
+	data->client = client;
+	data->pdata = pdata;
+	data->id = id->driver_data;
+	data->current_brightness = 0;
+	i2c_set_clientdata(client, data);
+
+	mutex_init(&data->lock);
+
+	memset(&props, 0, sizeof(props));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = props.brightness = ADP8870_MAX_BRIGHTNESS;
+	bl = backlight_device_register(dev_driver_string(&client->dev),
+			&client->dev, data, &adp8870_bl_ops, &props);
+	if (IS_ERR(bl)) {
+		dev_err(&client->dev, "failed to register backlight\n");
+		ret = PTR_ERR(bl);
+		goto out2;
+	}
+
+	data->bl = bl;
+
+	if (pdata->en_ambl_sens)
+		ret = sysfs_create_group(&bl->dev.kobj,
+			&adp8870_bl_attr_group);
+
+	if (ret) {
+		dev_err(&client->dev, "failed to register sysfs\n");
+		goto out1;
+	}
+
+	ret = adp8870_bl_setup(bl);
+	if (ret) {
+		ret = -EIO;
+		goto out;
+	}
+
+	backlight_update_status(bl);
+
+	dev_info(&client->dev, "Rev.%d Backlight\n", data->revid);
+
+	if (pdata->num_leds)
+		adp8870_led_probe(client);
+
+	return 0;
+
+out:
+	if (data->pdata->en_ambl_sens)
+		sysfs_remove_group(&data->bl->dev.kobj,
+			&adp8870_bl_attr_group);
+out1:
+	backlight_device_unregister(bl);
+out2:
+	i2c_set_clientdata(client, NULL);
+	kfree(data);
+
+	return ret;
+}
+
+static int __devexit adp8870_remove(struct i2c_client *client)
+{
+	struct adp8870_bl *data = i2c_get_clientdata(client);
+
+	adp8870_clr_bits(client, ADP8870_MDCR, NSTBY);
+
+	if (data->led)
+		adp8870_led_remove(client);
+
+	if (data->pdata->en_ambl_sens)
+		sysfs_remove_group(&data->bl->dev.kobj,
+			&adp8870_bl_attr_group);
+
+	backlight_device_unregister(data->bl);
+	i2c_set_clientdata(client, NULL);
+	kfree(data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int adp8870_i2c_suspend(struct i2c_client *client, pm_message_t message)
+{
+	adp8870_clr_bits(client, ADP8870_MDCR, NSTBY);
+
+	return 0;
+}
+
+static int adp8870_i2c_resume(struct i2c_client *client)
+{
+	adp8870_set_bits(client, ADP8870_MDCR, NSTBY);
+
+	return 0;
+}
+#else
+#define adp8870_i2c_suspend NULL
+#define adp8870_i2c_resume NULL
+#endif
+
+static const struct i2c_device_id adp8870_id[] = {
+	{ "adp8870", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adp8870_id);
+
+static struct i2c_driver adp8870_driver = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+	},
+	.probe    = adp8870_probe,
+	.remove   = __devexit_p(adp8870_remove),
+	.suspend = adp8870_i2c_suspend,
+	.resume  = adp8870_i2c_resume,
+	.id_table = adp8870_id,
+};
+
+static int __init adp8870_init(void)
+{
+	return i2c_add_driver(&adp8870_driver);
+}
+module_init(adp8870_init);
+
+static void __exit adp8870_exit(void)
+{
+	i2c_del_driver(&adp8870_driver);
+}
+module_exit(adp8870_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("ADP8870 Backlight driver");
+MODULE_ALIAS("platform:adp8870-backlight");
diff --git a/drivers/video/backlight/adx_bl.c b/drivers/video/backlight/adx_bl.c
index fe9af129c5dd..c861c41af442 100644
--- a/drivers/video/backlight/adx_bl.c
+++ b/drivers/video/backlight/adx_bl.c
@@ -104,6 +104,7 @@ static int __devinit adx_backlight_probe(struct platform_device *pdev)
 	}
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = 0xff;
 	bldev = backlight_device_register(dev_name(&pdev->dev), &pdev->dev,
 					  bl, &adx_backlight_ops, &props);
diff --git a/drivers/video/backlight/ams369fg06.c b/drivers/video/backlight/ams369fg06.c
new file mode 100644
index 000000000000..9f0a491e2a05
--- /dev/null
+++ b/drivers/video/backlight/ams369fg06.c
@@ -0,0 +1,646 @@
+/*
+ * ams369fg06 AMOLED LCD panel driver.
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Author: Jingoo Han  <jg1.han@samsung.com>
+ *
+ * Derived from drivers/video/s6e63m0.c
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ */
+
+#include <linux/wait.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/lcd.h>
+#include <linux/backlight.h>
+
+#define SLEEPMSEC		0x1000
+#define ENDDEF			0x2000
+#define	DEFMASK			0xFF00
+#define COMMAND_ONLY		0xFE
+#define DATA_ONLY		0xFF
+
+#define MAX_GAMMA_LEVEL		5
+#define GAMMA_TABLE_COUNT	21
+
+#define MIN_BRIGHTNESS		0
+#define MAX_BRIGHTNESS		255
+#define DEFAULT_BRIGHTNESS	150
+
+struct ams369fg06 {
+	struct device			*dev;
+	struct spi_device		*spi;
+	unsigned int			power;
+	struct lcd_device		*ld;
+	struct backlight_device		*bd;
+	struct lcd_platform_data	*lcd_pd;
+};
+
+static const unsigned short seq_display_on[] = {
+	0x14, 0x03,
+	ENDDEF, 0x0000
+};
+
+static const unsigned short seq_display_off[] = {
+	0x14, 0x00,
+	ENDDEF, 0x0000
+};
+
+static const unsigned short seq_stand_by_on[] = {
+	0x1D, 0xA1,
+	SLEEPMSEC, 200,
+	ENDDEF, 0x0000
+};
+
+static const unsigned short seq_stand_by_off[] = {
+	0x1D, 0xA0,
+	SLEEPMSEC, 250,
+	ENDDEF, 0x0000
+};
+
+static const unsigned short seq_setting[] = {
+	0x31, 0x08,
+	0x32, 0x14,
+	0x30, 0x02,
+	0x27, 0x01,
+	0x12, 0x08,
+	0x13, 0x08,
+	0x15, 0x00,
+	0x16, 0x00,
+
+	0xef, 0xd0,
+	DATA_ONLY, 0xe8,
+
+	0x39, 0x44,
+	0x40, 0x00,
+	0x41, 0x3f,
+	0x42, 0x2a,
+	0x43, 0x27,
+	0x44, 0x27,
+	0x45, 0x1f,
+	0x46, 0x44,
+	0x50, 0x00,
+	0x51, 0x00,
+	0x52, 0x17,
+	0x53, 0x24,
+	0x54, 0x26,
+	0x55, 0x1f,
+	0x56, 0x43,
+	0x60, 0x00,
+	0x61, 0x3f,
+	0x62, 0x2a,
+	0x63, 0x25,
+	0x64, 0x24,
+	0x65, 0x1b,
+	0x66, 0x5c,
+
+	0x17, 0x22,
+	0x18, 0x33,
+	0x19, 0x03,
+	0x1a, 0x01,
+	0x22, 0xa4,
+	0x23, 0x00,
+	0x26, 0xa0,
+
+	0x1d, 0xa0,
+	SLEEPMSEC, 300,
+
+	0x14, 0x03,
+
+	ENDDEF, 0x0000
+};
+
+/* gamma value: 2.2 */
+static const unsigned int ams369fg06_22_250[] = {
+	0x00, 0x3f, 0x2a, 0x27, 0x27, 0x1f, 0x44,
+	0x00, 0x00, 0x17, 0x24, 0x26, 0x1f, 0x43,
+	0x00, 0x3f, 0x2a, 0x25, 0x24, 0x1b, 0x5c,
+};
+
+static const unsigned int ams369fg06_22_200[] = {
+	0x00, 0x3f, 0x28, 0x29, 0x27, 0x21, 0x3e,
+	0x00, 0x00, 0x10, 0x25, 0x27, 0x20, 0x3d,
+	0x00, 0x3f, 0x28, 0x27, 0x25, 0x1d, 0x53,
+};
+
+static const unsigned int ams369fg06_22_150[] = {
+	0x00, 0x3f, 0x2d, 0x29, 0x28, 0x23, 0x37,
+	0x00, 0x00, 0x0b, 0x25, 0x28, 0x22, 0x36,
+	0x00, 0x3f, 0x2b, 0x28, 0x26, 0x1f, 0x4a,
+};
+
+static const unsigned int ams369fg06_22_100[] = {
+	0x00, 0x3f, 0x30, 0x2a, 0x2b, 0x24, 0x2f,
+	0x00, 0x00, 0x00, 0x25, 0x29, 0x24, 0x2e,
+	0x00, 0x3f, 0x2f, 0x29, 0x29, 0x21, 0x3f,
+};
+
+static const unsigned int ams369fg06_22_50[] = {
+	0x00, 0x3f, 0x3c, 0x2c, 0x2d, 0x27, 0x24,
+	0x00, 0x00, 0x00, 0x22, 0x2a, 0x27, 0x23,
+	0x00, 0x3f, 0x3b, 0x2c, 0x2b, 0x24, 0x31,
+};
+
+struct ams369fg06_gamma {
+	unsigned int *gamma_22_table[MAX_GAMMA_LEVEL];
+};
+
+static struct ams369fg06_gamma gamma_table = {
+	.gamma_22_table[0] = (unsigned int *)&ams369fg06_22_50,
+	.gamma_22_table[1] = (unsigned int *)&ams369fg06_22_100,
+	.gamma_22_table[2] = (unsigned int *)&ams369fg06_22_150,
+	.gamma_22_table[3] = (unsigned int *)&ams369fg06_22_200,
+	.gamma_22_table[4] = (unsigned int *)&ams369fg06_22_250,
+};
+
+static int ams369fg06_spi_write_byte(struct ams369fg06 *lcd, int addr, int data)
+{
+	u16 buf[1];
+	struct spi_message msg;
+
+	struct spi_transfer xfer = {
+		.len		= 2,
+		.tx_buf		= buf,
+	};
+
+	buf[0] = (addr << 8) | data;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	return spi_sync(lcd->spi, &msg);
+}
+
+static int ams369fg06_spi_write(struct ams369fg06 *lcd, unsigned char address,
+	unsigned char command)
+{
+	int ret = 0;
+
+	if (address != DATA_ONLY)
+		ret = ams369fg06_spi_write_byte(lcd, 0x70, address);
+	if (command != COMMAND_ONLY)
+		ret = ams369fg06_spi_write_byte(lcd, 0x72, command);
+
+	return ret;
+}
+
+static int ams369fg06_panel_send_sequence(struct ams369fg06 *lcd,
+	const unsigned short *wbuf)
+{
+	int ret = 0, i = 0;
+
+	while ((wbuf[i] & DEFMASK) != ENDDEF) {
+		if ((wbuf[i] & DEFMASK) != SLEEPMSEC) {
+			ret = ams369fg06_spi_write(lcd, wbuf[i], wbuf[i+1]);
+			if (ret)
+				break;
+		} else
+			mdelay(wbuf[i+1]);
+		i += 2;
+	}
+
+	return ret;
+}
+
+static int _ams369fg06_gamma_ctl(struct ams369fg06 *lcd,
+	const unsigned int *gamma)
+{
+	unsigned int i = 0;
+	int ret = 0;
+
+	for (i = 0 ; i < GAMMA_TABLE_COUNT / 3; i++) {
+		ret = ams369fg06_spi_write(lcd, 0x40 + i, gamma[i]);
+		ret = ams369fg06_spi_write(lcd, 0x50 + i, gamma[i+7*1]);
+		ret = ams369fg06_spi_write(lcd, 0x60 + i, gamma[i+7*2]);
+		if (ret) {
+			dev_err(lcd->dev, "failed to set gamma table.\n");
+			goto gamma_err;
+		}
+	}
+
+gamma_err:
+	return ret;
+}
+
+static int ams369fg06_gamma_ctl(struct ams369fg06 *lcd, int brightness)
+{
+	int ret = 0;
+	int gamma = 0;
+
+	if ((brightness >= 0) && (brightness <= 50))
+		gamma = 0;
+	else if ((brightness > 50) && (brightness <= 100))
+		gamma = 1;
+	else if ((brightness > 100) && (brightness <= 150))
+		gamma = 2;
+	else if ((brightness > 150) && (brightness <= 200))
+		gamma = 3;
+	else if ((brightness > 200) && (brightness <= 255))
+		gamma = 4;
+
+	ret = _ams369fg06_gamma_ctl(lcd, gamma_table.gamma_22_table[gamma]);
+
+	return ret;
+}
+
+static int ams369fg06_ldi_init(struct ams369fg06 *lcd)
+{
+	int ret, i;
+	static const unsigned short *init_seq[] = {
+		seq_setting,
+		seq_stand_by_off,
+	};
+
+	for (i = 0; i < ARRAY_SIZE(init_seq); i++) {
+		ret = ams369fg06_panel_send_sequence(lcd, init_seq[i]);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int ams369fg06_ldi_enable(struct ams369fg06 *lcd)
+{
+	int ret, i;
+	static const unsigned short *init_seq[] = {
+		seq_stand_by_off,
+		seq_display_on,
+	};
+
+	for (i = 0; i < ARRAY_SIZE(init_seq); i++) {
+		ret = ams369fg06_panel_send_sequence(lcd, init_seq[i]);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int ams369fg06_ldi_disable(struct ams369fg06 *lcd)
+{
+	int ret, i;
+
+	static const unsigned short *init_seq[] = {
+		seq_display_off,
+		seq_stand_by_on,
+	};
+
+	for (i = 0; i < ARRAY_SIZE(init_seq); i++) {
+		ret = ams369fg06_panel_send_sequence(lcd, init_seq[i]);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int ams369fg06_power_is_on(int power)
+{
+	return ((power) <= FB_BLANK_NORMAL);
+}
+
+static int ams369fg06_power_on(struct ams369fg06 *lcd)
+{
+	int ret = 0;
+	struct lcd_platform_data *pd = NULL;
+	struct backlight_device *bd = NULL;
+
+	pd = lcd->lcd_pd;
+	if (!pd) {
+		dev_err(lcd->dev, "platform data is NULL.\n");
+		return -EFAULT;
+	}
+
+	bd = lcd->bd;
+	if (!bd) {
+		dev_err(lcd->dev, "backlight device is NULL.\n");
+		return -EFAULT;
+	}
+
+	if (!pd->power_on) {
+		dev_err(lcd->dev, "power_on is NULL.\n");
+		return -EFAULT;
+	} else {
+		pd->power_on(lcd->ld, 1);
+		mdelay(pd->power_on_delay);
+	}
+
+	if (!pd->reset) {
+		dev_err(lcd->dev, "reset is NULL.\n");
+		return -EFAULT;
+	} else {
+		pd->reset(lcd->ld);
+		mdelay(pd->reset_delay);
+	}
+
+	ret = ams369fg06_ldi_init(lcd);
+	if (ret) {
+		dev_err(lcd->dev, "failed to initialize ldi.\n");
+		return ret;
+	}
+
+	ret = ams369fg06_ldi_enable(lcd);
+	if (ret) {
+		dev_err(lcd->dev, "failed to enable ldi.\n");
+		return ret;
+	}
+
+	/* set brightness to current value after power on or resume. */
+	ret = ams369fg06_gamma_ctl(lcd, bd->props.brightness);
+	if (ret) {
+		dev_err(lcd->dev, "lcd gamma setting failed.\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ams369fg06_power_off(struct ams369fg06 *lcd)
+{
+	int ret = 0;
+	struct lcd_platform_data *pd = NULL;
+
+	pd = lcd->lcd_pd;
+	if (!pd) {
+		dev_err(lcd->dev, "platform data is NULL\n");
+		return -EFAULT;
+	}
+
+	ret = ams369fg06_ldi_disable(lcd);
+	if (ret) {
+		dev_err(lcd->dev, "lcd setting failed.\n");
+		return -EIO;
+	}
+
+	mdelay(pd->power_off_delay);
+
+	if (!pd->power_on) {
+		dev_err(lcd->dev, "power_on is NULL.\n");
+		return -EFAULT;
+	} else
+		pd->power_on(lcd->ld, 0);
+
+	return 0;
+}
+
+static int ams369fg06_power(struct ams369fg06 *lcd, int power)
+{
+	int ret = 0;
+
+	if (ams369fg06_power_is_on(power) &&
+		!ams369fg06_power_is_on(lcd->power))
+		ret = ams369fg06_power_on(lcd);
+	else if (!ams369fg06_power_is_on(power) &&
+		ams369fg06_power_is_on(lcd->power))
+		ret = ams369fg06_power_off(lcd);
+
+	if (!ret)
+		lcd->power = power;
+
+	return ret;
+}
+
+static int ams369fg06_get_power(struct lcd_device *ld)
+{
+	struct ams369fg06 *lcd = lcd_get_data(ld);
+
+	return lcd->power;
+}
+
+static int ams369fg06_set_power(struct lcd_device *ld, int power)
+{
+	struct ams369fg06 *lcd = lcd_get_data(ld);
+
+	if (power != FB_BLANK_UNBLANK && power != FB_BLANK_POWERDOWN &&
+		power != FB_BLANK_NORMAL) {
+		dev_err(lcd->dev, "power value should be 0, 1 or 4.\n");
+		return -EINVAL;
+	}
+
+	return ams369fg06_power(lcd, power);
+}
+
+static int ams369fg06_get_brightness(struct backlight_device *bd)
+{
+	return bd->props.brightness;
+}
+
+static int ams369fg06_set_brightness(struct backlight_device *bd)
+{
+	int ret = 0;
+	int brightness = bd->props.brightness;
+	struct ams369fg06 *lcd = dev_get_drvdata(&bd->dev);
+
+	if (brightness < MIN_BRIGHTNESS ||
+		brightness > bd->props.max_brightness) {
+		dev_err(&bd->dev, "lcd brightness should be %d to %d.\n",
+			MIN_BRIGHTNESS, MAX_BRIGHTNESS);
+		return -EINVAL;
+	}
+
+	ret = ams369fg06_gamma_ctl(lcd, bd->props.brightness);
+	if (ret) {
+		dev_err(&bd->dev, "lcd brightness setting failed.\n");
+		return -EIO;
+	}
+
+	return ret;
+}
+
+static struct lcd_ops ams369fg06_lcd_ops = {
+	.get_power = ams369fg06_get_power,
+	.set_power = ams369fg06_set_power,
+};
+
+static const struct backlight_ops ams369fg06_backlight_ops = {
+	.get_brightness = ams369fg06_get_brightness,
+	.update_status = ams369fg06_set_brightness,
+};
+
+static int __devinit ams369fg06_probe(struct spi_device *spi)
+{
+	int ret = 0;
+	struct ams369fg06 *lcd = NULL;
+	struct lcd_device *ld = NULL;
+	struct backlight_device *bd = NULL;
+	struct backlight_properties props;
+
+	lcd = kzalloc(sizeof(struct ams369fg06), GFP_KERNEL);
+	if (!lcd)
+		return -ENOMEM;
+
+	/* ams369fg06 lcd panel uses 3-wire 16bits SPI Mode. */
+	spi->bits_per_word = 16;
+
+	ret = spi_setup(spi);
+	if (ret < 0) {
+		dev_err(&spi->dev, "spi setup failed.\n");
+		goto out_free_lcd;
+	}
+
+	lcd->spi = spi;
+	lcd->dev = &spi->dev;
+
+	lcd->lcd_pd = spi->dev.platform_data;
+	if (!lcd->lcd_pd) {
+		dev_err(&spi->dev, "platform data is NULL\n");
+		goto out_free_lcd;
+	}
+
+	ld = lcd_device_register("ams369fg06", &spi->dev, lcd,
+		&ams369fg06_lcd_ops);
+	if (IS_ERR(ld)) {
+		ret = PTR_ERR(ld);
+		goto out_free_lcd;
+	}
+
+	lcd->ld = ld;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = MAX_BRIGHTNESS;
+
+	bd = backlight_device_register("ams369fg06-bl", &spi->dev, lcd,
+		&ams369fg06_backlight_ops, &props);
+	if (IS_ERR(bd)) {
+		ret =  PTR_ERR(bd);
+		goto out_lcd_unregister;
+	}
+
+	bd->props.brightness = DEFAULT_BRIGHTNESS;
+	lcd->bd = bd;
+
+	if (!lcd->lcd_pd->lcd_enabled) {
+		/*
+		 * if lcd panel was off from bootloader then
+		 * current lcd status is powerdown and then
+		 * it enables lcd panel.
+		 */
+		lcd->power = FB_BLANK_POWERDOWN;
+
+		ams369fg06_power(lcd, FB_BLANK_UNBLANK);
+	} else
+		lcd->power = FB_BLANK_UNBLANK;
+
+	dev_set_drvdata(&spi->dev, lcd);
+
+	dev_info(&spi->dev, "ams369fg06 panel driver has been probed.\n");
+
+	return 0;
+
+out_lcd_unregister:
+	lcd_device_unregister(ld);
+out_free_lcd:
+	kfree(lcd);
+	return ret;
+}
+
+static int __devexit ams369fg06_remove(struct spi_device *spi)
+{
+	struct ams369fg06 *lcd = dev_get_drvdata(&spi->dev);
+
+	ams369fg06_power(lcd, FB_BLANK_POWERDOWN);
+	backlight_device_unregister(lcd->bd);
+	lcd_device_unregister(lcd->ld);
+	kfree(lcd);
+
+	return 0;
+}
+
+#if defined(CONFIG_PM)
+static unsigned int before_power;
+
+static int ams369fg06_suspend(struct spi_device *spi, pm_message_t mesg)
+{
+	int ret = 0;
+	struct ams369fg06 *lcd = dev_get_drvdata(&spi->dev);
+
+	dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+
+	before_power = lcd->power;
+
+	/*
+	 * when lcd panel is suspend, lcd panel becomes off
+	 * regardless of status.
+	 */
+	ret = ams369fg06_power(lcd, FB_BLANK_POWERDOWN);
+
+	return ret;
+}
+
+static int ams369fg06_resume(struct spi_device *spi)
+{
+	int ret = 0;
+	struct ams369fg06 *lcd = dev_get_drvdata(&spi->dev);
+
+	/*
+	 * after suspended, if lcd panel status is FB_BLANK_UNBLANK
+	 * (at that time, before_power is FB_BLANK_UNBLANK) then
+	 * it changes that status to FB_BLANK_POWERDOWN to get lcd on.
+	 */
+	if (before_power == FB_BLANK_UNBLANK)
+		lcd->power = FB_BLANK_POWERDOWN;
+
+	dev_dbg(&spi->dev, "before_power = %d\n", before_power);
+
+	ret = ams369fg06_power(lcd, before_power);
+
+	return ret;
+}
+#else
+#define ams369fg06_suspend	NULL
+#define ams369fg06_resume	NULL
+#endif
+
+static void ams369fg06_shutdown(struct spi_device *spi)
+{
+	struct ams369fg06 *lcd = dev_get_drvdata(&spi->dev);
+
+	ams369fg06_power(lcd, FB_BLANK_POWERDOWN);
+}
+
+static struct spi_driver ams369fg06_driver = {
+	.driver = {
+		.name	= "ams369fg06",
+		.bus	= &spi_bus_type,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ams369fg06_probe,
+	.remove		= __devexit_p(ams369fg06_remove),
+	.shutdown	= ams369fg06_shutdown,
+	.suspend	= ams369fg06_suspend,
+	.resume		= ams369fg06_resume,
+};
+
+static int __init ams369fg06_init(void)
+{
+	return spi_register_driver(&ams369fg06_driver);
+}
+
+static void __exit ams369fg06_exit(void)
+{
+	spi_unregister_driver(&ams369fg06_driver);
+}
+
+module_init(ams369fg06_init);
+module_exit(ams369fg06_exit);
+
+MODULE_AUTHOR("Jingoo Han <jg1.han@samsung.com>");
+MODULE_DESCRIPTION("ams369fg06 LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/apple_bl.c b/drivers/video/backlight/apple_bl.c
new file mode 100644
index 000000000000..be98d152b7fd
--- /dev/null
+++ b/drivers/video/backlight/apple_bl.c
@@ -0,0 +1,241 @@
+/*
+ *  Backlight Driver for Intel-based Apples
+ *
+ *  Copyright (c) Red Hat <mjg@redhat.com>
+ *  Based on code from Pommed:
+ *  Copyright (C) 2006 Nicolas Boichat <nicolas @boichat.ch>
+ *  Copyright (C) 2006 Felipe Alfaro Solana <felipe_alfaro @linuxmail.org>
+ *  Copyright (C) 2007 Julien BLACHE <jb@jblache.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This driver triggers SMIs which cause the firmware to change the
+ *  backlight brightness. This is icky in many ways, but it's impractical to
+ *  get at the firmware code in order to figure out what it's actually doing.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/backlight.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/pci.h>
+#include <linux/acpi.h>
+
+static struct backlight_device *apple_backlight_device;
+
+struct hw_data {
+	/* I/O resource to allocate. */
+	unsigned long iostart;
+	unsigned long iolen;
+	/* Backlight operations structure. */
+	const struct backlight_ops backlight_ops;
+	void (*set_brightness)(int);
+};
+
+static const struct hw_data *hw_data;
+
+#define DRIVER "apple_backlight: "
+
+/* Module parameters. */
+static int debug;
+module_param_named(debug, debug, int, 0644);
+MODULE_PARM_DESC(debug, "Set to one to enable debugging messages.");
+
+/*
+ * Implementation for machines with Intel chipset.
+ */
+static void intel_chipset_set_brightness(int intensity)
+{
+	outb(0x04 | (intensity << 4), 0xb3);
+	outb(0xbf, 0xb2);
+}
+
+static int intel_chipset_send_intensity(struct backlight_device *bd)
+{
+	int intensity = bd->props.brightness;
+
+	if (debug)
+		printk(KERN_DEBUG DRIVER "setting brightness to %d\n",
+		       intensity);
+
+	intel_chipset_set_brightness(intensity);
+	return 0;
+}
+
+static int intel_chipset_get_intensity(struct backlight_device *bd)
+{
+	int intensity;
+
+	outb(0x03, 0xb3);
+	outb(0xbf, 0xb2);
+	intensity = inb(0xb3) >> 4;
+
+	if (debug)
+		printk(KERN_DEBUG DRIVER "read brightness of %d\n",
+		       intensity);
+
+	return intensity;
+}
+
+static const struct hw_data intel_chipset_data = {
+	.iostart = 0xb2,
+	.iolen = 2,
+	.backlight_ops	= {
+		.options	= BL_CORE_SUSPENDRESUME,
+		.get_brightness	= intel_chipset_get_intensity,
+		.update_status	= intel_chipset_send_intensity,
+	},
+	.set_brightness = intel_chipset_set_brightness,
+};
+
+/*
+ * Implementation for machines with Nvidia chipset.
+ */
+static void nvidia_chipset_set_brightness(int intensity)
+{
+	outb(0x04 | (intensity << 4), 0x52f);
+	outb(0xbf, 0x52e);
+}
+
+static int nvidia_chipset_send_intensity(struct backlight_device *bd)
+{
+	int intensity = bd->props.brightness;
+
+	if (debug)
+		printk(KERN_DEBUG DRIVER "setting brightness to %d\n",
+		       intensity);
+
+	nvidia_chipset_set_brightness(intensity);
+	return 0;
+}
+
+static int nvidia_chipset_get_intensity(struct backlight_device *bd)
+{
+	int intensity;
+
+	outb(0x03, 0x52f);
+	outb(0xbf, 0x52e);
+	intensity = inb(0x52f) >> 4;
+
+	if (debug)
+		printk(KERN_DEBUG DRIVER "read brightness of %d\n",
+		       intensity);
+
+	return intensity;
+}
+
+static const struct hw_data nvidia_chipset_data = {
+	.iostart = 0x52e,
+	.iolen = 2,
+	.backlight_ops		= {
+		.options	= BL_CORE_SUSPENDRESUME,
+		.get_brightness	= nvidia_chipset_get_intensity,
+		.update_status	= nvidia_chipset_send_intensity
+	},
+	.set_brightness = nvidia_chipset_set_brightness,
+};
+
+static int __devinit apple_bl_add(struct acpi_device *dev)
+{
+	struct backlight_properties props;
+	struct pci_dev *host;
+	int intensity;
+
+	host = pci_get_bus_and_slot(0, 0);
+
+	if (!host) {
+		printk(KERN_ERR DRIVER "unable to find PCI host\n");
+		return -ENODEV;
+	}
+
+	if (host->vendor == PCI_VENDOR_ID_INTEL)
+		hw_data = &intel_chipset_data;
+	else if (host->vendor == PCI_VENDOR_ID_NVIDIA)
+		hw_data = &nvidia_chipset_data;
+
+	pci_dev_put(host);
+
+	if (!hw_data) {
+		printk(KERN_ERR DRIVER "unknown hardware\n");
+		return -ENODEV;
+	}
+
+	/* Check that the hardware responds - this may not work under EFI */
+
+	intensity = hw_data->backlight_ops.get_brightness(NULL);
+
+	if (!intensity) {
+		hw_data->set_brightness(1);
+		if (!hw_data->backlight_ops.get_brightness(NULL))
+			return -ENODEV;
+
+		hw_data->set_brightness(0);
+	}
+
+	if (!request_region(hw_data->iostart, hw_data->iolen,
+			    "Apple backlight"))
+		return -ENXIO;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_PLATFORM;
+	props.max_brightness = 15;
+	apple_backlight_device = backlight_device_register("apple_backlight",
+				  NULL, NULL, &hw_data->backlight_ops, &props);
+
+	if (IS_ERR(apple_backlight_device)) {
+		release_region(hw_data->iostart, hw_data->iolen);
+		return PTR_ERR(apple_backlight_device);
+	}
+
+	apple_backlight_device->props.brightness =
+		hw_data->backlight_ops.get_brightness(apple_backlight_device);
+	backlight_update_status(apple_backlight_device);
+
+	return 0;
+}
+
+static int __devexit apple_bl_remove(struct acpi_device *dev, int type)
+{
+	backlight_device_unregister(apple_backlight_device);
+
+	release_region(hw_data->iostart, hw_data->iolen);
+	hw_data = NULL;
+	return 0;
+}
+
+static const struct acpi_device_id apple_bl_ids[] = {
+	{"APP0002", 0},
+	{"", 0},
+};
+
+static struct acpi_driver apple_bl_driver = {
+	.name = "Apple backlight",
+	.ids = apple_bl_ids,
+	.ops = {
+		.add = apple_bl_add,
+		.remove = apple_bl_remove,
+	},
+};
+
+static int __init apple_bl_init(void)
+{
+	return acpi_bus_register_driver(&apple_bl_driver);
+}
+
+static void __exit apple_bl_exit(void)
+{
+	acpi_bus_unregister_driver(&apple_bl_driver);
+}
+
+module_init(apple_bl_init);
+module_exit(apple_bl_exit);
+
+MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
+MODULE_DESCRIPTION("Apple Backlight Driver");
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(acpi, apple_bl_ids);
+MODULE_ALIAS("mbp_nvidia_bl");
diff --git a/drivers/video/backlight/atmel-pwm-bl.c b/drivers/video/backlight/atmel-pwm-bl.c
index e6a66dab088c..0443a4f71858 100644
--- a/drivers/video/backlight/atmel-pwm-bl.c
+++ b/drivers/video/backlight/atmel-pwm-bl.c
@@ -168,6 +168,7 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
 	}
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = pdata->pwm_duty_max - pdata->pwm_duty_min;
 	bldev = backlight_device_register("atmel-pwm-bl", &pdev->dev, pwmbl,
 					  &atmel_pwm_bl_ops, &props);
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index 08703299ef61..80d292fb92d8 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -19,6 +19,12 @@
 #include <asm/backlight.h>
 #endif
 
+static const char const *backlight_types[] = {
+	[BACKLIGHT_RAW] = "raw",
+	[BACKLIGHT_PLATFORM] = "platform",
+	[BACKLIGHT_FIRMWARE] = "firmware",
+};
+
 #if defined(CONFIG_FB) || (defined(CONFIG_FB_MODULE) && \
 			   defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE))
 /* This callback gets called when something important happens inside a
@@ -169,6 +175,14 @@ static ssize_t backlight_store_brightness(struct device *dev,
 	return rc;
 }
 
+static ssize_t backlight_show_type(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct backlight_device *bd = to_backlight_device(dev);
+
+	return sprintf(buf, "%s\n", backlight_types[bd->props.type]);
+}
+
 static ssize_t backlight_show_max_brightness(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -234,6 +248,7 @@ static struct device_attribute bl_device_attributes[] = {
 	__ATTR(actual_brightness, 0444, backlight_show_actual_brightness,
 		     NULL),
 	__ATTR(max_brightness, 0444, backlight_show_max_brightness, NULL),
+	__ATTR(type, 0444, backlight_show_type, NULL),
 	__ATTR_NULL,
 };
 
@@ -292,9 +307,16 @@ struct backlight_device *backlight_device_register(const char *name,
 	dev_set_drvdata(&new_bd->dev, devdata);
 
 	/* Set default properties */
-	if (props)
+	if (props) {
 		memcpy(&new_bd->props, props,
 		       sizeof(struct backlight_properties));
+		if (props->type <= 0 || props->type >= BACKLIGHT_TYPE_MAX) {
+			WARN(1, "%s: invalid backlight type", name);
+			new_bd->props.type = BACKLIGHT_RAW;
+		}
+	} else {
+		new_bd->props.type = BACKLIGHT_RAW;
+	}
 
 	rc = device_register(&new_bd->dev);
 	if (rc) {
diff --git a/drivers/video/backlight/corgi_lcd.c b/drivers/video/backlight/corgi_lcd.c
index 1e71c35083bb..c6533bad26f8 100644
--- a/drivers/video/backlight/corgi_lcd.c
+++ b/drivers/video/backlight/corgi_lcd.c
@@ -109,7 +109,7 @@ static unsigned long corgibl_flags;
 #define CORGIBL_BATTLOW       0x02
 
 /*
- * This is only a psuedo I2C interface. We can't use the standard kernel
+ * This is only a pseudo I2C interface. We can't use the standard kernel
  * routines as the interface is write only. We just assume the data is acked...
  */
 static void lcdtg_ssp_i2c_send(struct corgi_lcd *lcd, uint8_t data)
@@ -562,6 +562,7 @@ static int __devinit corgi_lcd_probe(struct spi_device *spi)
 	lcd->mode = (pdata) ? pdata->init_mode : CORGI_LCD_MODE_VGA;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = pdata->max_intensity;
 	lcd->bl_dev = backlight_device_register("corgi_bl", &spi->dev, lcd,
 						&corgi_bl_ops, &props);
diff --git a/drivers/video/backlight/cr_bllcd.c b/drivers/video/backlight/cr_bllcd.c
index 397d15eb1ea8..6c8c54041fae 100644
--- a/drivers/video/backlight/cr_bllcd.c
+++ b/drivers/video/backlight/cr_bllcd.c
@@ -193,6 +193,7 @@ static int cr_backlight_probe(struct platform_device *pdev)
 	}
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	bdp = backlight_device_register("cr-backlight", &pdev->dev, NULL,
 					&cr_backlight_ops, &props);
 	if (IS_ERR(bdp)) {
diff --git a/drivers/video/backlight/da903x_bl.c b/drivers/video/backlight/da903x_bl.c
index 87659ed79bd7..62043f12a5a4 100644
--- a/drivers/video/backlight/da903x_bl.c
+++ b/drivers/video/backlight/da903x_bl.c
@@ -136,6 +136,7 @@ static int da903x_backlight_probe(struct platform_device *pdev)
 		da903x_write(data->da903x_dev, DA9034_WLED_CONTROL2,
 				DA9034_WLED_ISET(pdata->output_current));
 
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = max_brightness;
 	bl = backlight_device_register(pdev->name, data->da903x_dev, data,
 				       &da903x_backlight_ops, &props);
diff --git a/drivers/video/backlight/ep93xx_bl.c b/drivers/video/backlight/ep93xx_bl.c
index b0cc49184803..9f1e389d51d2 100644
--- a/drivers/video/backlight/ep93xx_bl.c
+++ b/drivers/video/backlight/ep93xx_bl.c
@@ -87,6 +87,7 @@ static int __init ep93xxbl_probe(struct platform_device *dev)
 	ep93xxbl->mmio = EP93XX_RASTER_BRIGHTNESS;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = EP93XX_MAX_BRIGHT;
 	bl = backlight_device_register(dev->name, &dev->dev, ep93xxbl,
 				       &ep93xxbl_ops, &props);
diff --git a/drivers/video/backlight/generic_bl.c b/drivers/video/backlight/generic_bl.c
index 312ca619735d..8c6befd65a33 100644
--- a/drivers/video/backlight/generic_bl.c
+++ b/drivers/video/backlight/generic_bl.c
@@ -91,6 +91,7 @@ static int genericbl_probe(struct platform_device *pdev)
 		name = machinfo->name;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = machinfo->max_intensity;
 	bd = backlight_device_register(name, &pdev->dev, NULL, &genericbl_ops,
 				       &props);
diff --git a/drivers/video/backlight/hp680_bl.c b/drivers/video/backlight/hp680_bl.c
index 267d23f8d645..38aa00272141 100644
--- a/drivers/video/backlight/hp680_bl.c
+++ b/drivers/video/backlight/hp680_bl.c
@@ -109,6 +109,7 @@ static int __devinit hp680bl_probe(struct platform_device *pdev)
 	struct backlight_device *bd;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = HP680_MAX_INTENSITY;
 	bd = backlight_device_register("hp680-bl", &pdev->dev, NULL,
 				       &hp680bl_ops, &props);
diff --git a/drivers/video/backlight/jornada720_bl.c b/drivers/video/backlight/jornada720_bl.c
index 2f177b3a4885..de65d80159be 100644
--- a/drivers/video/backlight/jornada720_bl.c
+++ b/drivers/video/backlight/jornada720_bl.c
@@ -106,6 +106,7 @@ static int jornada_bl_probe(struct platform_device *pdev)
 	struct backlight_device *bd;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = BL_MAX_BRIGHT;
 	bd = backlight_device_register(S1D_DEVICENAME, &pdev->dev, NULL,
 				       &jornada_bl_ops, &props);
@@ -146,12 +147,12 @@ static struct platform_driver jornada_bl_driver = {
 	},
 };
 
-int __init jornada_bl_init(void)
+static int __init jornada_bl_init(void)
 {
 	return platform_driver_register(&jornada_bl_driver);
 }
 
-void __exit jornada_bl_exit(void)
+static void __exit jornada_bl_exit(void)
 {
 	platform_driver_unregister(&jornada_bl_driver);
 }
diff --git a/drivers/video/backlight/jornada720_lcd.c b/drivers/video/backlight/jornada720_lcd.c
index cbbb167fd268..d2ff658b4144 100644
--- a/drivers/video/backlight/jornada720_lcd.c
+++ b/drivers/video/backlight/jornada720_lcd.c
@@ -135,12 +135,12 @@ static struct platform_driver jornada_lcd_driver = {
 	},
 };
 
-int __init jornada_lcd_init(void)
+static int __init jornada_lcd_init(void)
 {
 	return platform_driver_register(&jornada_lcd_driver);
 }
 
-void __exit jornada_lcd_exit(void)
+static void __exit jornada_lcd_exit(void)
 {
 	platform_driver_unregister(&jornada_lcd_driver);
 }
diff --git a/drivers/video/backlight/kb3886_bl.c b/drivers/video/backlight/kb3886_bl.c
index f439a8632287..72dd5556a35b 100644
--- a/drivers/video/backlight/kb3886_bl.c
+++ b/drivers/video/backlight/kb3886_bl.c
@@ -149,6 +149,7 @@ static int kb3886bl_probe(struct platform_device *pdev)
 		machinfo->limit_mask = -1;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = machinfo->max_intensity;
 	kb3886_backlight_device = backlight_device_register("kb3886-bl",
 							    &pdev->dev, NULL,
diff --git a/drivers/video/backlight/ld9040.c b/drivers/video/backlight/ld9040.c
new file mode 100644
index 000000000000..5934655eb1ff
--- /dev/null
+++ b/drivers/video/backlight/ld9040.c
@@ -0,0 +1,826 @@
+/*
+ * ld9040 AMOLED LCD panel driver.
+ *
+ * Copyright (c) 2011 Samsung Electronics
+ * Author: Donghwa Lee  <dh09.lee@samsung.com>
+ * Derived from drivers/video/backlight/s6e63m0.c
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ */
+
+#include <linux/wait.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/lcd.h>
+#include <linux/backlight.h>
+
+#include "ld9040_gamma.h"
+
+#define SLEEPMSEC		0x1000
+#define ENDDEF			0x2000
+#define	DEFMASK			0xFF00
+#define COMMAND_ONLY		0xFE
+#define DATA_ONLY		0xFF
+
+#define MIN_BRIGHTNESS		0
+#define MAX_BRIGHTNESS		24
+#define power_is_on(pwr)	((pwr) <= FB_BLANK_NORMAL)
+
+struct ld9040 {
+	struct device			*dev;
+	struct spi_device		*spi;
+	unsigned int			power;
+	unsigned int			current_brightness;
+
+	struct lcd_device		*ld;
+	struct backlight_device		*bd;
+	struct lcd_platform_data	*lcd_pd;
+};
+
+static const unsigned short seq_swreset[] = {
+	0x01, COMMAND_ONLY,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_user_setting[] = {
+	0xF0, 0x5A,
+
+	DATA_ONLY, 0x5A,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_elvss_on[] = {
+	0xB1, 0x0D,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x16,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_gtcon[] = {
+	0xF7, 0x09,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_panel_condition[] = {
+	0xF8, 0x05,
+
+	DATA_ONLY, 0x65,
+	DATA_ONLY, 0x96,
+	DATA_ONLY, 0x71,
+	DATA_ONLY, 0x7D,
+	DATA_ONLY, 0x19,
+	DATA_ONLY, 0x3B,
+	DATA_ONLY, 0x0D,
+	DATA_ONLY, 0x19,
+	DATA_ONLY, 0x7E,
+	DATA_ONLY, 0x0D,
+	DATA_ONLY, 0xE2,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x7E,
+	DATA_ONLY, 0x7D,
+	DATA_ONLY, 0x07,
+	DATA_ONLY, 0x07,
+	DATA_ONLY, 0x20,
+	DATA_ONLY, 0x20,
+	DATA_ONLY, 0x20,
+	DATA_ONLY, 0x02,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_gamma_set1[] = {
+	0xF9, 0x00,
+
+	DATA_ONLY, 0xA7,
+	DATA_ONLY, 0xB4,
+	DATA_ONLY, 0xAE,
+	DATA_ONLY, 0xBF,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x91,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0xB2,
+	DATA_ONLY, 0xB4,
+	DATA_ONLY, 0xAA,
+	DATA_ONLY, 0xBB,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0xAC,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0xB3,
+	DATA_ONLY, 0xB1,
+	DATA_ONLY, 0xAA,
+	DATA_ONLY, 0xBC,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0xB3,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_gamma_ctrl[] = {
+	0xFB, 0x02,
+
+	DATA_ONLY, 0x5A,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_gamma_start[] = {
+	0xF9, COMMAND_ONLY,
+
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_apon[] = {
+	0xF3, 0x00,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x0A,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_display_ctrl[] = {
+	0xF2, 0x02,
+
+	DATA_ONLY, 0x08,
+	DATA_ONLY, 0x08,
+	DATA_ONLY, 0x10,
+	DATA_ONLY, 0x10,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_manual_pwr[] = {
+	0xB0, 0x04,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_pwr_ctrl[] = {
+	0xF4, 0x0A,
+
+	DATA_ONLY, 0x87,
+	DATA_ONLY, 0x25,
+	DATA_ONLY, 0x6A,
+	DATA_ONLY, 0x44,
+	DATA_ONLY, 0x02,
+	DATA_ONLY, 0x88,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_sleep_out[] = {
+	0x11, COMMAND_ONLY,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_sleep_in[] = {
+	0x10, COMMAND_ONLY,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_display_on[] = {
+	0x29, COMMAND_ONLY,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_display_off[] = {
+	0x28, COMMAND_ONLY,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vci1_1st_en[] = {
+	0xF3, 0x10,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vl1_en[] = {
+	0xF3, 0x11,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vl2_en[] = {
+	0xF3, 0x13,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vci1_2nd_en[] = {
+	0xF3, 0x33,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vl3_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vreg1_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0x01,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vgh_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0x11,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vgl_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0x31,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x02,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vmos_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xB1,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vint_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xF1,
+	/* DATA_ONLY, 0x71,	VMOS/VBL/VBH not used */
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	/* DATA_ONLY, 0x02,	VMOS/VBL/VBH not used */
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vbh_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xF9,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_vbl_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xFD,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_gam_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xFF,
+	/* DATA_ONLY, 0x73,	VMOS/VBL/VBH not used */
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	/* DATA_ONLY, 0x02,	VMOS/VBL/VBH not used */
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_sd_amp_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xFF,
+	/* DATA_ONLY, 0x73,	VMOS/VBL/VBH not used */
+	DATA_ONLY, 0x80,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	/* DATA_ONLY, 0x02,	VMOS/VBL/VBH not used */
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_gls_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xFF,
+	/* DATA_ONLY, 0x73,	VMOS/VBL/VBH not used */
+	DATA_ONLY, 0x81,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	/* DATA_ONLY, 0x02,	VMOS/VBL/VBH not used */
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_els_en[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xFF,
+	/* DATA_ONLY, 0x73,	VMOS/VBL/VBH not used */
+	DATA_ONLY, 0x83,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	/* DATA_ONLY, 0x02,	VMOS/VBL/VBH not used */
+	ENDDEF, 0x00
+};
+
+static const unsigned short seq_el_on[] = {
+	0xF3, 0x37,
+
+	DATA_ONLY, 0xFF,
+	/* DATA_ONLY, 0x73,	VMOS/VBL/VBH not used */
+	DATA_ONLY, 0x87,
+	DATA_ONLY, 0x00,
+	DATA_ONLY, 0x03,
+	/* DATA_ONLY, 0x02,	VMOS/VBL/VBH not used */
+	ENDDEF, 0x00
+};
+
+static int ld9040_spi_write_byte(struct ld9040 *lcd, int addr, int data)
+{
+	u16 buf[1];
+	struct spi_message msg;
+
+	struct spi_transfer xfer = {
+		.len		= 2,
+		.tx_buf		= buf,
+	};
+
+	buf[0] = (addr << 8) | data;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	return spi_sync(lcd->spi, &msg);
+}
+
+static int ld9040_spi_write(struct ld9040 *lcd, unsigned char address,
+	unsigned char command)
+{
+	int ret = 0;
+
+	if (address != DATA_ONLY)
+		ret = ld9040_spi_write_byte(lcd, 0x0, address);
+	if (command != COMMAND_ONLY)
+		ret = ld9040_spi_write_byte(lcd, 0x1, command);
+
+	return ret;
+}
+
+static int ld9040_panel_send_sequence(struct ld9040 *lcd,
+	const unsigned short *wbuf)
+{
+	int ret = 0, i = 0;
+
+	while ((wbuf[i] & DEFMASK) != ENDDEF) {
+		if ((wbuf[i] & DEFMASK) != SLEEPMSEC) {
+			ret = ld9040_spi_write(lcd, wbuf[i], wbuf[i+1]);
+			if (ret)
+				break;
+		} else
+			udelay(wbuf[i+1]*1000);
+		i += 2;
+	}
+
+	return ret;
+}
+
+static int _ld9040_gamma_ctl(struct ld9040 *lcd, const unsigned int *gamma)
+{
+	unsigned int i = 0;
+	int ret = 0;
+
+	/* start gamma table updating. */
+	ret = ld9040_panel_send_sequence(lcd, seq_gamma_start);
+	if (ret) {
+		dev_err(lcd->dev, "failed to disable gamma table updating.\n");
+		goto gamma_err;
+	}
+
+	for (i = 0 ; i < GAMMA_TABLE_COUNT; i++) {
+		ret = ld9040_spi_write(lcd, DATA_ONLY, gamma[i]);
+		if (ret) {
+			dev_err(lcd->dev, "failed to set gamma table.\n");
+			goto gamma_err;
+		}
+	}
+
+	/* update gamma table. */
+	ret = ld9040_panel_send_sequence(lcd, seq_gamma_ctrl);
+	if (ret)
+		dev_err(lcd->dev, "failed to update gamma table.\n");
+
+gamma_err:
+	return ret;
+}
+
+static int ld9040_gamma_ctl(struct ld9040 *lcd, int gamma)
+{
+	int ret = 0;
+
+	ret = _ld9040_gamma_ctl(lcd, gamma_table.gamma_22_table[gamma]);
+
+	return ret;
+}
+
+
+static int ld9040_ldi_init(struct ld9040 *lcd)
+{
+	int ret, i;
+	static const unsigned short *init_seq[] = {
+		seq_user_setting,
+		seq_panel_condition,
+		seq_display_ctrl,
+		seq_manual_pwr,
+		seq_elvss_on,
+		seq_gtcon,
+		seq_gamma_set1,
+		seq_gamma_ctrl,
+		seq_sleep_out,
+	};
+
+	for (i = 0; i < ARRAY_SIZE(init_seq); i++) {
+		ret = ld9040_panel_send_sequence(lcd, init_seq[i]);
+		/* workaround: minimum delay time for transferring CMD */
+		udelay(300);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int ld9040_ldi_enable(struct ld9040 *lcd)
+{
+	int ret = 0;
+
+	ret = ld9040_panel_send_sequence(lcd, seq_display_on);
+
+	return ret;
+}
+
+static int ld9040_ldi_disable(struct ld9040 *lcd)
+{
+	int ret;
+
+	ret = ld9040_panel_send_sequence(lcd, seq_display_off);
+	ret = ld9040_panel_send_sequence(lcd, seq_sleep_in);
+
+	return ret;
+}
+
+static int ld9040_power_on(struct ld9040 *lcd)
+{
+	int ret = 0;
+	struct lcd_platform_data *pd = NULL;
+	pd = lcd->lcd_pd;
+	if (!pd) {
+		dev_err(lcd->dev, "platform data is NULL.\n");
+		return -EFAULT;
+	}
+
+	if (!pd->power_on) {
+		dev_err(lcd->dev, "power_on is NULL.\n");
+		return -EFAULT;
+	} else {
+		pd->power_on(lcd->ld, 1);
+		mdelay(pd->power_on_delay);
+	}
+
+	if (!pd->reset) {
+		dev_err(lcd->dev, "reset is NULL.\n");
+		return -EFAULT;
+	} else {
+		pd->reset(lcd->ld);
+		mdelay(pd->reset_delay);
+	}
+
+	ret = ld9040_ldi_init(lcd);
+	if (ret) {
+		dev_err(lcd->dev, "failed to initialize ldi.\n");
+		return ret;
+	}
+
+	ret = ld9040_ldi_enable(lcd);
+	if (ret) {
+		dev_err(lcd->dev, "failed to enable ldi.\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ld9040_power_off(struct ld9040 *lcd)
+{
+	int ret = 0;
+	struct lcd_platform_data *pd = NULL;
+
+	pd = lcd->lcd_pd;
+	if (!pd) {
+		dev_err(lcd->dev, "platform data is NULL.\n");
+		return -EFAULT;
+	}
+
+	ret = ld9040_ldi_disable(lcd);
+	if (ret) {
+		dev_err(lcd->dev, "lcd setting failed.\n");
+		return -EIO;
+	}
+
+	mdelay(pd->power_off_delay);
+
+	if (!pd->power_on) {
+		dev_err(lcd->dev, "power_on is NULL.\n");
+		return -EFAULT;
+	} else
+		pd->power_on(lcd->ld, 0);
+
+	return 0;
+}
+
+static int ld9040_power(struct ld9040 *lcd, int power)
+{
+	int ret = 0;
+
+	if (power_is_on(power) && !power_is_on(lcd->power))
+		ret = ld9040_power_on(lcd);
+	else if (!power_is_on(power) && power_is_on(lcd->power))
+		ret = ld9040_power_off(lcd);
+
+	if (!ret)
+		lcd->power = power;
+
+	return ret;
+}
+
+static int ld9040_set_power(struct lcd_device *ld, int power)
+{
+	struct ld9040 *lcd = lcd_get_data(ld);
+
+	if (power != FB_BLANK_UNBLANK && power != FB_BLANK_POWERDOWN &&
+		power != FB_BLANK_NORMAL) {
+		dev_err(lcd->dev, "power value should be 0, 1 or 4.\n");
+		return -EINVAL;
+	}
+
+	return ld9040_power(lcd, power);
+}
+
+static int ld9040_get_power(struct lcd_device *ld)
+{
+	struct ld9040 *lcd = lcd_get_data(ld);
+
+	return lcd->power;
+}
+
+static int ld9040_get_brightness(struct backlight_device *bd)
+{
+	return bd->props.brightness;
+}
+
+static int ld9040_set_brightness(struct backlight_device *bd)
+{
+	int ret = 0, brightness = bd->props.brightness;
+	struct ld9040 *lcd = bl_get_data(bd);
+
+	if (brightness < MIN_BRIGHTNESS ||
+		brightness > bd->props.max_brightness) {
+		dev_err(&bd->dev, "lcd brightness should be %d to %d.\n",
+			MIN_BRIGHTNESS, MAX_BRIGHTNESS);
+		return -EINVAL;
+	}
+
+	ret = ld9040_gamma_ctl(lcd, bd->props.brightness);
+	if (ret) {
+		dev_err(&bd->dev, "lcd brightness setting failed.\n");
+		return -EIO;
+	}
+
+	return ret;
+}
+
+static struct lcd_ops ld9040_lcd_ops = {
+	.set_power = ld9040_set_power,
+	.get_power = ld9040_get_power,
+};
+
+static const struct backlight_ops ld9040_backlight_ops  = {
+	.get_brightness = ld9040_get_brightness,
+	.update_status = ld9040_set_brightness,
+};
+
+
+static int ld9040_probe(struct spi_device *spi)
+{
+	int ret = 0;
+	struct ld9040 *lcd = NULL;
+	struct lcd_device *ld = NULL;
+	struct backlight_device *bd = NULL;
+	struct backlight_properties props;
+
+	lcd = kzalloc(sizeof(struct ld9040), GFP_KERNEL);
+	if (!lcd)
+		return -ENOMEM;
+
+	/* ld9040 lcd panel uses 3-wire 9bits SPI Mode. */
+	spi->bits_per_word = 9;
+
+	ret = spi_setup(spi);
+	if (ret < 0) {
+		dev_err(&spi->dev, "spi setup failed.\n");
+		goto out_free_lcd;
+	}
+
+	lcd->spi = spi;
+	lcd->dev = &spi->dev;
+
+	lcd->lcd_pd = spi->dev.platform_data;
+	if (!lcd->lcd_pd) {
+		dev_err(&spi->dev, "platform data is NULL.\n");
+		goto out_free_lcd;
+	}
+
+	ld = lcd_device_register("ld9040", &spi->dev, lcd, &ld9040_lcd_ops);
+	if (IS_ERR(ld)) {
+		ret = PTR_ERR(ld);
+		goto out_free_lcd;
+	}
+
+	lcd->ld = ld;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = MAX_BRIGHTNESS;
+
+	bd = backlight_device_register("ld9040-bl", &spi->dev,
+		lcd, &ld9040_backlight_ops, &props);
+	if (IS_ERR(bd)) {
+		ret = PTR_ERR(bd);
+		goto out_unregister_lcd;
+	}
+
+	bd->props.brightness = MAX_BRIGHTNESS;
+	lcd->bd = bd;
+
+	/*
+	 * if lcd panel was on from bootloader like u-boot then
+	 * do not lcd on.
+	 */
+	if (!lcd->lcd_pd->lcd_enabled) {
+		/*
+		 * if lcd panel was off from bootloader then
+		 * current lcd status is powerdown and then
+		 * it enables lcd panel.
+		 */
+		lcd->power = FB_BLANK_POWERDOWN;
+
+		ld9040_power(lcd, FB_BLANK_UNBLANK);
+	} else
+		lcd->power = FB_BLANK_UNBLANK;
+
+	dev_set_drvdata(&spi->dev, lcd);
+
+	dev_info(&spi->dev, "ld9040 panel driver has been probed.\n");
+	return 0;
+
+out_unregister_lcd:
+	lcd_device_unregister(lcd->ld);
+out_free_lcd:
+	kfree(lcd);
+	return ret;
+}
+
+static int __devexit ld9040_remove(struct spi_device *spi)
+{
+	struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+	ld9040_power(lcd, FB_BLANK_POWERDOWN);
+	backlight_device_unregister(lcd->bd);
+	lcd_device_unregister(lcd->ld);
+	kfree(lcd);
+
+	return 0;
+}
+
+#if defined(CONFIG_PM)
+static int ld9040_suspend(struct spi_device *spi, pm_message_t mesg)
+{
+	int ret = 0;
+	struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+	dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+
+	/*
+	 * when lcd panel is suspend, lcd panel becomes off
+	 * regardless of status.
+	 */
+	ret = ld9040_power(lcd, FB_BLANK_POWERDOWN);
+
+	return ret;
+}
+
+static int ld9040_resume(struct spi_device *spi)
+{
+	int ret = 0;
+	struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+	lcd->power = FB_BLANK_POWERDOWN;
+
+	ret = ld9040_power(lcd, FB_BLANK_UNBLANK);
+
+	return ret;
+}
+#else
+#define ld9040_suspend		NULL
+#define ld9040_resume		NULL
+#endif
+
+/* Power down all displays on reboot, poweroff or halt. */
+static void ld9040_shutdown(struct spi_device *spi)
+{
+	struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+	ld9040_power(lcd, FB_BLANK_POWERDOWN);
+}
+
+static struct spi_driver ld9040_driver = {
+	.driver = {
+		.name	= "ld9040",
+		.bus	= &spi_bus_type,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ld9040_probe,
+	.remove		= __devexit_p(ld9040_remove),
+	.shutdown	= ld9040_shutdown,
+	.suspend	= ld9040_suspend,
+	.resume		= ld9040_resume,
+};
+
+static int __init ld9040_init(void)
+{
+	return spi_register_driver(&ld9040_driver);
+}
+
+static void __exit ld9040_exit(void)
+{
+	spi_unregister_driver(&ld9040_driver);
+}
+
+module_init(ld9040_init);
+module_exit(ld9040_exit);
+
+MODULE_AUTHOR("Donghwa Lee <dh09.lee@samsung.com>");
+MODULE_DESCRIPTION("ld9040 LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/ld9040_gamma.h b/drivers/video/backlight/ld9040_gamma.h
new file mode 100644
index 000000000000..038d9c86ec03
--- /dev/null
+++ b/drivers/video/backlight/ld9040_gamma.h
@@ -0,0 +1,200 @@
+/*
+ * Gamma level definitions.
+ *
+ * Copyright (c) 2011 Samsung Electronics
+ * InKi Dae <inki.dae@samsung.com>
+ * Donghwa Lee <dh09.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef _LD9040_BRIGHTNESS_H
+#define _LD9040_BRIGHTNESS_H
+
+#define MAX_GAMMA_LEVEL		25
+#define GAMMA_TABLE_COUNT	21
+
+/* gamma value: 2.2 */
+static const unsigned int ld9040_22_300[] = {
+	0x00, 0xa7, 0xb4, 0xae, 0xbf, 0x00, 0x91,
+	0x00, 0xb2, 0xb4, 0xaa, 0xbb, 0x00, 0xac,
+	0x00, 0xb3, 0xb1, 0xaa, 0xbc, 0x00, 0xb3
+};
+
+static const unsigned int ld9040_22_290[] = {
+	0x00, 0xa9, 0xb7, 0xae, 0xbd, 0x00, 0x89,
+	0x00, 0xb7, 0xb6, 0xa8, 0xba, 0x00, 0xa4,
+	0x00, 0xb1, 0xb4, 0xaa, 0xbb, 0x00, 0xaa
+};
+
+static const unsigned int ld9040_22_280[] = {
+	0x00, 0xa9, 0xb6, 0xad, 0xbf, 0x00, 0x86,
+	0x00, 0xb8, 0xb5, 0xa8, 0xbc, 0x00, 0xa0,
+	0x00, 0xb3, 0xb3, 0xa9, 0xbc, 0x00, 0xa7
+};
+
+static const unsigned int ld9040_22_270[] = {
+	0x00, 0xa8, 0xb8, 0xae, 0xbe, 0x00, 0x84,
+	0x00, 0xb9, 0xb7, 0xa8, 0xbc, 0x00, 0x9d,
+	0x00, 0xb2, 0xb5, 0xaa, 0xbc, 0x00, 0xa4
+
+};
+static const unsigned int ld9040_22_260[] = {
+	0x00, 0xa4, 0xb8, 0xb0, 0xbf, 0x00, 0x80,
+	0x00, 0xb8, 0xb6, 0xaa, 0xbc, 0x00, 0x9a,
+	0x00, 0xb0, 0xb5, 0xab, 0xbd, 0x00, 0xa0
+};
+
+static const unsigned int ld9040_22_250[] = {
+	0x00, 0xa4, 0xb9, 0xaf, 0xc1, 0x00, 0x7d,
+	0x00, 0xb9, 0xb6, 0xaa, 0xbb, 0x00, 0x97,
+	0x00, 0xb1, 0xb5, 0xaa, 0xbf, 0x00, 0x9d
+};
+
+static const unsigned int ld9040_22_240[] = {
+	0x00, 0xa2, 0xb9, 0xaf, 0xc2, 0x00, 0x7a,
+	0x00, 0xb9, 0xb7, 0xaa, 0xbd, 0x00, 0x94,
+	0x00, 0xb0, 0xb5, 0xab, 0xbf, 0x00, 0x9a
+};
+
+static const unsigned int ld9040_22_230[] = {
+	0x00, 0xa0, 0xb9, 0xaf, 0xc3, 0x00, 0x77,
+	0x00, 0xb9, 0xb7, 0xab, 0xbe, 0x00, 0x90,
+	0x00, 0xb0, 0xb6, 0xab, 0xbf, 0x00, 0x97
+};
+
+static const unsigned int ld9040_22_220[] = {
+	0x00, 0x9e, 0xba, 0xb0, 0xc2, 0x00, 0x75,
+	0x00, 0xb9, 0xb8, 0xab, 0xbe, 0x00, 0x8e,
+	0x00, 0xb0, 0xb6, 0xac, 0xbf, 0x00, 0x94
+};
+
+static const unsigned int ld9040_22_210[] = {
+	0x00, 0x9c, 0xb9, 0xb0, 0xc4, 0x00, 0x72,
+	0x00, 0xb8, 0xb8, 0xac, 0xbf, 0x00, 0x8a,
+	0x00, 0xb0, 0xb6, 0xac, 0xc0, 0x00, 0x91
+};
+
+static const unsigned int ld9040_22_200[] = {
+	0x00, 0x9a, 0xba, 0xb1, 0xc4, 0x00, 0x6f,
+	0x00, 0xb8, 0xb8, 0xad, 0xc0, 0x00, 0x86,
+	0x00, 0xb0, 0xb7, 0xad, 0xc0, 0x00, 0x8d
+};
+
+static const unsigned int ld9040_22_190[] = {
+	0x00, 0x97, 0xba, 0xb2, 0xc5, 0x00, 0x6c,
+	0x00, 0xb8, 0xb8, 0xae, 0xc1, 0x00, 0x82,
+	0x00, 0xb0, 0xb6, 0xae, 0xc2, 0x00, 0x89
+};
+
+static const unsigned int ld9040_22_180[] = {
+	0x00, 0x93, 0xba, 0xb3, 0xc5, 0x00, 0x69,
+	0x00, 0xb8, 0xb9, 0xae, 0xc1, 0x00, 0x7f,
+	0x00, 0xb0, 0xb6, 0xae, 0xc3, 0x00, 0x85
+};
+
+static const unsigned int ld9040_22_170[] = {
+	0x00, 0x8b, 0xb9, 0xb3, 0xc7, 0x00, 0x65,
+	0x00, 0xb7, 0xb8, 0xaf, 0xc3, 0x00, 0x7a,
+	0x00, 0x80, 0xb6, 0xae, 0xc4, 0x00, 0x81
+};
+
+static const unsigned int ld9040_22_160[] = {
+	0x00, 0x89, 0xba, 0xb3, 0xc8, 0x00, 0x62,
+	0x00, 0xb6, 0xba, 0xaf, 0xc3, 0x00, 0x76,
+	0x00, 0xaf, 0xb7, 0xae, 0xc4, 0x00, 0x7e
+};
+
+static const unsigned int ld9040_22_150[] = {
+	0x00, 0x82, 0xba, 0xb4, 0xc7, 0x00, 0x5f,
+	0x00, 0xb5, 0xba, 0xb0, 0xc3, 0x00, 0x72,
+	0x00, 0xae, 0xb8, 0xb0, 0xc3, 0x00, 0x7a
+};
+
+static const unsigned int ld9040_22_140[] = {
+	0x00, 0x7b, 0xbb, 0xb4, 0xc8, 0x00, 0x5b,
+	0x00, 0xb5, 0xba, 0xb1, 0xc4, 0x00, 0x6e,
+	0x00, 0xae, 0xb9, 0xb0, 0xc5, 0x00, 0x75
+};
+
+static const unsigned int ld9040_22_130[] = {
+	0x00, 0x71, 0xbb, 0xb5, 0xc8, 0x00, 0x57,
+	0x00, 0xb5, 0xbb, 0xb0, 0xc5, 0x00, 0x6a,
+	0x00, 0xae, 0xb9, 0xb1, 0xc6, 0x00, 0x70
+};
+
+static const unsigned int ld9040_22_120[] = {
+	0x00, 0x47, 0xba, 0xb6, 0xca, 0x00, 0x53,
+	0x00, 0xb5, 0xbb, 0xb3, 0xc6, 0x00, 0x65,
+	0x00, 0xae, 0xb8, 0xb3, 0xc7, 0x00, 0x6c
+};
+
+static const unsigned int ld9040_22_110[] = {
+	0x00, 0x13, 0xbb, 0xb7, 0xca, 0x00, 0x4f,
+	0x00, 0xb4, 0xbb, 0xb3, 0xc7, 0x00, 0x60,
+	0x00, 0xad, 0xb8, 0xb4, 0xc7, 0x00, 0x67
+};
+
+static const unsigned int ld9040_22_100[] = {
+	0x00, 0x13, 0xba, 0xb8, 0xcb, 0x00, 0x4b,
+	0x00, 0xb3, 0xbc, 0xb4, 0xc7, 0x00, 0x5c,
+	0x00, 0xac, 0xb8, 0xb4, 0xc8, 0x00, 0x62
+};
+
+static const unsigned int ld9040_22_90[] = {
+	0x00, 0x13, 0xb9, 0xb8, 0xcd, 0x00, 0x46,
+	0x00, 0xb1, 0xbc, 0xb5, 0xc8, 0x00, 0x56,
+	0x00, 0xaa, 0xb8, 0xb4, 0xc9, 0x00, 0x5d
+};
+
+static const unsigned int ld9040_22_80[] = {
+	0x00, 0x13, 0xba, 0xb9, 0xcd, 0x00, 0x41,
+	0x00, 0xb0, 0xbe, 0xb5, 0xc9, 0x00, 0x51,
+	0x00, 0xa9, 0xb9, 0xb5, 0xca, 0x00, 0x57
+};
+
+static const unsigned int ld9040_22_70[] = {
+	0x00, 0x13, 0xb9, 0xb9, 0xd0, 0x00, 0x3c,
+	0x00, 0xaf, 0xbf, 0xb6, 0xcb, 0x00, 0x4b,
+	0x00, 0xa8, 0xb9, 0xb5, 0xcc, 0x00, 0x52
+};
+
+static const unsigned int ld9040_22_50[] = {
+	0x00, 0x13, 0xb2, 0xba, 0xd2, 0x00, 0x30,
+	0x00, 0xaf, 0xc0, 0xb8, 0xcd, 0x00, 0x3d,
+	0x00, 0xa8, 0xb8, 0xb7, 0xcd, 0x00, 0x44
+};
+
+struct ld9040_gamma {
+	unsigned int *gamma_22_table[MAX_GAMMA_LEVEL];
+} gamma_table = {
+	.gamma_22_table[0] = (unsigned int *)&ld9040_22_50,
+	.gamma_22_table[1] = (unsigned int *)&ld9040_22_70,
+	.gamma_22_table[2] = (unsigned int *)&ld9040_22_80,
+	.gamma_22_table[3] = (unsigned int *)&ld9040_22_90,
+	.gamma_22_table[4] = (unsigned int *)&ld9040_22_100,
+	.gamma_22_table[5] = (unsigned int *)&ld9040_22_110,
+	.gamma_22_table[6] = (unsigned int *)&ld9040_22_120,
+	.gamma_22_table[7] = (unsigned int *)&ld9040_22_130,
+	.gamma_22_table[8] = (unsigned int *)&ld9040_22_140,
+	.gamma_22_table[9] = (unsigned int *)&ld9040_22_150,
+	.gamma_22_table[10] = (unsigned int *)&ld9040_22_160,
+	.gamma_22_table[11] = (unsigned int *)&ld9040_22_170,
+	.gamma_22_table[12] = (unsigned int *)&ld9040_22_180,
+	.gamma_22_table[13] = (unsigned int *)&ld9040_22_190,
+	.gamma_22_table[14] = (unsigned int *)&ld9040_22_200,
+	.gamma_22_table[15] = (unsigned int *)&ld9040_22_210,
+	.gamma_22_table[16] = (unsigned int *)&ld9040_22_220,
+	.gamma_22_table[17] = (unsigned int *)&ld9040_22_230,
+	.gamma_22_table[18] = (unsigned int *)&ld9040_22_240,
+	.gamma_22_table[19] = (unsigned int *)&ld9040_22_250,
+	.gamma_22_table[20] = (unsigned int *)&ld9040_22_260,
+	.gamma_22_table[21] = (unsigned int *)&ld9040_22_270,
+	.gamma_22_table[22] = (unsigned int *)&ld9040_22_280,
+	.gamma_22_table[23] = (unsigned int *)&ld9040_22_290,
+	.gamma_22_table[24] = (unsigned int *)&ld9040_22_300,
+};
+
+#endif
diff --git a/drivers/video/backlight/locomolcd.c b/drivers/video/backlight/locomolcd.c
index d2f59015d517..be20b5cbe26c 100644
--- a/drivers/video/backlight/locomolcd.c
+++ b/drivers/video/backlight/locomolcd.c
@@ -6,7 +6,7 @@
  * GPL v2
  *
  * This driver assumes single CPU. That's okay, because collie is
- * slightly old hardware, and noone is going to retrofit second CPU to
+ * slightly old hardware, and no one is going to retrofit second CPU to
  * old PDA.
  */
 
@@ -184,6 +184,7 @@ static int locomolcd_probe(struct locomo_dev *ldev)
 	local_irq_restore(flags);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = 4;
 	locomolcd_bl_device = backlight_device_register("locomo-bl",
 							&ldev->dev, NULL,
diff --git a/drivers/video/backlight/ltv350qv.c b/drivers/video/backlight/ltv350qv.c
index 8010aaeb5adb..cca43c06d3c8 100644
--- a/drivers/video/backlight/ltv350qv.c
+++ b/drivers/video/backlight/ltv350qv.c
@@ -239,11 +239,15 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 	lcd->spi = spi;
 	lcd->power = FB_BLANK_POWERDOWN;
 	lcd->buffer = kzalloc(8, GFP_KERNEL);
+	if (!lcd->buffer) {
+		ret = -ENOMEM;
+		goto out_free_lcd;
+	}
 
 	ld = lcd_device_register("ltv350qv", &spi->dev, lcd, &ltv_ops);
 	if (IS_ERR(ld)) {
 		ret = PTR_ERR(ld);
-		goto out_free_lcd;
+		goto out_free_buffer;
 	}
 	lcd->ld = ld;
 
@@ -257,6 +261,8 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 
 out_unregister:
 	lcd_device_unregister(ld);
+out_free_buffer:
+	kfree(lcd->buffer);
 out_free_lcd:
 	kfree(lcd);
 	return ret;
@@ -268,6 +274,7 @@ static int __devexit ltv350qv_remove(struct spi_device *spi)
 
 	ltv350qv_power(lcd, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(lcd->ld);
+	kfree(lcd->buffer);
 	kfree(lcd);
 
 	return 0;
@@ -326,7 +333,7 @@ static void __exit ltv350qv_exit(void)
 module_init(ltv350qv_init);
 module_exit(ltv350qv_exit);
 
-MODULE_AUTHOR("Haavard Skinnemoen <hskinnemoen@atmel.com>");
+MODULE_AUTHOR("Haavard Skinnemoen (Atmel)");
 MODULE_DESCRIPTION("Samsung LTV350QV LCD Driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("spi:ltv350qv");
diff --git a/drivers/video/backlight/max8925_bl.c b/drivers/video/backlight/max8925_bl.c
index 209acc105cbc..07e8e273ced0 100644
--- a/drivers/video/backlight/max8925_bl.c
+++ b/drivers/video/backlight/max8925_bl.c
@@ -136,6 +136,7 @@ static int __devinit max8925_backlight_probe(struct platform_device *pdev)
 	data->current_brightness = 0;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = MAX_BRIGHTNESS;
 	bl = backlight_device_register(name, &pdev->dev, data,
 					&max8925_backlight_ops, &props);
diff --git a/drivers/video/backlight/mbp_nvidia_bl.c b/drivers/video/backlight/mbp_nvidia_bl.c
deleted file mode 100644
index 1485f7345f49..000000000000
--- a/drivers/video/backlight/mbp_nvidia_bl.c
+++ /dev/null
@@ -1,400 +0,0 @@
-/*
- *  Backlight Driver for Nvidia 8600 in Macbook Pro
- *
- *  Copyright (c) Red Hat <mjg@redhat.com>
- *  Based on code from Pommed:
- *  Copyright (C) 2006 Nicolas Boichat <nicolas @boichat.ch>
- *  Copyright (C) 2006 Felipe Alfaro Solana <felipe_alfaro @linuxmail.org>
- *  Copyright (C) 2007 Julien BLACHE <jb@jblache.org>
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License version 2 as
- *  published by the Free Software Foundation.
- *
- *  This driver triggers SMIs which cause the firmware to change the
- *  backlight brightness. This is icky in many ways, but it's impractical to
- *  get at the firmware code in order to figure out what it's actually doing.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/backlight.h>
-#include <linux/err.h>
-#include <linux/dmi.h>
-#include <linux/io.h>
-
-static struct backlight_device *mbp_backlight_device;
-
-/* Structure to be passed to the DMI_MATCH function. */
-struct dmi_match_data {
-	/* I/O resource to allocate. */
-	unsigned long iostart;
-	unsigned long iolen;
-	/* Backlight operations structure. */
-	const struct backlight_ops backlight_ops;
-};
-
-/* Module parameters. */
-static int debug;
-module_param_named(debug, debug, int, 0644);
-MODULE_PARM_DESC(debug, "Set to one to enable debugging messages.");
-
-/*
- * Implementation for MacBooks with Intel chipset.
- */
-static int intel_chipset_send_intensity(struct backlight_device *bd)
-{
-	int intensity = bd->props.brightness;
-
-	if (debug)
-		printk(KERN_DEBUG "mbp_nvidia_bl: setting brightness to %d\n",
-		       intensity);
-
-	outb(0x04 | (intensity << 4), 0xb3);
-	outb(0xbf, 0xb2);
-	return 0;
-}
-
-static int intel_chipset_get_intensity(struct backlight_device *bd)
-{
-	int intensity;
-
-	outb(0x03, 0xb3);
-	outb(0xbf, 0xb2);
-	intensity = inb(0xb3) >> 4;
-
-	if (debug)
-		printk(KERN_DEBUG "mbp_nvidia_bl: read brightness of %d\n",
-		       intensity);
-
-	return intensity;
-}
-
-static const struct dmi_match_data intel_chipset_data = {
-	.iostart = 0xb2,
-	.iolen = 2,
-	.backlight_ops	= {
-		.options	= BL_CORE_SUSPENDRESUME,
-		.get_brightness	= intel_chipset_get_intensity,
-		.update_status	= intel_chipset_send_intensity,
-	}
-};
-
-/*
- * Implementation for MacBooks with Nvidia chipset.
- */
-static int nvidia_chipset_send_intensity(struct backlight_device *bd)
-{
-	int intensity = bd->props.brightness;
-
-	if (debug)
-		printk(KERN_DEBUG "mbp_nvidia_bl: setting brightness to %d\n",
-		       intensity);
-
-	outb(0x04 | (intensity << 4), 0x52f);
-	outb(0xbf, 0x52e);
-	return 0;
-}
-
-static int nvidia_chipset_get_intensity(struct backlight_device *bd)
-{
-	int intensity;
-
-	outb(0x03, 0x52f);
-	outb(0xbf, 0x52e);
-	intensity = inb(0x52f) >> 4;
-
-	if (debug)
-		printk(KERN_DEBUG "mbp_nvidia_bl: read brightness of %d\n",
-		       intensity);
-
-	return intensity;
-}
-
-static const struct dmi_match_data nvidia_chipset_data = {
-	.iostart = 0x52e,
-	.iolen = 2,
-	.backlight_ops		= {
-		.options	= BL_CORE_SUSPENDRESUME,
-		.get_brightness	= nvidia_chipset_get_intensity,
-		.update_status	= nvidia_chipset_send_intensity
-	}
-};
-
-/*
- * DMI matching.
- */
-static /* const */ struct dmi_match_data *driver_data;
-
-static int mbp_dmi_match(const struct dmi_system_id *id)
-{
-	driver_data = id->driver_data;
-
-	printk(KERN_INFO "mbp_nvidia_bl: %s detected\n", id->ident);
-	return 1;
-}
-
-static const struct dmi_system_id __initdata mbp_device_table[] = {
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 1,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Computer, Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook1,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 2,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook2,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 3,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook3,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 4,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook4,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 4,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook4,2"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 1,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro1,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 1,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro1,2"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 2,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro2,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 2,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro2,2"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 3,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro3,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 3,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro3,2"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 4,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro4,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookAir 1,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir1,1"),
-		},
-		.driver_data	= (void *)&intel_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 5,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook5,1"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 5,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook5,2"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBook 6,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBook6,1"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookAir 2,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir2,1"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 5,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,1"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 5,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,2"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 5,3",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,3"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 5,4",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,4"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookPro 5,5",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,5"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookAir 3,1",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir3,1"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{
-		.callback	= mbp_dmi_match,
-		.ident		= "MacBookAir 3,2",
-		.matches	= {
-			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir3,2"),
-		},
-		.driver_data	= (void *)&nvidia_chipset_data,
-	},
-	{ }
-};
-
-static int __init mbp_init(void)
-{
-	struct backlight_properties props;
-	if (!dmi_check_system(mbp_device_table))
-		return -ENODEV;
-
-	if (!request_region(driver_data->iostart, driver_data->iolen, 
-						"Macbook Pro backlight"))
-		return -ENXIO;
-
-	memset(&props, 0, sizeof(struct backlight_properties));
-	props.max_brightness = 15;
-	mbp_backlight_device = backlight_device_register("mbp_backlight", NULL,
-							 NULL,
-							 &driver_data->backlight_ops,
-							 &props);
-	if (IS_ERR(mbp_backlight_device)) {
-		release_region(driver_data->iostart, driver_data->iolen);
-		return PTR_ERR(mbp_backlight_device);
-	}
-
-	mbp_backlight_device->props.brightness =
-		driver_data->backlight_ops.get_brightness(mbp_backlight_device);
-	backlight_update_status(mbp_backlight_device);
-
-	return 0;
-}
-
-static void __exit mbp_exit(void)
-{
-	backlight_device_unregister(mbp_backlight_device);
-
-	release_region(driver_data->iostart, driver_data->iolen);
-}
-
-module_init(mbp_init);
-module_exit(mbp_exit);
-
-MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
-MODULE_DESCRIPTION("Nvidia-based Macbook Pro Backlight Driver");
-MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(dmi, mbp_device_table);
diff --git a/drivers/video/backlight/omap1_bl.c b/drivers/video/backlight/omap1_bl.c
index d3bc56296c8d..08d26a72394c 100644
--- a/drivers/video/backlight/omap1_bl.c
+++ b/drivers/video/backlight/omap1_bl.c
@@ -146,6 +146,7 @@ static int omapbl_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = OMAPBL_MAX_INTENSITY;
 	dev = backlight_device_register("omap-bl", &pdev->dev, bl, &omapbl_ops,
 					&props);
diff --git a/drivers/video/backlight/pcf50633-backlight.c b/drivers/video/backlight/pcf50633-backlight.c
index 3c424f7efdcc..ef5628d60563 100644
--- a/drivers/video/backlight/pcf50633-backlight.c
+++ b/drivers/video/backlight/pcf50633-backlight.c
@@ -112,6 +112,7 @@ static int __devinit pcf50633_bl_probe(struct platform_device *pdev)
 	if (!pcf_bl)
 		return -ENOMEM;
 
+	bl_props.type = BACKLIGHT_RAW;
 	bl_props.max_brightness = 0x3f;
 	bl_props.power = FB_BLANK_UNBLANK;
 
diff --git a/drivers/video/backlight/progear_bl.c b/drivers/video/backlight/progear_bl.c
index 809278c90738..6af183d6465e 100644
--- a/drivers/video/backlight/progear_bl.c
+++ b/drivers/video/backlight/progear_bl.c
@@ -84,6 +84,7 @@ static int progearbl_probe(struct platform_device *pdev)
 	pci_write_config_byte(sb_dev, SB_MPS1, temp | 0x20);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = HW_LEVEL_MAX - HW_LEVEL_MIN;
 	progear_backlight_device = backlight_device_register("progear-bl",
 							     &pdev->dev, NULL,
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 21866ec69656..b8f38ec6eb18 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -28,6 +28,7 @@ struct pwm_bl_data {
 	unsigned int		lth_brightness;
 	int			(*notify)(struct device *,
 					  int brightness);
+	int			(*check_fb)(struct device *, struct fb_info *);
 };
 
 static int pwm_backlight_update_status(struct backlight_device *bl)
@@ -62,9 +63,18 @@ static int pwm_backlight_get_brightness(struct backlight_device *bl)
 	return bl->props.brightness;
 }
 
+static int pwm_backlight_check_fb(struct backlight_device *bl,
+				  struct fb_info *info)
+{
+	struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
+
+	return !pb->check_fb || pb->check_fb(pb->dev, info);
+}
+
 static const struct backlight_ops pwm_backlight_ops = {
 	.update_status	= pwm_backlight_update_status,
 	.get_brightness	= pwm_backlight_get_brightness,
+	.check_fb	= pwm_backlight_check_fb,
 };
 
 static int pwm_backlight_probe(struct platform_device *pdev)
@@ -95,6 +105,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 
 	pb->period = data->pwm_period_ns;
 	pb->notify = data->notify;
+	pb->check_fb = data->check_fb;
 	pb->lth_brightness = data->lth_brightness *
 		(data->pwm_period_ns / data->max_brightness);
 	pb->dev = &pdev->dev;
@@ -108,6 +119,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 		dev_dbg(&pdev->dev, "got pwm for backlight\n");
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = data->max_brightness;
 	bl = backlight_device_register(dev_name(&pdev->dev), &pdev->dev, pb,
 				       &pwm_backlight_ops, &props);
diff --git a/drivers/video/backlight/s6e63m0.c b/drivers/video/backlight/s6e63m0.c
index 5927db0da999..694e5aab0d69 100644
--- a/drivers/video/backlight/s6e63m0.c
+++ b/drivers/video/backlight/s6e63m0.c
@@ -738,6 +738,7 @@ static int __devinit s6e63m0_probe(struct spi_device *spi)
 	struct s6e63m0 *lcd = NULL;
 	struct lcd_device *ld = NULL;
 	struct backlight_device *bd = NULL;
+	struct backlight_properties props;
 
 	lcd = kzalloc(sizeof(struct s6e63m0), GFP_KERNEL);
 	if (!lcd)
@@ -769,14 +770,17 @@ static int __devinit s6e63m0_probe(struct spi_device *spi)
 
 	lcd->ld = ld;
 
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = MAX_BRIGHTNESS;
+
 	bd = backlight_device_register("s6e63m0bl-bl", &spi->dev, lcd,
-		&s6e63m0_backlight_ops, NULL);
+		&s6e63m0_backlight_ops, &props);
 	if (IS_ERR(bd)) {
 		ret =  PTR_ERR(bd);
 		goto out_lcd_unregister;
 	}
 
-	bd->props.max_brightness = MAX_BRIGHTNESS;
 	bd->props.brightness = MAX_BRIGHTNESS;
 	lcd->bd = bd;
 
@@ -839,7 +843,7 @@ static int __devexit s6e63m0_remove(struct spi_device *spi)
 }
 
 #if defined(CONFIG_PM)
-unsigned int before_power;
+static unsigned int before_power;
 
 static int s6e63m0_suspend(struct spi_device *spi, pm_message_t mesg)
 {
diff --git a/drivers/video/backlight/tosa_bl.c b/drivers/video/backlight/tosa_bl.c
index 2a04b382ec48..425a7365470b 100644
--- a/drivers/video/backlight/tosa_bl.c
+++ b/drivers/video/backlight/tosa_bl.c
@@ -102,6 +102,7 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
 	data->i2c = client;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = 512 - 1;
 	data->bl = backlight_device_register("tosa-bl", &client->dev, data,
 					     &bl_ops, &props);
diff --git a/drivers/video/backlight/wm831x_bl.c b/drivers/video/backlight/wm831x_bl.c
index 08fd87f3aecc..d4c6eb248ff9 100644
--- a/drivers/video/backlight/wm831x_bl.c
+++ b/drivers/video/backlight/wm831x_bl.c
@@ -193,6 +193,7 @@ static int wm831x_backlight_probe(struct platform_device *pdev)
 	data->current_brightness = 0;
 	data->isink_reg = isink_reg;
 
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = max_isel;
 	bl = backlight_device_register("wm831x", &pdev->dev, data,
 				       &wm831x_backlight_ops, &props);
diff --git a/drivers/video/bf537-lq035.c b/drivers/video/bf537-lq035.c
index 47c21fb2c82f..bea53c1a4950 100644
--- a/drivers/video/bf537-lq035.c
+++ b/drivers/video/bf537-lq035.c
@@ -789,6 +789,7 @@ static int __devinit bfin_lq035_probe(struct platform_device *pdev)
 	i2c_add_driver(&ad5280_driver);
 
 	memset(&props, 0, sizeof(props));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = MAX_BRIGHENESS;
 	bl_dev = backlight_device_register("bf537-bl", NULL, NULL,
 					   &bfin_lq035fb_bl_ops, &props);
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index e7d0f525041e..2464b910b590 100644
--- a/drivers/video/bf54x-lq043fb.c
+++ b/drivers/video/bf54x-lq043fb.c
@@ -649,6 +649,7 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 	}
 #ifndef NO_BL_SUPPORT
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = 255;
 	bl_dev = backlight_device_register("bf54x-bl", NULL, NULL,
 					   &bfin_lq043fb_bl_ops, &props);
diff --git a/drivers/video/bfin-lq035q1-fb.c b/drivers/video/bfin-lq035q1-fb.c
index c8e1f04941bd..23b6c4b62c78 100644
--- a/drivers/video/bfin-lq035q1-fb.c
+++ b/drivers/video/bfin-lq035q1-fb.c
@@ -154,8 +154,10 @@ static int __devinit lq035q1_spidev_probe(struct spi_device *spi)
 
 	ret = lq035q1_control(spi, LQ035_SHUT_CTL, LQ035_ON);
 	ret |= lq035q1_control(spi, LQ035_DRIVER_OUTPUT_CTL, ctl->mode);
-	if (ret)
+	if (ret) {
+		kfree(ctl);
 		return ret;
+	}
 
 	spi_set_drvdata(spi, ctl);
 
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 3cf77676947c..d8de29f0dd8d 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -545,6 +545,7 @@ static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
 	}
 #ifndef NO_BL_SUPPORT
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = 255;
 	bl_dev = backlight_device_register("bf52x-bl", NULL, NULL,
 					   &bfin_lq043fb_bl_ops, &props);
diff --git a/drivers/video/bfin_adv7393fb.h b/drivers/video/bfin_adv7393fb.h
index 8c7f9e4fc6eb..cd591b5152a5 100644
--- a/drivers/video/bfin_adv7393fb.h
+++ b/drivers/video/bfin_adv7393fb.h
@@ -87,12 +87,12 @@ static const u8 init_NTSC_TESTPATTERN[] = {
 
 static const u8 init_NTSC[] = {
 	0x00, 0x1E,	/* Power up all DACs and PLL */
-	0xC3, 0x26,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xC5, 0x12,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xC2, 0x4A,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xC6, 0x5E,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xBD, 0x19,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xBF, 0x42,	/* Program RGB->YCrCb Color Space convertion matrix */
+	0xC3, 0x26,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xC5, 0x12,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xC2, 0x4A,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xC6, 0x5E,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xBD, 0x19,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xBF, 0x42,	/* Program RGB->YCrCb Color Space conversion matrix */
 	0x8C, 0x1F,	/* NTSC Subcarrier Frequency */
 	0x8D, 0x7C,	/* NTSC Subcarrier Frequency */
 	0x8E, 0xF0,	/* NTSC Subcarrier Frequency */
@@ -109,12 +109,12 @@ static const u8 init_NTSC[] = {
 
 static const u8 init_PAL[] = {
 	0x00, 0x1E,	/* Power up all DACs and PLL */
-	0xC3, 0x26,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xC5, 0x12,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xC2, 0x4A,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xC6, 0x5E,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xBD, 0x19,	/* Program RGB->YCrCb Color Space convertion matrix */
-	0xBF, 0x42,	/* Program RGB->YCrCb Color Space convertion matrix */
+	0xC3, 0x26,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xC5, 0x12,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xC2, 0x4A,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xC6, 0x5E,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xBD, 0x19,	/* Program RGB->YCrCb Color Space conversion matrix */
+	0xBF, 0x42,	/* Program RGB->YCrCb Color Space conversion matrix */
 	0x8C, 0xCB,	/* PAL Subcarrier Frequency */
 	0x8D, 0x8A,	/* PAL Subcarrier Frequency */
 	0x8E, 0x09,	/* PAL Subcarrier Frequency */
diff --git a/drivers/video/broadsheetfb.c b/drivers/video/broadsheetfb.c
index ebda6876d3a9..377dde3d5bfc 100644
--- a/drivers/video/broadsheetfb.c
+++ b/drivers/video/broadsheetfb.c
@@ -1101,12 +1101,10 @@ static int __devinit broadsheetfb_probe(struct platform_device *dev)
 
 	videomemorysize = roundup((dpyw*dpyh), PAGE_SIZE);
 
-	videomemory = vmalloc(videomemorysize);
+	videomemory = vzalloc(videomemorysize);
 	if (!videomemory)
 		goto err_fb_rel;
 
-	memset(videomemory, 0, videomemorysize);
-
 	info->screen_base = (char *)videomemory;
 	info->fbops = &broadsheetfb_ops;
 
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 4dc13467281d..7ba74cd4be61 100644
--- a/drivers/video/bw2.c
+++ b/drivers/video/bw2.c
@@ -273,7 +273,7 @@ static int __devinit bw2_do_default_mode(struct bw2_par *par,
 	return 0;
 }
 
-static int __devinit bw2_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit bw2_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -375,7 +375,7 @@ static const struct of_device_id bw2_match[] = {
 };
 MODULE_DEVICE_TABLE(of, bw2_match);
 
-static struct of_platform_driver bw2_driver = {
+static struct platform_driver bw2_driver = {
 	.driver = {
 		.name = "bw2",
 		.owner = THIS_MODULE,
@@ -390,12 +390,12 @@ static int __init bw2_init(void)
 	if (fb_get_options("bw2fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&bw2_driver);
+	return platform_driver_register(&bw2_driver);
 }
 
 static void __exit bw2_exit(void)
 {
-	of_unregister_platform_driver(&bw2_driver);
+	platform_driver_unregister(&bw2_driver);
 }
 
 module_init(bw2_init);
diff --git a/drivers/video/cg14.c b/drivers/video/cg14.c
index 24249535ac86..f18895006627 100644
--- a/drivers/video/cg14.c
+++ b/drivers/video/cg14.c
@@ -463,7 +463,7 @@ static void cg14_unmap_regs(struct platform_device *op, struct fb_info *info,
 			   info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit cg14_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit cg14_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -565,6 +565,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
 	cg14_unmap_regs(op, info, par);
+	framebuffer_release(info);
 
 out_err:
 	return err;
@@ -595,7 +596,7 @@ static const struct of_device_id cg14_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg14_match);
 
-static struct of_platform_driver cg14_driver = {
+static struct platform_driver cg14_driver = {
 	.driver = {
 		.name = "cg14",
 		.owner = THIS_MODULE,
@@ -610,12 +611,12 @@ static int __init cg14_init(void)
 	if (fb_get_options("cg14fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&cg14_driver);
+	return platform_driver_register(&cg14_driver);
 }
 
 static void __exit cg14_exit(void)
 {
-	of_unregister_platform_driver(&cg14_driver);
+	platform_driver_unregister(&cg14_driver);
 }
 
 module_init(cg14_init);
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index 09c0c3c42482..f927a7b1a8d4 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -346,8 +346,7 @@ static int __devinit cg3_do_default_mode(struct cg3_par *par)
 	return 0;
 }
 
-static int __devinit cg3_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit cg3_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -462,7 +461,7 @@ static const struct of_device_id cg3_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg3_match);
 
-static struct of_platform_driver cg3_driver = {
+static struct platform_driver cg3_driver = {
 	.driver = {
 		.name = "cg3",
 		.owner = THIS_MODULE,
@@ -477,12 +476,12 @@ static int __init cg3_init(void)
 	if (fb_get_options("cg3fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&cg3_driver);
+	return platform_driver_register(&cg3_driver);
 }
 
 static void __exit cg3_exit(void)
 {
-	of_unregister_platform_driver(&cg3_driver);
+	platform_driver_unregister(&cg3_driver);
 }
 
 module_init(cg3_init);
diff --git a/drivers/video/cg6.c b/drivers/video/cg6.c
index 2b5a97058b08..179e96cdb323 100644
--- a/drivers/video/cg6.c
+++ b/drivers/video/cg6.c
@@ -737,8 +737,7 @@ static void cg6_unmap_regs(struct platform_device *op, struct fb_info *info,
 			   info->fix.smem_len);
 }
 
-static int __devinit cg6_probe(struct platform_device *op,
-				const struct of_device_id *match)
+static int __devinit cg6_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -822,6 +821,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
 	cg6_unmap_regs(op, info, par);
+	framebuffer_release(info);
 
 out_err:
 	return err;
@@ -855,7 +855,7 @@ static const struct of_device_id cg6_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg6_match);
 
-static struct of_platform_driver cg6_driver = {
+static struct platform_driver cg6_driver = {
 	.driver = {
 		.name = "cg6",
 		.owner = THIS_MODULE,
@@ -870,12 +870,12 @@ static int __init cg6_init(void)
 	if (fb_get_options("cg6fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&cg6_driver);
+	return platform_driver_register(&cg6_driver);
 }
 
 static void __exit cg6_exit(void)
 {
-	of_unregister_platform_driver(&cg6_driver);
+	platform_driver_unregister(&cg6_driver);
 }
 
 module_init(cg6_init);
diff --git a/drivers/video/cobalt_lcdfb.c b/drivers/video/cobalt_lcdfb.c
index 42fe155aba0e..e02764319ff7 100644
--- a/drivers/video/cobalt_lcdfb.c
+++ b/drivers/video/cobalt_lcdfb.c
@@ -303,7 +303,7 @@ static int __devinit cobalt_lcdfb_probe(struct platform_device *dev)
 		return -EBUSY;
 	}
 
-	info->screen_size = res->end - res->start + 1;
+	info->screen_size = resource_size(res);
 	info->screen_base = ioremap(res->start, info->screen_size);
 	info->fbops = &cobalt_lcd_fbops;
 	info->fix = cobalt_lcdfb_fix;
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index 9c092b8d64e6..8745637e4b7e 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -370,7 +370,6 @@ static void fb_flashcursor(struct work_struct *work)
 {
 	struct fb_info *info = container_of(work, struct fb_info, queue);
 	struct fbcon_ops *ops = info->fbcon_par;
-	struct display *p;
 	struct vc_data *vc = NULL;
 	int c;
 	int mode;
@@ -386,7 +385,6 @@ static void fb_flashcursor(struct work_struct *work)
 		return;
 	}
 
-	p = &fb_display[vc->vc_num];
 	c = scr_readw((u16 *) vc->vc_pos);
 	mode = (!ops->cursor_flash || ops->cursor_state.enable) ?
 		CM_ERASE : CM_DRAW;
@@ -823,10 +821,10 @@ static int set_con2fb_map(int unit, int newidx, int user)
 	if (oldidx == newidx)
 		return 0;
 
-	if (!info || fbcon_has_exited)
+	if (!info)
 		return -EINVAL;
 
- 	if (!err && !search_for_mapped_con()) {
+	if (!search_for_mapped_con() || !con_is_bound(&fb_con)) {
 		info_idx = newidx;
 		return fbcon_takeover(0);
 	}
diff --git a/drivers/video/console/font_mini_4x6.c b/drivers/video/console/font_mini_4x6.c
index a19a7f33133e..fa6e698e63c4 100644
--- a/drivers/video/console/font_mini_4x6.c
+++ b/drivers/video/console/font_mini_4x6.c
@@ -1,5 +1,5 @@
 
-/* Hand composed "Miniscule" 4x6 font, with binary data generated using
+/* Hand composed "Minuscule" 4x6 font, with binary data generated using
  * Perl stub.
  *
  * Use 'perl -x mini_4x6.c < mini_4x6.c > new_version.c' to regenerate
diff --git a/drivers/video/console/newport_con.c b/drivers/video/console/newport_con.c
index 3772433c49d1..93317b5b8740 100644
--- a/drivers/video/console/newport_con.c
+++ b/drivers/video/console/newport_con.c
@@ -6,7 +6,7 @@
  * 
  * This driver is based on sgicons.c and cons_newport.
  * 
- * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
+ * Copyright (C) 1996 David S. Miller (davem@davemloft.net)
  * Copyright (C) 1997 Miguel de Icaza (miguel@nuclecu.unam.mx)
  */
 #include <linux/init.h>
diff --git a/drivers/video/console/tileblit.c b/drivers/video/console/tileblit.c
index 0056a41e5c35..15e8e1a89c45 100644
--- a/drivers/video/console/tileblit.c
+++ b/drivers/video/console/tileblit.c
@@ -83,7 +83,7 @@ static void tile_cursor(struct vc_data *vc, struct fb_info *info, int mode,
 			int softback_lines, int fg, int bg)
 {
 	struct fb_tilecursor cursor;
-	int use_sw = (vc->vc_cursor_type & 0x01);
+	int use_sw = (vc->vc_cursor_type & 0x10);
 
 	cursor.sx = vc->vc_x;
 	cursor.sy = vc->vc_y;
diff --git a/drivers/video/controlfb.c b/drivers/video/controlfb.c
index c225dcce89e7..9075bea55879 100644
--- a/drivers/video/controlfb.c
+++ b/drivers/video/controlfb.c
@@ -709,11 +709,11 @@ static int __init control_of_init(struct device_node *dp)
 
 	/* Map in frame buffer and registers */
 	p->fb_orig_base = fb_res.start;
-	p->fb_orig_size = fb_res.end - fb_res.start + 1;
+	p->fb_orig_size = resource_size(&fb_res);
 	/* use the big-endian aperture (??) */
 	p->frame_buffer_phys = fb_res.start + 0x800000;
 	p->control_regs_phys = reg_res.start;
-	p->control_regs_size = reg_res.end - reg_res.start + 1;
+	p->control_regs_size = resource_size(&reg_res);
 
 	if (!p->fb_orig_base ||
 	    !request_mem_region(p->fb_orig_base,p->fb_orig_size,"controlfb")) {
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index 0c1afd13ddd3..850380795b05 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -47,6 +47,8 @@
 #include <linux/pci.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
 
 #include <asm/pgtable.h>
 #include <asm/system.h>
@@ -61,10 +63,10 @@ struct cfb_info {
 	struct fb_info		fb;
 	struct display_switch	*dispsw;
 	struct display		*display;
-	struct pci_dev		*dev;
 	unsigned char		__iomem *region;
 	unsigned char		__iomem *regs;
 	u_int			id;
+	u_int			irq;
 	int			func_use_count;
 	u_long			ref_ps;
 
@@ -88,6 +90,19 @@ struct cfb_info {
 	u_char			ramdac_powerdown;
 
 	u32			pseudo_palette[16];
+
+	spinlock_t		reg_b0_lock;
+
+#ifdef CONFIG_FB_CYBER2000_DDC
+	bool			ddc_registered;
+	struct i2c_adapter	ddc_adapter;
+	struct i2c_algo_bit_data	ddc_algo;
+#endif
+
+#ifdef CONFIG_FB_CYBER2000_I2C
+	struct i2c_adapter	i2c_adapter;
+	struct i2c_algo_bit_data i2c_algo;
+#endif
 };
 
 static char *default_font = "Acorn8x8";
@@ -494,6 +509,7 @@ static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
 	cyber2000_attrw(0x14, 0x00, cfb);
 
 	/* PLL registers */
+	spin_lock(&cfb->reg_b0_lock);
 	cyber2000_grphw(EXT_DCLK_MULT, hw->clock_mult, cfb);
 	cyber2000_grphw(EXT_DCLK_DIV, hw->clock_div, cfb);
 	cyber2000_grphw(EXT_MCLK_MULT, cfb->mclk_mult, cfb);
@@ -501,6 +517,7 @@ static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
 	cyber2000_grphw(0x90, 0x01, cfb);
 	cyber2000_grphw(0xb9, 0x80, cfb);
 	cyber2000_grphw(0xb9, 0x00, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
 
 	cfb->ramdac_ctrl = hw->ramdac;
 	cyber2000fb_write_ramdac_ctrl(cfb);
@@ -681,9 +698,9 @@ cyber2000fb_decode_clock(struct par_info *hw, struct cfb_info *cfb,
 	 *    pll_ps_calc = best_div1 / (ref_ps * best_mult)
 	 */
 	best_diff = 0x7fffffff;
-	best_mult = 32;
-	best_div1 = 255;
-	for (t_div1 = 32; t_div1 > 1; t_div1 -= 1) {
+	best_mult = 2;
+	best_div1 = 32;
+	for (t_div1 = 2; t_div1 < 32; t_div1 += 1) {
 		u_int rr, t_mult, t_pll_ps;
 		int diff;
 
@@ -1105,24 +1122,22 @@ void cyber2000fb_disable_extregs(struct cfb_info *cfb)
 }
 EXPORT_SYMBOL(cyber2000fb_disable_extregs);
 
-void cyber2000fb_get_fb_var(struct cfb_info *cfb, struct fb_var_screeninfo *var)
-{
-	memcpy(var, &cfb->fb.var, sizeof(struct fb_var_screeninfo));
-}
-EXPORT_SYMBOL(cyber2000fb_get_fb_var);
-
 /*
  * Attach a capture/tv driver to the core CyberX0X0 driver.
  */
 int cyber2000fb_attach(struct cyberpro_info *info, int idx)
 {
 	if (int_cfb_info != NULL) {
-		info->dev	      = int_cfb_info->dev;
+		info->dev	      = int_cfb_info->fb.device;
+#ifdef CONFIG_FB_CYBER2000_I2C
+		info->i2c	      = &int_cfb_info->i2c_adapter;
+#else
+		info->i2c	      = NULL;
+#endif
 		info->regs	      = int_cfb_info->regs;
+		info->irq             = int_cfb_info->irq;
 		info->fb	      = int_cfb_info->fb.screen_base;
 		info->fb_size	      = int_cfb_info->fb.fix.smem_len;
-		info->enable_extregs  = cyber2000fb_enable_extregs;
-		info->disable_extregs = cyber2000fb_disable_extregs;
 		info->info	      = int_cfb_info;
 
 		strlcpy(info->dev_name, int_cfb_info->fb.fix.id,
@@ -1141,6 +1156,183 @@ void cyber2000fb_detach(int idx)
 }
 EXPORT_SYMBOL(cyber2000fb_detach);
 
+#ifdef CONFIG_FB_CYBER2000_DDC
+
+#define DDC_REG		0xb0
+#define DDC_SCL_OUT	(1 << 0)
+#define DDC_SDA_OUT	(1 << 4)
+#define DDC_SCL_IN	(1 << 2)
+#define DDC_SDA_IN	(1 << 6)
+
+static void cyber2000fb_enable_ddc(struct cfb_info *cfb)
+{
+	spin_lock(&cfb->reg_b0_lock);
+	cyber2000fb_writew(0x1bf, 0x3ce, cfb);
+}
+
+static void cyber2000fb_disable_ddc(struct cfb_info *cfb)
+{
+	cyber2000fb_writew(0x0bf, 0x3ce, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
+}
+
+
+static void cyber2000fb_ddc_setscl(void *data, int val)
+{
+	struct cfb_info *cfb = data;
+	unsigned char reg;
+
+	cyber2000fb_enable_ddc(cfb);
+	reg = cyber2000_grphr(DDC_REG, cfb);
+	if (!val)	/* bit is inverted */
+		reg |= DDC_SCL_OUT;
+	else
+		reg &= ~DDC_SCL_OUT;
+	cyber2000_grphw(DDC_REG, reg, cfb);
+	cyber2000fb_disable_ddc(cfb);
+}
+
+static void cyber2000fb_ddc_setsda(void *data, int val)
+{
+	struct cfb_info *cfb = data;
+	unsigned char reg;
+
+	cyber2000fb_enable_ddc(cfb);
+	reg = cyber2000_grphr(DDC_REG, cfb);
+	if (!val)	/* bit is inverted */
+		reg |= DDC_SDA_OUT;
+	else
+		reg &= ~DDC_SDA_OUT;
+	cyber2000_grphw(DDC_REG, reg, cfb);
+	cyber2000fb_disable_ddc(cfb);
+}
+
+static int cyber2000fb_ddc_getscl(void *data)
+{
+	struct cfb_info *cfb = data;
+	int retval;
+
+	cyber2000fb_enable_ddc(cfb);
+	retval = !!(cyber2000_grphr(DDC_REG, cfb) & DDC_SCL_IN);
+	cyber2000fb_disable_ddc(cfb);
+
+	return retval;
+}
+
+static int cyber2000fb_ddc_getsda(void *data)
+{
+	struct cfb_info *cfb = data;
+	int retval;
+
+	cyber2000fb_enable_ddc(cfb);
+	retval = !!(cyber2000_grphr(DDC_REG, cfb) & DDC_SDA_IN);
+	cyber2000fb_disable_ddc(cfb);
+
+	return retval;
+}
+
+static int __devinit cyber2000fb_setup_ddc_bus(struct cfb_info *cfb)
+{
+	strlcpy(cfb->ddc_adapter.name, cfb->fb.fix.id,
+		sizeof(cfb->ddc_adapter.name));
+	cfb->ddc_adapter.owner		= THIS_MODULE;
+	cfb->ddc_adapter.class		= I2C_CLASS_DDC;
+	cfb->ddc_adapter.algo_data	= &cfb->ddc_algo;
+	cfb->ddc_adapter.dev.parent	= cfb->fb.device;
+	cfb->ddc_algo.setsda		= cyber2000fb_ddc_setsda;
+	cfb->ddc_algo.setscl		= cyber2000fb_ddc_setscl;
+	cfb->ddc_algo.getsda		= cyber2000fb_ddc_getsda;
+	cfb->ddc_algo.getscl		= cyber2000fb_ddc_getscl;
+	cfb->ddc_algo.udelay		= 10;
+	cfb->ddc_algo.timeout		= 20;
+	cfb->ddc_algo.data		= cfb;
+
+	i2c_set_adapdata(&cfb->ddc_adapter, cfb);
+
+	return i2c_bit_add_bus(&cfb->ddc_adapter);
+}
+#endif /* CONFIG_FB_CYBER2000_DDC */
+
+#ifdef CONFIG_FB_CYBER2000_I2C
+static void cyber2000fb_i2c_setsda(void *data, int state)
+{
+	struct cfb_info *cfb = data;
+	unsigned int latch2;
+
+	spin_lock(&cfb->reg_b0_lock);
+	latch2 = cyber2000_grphr(EXT_LATCH2, cfb);
+	latch2 &= EXT_LATCH2_I2C_CLKEN;
+	if (state)
+		latch2 |= EXT_LATCH2_I2C_DATEN;
+	cyber2000_grphw(EXT_LATCH2, latch2, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
+}
+
+static void cyber2000fb_i2c_setscl(void *data, int state)
+{
+	struct cfb_info *cfb = data;
+	unsigned int latch2;
+
+	spin_lock(&cfb->reg_b0_lock);
+	latch2 = cyber2000_grphr(EXT_LATCH2, cfb);
+	latch2 &= EXT_LATCH2_I2C_DATEN;
+	if (state)
+		latch2 |= EXT_LATCH2_I2C_CLKEN;
+	cyber2000_grphw(EXT_LATCH2, latch2, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
+}
+
+static int cyber2000fb_i2c_getsda(void *data)
+{
+	struct cfb_info *cfb = data;
+	int ret;
+
+	spin_lock(&cfb->reg_b0_lock);
+	ret = !!(cyber2000_grphr(EXT_LATCH2, cfb) & EXT_LATCH2_I2C_DAT);
+	spin_unlock(&cfb->reg_b0_lock);
+
+	return ret;
+}
+
+static int cyber2000fb_i2c_getscl(void *data)
+{
+	struct cfb_info *cfb = data;
+	int ret;
+
+	spin_lock(&cfb->reg_b0_lock);
+	ret = !!(cyber2000_grphr(EXT_LATCH2, cfb) & EXT_LATCH2_I2C_CLK);
+	spin_unlock(&cfb->reg_b0_lock);
+
+	return ret;
+}
+
+static int __devinit cyber2000fb_i2c_register(struct cfb_info *cfb)
+{
+	strlcpy(cfb->i2c_adapter.name, cfb->fb.fix.id,
+		sizeof(cfb->i2c_adapter.name));
+	cfb->i2c_adapter.owner = THIS_MODULE;
+	cfb->i2c_adapter.algo_data = &cfb->i2c_algo;
+	cfb->i2c_adapter.dev.parent = cfb->fb.device;
+	cfb->i2c_algo.setsda = cyber2000fb_i2c_setsda;
+	cfb->i2c_algo.setscl = cyber2000fb_i2c_setscl;
+	cfb->i2c_algo.getsda = cyber2000fb_i2c_getsda;
+	cfb->i2c_algo.getscl = cyber2000fb_i2c_getscl;
+	cfb->i2c_algo.udelay = 5;
+	cfb->i2c_algo.timeout = msecs_to_jiffies(100);
+	cfb->i2c_algo.data = cfb;
+
+	return i2c_bit_add_bus(&cfb->i2c_adapter);
+}
+
+static void cyber2000fb_i2c_unregister(struct cfb_info *cfb)
+{
+	i2c_del_adapter(&cfb->i2c_adapter);
+}
+#else
+#define cyber2000fb_i2c_register(cfb)	(0)
+#define cyber2000fb_i2c_unregister(cfb)	do { } while (0)
+#endif
+
 /*
  * These parameters give
  * 640x480, hsync 31.5kHz, vsync 60Hz
@@ -1275,6 +1467,8 @@ static struct cfb_info __devinit *cyberpro_alloc_fb_info(unsigned int id,
 	cfb->fb.flags		= FBINFO_DEFAULT | FBINFO_HWACCEL_YPAN;
 	cfb->fb.pseudo_palette	= cfb->pseudo_palette;
 
+	spin_lock_init(&cfb->reg_b0_lock);
+
 	fb_alloc_cmap(&cfb->fb.cmap, NR_PALETTE, 0);
 
 	return cfb;
@@ -1369,6 +1563,11 @@ static int __devinit cyberpro_common_probe(struct cfb_info *cfb)
 	cfb->fb.fix.mmio_len   = MMIO_SIZE;
 	cfb->fb.screen_base    = cfb->region;
 
+#ifdef CONFIG_FB_CYBER2000_DDC
+	if (cyber2000fb_setup_ddc_bus(cfb) == 0)
+		cfb->ddc_registered = true;
+#endif
+
 	err = -EINVAL;
 	if (!fb_find_mode(&cfb->fb.var, &cfb->fb, NULL, NULL, 0,
 			  &cyber2000fb_default_mode, 8)) {
@@ -1401,14 +1600,32 @@ static int __devinit cyberpro_common_probe(struct cfb_info *cfb)
 		cfb->fb.var.xres, cfb->fb.var.yres,
 		h_sync / 1000, h_sync % 1000, v_sync);
 
-	if (cfb->dev)
-		cfb->fb.device = &cfb->dev->dev;
+	err = cyber2000fb_i2c_register(cfb);
+	if (err)
+		goto failed;
+
 	err = register_framebuffer(&cfb->fb);
+	if (err)
+		cyber2000fb_i2c_unregister(cfb);
 
 failed:
+#ifdef CONFIG_FB_CYBER2000_DDC
+	if (err && cfb->ddc_registered)
+		i2c_del_adapter(&cfb->ddc_adapter);
+#endif
 	return err;
 }
 
+static void __devexit cyberpro_common_remove(struct cfb_info *cfb)
+{
+	unregister_framebuffer(&cfb->fb);
+#ifdef CONFIG_FB_CYBER2000_DDC
+	if (cfb->ddc_registered)
+		i2c_del_adapter(&cfb->ddc_adapter);
+#endif
+	cyber2000fb_i2c_unregister(cfb);
+}
+
 static void cyberpro_common_resume(struct cfb_info *cfb)
 {
 	cyberpro_init_hw(cfb);
@@ -1442,12 +1659,13 @@ static int __devinit cyberpro_vl_probe(void)
 	if (!cfb)
 		goto failed_release;
 
-	cfb->dev = NULL;
+	cfb->irq = -1;
 	cfb->region = ioremap(FB_START, FB_SIZE);
 	if (!cfb->region)
 		goto failed_ioremap;
 
 	cfb->regs = cfb->region + MMIO_OFFSET;
+	cfb->fb.device = NULL;
 	cfb->fb.fix.mmio_start = FB_START + MMIO_OFFSET;
 	cfb->fb.fix.smem_start = FB_START;
 
@@ -1585,12 +1803,13 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (err)
 		goto failed_regions;
 
-	cfb->dev = dev;
+	cfb->irq = dev->irq;
 	cfb->region = pci_ioremap_bar(dev, 0);
 	if (!cfb->region)
 		goto failed_ioremap;
 
 	cfb->regs = cfb->region + MMIO_OFFSET;
+	cfb->fb.device = &dev->dev;
 	cfb->fb.fix.mmio_start = pci_resource_start(dev, 0) + MMIO_OFFSET;
 	cfb->fb.fix.smem_start = pci_resource_start(dev, 0);
 
@@ -1648,15 +1867,7 @@ static void __devexit cyberpro_pci_remove(struct pci_dev *dev)
 	struct cfb_info *cfb = pci_get_drvdata(dev);
 
 	if (cfb) {
-		/*
-		 * If unregister_framebuffer fails, then
-		 * we will be leaving hooks that could cause
-		 * oopsen laying around.
-		 */
-		if (unregister_framebuffer(&cfb->fb))
-			printk(KERN_WARNING "%s: danger Will Robinson, "
-				"danger danger!  Oopsen imminent!\n",
-				cfb->fb.fix.id);
+		cyberpro_common_remove(cfb);
 		iounmap(cfb->region);
 		cyberpro_free_fb_info(cfb);
 
diff --git a/drivers/video/cyber2000fb.h b/drivers/video/cyber2000fb.h
index de4fc43e51c1..bad69102e774 100644
--- a/drivers/video/cyber2000fb.h
+++ b/drivers/video/cyber2000fb.h
@@ -464,12 +464,14 @@ static void debug_printf(char *fmt, ...)
 struct cfb_info;
 
 struct cyberpro_info {
-	struct pci_dev	*dev;
+	struct device	*dev;
+	struct i2c_adapter *i2c;
 	unsigned char	__iomem *regs;
 	char		__iomem *fb;
 	char		dev_name[32];
 	unsigned int	fb_size;
 	unsigned int	chip_id;
+	unsigned int	irq;
 
 	/*
 	 * The following is a pointer to be passed into the
@@ -478,15 +480,6 @@ struct cyberpro_info {
 	 * is within this structure.
 	 */
 	struct cfb_info *info;
-
-	/*
-	 * Use these to enable the BM or TV registers.  In an SMP
-	 * environment, these two function pointers should only be
-	 * called from the module_init() or module_exit()
-	 * functions.
-	 */
-	void (*enable_extregs)(struct cfb_info *);
-	void (*disable_extregs)(struct cfb_info *);
 };
 
 #define ID_IGA_1682		0
@@ -494,8 +487,6 @@ struct cyberpro_info {
 #define ID_CYBERPRO_2010	2
 #define ID_CYBERPRO_5000	3
 
-struct fb_var_screeninfo;
-
 /*
  * Note! Writing to the Cyber20x0 registers from an interrupt
  * routine is definitely a bad idea atm.
@@ -504,4 +495,3 @@ int cyber2000fb_attach(struct cyberpro_info *info, int idx);
 void cyber2000fb_detach(int idx);
 void cyber2000fb_enable_extregs(struct cfb_info *cfb);
 void cyber2000fb_disable_extregs(struct cfb_info *cfb);
-void cyber2000fb_get_fb_var(struct cfb_info *cfb, struct fb_var_screeninfo *var);
diff --git a/drivers/video/da8xx-fb.c b/drivers/video/da8xx-fb.c
index 8d61ef96eedd..fcdac872522d 100644
--- a/drivers/video/da8xx-fb.c
+++ b/drivers/video/da8xx-fb.c
@@ -763,7 +763,7 @@ static int fb_wait_for_vsync(struct fb_info *info)
 
 	/*
 	 * Set flag to 0 and wait for isr to set to 1. It would seem there is a
-	 * race condition here where the ISR could have occured just before or
+	 * race condition here where the ISR could have occurred just before or
 	 * just after this set. But since we are just coarsely waiting for
 	 * a frame to complete then that's OK. i.e. if the frame completed
 	 * just before this code executed then we have to wait another full
@@ -899,7 +899,7 @@ static struct fb_ops da8xx_fb_ops = {
 	.fb_blank = cfb_blank,
 };
 
-static int __init fb_probe(struct platform_device *device)
+static int __devinit fb_probe(struct platform_device *device)
 {
 	struct da8xx_lcdc_platform_data *fb_pdata =
 						device->dev.platform_data;
@@ -1165,7 +1165,7 @@ static int fb_resume(struct platform_device *dev)
 
 static struct platform_driver da8xx_fb_driver = {
 	.probe = fb_probe,
-	.remove = fb_remove,
+	.remove = __devexit_p(fb_remove),
 	.suspend = fb_suspend,
 	.resume = fb_resume,
 	.driver = {
diff --git a/drivers/video/display/display-sysfs.c b/drivers/video/display/display-sysfs.c
index f6a09ab0dac6..0c647d7af0ee 100644
--- a/drivers/video/display/display-sysfs.c
+++ b/drivers/video/display/display-sysfs.c
@@ -182,7 +182,7 @@ void display_device_unregister(struct display_device *ddev)
 	mutex_lock(&ddev->lock);
 	device_unregister(ddev->dev);
 	mutex_unlock(&ddev->lock);
-	// Mark device index as avaliable
+	// Mark device index as available
 	mutex_lock(&allocated_dsp_lock);
 	idr_remove(&allocated_dsp, ddev->idx);
 	mutex_unlock(&allocated_dsp_lock);
diff --git a/drivers/video/edid.h b/drivers/video/edid.h
index bd89fb3be8c2..d03a232d90b2 100644
--- a/drivers/video/edid.h
+++ b/drivers/video/edid.h
@@ -101,8 +101,8 @@
 #define V_SYNC_WIDTH       COMBINE_HI_4LO( V_SYNC_WIDTH_HI, V_SYNC_WIDTH_LO )
 #define V_SYNC_OFFSET      COMBINE_HI_4LO( V_SYNC_OFFSET_HI, V_SYNC_OFFSET_LO )
 
-#define H_SYNC_WIDTH       COMBINE_HI_4LO( H_SYNC_WIDTH_HI, H_SYNC_WIDTH_LO )
-#define H_SYNC_OFFSET      COMBINE_HI_4LO( H_SYNC_OFFSET_HI, H_SYNC_OFFSET_LO )
+#define H_SYNC_WIDTH       COMBINE_HI_8LO( H_SYNC_WIDTH_HI, H_SYNC_WIDTH_LO )
+#define H_SYNC_OFFSET      COMBINE_HI_8LO( H_SYNC_OFFSET_HI, H_SYNC_OFFSET_LO )
 
 #define H_SIZE_LO          (unsigned)block[ 12 ]
 #define V_SIZE_LO          (unsigned)block[ 13 ]
diff --git a/drivers/video/efifb.c b/drivers/video/efifb.c
index 70477c2e4b61..784139aed079 100644
--- a/drivers/video/efifb.c
+++ b/drivers/video/efifb.c
@@ -16,6 +16,8 @@
 #include <linux/pci.h>
 #include <video/vga.h>
 
+static bool request_mem_succeeded = false;
+
 static struct fb_var_screeninfo efifb_defined __devinitdata = {
 	.activate		= FB_ACTIVATE_NOW,
 	.height			= -1,
@@ -53,6 +55,7 @@ enum {
 	M_MB_7_1,	/* MacBook, 7th rev. */
 	M_MB_SR,	/* MacBook, 2nd gen, (Santa Rosa) */
 	M_MBA,		/* MacBook Air */
+	M_MBA_3,	/* Macbook Air, 3rd rev */
 	M_MBP,		/* MacBook Pro */
 	M_MBP_2,	/* MacBook Pro 2nd gen */
 	M_MBP_2_2,	/* MacBook Pro 2,2nd gen */
@@ -64,43 +67,54 @@ enum {
 	M_MBP_6_1,	/* MacBook Pro, 6,1th gen */
 	M_MBP_6_2,	/* MacBook Pro, 6,2th gen */
 	M_MBP_7_1,	/* MacBook Pro, 7,1th gen */
+	M_MBP_8_2,	/* MacBook Pro, 8,2nd gen */
 	M_UNKNOWN	/* placeholder */
 };
 
+#define OVERRIDE_NONE	0x0
+#define OVERRIDE_BASE	0x1
+#define OVERRIDE_STRIDE	0x2
+#define OVERRIDE_HEIGHT	0x4
+#define OVERRIDE_WIDTH	0x8
+
 static struct efifb_dmi_info {
 	char *optname;
 	unsigned long base;
 	int stride;
 	int width;
 	int height;
+	int flags;
 } dmi_list[] __initdata = {
-	[M_I17] = { "i17", 0x80010000, 1472 * 4, 1440, 900 },
-	[M_I20] = { "i20", 0x80010000, 1728 * 4, 1680, 1050 }, /* guess */
-	[M_I20_SR] = { "imac7", 0x40010000, 1728 * 4, 1680, 1050 },
-	[M_I24] = { "i24", 0x80010000, 2048 * 4, 1920, 1200 }, /* guess */
-	[M_I24_8_1] = { "imac8", 0xc0060000, 2048 * 4, 1920, 1200 },
-	[M_I24_10_1] = { "imac10", 0xc0010000, 2048 * 4, 1920, 1080 },
-	[M_I27_11_1] = { "imac11", 0xc0010000, 2560 * 4, 2560, 1440 },
-	[M_MINI]= { "mini", 0x80000000, 2048 * 4, 1024, 768 },
-	[M_MINI_3_1] = { "mini31", 0x40010000, 1024 * 4, 1024, 768 },
-	[M_MINI_4_1] = { "mini41", 0xc0010000, 2048 * 4, 1920, 1200 },
-	[M_MB] = { "macbook", 0x80000000, 2048 * 4, 1280, 800 },
-	[M_MB_5_1] = { "macbook51", 0x80010000, 2048 * 4, 1280, 800 },
-	[M_MB_6_1] = { "macbook61", 0x80010000, 2048 * 4, 1280, 800 },
-	[M_MB_7_1] = { "macbook71", 0x80010000, 2048 * 4, 1280, 800 },
-	[M_MBA] = { "mba", 0x80000000, 2048 * 4, 1280, 800 },
-	[M_MBP] = { "mbp", 0x80010000, 1472 * 4, 1440, 900 },
-	[M_MBP_2] = { "mbp2", 0, 0, 0, 0 }, /* placeholder */
-	[M_MBP_2_2] = { "mbp22", 0x80010000, 1472 * 4, 1440, 900 },
-	[M_MBP_SR] = { "mbp3", 0x80030000, 2048 * 4, 1440, 900 },
-	[M_MBP_4] = { "mbp4", 0xc0060000, 2048 * 4, 1920, 1200 },
-	[M_MBP_5_1] = { "mbp51", 0xc0010000, 2048 * 4, 1440, 900 },
-	[M_MBP_5_2] = { "mbp52", 0xc0010000, 2048 * 4, 1920, 1200 },
-	[M_MBP_5_3] = { "mbp53", 0xd0010000, 2048 * 4, 1440, 900 },
-	[M_MBP_6_1] = { "mbp61", 0x90030000, 2048 * 4, 1920, 1200 },
-	[M_MBP_6_2] = { "mbp62", 0x90030000, 2048 * 4, 1680, 1050 },
-	[M_MBP_7_1] = { "mbp71", 0xc0010000, 2048 * 4, 1280, 800 },
-	[M_UNKNOWN] = { NULL, 0, 0, 0, 0 }
+	[M_I17] = { "i17", 0x80010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_I20] = { "i20", 0x80010000, 1728 * 4, 1680, 1050, OVERRIDE_NONE }, /* guess */
+	[M_I20_SR] = { "imac7", 0x40010000, 1728 * 4, 1680, 1050, OVERRIDE_NONE },
+	[M_I24] = { "i24", 0x80010000, 2048 * 4, 1920, 1200, OVERRIDE_NONE }, /* guess */
+	[M_I24_8_1] = { "imac8", 0xc0060000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+	[M_I24_10_1] = { "imac10", 0xc0010000, 2048 * 4, 1920, 1080, OVERRIDE_NONE },
+	[M_I27_11_1] = { "imac11", 0xc0010000, 2560 * 4, 2560, 1440, OVERRIDE_NONE },
+	[M_MINI]= { "mini", 0x80000000, 2048 * 4, 1024, 768, OVERRIDE_NONE },
+	[M_MINI_3_1] = { "mini31", 0x40010000, 1024 * 4, 1024, 768, OVERRIDE_NONE },
+	[M_MINI_4_1] = { "mini41", 0xc0010000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+	[M_MB] = { "macbook", 0x80000000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+	[M_MB_5_1] = { "macbook51", 0x80010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+	[M_MB_6_1] = { "macbook61", 0x80010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+	[M_MB_7_1] = { "macbook71", 0x80010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+	[M_MBA] = { "mba", 0x80000000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+	/* 11" Macbook Air 3,1 passes the wrong stride */
+	[M_MBA_3] = { "mba3", 0, 2048 * 4, 0, 0, OVERRIDE_STRIDE },
+	[M_MBP] = { "mbp", 0x80010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_MBP_2] = { "mbp2", 0, 0, 0, 0, OVERRIDE_NONE }, /* placeholder */
+	[M_MBP_2_2] = { "mbp22", 0x80010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_MBP_SR] = { "mbp3", 0x80030000, 2048 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_MBP_4] = { "mbp4", 0xc0060000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+	[M_MBP_5_1] = { "mbp51", 0xc0010000, 2048 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_MBP_5_2] = { "mbp52", 0xc0010000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+	[M_MBP_5_3] = { "mbp53", 0xd0010000, 2048 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_MBP_6_1] = { "mbp61", 0x90030000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+	[M_MBP_6_2] = { "mbp62", 0x90030000, 2048 * 4, 1680, 1050, OVERRIDE_NONE },
+	[M_MBP_7_1] = { "mbp71", 0xc0010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+	[M_MBP_8_2] = { "mbp82", 0x90010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+	[M_UNKNOWN] = { NULL, 0, 0, 0, 0, OVERRIDE_NONE }
 };
 
 static int set_system(const struct dmi_system_id *id);
@@ -138,6 +152,7 @@ static const struct dmi_system_id dmi_system_table[] __initconst = {
 	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook6,1", M_MB_6_1),
 	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook7,1", M_MB_7_1),
 	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookAir1,1", M_MBA),
+	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookAir3,1", M_MBA_3),
 	EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro1,1", M_MBP),
 	EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro2,1", M_MBP_2),
 	EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro2,2", M_MBP_2_2),
@@ -151,19 +166,26 @@ static const struct dmi_system_id dmi_system_table[] __initconst = {
 	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro6,1", M_MBP_6_1),
 	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro6,2", M_MBP_6_2),
 	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro7,1", M_MBP_7_1),
+	EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro8,2", M_MBP_8_2),
 	{},
 };
 
+#define choose_value(dmivalue, fwvalue, field, flags) ({	\
+		typeof(fwvalue) _ret_ = fwvalue;		\
+		if ((flags) & (field))				\
+			_ret_ = dmivalue;			\
+		else if ((fwvalue) == 0)			\
+			_ret_ = dmivalue;			\
+		_ret_;						\
+	})
+
 static int set_system(const struct dmi_system_id *id)
 {
 	struct efifb_dmi_info *info = id->driver_data;
-	if (info->base == 0)
-		return 0;
 
-	printk(KERN_INFO "efifb: dmi detected %s - framebuffer at %p "
-			 "(%dx%d, stride %d)\n", id->ident,
-			 (void *)info->base, info->width, info->height,
-			 info->stride);
+	if (info->base == 0 && info->height == 0 && info->width == 0
+			&& info->stride == 0)
+		return 0;
 
 	/* Trust the bootloader over the DMI tables */
 	if (screen_info.lfb_base == 0) {
@@ -171,40 +193,47 @@ static int set_system(const struct dmi_system_id *id)
 		struct pci_dev *dev = NULL;
 		int found_bar = 0;
 #endif
-		screen_info.lfb_base = info->base;
+		if (info->base) {
+			screen_info.lfb_base = choose_value(info->base,
+				screen_info.lfb_base, OVERRIDE_BASE,
+				info->flags);
 
 #if defined(CONFIG_PCI)
-		/* make sure that the address in the table is actually on a
-		 * VGA device's PCI BAR */
-
-		for_each_pci_dev(dev) {
-			int i;
-			if ((dev->class >> 8) != PCI_CLASS_DISPLAY_VGA)
-				continue;
-			for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
-				resource_size_t start, end;
-
-				start = pci_resource_start(dev, i);
-				if (start == 0)
-					break;
-				end = pci_resource_end(dev, i);
-				if (screen_info.lfb_base >= start &&
-						screen_info.lfb_base < end) {
-					found_bar = 1;
+			/* make sure that the address in the table is actually
+			 * on a VGA device's PCI BAR */
+
+			for_each_pci_dev(dev) {
+				int i;
+				if ((dev->class >> 8) != PCI_CLASS_DISPLAY_VGA)
+					continue;
+				for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
+					resource_size_t start, end;
+
+					start = pci_resource_start(dev, i);
+					if (start == 0)
+						break;
+					end = pci_resource_end(dev, i);
+					if (screen_info.lfb_base >= start &&
+					    screen_info.lfb_base < end) {
+						found_bar = 1;
+					}
 				}
 			}
-		}
-		if (!found_bar)
-			screen_info.lfb_base = 0;
+			if (!found_bar)
+				screen_info.lfb_base = 0;
 #endif
+		}
 	}
 	if (screen_info.lfb_base) {
-		if (screen_info.lfb_linelength == 0)
-			screen_info.lfb_linelength = info->stride;
-		if (screen_info.lfb_width == 0)
-			screen_info.lfb_width = info->width;
-		if (screen_info.lfb_height == 0)
-			screen_info.lfb_height = info->height;
+		screen_info.lfb_linelength = choose_value(info->stride,
+			screen_info.lfb_linelength, OVERRIDE_STRIDE,
+			info->flags);
+		screen_info.lfb_width = choose_value(info->width,
+			screen_info.lfb_width, OVERRIDE_WIDTH,
+			info->flags);
+		screen_info.lfb_height = choose_value(info->height,
+			screen_info.lfb_height, OVERRIDE_HEIGHT,
+			info->flags);
 		if (screen_info.orig_video_isVGA == 0)
 			screen_info.orig_video_isVGA = VIDEO_TYPE_EFI;
 	} else {
@@ -214,6 +243,13 @@ static int set_system(const struct dmi_system_id *id)
 		screen_info.orig_video_isVGA = 0;
 		return 0;
 	}
+
+	printk(KERN_INFO "efifb: dmi detected %s - framebuffer at 0x%08x "
+			 "(%dx%d, stride %d)\n", id->ident,
+			 screen_info.lfb_base, screen_info.lfb_width,
+			 screen_info.lfb_height, screen_info.lfb_linelength);
+
+
 	return 1;
 }
 
@@ -247,7 +283,9 @@ static void efifb_destroy(struct fb_info *info)
 {
 	if (info->screen_base)
 		iounmap(info->screen_base);
-	release_mem_region(info->apertures->ranges[0].base, info->apertures->ranges[0].size);
+	if (request_mem_succeeded)
+		release_mem_region(info->apertures->ranges[0].base,
+				   info->apertures->ranges[0].size);
 	framebuffer_release(info);
 }
 
@@ -292,14 +330,13 @@ static int __init efifb_setup(char *options)
 	return 0;
 }
 
-static int __devinit efifb_probe(struct platform_device *dev)
+static int __init efifb_probe(struct platform_device *dev)
 {
 	struct fb_info *info;
 	int err;
 	unsigned int size_vmode;
 	unsigned int size_remap;
 	unsigned int size_total;
-	int request_succeeded = 0;
 
 	if (!screen_info.lfb_depth)
 		screen_info.lfb_depth = 32;
@@ -353,7 +390,7 @@ static int __devinit efifb_probe(struct platform_device *dev)
 	efifb_fix.smem_len = size_remap;
 
 	if (request_mem_region(efifb_fix.smem_start, size_remap, "efifb")) {
-		request_succeeded = 1;
+		request_mem_succeeded = true;
 	} else {
 		/* We cannot make this fatal. Sometimes this comes from magic
 		   spaces our resource handlers simply don't know about */
@@ -379,7 +416,7 @@ static int __devinit efifb_probe(struct platform_device *dev)
 	info->apertures->ranges[0].base = efifb_fix.smem_start;
 	info->apertures->ranges[0].size = size_remap;
 
-	info->screen_base = ioremap(efifb_fix.smem_start, efifb_fix.smem_len);
+	info->screen_base = ioremap_wc(efifb_fix.smem_start, efifb_fix.smem_len);
 	if (!info->screen_base) {
 		printk(KERN_ERR "efifb: abort, cannot ioremap video memory "
 				"0x%x @ 0x%lx\n",
@@ -457,13 +494,12 @@ err_unmap:
 err_release_fb:
 	framebuffer_release(info);
 err_release_mem:
-	if (request_succeeded)
+	if (request_mem_succeeded)
 		release_mem_region(efifb_fix.smem_start, size_total);
 	return err;
 }
 
 static struct platform_driver efifb_driver = {
-	.probe	= efifb_probe,
 	.driver	= {
 		.name	= "efifb",
 	},
@@ -494,13 +530,21 @@ static int __init efifb_init(void)
 	if (!screen_info.lfb_linelength)
 		return -ENODEV;
 
-	ret = platform_driver_register(&efifb_driver);
+	ret = platform_device_register(&efifb_device);
+	if (ret)
+		return ret;
 
-	if (!ret) {
-		ret = platform_device_register(&efifb_device);
-		if (ret)
-			platform_driver_unregister(&efifb_driver);
+	/*
+	 * This is not just an optimization.  We will interfere
+	 * with a real driver if we get reprobed, so don't allow
+	 * it.
+	 */
+	ret = platform_driver_probe(&efifb_driver, efifb_probe);
+	if (ret) {
+		platform_device_unregister(&efifb_device);
+		return ret;
 	}
+
 	return ret;
 }
 module_init(efifb_init);
diff --git a/drivers/video/ep93xx-fb.c b/drivers/video/ep93xx-fb.c
index b358d045f130..40e5f17d1e4b 100644
--- a/drivers/video/ep93xx-fb.c
+++ b/drivers/video/ep93xx-fb.c
@@ -4,7 +4,7 @@
  * Framebuffer support for the EP93xx series.
  *
  * Copyright (C) 2007 Bluewater Systems Ltd
- * Author: Ryan Mallon <ryan@bluewatersys.com>
+ * Author: Ryan Mallon
  *
  * Copyright (c) 2009 H Hartley Sweeten <hsweeten@visionengravers.com>
  *
@@ -456,7 +456,7 @@ static int __init ep93xxfb_alloc_videomem(struct fb_info *info)
 	 * There is a bug in the ep93xx framebuffer which causes problems
 	 * if bit 27 of the physical address is set.
 	 * See: http://marc.info/?l=linux-arm-kernel&m=110061245502000&w=2
-	 * There does not seem to be any offical errata for this, but I
+	 * There does not seem to be any official errata for this, but I
 	 * have confirmed the problem exists on my hardware (ep9315) at
 	 * least.
 	 */
@@ -644,6 +644,6 @@ module_exit(ep93xxfb_exit);
 
 MODULE_DESCRIPTION("EP93XX Framebuffer Driver");
 MODULE_ALIAS("platform:ep93xx-fb");
-MODULE_AUTHOR("Ryan Mallon <ryan&bluewatersys.com>, "
+MODULE_AUTHOR("Ryan Mallon, "
 	      "H Hartley Sweeten <hsweeten@visionengravers.com");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fb-puv3.c b/drivers/video/fb-puv3.c
new file mode 100644
index 000000000000..27f2c57e06e9
--- /dev/null
+++ b/drivers/video/fb-puv3.c
@@ -0,0 +1,852 @@
+/*
+ * Frame Buffer Driver for PKUnity-v3 Unigfx
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *	Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *	Copyright (C) 2001-2010 Guan Xuetao
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/console.h>
+
+#include <asm/sizes.h>
+#include <mach/hardware.h>
+
+/* Platform_data reserved for unifb registers. */
+#define UNIFB_REGS_NUM		10
+/* RAM reserved for the frame buffer. */
+#define UNIFB_MEMSIZE		(SZ_4M)		/* 4 MB for 1024*768*32b */
+
+/*
+ * cause UNIGFX don not have EDID
+ * all the modes are organized as follow
+ */
+static const struct fb_videomode unifb_modes[] = {
+	/* 0 640x480-60 VESA */
+	{ "640x480@60",  60,  640, 480,  25175000,  48, 16, 34, 10,  96, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 1 640x480-75 VESA */
+	{ "640x480@75",  75,  640, 480,  31500000, 120, 16, 18,  1,  64, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 2 800x600-60 VESA */
+	{ "800x600@60",  60,  800, 600,  40000000,  88, 40, 26,  1, 128, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 3 800x600-75 VESA */
+	{ "800x600@75",  75,  800, 600,  49500000, 160, 16, 23,  1,  80, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 4 1024x768-60 VESA */
+	{ "1024x768@60", 60, 1024, 768,  65000000, 160, 24, 34,  3, 136, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 5 1024x768-75 VESA */
+	{ "1024x768@75", 75, 1024, 768,  78750000, 176, 16, 30,  1,  96, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 6 1280x960-60 VESA */
+	{ "1280x960@60", 60, 1280, 960, 108000000, 312, 96, 38,  1, 112, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 7 1440x900-60 VESA */
+	{ "1440x900@60", 60, 1440, 900, 106500000, 232, 80, 30,  3, 152, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 8 FIXME 9 1024x600-60 VESA UNTESTED */
+	{ "1024x600@60", 60, 1024, 600,  50650000, 160, 24, 26,  1, 136, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 9 FIXME 10 1024x600-75 VESA UNTESTED */
+	{ "1024x600@75", 75, 1024, 600,  61500000, 176, 16, 23,  1,  96, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 10 FIXME 11 1366x768-60 VESA UNTESTED */
+	{ "1366x768@60", 60, 1366, 768,  85500000, 256, 58, 18,  1,  112, 3,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+};
+
+static struct fb_var_screeninfo unifb_default = {
+	.xres =		640,
+	.yres =		480,
+	.xres_virtual =	640,
+	.yres_virtual =	480,
+	.bits_per_pixel = 16,
+	.red =		{ 11, 5, 0 },
+	.green =	{ 5,  6, 0 },
+	.blue =		{ 0,  5, 0 },
+	.activate =	FB_ACTIVATE_NOW,
+	.height =	-1,
+	.width =	-1,
+	.pixclock =	25175000,
+	.left_margin =	48,
+	.right_margin =	16,
+	.upper_margin =	33,
+	.lower_margin =	10,
+	.hsync_len =	96,
+	.vsync_len =	2,
+	.vmode =	FB_VMODE_NONINTERLACED,
+};
+
+static struct fb_fix_screeninfo unifb_fix = {
+	.id =		"UNIGFX FB",
+	.type =		FB_TYPE_PACKED_PIXELS,
+	.visual =	FB_VISUAL_TRUECOLOR,
+	.xpanstep =	1,
+	.ypanstep =	1,
+	.ywrapstep =	1,
+	.accel =	FB_ACCEL_NONE,
+};
+
+static void unifb_sync(struct fb_info *info)
+{
+	/* TODO: may, this can be replaced by interrupt */
+	int cnt;
+
+	for (cnt = 0; cnt < 0x10000000; cnt++) {
+		if (readl(UGE_COMMAND) & 0x1000000)
+			return;
+	}
+
+	if (cnt > 0x8000000)
+		dev_warn(info->device, "Warning: UniGFX GE time out ...\n");
+}
+
+static void unifb_prim_fillrect(struct fb_info *info,
+				const struct fb_fillrect *region)
+{
+	int awidth = region->width;
+	int aheight = region->height;
+	int m_iBpp = info->var.bits_per_pixel;
+	int screen_width = info->var.xres;
+	int src_sel = 1;	/* from fg_color */
+	int pat_sel = 1;
+	int src_x0 = 0;
+	int dst_x0 = region->dx;
+	int src_y0 = 0;
+	int dst_y0 = region->dy;
+	int rop_alpha_sel = 0;
+	int rop_alpha_code = 0xCC;
+	int x_dir = 1;
+	int y_dir = 1;
+	int alpha_r = 0;
+	int alpha_sel = 0;
+	int dst_pitch = screen_width * (m_iBpp / 8);
+	int dst_offset = dst_y0 * dst_pitch + dst_x0 * (m_iBpp / 8);
+	int src_pitch = screen_width * (m_iBpp / 8);
+	int src_offset = src_y0 * src_pitch + src_x0 * (m_iBpp / 8);
+	unsigned int command = 0;
+	int clip_region = 0;
+	int clip_en = 0;
+	int tp_en = 0;
+	int fg_color = 0;
+	int bottom = info->var.yres - 1;
+	int right = info->var.xres - 1;
+	int top = 0;
+
+	bottom = (bottom << 16) | right;
+	command = (rop_alpha_sel << 26) | (pat_sel << 18) | (src_sel << 16)
+		| (x_dir << 20) | (y_dir << 21) | (command << 24)
+		| (clip_region << 23) | (clip_en << 22) | (tp_en << 27);
+	src_pitch = (dst_pitch << 16) | src_pitch;
+	awidth = awidth | (aheight << 16);
+	alpha_r = ((rop_alpha_code & 0xff) << 8) | (alpha_r & 0xff)
+		| (alpha_sel << 16);
+	src_x0 = (src_x0 & 0x1fff) | ((src_y0 & 0x1fff) << 16);
+	dst_x0 = (dst_x0 & 0x1fff) | ((dst_y0 & 0x1fff) << 16);
+	fg_color = region->color;
+
+	unifb_sync(info);
+
+	writel(((u32 *)(info->pseudo_palette))[fg_color], UGE_FCOLOR);
+	writel(0, UGE_BCOLOR);
+	writel(src_pitch, UGE_PITCH);
+	writel(src_offset, UGE_SRCSTART);
+	writel(dst_offset, UGE_DSTSTART);
+	writel(awidth, UGE_WIDHEIGHT);
+	writel(top, UGE_CLIP0);
+	writel(bottom, UGE_CLIP1);
+	writel(alpha_r, UGE_ROPALPHA);
+	writel(src_x0, UGE_SRCXY);
+	writel(dst_x0, UGE_DSTXY);
+	writel(command, UGE_COMMAND);
+}
+
+static void unifb_fillrect(struct fb_info *info,
+		const struct fb_fillrect *region)
+{
+	struct fb_fillrect modded;
+	int vxres, vyres;
+
+	if (info->flags & FBINFO_HWACCEL_DISABLED) {
+		sys_fillrect(info, region);
+		return;
+	}
+
+	vxres = info->var.xres_virtual;
+	vyres = info->var.yres_virtual;
+
+	memcpy(&modded, region, sizeof(struct fb_fillrect));
+
+	if (!modded.width || !modded.height ||
+	    modded.dx >= vxres || modded.dy >= vyres)
+		return;
+
+	if (modded.dx + modded.width > vxres)
+		modded.width = vxres - modded.dx;
+	if (modded.dy + modded.height > vyres)
+		modded.height = vyres - modded.dy;
+
+	unifb_prim_fillrect(info, &modded);
+}
+
+static void unifb_prim_copyarea(struct fb_info *info,
+				const struct fb_copyarea *area)
+{
+	int awidth = area->width;
+	int aheight = area->height;
+	int m_iBpp = info->var.bits_per_pixel;
+	int screen_width = info->var.xres;
+	int src_sel = 2;	/* from mem */
+	int pat_sel = 0;
+	int src_x0 = area->sx;
+	int dst_x0 = area->dx;
+	int src_y0 = area->sy;
+	int dst_y0 = area->dy;
+
+	int rop_alpha_sel = 0;
+	int rop_alpha_code = 0xCC;
+	int x_dir = 1;
+	int y_dir = 1;
+
+	int alpha_r = 0;
+	int alpha_sel = 0;
+	int dst_pitch = screen_width * (m_iBpp / 8);
+	int dst_offset = dst_y0 * dst_pitch + dst_x0 * (m_iBpp / 8);
+	int src_pitch = screen_width * (m_iBpp / 8);
+	int src_offset = src_y0 * src_pitch + src_x0 * (m_iBpp / 8);
+	unsigned int command = 0;
+	int clip_region = 0;
+	int clip_en = 1;
+	int tp_en = 0;
+	int top = 0;
+	int bottom = info->var.yres;
+	int right = info->var.xres;
+	int fg_color = 0;
+	int bg_color = 0;
+
+	if (src_x0 < 0)
+		src_x0 = 0;
+	if (src_y0 < 0)
+		src_y0 = 0;
+
+	if (src_y0 - dst_y0 > 0) {
+		y_dir = 1;
+	} else {
+		y_dir = 0;
+		src_offset = (src_y0 + aheight) * src_pitch +
+				src_x0 * (m_iBpp / 8);
+		dst_offset = (dst_y0 + aheight) * dst_pitch +
+				dst_x0 * (m_iBpp / 8);
+		src_y0 += aheight;
+		dst_y0 += aheight;
+	}
+
+	command = (rop_alpha_sel << 26) | (pat_sel << 18) | (src_sel << 16) |
+		(x_dir << 20) | (y_dir << 21) | (command << 24) |
+		(clip_region << 23) | (clip_en << 22) | (tp_en << 27);
+	src_pitch = (dst_pitch << 16) | src_pitch;
+	awidth = awidth | (aheight << 16);
+	alpha_r = ((rop_alpha_code & 0xff) << 8) | (alpha_r & 0xff) |
+		(alpha_sel << 16);
+	src_x0 = (src_x0 & 0x1fff) | ((src_y0 & 0x1fff) << 16);
+	dst_x0 = (dst_x0 & 0x1fff) | ((dst_y0 & 0x1fff) << 16);
+	bottom = (bottom << 16) | right;
+
+	unifb_sync(info);
+
+	writel(src_pitch, UGE_PITCH);
+	writel(src_offset, UGE_SRCSTART);
+	writel(dst_offset, UGE_DSTSTART);
+	writel(awidth, UGE_WIDHEIGHT);
+	writel(top, UGE_CLIP0);
+	writel(bottom, UGE_CLIP1);
+	writel(bg_color, UGE_BCOLOR);
+	writel(fg_color, UGE_FCOLOR);
+	writel(alpha_r, UGE_ROPALPHA);
+	writel(src_x0, UGE_SRCXY);
+	writel(dst_x0, UGE_DSTXY);
+	writel(command, UGE_COMMAND);
+}
+
+static void unifb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
+{
+	struct fb_copyarea modded;
+	u32 vxres, vyres;
+	modded.sx = area->sx;
+	modded.sy = area->sy;
+	modded.dx = area->dx;
+	modded.dy = area->dy;
+	modded.width = area->width;
+	modded.height = area->height;
+
+	if (info->flags & FBINFO_HWACCEL_DISABLED) {
+		sys_copyarea(info, area);
+		return;
+	}
+
+	vxres = info->var.xres_virtual;
+	vyres = info->var.yres_virtual;
+
+	if (!modded.width || !modded.height ||
+	    modded.sx >= vxres || modded.sy >= vyres ||
+	    modded.dx >= vxres || modded.dy >= vyres)
+		return;
+
+	if (modded.sx + modded.width > vxres)
+		modded.width = vxres - modded.sx;
+	if (modded.dx + modded.width > vxres)
+		modded.width = vxres - modded.dx;
+	if (modded.sy + modded.height > vyres)
+		modded.height = vyres - modded.sy;
+	if (modded.dy + modded.height > vyres)
+		modded.height = vyres - modded.dy;
+
+	unifb_prim_copyarea(info, &modded);
+}
+
+static void unifb_imageblit(struct fb_info *info, const struct fb_image *image)
+{
+	sys_imageblit(info, image);
+}
+
+static u_long get_line_length(int xres_virtual, int bpp)
+{
+	u_long length;
+
+	length = xres_virtual * bpp;
+	length = (length + 31) & ~31;
+	length >>= 3;
+	return length;
+}
+
+/*
+ *  Setting the video mode has been split into two parts.
+ *  First part, xxxfb_check_var, must not write anything
+ *  to hardware, it should only verify and adjust var.
+ *  This means it doesn't alter par but it does use hardware
+ *  data from it to check this var.
+ */
+static int unifb_check_var(struct fb_var_screeninfo *var,
+			 struct fb_info *info)
+{
+	u_long line_length;
+
+	/*
+	 *  FB_VMODE_CONUPDATE and FB_VMODE_SMOOTH_XPAN are equal!
+	 *  as FB_VMODE_SMOOTH_XPAN is only used internally
+	 */
+
+	if (var->vmode & FB_VMODE_CONUPDATE) {
+		var->vmode |= FB_VMODE_YWRAP;
+		var->xoffset = info->var.xoffset;
+		var->yoffset = info->var.yoffset;
+	}
+
+	/*
+	 *  Some very basic checks
+	 */
+	if (!var->xres)
+		var->xres = 1;
+	if (!var->yres)
+		var->yres = 1;
+	if (var->xres > var->xres_virtual)
+		var->xres_virtual = var->xres;
+	if (var->yres > var->yres_virtual)
+		var->yres_virtual = var->yres;
+	if (var->bits_per_pixel <= 1)
+		var->bits_per_pixel = 1;
+	else if (var->bits_per_pixel <= 8)
+		var->bits_per_pixel = 8;
+	else if (var->bits_per_pixel <= 16)
+		var->bits_per_pixel = 16;
+	else if (var->bits_per_pixel <= 24)
+		var->bits_per_pixel = 24;
+	else if (var->bits_per_pixel <= 32)
+		var->bits_per_pixel = 32;
+	else
+		return -EINVAL;
+
+	if (var->xres_virtual < var->xoffset + var->xres)
+		var->xres_virtual = var->xoffset + var->xres;
+	if (var->yres_virtual < var->yoffset + var->yres)
+		var->yres_virtual = var->yoffset + var->yres;
+
+	/*
+	 *  Memory limit
+	 */
+	line_length =
+	    get_line_length(var->xres_virtual, var->bits_per_pixel);
+	if (line_length * var->yres_virtual > UNIFB_MEMSIZE)
+		return -ENOMEM;
+
+	/*
+	 * Now that we checked it we alter var. The reason being is that the
+	 * video mode passed in might not work but slight changes to it might
+	 * make it work. This way we let the user know what is acceptable.
+	 */
+	switch (var->bits_per_pixel) {
+	case 1:
+	case 8:
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 0;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+	case 16:		/* RGBA 5551 */
+		if (var->transp.length) {
+			var->red.offset = 0;
+			var->red.length = 5;
+			var->green.offset = 5;
+			var->green.length = 5;
+			var->blue.offset = 10;
+			var->blue.length = 5;
+			var->transp.offset = 15;
+			var->transp.length = 1;
+		} else {	/* RGB 565 */
+			var->red.offset = 11;
+			var->red.length = 5;
+			var->green.offset = 5;
+			var->green.length = 6;
+			var->blue.offset = 0;
+			var->blue.length = 5;
+			var->transp.offset = 0;
+			var->transp.length = 0;
+		}
+		break;
+	case 24:		/* RGB 888 */
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 16;
+		var->blue.length = 8;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+	case 32:		/* RGBA 8888 */
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->transp.offset = 24;
+		var->transp.length = 8;
+		break;
+	}
+	var->red.msb_right = 0;
+	var->green.msb_right = 0;
+	var->blue.msb_right = 0;
+	var->transp.msb_right = 0;
+
+	return 0;
+}
+
+/*
+ * This routine actually sets the video mode. It's in here where we
+ * the hardware state info->par and fix which can be affected by the
+ * change in par. For this driver it doesn't do much.
+ */
+static int unifb_set_par(struct fb_info *info)
+{
+	int hTotal, vTotal, hSyncStart, hSyncEnd, vSyncStart, vSyncEnd;
+	int format;
+
+#ifdef CONFIG_PUV3_PM
+	struct clk *clk_vga;
+	u32 pixclk = 0;
+	int i;
+
+	for (i = 0; i <= 10; i++) {
+		if    (info->var.xres         == unifb_modes[i].xres
+		    && info->var.yres         == unifb_modes[i].yres
+		    && info->var.upper_margin == unifb_modes[i].upper_margin
+		    && info->var.lower_margin == unifb_modes[i].lower_margin
+		    && info->var.left_margin  == unifb_modes[i].left_margin
+		    && info->var.right_margin == unifb_modes[i].right_margin
+		    && info->var.hsync_len    == unifb_modes[i].hsync_len
+		    && info->var.vsync_len    == unifb_modes[i].vsync_len) {
+			pixclk = unifb_modes[i].pixclock;
+			break;
+		}
+	}
+
+	/* set clock rate */
+	clk_vga = clk_get(info->device, "VGA_CLK");
+	if (clk_vga == ERR_PTR(-ENOENT))
+		return -ENOENT;
+
+	if (pixclk != 0) {
+		if (clk_set_rate(clk_vga, pixclk)) { /* set clock failed */
+			info->fix = unifb_fix;
+			info->var = unifb_default;
+			if (clk_set_rate(clk_vga, unifb_default.pixclock))
+				return -EINVAL;
+		}
+	}
+#endif
+
+	info->fix.line_length = get_line_length(info->var.xres_virtual,
+						info->var.bits_per_pixel);
+
+	hSyncStart = info->var.xres + info->var.right_margin;
+	hSyncEnd = hSyncStart + info->var.hsync_len;
+	hTotal = hSyncEnd + info->var.left_margin;
+
+	vSyncStart = info->var.yres + info->var.lower_margin;
+	vSyncEnd = vSyncStart + info->var.vsync_len;
+	vTotal = vSyncEnd + info->var.upper_margin;
+
+	switch (info->var.bits_per_pixel) {
+	case 8:
+		format = UDE_CFG_DST8;
+		break;
+	case 16:
+		format = UDE_CFG_DST16;
+		break;
+	case 24:
+		format = UDE_CFG_DST24;
+		break;
+	case 32:
+		format = UDE_CFG_DST32;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	writel(info->fix.smem_start, UDE_FSA);
+	writel(info->var.yres, UDE_LS);
+	writel(get_line_length(info->var.xres,
+			info->var.bits_per_pixel) >> 3, UDE_PS);
+			/* >> 3 for hardware required. */
+	writel((hTotal << 16) | (info->var.xres), UDE_HAT);
+	writel(((hTotal - 1) << 16) | (info->var.xres - 1), UDE_HBT);
+	writel(((hSyncEnd - 1) << 16) | (hSyncStart - 1), UDE_HST);
+	writel((vTotal << 16) | (info->var.yres), UDE_VAT);
+	writel(((vTotal - 1) << 16) | (info->var.yres - 1), UDE_VBT);
+	writel(((vSyncEnd - 1) << 16) | (vSyncStart - 1), UDE_VST);
+	writel(UDE_CFG_GDEN_ENABLE | UDE_CFG_TIMEUP_ENABLE
+			| format | 0xC0000001, UDE_CFG);
+
+	return 0;
+}
+
+/*
+ *  Set a single color register. The values supplied are already
+ *  rounded down to the hardware's capabilities (according to the
+ *  entries in the var structure). Return != 0 for invalid regno.
+ */
+static int unifb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+			 u_int transp, struct fb_info *info)
+{
+	if (regno >= 256)	/* no. of hw registers */
+		return 1;
+
+	/* grayscale works only partially under directcolor */
+	if (info->var.grayscale) {
+		/* grayscale = 0.30*R + 0.59*G + 0.11*B */
+		red = green = blue =
+		    (red * 77 + green * 151 + blue * 28) >> 8;
+	}
+
+#define CNVT_TOHW(val, width) ((((val)<<(width))+0x7FFF-(val))>>16)
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		red = CNVT_TOHW(red, info->var.red.length);
+		green = CNVT_TOHW(green, info->var.green.length);
+		blue = CNVT_TOHW(blue, info->var.blue.length);
+		transp = CNVT_TOHW(transp, info->var.transp.length);
+		break;
+	case FB_VISUAL_DIRECTCOLOR:
+		red = CNVT_TOHW(red, 8);	/* expect 8 bit DAC */
+		green = CNVT_TOHW(green, 8);
+		blue = CNVT_TOHW(blue, 8);
+		/* hey, there is bug in transp handling... */
+		transp = CNVT_TOHW(transp, 8);
+		break;
+	}
+#undef CNVT_TOHW
+	/* Truecolor has hardware independent palette */
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+		u32 v;
+
+		if (regno >= 16)
+			return 1;
+
+		v = (red << info->var.red.offset) |
+		    (green << info->var.green.offset) |
+		    (blue << info->var.blue.offset) |
+		    (transp << info->var.transp.offset);
+		switch (info->var.bits_per_pixel) {
+		case 8:
+			break;
+		case 16:
+		case 24:
+		case 32:
+			((u32 *) (info->pseudo_palette))[regno] = v;
+			break;
+		default:
+			return 1;
+		}
+		return 0;
+	}
+	return 0;
+}
+
+/*
+ *  Pan or Wrap the Display
+ *
+ *  This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
+ */
+static int unifb_pan_display(struct fb_var_screeninfo *var,
+			   struct fb_info *info)
+{
+	if (var->vmode & FB_VMODE_YWRAP) {
+		if (var->yoffset < 0
+		    || var->yoffset >= info->var.yres_virtual
+		    || var->xoffset)
+			return -EINVAL;
+	} else {
+		if (var->xoffset + var->xres > info->var.xres_virtual ||
+		    var->yoffset + var->yres > info->var.yres_virtual)
+			return -EINVAL;
+	}
+	info->var.xoffset = var->xoffset;
+	info->var.yoffset = var->yoffset;
+	if (var->vmode & FB_VMODE_YWRAP)
+		info->var.vmode |= FB_VMODE_YWRAP;
+	else
+		info->var.vmode &= ~FB_VMODE_YWRAP;
+	return 0;
+}
+
+int unifb_mmap(struct fb_info *info,
+		    struct vm_area_struct *vma)
+{
+	unsigned long size = vma->vm_end - vma->vm_start;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long pos = info->fix.smem_start + offset;
+
+	if (offset + size > info->fix.smem_len)
+		return -EINVAL;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (io_remap_pfn_range(vma, vma->vm_start, pos >> PAGE_SHIFT, size,
+				vma->vm_page_prot))
+		return -EAGAIN;
+
+	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
+	return 0;
+
+}
+
+static struct fb_ops unifb_ops = {
+	.fb_read        = fb_sys_read,
+	.fb_write       = fb_sys_write,
+	.fb_check_var	= unifb_check_var,
+	.fb_set_par	= unifb_set_par,
+	.fb_setcolreg	= unifb_setcolreg,
+	.fb_pan_display	= unifb_pan_display,
+	.fb_fillrect	= unifb_fillrect,
+	.fb_copyarea	= unifb_copyarea,
+	.fb_imageblit   = unifb_imageblit,
+	.fb_mmap	= unifb_mmap,
+};
+
+/*
+ *  Initialisation
+ */
+static int unifb_probe(struct platform_device *dev)
+{
+	struct fb_info *info;
+	u32 unifb_regs[UNIFB_REGS_NUM];
+	int retval = -ENOMEM;
+	struct resource *iomem;
+	void *videomemory;
+
+	videomemory = (void *)__get_free_pages(GFP_KERNEL | __GFP_COMP,
+				get_order(UNIFB_MEMSIZE));
+	if (!videomemory)
+		goto err;
+
+	memset(videomemory, 0, UNIFB_MEMSIZE);
+
+	unifb_fix.smem_start = virt_to_phys(videomemory);
+	unifb_fix.smem_len = UNIFB_MEMSIZE;
+
+	iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	unifb_fix.mmio_start = iomem->start;
+
+	info = framebuffer_alloc(sizeof(u32)*256, &dev->dev);
+	if (!info)
+		goto err;
+
+	info->screen_base = (char __iomem *)videomemory;
+	info->fbops = &unifb_ops;
+
+	retval = fb_find_mode(&info->var, info, NULL,
+			      unifb_modes, 10, &unifb_modes[0], 16);
+
+	if (!retval || (retval == 4))
+		info->var = unifb_default;
+
+	info->fix = unifb_fix;
+	info->pseudo_palette = info->par;
+	info->par = NULL;
+	info->flags = FBINFO_FLAG_DEFAULT;
+#ifdef FB_ACCEL_PUV3_UNIGFX
+	info->fix.accel = FB_ACCEL_PUV3_UNIGFX;
+#endif
+
+	retval = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (retval < 0)
+		goto err1;
+
+	retval = register_framebuffer(info);
+	if (retval < 0)
+		goto err2;
+	platform_set_drvdata(dev, info);
+	platform_device_add_data(dev, unifb_regs, sizeof(u32) * UNIFB_REGS_NUM);
+
+	printk(KERN_INFO
+	       "fb%d: Virtual frame buffer device, using %dM of video memory\n",
+	       info->node, UNIFB_MEMSIZE >> 20);
+	return 0;
+err2:
+	fb_dealloc_cmap(&info->cmap);
+err1:
+	framebuffer_release(info);
+err:
+	return retval;
+}
+
+static int unifb_remove(struct platform_device *dev)
+{
+	struct fb_info *info = platform_get_drvdata(dev);
+
+	if (info) {
+		unregister_framebuffer(info);
+		fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int unifb_resume(struct platform_device *dev)
+{
+	int rc = 0;
+	u32 *unifb_regs = dev->dev.platform_data;
+
+	if (dev->dev.power.power_state.event == PM_EVENT_ON)
+		return 0;
+
+	console_lock();
+
+	if (dev->dev.power.power_state.event == PM_EVENT_SUSPEND) {
+		writel(unifb_regs[0], UDE_FSA);
+		writel(unifb_regs[1], UDE_LS);
+		writel(unifb_regs[2], UDE_PS);
+		writel(unifb_regs[3], UDE_HAT);
+		writel(unifb_regs[4], UDE_HBT);
+		writel(unifb_regs[5], UDE_HST);
+		writel(unifb_regs[6], UDE_VAT);
+		writel(unifb_regs[7], UDE_VBT);
+		writel(unifb_regs[8], UDE_VST);
+		writel(unifb_regs[9], UDE_CFG);
+	}
+	dev->dev.power.power_state = PMSG_ON;
+
+	console_unlock();
+
+	return rc;
+}
+
+static int unifb_suspend(struct platform_device *dev, pm_message_t mesg)
+{
+	u32 *unifb_regs = dev->dev.platform_data;
+
+	unifb_regs[0] = readl(UDE_FSA);
+	unifb_regs[1] = readl(UDE_LS);
+	unifb_regs[2] = readl(UDE_PS);
+	unifb_regs[3] = readl(UDE_HAT);
+	unifb_regs[4] = readl(UDE_HBT);
+	unifb_regs[5] = readl(UDE_HST);
+	unifb_regs[6] = readl(UDE_VAT);
+	unifb_regs[7] = readl(UDE_VBT);
+	unifb_regs[8] = readl(UDE_VST);
+	unifb_regs[9] = readl(UDE_CFG);
+
+	if (mesg.event == dev->dev.power.power_state.event)
+		return 0;
+
+	switch (mesg.event) {
+	case PM_EVENT_FREEZE:		/* about to take snapshot */
+	case PM_EVENT_PRETHAW:		/* before restoring snapshot */
+		goto done;
+	}
+
+	console_lock();
+
+	/* do nothing... */
+
+	console_unlock();
+
+done:
+	dev->dev.power.power_state = mesg;
+
+	return 0;
+}
+#else
+#define	unifb_resume	NULL
+#define unifb_suspend	NULL
+#endif
+
+static struct platform_driver unifb_driver = {
+	.probe	 = unifb_probe,
+	.remove  = unifb_remove,
+	.resume  = unifb_resume,
+	.suspend = unifb_suspend,
+	.driver  = {
+		.name	= "PKUnity-v3-UNIGFX",
+	},
+};
+
+static int __init unifb_init(void)
+{
+#ifndef MODULE
+	if (fb_get_options("unifb", NULL))
+		return -ENODEV;
+#endif
+
+	return platform_driver_register(&unifb_driver);
+}
+
+module_init(unifb_init);
+
+static void __exit unifb_exit(void)
+{
+	platform_driver_unregister(&unifb_driver);
+}
+
+module_exit(unifb_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/fb_defio.c b/drivers/video/fb_defio.c
index 804000183c5e..32814e8800e0 100644
--- a/drivers/video/fb_defio.c
+++ b/drivers/video/fb_defio.c
@@ -66,19 +66,26 @@ static int fb_deferred_io_fault(struct vm_area_struct *vma,
 	return 0;
 }
 
-int fb_deferred_io_fsync(struct file *file, int datasync)
+int fb_deferred_io_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 {
 	struct fb_info *info = file->private_data;
+	struct inode *inode = file->f_path.dentry->d_inode;
+	int err = filemap_write_and_wait_range(inode->i_mapping, start, end);
+	if (err)
+		return err;
 
 	/* Skip if deferred io is compiled-in but disabled on this fbdev */
 	if (!info->fbdefio)
 		return 0;
 
+	mutex_lock(&inode->i_mutex);
 	/* Kill off the delayed work */
 	cancel_delayed_work_sync(&info->deferred_work);
 
 	/* Run it immediately */
-	return schedule_delayed_work(&info->deferred_work, 0);
+	err = schedule_delayed_work(&info->deferred_work, 0);
+	mutex_unlock(&inode->i_mutex);
+	return err;
 }
 EXPORT_SYMBOL_GPL(fb_deferred_io_fsync);
 
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index e2bf95370e40..5aac00eb1830 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -42,9 +42,34 @@
 
 #define FBPIXMAPSIZE	(1024 * 8)
 
+static DEFINE_MUTEX(registration_lock);
 struct fb_info *registered_fb[FB_MAX] __read_mostly;
 int num_registered_fb __read_mostly;
 
+static struct fb_info *get_fb_info(unsigned int idx)
+{
+	struct fb_info *fb_info;
+
+	if (idx >= FB_MAX)
+		return ERR_PTR(-ENODEV);
+
+	mutex_lock(&registration_lock);
+	fb_info = registered_fb[idx];
+	if (fb_info)
+		atomic_inc(&fb_info->count);
+	mutex_unlock(&registration_lock);
+
+	return fb_info;
+}
+
+static void put_fb_info(struct fb_info *fb_info)
+{
+	if (!atomic_dec_and_test(&fb_info->count))
+		return;
+	if (fb_info->fbops->fb_destroy)
+		fb_info->fbops->fb_destroy(fb_info);
+}
+
 int lock_fb_info(struct fb_info *info)
 {
 	mutex_lock(&info->lock);
@@ -647,6 +672,7 @@ int fb_show_logo(struct fb_info *info, int rotate) { return 0; }
 
 static void *fb_seq_start(struct seq_file *m, loff_t *pos)
 {
+	mutex_lock(&registration_lock);
 	return (*pos < FB_MAX) ? pos : NULL;
 }
 
@@ -658,6 +684,7 @@ static void *fb_seq_next(struct seq_file *m, void *v, loff_t *pos)
 
 static void fb_seq_stop(struct seq_file *m, void *v)
 {
+	mutex_unlock(&registration_lock);
 }
 
 static int fb_seq_show(struct seq_file *m, void *v)
@@ -690,13 +717,30 @@ static const struct file_operations fb_proc_fops = {
 	.release	= seq_release,
 };
 
-static ssize_t
-fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
+/*
+ * We hold a reference to the fb_info in file->private_data,
+ * but if the current registered fb has changed, we don't
+ * actually want to use it.
+ *
+ * So look up the fb_info using the inode minor number,
+ * and just verify it against the reference we have.
+ */
+static struct fb_info *file_fb_info(struct file *file)
 {
-	unsigned long p = *ppos;
 	struct inode *inode = file->f_path.dentry->d_inode;
 	int fbidx = iminor(inode);
 	struct fb_info *info = registered_fb[fbidx];
+
+	if (info != file->private_data)
+		info = NULL;
+	return info;
+}
+
+static ssize_t
+fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
+{
+	unsigned long p = *ppos;
+	struct fb_info *info = file_fb_info(file);
 	u8 *buffer, *dst;
 	u8 __iomem *src;
 	int c, cnt = 0, err = 0;
@@ -761,9 +805,7 @@ static ssize_t
 fb_write(struct file *file, const char __user *buf, size_t count, loff_t *ppos)
 {
 	unsigned long p = *ppos;
-	struct inode *inode = file->f_path.dentry->d_inode;
-	int fbidx = iminor(inode);
-	struct fb_info *info = registered_fb[fbidx];
+	struct fb_info *info = file_fb_info(file);
 	u8 *buffer, *src;
 	u8 __iomem *dst;
 	int c, cnt = 0, err = 0;
@@ -1141,10 +1183,10 @@ static long do_fb_ioctl(struct fb_info *info, unsigned int cmd,
 
 static long fb_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
-	struct inode *inode = file->f_path.dentry->d_inode;
-	int fbidx = iminor(inode);
-	struct fb_info *info = registered_fb[fbidx];
+	struct fb_info *info = file_fb_info(file);
 
+	if (!info)
+		return -ENODEV;
 	return do_fb_ioctl(info, cmd, arg);
 }
 
@@ -1265,12 +1307,13 @@ static int fb_get_fscreeninfo(struct fb_info *info, unsigned int cmd,
 static long fb_compat_ioctl(struct file *file, unsigned int cmd,
 			    unsigned long arg)
 {
-	struct inode *inode = file->f_path.dentry->d_inode;
-	int fbidx = iminor(inode);
-	struct fb_info *info = registered_fb[fbidx];
-	struct fb_ops *fb = info->fbops;
+	struct fb_info *info = file_fb_info(file);
+	struct fb_ops *fb;
 	long ret = -ENOIOCTLCMD;
 
+	if (!info)
+		return -ENODEV;
+	fb = info->fbops;
 	switch(cmd) {
 	case FBIOGET_VSCREENINFO:
 	case FBIOPUT_VSCREENINFO:
@@ -1303,16 +1346,18 @@ static long fb_compat_ioctl(struct file *file, unsigned int cmd,
 static int
 fb_mmap(struct file *file, struct vm_area_struct * vma)
 {
-	int fbidx = iminor(file->f_path.dentry->d_inode);
-	struct fb_info *info = registered_fb[fbidx];
-	struct fb_ops *fb = info->fbops;
+	struct fb_info *info = file_fb_info(file);
+	struct fb_ops *fb;
 	unsigned long off;
 	unsigned long start;
 	u32 len;
 
+	if (!info)
+		return -ENODEV;
 	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
 		return -EINVAL;
 	off = vma->vm_pgoff << PAGE_SHIFT;
+	fb = info->fbops;
 	if (!fb)
 		return -ENODEV;
 	mutex_lock(&info->mm_lock);
@@ -1361,14 +1406,16 @@ __releases(&info->lock)
 	struct fb_info *info;
 	int res = 0;
 
-	if (fbidx >= FB_MAX)
-		return -ENODEV;
-	info = registered_fb[fbidx];
-	if (!info)
+	info = get_fb_info(fbidx);
+	if (!info) {
 		request_module("fb%d", fbidx);
-	info = registered_fb[fbidx];
-	if (!info)
-		return -ENODEV;
+		info = get_fb_info(fbidx);
+		if (!info)
+			return -ENODEV;
+	}
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
 	mutex_lock(&info->lock);
 	if (!try_module_get(info->fbops->owner)) {
 		res = -ENODEV;
@@ -1386,6 +1433,8 @@ __releases(&info->lock)
 #endif
 out:
 	mutex_unlock(&info->lock);
+	if (res)
+		put_fb_info(info);
 	return res;
 }
 
@@ -1401,6 +1450,7 @@ __releases(&info->lock)
 		info->fbops->fb_release(info,1);
 	module_put(info->fbops->owner);
 	mutex_unlock(&info->lock);
+	put_fb_info(info);
 	return 0;
 }
 
@@ -1487,8 +1537,10 @@ static bool fb_do_apertures_overlap(struct apertures_struct *gena,
 	return false;
 }
 
+static int do_unregister_framebuffer(struct fb_info *fb_info);
+
 #define VGA_FB_PHYS 0xA0000
-void remove_conflicting_framebuffers(struct apertures_struct *a,
+static void do_remove_conflicting_framebuffers(struct apertures_struct *a,
 				     const char *name, bool primary)
 {
 	int i;
@@ -1507,46 +1559,35 @@ void remove_conflicting_framebuffers(struct apertures_struct *a,
 			(primary && gen_aper && gen_aper->count &&
 			 gen_aper->ranges[0].base == VGA_FB_PHYS)) {
 
-			printk(KERN_ERR "fb: conflicting fb hw usage "
+			printk(KERN_INFO "fb: conflicting fb hw usage "
 			       "%s vs %s - removing generic driver\n",
 			       name, registered_fb[i]->fix.id);
-			unregister_framebuffer(registered_fb[i]);
+			do_unregister_framebuffer(registered_fb[i]);
 		}
 	}
 }
-EXPORT_SYMBOL(remove_conflicting_framebuffers);
 
-/**
- *	register_framebuffer - registers a frame buffer device
- *	@fb_info: frame buffer info structure
- *
- *	Registers a frame buffer device @fb_info.
- *
- *	Returns negative errno on error, or zero for success.
- *
- */
-
-int
-register_framebuffer(struct fb_info *fb_info)
+static int do_register_framebuffer(struct fb_info *fb_info)
 {
 	int i;
 	struct fb_event event;
 	struct fb_videomode mode;
 
-	if (num_registered_fb == FB_MAX)
-		return -ENXIO;
-
 	if (fb_check_foreignness(fb_info))
 		return -ENOSYS;
 
-	remove_conflicting_framebuffers(fb_info->apertures, fb_info->fix.id,
+	do_remove_conflicting_framebuffers(fb_info->apertures, fb_info->fix.id,
 					 fb_is_primary_device(fb_info));
 
+	if (num_registered_fb == FB_MAX)
+		return -ENXIO;
+
 	num_registered_fb++;
 	for (i = 0 ; i < FB_MAX; i++)
 		if (!registered_fb[i])
 			break;
 	fb_info->node = i;
+	atomic_set(&fb_info->count, 1);
 	mutex_init(&fb_info->lock);
 	mutex_init(&fb_info->mm_lock);
 
@@ -1592,36 +1633,14 @@ register_framebuffer(struct fb_info *fb_info)
 	return 0;
 }
 
-
-/**
- *	unregister_framebuffer - releases a frame buffer device
- *	@fb_info: frame buffer info structure
- *
- *	Unregisters a frame buffer device @fb_info.
- *
- *	Returns negative errno on error, or zero for success.
- *
- *      This function will also notify the framebuffer console
- *      to release the driver.
- *
- *      This is meant to be called within a driver's module_exit()
- *      function. If this is called outside module_exit(), ensure
- *      that the driver implements fb_open() and fb_release() to
- *      check that no processes are using the device.
- */
-
-int
-unregister_framebuffer(struct fb_info *fb_info)
+static int do_unregister_framebuffer(struct fb_info *fb_info)
 {
 	struct fb_event event;
 	int i, ret = 0;
 
 	i = fb_info->node;
-	if (!registered_fb[i]) {
-		ret = -EINVAL;
-		goto done;
-	}
-
+	if (i < 0 || i >= FB_MAX || registered_fb[i] != fb_info)
+		return -EINVAL;
 
 	if (!lock_fb_info(fb_info))
 		return -ENODEV;
@@ -1629,16 +1648,14 @@ unregister_framebuffer(struct fb_info *fb_info)
 	ret = fb_notifier_call_chain(FB_EVENT_FB_UNBIND, &event);
 	unlock_fb_info(fb_info);
 
-	if (ret) {
-		ret = -EINVAL;
-		goto done;
-	}
+	if (ret)
+		return -EINVAL;
 
 	if (fb_info->pixmap.addr &&
 	    (fb_info->pixmap.flags & FB_PIXMAP_DEFAULT))
 		kfree(fb_info->pixmap.addr);
 	fb_destroy_modelist(&fb_info->modelist);
-	registered_fb[i]=NULL;
+	registered_fb[i] = NULL;
 	num_registered_fb--;
 	fb_cleanup_device(fb_info);
 	device_destroy(fb_class, MKDEV(FB_MAJOR, i));
@@ -1646,9 +1663,65 @@ unregister_framebuffer(struct fb_info *fb_info)
 	fb_notifier_call_chain(FB_EVENT_FB_UNREGISTERED, &event);
 
 	/* this may free fb info */
-	if (fb_info->fbops->fb_destroy)
-		fb_info->fbops->fb_destroy(fb_info);
-done:
+	put_fb_info(fb_info);
+	return 0;
+}
+
+void remove_conflicting_framebuffers(struct apertures_struct *a,
+				     const char *name, bool primary)
+{
+	mutex_lock(&registration_lock);
+	do_remove_conflicting_framebuffers(a, name, primary);
+	mutex_unlock(&registration_lock);
+}
+EXPORT_SYMBOL(remove_conflicting_framebuffers);
+
+/**
+ *	register_framebuffer - registers a frame buffer device
+ *	@fb_info: frame buffer info structure
+ *
+ *	Registers a frame buffer device @fb_info.
+ *
+ *	Returns negative errno on error, or zero for success.
+ *
+ */
+int
+register_framebuffer(struct fb_info *fb_info)
+{
+	int ret;
+
+	mutex_lock(&registration_lock);
+	ret = do_register_framebuffer(fb_info);
+	mutex_unlock(&registration_lock);
+
+	return ret;
+}
+
+/**
+ *	unregister_framebuffer - releases a frame buffer device
+ *	@fb_info: frame buffer info structure
+ *
+ *	Unregisters a frame buffer device @fb_info.
+ *
+ *	Returns negative errno on error, or zero for success.
+ *
+ *      This function will also notify the framebuffer console
+ *      to release the driver.
+ *
+ *      This is meant to be called within a driver's module_exit()
+ *      function. If this is called outside module_exit(), ensure
+ *      that the driver implements fb_open() and fb_release() to
+ *      check that no processes are using the device.
+ */
+int
+unregister_framebuffer(struct fb_info *fb_info)
+{
+	int ret;
+
+	mutex_lock(&registration_lock);
+	ret = do_unregister_framebuffer(fb_info);
+	mutex_unlock(&registration_lock);
+
 	return ret;
 }
 
diff --git a/drivers/video/fbsysfs.c b/drivers/video/fbsysfs.c
index f4a32779168b..04251ce89184 100644
--- a/drivers/video/fbsysfs.c
+++ b/drivers/video/fbsysfs.c
@@ -33,7 +33,7 @@
  * for driver private data (info->par). info->par (if any) will be
  * aligned to sizeof(long).
  *
- * Returns the new structure, or NULL if an error occured.
+ * Returns the new structure, or NULL if an error occurred.
  *
  */
 struct fb_info *framebuffer_alloc(size_t size, struct device *dev)
diff --git a/drivers/video/ffb.c b/drivers/video/ffb.c
index 6739b2af3bc0..14102a3f70f5 100644
--- a/drivers/video/ffb.c
+++ b/drivers/video/ffb.c
@@ -893,8 +893,7 @@ static void ffb_init_fix(struct fb_info *info)
 	info->fix.accel = FB_ACCEL_SUN_CREATOR;
 }
 
-static int __devinit ffb_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit ffb_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct ffb_fbc __iomem *fbc;
@@ -1011,7 +1010,7 @@ out_dealloc_cmap:
 	fb_dealloc_cmap(&info->cmap);
 
 out_unmap_dac:
-	of_iounmap(&op->resource[2], par->fbc, sizeof(struct ffb_fbc));
+	of_iounmap(&op->resource[1], par->dac, sizeof(struct ffb_dac));
 
 out_unmap_fbc:
 	of_iounmap(&op->resource[2], par->fbc, sizeof(struct ffb_fbc));
@@ -1052,7 +1051,7 @@ static const struct of_device_id ffb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ffb_match);
 
-static struct of_platform_driver ffb_driver = {
+static struct platform_driver ffb_driver = {
 	.driver = {
 		.name = "ffb",
 		.owner = THIS_MODULE,
@@ -1067,12 +1066,12 @@ static int __init ffb_init(void)
 	if (fb_get_options("ffb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&ffb_driver);
+	return platform_driver_register(&ffb_driver);
 }
 
 static void __exit ffb_exit(void)
 {
-	of_unregister_platform_driver(&ffb_driver);
+	platform_driver_unregister(&ffb_driver);
 }
 
 module_init(ffb_init);
diff --git a/drivers/video/fm2fb.c b/drivers/video/fm2fb.c
index 1b0feb8e7244..d0533b7aad79 100644
--- a/drivers/video/fm2fb.c
+++ b/drivers/video/fm2fb.c
@@ -45,7 +45,7 @@
  *	buffer needs an amount of memory of 1.769.472 bytes which
  *	is near to 2 MByte (the allocated address space of Zorro2).
  *	The memory is channel interleaved. That means every channel
- *	owns four VRAMs. Unfortunatly most FrameMasters II are
+ *	owns four VRAMs. Unfortunately most FrameMasters II are
  *	not assembled with memory for the alpha channel. In this
  *	case it could be possible to add the frame buffer into the
  *	normal memory pool.
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 8bbbf08fa3ce..0acc7d65aeaa 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -555,8 +555,6 @@ static void adjust_aoi_size_position(struct fb_var_screeninfo *var,
 static int fsl_diu_check_var(struct fb_var_screeninfo *var,
 				struct fb_info *info)
 {
-	unsigned long htotal, vtotal;
-
 	pr_debug("check_var xres: %d\n", var->xres);
 	pr_debug("check_var yres: %d\n", var->yres);
 
@@ -635,20 +633,6 @@ static int fsl_diu_check_var(struct fb_var_screeninfo *var,
 
 		break;
 	}
-	/* If the pixclock is below the minimum spec'd value then set to
-	 * refresh rate for 60Hz since this is supported by most monitors.
-	 * Refer to Documentation/fb/ for calculations.
-	 */
-	if ((var->pixclock < MIN_PIX_CLK) || (var->pixclock > MAX_PIX_CLK)) {
-		htotal = var->xres + var->right_margin + var->hsync_len +
-		    var->left_margin;
-		vtotal = var->yres + var->lower_margin + var->vsync_len +
-		    var->upper_margin;
-		var->pixclock = (vtotal * htotal * 6UL) / 100UL;
-		var->pixclock = KHZ2PICOS(var->pixclock);
-		pr_debug("pixclock set for 60Hz refresh = %u ps\n",
-			var->pixclock);
-	}
 
 	var->height = -1;
 	var->width = -1;
@@ -882,7 +866,7 @@ static inline __u32 CNVT_TOHW(__u32 val, __u32 width)
  * which needs to be scaled in this function for the hardware. Things to take
  * into consideration are how many color registers, if any, are supported with
  * the current color visual. With truecolor mode no color palettes are
- * supported. Here a psuedo palette is created which we store the value in
+ * supported. Here a pseudo palette is created which we store the value in
  * pseudo_palette in struct fb_info. For pseudocolor mode we have a limited
  * color palette.
  */
@@ -1487,8 +1471,7 @@ static ssize_t show_monitor(struct device *device,
 	return diu_ops.show_monitor_port(machine_data->monitor_port, buf);
 }
 
-static int __devinit fsl_diu_probe(struct platform_device *ofdev,
-	const struct of_device_id *match)
+static int __devinit fsl_diu_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct mfb_info *mfbi;
@@ -1735,7 +1718,7 @@ static struct of_device_id fsl_diu_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fsl_diu_match);
 
-static struct of_platform_driver fsl_diu_driver = {
+static struct platform_driver fsl_diu_driver = {
 	.driver = {
 		.name = "fsl_diu",
 		.owner = THIS_MODULE,
@@ -1797,7 +1780,7 @@ static int __init fsl_diu_init(void)
 	if (!coherence_data)
 		return -ENOMEM;
 #endif
-	ret = of_register_platform_driver(&fsl_diu_driver);
+	ret = platform_driver_register(&fsl_diu_driver);
 	if (ret) {
 		printk(KERN_ERR
 			"fsl-diu: failed to register platform driver\n");
@@ -1811,7 +1794,7 @@ static int __init fsl_diu_init(void)
 
 static void __exit fsl_diu_exit(void)
 {
-	of_unregister_platform_driver(&fsl_diu_driver);
+	platform_driver_unregister(&fsl_diu_driver);
 #if defined(CONFIG_NOT_COHERENT_CACHE)
 	vfree(coherence_data);
 #endif
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index 933899dca33a..7e7b7a9ba274 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -721,7 +721,7 @@ static int gbefb_set_par(struct fb_info *info)
 
 	   Tiles have the advantage that they can be allocated individually in
 	   memory. However, this mapping is not linear at all, which is not
-	   really convienient. In order to support linear addressing, the GBE
+	   really convenient. In order to support linear addressing, the GBE
 	   DMA hardware is fooled into thinking the screen is only one tile
 	   large and but has a greater height, so that the DMA transfer covers
 	   the same region.
diff --git a/drivers/video/geode/gx1fb_core.c b/drivers/video/geode/gx1fb_core.c
index c6b554f72c6d..5a5d0928df33 100644
--- a/drivers/video/geode/gx1fb_core.c
+++ b/drivers/video/geode/gx1fb_core.c
@@ -29,7 +29,7 @@ static int  crt_option = 1;
 static char panel_option[32] = "";
 
 /* Modes relevant to the GX1 (taken from modedb.c) */
-static const struct fb_videomode __initdata gx1_modedb[] = {
+static const struct fb_videomode __devinitdata gx1_modedb[] = {
 	/* 640x480-60 VESA */
 	{ NULL, 60, 640, 480, 39682,  48, 16, 33, 10, 96, 2,
 	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
@@ -195,7 +195,7 @@ static int gx1fb_blank(int blank_mode, struct fb_info *info)
 	return par->vid_ops->blank_display(info, blank_mode);
 }
 
-static int __init gx1fb_map_video_memory(struct fb_info *info, struct pci_dev *dev)
+static int __devinit gx1fb_map_video_memory(struct fb_info *info, struct pci_dev *dev)
 {
 	struct geodefb_par *par = info->par;
 	unsigned gx_base;
@@ -268,7 +268,7 @@ static struct fb_ops gx1fb_ops = {
 	.fb_imageblit	= cfb_imageblit,
 };
 
-static struct fb_info * __init gx1fb_init_fbinfo(struct device *dev)
+static struct fb_info * __devinit gx1fb_init_fbinfo(struct device *dev)
 {
 	struct geodefb_par *par;
 	struct fb_info *info;
@@ -318,7 +318,7 @@ static struct fb_info * __init gx1fb_init_fbinfo(struct device *dev)
 	return info;
 }
 
-static int __init gx1fb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+static int __devinit gx1fb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct geodefb_par *par;
 	struct fb_info *info;
@@ -382,7 +382,7 @@ static int __init gx1fb_probe(struct pci_dev *pdev, const struct pci_device_id *
 	return ret;
 }
 
-static void gx1fb_remove(struct pci_dev *pdev)
+static void __devexit gx1fb_remove(struct pci_dev *pdev)
 {
 	struct fb_info *info = pci_get_drvdata(pdev);
 	struct geodefb_par *par = info->par;
@@ -441,7 +441,7 @@ static struct pci_driver gx1fb_driver = {
 	.name		= "gx1fb",
 	.id_table	= gx1fb_id_table,
 	.probe		= gx1fb_probe,
-	.remove		= gx1fb_remove,
+	.remove		= __devexit_p(gx1fb_remove),
 };
 
 static int __init gx1fb_init(void)
@@ -456,7 +456,7 @@ static int __init gx1fb_init(void)
 	return pci_register_driver(&gx1fb_driver);
 }
 
-static void __exit gx1fb_cleanup(void)
+static void __devexit gx1fb_cleanup(void)
 {
 	pci_unregister_driver(&gx1fb_driver);
 }
diff --git a/drivers/video/geode/lxfb.h b/drivers/video/geode/lxfb.h
index be8ccb47ebe0..cfcd8090f313 100644
--- a/drivers/video/geode/lxfb.h
+++ b/drivers/video/geode/lxfb.h
@@ -117,7 +117,7 @@ enum gp_registers {
 };
 
 #define GP_BLT_STATUS_CE		(1 << 4)	/* cmd buf empty */
-#define GP_BLT_STATUS_PB		(1 << 0)	/* primative busy */
+#define GP_BLT_STATUS_PB		(1 << 0)	/* primitive busy */
 
 
 /* Display Controller registers (table 6-47 from the data book) */
diff --git a/drivers/video/hecubafb.c b/drivers/video/hecubafb.c
index c77bcc6ab463..614251a9af91 100644
--- a/drivers/video/hecubafb.c
+++ b/drivers/video/hecubafb.c
@@ -231,10 +231,9 @@ static int __devinit hecubafb_probe(struct platform_device *dev)
 
 	videomemorysize = (DPY_W*DPY_H)/8;
 
-	if (!(videomemory = vmalloc(videomemorysize)))
-		return retval;
-
-	memset(videomemory, 0, videomemorysize);
+	videomemory = vzalloc(videomemorysize);
+	if (!videomemory)
+		goto err_videomem_alloc;
 
 	info = framebuffer_alloc(sizeof(struct hecubafb_par), &dev->dev);
 	if (!info)
@@ -276,6 +275,7 @@ err_fbreg:
 	framebuffer_release(info);
 err_fballoc:
 	vfree(videomemory);
+err_videomem_alloc:
 	module_put(board->owner);
 	return retval;
 }
@@ -299,7 +299,7 @@ static int __devexit hecubafb_remove(struct platform_device *dev)
 
 static struct platform_driver hecubafb_driver = {
 	.probe	= hecubafb_probe,
-	.remove = hecubafb_remove,
+	.remove = __devexit_p(hecubafb_remove),
 	.driver	= {
 		.owner	= THIS_MODULE,
 		.name	= "hecubafb",
diff --git a/drivers/video/hpfb.c b/drivers/video/hpfb.c
index c8e280f1bb0b..ebf8495ff198 100644
--- a/drivers/video/hpfb.c
+++ b/drivers/video/hpfb.c
@@ -321,11 +321,11 @@ static int __devinit hpfb_dio_probe(struct dio_dev * d, const struct dio_device_
 	unsigned long paddr, vaddr;
 
 	paddr = d->resource.start;
-	if (!request_mem_region(d->resource.start, d->resource.end - d->resource.start, d->name))
+	if (!request_mem_region(d->resource.start, resource_size(&d->resource), d->name))
                 return -EBUSY;
 
 	if (d->scode >= DIOII_SCBASE) {
-		vaddr = (unsigned long)ioremap(paddr, d->resource.end - d->resource.start);
+		vaddr = (unsigned long)ioremap(paddr, resource_size(&d->resource));
 	} else {
 		vaddr = paddr + DIO_VIRADDRBASE;
 	}
@@ -344,7 +344,7 @@ static void __devexit hpfb_remove_one(struct dio_dev *d)
 	unregister_framebuffer(&fb_info);
 	if (d->scode >= DIOII_SCBASE)
 		iounmap((void *)fb_regs);
-        release_mem_region(d->resource.start, d->resource.end - d->resource.start);
+	release_mem_region(d->resource.start, resource_size(&d->resource));
 }
 
 static struct dio_device_id hpfb_dio_tbl[] = {
diff --git a/drivers/video/i810/i810.h b/drivers/video/i810/i810.h
index f37de60ecc59..1414b73ac55b 100644
--- a/drivers/video/i810/i810.h
+++ b/drivers/video/i810/i810.h
@@ -137,7 +137,7 @@
 #define DRAM_ON                     0x08            
 #define DRAM_OFF                    0xE7
 #define PG_ENABLE_MASK              0x01
-#define RING_SIZE_MASK              (RINGBUFFER_SIZE - 1);
+#define RING_SIZE_MASK              (RINGBUFFER_SIZE - 1)
 
 /* defines for restoring registers partially */
 #define ADDR_MAP_MASK               (0x07 << 5)
diff --git a/drivers/video/i810/i810_accel.c b/drivers/video/i810/i810_accel.c
index f5bedee4310a..7672d2ea9b35 100644
--- a/drivers/video/i810/i810_accel.c
+++ b/drivers/video/i810/i810_accel.c
@@ -112,7 +112,7 @@ static inline int wait_for_engine_idle(struct fb_info *info)
  * @par: pointer to i810fb_par structure
  *
  * DESCRIPTION:
- * Checks/waits for sufficent space in ringbuffer of size
+ * Checks/waits for sufficient space in ringbuffer of size
  * space.  Returns the tail of the buffer
  */ 
 static inline u32 begin_iring(struct fb_info *info, u32 space)
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index 69bd4a581d4a..f135dbead07d 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -65,12 +65,6 @@
 #define CPOS_OP		(1<<28)
 #define CPOS_CXP(x)	(((x) & 3ff) << 16)
 
-#ifdef CONFIG_ARCH_MX1
-#define CPOS_CYP(y)	((y) & 0x1ff)
-#else
-#define CPOS_CYP(y)	((y) & 0x3ff)
-#endif
-
 #define LCDC_LCWHB	0x10
 #define LCWHB_BK_EN	(1<<31)
 #define LCWHB_CW(w)	(((w) & 0x1f) << 24)
@@ -79,16 +73,6 @@
 
 #define LCDC_LCHCC	0x14
 
-#ifdef CONFIG_ARCH_MX1
-#define LCHCC_CUR_COL_R(r) (((r) & 0x1f) << 11)
-#define LCHCC_CUR_COL_G(g) (((g) & 0x3f) << 5)
-#define LCHCC_CUR_COL_B(b) ((b) & 0x1f)
-#else
-#define LCHCC_CUR_COL_R(r) (((r) & 0x3f) << 12)
-#define LCHCC_CUR_COL_G(g) (((g) & 0x3f) << 6)
-#define LCHCC_CUR_COL_B(b) ((b) & 0x3f)
-#endif
-
 #define LCDC_PCR	0x18
 
 #define LCDC_HCR	0x1C
@@ -115,11 +99,7 @@
 
 #define LCDC_RMCR	0x34
 
-#ifdef CONFIG_ARCH_MX1
-#define RMCR_LCDC_EN	(1<<1)
-#else
-#define RMCR_LCDC_EN	0
-#endif
+#define RMCR_LCDC_EN_MX1	(1<<1)
 
 #define RMCR_SELF_REF	(1<<0)
 
@@ -499,6 +479,7 @@ static void imxfb_init_backlight(struct imxfb_info *fbi)
 
 	memset(&props, 0, sizeof(struct backlight_properties));
 	props.max_brightness = 0xff;
+	props.type = BACKLIGHT_RAW;
 	writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
 
 	bl = backlight_device_register("imxfb-bl", &fbi->pdev->dev, fbi,
@@ -535,7 +516,11 @@ static void imxfb_enable_controller(struct imxfb_info *fbi)
 	writel(readl(fbi->regs + LCDC_CPOS) & ~(CPOS_CC0 | CPOS_CC1),
 		fbi->regs + LCDC_CPOS);
 
-	writel(RMCR_LCDC_EN, fbi->regs + LCDC_RMCR);
+	/*
+	 * RMCR_LCDC_EN_MX1 is present on i.MX1 only, but doesn't hurt
+	 * on other SoCs
+	 */
+	writel(RMCR_LCDC_EN_MX1, fbi->regs + LCDC_RMCR);
 
 	clk_enable(fbi->clk);
 
@@ -871,10 +856,10 @@ failed_platform_init:
 		dma_free_writecombine(&pdev->dev,fbi->map_size,fbi->map_cpu,
 			fbi->map_dma);
 failed_map:
-	clk_put(fbi->clk);
-failed_getclock:
 	iounmap(fbi->regs);
 failed_ioremap:
+	clk_put(fbi->clk);
+failed_getclock:
 	release_mem_region(res->start, resource_size(res));
 failed_req:
 	kfree(info->pseudo_palette);
diff --git a/drivers/video/intelfb/Makefile b/drivers/video/intelfb/Makefile
index 6c782d3ae1be..f7d631ebee8e 100644
--- a/drivers/video/intelfb/Makefile
+++ b/drivers/video/intelfb/Makefile
@@ -4,7 +4,4 @@ intelfb-y := intelfbdrv.o intelfbhw.o
 intelfb-$(CONFIG_FB_INTEL_I2C) += intelfb_i2c.o
 intelfb-objs := $(intelfb-y)
 
-ifdef CONFIG_FB_INTEL_DEBUG
-#EXTRA_CFLAGS += -DDEBUG -DVERBOSE -DREGDUMP
-EXTRA_CFLAGS += -DDEBUG -DREGDUMP
-endif
+ccflags-$(CONFIG_FB_INTEL_DEBUG) := -DDEBUG -DREGDUMP
diff --git a/drivers/video/kyro/STG4000OverlayDevice.c b/drivers/video/kyro/STG4000OverlayDevice.c
index a8c9713413e6..0aeeaa10708b 100644
--- a/drivers/video/kyro/STG4000OverlayDevice.c
+++ b/drivers/video/kyro/STG4000OverlayDevice.c
@@ -417,7 +417,7 @@ int SetOverlayViewPort(volatile STG4000REG __iomem *pSTGReg,
 	/***************** Horizontal decimation/scaling ***************************/
 
 	/*
-	 * Now we handle the horizontal case, this is a simplified verison of
+	 * Now we handle the horizontal case, this is a simplified version of
 	 * the vertical case in that we decimate by factors of 2.  as we are
 	 * working in words we should always be able to decimate by these
 	 * factors.  as we always have to have a buffer which is aligned to a
diff --git a/drivers/video/kyro/STG4000Reg.h b/drivers/video/kyro/STG4000Reg.h
index 244549e61368..5d6269882589 100644
--- a/drivers/video/kyro/STG4000Reg.h
+++ b/drivers/video/kyro/STG4000Reg.h
@@ -16,7 +16,7 @@
 
 /*
  * Macros that access memory mapped card registers in PCI space
- * Add an appropraite section for your OS or processor architecture.
+ * Add an appropriate section for your OS or processor architecture.
  */
 #if defined(__KERNEL__)
 #include <asm/page.h>
diff --git a/drivers/video/leo.c b/drivers/video/leo.c
index b599e5e36ced..9e946e2c1da9 100644
--- a/drivers/video/leo.c
+++ b/drivers/video/leo.c
@@ -547,8 +547,7 @@ static void leo_unmap_regs(struct platform_device *op, struct fb_info *info,
 		of_iounmap(&op->resource[0], info->screen_base, 0x800000);
 }
 
-static int __devinit leo_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit leo_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -662,7 +661,7 @@ static const struct of_device_id leo_match[] = {
 };
 MODULE_DEVICE_TABLE(of, leo_match);
 
-static struct of_platform_driver leo_driver = {
+static struct platform_driver leo_driver = {
 	.driver = {
 		.name = "leo",
 		.owner = THIS_MODULE,
@@ -677,12 +676,12 @@ static int __init leo_init(void)
 	if (fb_get_options("leofb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&leo_driver);
+	return platform_driver_register(&leo_driver);
 }
 
 static void __exit leo_exit(void)
 {
-	of_unregister_platform_driver(&leo_driver);
+	platform_driver_unregister(&leo_driver);
 }
 
 module_init(leo_init);
diff --git a/drivers/video/matrox/matroxfb_DAC1064.h b/drivers/video/matrox/matroxfb_DAC1064.h
index c6ed7801efe2..1e6e45b57b78 100644
--- a/drivers/video/matrox/matroxfb_DAC1064.h
+++ b/drivers/video/matrox/matroxfb_DAC1064.h
@@ -46,7 +46,7 @@ void DAC1064_global_restore(struct matrox_fb_info *minfo);
 #define      M1064_XDVICLKCTRL_DVILOOPCTL       0x30
 	/* CRTC2 pixel clock allowed to(0)/blocked from(1) driving CRTC2 */
 #define      M1064_XDVICLKCTRL_C2DVICLKEN       0x40
-	/* P1PLL loop filter bandwith selection */
+	/* P1PLL loop filter bandwidth selection */
 #define      M1064_XDVICLKCTRL_P1LOOPBWDTCTL    0x80
 #define M1064_XCURCOL0RED	0x08
 #define M1064_XCURCOL0GREEN	0x09
diff --git a/drivers/video/matrox/matroxfb_Ti3026.c b/drivers/video/matrox/matroxfb_Ti3026.c
index 835aaaae6b96..9a44cec394b5 100644
--- a/drivers/video/matrox/matroxfb_Ti3026.c
+++ b/drivers/video/matrox/matroxfb_Ti3026.c
@@ -387,7 +387,7 @@ static int Ti3026_init(struct matrox_fb_info *minfo, struct my_timming *m)
 			hw->DACreg[POS3026_XMISCCTRL] = TVP3026_XMISCCTRL_DAC_PUP | TVP3026_XMISCCTRL_DAC_8BIT | TVP3026_XMISCCTRL_PSEL_DIS | TVP3026_XMISCCTRL_PSEL_LOW;
 			break;
 		case 16:
-			/* XLATCHCTRL should be _4_1 / _2_1... Why is not? (_2_1 is used everytime) */
+			/* XLATCHCTRL should be _4_1 / _2_1... Why is not? (_2_1 is used every time) */
 			hw->DACreg[POS3026_XTRUECOLORCTRL] = (minfo->fbcon.var.green.length == 5) ? (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_ORGB_1555) : (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_RGB_565);
 			hw->DACreg[POS3026_XMUXCTRL] = muxctrl | TVP3026_XMUXCTRL_PIXEL_16BIT;
 			hw->DACreg[POS3026_XCLKCTRL] = TVP3026_XCLKCTRL_SRC_PLL | TVP3026_XCLKCTRL_DIV2;
@@ -399,7 +399,7 @@ static int Ti3026_init(struct matrox_fb_info *minfo, struct my_timming *m)
 			hw->DACreg[POS3026_XCLKCTRL] = TVP3026_XCLKCTRL_SRC_PLL | TVP3026_XCLKCTRL_DIV4;
 			break;
 		case 32:
-			/* XLATCHCTRL should be _2_1 / _1_1... Why is not? (_2_1 is used everytime) */
+			/* XLATCHCTRL should be _2_1 / _1_1... Why is not? (_2_1 is used every time) */
 			hw->DACreg[POS3026_XMUXCTRL] = muxctrl | TVP3026_XMUXCTRL_PIXEL_32BIT;
 			break;
 		default:
diff --git a/drivers/video/matrox/matroxfb_base.c b/drivers/video/matrox/matroxfb_base.c
index a082debe824b..44bf8d4a216b 100644
--- a/drivers/video/matrox/matroxfb_base.c
+++ b/drivers/video/matrox/matroxfb_base.c
@@ -101,8 +101,6 @@
 
 #include <linux/version.h>
 
-#define __OLD_VIDIOC_
-
 #include "matroxfb_base.h"
 #include "matroxfb_misc.h"
 #include "matroxfb_accel.h"
@@ -623,7 +621,7 @@ static int matroxfb_decode_var(const struct matrox_fb_info *minfo,
 		var->yoffset = var->yres_virtual - var->yres;
 
 	if (bpp == 16 && var->green.length == 5) {
-		bpp--; /* an artifical value - 15 */
+		bpp--; /* an artificial value - 15 */
 	}
 
 	for (rgbt = table; rgbt->bpp < bpp; rgbt++);
@@ -1152,7 +1150,6 @@ static int matroxfb_ioctl(struct fb_info *info,
 					return -EFAULT;
 				return err;
 			}
-		case VIDIOC_S_CTRL_OLD:
 		case VIDIOC_S_CTRL:
 			{
 				struct v4l2_control ctrl;
@@ -1461,13 +1458,6 @@ static struct board {
 		MGA_G100,
 		&vbG100,
 		"MGA-G100 (AGP)"},
-	{PCI_VENDOR_ID_MATROX,	PCI_DEVICE_ID_MATROX_G200EV_PCI,	0xFF,
-		0,			0,
-		DEVF_G200,
-		230000,
-		MGA_G200,
-		&vbG200,
-		"MGA-G200eV (PCI)"},
 	{PCI_VENDOR_ID_MATROX,	PCI_DEVICE_ID_MATROX_G200_PCI,	0xFF,
 		0,			0,
 		DEVF_G200,
@@ -2119,8 +2109,6 @@ static struct pci_device_id matroxfb_devices[] = {
 		PCI_ANY_ID,	PCI_ANY_ID,	0, 0, 0},
 	{PCI_VENDOR_ID_MATROX,	PCI_DEVICE_ID_MATROX_G100_AGP,
 		PCI_ANY_ID,	PCI_ANY_ID,	0, 0, 0},
-	{PCI_VENDOR_ID_MATROX,	PCI_DEVICE_ID_MATROX_G200EV_PCI,
-		PCI_ANY_ID,	PCI_ANY_ID,	0, 0, 0},
 	{PCI_VENDOR_ID_MATROX,	PCI_DEVICE_ID_MATROX_G200_PCI,
 		PCI_ANY_ID,	PCI_ANY_ID,	0, 0, 0},
 	{PCI_VENDOR_ID_MATROX,	PCI_DEVICE_ID_MATROX_G200_AGP,
diff --git a/drivers/video/matrox/matroxfb_base.h b/drivers/video/matrox/matroxfb_base.h
index f96a471cb1a8..11ed57bb704e 100644
--- a/drivers/video/matrox/matroxfb_base.h
+++ b/drivers/video/matrox/matroxfb_base.h
@@ -12,7 +12,7 @@
 #undef MATROXFB_DEBUG
 
 /* heavy debugging: */
-/* -- logs putc[s], so everytime a char is displayed, it's logged */
+/* -- logs putc[s], so every time a char is displayed, it's logged */
 #undef MATROXFB_DEBUG_HEAVY
 
 /* This one _could_ cause infinite loops */
diff --git a/drivers/video/mb862xx/Makefile b/drivers/video/mb862xx/Makefile
index d7777714166b..5707ed0e31a7 100644
--- a/drivers/video/mb862xx/Makefile
+++ b/drivers/video/mb862xx/Makefile
@@ -2,4 +2,7 @@
 # Makefile for the MB862xx framebuffer driver
 #
 
-obj-$(CONFIG_FB_MB862XX)	:= mb862xxfb.o mb862xxfb_accel.o
+obj-$(CONFIG_FB_MB862XX) += mb862xxfb.o
+
+mb862xxfb-y := mb862xxfbdrv.o mb862xxfb_accel.o
+mb862xxfb-$(CONFIG_FB_MB862XX_I2C) += mb862xx-i2c.o
diff --git a/drivers/video/mb862xx/mb862xx-i2c.c b/drivers/video/mb862xx/mb862xx-i2c.c
new file mode 100644
index 000000000000..b953099edd8e
--- /dev/null
+++ b/drivers/video/mb862xx/mb862xx-i2c.c
@@ -0,0 +1,178 @@
+/*
+ * Coral-P(A)/Lime I2C adapter driver
+ *
+ * (C) 2011 DENX Software Engineering, Anatolij Gustschin <agust@denx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/fb.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include "mb862xxfb.h"
+#include "mb862xx_reg.h"
+
+static int mb862xx_i2c_wait_event(struct i2c_adapter *adap)
+{
+	struct mb862xxfb_par *par = adap->algo_data;
+	u32 reg;
+
+	do {
+		udelay(1);
+		reg = inreg(i2c, GC_I2C_BCR);
+		if (reg & (I2C_INT | I2C_BER))
+			break;
+	} while (1);
+
+	return (reg & I2C_BER) ? 0 : 1;
+}
+
+static int mb862xx_i2c_do_address(struct i2c_adapter *adap, int addr)
+{
+	struct mb862xxfb_par *par = adap->algo_data;
+
+	outreg(i2c, GC_I2C_DAR, addr);
+	outreg(i2c, GC_I2C_CCR, I2C_CLOCK_AND_ENABLE);
+	outreg(i2c, GC_I2C_BCR, par->i2c_rs ? I2C_REPEATED_START : I2C_START);
+	if (!mb862xx_i2c_wait_event(adap))
+		return -EIO;
+	par->i2c_rs = !(inreg(i2c, GC_I2C_BSR) & I2C_LRB);
+	return par->i2c_rs;
+}
+
+static int mb862xx_i2c_write_byte(struct i2c_adapter *adap, u8 byte)
+{
+	struct mb862xxfb_par *par = adap->algo_data;
+
+	outreg(i2c, GC_I2C_DAR, byte);
+	outreg(i2c, GC_I2C_BCR, I2C_START);
+	if (!mb862xx_i2c_wait_event(adap))
+		return -EIO;
+	return !(inreg(i2c, GC_I2C_BSR) & I2C_LRB);
+}
+
+static int mb862xx_i2c_read_byte(struct i2c_adapter *adap, u8 *byte, int last)
+{
+	struct mb862xxfb_par *par = adap->algo_data;
+
+	outreg(i2c, GC_I2C_BCR, I2C_START | (last ? 0 : I2C_ACK));
+	if (!mb862xx_i2c_wait_event(adap))
+		return 0;
+	*byte = inreg(i2c, GC_I2C_DAR);
+	return 1;
+}
+
+void mb862xx_i2c_stop(struct i2c_adapter *adap)
+{
+	struct mb862xxfb_par *par = adap->algo_data;
+
+	outreg(i2c, GC_I2C_BCR, I2C_STOP);
+	outreg(i2c, GC_I2C_CCR, I2C_DISABLE);
+	par->i2c_rs = 0;
+}
+
+static int mb862xx_i2c_read(struct i2c_adapter *adap, struct i2c_msg *m)
+{
+	int i, ret = 0;
+	int last = m->len - 1;
+
+	for (i = 0; i < m->len; i++) {
+		if (!mb862xx_i2c_read_byte(adap, &m->buf[i], i == last)) {
+			ret = -EIO;
+			break;
+		}
+	}
+	return ret;
+}
+
+static int mb862xx_i2c_write(struct i2c_adapter *adap, struct i2c_msg *m)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < m->len; i++) {
+		if (!mb862xx_i2c_write_byte(adap, m->buf[i])) {
+			ret = -EIO;
+			break;
+		}
+	}
+	return ret;
+}
+
+static int mb862xx_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+			int num)
+{
+	struct mb862xxfb_par *par = adap->algo_data;
+	struct i2c_msg *m;
+	int addr;
+	int i = 0, err = 0;
+
+	dev_dbg(par->dev, "%s: %d msgs\n", __func__, num);
+
+	for (i = 0; i < num; i++) {
+		m = &msgs[i];
+		if (!m->len) {
+			dev_dbg(par->dev, "%s: null msgs\n", __func__);
+			continue;
+		}
+		addr = m->addr;
+		if (m->flags & I2C_M_RD)
+			addr |= 1;
+
+		err = mb862xx_i2c_do_address(adap, addr);
+		if (err < 0)
+			break;
+		if (m->flags & I2C_M_RD)
+			err = mb862xx_i2c_read(adap, m);
+		else
+			err = mb862xx_i2c_write(adap, m);
+	}
+
+	if (i)
+		mb862xx_i2c_stop(adap);
+
+	return (err < 0) ? err : i;
+}
+
+static u32 mb862xx_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_SMBUS_BYTE_DATA;
+}
+
+static const struct i2c_algorithm mb862xx_algo = {
+	.master_xfer	= mb862xx_xfer,
+	.functionality	= mb862xx_func,
+};
+
+static struct i2c_adapter mb862xx_i2c_adapter = {
+	.name		= "MB862xx I2C adapter",
+	.algo		= &mb862xx_algo,
+	.owner		= THIS_MODULE,
+};
+
+int mb862xx_i2c_init(struct mb862xxfb_par *par)
+{
+	int ret;
+
+	mb862xx_i2c_adapter.algo_data = par;
+	par->adap = &mb862xx_i2c_adapter;
+
+	ret = i2c_add_adapter(par->adap);
+	if (ret < 0) {
+		dev_err(par->dev, "failed to add %s\n",
+			mb862xx_i2c_adapter.name);
+	}
+	return ret;
+}
+
+void mb862xx_i2c_exit(struct mb862xxfb_par *par)
+{
+	if (par->adap) {
+		i2c_del_adapter(par->adap);
+		par->adap = NULL;
+	}
+}
diff --git a/drivers/video/mb862xx/mb862xx_reg.h b/drivers/video/mb862xx/mb862xx_reg.h
index 2ba65e118500..9df48b8edc94 100644
--- a/drivers/video/mb862xx/mb862xx_reg.h
+++ b/drivers/video/mb862xx/mb862xx_reg.h
@@ -5,11 +5,8 @@
 #ifndef _MB862XX_REG_H
 #define _MB862XX_REG_H
 
-#ifdef MB862XX_MMIO_BOTTOM
-#define MB862XX_MMIO_BASE	0x03fc0000
-#else
 #define MB862XX_MMIO_BASE	0x01fc0000
-#endif
+#define MB862XX_MMIO_HIGH_BASE	0x03fc0000
 #define MB862XX_I2C_BASE	0x0000c000
 #define MB862XX_DISP_BASE	0x00010000
 #define MB862XX_CAP_BASE	0x00018000
@@ -23,6 +20,7 @@
 #define GC_IMASK		0x00000024
 #define GC_SRST			0x0000002c
 #define GC_CCF			0x00000038
+#define GC_RSW			0x0000005c
 #define GC_CID			0x000000f0
 #define GC_REVISION		0x00000084
 
@@ -53,10 +51,16 @@
 #define GC_L0OA0		0x00000024
 #define GC_L0DA0		0x00000028
 #define GC_L0DY_L0DX		0x0000002c
+#define GC_L1M			0x00000030
+#define GC_L1DA			0x00000034
 #define GC_DCM1			0x00000100
 #define GC_L0EM			0x00000110
 #define GC_L0WY_L0WX		0x00000114
 #define GC_L0WH_L0WW		0x00000118
+#define GC_L1EM			0x00000120
+#define GC_L1WY_L1WX		0x00000124
+#define GC_L1WH_L1WW		0x00000128
+#define GC_DLS			0x00000180
 #define GC_DCM2			0x00000104
 #define GC_DCM3			0x00000108
 #define GC_CPM_CUTC		0x000000a0
@@ -68,6 +72,11 @@
 
 #define GC_CPM_CEN0		0x00100000
 #define GC_CPM_CEN1		0x00200000
+#define GC_DCM1_DEN		0x80000000
+#define GC_DCM1_L1E		0x00020000
+#define GC_L1M_16		0x80000000
+#define GC_L1M_YC		0x40000000
+#define GC_L1M_CS		0x20000000
 
 #define GC_DCM01_ESY		0x00000004
 #define GC_DCM01_SC		0x00003f00
@@ -79,9 +88,50 @@
 #define GC_L0M_L0C_16		0x80000000
 #define GC_L0EM_L0EC_24		0x40000000
 #define GC_L0M_L0W_UNIT		64
+#define GC_L1EM_DM		0x02000000
 
 #define GC_DISP_REFCLK_400	400
 
+/* I2C */
+#define GC_I2C_BSR		0x00000000	/* BSR */
+#define GC_I2C_BCR		0x00000004	/* BCR */
+#define GC_I2C_CCR		0x00000008	/* CCR */
+#define GC_I2C_ADR		0x0000000C	/* ADR */
+#define GC_I2C_DAR		0x00000010	/* DAR */
+
+#define I2C_DISABLE		0x00000000
+#define I2C_STOP		0x00000000
+#define I2C_START		0x00000010
+#define I2C_REPEATED_START	0x00000030
+#define I2C_CLOCK_AND_ENABLE	0x0000003f
+#define I2C_READY		0x01
+#define I2C_INT			0x01
+#define I2C_INTE		0x02
+#define I2C_ACK			0x08
+#define I2C_BER			0x80
+#define I2C_BEIE		0x40
+#define I2C_TRX			0x80
+#define I2C_LRB			0x10
+
+/* Capture registers and bits */
+#define GC_CAP_VCM		0x00000000
+#define GC_CAP_CSC		0x00000004
+#define GC_CAP_VCS		0x00000008
+#define GC_CAP_CBM		0x00000010
+#define GC_CAP_CBOA		0x00000014
+#define GC_CAP_CBLA		0x00000018
+#define GC_CAP_IMG_START	0x0000001C
+#define GC_CAP_IMG_END		0x00000020
+#define GC_CAP_CMSS		0x00000048
+#define GC_CAP_CMDS		0x0000004C
+
+#define GC_VCM_VIE		0x80000000
+#define GC_VCM_CM		0x03000000
+#define GC_VCM_VS_PAL		0x00000002
+#define GC_CBM_OO		0x80000000
+#define GC_CBM_HRV		0x00000010
+#define GC_CBM_CBST		0x00000001
+
 /* Carmine specific */
 #define MB86297_DRAW_BASE		0x00020000
 #define MB86297_DISP0_BASE		0x00100000
diff --git a/drivers/video/mb862xx/mb862xxfb.h b/drivers/video/mb862xx/mb862xxfb.h
index d7e7cb76bbf2..8550630c1e01 100644
--- a/drivers/video/mb862xx/mb862xxfb.h
+++ b/drivers/video/mb862xx/mb862xxfb.h
@@ -1,6 +1,26 @@
 #ifndef __MB862XX_H__
 #define __MB862XX_H__
 
+struct mb862xx_l1_cfg {
+	unsigned short sx;
+	unsigned short sy;
+	unsigned short sw;
+	unsigned short sh;
+	unsigned short dx;
+	unsigned short dy;
+	unsigned short dw;
+	unsigned short dh;
+	int mirror;
+};
+
+#define MB862XX_BASE		'M'
+#define MB862XX_L1_GET_CFG	_IOR(MB862XX_BASE, 0, struct mb862xx_l1_cfg*)
+#define MB862XX_L1_SET_CFG	_IOW(MB862XX_BASE, 1, struct mb862xx_l1_cfg*)
+#define MB862XX_L1_ENABLE	_IOW(MB862XX_BASE, 2, int)
+#define MB862XX_L1_CAP_CTL	_IOW(MB862XX_BASE, 3, int)
+
+#ifdef __KERNEL__
+
 #define PCI_VENDOR_ID_FUJITSU_LIMITED	0x10cf
 #define PCI_DEVICE_ID_FUJITSU_CORALP	0x2019
 #define PCI_DEVICE_ID_FUJITSU_CORALPA	0x201e
@@ -38,6 +58,8 @@ struct mb862xxfb_par {
 	void __iomem		*mmio_base;	/* remapped registers */
 	size_t			mapped_vram;	/* length of remapped vram */
 	size_t			mmio_len;	/* length of register region */
+	unsigned long		cap_buf;	/* capture buffers offset */
+	size_t			cap_len;	/* length of capture buffers */
 
 	void __iomem		*host;		/* relocatable reg. bases */
 	void __iomem		*i2c;
@@ -57,11 +79,23 @@ struct mb862xxfb_par {
 	unsigned int		refclk;		/* disp. reference clock */
 	struct mb862xx_gc_mode	*gc_mode;	/* GDC mode init data */
 	int			pre_init;	/* don't init display if 1 */
+	struct i2c_adapter	*adap;		/* GDC I2C bus adapter */
+	int			i2c_rs;
+
+	struct mb862xx_l1_cfg	l1_cfg;
+	int			l1_stride;
 
 	u32			pseudo_palette[16];
 };
 
 extern void mb862xxfb_init_accel(struct fb_info *info, int xres);
+#ifdef CONFIG_FB_MB862XX_I2C
+extern int mb862xx_i2c_init(struct mb862xxfb_par *par);
+extern void mb862xx_i2c_exit(struct mb862xxfb_par *par);
+#else
+static inline int mb862xx_i2c_init(struct mb862xxfb_par *par) { return 0; }
+static inline void mb862xx_i2c_exit(struct mb862xxfb_par *par) { }
+#endif
 
 #if defined(CONFIG_FB_MB862XX_LIME) && defined(CONFIG_FB_MB862XX_PCI_GDC)
 #error	"Select Lime GDC or CoralP/Carmine support, but not both together"
@@ -82,4 +116,6 @@ extern void mb862xxfb_init_accel(struct fb_info *info, int xres);
 
 #define pack(a, b)	(((a) << 16) | (b))
 
+#endif /* __KERNEL__ */
+
 #endif
diff --git a/drivers/video/mb862xx/mb862xxfb.c b/drivers/video/mb862xx/mb862xxfbdrv.c
index b1c4374cf940..ee1de3e26dec 100644
--- a/drivers/video/mb862xx/mb862xxfb.c
+++ b/drivers/video/mb862xx/mb862xxfbdrv.c
@@ -16,6 +16,7 @@
 
 #include <linux/fb.h>
 #include <linux/delay.h>
+#include <linux/uaccess.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/pci.h>
@@ -27,7 +28,7 @@
 
 #define NR_PALETTE		256
 #define MB862XX_MEM_SIZE	0x1000000
-#define CORALP_MEM_SIZE		0x4000000
+#define CORALP_MEM_SIZE		0x2000000
 #define CARMINE_MEM_SIZE	0x8000000
 #define DRV_NAME		"mb862xxfb"
 
@@ -309,6 +310,97 @@ static int mb862xxfb_blank(int mode, struct fb_info *fbi)
 	return 0;
 }
 
+static int mb862xxfb_ioctl(struct fb_info *fbi, unsigned int cmd,
+			   unsigned long arg)
+{
+	struct mb862xxfb_par *par = fbi->par;
+	struct mb862xx_l1_cfg *l1_cfg = &par->l1_cfg;
+	void __user *argp = (void __user *)arg;
+	int *enable;
+	u32 l1em = 0;
+
+	switch (cmd) {
+	case MB862XX_L1_GET_CFG:
+		if (copy_to_user(argp, l1_cfg, sizeof(*l1_cfg)))
+			return -EFAULT;
+		break;
+	case MB862XX_L1_SET_CFG:
+		if (copy_from_user(l1_cfg, argp, sizeof(*l1_cfg)))
+			return -EFAULT;
+		if ((l1_cfg->sw >= l1_cfg->dw) && (l1_cfg->sh >= l1_cfg->dh)) {
+			/* downscaling */
+			outreg(cap, GC_CAP_CSC,
+				pack((l1_cfg->sh << 11) / l1_cfg->dh,
+				     (l1_cfg->sw << 11) / l1_cfg->dw));
+			l1em = inreg(disp, GC_L1EM);
+			l1em &= ~GC_L1EM_DM;
+		} else if ((l1_cfg->sw <= l1_cfg->dw) &&
+			   (l1_cfg->sh <= l1_cfg->dh)) {
+			/* upscaling */
+			outreg(cap, GC_CAP_CSC,
+				pack((l1_cfg->sh << 11) / l1_cfg->dh,
+				     (l1_cfg->sw << 11) / l1_cfg->dw));
+			outreg(cap, GC_CAP_CMSS,
+				pack(l1_cfg->sw >> 1, l1_cfg->sh));
+			outreg(cap, GC_CAP_CMDS,
+				pack(l1_cfg->dw >> 1, l1_cfg->dh));
+			l1em = inreg(disp, GC_L1EM);
+			l1em |= GC_L1EM_DM;
+		}
+
+		if (l1_cfg->mirror) {
+			outreg(cap, GC_CAP_CBM,
+				inreg(cap, GC_CAP_CBM) | GC_CBM_HRV);
+			l1em |= l1_cfg->dw * 2 - 8;
+		} else {
+			outreg(cap, GC_CAP_CBM,
+				inreg(cap, GC_CAP_CBM) & ~GC_CBM_HRV);
+			l1em &= 0xffff0000;
+		}
+		outreg(disp, GC_L1EM, l1em);
+		break;
+	case MB862XX_L1_ENABLE:
+		enable = (int *)arg;
+		if (*enable) {
+			outreg(disp, GC_L1DA, par->cap_buf);
+			outreg(cap, GC_CAP_IMG_START,
+				pack(l1_cfg->sy >> 1, l1_cfg->sx));
+			outreg(cap, GC_CAP_IMG_END,
+				pack(l1_cfg->sh, l1_cfg->sw));
+			outreg(disp, GC_L1M, GC_L1M_16 | GC_L1M_YC | GC_L1M_CS |
+					     (par->l1_stride << 16));
+			outreg(disp, GC_L1WY_L1WX,
+				pack(l1_cfg->dy, l1_cfg->dx));
+			outreg(disp, GC_L1WH_L1WW,
+				pack(l1_cfg->dh - 1, l1_cfg->dw));
+			outreg(disp, GC_DLS, 1);
+			outreg(cap, GC_CAP_VCM,
+				GC_VCM_VIE | GC_VCM_CM | GC_VCM_VS_PAL);
+			outreg(disp, GC_DCM1, inreg(disp, GC_DCM1) |
+					      GC_DCM1_DEN | GC_DCM1_L1E);
+		} else {
+			outreg(cap, GC_CAP_VCM,
+				inreg(cap, GC_CAP_VCM) & ~GC_VCM_VIE);
+			outreg(disp, GC_DCM1,
+				inreg(disp, GC_DCM1) & ~GC_DCM1_L1E);
+		}
+		break;
+	case MB862XX_L1_CAP_CTL:
+		enable = (int *)arg;
+		if (*enable) {
+			outreg(cap, GC_CAP_VCM,
+				inreg(cap, GC_CAP_VCM) | GC_VCM_VIE);
+		} else {
+			outreg(cap, GC_CAP_VCM,
+				inreg(cap, GC_CAP_VCM) & ~GC_VCM_VIE);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 /* framebuffer ops */
 static struct fb_ops mb862xxfb_ops = {
 	.owner		= THIS_MODULE,
@@ -320,6 +412,7 @@ static struct fb_ops mb862xxfb_ops = {
 	.fb_fillrect	= cfb_fillrect,
 	.fb_copyarea	= cfb_copyarea,
 	.fb_imageblit	= cfb_imageblit,
+	.fb_ioctl	= mb862xxfb_ioctl,
 };
 
 /* initialize fb_info data */
@@ -328,6 +421,7 @@ static int mb862xxfb_init_fbinfo(struct fb_info *fbi)
 	struct mb862xxfb_par *par = fbi->par;
 	struct mb862xx_gc_mode *mode = par->gc_mode;
 	unsigned long reg;
+	int stride;
 
 	fbi->fbops = &mb862xxfb_ops;
 	fbi->pseudo_palette = par->pseudo_palette;
@@ -336,7 +430,6 @@ static int mb862xxfb_init_fbinfo(struct fb_info *fbi)
 
 	strcpy(fbi->fix.id, DRV_NAME);
 	fbi->fix.smem_start = (unsigned long)par->fb_base_phys;
-	fbi->fix.smem_len = par->mapped_vram;
 	fbi->fix.mmio_start = (unsigned long)par->mmio_base_phys;
 	fbi->fix.mmio_len = par->mmio_len;
 	fbi->fix.accel = FB_ACCEL_NONE;
@@ -420,6 +513,28 @@ static int mb862xxfb_init_fbinfo(struct fb_info *fbi)
 			 FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
 	fbi->fix.line_length = (fbi->var.xres_virtual *
 				fbi->var.bits_per_pixel) / 8;
+	fbi->fix.smem_len = fbi->fix.line_length * fbi->var.yres_virtual;
+
+	/*
+	 * reserve space for capture buffers and two cursors
+	 * at the end of vram: 720x576 * 2 * 2.2 + 64x64 * 16.
+	 */
+	par->cap_buf = par->mapped_vram - 0x1bd800 - 0x10000;
+	par->cap_len = 0x1bd800;
+	par->l1_cfg.sx = 0;
+	par->l1_cfg.sy = 0;
+	par->l1_cfg.sw = 720;
+	par->l1_cfg.sh = 576;
+	par->l1_cfg.dx = 0;
+	par->l1_cfg.dy = 0;
+	par->l1_cfg.dw = 720;
+	par->l1_cfg.dh = 576;
+	stride = par->l1_cfg.sw * (fbi->var.bits_per_pixel / 8);
+	par->l1_stride = stride / 64 + ((stride % 64) ? 1 : 0);
+	outreg(cap, GC_CAP_CBM, GC_CBM_OO | GC_CBM_CBST |
+				(par->l1_stride << 16));
+	outreg(cap, GC_CAP_CBOA, par->cap_buf);
+	outreg(cap, GC_CAP_CBLA, par->cap_buf + par->cap_len);
 	return 0;
 }
 
@@ -550,8 +665,7 @@ static int mb862xx_gdc_init(struct mb862xxfb_par *par)
 	return 0;
 }
 
-static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev,
-					       const struct of_device_id *id)
+static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct device *dev = &ofdev->dev;
@@ -583,7 +697,7 @@ static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev,
 		goto fbrel;
 	}
 
-	res_size = 1 + res.end - res.start;
+	res_size = resource_size(&res);
 	par->res = request_mem_region(res.start, res_size, DRV_NAME);
 	if (par->res == NULL) {
 		dev_err(dev, "Cannot claim framebuffer/mmio\n");
@@ -673,7 +787,7 @@ static int __devexit of_platform_mb862xx_remove(struct platform_device *ofdev)
 {
 	struct fb_info *fbi = dev_get_drvdata(&ofdev->dev);
 	struct mb862xxfb_par *par = fbi->par;
-	resource_size_t res_size = 1 + par->res->end - par->res->start;
+	resource_size_t res_size = resource_size(par->res);
 	unsigned long reg;
 
 	dev_dbg(fbi->dev, "%s release\n", fbi->fix.id);
@@ -717,7 +831,7 @@ static struct of_device_id __devinitdata of_platform_mb862xx_tbl[] = {
 	{ /* end */ }
 };
 
-static struct of_platform_driver of_platform_mb862xxfb_driver = {
+static struct platform_driver of_platform_mb862xxfb_driver = {
 	.driver = {
 		.name = DRV_NAME,
 		.owner = THIS_MODULE,
@@ -743,22 +857,38 @@ static int coralp_init(struct mb862xxfb_par *par)
 
 	par->refclk = GC_DISP_REFCLK_400;
 
+	if (par->mapped_vram >= 0x2000000) {
+		/* relocate gdc registers space */
+		writel(1, par->fb_base + MB862XX_MMIO_BASE + GC_RSW);
+		udelay(1); /* wait at least 20 bus cycles */
+	}
+
 	ver = inreg(host, GC_CID);
 	cn = (ver & GC_CID_CNAME_MSK) >> 8;
 	ver = ver & GC_CID_VERSION_MSK;
 	if (cn == 3) {
+		unsigned long reg;
+
 		dev_info(par->dev, "Fujitsu Coral-%s GDC Rev.%d found\n",\
 			 (ver == 6) ? "P" : (ver == 8) ? "PA" : "?",
 			 par->pdev->revision);
-		outreg(host, GC_CCF, GC_CCF_CGE_166 | GC_CCF_COT_133);
-		udelay(200);
-		outreg(host, GC_MMR, GC_MMR_CORALP_EVB_VAL);
-		udelay(10);
+		reg = inreg(disp, GC_DCM1);
+		if (reg & GC_DCM01_DEN && reg & GC_DCM01_L0E)
+			par->pre_init = 1;
+
+		if (!par->pre_init) {
+			outreg(host, GC_CCF, GC_CCF_CGE_166 | GC_CCF_COT_133);
+			udelay(200);
+			outreg(host, GC_MMR, GC_MMR_CORALP_EVB_VAL);
+			udelay(10);
+		}
 		/* Clear interrupt status */
 		outreg(host, GC_IST, 0);
 	} else {
 		return -ENODEV;
 	}
+
+	mb862xx_i2c_init(par);
 	return 0;
 }
 
@@ -900,7 +1030,13 @@ static int __devinit mb862xx_pci_probe(struct pci_dev *pdev,
 	case PCI_DEVICE_ID_FUJITSU_CORALPA:
 		par->fb_base_phys = pci_resource_start(par->pdev, 0);
 		par->mapped_vram = CORALP_MEM_SIZE;
-		par->mmio_base_phys = par->fb_base_phys + MB862XX_MMIO_BASE;
+		if (par->mapped_vram >= 0x2000000) {
+			par->mmio_base_phys = par->fb_base_phys +
+					      MB862XX_MMIO_HIGH_BASE;
+		} else {
+			par->mmio_base_phys = par->fb_base_phys +
+					      MB862XX_MMIO_BASE;
+		}
 		par->mmio_len = MB862XX_MMIO_SIZE;
 		par->type = BT_CORALP;
 		break;
@@ -1010,6 +1146,8 @@ static void __devexit mb862xx_pci_remove(struct pci_dev *pdev)
 		outreg(host, GC_IMASK, 0);
 	}
 
+	mb862xx_i2c_exit(par);
+
 	device_remove_file(&pdev->dev, &dev_attr_dispregs);
 
 	pci_set_drvdata(pdev, NULL);
@@ -1038,7 +1176,7 @@ static int __devinit mb862xxfb_init(void)
 	int ret = -ENODEV;
 
 #if defined(CONFIG_FB_MB862XX_LIME)
-	ret = of_register_platform_driver(&of_platform_mb862xxfb_driver);
+	ret = platform_driver_register(&of_platform_mb862xxfb_driver);
 #endif
 #if defined(CONFIG_FB_MB862XX_PCI_GDC)
 	ret = pci_register_driver(&mb862xxfb_pci_driver);
@@ -1049,7 +1187,7 @@ static int __devinit mb862xxfb_init(void)
 static void __exit mb862xxfb_exit(void)
 {
 #if defined(CONFIG_FB_MB862XX_LIME)
-	of_unregister_platform_driver(&of_platform_mb862xxfb_driver);
+	platform_driver_unregister(&of_platform_mb862xxfb_driver);
 #endif
 #if defined(CONFIG_FB_MB862XX_PCI_GDC)
 	pci_unregister_driver(&mb862xxfb_pci_driver);
diff --git a/drivers/video/metronomefb.c b/drivers/video/metronomefb.c
index 63ed3b72b01c..97d45e5115e2 100644
--- a/drivers/video/metronomefb.c
+++ b/drivers/video/metronomefb.c
@@ -628,12 +628,10 @@ static int __devinit metronomefb_probe(struct platform_device *dev)
 	/* we need to add a spare page because our csum caching scheme walks
 	 * to the end of the page */
 	videomemorysize = PAGE_SIZE + (fw * fh);
-	videomemory = vmalloc(videomemorysize);
+	videomemory = vzalloc(videomemorysize);
 	if (!videomemory)
 		goto err_fb_rel;
 
-	memset(videomemory, 0, videomemorysize);
-
 	info->screen_base = (char __force __iomem *)videomemory;
 	info->fbops = &metronomefb_ops;
 
@@ -765,7 +763,7 @@ static int __devexit metronomefb_remove(struct platform_device *dev)
 
 static struct platform_driver metronomefb_driver = {
 	.probe	= metronomefb_probe,
-	.remove = metronomefb_remove,
+	.remove = __devexit_p(metronomefb_remove),
 	.driver	= {
 		.owner	= THIS_MODULE,
 		.name	= "metronomefb",
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index 48c3ea8652b6..cb175fe7abc0 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -1128,3 +1128,4 @@ EXPORT_SYMBOL(fb_find_best_mode);
 EXPORT_SYMBOL(fb_find_nearest_mode);
 EXPORT_SYMBOL(fb_videomode_to_modelist);
 EXPORT_SYMBOL(fb_find_mode);
+EXPORT_SYMBOL(fb_find_mode_cvt);
diff --git a/drivers/video/msm/mddi.c b/drivers/video/msm/mddi.c
index b66d86ac7cea..178b0720bd79 100644
--- a/drivers/video/msm/mddi.c
+++ b/drivers/video/msm/mddi.c
@@ -679,7 +679,7 @@ static int __devinit mddi_probe(struct platform_device *pdev)
 		printk(KERN_ERR "mddi: no associated mem resource!\n");
 		return -ENOMEM;
 	}
-	mddi->base = ioremap(resource->start, resource->end - resource->start);
+	mddi->base = ioremap(resource->start, resource_size(resource));
 	if (!mddi->base) {
 		printk(KERN_ERR "mddi: failed to remap base!\n");
 		ret = -EINVAL;
diff --git a/drivers/video/msm/mdp.c b/drivers/video/msm/mdp.c
index c3636d55a3c5..243d16f09b8a 100644
--- a/drivers/video/msm/mdp.c
+++ b/drivers/video/msm/mdp.c
@@ -406,8 +406,7 @@ int mdp_probe(struct platform_device *pdev)
 		goto error_get_irq;
 	}
 
-	mdp->base = ioremap(resource->start,
-			    resource->end - resource->start);
+	mdp->base = ioremap(resource->start, resource_size(resource));
 	if (mdp->base == 0) {
 		printk(KERN_ERR "msmfb: cannot allocate mdp regs!\n");
 		ret = -ENOMEM;
diff --git a/drivers/video/msm/mdp_hw.h b/drivers/video/msm/mdp_hw.h
index 4e3deb4e592b..d80477415caa 100644
--- a/drivers/video/msm/mdp_hw.h
+++ b/drivers/video/msm/mdp_hw.h
@@ -449,6 +449,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 #define PPP_CFG_MDP_XRGB_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 #define PPP_CFG_MDP_RGBA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 #define PPP_CFG_MDP_BGRA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
+#define PPP_CFG_MDP_RGBX_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 
 #define PPP_CFG_MDP_Y_CBCR_H2V2(dir)   (PPP_##dir##_C2R_8BIT | \
 					PPP_##dir##_C0G_8BIT | \
@@ -488,12 +489,14 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 	MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8)
 #define PPP_PACK_PATTERN_MDP_RGB_888 PPP_PACK_PATTERN_MDP_RGB_565
 #define PPP_PACK_PATTERN_MDP_XRGB_8888 \
-	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
+	MDP_GET_PACK_PATTERN(CLR_B, CLR_G, CLR_R, CLR_ALPHA, 8)
 #define PPP_PACK_PATTERN_MDP_ARGB_8888 PPP_PACK_PATTERN_MDP_XRGB_8888
 #define PPP_PACK_PATTERN_MDP_RGBA_8888 \
 	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
 #define PPP_PACK_PATTERN_MDP_BGRA_8888 \
 	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
+#define PPP_PACK_PATTERN_MDP_RGBX_8888 \
+	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
 #define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1 \
 	MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8)
 #define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V2 PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1
@@ -509,6 +512,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 #define PPP_CHROMA_SAMP_MDP_ARGB_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_RGBA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_BGRA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
+#define PPP_CHROMA_SAMP_MDP_RGBX_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
 #define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V2(dir) PPP_OP_##dir##_CHROMA_420
 #define PPP_CHROMA_SAMP_MDP_Y_CRCB_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
@@ -523,6 +527,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 	[MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888,\
 	[MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888,\
 	[MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888,\
+	[MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888,\
 	[MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1,\
 	[MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2,\
 	[MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1,\
@@ -536,6 +541,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 	[MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888(dir),\
 	[MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888(dir),\
 	[MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888(dir),\
+	[MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888(dir),\
 	[MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1(dir),\
 	[MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2(dir),\
 	[MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1(dir),\
@@ -547,7 +553,8 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 		       (img == MDP_YCRYCB_H2V1))
 #define IS_RGB(img) ((img == MDP_RGB_565) | (img == MDP_RGB_888) | \
 		     (img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
-		     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888))
+		     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888) | \
+		     (img == MDP_RGBX_8888))
 #define HAS_ALPHA(img) ((img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
 			(img == MDP_BGRA_8888))
 
diff --git a/drivers/video/msm/mdp_ppp.c b/drivers/video/msm/mdp_ppp.c
index 4ff001f4cbbd..2b6564e8bfea 100644
--- a/drivers/video/msm/mdp_ppp.c
+++ b/drivers/video/msm/mdp_ppp.c
@@ -69,6 +69,7 @@ static uint32_t bytes_per_pixel[] = {
 	[MDP_ARGB_8888] = 4,
 	[MDP_RGBA_8888] = 4,
 	[MDP_BGRA_8888] = 4,
+	[MDP_RGBX_8888] = 4,
 	[MDP_Y_CBCR_H2V1] = 1,
 	[MDP_Y_CBCR_H2V2] = 1,
 	[MDP_Y_CRCB_H2V1] = 1,
diff --git a/drivers/video/msm/msm_fb.c b/drivers/video/msm/msm_fb.c
index debe5933fd2e..c6e3b4fcdd68 100644
--- a/drivers/video/msm/msm_fb.c
+++ b/drivers/video/msm/msm_fb.c
@@ -81,7 +81,6 @@ struct msmfb_info {
 	spinlock_t update_lock;
 	struct mutex panel_init_lock;
 	wait_queue_head_t frame_wq;
-	struct workqueue_struct *resume_workqueue;
 	struct work_struct resume_work;
 	struct msmfb_callback dma_callback;
 	struct msmfb_callback vsync_callback;
@@ -111,7 +110,7 @@ static void msmfb_handle_dma_interrupt(struct msmfb_callback *callback)
 	if (msmfb->sleeping == UPDATING &&
 	    msmfb->frame_done == msmfb->update_frame) {
 		DLOG(SUSPEND_RESUME, "full update completed\n");
-		queue_work(msmfb->resume_workqueue, &msmfb->resume_work);
+		schedule_work(&msmfb->resume_work);
 	}
 	spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
 	wake_up(&msmfb->frame_wq);
@@ -220,8 +219,8 @@ restart:
 
 	sleeping = msmfb->sleeping;
 	/* on a full update, if the last frame has not completed, wait for it */
-	if (pan_display && (msmfb->frame_requested != msmfb->frame_done ||
-			    sleeping == UPDATING)) {
+	if ((pan_display && msmfb->frame_requested != msmfb->frame_done) ||
+			    sleeping == UPDATING) {
 		int ret;
 		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
 		ret = wait_event_interruptible_timeout(msmfb->frame_wq,
@@ -470,6 +469,18 @@ static void setup_fb_info(struct msmfb_info *msmfb)
 	fb_info->var.yoffset = 0;
 
 	if (msmfb->panel->caps & MSMFB_CAP_PARTIAL_UPDATES) {
+		/*
+		 * Set the param in the fixed screen, so userspace can't
+		 * change it. This will be used to check for the
+		 * capability.
+		 */
+		fb_info->fix.reserved[0] = 0x5444;
+		fb_info->fix.reserved[1] = 0x5055;
+
+		/*
+		 * This preloads the value so that if userspace doesn't
+		 * change it, it will be a full update
+		 */
 		fb_info->var.reserved[0] = 0x54445055;
 		fb_info->var.reserved[1] = 0;
 		fb_info->var.reserved[2] = (uint16_t)msmfb->xres |
@@ -514,10 +525,9 @@ static int setup_fbmem(struct msmfb_info *msmfb, struct platform_device *pdev)
 		return -ENOMEM;
 	}
 	fb->fix.smem_start = resource->start;
-	fb->fix.smem_len = resource->end - resource->start;
-	fbram = ioremap(resource->start,
-			resource->end - resource->start);
-	if (fbram == 0) {
+	fb->fix.smem_len = resource_size(resource);
+	fbram = ioremap(resource->start, resource_size(resource));
+	if (fbram == NULL) {
 		printk(KERN_ERR "msmfb: cannot allocate fbram!\n");
 		return -ENOMEM;
 	}
@@ -559,12 +569,6 @@ static int msmfb_probe(struct platform_device *pdev)
 	spin_lock_init(&msmfb->update_lock);
 	mutex_init(&msmfb->panel_init_lock);
 	init_waitqueue_head(&msmfb->frame_wq);
-	msmfb->resume_workqueue = create_workqueue("panel_on");
-	if (msmfb->resume_workqueue == NULL) {
-		printk(KERN_ERR "failed to create panel_on workqueue\n");
-		ret = -ENOMEM;
-		goto error_create_workqueue;
-	}
 	INIT_WORK(&msmfb->resume_work, power_on_panel);
 	msmfb->black = kzalloc(msmfb->fb->var.bits_per_pixel*msmfb->xres,
 			       GFP_KERNEL);
@@ -589,8 +593,6 @@ static int msmfb_probe(struct platform_device *pdev)
 	return 0;
 
 error_register_framebuffer:
-	destroy_workqueue(msmfb->resume_workqueue);
-error_create_workqueue:
 	iounmap(fb->screen_base);
 error_setup_fbmem:
 	framebuffer_release(msmfb->fb);
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
new file mode 100644
index 000000000000..0b2f2dd41416
--- /dev/null
+++ b/drivers/video/mxsfb.c
@@ -0,0 +1,919 @@
+/*
+ * Copyright (C) 2010 Juergen Beisert, Pengutronix
+ *
+ * This code is based on:
+ * Author: Vitaly Wool <vital@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#define DRIVER_NAME "mxsfb"
+
+/**
+ * @file
+ * @brief LCDIF driver for i.MX23 and i.MX28
+ *
+ * The LCDIF support four modes of operation
+ * - MPU interface (to drive smart displays) -> not supported yet
+ * - VSYNC interface (like MPU interface plus Vsync) -> not supported yet
+ * - Dotclock interface (to drive LC displays with RGB data and sync signals)
+ * - DVI (to drive ITU-R BT656)  -> not supported yet
+ *
+ * This driver depends on a correct setup of the pins used for this purpose
+ * (platform specific).
+ *
+ * For the developer: Don't forget to set the data bus width to the display
+ * in the imx_fb_videomode structure. You will else end up with ugly colours.
+ * If you fight against jitter you can vary the clock delay. This is a feature
+ * of the i.MX28 and you can vary it between 2 ns ... 8 ns in 2 ns steps. Give
+ * the required value in the imx_fb_videomode structure.
+ */
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <mach/mxsfb.h>
+
+#define REG_SET	4
+#define REG_CLR	8
+
+#define LCDC_CTRL			0x00
+#define LCDC_CTRL1			0x10
+#define LCDC_V4_CTRL2			0x20
+#define LCDC_V3_TRANSFER_COUNT		0x20
+#define LCDC_V4_TRANSFER_COUNT		0x30
+#define LCDC_V4_CUR_BUF			0x40
+#define LCDC_V4_NEXT_BUF		0x50
+#define LCDC_V3_CUR_BUF			0x30
+#define LCDC_V3_NEXT_BUF		0x40
+#define LCDC_TIMING			0x60
+#define LCDC_VDCTRL0			0x70
+#define LCDC_VDCTRL1			0x80
+#define LCDC_VDCTRL2			0x90
+#define LCDC_VDCTRL3			0xa0
+#define LCDC_VDCTRL4			0xb0
+#define LCDC_DVICTRL0			0xc0
+#define LCDC_DVICTRL1			0xd0
+#define LCDC_DVICTRL2			0xe0
+#define LCDC_DVICTRL3			0xf0
+#define LCDC_DVICTRL4			0x100
+#define LCDC_V4_DATA			0x180
+#define LCDC_V3_DATA			0x1b0
+#define LCDC_V4_DEBUG0			0x1d0
+#define LCDC_V3_DEBUG0			0x1f0
+
+#define CTRL_SFTRST			(1 << 31)
+#define CTRL_CLKGATE			(1 << 30)
+#define CTRL_BYPASS_COUNT		(1 << 19)
+#define CTRL_VSYNC_MODE			(1 << 18)
+#define CTRL_DOTCLK_MODE		(1 << 17)
+#define CTRL_DATA_SELECT		(1 << 16)
+#define CTRL_SET_BUS_WIDTH(x)		(((x) & 0x3) << 10)
+#define CTRL_GET_BUS_WIDTH(x)		(((x) >> 10) & 0x3)
+#define CTRL_SET_WORD_LENGTH(x)		(((x) & 0x3) << 8)
+#define CTRL_GET_WORD_LENGTH(x)		(((x) >> 8) & 0x3)
+#define CTRL_MASTER			(1 << 5)
+#define CTRL_DF16			(1 << 3)
+#define CTRL_DF18			(1 << 2)
+#define CTRL_DF24			(1 << 1)
+#define CTRL_RUN			(1 << 0)
+
+#define CTRL1_FIFO_CLEAR		(1 << 21)
+#define CTRL1_SET_BYTE_PACKAGING(x)	(((x) & 0xf) << 16)
+#define CTRL1_GET_BYTE_PACKAGING(x)	(((x) >> 16) & 0xf)
+
+#define TRANSFER_COUNT_SET_VCOUNT(x)	(((x) & 0xffff) << 16)
+#define TRANSFER_COUNT_GET_VCOUNT(x)	(((x) >> 16) & 0xffff)
+#define TRANSFER_COUNT_SET_HCOUNT(x)	((x) & 0xffff)
+#define TRANSFER_COUNT_GET_HCOUNT(x)	((x) & 0xffff)
+
+
+#define VDCTRL0_ENABLE_PRESENT		(1 << 28)
+#define VDCTRL0_VSYNC_ACT_HIGH		(1 << 27)
+#define VDCTRL0_HSYNC_ACT_HIGH		(1 << 26)
+#define VDCTRL0_DOTCLK_ACT_FAILING	(1 << 25)
+#define VDCTRL0_ENABLE_ACT_HIGH		(1 << 24)
+#define VDCTRL0_VSYNC_PERIOD_UNIT	(1 << 21)
+#define VDCTRL0_VSYNC_PULSE_WIDTH_UNIT	(1 << 20)
+#define VDCTRL0_HALF_LINE		(1 << 19)
+#define VDCTRL0_HALF_LINE_MODE		(1 << 18)
+#define VDCTRL0_SET_VSYNC_PULSE_WIDTH(x) ((x) & 0x3ffff)
+#define VDCTRL0_GET_VSYNC_PULSE_WIDTH(x) ((x) & 0x3ffff)
+
+#define VDCTRL2_SET_HSYNC_PERIOD(x)	((x) & 0x3ffff)
+#define VDCTRL2_GET_HSYNC_PERIOD(x)	((x) & 0x3ffff)
+
+#define VDCTRL3_MUX_SYNC_SIGNALS	(1 << 29)
+#define VDCTRL3_VSYNC_ONLY		(1 << 28)
+#define SET_HOR_WAIT_CNT(x)		(((x) & 0xfff) << 16)
+#define GET_HOR_WAIT_CNT(x)		(((x) >> 16) & 0xfff)
+#define SET_VERT_WAIT_CNT(x)		((x) & 0xffff)
+#define GET_VERT_WAIT_CNT(x)		((x) & 0xffff)
+
+#define VDCTRL4_SET_DOTCLK_DLY(x)	(((x) & 0x7) << 29) /* v4 only */
+#define VDCTRL4_GET_DOTCLK_DLY(x)	(((x) >> 29) & 0x7) /* v4 only */
+#define VDCTRL4_SYNC_SIGNALS_ON		(1 << 18)
+#define SET_DOTCLK_H_VALID_DATA_CNT(x)	((x) & 0x3ffff)
+
+#define DEBUG0_HSYNC			(1 < 26)
+#define DEBUG0_VSYNC			(1 < 25)
+
+#define MIN_XRES			120
+#define MIN_YRES			120
+
+#define RED 0
+#define GREEN 1
+#define BLUE 2
+#define TRANSP 3
+
+enum mxsfb_devtype {
+	MXSFB_V3,
+	MXSFB_V4,
+};
+
+/* CPU dependent register offsets */
+struct mxsfb_devdata {
+	unsigned transfer_count;
+	unsigned cur_buf;
+	unsigned next_buf;
+	unsigned debug0;
+	unsigned hs_wdth_mask;
+	unsigned hs_wdth_shift;
+	unsigned ipversion;
+};
+
+struct mxsfb_info {
+	struct fb_info fb_info;
+	struct platform_device *pdev;
+	struct clk *clk;
+	void __iomem *base;	/* registers */
+	unsigned allocated_size;
+	int enabled;
+	unsigned ld_intf_width;
+	unsigned dotclk_delay;
+	const struct mxsfb_devdata *devdata;
+	int mapped;
+};
+
+#define mxsfb_is_v3(host) (host->devdata->ipversion == 3)
+#define mxsfb_is_v4(host) (host->devdata->ipversion == 4)
+
+static const struct mxsfb_devdata mxsfb_devdata[] = {
+	[MXSFB_V3] = {
+		.transfer_count = LCDC_V3_TRANSFER_COUNT,
+		.cur_buf = LCDC_V3_CUR_BUF,
+		.next_buf = LCDC_V3_NEXT_BUF,
+		.debug0 = LCDC_V3_DEBUG0,
+		.hs_wdth_mask = 0xff,
+		.hs_wdth_shift = 24,
+		.ipversion = 3,
+	},
+	[MXSFB_V4] = {
+		.transfer_count = LCDC_V4_TRANSFER_COUNT,
+		.cur_buf = LCDC_V4_CUR_BUF,
+		.next_buf = LCDC_V4_NEXT_BUF,
+		.debug0 = LCDC_V4_DEBUG0,
+		.hs_wdth_mask = 0x3fff,
+		.hs_wdth_shift = 18,
+		.ipversion = 4,
+	},
+};
+
+#define to_imxfb_host(x) (container_of(x, struct mxsfb_info, fb_info))
+
+/* mask and shift depends on architecture */
+static inline u32 set_hsync_pulse_width(struct mxsfb_info *host, unsigned val)
+{
+	return (val & host->devdata->hs_wdth_mask) <<
+		host->devdata->hs_wdth_shift;
+}
+
+static inline u32 get_hsync_pulse_width(struct mxsfb_info *host, unsigned val)
+{
+	return (val >> host->devdata->hs_wdth_shift) &
+		host->devdata->hs_wdth_mask;
+}
+
+static const struct fb_bitfield def_rgb565[] = {
+	[RED] = {
+		.offset = 11,
+		.length = 5,
+	},
+	[GREEN] = {
+		.offset = 5,
+		.length = 6,
+	},
+	[BLUE] = {
+		.offset = 0,
+		.length = 5,
+	},
+	[TRANSP] = {	/* no support for transparency */
+		.length = 0,
+	}
+};
+
+static const struct fb_bitfield def_rgb666[] = {
+	[RED] = {
+		.offset = 16,
+		.length = 6,
+	},
+	[GREEN] = {
+		.offset = 8,
+		.length = 6,
+	},
+	[BLUE] = {
+		.offset = 0,
+		.length = 6,
+	},
+	[TRANSP] = {	/* no support for transparency */
+		.length = 0,
+	}
+};
+
+static const struct fb_bitfield def_rgb888[] = {
+	[RED] = {
+		.offset = 16,
+		.length = 8,
+	},
+	[GREEN] = {
+		.offset = 8,
+		.length = 8,
+	},
+	[BLUE] = {
+		.offset = 0,
+		.length = 8,
+	},
+	[TRANSP] = {	/* no support for transparency */
+		.length = 0,
+	}
+};
+
+static inline unsigned chan_to_field(unsigned chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int mxsfb_check_var(struct fb_var_screeninfo *var,
+		struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	const struct fb_bitfield *rgb = NULL;
+
+	if (var->xres < MIN_XRES)
+		var->xres = MIN_XRES;
+	if (var->yres < MIN_YRES)
+		var->yres = MIN_YRES;
+
+	var->xres_virtual = var->xres;
+
+	var->yres_virtual = var->yres;
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		/* always expect RGB 565 */
+		rgb = def_rgb565;
+		break;
+	case 32:
+		switch (host->ld_intf_width) {
+		case STMLCDIF_8BIT:
+			pr_debug("Unsupported LCD bus width mapping\n");
+			break;
+		case STMLCDIF_16BIT:
+		case STMLCDIF_18BIT:
+			/* 24 bit to 18 bit mapping */
+			rgb = def_rgb666;
+			break;
+		case STMLCDIF_24BIT:
+			/* real 24 bit */
+			rgb = def_rgb888;
+			break;
+		}
+		break;
+	default:
+		pr_debug("Unsupported colour depth: %u\n", var->bits_per_pixel);
+		return -EINVAL;
+	}
+
+	/*
+	 * Copy the RGB parameters for this display
+	 * from the machine specific parameters.
+	 */
+	var->red    = rgb[RED];
+	var->green  = rgb[GREEN];
+	var->blue   = rgb[BLUE];
+	var->transp = rgb[TRANSP];
+
+	return 0;
+}
+
+static void mxsfb_enable_controller(struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	u32 reg;
+
+	dev_dbg(&host->pdev->dev, "%s\n", __func__);
+
+	clk_enable(host->clk);
+	clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
+
+	/* if it was disabled, re-enable the mode again */
+	writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_SET);
+
+	/* enable the SYNC signals first, then the DMA engine */
+	reg = readl(host->base + LCDC_VDCTRL4);
+	reg |= VDCTRL4_SYNC_SIGNALS_ON;
+	writel(reg, host->base + LCDC_VDCTRL4);
+
+	writel(CTRL_RUN, host->base + LCDC_CTRL + REG_SET);
+
+	host->enabled = 1;
+}
+
+static void mxsfb_disable_controller(struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	unsigned loop;
+	u32 reg;
+
+	dev_dbg(&host->pdev->dev, "%s\n", __func__);
+
+	/*
+	 * Even if we disable the controller here, it will still continue
+	 * until its FIFOs are running out of data
+	 */
+	writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_CLR);
+
+	loop = 1000;
+	while (loop) {
+		reg = readl(host->base + LCDC_CTRL);
+		if (!(reg & CTRL_RUN))
+			break;
+		loop--;
+	}
+
+	writel(VDCTRL4_SYNC_SIGNALS_ON, host->base + LCDC_VDCTRL4 + REG_CLR);
+
+	clk_disable(host->clk);
+
+	host->enabled = 0;
+}
+
+static int mxsfb_set_par(struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	u32 ctrl, vdctrl0, vdctrl4;
+	int line_size, fb_size;
+	int reenable = 0;
+
+	line_size =  fb_info->var.xres * (fb_info->var.bits_per_pixel >> 3);
+	fb_size = fb_info->var.yres_virtual * line_size;
+
+	if (fb_size > fb_info->fix.smem_len)
+		return -ENOMEM;
+
+	fb_info->fix.line_length = line_size;
+
+	/*
+	 * It seems, you can't re-program the controller if it is still running.
+	 * This may lead into shifted pictures (FIFO issue?).
+	 * So, first stop the controller and drain its FIFOs
+	 */
+	if (host->enabled) {
+		reenable = 1;
+		mxsfb_disable_controller(fb_info);
+	}
+
+	/* clear the FIFOs */
+	writel(CTRL1_FIFO_CLEAR, host->base + LCDC_CTRL1 + REG_SET);
+
+	ctrl = CTRL_BYPASS_COUNT | CTRL_MASTER |
+		CTRL_SET_BUS_WIDTH(host->ld_intf_width);
+
+	switch (fb_info->var.bits_per_pixel) {
+	case 16:
+		dev_dbg(&host->pdev->dev, "Setting up RGB565 mode\n");
+		ctrl |= CTRL_SET_WORD_LENGTH(0);
+		writel(CTRL1_SET_BYTE_PACKAGING(0xf), host->base + LCDC_CTRL1);
+		break;
+	case 32:
+		dev_dbg(&host->pdev->dev, "Setting up RGB888/666 mode\n");
+		ctrl |= CTRL_SET_WORD_LENGTH(3);
+		switch (host->ld_intf_width) {
+		case STMLCDIF_8BIT:
+			dev_dbg(&host->pdev->dev,
+					"Unsupported LCD bus width mapping\n");
+			return -EINVAL;
+		case STMLCDIF_16BIT:
+		case STMLCDIF_18BIT:
+			/* 24 bit to 18 bit mapping */
+			ctrl |= CTRL_DF24; /* ignore the upper 2 bits in
+					    *  each colour component
+					    */
+			break;
+		case STMLCDIF_24BIT:
+			/* real 24 bit */
+			break;
+		}
+		/* do not use packed pixels = one pixel per word instead */
+		writel(CTRL1_SET_BYTE_PACKAGING(0x7), host->base + LCDC_CTRL1);
+		break;
+	default:
+		dev_dbg(&host->pdev->dev, "Unhandled color depth of %u\n",
+				fb_info->var.bits_per_pixel);
+		return -EINVAL;
+	}
+
+	writel(ctrl, host->base + LCDC_CTRL);
+
+	writel(TRANSFER_COUNT_SET_VCOUNT(fb_info->var.yres) |
+			TRANSFER_COUNT_SET_HCOUNT(fb_info->var.xres),
+			host->base + host->devdata->transfer_count);
+
+	vdctrl0 = VDCTRL0_ENABLE_PRESENT |	/* always in DOTCLOCK mode */
+		VDCTRL0_VSYNC_PERIOD_UNIT |
+		VDCTRL0_VSYNC_PULSE_WIDTH_UNIT |
+		VDCTRL0_SET_VSYNC_PULSE_WIDTH(fb_info->var.vsync_len);
+	if (fb_info->var.sync & FB_SYNC_HOR_HIGH_ACT)
+		vdctrl0 |= VDCTRL0_HSYNC_ACT_HIGH;
+	if (fb_info->var.sync & FB_SYNC_VERT_HIGH_ACT)
+		vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
+	if (fb_info->var.sync & FB_SYNC_DATA_ENABLE_HIGH_ACT)
+		vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
+	if (fb_info->var.sync & FB_SYNC_DOTCLK_FAILING_ACT)
+		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FAILING;
+
+	writel(vdctrl0, host->base + LCDC_VDCTRL0);
+
+	/* frame length in lines */
+	writel(fb_info->var.upper_margin + fb_info->var.vsync_len +
+		fb_info->var.lower_margin + fb_info->var.yres,
+		host->base + LCDC_VDCTRL1);
+
+	/* line length in units of clocks or pixels */
+	writel(set_hsync_pulse_width(host, fb_info->var.hsync_len) |
+		VDCTRL2_SET_HSYNC_PERIOD(fb_info->var.left_margin +
+		fb_info->var.hsync_len + fb_info->var.right_margin +
+		fb_info->var.xres),
+		host->base + LCDC_VDCTRL2);
+
+	writel(SET_HOR_WAIT_CNT(fb_info->var.left_margin +
+		fb_info->var.hsync_len) |
+		SET_VERT_WAIT_CNT(fb_info->var.upper_margin +
+			fb_info->var.vsync_len),
+		host->base + LCDC_VDCTRL3);
+
+	vdctrl4 = SET_DOTCLK_H_VALID_DATA_CNT(fb_info->var.xres);
+	if (mxsfb_is_v4(host))
+		vdctrl4 |= VDCTRL4_SET_DOTCLK_DLY(host->dotclk_delay);
+	writel(vdctrl4, host->base + LCDC_VDCTRL4);
+
+	writel(fb_info->fix.smem_start +
+			fb_info->fix.line_length * fb_info->var.yoffset,
+			host->base + host->devdata->next_buf);
+
+	if (reenable)
+		mxsfb_enable_controller(fb_info);
+
+	return 0;
+}
+
+static int mxsfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+		u_int transp, struct fb_info *fb_info)
+{
+	unsigned int val;
+	int ret = -EINVAL;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (fb_info->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+					7471 * blue) >> 16;
+
+	switch (fb_info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 12 or 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = fb_info->pseudo_palette;
+
+			val  = chan_to_field(red, &fb_info->var.red);
+			val |= chan_to_field(green, &fb_info->var.green);
+			val |= chan_to_field(blue, &fb_info->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+static int mxsfb_blank(int blank, struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		if (host->enabled)
+			mxsfb_disable_controller(fb_info);
+		break;
+
+	case FB_BLANK_UNBLANK:
+		if (!host->enabled)
+			mxsfb_enable_controller(fb_info);
+		break;
+	}
+	return 0;
+}
+
+static int mxsfb_pan_display(struct fb_var_screeninfo *var,
+		struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	unsigned offset;
+
+	if (var->xoffset != 0)
+		return -EINVAL;
+
+	offset = fb_info->fix.line_length * var->yoffset;
+
+	/* update on next VSYNC */
+	writel(fb_info->fix.smem_start + offset,
+			host->base + host->devdata->next_buf);
+
+	return 0;
+}
+
+static struct fb_ops mxsfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = mxsfb_check_var,
+	.fb_set_par = mxsfb_set_par,
+	.fb_setcolreg = mxsfb_setcolreg,
+	.fb_blank = mxsfb_blank,
+	.fb_pan_display = mxsfb_pan_display,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+static int __devinit mxsfb_restore_mode(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+	unsigned line_count;
+	unsigned period;
+	unsigned long pa, fbsize;
+	int bits_per_pixel, ofs;
+	u32 transfer_count, vdctrl0, vdctrl2, vdctrl3, vdctrl4, ctrl;
+	struct fb_videomode vmode;
+
+	/* Only restore the mode when the controller is running */
+	ctrl = readl(host->base + LCDC_CTRL);
+	if (!(ctrl & CTRL_RUN))
+		return -EINVAL;
+
+	vdctrl0 = readl(host->base + LCDC_VDCTRL0);
+	vdctrl2 = readl(host->base + LCDC_VDCTRL2);
+	vdctrl3 = readl(host->base + LCDC_VDCTRL3);
+	vdctrl4 = readl(host->base + LCDC_VDCTRL4);
+
+	transfer_count = readl(host->base + host->devdata->transfer_count);
+
+	vmode.xres = TRANSFER_COUNT_GET_HCOUNT(transfer_count);
+	vmode.yres = TRANSFER_COUNT_GET_VCOUNT(transfer_count);
+
+	switch (CTRL_GET_WORD_LENGTH(ctrl)) {
+	case 0:
+		bits_per_pixel = 16;
+		break;
+	case 3:
+		bits_per_pixel = 32;
+	case 1:
+	default:
+		return -EINVAL;
+	}
+
+	fb_info->var.bits_per_pixel = bits_per_pixel;
+
+	vmode.pixclock = KHZ2PICOS(clk_get_rate(host->clk) / 1000U);
+	vmode.hsync_len = get_hsync_pulse_width(host, vdctrl2);
+	vmode.left_margin = GET_HOR_WAIT_CNT(vdctrl3) - vmode.hsync_len;
+	vmode.right_margin = VDCTRL2_GET_HSYNC_PERIOD(vdctrl2) - vmode.hsync_len -
+		vmode.left_margin - vmode.xres;
+	vmode.vsync_len = VDCTRL0_GET_VSYNC_PULSE_WIDTH(vdctrl0);
+	period = readl(host->base + LCDC_VDCTRL1);
+	vmode.upper_margin = GET_VERT_WAIT_CNT(vdctrl3) - vmode.vsync_len;
+	vmode.lower_margin = period - vmode.vsync_len - vmode.upper_margin - vmode.yres;
+
+	vmode.vmode = FB_VMODE_NONINTERLACED;
+
+	vmode.sync = 0;
+	if (vdctrl0 & VDCTRL0_HSYNC_ACT_HIGH)
+		vmode.sync |= FB_SYNC_HOR_HIGH_ACT;
+	if (vdctrl0 & VDCTRL0_VSYNC_ACT_HIGH)
+		vmode.sync |= FB_SYNC_VERT_HIGH_ACT;
+
+	pr_debug("Reconstructed video mode:\n");
+	pr_debug("%dx%d, hsync: %u left: %u, right: %u, vsync: %u, upper: %u, lower: %u\n",
+			vmode.xres, vmode.yres,
+			vmode.hsync_len, vmode.left_margin, vmode.right_margin,
+			vmode.vsync_len, vmode.upper_margin, vmode.lower_margin);
+	pr_debug("pixclk: %ldkHz\n", PICOS2KHZ(vmode.pixclock));
+
+	fb_add_videomode(&vmode, &fb_info->modelist);
+
+	host->ld_intf_width = CTRL_GET_BUS_WIDTH(ctrl);
+	host->dotclk_delay = VDCTRL4_GET_DOTCLK_DLY(vdctrl4);
+
+	fb_info->fix.line_length = vmode.xres * (bits_per_pixel >> 3);
+
+	pa = readl(host->base + host->devdata->cur_buf);
+	fbsize = fb_info->fix.line_length * vmode.yres;
+	if (pa < fb_info->fix.smem_start)
+		return -EINVAL;
+	if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len)
+		return -EINVAL;
+	ofs = pa - fb_info->fix.smem_start;
+	if (ofs) {
+		memmove(fb_info->screen_base, fb_info->screen_base + ofs, fbsize);
+		writel(fb_info->fix.smem_start, host->base + host->devdata->next_buf);
+	}
+
+	line_count = fb_info->fix.smem_len / fb_info->fix.line_length;
+	fb_info->fix.ypanstep = 1;
+
+	clk_enable(host->clk);
+	host->enabled = 1;
+
+	return 0;
+}
+
+static int __devinit mxsfb_init_fbinfo(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+	struct fb_var_screeninfo *var = &fb_info->var;
+	struct mxsfb_platform_data *pdata = host->pdev->dev.platform_data;
+	dma_addr_t fb_phys;
+	void *fb_virt;
+	unsigned fb_size = pdata->fb_size;
+
+	fb_info->fbops = &mxsfb_ops;
+	fb_info->flags = FBINFO_FLAG_DEFAULT | FBINFO_READS_FAST;
+	strlcpy(fb_info->fix.id, "mxs", sizeof(fb_info->fix.id));
+	fb_info->fix.type = FB_TYPE_PACKED_PIXELS;
+	fb_info->fix.ypanstep = 1;
+	fb_info->fix.visual = FB_VISUAL_TRUECOLOR,
+	fb_info->fix.accel = FB_ACCEL_NONE;
+
+	var->bits_per_pixel = pdata->default_bpp ? pdata->default_bpp : 16;
+	var->nonstd = 0;
+	var->activate = FB_ACTIVATE_NOW;
+	var->accel_flags = 0;
+	var->vmode = FB_VMODE_NONINTERLACED;
+
+	host->dotclk_delay = pdata->dotclk_delay;
+	host->ld_intf_width = pdata->ld_intf_width;
+
+	/* Memory allocation for framebuffer */
+	if (pdata->fb_phys) {
+		if (!fb_size)
+			return -EINVAL;
+
+		fb_phys = pdata->fb_phys;
+
+		if (!request_mem_region(fb_phys, fb_size, host->pdev->name))
+			return -ENOMEM;
+
+		fb_virt = ioremap(fb_phys, fb_size);
+		if (!fb_virt) {
+			release_mem_region(fb_phys, fb_size);
+			return -ENOMEM;
+		}
+		host->mapped = 1;
+	} else {
+		if (!fb_size)
+			fb_size = SZ_2M; /* default */
+		fb_virt = alloc_pages_exact(fb_size, GFP_DMA);
+		if (!fb_virt)
+			return -ENOMEM;
+
+		fb_phys = virt_to_phys(fb_virt);
+	}
+
+	fb_info->fix.smem_start = fb_phys;
+	fb_info->screen_base = fb_virt;
+	fb_info->screen_size = fb_info->fix.smem_len = fb_size;
+
+	if (mxsfb_restore_mode(host))
+		memset(fb_virt, 0, fb_size);
+
+	return 0;
+}
+
+static void __devexit mxsfb_free_videomem(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+
+	if (host->mapped) {
+		iounmap(fb_info->screen_base);
+		release_mem_region(fb_info->fix.smem_start,
+				fb_info->screen_size);
+	} else {
+		free_pages_exact(fb_info->screen_base, fb_info->fix.smem_len);
+	}
+}
+
+static int __devinit mxsfb_probe(struct platform_device *pdev)
+{
+	struct mxsfb_platform_data *pdata = pdev->dev.platform_data;
+	struct resource *res;
+	struct mxsfb_info *host;
+	struct fb_info *fb_info;
+	struct fb_modelist *modelist;
+	int i, ret;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platformdata. Giving up\n");
+		return -ENODEV;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Cannot get memory IO resource\n");
+		return -ENODEV;
+	}
+
+	if (!request_mem_region(res->start, resource_size(res), pdev->name))
+		return -EBUSY;
+
+	fb_info = framebuffer_alloc(sizeof(struct mxsfb_info), &pdev->dev);
+	if (!fb_info) {
+		dev_err(&pdev->dev, "Failed to allocate fbdev\n");
+		ret = -ENOMEM;
+		goto error_alloc_info;
+	}
+
+	host = to_imxfb_host(fb_info);
+
+	host->base = ioremap(res->start, resource_size(res));
+	if (!host->base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		ret = -ENOMEM;
+		goto error_ioremap;
+	}
+
+	host->pdev = pdev;
+	platform_set_drvdata(pdev, host);
+
+	host->devdata = &mxsfb_devdata[pdev->id_entry->driver_data];
+
+	host->clk = clk_get(&host->pdev->dev, NULL);
+	if (IS_ERR(host->clk)) {
+		ret = PTR_ERR(host->clk);
+		goto error_getclock;
+	}
+
+	fb_info->pseudo_palette = kmalloc(sizeof(u32) * 16, GFP_KERNEL);
+	if (!fb_info->pseudo_palette) {
+		ret = -ENOMEM;
+		goto error_pseudo_pallette;
+	}
+
+	INIT_LIST_HEAD(&fb_info->modelist);
+
+	ret = mxsfb_init_fbinfo(host);
+	if (ret != 0)
+		goto error_init_fb;
+
+	for (i = 0; i < pdata->mode_count; i++)
+		fb_add_videomode(&pdata->mode_list[i], &fb_info->modelist);
+
+	modelist = list_first_entry(&fb_info->modelist,
+			struct fb_modelist, list);
+	fb_videomode_to_var(&fb_info->var, &modelist->mode);
+
+	/* init the color fields */
+	mxsfb_check_var(&fb_info->var, fb_info);
+
+	platform_set_drvdata(pdev, fb_info);
+
+	ret = register_framebuffer(fb_info);
+	if (ret != 0) {
+		dev_err(&pdev->dev,"Failed to register framebuffer\n");
+		goto error_register;
+	}
+
+	if (!host->enabled) {
+		writel(0, host->base + LCDC_CTRL);
+		mxsfb_set_par(fb_info);
+		mxsfb_enable_controller(fb_info);
+	}
+
+	dev_info(&pdev->dev, "initialized\n");
+
+	return 0;
+
+error_register:
+	if (host->enabled)
+		clk_disable(host->clk);
+	fb_destroy_modelist(&fb_info->modelist);
+error_init_fb:
+	kfree(fb_info->pseudo_palette);
+error_pseudo_pallette:
+	clk_put(host->clk);
+error_getclock:
+	iounmap(host->base);
+error_ioremap:
+	framebuffer_release(fb_info);
+error_alloc_info:
+	release_mem_region(res->start, resource_size(res));
+
+	return ret;
+}
+
+static int __devexit mxsfb_remove(struct platform_device *pdev)
+{
+	struct fb_info *fb_info = platform_get_drvdata(pdev);
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (host->enabled)
+		mxsfb_disable_controller(fb_info);
+
+	unregister_framebuffer(fb_info);
+	kfree(fb_info->pseudo_palette);
+	mxsfb_free_videomem(host);
+	iounmap(host->base);
+	clk_put(host->clk);
+
+	framebuffer_release(fb_info);
+	release_mem_region(res->start, resource_size(res));
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_device_id mxsfb_devtype[] = {
+	{
+		.name = "imx23-fb",
+		.driver_data = MXSFB_V3,
+	}, {
+		.name = "imx28-fb",
+		.driver_data = MXSFB_V4,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, mxsfb_devtype);
+
+static struct platform_driver mxsfb_driver = {
+	.probe = mxsfb_probe,
+	.remove = __devexit_p(mxsfb_remove),
+	.id_table = mxsfb_devtype,
+	.driver = {
+		   .name = DRIVER_NAME,
+	},
+};
+
+static int __init mxsfb_init(void)
+{
+	return platform_driver_register(&mxsfb_driver);
+}
+
+static void __exit mxsfb_exit(void)
+{
+	platform_driver_unregister(&mxsfb_driver);
+}
+
+module_init(mxsfb_init);
+module_exit(mxsfb_exit);
+
+MODULE_DESCRIPTION("Freescale mxs framebuffer driver");
+MODULE_AUTHOR("Sascha Hauer, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/nuc900fb.c b/drivers/video/nuc900fb.c
index f838d9e277f0..0fff59782e45 100644
--- a/drivers/video/nuc900fb.c
+++ b/drivers/video/nuc900fb.c
@@ -551,7 +551,7 @@ static int __devinit nuc900fb_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	size = (res->end - res->start) + 1;
+	size = resource_size(res);
 	fbi->mem = request_mem_region(res->start, size, pdev->name);
 	if (fbi->mem == NULL) {
 		dev_err(&pdev->dev, "failed to alloc memory region\n");
diff --git a/drivers/video/nuc900fb.h b/drivers/video/nuc900fb.h
index 6c23aa3d3b89..bc7c9300f276 100644
--- a/drivers/video/nuc900fb.h
+++ b/drivers/video/nuc900fb.h
@@ -8,7 +8,7 @@
  * the Free Software Foundation; either version 2 of the License, or
  * (at your option) any later version.
  *
- *   Auther:
+ *   Author:
  *        Wang Qiang(rurality.linux@gmail.com)  2009/12/16
  */
 
diff --git a/drivers/video/nvidia/nv_backlight.c b/drivers/video/nvidia/nv_backlight.c
index 6aac6d1b937b..8471008aa6ff 100644
--- a/drivers/video/nvidia/nv_backlight.c
+++ b/drivers/video/nvidia/nv_backlight.c
@@ -111,6 +111,7 @@ void nvidia_bl_init(struct nvidia_par *par)
 	snprintf(name, sizeof(name), "nvidiabl%d", info->node);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
 	bd = backlight_device_register(name, info->dev, par, &nvidia_bl_ops,
 				       &props);
diff --git a/drivers/video/omap/Kconfig b/drivers/video/omap/Kconfig
index 083c8fe53e24..196fa2e7f438 100644
--- a/drivers/video/omap/Kconfig
+++ b/drivers/video/omap/Kconfig
@@ -5,13 +5,18 @@ config FB_OMAP
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
+	select TWL4030_CORE if MACH_OMAP_2430SDP
 	help
           Frame buffer driver for OMAP based boards.
 
 config FB_OMAP_LCD_VGA
         bool "Use LCD in VGA mode"
 	        depends on MACH_OMAP_3430SDP || MACH_OMAP_LDP
-
+		help
+		  Set LCD resolution as VGA (640 X 480).
+		  Default resolution without this option is QVGA(320 X 240).
+		  Please take a look at drivers/video/omap/lcd_ldp.c file
+		  for lcd driver code.
 choice
 	depends on FB_OMAP && MACH_OVERO
 	prompt "Screen resolution"
@@ -59,7 +64,7 @@ config FB_OMAP_MANUAL_UPDATE
 	depends on FB_OMAP && FB_OMAP_LCDC_EXTERNAL
 	help
 	  Say Y here, if your user-space applications are capable of
-	  notifying the frame buffer driver when a change has occured in
+	  notifying the frame buffer driver when a change has occurred in
 	  the frame buffer content and thus a reload of the image data to
 	  the external frame buffer is required. If unsure, say N.
 
diff --git a/drivers/video/omap/Makefile b/drivers/video/omap/Makefile
index 49226a1b909e..25db55696e14 100644
--- a/drivers/video/omap/Makefile
+++ b/drivers/video/omap/Makefile
@@ -30,7 +30,6 @@ objs-y$(CONFIG_MACH_OMAP_APOLLON) += lcd_apollon.o
 objs-y$(CONFIG_MACH_OMAP_2430SDP) += lcd_2430sdp.o
 objs-y$(CONFIG_MACH_OMAP_3430SDP) += lcd_2430sdp.o
 objs-y$(CONFIG_MACH_OMAP_LDP) += lcd_ldp.o
-objs-y$(CONFIG_MACH_OMAP2EVM) += lcd_omap2evm.o
 objs-y$(CONFIG_MACH_OMAP3EVM) += lcd_omap3evm.o
 objs-y$(CONFIG_MACH_OMAP3_BEAGLE) += lcd_omap3beagle.o
 objs-y$(CONFIG_FB_OMAP_LCD_MIPID) += lcd_mipid.o
diff --git a/drivers/video/omap/blizzard.c b/drivers/video/omap/blizzard.c
index 87785c215a52..c0504a8a5079 100644
--- a/drivers/video/omap/blizzard.c
+++ b/drivers/video/omap/blizzard.c
@@ -397,8 +397,7 @@ static inline void free_req(struct blizzard_request *req)
 
 	spin_lock_irqsave(&blizzard.req_lock, flags);
 
-	list_del(&req->entry);
-	list_add(&req->entry, &blizzard.free_req_list);
+	list_move(&req->entry, &blizzard.free_req_list);
 	if (!(req->flags & REQ_FROM_IRQ_POOL))
 		up(&blizzard.req_sema);
 
diff --git a/drivers/video/omap/dispc.c b/drivers/video/omap/dispc.c
index 529483467abf..0ccd7adf47bb 100644
--- a/drivers/video/omap/dispc.c
+++ b/drivers/video/omap/dispc.c
@@ -922,14 +922,14 @@ static int get_dss_clocks(void)
 		return PTR_ERR(dispc.dss_ick);
 	}
 
-	dispc.dss1_fck = clk_get(&dispc.fbdev->dssdev->dev, "dss1_fck");
+	dispc.dss1_fck = clk_get(&dispc.fbdev->dssdev->dev, "fck");
 	if (IS_ERR(dispc.dss1_fck)) {
 		dev_err(dispc.fbdev->dev, "can't get dss1_fck\n");
 		clk_put(dispc.dss_ick);
 		return PTR_ERR(dispc.dss1_fck);
 	}
 
-	dispc.dss_54m_fck = clk_get(&dispc.fbdev->dssdev->dev, "tv_fck");
+	dispc.dss_54m_fck = clk_get(&dispc.fbdev->dssdev->dev, "tv_clk");
 	if (IS_ERR(dispc.dss_54m_fck)) {
 		dev_err(dispc.fbdev->dev, "can't get tv_fck\n");
 		clk_put(dispc.dss_ick);
diff --git a/drivers/video/omap/hwa742.c b/drivers/video/omap/hwa742.c
index 0016f77cd13f..084aa0ac562b 100644
--- a/drivers/video/omap/hwa742.c
+++ b/drivers/video/omap/hwa742.c
@@ -269,8 +269,7 @@ static inline void free_req(struct hwa742_request *req)
 
 	spin_lock_irqsave(&hwa742.req_lock, flags);
 
-	list_del(&req->entry);
-	list_add(&req->entry, &hwa742.free_req_list);
+	list_move(&req->entry, &hwa742.free_req_list);
 	if (!(req->flags & REQ_FROM_IRQ_POOL))
 		up(&hwa742.req_sema);
 
diff --git a/drivers/video/omap/lcd_omap2evm.c b/drivers/video/omap/lcd_omap2evm.c
deleted file mode 100644
index 7e7a65c08452..000000000000
--- a/drivers/video/omap/lcd_omap2evm.c
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * LCD panel support for the MISTRAL OMAP2EVM board
- *
- * Author: Arun C <arunedarath@mistralsolutions.com>
- *
- * Derived from drivers/video/omap/lcd_omap3evm.c
- * Derived from drivers/video/omap/lcd-apollon.c
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation; either version 2 of the License, or (at your
- * option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/gpio.h>
-#include <linux/i2c/twl.h>
-
-#include <plat/mux.h>
-#include <asm/mach-types.h>
-
-#include "omapfb.h"
-
-#define LCD_PANEL_ENABLE_GPIO	154
-#define LCD_PANEL_LR		128
-#define LCD_PANEL_UD		129
-#define LCD_PANEL_INI		152
-#define LCD_PANEL_QVGA		148
-#define LCD_PANEL_RESB		153
-
-#define TWL_LED_LEDEN		0x00
-#define TWL_PWMA_PWMAON		0x00
-#define TWL_PWMA_PWMAOFF	0x01
-
-static unsigned int bklight_level;
-
-static int omap2evm_panel_init(struct lcd_panel *panel,
-				struct omapfb_device *fbdev)
-{
-	gpio_request(LCD_PANEL_ENABLE_GPIO, "LCD enable");
-	gpio_request(LCD_PANEL_LR, "LCD lr");
-	gpio_request(LCD_PANEL_UD, "LCD ud");
-	gpio_request(LCD_PANEL_INI, "LCD ini");
-	gpio_request(LCD_PANEL_QVGA, "LCD qvga");
-	gpio_request(LCD_PANEL_RESB, "LCD resb");
-
-	gpio_direction_output(LCD_PANEL_ENABLE_GPIO, 1);
-	gpio_direction_output(LCD_PANEL_RESB, 1);
-	gpio_direction_output(LCD_PANEL_INI, 1);
-	gpio_direction_output(LCD_PANEL_QVGA, 0);
-	gpio_direction_output(LCD_PANEL_LR, 1);
-	gpio_direction_output(LCD_PANEL_UD, 1);
-
-	twl_i2c_write_u8(TWL4030_MODULE_LED, 0x11, TWL_LED_LEDEN);
-	twl_i2c_write_u8(TWL4030_MODULE_PWMA, 0x01, TWL_PWMA_PWMAON);
-	twl_i2c_write_u8(TWL4030_MODULE_PWMA, 0x02, TWL_PWMA_PWMAOFF);
-	bklight_level = 100;
-
-	return 0;
-}
-
-static void omap2evm_panel_cleanup(struct lcd_panel *panel)
-{
-	gpio_free(LCD_PANEL_RESB);
-	gpio_free(LCD_PANEL_QVGA);
-	gpio_free(LCD_PANEL_INI);
-	gpio_free(LCD_PANEL_UD);
-	gpio_free(LCD_PANEL_LR);
-	gpio_free(LCD_PANEL_ENABLE_GPIO);
-}
-
-static int omap2evm_panel_enable(struct lcd_panel *panel)
-{
-	gpio_set_value(LCD_PANEL_ENABLE_GPIO, 0);
-	return 0;
-}
-
-static void omap2evm_panel_disable(struct lcd_panel *panel)
-{
-	gpio_set_value(LCD_PANEL_ENABLE_GPIO, 1);
-}
-
-static unsigned long omap2evm_panel_get_caps(struct lcd_panel *panel)
-{
-	return 0;
-}
-
-static int omap2evm_bklight_setlevel(struct lcd_panel *panel,
-						unsigned int level)
-{
-	u8 c;
-	if ((level >= 0) && (level <= 100)) {
-		c = (125 * (100 - level)) / 100 + 2;
-		twl_i2c_write_u8(TWL4030_MODULE_PWMA, c, TWL_PWMA_PWMAOFF);
-		bklight_level = level;
-	}
-	return 0;
-}
-
-static unsigned int omap2evm_bklight_getlevel(struct lcd_panel *panel)
-{
-	return bklight_level;
-}
-
-static unsigned int omap2evm_bklight_getmaxlevel(struct lcd_panel *panel)
-{
-	return 100;
-}
-
-struct lcd_panel omap2evm_panel = {
-	.name		= "omap2evm",
-	.config		= OMAP_LCDC_PANEL_TFT | OMAP_LCDC_INV_VSYNC |
-			  OMAP_LCDC_INV_HSYNC,
-
-	.bpp		= 16,
-	.data_lines	= 18,
-	.x_res		= 480,
-	.y_res		= 640,
-	.hsw		= 3,
-	.hfp		= 0,
-	.hbp		= 28,
-	.vsw		= 2,
-	.vfp		= 1,
-	.vbp		= 0,
-
-	.pixel_clock	= 20000,
-
-	.init		= omap2evm_panel_init,
-	.cleanup	= omap2evm_panel_cleanup,
-	.enable		= omap2evm_panel_enable,
-	.disable	= omap2evm_panel_disable,
-	.get_caps	= omap2evm_panel_get_caps,
-	.set_bklight_level      = omap2evm_bklight_setlevel,
-	.get_bklight_level      = omap2evm_bklight_getlevel,
-	.get_bklight_max        = omap2evm_bklight_getmaxlevel,
-};
-
-static int omap2evm_panel_probe(struct platform_device *pdev)
-{
-	omapfb_register_panel(&omap2evm_panel);
-	return 0;
-}
-
-static int omap2evm_panel_remove(struct platform_device *pdev)
-{
-	return 0;
-}
-
-static int omap2evm_panel_suspend(struct platform_device *pdev,
-				   pm_message_t mesg)
-{
-	return 0;
-}
-
-static int omap2evm_panel_resume(struct platform_device *pdev)
-{
-	return 0;
-}
-
-struct platform_driver omap2evm_panel_driver = {
-	.probe		= omap2evm_panel_probe,
-	.remove		= omap2evm_panel_remove,
-	.suspend	= omap2evm_panel_suspend,
-	.resume		= omap2evm_panel_resume,
-	.driver		= {
-		.name	= "omap2evm_lcd",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init omap2evm_panel_drv_init(void)
-{
-	return platform_driver_register(&omap2evm_panel_driver);
-}
-
-static void __exit omap2evm_panel_drv_exit(void)
-{
-	platform_driver_unregister(&omap2evm_panel_driver);
-}
-
-module_init(omap2evm_panel_drv_init);
-module_exit(omap2evm_panel_drv_exit);
diff --git a/drivers/video/omap/omapfb_main.c b/drivers/video/omap/omapfb_main.c
index e264efd0278f..b3ddd743d8a6 100644
--- a/drivers/video/omap/omapfb_main.c
+++ b/drivers/video/omap/omapfb_main.c
@@ -90,7 +90,7 @@ static void omapdss_release(struct device *dev)
 
 /* dummy device for clocks */
 static struct platform_device omapdss_device = {
-	.name		= "omapdss",
+	.name		= "omapdss_dss",
 	.id		= -1,
 	.dev            = {
 		.release = omapdss_release,
diff --git a/drivers/video/omap/rfbi.c b/drivers/video/omap/rfbi.c
index eada9f12efc7..0c6981f1a4a3 100644
--- a/drivers/video/omap/rfbi.c
+++ b/drivers/video/omap/rfbi.c
@@ -90,7 +90,7 @@ static int rfbi_get_clocks(void)
 		return PTR_ERR(rfbi.dss_ick);
 	}
 
-	rfbi.dss1_fck = clk_get(&rfbi.fbdev->dssdev->dev, "dss1_fck");
+	rfbi.dss1_fck = clk_get(&rfbi.fbdev->dssdev->dev, "fck");
 	if (IS_ERR(rfbi.dss1_fck)) {
 		dev_err(rfbi.fbdev->dev, "can't get dss1_fck\n");
 		clk_put(rfbi.dss_ick);
diff --git a/drivers/video/omap2/Makefile b/drivers/video/omap2/Makefile
index d853d05dad31..5ddef129f798 100644
--- a/drivers/video/omap2/Makefile
+++ b/drivers/video/omap2/Makefile
@@ -1,6 +1,6 @@
 obj-$(CONFIG_OMAP2_VRAM) += vram.o
 obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
 
-obj-y += dss/
-obj-y += omapfb/
+obj-$(CONFIG_OMAP2_DSS) += dss/
+obj-$(CONFIG_FB_OMAP2) += omapfb/
 obj-y += displays/
diff --git a/drivers/video/omap2/displays/Kconfig b/drivers/video/omap2/displays/Kconfig
index 940cab394c2e..609a28073178 100644
--- a/drivers/video/omap2/displays/Kconfig
+++ b/drivers/video/omap2/displays/Kconfig
@@ -3,22 +3,29 @@ menu "OMAP2/3 Display Device Drivers"
 
 config PANEL_GENERIC_DPI
         tristate "Generic DPI Panel"
+	depends on OMAP2_DSS_DPI
         help
 	  Generic DPI panel driver.
 	  Supports DVI output for Beagle and OMAP3 SDP.
 	  Supports LCD Panel used in TI SDP3430 and EVM boards,
 	  OMAP3517 EVM boards and CM-T35.
 
+config PANEL_LGPHILIPS_LB035Q02
+	tristate "LG.Philips LB035Q02 LCD Panel"
+	depends on OMAP2_DSS_DPI && SPI
+	help
+	  LCD Panel used on the Gumstix Overo Palo35
+
 config PANEL_SHARP_LS037V7DW01
         tristate "Sharp LS037V7DW01 LCD Panel"
-        depends on OMAP2_DSS
+        depends on OMAP2_DSS_DPI
         select BACKLIGHT_CLASS_DEVICE
         help
           LCD Panel used in TI's SDP3430 and EVM boards
 
 config PANEL_NEC_NL8048HL11_01B
 	tristate "NEC NL8048HL11-01B Panel"
-	depends on OMAP2_DSS
+	depends on OMAP2_DSS_DPI
 	help
 		This NEC NL8048HL11-01B panel is TFT LCD
 		used in the Zoom2/3/3630 sdp boards.
@@ -31,7 +38,7 @@ config PANEL_TAAL
 
 config PANEL_TPO_TD043MTEA1
         tristate "TPO TD043MTEA1 LCD Panel"
-        depends on OMAP2_DSS && SPI
+        depends on OMAP2_DSS_DPI && SPI
         help
           LCD Panel used in OMAP3 Pandora
 
diff --git a/drivers/video/omap2/displays/Makefile b/drivers/video/omap2/displays/Makefile
index 861f0255ec6b..0f601ab3abf4 100644
--- a/drivers/video/omap2/displays/Makefile
+++ b/drivers/video/omap2/displays/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_PANEL_GENERIC_DPI) += panel-generic-dpi.o
+obj-$(CONFIG_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
 obj-$(CONFIG_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
 obj-$(CONFIG_PANEL_NEC_NL8048HL11_01B) += panel-nec-nl8048hl11-01b.o
 
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index e77310653207..dbd59b8e5b36 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -30,7 +30,7 @@
 #include <linux/backlight.h>
 #include <linux/fb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #define MIPID_CMD_READ_DISP_ID		0x04
 #define MIPID_CMD_READ_RED		0x06
@@ -534,6 +534,7 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 
 	props.fb_blank = FB_BLANK_UNBLANK;
 	props.power = FB_BLANK_UNBLANK;
+	props.type = BACKLIGHT_RAW;
 
 	bldev = backlight_device_register("acx565akm", &md->spi->dev,
 			md, &acx565akm_bl_ops, &props);
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index 07eb30ee59c8..9c90f75653fb 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -33,8 +33,9 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
+#include <video/omapdss.h>
 
-#include <plat/panel-generic-dpi.h>
+#include <video/omap-panel-generic-dpi.h>
 
 struct panel_config {
 	struct omap_video_timings timings;
@@ -156,6 +157,81 @@ static struct panel_config generic_dpi_panels[] = {
 		.power_off_delay	= 0,
 		.name			= "toppoly_tdo35s",
 	},
+
+	/* Samsung LTE430WQ-F0C */
+	{
+		{
+			.x_res		= 480,
+			.y_res		= 272,
+
+			.pixel_clock	= 9200,
+
+			.hfp		= 8,
+			.hsw		= 41,
+			.hbp		= 45 - 41,
+
+			.vfp		= 4,
+			.vsw		= 10,
+			.vbp		= 12 - 10,
+		},
+		.acbi			= 0x0,
+		.acb			= 0x0,
+		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+						OMAP_DSS_LCD_IHS,
+		.power_on_delay		= 0,
+		.power_off_delay	= 0,
+		.name			= "samsung_lte430wq_f0c",
+	},
+
+	/* Seiko 70WVW1TZ3Z3 */
+	{
+		{
+			.x_res		= 800,
+			.y_res		= 480,
+
+			.pixel_clock	= 33000,
+
+			.hsw		= 128,
+			.hfp		= 10,
+			.hbp		= 10,
+
+			.vsw		= 2,
+			.vfp		= 4,
+			.vbp		= 11,
+		},
+		.acbi			= 0x0,
+		.acb			= 0x0,
+		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+						OMAP_DSS_LCD_IHS,
+		.power_on_delay		= 0,
+		.power_off_delay	= 0,
+		.name			= "seiko_70wvw1tz3",
+	},
+
+	/* Powertip PH480272T */
+	{
+		{
+			.x_res		= 480,
+			.y_res		= 272,
+
+			.pixel_clock	= 9000,
+
+			.hsw		= 40,
+			.hfp		= 2,
+			.hbp		= 2,
+
+			.vsw		= 10,
+			.vfp		= 2,
+			.vbp		= 2,
+		},
+		.acbi			= 0x0,
+		.acb			= 0x0,
+		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO,
+		.power_on_delay		= 0,
+		.power_off_delay	= 0,
+		.name			= "powertip_ph480272t",
+	},
 };
 
 struct panel_drv_data {
@@ -260,7 +336,7 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void generic_dpi_panel_remove(struct omap_dss_device *dssdev)
+static void __exit generic_dpi_panel_remove(struct omap_dss_device *dssdev)
 {
 	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
 
@@ -333,7 +409,7 @@ static int generic_dpi_panel_check_timings(struct omap_dss_device *dssdev,
 
 static struct omap_dss_driver dpi_driver = {
 	.probe		= generic_dpi_panel_probe,
-	.remove		= generic_dpi_panel_remove,
+	.remove		= __exit_p(generic_dpi_panel_remove),
 
 	.enable		= generic_dpi_panel_enable,
 	.disable	= generic_dpi_panel_disable,
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
new file mode 100644
index 000000000000..e0eb35be303e
--- /dev/null
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -0,0 +1,279 @@
+/*
+ * LCD panel driver for LG.Philips LB035Q02
+ *
+ * Author: Steve Sakoman <steve@sakoman.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/mutex.h>
+
+#include <video/omapdss.h>
+
+struct lb035q02_data {
+	struct mutex lock;
+};
+
+static struct omap_video_timings lb035q02_timings = {
+	.x_res = 320,
+	.y_res = 240,
+
+	.pixel_clock	= 6500,
+
+	.hsw		= 2,
+	.hfp		= 20,
+	.hbp		= 68,
+
+	.vsw		= 2,
+	.vfp		= 4,
+	.vbp		= 18,
+};
+
+static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+		return 0;
+
+	r = omapdss_dpi_display_enable(dssdev);
+	if (r)
+		goto err0;
+
+	if (dssdev->platform_enable) {
+		r = dssdev->platform_enable(dssdev);
+		if (r)
+			goto err1;
+	}
+
+	return 0;
+err1:
+	omapdss_dpi_display_disable(dssdev);
+err0:
+	return r;
+}
+
+static void lb035q02_panel_power_off(struct omap_dss_device *dssdev)
+{
+	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+		return;
+
+	if (dssdev->platform_disable)
+		dssdev->platform_disable(dssdev);
+
+	omapdss_dpi_display_disable(dssdev);
+}
+
+static int lb035q02_panel_probe(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld;
+	int r;
+
+	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+		OMAP_DSS_LCD_IHS;
+	dssdev->panel.timings = lb035q02_timings;
+
+	ld = kzalloc(sizeof(*ld), GFP_KERNEL);
+	if (!ld) {
+		r = -ENOMEM;
+		goto err;
+	}
+	mutex_init(&ld->lock);
+	dev_set_drvdata(&dssdev->dev, ld);
+	return 0;
+err:
+	return r;
+}
+
+static void lb035q02_panel_remove(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+	kfree(ld);
+}
+
+static int lb035q02_panel_enable(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&ld->lock);
+
+	r = lb035q02_panel_power_on(dssdev);
+	if (r)
+		goto err;
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	mutex_unlock(&ld->lock);
+	return 0;
+err:
+	mutex_unlock(&ld->lock);
+	return r;
+}
+
+static void lb035q02_panel_disable(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&ld->lock);
+
+	lb035q02_panel_power_off(dssdev);
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+	mutex_unlock(&ld->lock);
+}
+
+static int lb035q02_panel_suspend(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&ld->lock);
+
+	lb035q02_panel_power_off(dssdev);
+	dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+	mutex_unlock(&ld->lock);
+	return 0;
+}
+
+static int lb035q02_panel_resume(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&ld->lock);
+
+	r = lb035q02_panel_power_on(dssdev);
+	if (r)
+		goto err;
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	mutex_unlock(&ld->lock);
+	return 0;
+err:
+	mutex_unlock(&ld->lock);
+	return r;
+}
+
+static struct omap_dss_driver lb035q02_driver = {
+	.probe		= lb035q02_panel_probe,
+	.remove		= lb035q02_panel_remove,
+
+	.enable		= lb035q02_panel_enable,
+	.disable	= lb035q02_panel_disable,
+	.suspend	= lb035q02_panel_suspend,
+	.resume		= lb035q02_panel_resume,
+
+	.driver         = {
+		.name   = "lgphilips_lb035q02_panel",
+		.owner  = THIS_MODULE,
+	},
+};
+
+static int lb035q02_write_reg(struct spi_device *spi, u8 reg, u16 val)
+{
+	struct spi_message msg;
+	struct spi_transfer index_xfer = {
+		.len		= 3,
+		.cs_change	= 1,
+	};
+	struct spi_transfer value_xfer = {
+		.len		= 3,
+	};
+	u8	buffer[16];
+
+	spi_message_init(&msg);
+
+	/* register index */
+	buffer[0] = 0x70;
+	buffer[1] = 0x00;
+	buffer[2] = reg & 0x7f;
+	index_xfer.tx_buf = buffer;
+	spi_message_add_tail(&index_xfer, &msg);
+
+	/* register value */
+	buffer[4] = 0x72;
+	buffer[5] = val >> 8;
+	buffer[6] = val;
+	value_xfer.tx_buf = buffer + 4;
+	spi_message_add_tail(&value_xfer, &msg);
+
+	return spi_sync(spi, &msg);
+}
+
+static void init_lb035q02_panel(struct spi_device *spi)
+{
+	/* Init sequence from page 28 of the lb035q02 spec */
+	lb035q02_write_reg(spi, 0x01, 0x6300);
+	lb035q02_write_reg(spi, 0x02, 0x0200);
+	lb035q02_write_reg(spi, 0x03, 0x0177);
+	lb035q02_write_reg(spi, 0x04, 0x04c7);
+	lb035q02_write_reg(spi, 0x05, 0xffc0);
+	lb035q02_write_reg(spi, 0x06, 0xe806);
+	lb035q02_write_reg(spi, 0x0a, 0x4008);
+	lb035q02_write_reg(spi, 0x0b, 0x0000);
+	lb035q02_write_reg(spi, 0x0d, 0x0030);
+	lb035q02_write_reg(spi, 0x0e, 0x2800);
+	lb035q02_write_reg(spi, 0x0f, 0x0000);
+	lb035q02_write_reg(spi, 0x16, 0x9f80);
+	lb035q02_write_reg(spi, 0x17, 0x0a0f);
+	lb035q02_write_reg(spi, 0x1e, 0x00c1);
+	lb035q02_write_reg(spi, 0x30, 0x0300);
+	lb035q02_write_reg(spi, 0x31, 0x0007);
+	lb035q02_write_reg(spi, 0x32, 0x0000);
+	lb035q02_write_reg(spi, 0x33, 0x0000);
+	lb035q02_write_reg(spi, 0x34, 0x0707);
+	lb035q02_write_reg(spi, 0x35, 0x0004);
+	lb035q02_write_reg(spi, 0x36, 0x0302);
+	lb035q02_write_reg(spi, 0x37, 0x0202);
+	lb035q02_write_reg(spi, 0x3a, 0x0a0d);
+	lb035q02_write_reg(spi, 0x3b, 0x0806);
+}
+
+static int __devinit lb035q02_panel_spi_probe(struct spi_device *spi)
+{
+	init_lb035q02_panel(spi);
+	return omap_dss_register_driver(&lb035q02_driver);
+}
+
+static int __devexit lb035q02_panel_spi_remove(struct spi_device *spi)
+{
+	omap_dss_unregister_driver(&lb035q02_driver);
+	return 0;
+}
+
+static struct spi_driver lb035q02_spi_driver = {
+	.driver		= {
+		.name	= "lgphilips_lb035q02_panel-spi",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= lb035q02_panel_spi_probe,
+	.remove		= __devexit_p(lb035q02_panel_spi_remove),
+};
+
+static int __init lb035q02_panel_drv_init(void)
+{
+	return spi_register_driver(&lb035q02_spi_driver);
+}
+
+static void __exit lb035q02_panel_drv_exit(void)
+{
+	spi_unregister_driver(&lb035q02_spi_driver);
+}
+
+module_init(lb035q02_panel_drv_init);
+module_exit(lb035q02_panel_drv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
index 925e0fadff54..2ba9d0ca187c 100644
--- a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
+++ b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
@@ -22,7 +22,7 @@
 #include <linux/backlight.h>
 #include <linux/fb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #define LCD_XRES		800
 #define LCD_YRES		480
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index 9a138f650e05..ba38b3ad17d6 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -25,7 +25,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 struct sharp_data {
 	struct backlight_device *bl;
@@ -99,6 +99,7 @@ static int sharp_ls_panel_probe(struct omap_dss_device *dssdev)
 
 	memset(&props, 0, sizeof(struct backlight_properties));
 	props.max_brightness = dssdev->max_backlight_level;
+	props.type = BACKLIGHT_RAW;
 
 	bl = backlight_device_register("sharp-ls", &dssdev->dev, dssdev,
 			&sharp_ls_bl_ops, &props);
@@ -119,7 +120,7 @@ static int sharp_ls_panel_probe(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void sharp_ls_panel_remove(struct omap_dss_device *dssdev)
+static void __exit sharp_ls_panel_remove(struct omap_dss_device *dssdev)
 {
 	struct sharp_data *sd = dev_get_drvdata(&dssdev->dev);
 	struct backlight_device *bl = sd->bl;
@@ -204,7 +205,7 @@ static int sharp_ls_panel_resume(struct omap_dss_device *dssdev)
 
 static struct omap_dss_driver sharp_ls_driver = {
 	.probe		= sharp_ls_panel_probe,
-	.remove		= sharp_ls_panel_remove,
+	.remove		= __exit_p(sharp_ls_panel_remove),
 
 	.enable		= sharp_ls_panel_enable,
 	.disable	= sharp_ls_panel_disable,
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index 61026f96ad20..4e888ac09b3f 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -33,8 +33,8 @@
 #include <linux/regulator/consumer.h>
 #include <linux/mutex.h>
 
-#include <plat/display.h>
-#include <plat/nokia-dsi-panel.h>
+#include <video/omapdss.h>
+#include <video/omap-panel-nokia-dsi.h>
 
 /* DSI Virtual channel. Hardcoded for now. */
 #define TCH 0
@@ -63,12 +63,12 @@
 #define DCS_GET_ID2		0xdb
 #define DCS_GET_ID3		0xdc
 
-#define TAAL_ESD_CHECK_PERIOD	msecs_to_jiffies(5000)
-
 static irqreturn_t taal_te_isr(int irq, void *data);
 static void taal_te_timeout_work_callback(struct work_struct *work);
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable);
 
+static int taal_panel_reset(struct omap_dss_device *dssdev);
+
 struct panel_regulator {
 	struct regulator *regulator;
 	const char *name;
@@ -218,6 +218,8 @@ struct taal_data {
 		u16 w;
 		u16 h;
 	} update_region;
+	int channel;
+
 	struct delayed_work te_timeout_work;
 
 	bool use_dsi_bl;
@@ -227,8 +229,14 @@ struct taal_data {
 
 	bool intro_printed;
 
-	struct workqueue_struct *esd_wq;
+	struct workqueue_struct *workqueue;
+
 	struct delayed_work esd_work;
+	unsigned esd_interval;
+
+	bool ulps_enabled;
+	unsigned ulps_timeout;
+	struct delayed_work ulps_work;
 
 	struct panel_config *panel_config;
 };
@@ -240,6 +248,7 @@ static inline struct nokia_dsi_panel_data
 }
 
 static void taal_esd_work(struct work_struct *work);
+static void taal_ulps_work(struct work_struct *work);
 
 static void hw_guard_start(struct taal_data *td, int guard_msec)
 {
@@ -257,12 +266,12 @@ static void hw_guard_wait(struct taal_data *td)
 	}
 }
 
-static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
+static int taal_dcs_read_1(struct taal_data *td, u8 dcs_cmd, u8 *data)
 {
 	int r;
 	u8 buf[1];
 
-	r = dsi_vc_dcs_read(TCH, dcs_cmd, buf, 1);
+	r = dsi_vc_dcs_read(td->dssdev, td->channel, dcs_cmd, buf, 1);
 
 	if (r < 0)
 		return r;
@@ -272,17 +281,17 @@ static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
 	return 0;
 }
 
-static int taal_dcs_write_0(u8 dcs_cmd)
+static int taal_dcs_write_0(struct taal_data *td, u8 dcs_cmd)
 {
-	return dsi_vc_dcs_write(TCH, &dcs_cmd, 1);
+	return dsi_vc_dcs_write(td->dssdev, td->channel, &dcs_cmd, 1);
 }
 
-static int taal_dcs_write_1(u8 dcs_cmd, u8 param)
+static int taal_dcs_write_1(struct taal_data *td, u8 dcs_cmd, u8 param)
 {
 	u8 buf[2];
 	buf[0] = dcs_cmd;
 	buf[1] = param;
-	return dsi_vc_dcs_write(TCH, buf, 2);
+	return dsi_vc_dcs_write(td->dssdev, td->channel, buf, 2);
 }
 
 static int taal_sleep_in(struct taal_data *td)
@@ -294,7 +303,7 @@ static int taal_sleep_in(struct taal_data *td)
 	hw_guard_wait(td);
 
 	cmd = DCS_SLEEP_IN;
-	r = dsi_vc_dcs_write_nosync(TCH, &cmd, 1);
+	r = dsi_vc_dcs_write_nosync(td->dssdev, td->channel, &cmd, 1);
 	if (r)
 		return r;
 
@@ -312,7 +321,7 @@ static int taal_sleep_out(struct taal_data *td)
 
 	hw_guard_wait(td);
 
-	r = taal_dcs_write_0(DCS_SLEEP_OUT);
+	r = taal_dcs_write_0(td, DCS_SLEEP_OUT);
 	if (r)
 		return r;
 
@@ -324,30 +333,30 @@ static int taal_sleep_out(struct taal_data *td)
 	return 0;
 }
 
-static int taal_get_id(u8 *id1, u8 *id2, u8 *id3)
+static int taal_get_id(struct taal_data *td, u8 *id1, u8 *id2, u8 *id3)
 {
 	int r;
 
-	r = taal_dcs_read_1(DCS_GET_ID1, id1);
+	r = taal_dcs_read_1(td, DCS_GET_ID1, id1);
 	if (r)
 		return r;
-	r = taal_dcs_read_1(DCS_GET_ID2, id2);
+	r = taal_dcs_read_1(td, DCS_GET_ID2, id2);
 	if (r)
 		return r;
-	r = taal_dcs_read_1(DCS_GET_ID3, id3);
+	r = taal_dcs_read_1(td, DCS_GET_ID3, id3);
 	if (r)
 		return r;
 
 	return 0;
 }
 
-static int taal_set_addr_mode(u8 rotate, bool mirror)
+static int taal_set_addr_mode(struct taal_data *td, u8 rotate, bool mirror)
 {
 	int r;
 	u8 mode;
 	int b5, b6, b7;
 
-	r = taal_dcs_read_1(DCS_READ_MADCTL, &mode);
+	r = taal_dcs_read_1(td, DCS_READ_MADCTL, &mode);
 	if (r)
 		return r;
 
@@ -381,10 +390,11 @@ static int taal_set_addr_mode(u8 rotate, bool mirror)
 	mode &= ~((1<<7) | (1<<6) | (1<<5));
 	mode |= (b7 << 7) | (b6 << 6) | (b5 << 5);
 
-	return taal_dcs_write_1(DCS_MEM_ACC_CTRL, mode);
+	return taal_dcs_write_1(td, DCS_MEM_ACC_CTRL, mode);
 }
 
-static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
+static int taal_set_update_window(struct taal_data *td,
+		u16 x, u16 y, u16 w, u16 h)
 {
 	int r;
 	u16 x1 = x;
@@ -399,7 +409,7 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
 	buf[3] = (x2 >> 8) & 0xff;
 	buf[4] = (x2 >> 0) & 0xff;
 
-	r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+	r = dsi_vc_dcs_write_nosync(td->dssdev, td->channel, buf, sizeof(buf));
 	if (r)
 		return r;
 
@@ -409,15 +419,138 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
 	buf[3] = (y2 >> 8) & 0xff;
 	buf[4] = (y2 >> 0) & 0xff;
 
-	r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+	r = dsi_vc_dcs_write_nosync(td->dssdev, td->channel, buf, sizeof(buf));
 	if (r)
 		return r;
 
-	dsi_vc_send_bta_sync(TCH);
+	dsi_vc_send_bta_sync(td->dssdev, td->channel);
+
+	return r;
+}
+
+static void taal_queue_esd_work(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+	if (td->esd_interval > 0)
+		queue_delayed_work(td->workqueue, &td->esd_work,
+				msecs_to_jiffies(td->esd_interval));
+}
+
+static void taal_cancel_esd_work(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+	cancel_delayed_work(&td->esd_work);
+}
+
+static void taal_queue_ulps_work(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+	if (td->ulps_timeout > 0)
+		queue_delayed_work(td->workqueue, &td->ulps_work,
+				msecs_to_jiffies(td->ulps_timeout));
+}
+
+static void taal_cancel_ulps_work(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+	cancel_delayed_work(&td->ulps_work);
+}
+
+static int taal_enter_ulps(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+	int r;
+
+	if (td->ulps_enabled)
+		return 0;
+
+	taal_cancel_ulps_work(dssdev);
+
+	r = _taal_enable_te(dssdev, false);
+	if (r)
+		goto err;
+
+	disable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+
+	omapdss_dsi_display_disable(dssdev, false, true);
+
+	td->ulps_enabled = true;
+
+	return 0;
+
+err:
+	dev_err(&dssdev->dev, "enter ULPS failed");
+	taal_panel_reset(dssdev);
+
+	td->ulps_enabled = false;
+
+	taal_queue_ulps_work(dssdev);
 
 	return r;
 }
 
+static int taal_exit_ulps(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+	int r;
+
+	if (!td->ulps_enabled)
+		return 0;
+
+	r = omapdss_dsi_display_enable(dssdev);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to enable DSI\n");
+		goto err1;
+	}
+
+	omapdss_dsi_vc_enable_hs(dssdev, td->channel, true);
+
+	r = _taal_enable_te(dssdev, true);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to re-enable TE");
+		goto err2;
+	}
+
+	enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+
+	taal_queue_ulps_work(dssdev);
+
+	td->ulps_enabled = false;
+
+	return 0;
+
+err2:
+	dev_err(&dssdev->dev, "failed to exit ULPS");
+
+	r = taal_panel_reset(dssdev);
+	if (!r) {
+		enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+		td->ulps_enabled = false;
+	}
+err1:
+	taal_queue_ulps_work(dssdev);
+
+	return r;
+}
+
+static int taal_wake_up(struct omap_dss_device *dssdev)
+{
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+	if (td->ulps_enabled)
+		return taal_exit_ulps(dssdev);
+
+	taal_cancel_ulps_work(dssdev);
+	taal_queue_ulps_work(dssdev);
+	return 0;
+}
+
 static int taal_bl_update_status(struct backlight_device *dev)
 {
 	struct omap_dss_device *dssdev = dev_get_drvdata(&dev->dev);
@@ -438,9 +571,13 @@ static int taal_bl_update_status(struct backlight_device *dev)
 
 	if (td->use_dsi_bl) {
 		if (td->enabled) {
-			dsi_bus_lock();
-			r = taal_dcs_write_1(DCS_BRIGHTNESS, level);
-			dsi_bus_unlock();
+			dsi_bus_lock(dssdev);
+
+			r = taal_wake_up(dssdev);
+			if (!r)
+				r = taal_dcs_write_1(td, DCS_BRIGHTNESS, level);
+
+			dsi_bus_unlock(dssdev);
 		} else {
 			r = 0;
 		}
@@ -501,9 +638,13 @@ static ssize_t taal_num_errors_show(struct device *dev,
 	mutex_lock(&td->lock);
 
 	if (td->enabled) {
-		dsi_bus_lock();
-		r = taal_dcs_read_1(DCS_READ_NUM_ERRORS, &errors);
-		dsi_bus_unlock();
+		dsi_bus_lock(dssdev);
+
+		r = taal_wake_up(dssdev);
+		if (!r)
+			r = taal_dcs_read_1(td, DCS_READ_NUM_ERRORS, &errors);
+
+		dsi_bus_unlock(dssdev);
 	} else {
 		r = -ENODEV;
 	}
@@ -527,9 +668,13 @@ static ssize_t taal_hw_revision_show(struct device *dev,
 	mutex_lock(&td->lock);
 
 	if (td->enabled) {
-		dsi_bus_lock();
-		r = taal_get_id(&id1, &id2, &id3);
-		dsi_bus_unlock();
+		dsi_bus_lock(dssdev);
+
+		r = taal_wake_up(dssdev);
+		if (!r)
+			r = taal_get_id(td, &id1, &id2, &id3);
+
+		dsi_bus_unlock(dssdev);
 	} else {
 		r = -ENODEV;
 	}
@@ -576,6 +721,7 @@ static ssize_t store_cabc_mode(struct device *dev,
 	struct omap_dss_device *dssdev = to_dss_device(dev);
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
 	int i;
+	int r;
 
 	for (i = 0; i < ARRAY_SIZE(cabc_modes); i++) {
 		if (sysfs_streq(cabc_modes[i], buf))
@@ -588,10 +734,19 @@ static ssize_t store_cabc_mode(struct device *dev,
 	mutex_lock(&td->lock);
 
 	if (td->enabled) {
-		dsi_bus_lock();
-		if (!td->cabc_broken)
-			taal_dcs_write_1(DCS_WRITE_CABC, i);
-		dsi_bus_unlock();
+		dsi_bus_lock(dssdev);
+
+		if (!td->cabc_broken) {
+			r = taal_wake_up(dssdev);
+			if (r)
+				goto err;
+
+			r = taal_dcs_write_1(td, DCS_WRITE_CABC, i);
+			if (r)
+				goto err;
+		}
+
+		dsi_bus_unlock(dssdev);
 	}
 
 	td->cabc_mode = i;
@@ -599,6 +754,10 @@ static ssize_t store_cabc_mode(struct device *dev,
 	mutex_unlock(&td->lock);
 
 	return count;
+err:
+	dsi_bus_unlock(dssdev);
+	mutex_unlock(&td->lock);
+	return r;
 }
 
 static ssize_t show_cabc_available_modes(struct device *dev,
@@ -617,18 +776,161 @@ static ssize_t show_cabc_available_modes(struct device *dev,
 	return len < PAGE_SIZE ? len : PAGE_SIZE - 1;
 }
 
+static ssize_t taal_store_esd_interval(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+	unsigned long t;
+	int r;
+
+	r = strict_strtoul(buf, 10, &t);
+	if (r)
+		return r;
+
+	mutex_lock(&td->lock);
+	taal_cancel_esd_work(dssdev);
+	td->esd_interval = t;
+	if (td->enabled)
+		taal_queue_esd_work(dssdev);
+	mutex_unlock(&td->lock);
+
+	return count;
+}
+
+static ssize_t taal_show_esd_interval(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	unsigned t;
+
+	mutex_lock(&td->lock);
+	t = td->esd_interval;
+	mutex_unlock(&td->lock);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static ssize_t taal_store_ulps(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	unsigned long t;
+	int r;
+
+	r = strict_strtoul(buf, 10, &t);
+	if (r)
+		return r;
+
+	mutex_lock(&td->lock);
+
+	if (td->enabled) {
+		dsi_bus_lock(dssdev);
+
+		if (t)
+			r = taal_enter_ulps(dssdev);
+		else
+			r = taal_wake_up(dssdev);
+
+		dsi_bus_unlock(dssdev);
+	}
+
+	mutex_unlock(&td->lock);
+
+	if (r)
+		return r;
+
+	return count;
+}
+
+static ssize_t taal_show_ulps(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	unsigned t;
+
+	mutex_lock(&td->lock);
+	t = td->ulps_enabled;
+	mutex_unlock(&td->lock);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static ssize_t taal_store_ulps_timeout(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	unsigned long t;
+	int r;
+
+	r = strict_strtoul(buf, 10, &t);
+	if (r)
+		return r;
+
+	mutex_lock(&td->lock);
+	td->ulps_timeout = t;
+
+	if (td->enabled) {
+		/* taal_wake_up will restart the timer */
+		dsi_bus_lock(dssdev);
+		r = taal_wake_up(dssdev);
+		dsi_bus_unlock(dssdev);
+	}
+
+	mutex_unlock(&td->lock);
+
+	if (r)
+		return r;
+
+	return count;
+}
+
+static ssize_t taal_show_ulps_timeout(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	unsigned t;
+
+	mutex_lock(&td->lock);
+	t = td->ulps_timeout;
+	mutex_unlock(&td->lock);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
 static DEVICE_ATTR(num_dsi_errors, S_IRUGO, taal_num_errors_show, NULL);
 static DEVICE_ATTR(hw_revision, S_IRUGO, taal_hw_revision_show, NULL);
 static DEVICE_ATTR(cabc_mode, S_IRUGO | S_IWUSR,
 		show_cabc_mode, store_cabc_mode);
 static DEVICE_ATTR(cabc_available_modes, S_IRUGO,
 		show_cabc_available_modes, NULL);
+static DEVICE_ATTR(esd_interval, S_IRUGO | S_IWUSR,
+		taal_show_esd_interval, taal_store_esd_interval);
+static DEVICE_ATTR(ulps, S_IRUGO | S_IWUSR,
+		taal_show_ulps, taal_store_ulps);
+static DEVICE_ATTR(ulps_timeout, S_IRUGO | S_IWUSR,
+		taal_show_ulps_timeout, taal_store_ulps_timeout);
 
 static struct attribute *taal_attrs[] = {
 	&dev_attr_num_dsi_errors.attr,
 	&dev_attr_hw_revision.attr,
 	&dev_attr_cabc_mode.attr,
 	&dev_attr_cabc_available_modes.attr,
+	&dev_attr_esd_interval.attr,
+	&dev_attr_ulps.attr,
+	&dev_attr_ulps_timeout.attr,
 	NULL,
 };
 
@@ -697,6 +999,9 @@ static int taal_probe(struct omap_dss_device *dssdev)
 	}
 	td->dssdev = dssdev;
 	td->panel_config = panel_config;
+	td->esd_interval = panel_data->esd_interval;
+	td->ulps_enabled = false;
+	td->ulps_timeout = panel_data->ulps_timeout;
 
 	mutex_init(&td->lock);
 
@@ -707,13 +1012,14 @@ static int taal_probe(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_reg;
 
-	td->esd_wq = create_singlethread_workqueue("taal_esd");
-	if (td->esd_wq == NULL) {
+	td->workqueue = create_singlethread_workqueue("taal_esd");
+	if (td->workqueue == NULL) {
 		dev_err(&dssdev->dev, "can't create ESD workqueue\n");
 		r = -ENOMEM;
 		goto err_wq;
 	}
 	INIT_DELAYED_WORK_DEFERRABLE(&td->esd_work, taal_esd_work);
+	INIT_DELAYED_WORK(&td->ulps_work, taal_ulps_work);
 
 	dev_set_drvdata(&dssdev->dev, td);
 
@@ -729,8 +1035,10 @@ static int taal_probe(struct omap_dss_device *dssdev)
 		props.max_brightness = 255;
 	else
 		props.max_brightness = 127;
-	bldev = backlight_device_register("taal", &dssdev->dev, dssdev,
-					  &taal_bl_ops, &props);
+
+	props.type = BACKLIGHT_RAW;
+	bldev = backlight_device_register(dev_name(&dssdev->dev), &dssdev->dev,
+					dssdev, &taal_bl_ops, &props);
 	if (IS_ERR(bldev)) {
 		r = PTR_ERR(bldev);
 		goto err_bl;
@@ -774,14 +1082,29 @@ static int taal_probe(struct omap_dss_device *dssdev)
 		dev_dbg(&dssdev->dev, "Using GPIO TE\n");
 	}
 
+	r = omap_dsi_request_vc(dssdev, &td->channel);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to get virtual channel\n");
+		goto err_req_vc;
+	}
+
+	r = omap_dsi_set_vc_id(dssdev, td->channel, TCH);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to set VC_ID\n");
+		goto err_vc_id;
+	}
+
 	r = sysfs_create_group(&dssdev->dev.kobj, &taal_attr_group);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to create sysfs files\n");
-		goto err_sysfs;
+		goto err_vc_id;
 	}
 
 	return 0;
-err_sysfs:
+
+err_vc_id:
+	omap_dsi_release_vc(dssdev, td->channel);
+err_req_vc:
 	if (panel_data->use_ext_te)
 		free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
 err_irq:
@@ -790,7 +1113,7 @@ err_irq:
 err_gpio:
 	backlight_device_unregister(bldev);
 err_bl:
-	destroy_workqueue(td->esd_wq);
+	destroy_workqueue(td->workqueue);
 err_wq:
 	free_regulators(panel_config->regulators, panel_config->num_regulators);
 err_reg:
@@ -799,7 +1122,7 @@ err:
 	return r;
 }
 
-static void taal_remove(struct omap_dss_device *dssdev)
+static void __exit taal_remove(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
 	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
@@ -808,6 +1131,7 @@ static void taal_remove(struct omap_dss_device *dssdev)
 	dev_dbg(&dssdev->dev, "remove\n");
 
 	sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
+	omap_dsi_release_vc(dssdev, td->channel);
 
 	if (panel_data->use_ext_te) {
 		int gpio = panel_data->ext_te_gpio;
@@ -820,8 +1144,9 @@ static void taal_remove(struct omap_dss_device *dssdev)
 	taal_bl_update_status(bldev);
 	backlight_device_unregister(bldev);
 
-	cancel_delayed_work(&td->esd_work);
-	destroy_workqueue(td->esd_wq);
+	taal_cancel_ulps_work(dssdev);
+	taal_cancel_esd_work(dssdev);
+	destroy_workqueue(td->workqueue);
 
 	/* reset, to be sure that the panel is in a valid state */
 	taal_hw_reset(dssdev);
@@ -846,13 +1171,13 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 
 	taal_hw_reset(dssdev);
 
-	omapdss_dsi_vc_enable_hs(TCH, false);
+	omapdss_dsi_vc_enable_hs(dssdev, td->channel, false);
 
 	r = taal_sleep_out(td);
 	if (r)
 		goto err;
 
-	r = taal_get_id(&id1, &id2, &id3);
+	r = taal_get_id(td, &id1, &id2, &id3);
 	if (r)
 		goto err;
 
@@ -861,30 +1186,30 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 		(id2 == 0x00 || id2 == 0xff || id2 == 0x81))
 		td->cabc_broken = true;
 
-	r = taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
+	r = taal_dcs_write_1(td, DCS_BRIGHTNESS, 0xff);
 	if (r)
 		goto err;
 
-	r = taal_dcs_write_1(DCS_CTRL_DISPLAY,
+	r = taal_dcs_write_1(td, DCS_CTRL_DISPLAY,
 			(1<<2) | (1<<5));	/* BL | BCTRL */
 	if (r)
 		goto err;
 
-	r = taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+	r = taal_dcs_write_1(td, DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
 	if (r)
 		goto err;
 
-	r = taal_set_addr_mode(td->rotate, td->mirror);
+	r = taal_set_addr_mode(td, td->rotate, td->mirror);
 	if (r)
 		goto err;
 
 	if (!td->cabc_broken) {
-		r = taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+		r = taal_dcs_write_1(td, DCS_WRITE_CABC, td->cabc_mode);
 		if (r)
 			goto err;
 	}
 
-	r = taal_dcs_write_0(DCS_DISPLAY_ON);
+	r = taal_dcs_write_0(td, DCS_DISPLAY_ON);
 	if (r)
 		goto err;
 
@@ -903,7 +1228,7 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 		td->intro_printed = true;
 	}
 
-	omapdss_dsi_vc_enable_hs(TCH, true);
+	omapdss_dsi_vc_enable_hs(dssdev, td->channel, true);
 
 	return 0;
 err:
@@ -911,7 +1236,7 @@ err:
 
 	taal_hw_reset(dssdev);
 
-	omapdss_dsi_display_disable(dssdev);
+	omapdss_dsi_display_disable(dssdev, true, false);
 err0:
 	return r;
 }
@@ -921,12 +1246,9 @@ static void taal_power_off(struct omap_dss_device *dssdev)
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
 	int r;
 
-	r = taal_dcs_write_0(DCS_DISPLAY_OFF);
-	if (!r) {
+	r = taal_dcs_write_0(td, DCS_DISPLAY_OFF);
+	if (!r)
 		r = taal_sleep_in(td);
-		/* HACK: wait a bit so that the message goes through */
-		msleep(10);
-	}
 
 	if (r) {
 		dev_err(&dssdev->dev,
@@ -934,15 +1256,23 @@ static void taal_power_off(struct omap_dss_device *dssdev)
 		taal_hw_reset(dssdev);
 	}
 
-	omapdss_dsi_display_disable(dssdev);
+	omapdss_dsi_display_disable(dssdev, true, false);
 
 	td->enabled = 0;
 }
 
+static int taal_panel_reset(struct omap_dss_device *dssdev)
+{
+	dev_err(&dssdev->dev, "performing LCD reset\n");
+
+	taal_power_off(dssdev);
+	taal_hw_reset(dssdev);
+	return taal_power_on(dssdev);
+}
+
 static int taal_enable(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	dev_dbg(&dssdev->dev, "enable\n");
@@ -954,18 +1284,16 @@ static int taal_enable(struct omap_dss_device *dssdev)
 		goto err;
 	}
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
 	r = taal_power_on(dssdev);
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 
 	if (r)
 		goto err;
 
-	if (panel_data->use_esd_check)
-		queue_delayed_work(td->esd_wq, &td->esd_work,
-				TAAL_ESD_CHECK_PERIOD);
+	taal_queue_esd_work(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
@@ -986,14 +1314,20 @@ static void taal_disable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&td->lock);
 
-	cancel_delayed_work(&td->esd_work);
+	taal_cancel_ulps_work(dssdev);
+	taal_cancel_esd_work(dssdev);
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
-	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
-		taal_power_off(dssdev);
+	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+		int r;
+
+		r = taal_wake_up(dssdev);
+		if (!r)
+			taal_power_off(dssdev);
+	}
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 
@@ -1014,13 +1348,16 @@ static int taal_suspend(struct omap_dss_device *dssdev)
 		goto err;
 	}
 
-	cancel_delayed_work(&td->esd_work);
+	taal_cancel_ulps_work(dssdev);
+	taal_cancel_esd_work(dssdev);
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
-	taal_power_off(dssdev);
+	r = taal_wake_up(dssdev);
+	if (!r)
+		taal_power_off(dssdev);
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
@@ -1035,7 +1372,6 @@ err:
 static int taal_resume(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	dev_dbg(&dssdev->dev, "resume\n");
@@ -1047,19 +1383,17 @@ static int taal_resume(struct omap_dss_device *dssdev)
 		goto err;
 	}
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
 	r = taal_power_on(dssdev);
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 
 	if (r) {
 		dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 	} else {
 		dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-		if (panel_data->use_esd_check)
-			queue_delayed_work(td->esd_wq, &td->esd_work,
-					TAAL_ESD_CHECK_PERIOD);
+		taal_queue_esd_work(dssdev);
 	}
 
 	mutex_unlock(&td->lock);
@@ -1074,7 +1408,7 @@ static void taal_framedone_cb(int err, void *data)
 {
 	struct omap_dss_device *dssdev = data;
 	dev_dbg(&dssdev->dev, "framedone, err %d\n", err);
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 }
 
 static irqreturn_t taal_te_isr(int irq, void *data)
@@ -1089,7 +1423,7 @@ static irqreturn_t taal_te_isr(int irq, void *data)
 	if (old) {
 		cancel_delayed_work(&td->te_timeout_work);
 
-		r = omap_dsi_update(dssdev, TCH,
+		r = omap_dsi_update(dssdev, td->channel,
 				td->update_region.x,
 				td->update_region.y,
 				td->update_region.w,
@@ -1102,7 +1436,7 @@ static irqreturn_t taal_te_isr(int irq, void *data)
 	return IRQ_HANDLED;
 err:
 	dev_err(&dssdev->dev, "start update failed\n");
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 	return IRQ_HANDLED;
 }
 
@@ -1115,7 +1449,7 @@ static void taal_te_timeout_work_callback(struct work_struct *work)
 	dev_err(&dssdev->dev, "TE not received for 250ms!\n");
 
 	atomic_set(&td->do_update, 0);
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 }
 
 static int taal_update(struct omap_dss_device *dssdev,
@@ -1128,7 +1462,11 @@ static int taal_update(struct omap_dss_device *dssdev,
 	dev_dbg(&dssdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
 
 	mutex_lock(&td->lock);
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
+
+	r = taal_wake_up(dssdev);
+	if (r)
+		goto err;
 
 	if (!td->enabled) {
 		r = 0;
@@ -1139,7 +1477,7 @@ static int taal_update(struct omap_dss_device *dssdev,
 	if (r)
 		goto err;
 
-	r = taal_set_update_window(x, y, w, h);
+	r = taal_set_update_window(td, x, y, w, h);
 	if (r)
 		goto err;
 
@@ -1153,7 +1491,7 @@ static int taal_update(struct omap_dss_device *dssdev,
 				msecs_to_jiffies(250));
 		atomic_set(&td->do_update, 1);
 	} else {
-		r = omap_dsi_update(dssdev, TCH, x, y, w, h,
+		r = omap_dsi_update(dssdev, td->channel, x, y, w, h,
 				taal_framedone_cb, dssdev);
 		if (r)
 			goto err;
@@ -1163,7 +1501,7 @@ static int taal_update(struct omap_dss_device *dssdev,
 	mutex_unlock(&td->lock);
 	return 0;
 err:
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 	mutex_unlock(&td->lock);
 	return r;
 }
@@ -1175,8 +1513,8 @@ static int taal_sync(struct omap_dss_device *dssdev)
 	dev_dbg(&dssdev->dev, "sync\n");
 
 	mutex_lock(&td->lock);
-	dsi_bus_lock();
-	dsi_bus_unlock();
+	dsi_bus_lock(dssdev);
+	dsi_bus_unlock(dssdev);
 	mutex_unlock(&td->lock);
 
 	dev_dbg(&dssdev->dev, "sync done\n");
@@ -1191,9 +1529,9 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 	int r;
 
 	if (enable)
-		r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+		r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
 	else
-		r = taal_dcs_write_0(DCS_TEAR_OFF);
+		r = taal_dcs_write_0(td, DCS_TEAR_OFF);
 
 	if (!panel_data->use_ext_te)
 		omapdss_dsi_enable_te(dssdev, enable);
@@ -1214,9 +1552,13 @@ static int taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 	if (td->te_enabled == enable)
 		goto end;
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
 	if (td->enabled) {
+		r = taal_wake_up(dssdev);
+		if (r)
+			goto err;
+
 		r = _taal_enable_te(dssdev, enable);
 		if (r)
 			goto err;
@@ -1224,13 +1566,13 @@ static int taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 
 	td->te_enabled = enable;
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 end:
 	mutex_unlock(&td->lock);
 
 	return 0;
 err:
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 	mutex_unlock(&td->lock);
 
 	return r;
@@ -1260,22 +1602,26 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
 	if (td->rotate == rotate)
 		goto end;
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
 	if (td->enabled) {
-		r = taal_set_addr_mode(rotate, td->mirror);
+		r = taal_wake_up(dssdev);
+		if (r)
+			goto err;
+
+		r = taal_set_addr_mode(td, rotate, td->mirror);
 		if (r)
 			goto err;
 	}
 
 	td->rotate = rotate;
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 end:
 	mutex_unlock(&td->lock);
 	return 0;
 err:
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 	mutex_unlock(&td->lock);
 	return r;
 }
@@ -1304,21 +1650,25 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
 	if (td->mirror == enable)
 		goto end;
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 	if (td->enabled) {
-		r = taal_set_addr_mode(td->rotate, enable);
+		r = taal_wake_up(dssdev);
+		if (r)
+			goto err;
+
+		r = taal_set_addr_mode(td, td->rotate, enable);
 		if (r)
 			goto err;
 	}
 
 	td->mirror = enable;
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 end:
 	mutex_unlock(&td->lock);
 	return 0;
 err:
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 	mutex_unlock(&td->lock);
 	return r;
 }
@@ -1348,23 +1698,27 @@ static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
 		goto err1;
 	}
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
-	r = taal_dcs_read_1(DCS_GET_ID1, &id1);
+	r = taal_wake_up(dssdev);
 	if (r)
 		goto err2;
-	r = taal_dcs_read_1(DCS_GET_ID2, &id2);
+
+	r = taal_dcs_read_1(td, DCS_GET_ID1, &id1);
 	if (r)
 		goto err2;
-	r = taal_dcs_read_1(DCS_GET_ID3, &id3);
+	r = taal_dcs_read_1(td, DCS_GET_ID2, &id2);
+	if (r)
+		goto err2;
+	r = taal_dcs_read_1(td, DCS_GET_ID3, &id3);
 	if (r)
 		goto err2;
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 	mutex_unlock(&td->lock);
 	return 0;
 err2:
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 err1:
 	mutex_unlock(&td->lock);
 	return r;
@@ -1394,7 +1748,11 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 			dssdev->panel.timings.x_res *
 			dssdev->panel.timings.y_res * 3);
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
+
+	r = taal_wake_up(dssdev);
+	if (r)
+		goto err2;
 
 	/* plen 1 or 2 goes into short packet. until checksum error is fixed,
 	 * use short packets. plen 32 works, but bigger packets seem to cause
@@ -1404,9 +1762,9 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 	else
 		plen = 2;
 
-	taal_set_update_window(x, y, w, h);
+	taal_set_update_window(td, x, y, w, h);
 
-	r = dsi_vc_set_max_rx_packet_size(TCH, plen);
+	r = dsi_vc_set_max_rx_packet_size(dssdev, td->channel, plen);
 	if (r)
 		goto err2;
 
@@ -1414,7 +1772,7 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 		u8 dcs_cmd = first ? 0x2e : 0x3e;
 		first = 0;
 
-		r = dsi_vc_dcs_read(TCH, dcs_cmd,
+		r = dsi_vc_dcs_read(dssdev, td->channel, dcs_cmd,
 				buf + buf_used, size - buf_used);
 
 		if (r < 0) {
@@ -1440,14 +1798,35 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 	r = buf_used;
 
 err3:
-	dsi_vc_set_max_rx_packet_size(TCH, 1);
+	dsi_vc_set_max_rx_packet_size(dssdev, td->channel, 1);
 err2:
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 err1:
 	mutex_unlock(&td->lock);
 	return r;
 }
 
+static void taal_ulps_work(struct work_struct *work)
+{
+	struct taal_data *td = container_of(work, struct taal_data,
+			ulps_work.work);
+	struct omap_dss_device *dssdev = td->dssdev;
+
+	mutex_lock(&td->lock);
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE || !td->enabled) {
+		mutex_unlock(&td->lock);
+		return;
+	}
+
+	dsi_bus_lock(dssdev);
+
+	taal_enter_ulps(dssdev);
+
+	dsi_bus_unlock(dssdev);
+	mutex_unlock(&td->lock);
+}
+
 static void taal_esd_work(struct work_struct *work)
 {
 	struct taal_data *td = container_of(work, struct taal_data,
@@ -1464,9 +1843,15 @@ static void taal_esd_work(struct work_struct *work)
 		return;
 	}
 
-	dsi_bus_lock();
+	dsi_bus_lock(dssdev);
 
-	r = taal_dcs_read_1(DCS_RDDSDR, &state1);
+	r = taal_wake_up(dssdev);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to exit ULPS\n");
+		goto err;
+	}
+
+	r = taal_dcs_read_1(td, DCS_RDDSDR, &state1);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to read Taal status\n");
 		goto err;
@@ -1479,7 +1864,7 @@ static void taal_esd_work(struct work_struct *work)
 		goto err;
 	}
 
-	r = taal_dcs_read_1(DCS_RDDSDR, &state2);
+	r = taal_dcs_read_1(td, DCS_RDDSDR, &state2);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to read Taal status\n");
 		goto err;
@@ -1495,57 +1880,38 @@ static void taal_esd_work(struct work_struct *work)
 	/* Self-diagnostics result is also shown on TE GPIO line. We need
 	 * to re-enable TE after self diagnostics */
 	if (td->te_enabled && panel_data->use_ext_te) {
-		r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+		r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
 		if (r)
 			goto err;
 	}
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 
-	queue_delayed_work(td->esd_wq, &td->esd_work, TAAL_ESD_CHECK_PERIOD);
+	taal_queue_esd_work(dssdev);
 
 	mutex_unlock(&td->lock);
 	return;
 err:
 	dev_err(&dssdev->dev, "performing LCD reset\n");
 
-	taal_power_off(dssdev);
-	taal_hw_reset(dssdev);
-	taal_power_on(dssdev);
+	taal_panel_reset(dssdev);
 
-	dsi_bus_unlock();
+	dsi_bus_unlock(dssdev);
 
-	queue_delayed_work(td->esd_wq, &td->esd_work, TAAL_ESD_CHECK_PERIOD);
+	taal_queue_esd_work(dssdev);
 
 	mutex_unlock(&td->lock);
 }
 
-static int taal_set_update_mode(struct omap_dss_device *dssdev,
-		enum omap_dss_update_mode mode)
-{
-	if (mode != OMAP_DSS_UPDATE_MANUAL)
-		return -EINVAL;
-	return 0;
-}
-
-static enum omap_dss_update_mode taal_get_update_mode(
-		struct omap_dss_device *dssdev)
-{
-	return OMAP_DSS_UPDATE_MANUAL;
-}
-
 static struct omap_dss_driver taal_driver = {
 	.probe		= taal_probe,
-	.remove		= taal_remove,
+	.remove		= __exit_p(taal_remove),
 
 	.enable		= taal_enable,
 	.disable	= taal_disable,
 	.suspend	= taal_suspend,
 	.resume		= taal_resume,
 
-	.set_update_mode = taal_set_update_mode,
-	.get_update_mode = taal_get_update_mode,
-
 	.update		= taal_update,
 	.sync		= taal_sync,
 
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index dbe9d43b4850..2462b9ec6662 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -17,7 +17,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #define TPO_R02_MODE(x)		((x) & 7)
 #define TPO_R02_MODE_800x480	7
@@ -144,13 +144,15 @@ static ssize_t tpo_td043_vmirror_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct tpo_td043_device *tpo_td043 = dev_get_drvdata(dev);
-	long val;
+	int val;
 	int ret;
 
-	ret = strict_strtol(buf, 0, &val);
+	ret = kstrtoint(buf, 0, &val);
 	if (ret < 0)
 		return ret;
 
+	val = !!val;
+
 	ret = tpo_td043_write_mirror(tpo_td043->spi, tpo_td043->hmirror, val);
 	if (ret < 0)
 		return ret;
@@ -175,7 +177,7 @@ static ssize_t tpo_td043_mode_store(struct device *dev,
 	long val;
 	int ret;
 
-	ret = strict_strtol(buf, 0, &val);
+	ret = kstrtol(buf, 0, &val);
 	if (ret != 0 || val & ~7)
 		return -EINVAL;
 
diff --git a/drivers/video/omap2/dss/Kconfig b/drivers/video/omap2/dss/Kconfig
index 43b64403eaa4..0d12524db14b 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -1,8 +1,8 @@
 menuconfig OMAP2_DSS
-        tristate "OMAP2/3 Display Subsystem support (EXPERIMENTAL)"
-        depends on ARCH_OMAP2 || ARCH_OMAP3
+        tristate "OMAP2+ Display Subsystem support (EXPERIMENTAL)"
+        depends on ARCH_OMAP2PLUS
         help
-          OMAP2/3 Display Subsystem support.
+	  OMAP2+ Display Subsystem support.
 
 if OMAP2_DSS
 
@@ -60,6 +60,14 @@ config OMAP2_DSS_VENC
 	help
 	  OMAP Video Encoder support for S-Video and composite TV-out.
 
+config OMAP4_DSS_HDMI
+	bool "HDMI support"
+	depends on ARCH_OMAP4
+        default y
+	help
+	  HDMI Interface. This adds the High Definition Multimedia Interface.
+	  See http://www.hdmi.org/ for HDMI specification.
+
 config OMAP2_DSS_SDI
 	bool "SDI support"
 	depends on ARCH_OMAP3
@@ -72,7 +80,7 @@ config OMAP2_DSS_SDI
 
 config OMAP2_DSS_DSI
 	bool "DSI support"
-	depends on ARCH_OMAP3
+	depends on ARCH_OMAP3 || ARCH_OMAP4
         default n
 	help
 	  MIPI DSI (Display Serial Interface) support.
@@ -82,14 +90,6 @@ config OMAP2_DSS_DSI
 
 	  See http://www.mipi.org/ for DSI spesifications.
 
-config OMAP2_DSS_USE_DSI_PLL
-	bool "Use DSI PLL for PCLK (EXPERIMENTAL)"
-	default n
-	depends on OMAP2_DSS_DSI
-	help
-	  Use DSI PLL to generate pixel clock.  Currently only for DPI output.
-	  DSI PLL can be used to generate higher and more precise pixel clocks.
-
 config OMAP2_DSS_FAKE_VSYNC
 	bool "Fake VSYNC irq from manual update displays"
 	default n
@@ -117,4 +117,15 @@ config OMAP2_DSS_MIN_FCK_PER_PCK
 	  Max FCK is 173MHz, so this doesn't work if your PCK
 	  is very high.
 
+config OMAP2_DSS_SLEEP_AFTER_VENC_RESET
+	bool "Sleep 20ms after VENC reset"
+	default y
+	help
+	  There is a 20ms sleep after VENC reset which seemed to fix the
+	  reset. The reason for the bug is unclear, and it's also unclear
+	  on what platforms this happens.
+
+	  This option enables the sleep, and is enabled by default. You can
+	  disable the sleep if it doesn't cause problems on your platform.
+
 endif
diff --git a/drivers/video/omap2/dss/Makefile b/drivers/video/omap2/dss/Makefile
index 7db17b5e570c..10d9d3bb3e24 100644
--- a/drivers/video/omap2/dss/Makefile
+++ b/drivers/video/omap2/dss/Makefile
@@ -5,3 +5,5 @@ omapdss-$(CONFIG_OMAP2_DSS_RFBI) += rfbi.o
 omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o
 omapdss-$(CONFIG_OMAP2_DSS_SDI) += sdi.o
 omapdss-$(CONFIG_OMAP2_DSS_DSI) += dsi.o
+omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi.o \
+				    hdmi_omap4_panel.o
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index 8e89f6049280..76821fefce9a 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -33,332 +33,29 @@
 #include <linux/device.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
-#include <plat/clock.h>
+#include <video/omapdss.h>
 
 #include "dss.h"
 #include "dss_features.h"
 
 static struct {
 	struct platform_device *pdev;
-	int		ctx_id;
-
-	struct clk      *dss_ick;
-	struct clk	*dss1_fck;
-	struct clk	*dss2_fck;
-	struct clk      *dss_54m_fck;
-	struct clk	*dss_96m_fck;
-	unsigned	num_clks_enabled;
 
 	struct regulator *vdds_dsi_reg;
 	struct regulator *vdds_sdi_reg;
-	struct regulator *vdda_dac_reg;
 } core;
 
-static void dss_clk_enable_all_no_ctx(void);
-static void dss_clk_disable_all_no_ctx(void);
-static void dss_clk_enable_no_ctx(enum dss_clock clks);
-static void dss_clk_disable_no_ctx(enum dss_clock clks);
-
 static char *def_disp_name;
 module_param_named(def_disp, def_disp_name, charp, 0);
-MODULE_PARM_DESC(def_disp_name, "default display name");
+MODULE_PARM_DESC(def_disp, "default display name");
 
 #ifdef DEBUG
 unsigned int dss_debug;
 module_param_named(debug, dss_debug, bool, 0644);
 #endif
 
-/* CONTEXT */
-static int dss_get_ctx_id(void)
-{
-	struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
-	int r;
-
-	if (!pdata->get_last_off_on_transaction_id)
-		return 0;
-	r = pdata->get_last_off_on_transaction_id(&core.pdev->dev);
-	if (r < 0) {
-		dev_err(&core.pdev->dev, "getting transaction ID failed, "
-				"will force context restore\n");
-		r = -1;
-	}
-	return r;
-}
-
-int dss_need_ctx_restore(void)
-{
-	int id = dss_get_ctx_id();
-
-	if (id < 0 || id != core.ctx_id) {
-		DSSDBG("ctx id %d -> id %d\n",
-				core.ctx_id, id);
-		core.ctx_id = id;
-		return 1;
-	} else {
-		return 0;
-	}
-}
-
-static void save_all_ctx(void)
-{
-	DSSDBG("save context\n");
-
-	dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-	dss_save_context();
-	dispc_save_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_save_context();
-#endif
-
-	dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-}
-
-static void restore_all_ctx(void)
-{
-	DSSDBG("restore context\n");
-
-	dss_clk_enable_all_no_ctx();
-
-	dss_restore_context();
-	dispc_restore_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_restore_context();
-#endif
-
-	dss_clk_disable_all_no_ctx();
-}
-
-#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-/* CLOCKS */
-static void core_dump_clocks(struct seq_file *s)
-{
-	int i;
-	struct clk *clocks[5] = {
-		core.dss_ick,
-		core.dss1_fck,
-		core.dss2_fck,
-		core.dss_54m_fck,
-		core.dss_96m_fck
-	};
-
-	seq_printf(s, "- CORE -\n");
-
-	seq_printf(s, "internal clk count\t\t%u\n", core.num_clks_enabled);
-
-	for (i = 0; i < 5; i++) {
-		if (!clocks[i])
-			continue;
-		seq_printf(s, "%-15s\t%lu\t%d\n",
-				clocks[i]->name,
-				clk_get_rate(clocks[i]),
-				clocks[i]->usecount);
-	}
-}
-#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
-
-static int dss_get_clock(struct clk **clock, const char *clk_name)
-{
-	struct clk *clk;
-
-	clk = clk_get(&core.pdev->dev, clk_name);
-
-	if (IS_ERR(clk)) {
-		DSSERR("can't get clock %s", clk_name);
-		return PTR_ERR(clk);
-	}
-
-	*clock = clk;
-
-	DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
-
-	return 0;
-}
-
-static int dss_get_clocks(void)
-{
-	int r;
-
-	core.dss_ick = NULL;
-	core.dss1_fck = NULL;
-	core.dss2_fck = NULL;
-	core.dss_54m_fck = NULL;
-	core.dss_96m_fck = NULL;
-
-	r = dss_get_clock(&core.dss_ick, "ick");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss1_fck, "dss1_fck");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss2_fck, "dss2_fck");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss_54m_fck, "tv_fck");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss_96m_fck, "video_fck");
-	if (r)
-		goto err;
-
-	return 0;
-
-err:
-	if (core.dss_ick)
-		clk_put(core.dss_ick);
-	if (core.dss1_fck)
-		clk_put(core.dss1_fck);
-	if (core.dss2_fck)
-		clk_put(core.dss2_fck);
-	if (core.dss_54m_fck)
-		clk_put(core.dss_54m_fck);
-	if (core.dss_96m_fck)
-		clk_put(core.dss_96m_fck);
-
-	return r;
-}
-
-static void dss_put_clocks(void)
-{
-	if (core.dss_96m_fck)
-		clk_put(core.dss_96m_fck);
-	clk_put(core.dss_54m_fck);
-	clk_put(core.dss1_fck);
-	clk_put(core.dss2_fck);
-	clk_put(core.dss_ick);
-}
-
-unsigned long dss_clk_get_rate(enum dss_clock clk)
-{
-	switch (clk) {
-	case DSS_CLK_ICK:
-		return clk_get_rate(core.dss_ick);
-	case DSS_CLK_FCK1:
-		return clk_get_rate(core.dss1_fck);
-	case DSS_CLK_FCK2:
-		return clk_get_rate(core.dss2_fck);
-	case DSS_CLK_54M:
-		return clk_get_rate(core.dss_54m_fck);
-	case DSS_CLK_96M:
-		return clk_get_rate(core.dss_96m_fck);
-	}
-
-	BUG();
-	return 0;
-}
-
-static unsigned count_clk_bits(enum dss_clock clks)
-{
-	unsigned num_clks = 0;
-
-	if (clks & DSS_CLK_ICK)
-		++num_clks;
-	if (clks & DSS_CLK_FCK1)
-		++num_clks;
-	if (clks & DSS_CLK_FCK2)
-		++num_clks;
-	if (clks & DSS_CLK_54M)
-		++num_clks;
-	if (clks & DSS_CLK_96M)
-		++num_clks;
-
-	return num_clks;
-}
-
-static void dss_clk_enable_no_ctx(enum dss_clock clks)
-{
-	unsigned num_clks = count_clk_bits(clks);
-
-	if (clks & DSS_CLK_ICK)
-		clk_enable(core.dss_ick);
-	if (clks & DSS_CLK_FCK1)
-		clk_enable(core.dss1_fck);
-	if (clks & DSS_CLK_FCK2)
-		clk_enable(core.dss2_fck);
-	if (clks & DSS_CLK_54M)
-		clk_enable(core.dss_54m_fck);
-	if (clks & DSS_CLK_96M)
-		clk_enable(core.dss_96m_fck);
-
-	core.num_clks_enabled += num_clks;
-}
-
-void dss_clk_enable(enum dss_clock clks)
-{
-	bool check_ctx = core.num_clks_enabled == 0;
-
-	dss_clk_enable_no_ctx(clks);
-
-	if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
-		restore_all_ctx();
-}
-
-static void dss_clk_disable_no_ctx(enum dss_clock clks)
-{
-	unsigned num_clks = count_clk_bits(clks);
-
-	if (clks & DSS_CLK_ICK)
-		clk_disable(core.dss_ick);
-	if (clks & DSS_CLK_FCK1)
-		clk_disable(core.dss1_fck);
-	if (clks & DSS_CLK_FCK2)
-		clk_disable(core.dss2_fck);
-	if (clks & DSS_CLK_54M)
-		clk_disable(core.dss_54m_fck);
-	if (clks & DSS_CLK_96M)
-		clk_disable(core.dss_96m_fck);
-
-	core.num_clks_enabled -= num_clks;
-}
-
-void dss_clk_disable(enum dss_clock clks)
-{
-	if (cpu_is_omap34xx()) {
-		unsigned num_clks = count_clk_bits(clks);
-
-		BUG_ON(core.num_clks_enabled < num_clks);
-
-		if (core.num_clks_enabled == num_clks)
-			save_all_ctx();
-	}
-
-	dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_enable_all_no_ctx(void)
-{
-	enum dss_clock clks;
-
-	clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-	if (cpu_is_omap34xx())
-		clks |= DSS_CLK_96M;
-	dss_clk_enable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all_no_ctx(void)
-{
-	enum dss_clock clks;
-
-	clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-	if (cpu_is_omap34xx())
-		clks |= DSS_CLK_96M;
-	dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all(void)
-{
-	enum dss_clock clks;
-
-	clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-	if (cpu_is_omap34xx())
-		clks |= DSS_CLK_96M;
-	dss_clk_disable(clks);
-}
+static int omap_dss_register_device(struct omap_dss_device *);
+static void omap_dss_unregister_device(struct omap_dss_device *);
 
 /* REGULATORS */
 
@@ -390,32 +87,7 @@ struct regulator *dss_get_vdds_sdi(void)
 	return reg;
 }
 
-struct regulator *dss_get_vdda_dac(void)
-{
-	struct regulator *reg;
-
-	if (core.vdda_dac_reg != NULL)
-		return core.vdda_dac_reg;
-
-	reg = regulator_get(&core.pdev->dev, "vdda_dac");
-	if (!IS_ERR(reg))
-		core.vdda_dac_reg = reg;
-
-	return reg;
-}
-
-/* DEBUGFS */
 #if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-static void dss_debug_dump_clocks(struct seq_file *s)
-{
-	core_dump_clocks(s);
-	dss_dump_clocks(s);
-	dispc_dump_clocks(s);
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_dump_clocks(s);
-#endif
-}
-
 static int dss_debug_show(struct seq_file *s, void *unused)
 {
 	void (*func)(struct seq_file *) = s->private;
@@ -455,8 +127,7 @@ static int dss_initialize_debugfs(void)
 #endif
 
 #if defined(CONFIG_OMAP2_DSS_DSI) && defined(CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS)
-	debugfs_create_file("dsi_irq", S_IRUGO, dss_debugfs_dir,
-			&dsi_dump_irqs, &dss_debug_fops);
+	dsi_create_debugfs_files_irq(dss_debugfs_dir, &dss_debug_fops);
 #endif
 
 	debugfs_create_file("dss", S_IRUGO, dss_debugfs_dir,
@@ -468,8 +139,7 @@ static int dss_initialize_debugfs(void)
 			&rfbi_dump_regs, &dss_debug_fops);
 #endif
 #ifdef CONFIG_OMAP2_DSS_DSI
-	debugfs_create_file("dsi", S_IRUGO, dss_debugfs_dir,
-			&dsi_dump_regs, &dss_debug_fops);
+	dsi_create_debugfs_files_reg(dss_debugfs_dir, &dss_debug_fops);
 #endif
 #ifdef CONFIG_OMAP2_DSS_VENC
 	debugfs_create_file("venc", S_IRUGO, dss_debugfs_dir,
@@ -497,7 +167,6 @@ static inline void dss_uninitialize_debugfs(void)
 static int omap_dss_probe(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int skip_init = 0;
 	int r;
 	int i;
 
@@ -508,63 +177,40 @@ static int omap_dss_probe(struct platform_device *pdev)
 	dss_init_overlay_managers(pdev);
 	dss_init_overlays(pdev);
 
-	r = dss_get_clocks();
-	if (r)
-		goto err_clocks;
-
-	dss_clk_enable_all_no_ctx();
-
-	core.ctx_id = dss_get_ctx_id();
-	DSSDBG("initial ctx id %u\n", core.ctx_id);
-
-#ifdef CONFIG_FB_OMAP_BOOTLOADER_INIT
-	/* DISPC_CONTROL */
-	if (omap_readl(0x48050440) & 1)	/* LCD enabled? */
-		skip_init = 1;
-#endif
-
-	r = dss_init(skip_init);
+	r = dss_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize DSS\n");
+		DSSERR("Failed to initialize DSS platform driver\n");
 		goto err_dss;
 	}
 
-	r = rfbi_init();
+	r = dispc_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize rfbi\n");
-		goto err_rfbi;
+		DSSERR("Failed to initialize dispc platform driver\n");
+		goto err_dispc;
 	}
 
-	r = dpi_init(pdev);
+	r = rfbi_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize dpi\n");
-		goto err_dpi;
+		DSSERR("Failed to initialize rfbi platform driver\n");
+		goto err_rfbi;
 	}
 
-	r = dispc_init();
+	r = venc_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize dispc\n");
-		goto err_dispc;
+		DSSERR("Failed to initialize venc platform driver\n");
+		goto err_venc;
 	}
 
-	r = venc_init(pdev);
+	r = dsi_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize venc\n");
-		goto err_venc;
+		DSSERR("Failed to initialize DSI platform driver\n");
+		goto err_dsi;
 	}
 
-	if (cpu_is_omap34xx()) {
-		r = sdi_init(skip_init);
-		if (r) {
-			DSSERR("Failed to initialize SDI\n");
-			goto err_sdi;
-		}
-
-		r = dsi_init(pdev);
-		if (r) {
-			DSSERR("Failed to initialize DSI\n");
-			goto err_dsi;
-		}
+	r = hdmi_init_platform_driver();
+	if (r) {
+		DSSERR("Failed to initialize hdmi\n");
+		goto err_hdmi;
 	}
 
 	r = dss_initialize_debugfs();
@@ -589,32 +235,23 @@ static int omap_dss_probe(struct platform_device *pdev)
 			pdata->default_device = dssdev;
 	}
 
-	dss_clk_disable_all();
-
 	return 0;
 
 err_register:
 	dss_uninitialize_debugfs();
 err_debugfs:
-	if (cpu_is_omap34xx())
-		dsi_exit();
+	hdmi_uninit_platform_driver();
+err_hdmi:
+	dsi_uninit_platform_driver();
 err_dsi:
-	if (cpu_is_omap34xx())
-		sdi_exit();
-err_sdi:
-	venc_exit();
+	venc_uninit_platform_driver();
 err_venc:
-	dispc_exit();
+	dispc_uninit_platform_driver();
 err_dispc:
-	dpi_exit();
-err_dpi:
-	rfbi_exit();
+	rfbi_uninit_platform_driver();
 err_rfbi:
-	dss_exit();
+	dss_uninit_platform_driver();
 err_dss:
-	dss_clk_disable_all_no_ctx();
-	dss_put_clocks();
-err_clocks:
 
 	return r;
 }
@@ -623,61 +260,15 @@ static int omap_dss_remove(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	int i;
-	int c;
 
 	dss_uninitialize_debugfs();
 
-	venc_exit();
-	dispc_exit();
-	dpi_exit();
-	rfbi_exit();
-	if (cpu_is_omap34xx()) {
-		dsi_exit();
-		sdi_exit();
-	}
-
-	dss_exit();
-
-	/* these should be removed at some point */
-	c = core.dss_ick->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss_ick usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss_ick);
-	}
-
-	c = core.dss1_fck->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss1_fck usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss1_fck);
-	}
-
-	c = core.dss2_fck->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss2_fck usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss2_fck);
-	}
-
-	c = core.dss_54m_fck->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss_54m_fck usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss_54m_fck);
-	}
-
-	if (core.dss_96m_fck) {
-		c = core.dss_96m_fck->usecount;
-		if (c > 0) {
-			DSSERR("warning: dss_96m_fck usecount %d, disabling\n",
-					c);
-			while (c-- > 0)
-				clk_disable(core.dss_96m_fck);
-		}
-	}
-
-	dss_put_clocks();
+	hdmi_uninit_platform_driver();
+	dsi_uninit_platform_driver();
+	venc_uninit_platform_driver();
+	rfbi_uninit_platform_driver();
+	dispc_uninit_platform_driver();
+	dss_uninit_platform_driver();
 
 	dss_uninit_overlays(pdev);
 	dss_uninit_overlay_managers(pdev);
@@ -885,7 +476,7 @@ static void omap_dss_dev_release(struct device *dev)
 	reset_device(dev, 0);
 }
 
-int omap_dss_register_device(struct omap_dss_device *dssdev)
+static int omap_dss_register_device(struct omap_dss_device *dssdev)
 {
 	static int dev_num;
 
@@ -899,7 +490,7 @@ int omap_dss_register_device(struct omap_dss_device *dssdev)
 	return device_register(&dssdev->dev);
 }
 
-void omap_dss_unregister_device(struct omap_dss_device *dssdev)
+static void omap_dss_unregister_device(struct omap_dss_device *dssdev)
 {
 	device_unregister(&dssdev->dev);
 }
@@ -965,11 +556,6 @@ static void __exit omap_dss_exit(void)
 		core.vdds_sdi_reg = NULL;
 	}
 
-	if (core.vdda_dac_reg != NULL) {
-		regulator_put(core.vdda_dac_reg);
-		core.vdda_dac_reg = NULL;
-	}
-
 	platform_driver_unregister(&omap_dss_driver);
 
 	omap_dss_bus_unregister();
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index 9f8c69f16e61..0f3961a1ce26 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -32,104 +32,22 @@
 #include <linux/delay.h>
 #include <linux/workqueue.h>
 #include <linux/hardirq.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 
 #include <plat/sram.h>
 #include <plat/clock.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #include "dss.h"
 #include "dss_features.h"
+#include "dispc.h"
 
 /* DISPC */
-#define DISPC_BASE			0x48050400
-
 #define DISPC_SZ_REGS			SZ_4K
 
-struct dispc_reg { u16 idx; };
-
-#define DISPC_REG(idx)			((const struct dispc_reg) { idx })
-
-/*
- * DISPC common registers and
- * DISPC channel registers , ch = 0 for LCD, ch = 1 for
- * DIGIT, and ch = 2 for LCD2
- */
-#define DISPC_REVISION			DISPC_REG(0x0000)
-#define DISPC_SYSCONFIG			DISPC_REG(0x0010)
-#define DISPC_SYSSTATUS			DISPC_REG(0x0014)
-#define DISPC_IRQSTATUS			DISPC_REG(0x0018)
-#define DISPC_IRQENABLE			DISPC_REG(0x001C)
-#define DISPC_CONTROL			DISPC_REG(0x0040)
-#define DISPC_CONTROL2			DISPC_REG(0x0238)
-#define DISPC_CONFIG			DISPC_REG(0x0044)
-#define DISPC_CONFIG2			DISPC_REG(0x0620)
-#define DISPC_CAPABLE			DISPC_REG(0x0048)
-#define DISPC_DEFAULT_COLOR(ch)		DISPC_REG(ch == 0 ? 0x004C : \
-					(ch == 1 ? 0x0050 : 0x03AC))
-#define DISPC_TRANS_COLOR(ch)		DISPC_REG(ch == 0 ? 0x0054 : \
-					(ch == 1 ? 0x0058 : 0x03B0))
-#define DISPC_LINE_STATUS		DISPC_REG(0x005C)
-#define DISPC_LINE_NUMBER		DISPC_REG(0x0060)
-#define DISPC_TIMING_H(ch)		DISPC_REG(ch != 2 ? 0x0064 : 0x0400)
-#define DISPC_TIMING_V(ch)		DISPC_REG(ch != 2 ? 0x0068 : 0x0404)
-#define DISPC_POL_FREQ(ch)		DISPC_REG(ch != 2 ? 0x006C : 0x0408)
-#define DISPC_DIVISOR(ch)		DISPC_REG(ch != 2 ? 0x0070 : 0x040C)
-#define DISPC_GLOBAL_ALPHA		DISPC_REG(0x0074)
-#define DISPC_SIZE_DIG			DISPC_REG(0x0078)
-#define DISPC_SIZE_LCD(ch)		DISPC_REG(ch != 2 ? 0x007C : 0x03CC)
-
-/* DISPC GFX plane */
-#define DISPC_GFX_BA0			DISPC_REG(0x0080)
-#define DISPC_GFX_BA1			DISPC_REG(0x0084)
-#define DISPC_GFX_POSITION		DISPC_REG(0x0088)
-#define DISPC_GFX_SIZE			DISPC_REG(0x008C)
-#define DISPC_GFX_ATTRIBUTES		DISPC_REG(0x00A0)
-#define DISPC_GFX_FIFO_THRESHOLD	DISPC_REG(0x00A4)
-#define DISPC_GFX_FIFO_SIZE_STATUS	DISPC_REG(0x00A8)
-#define DISPC_GFX_ROW_INC		DISPC_REG(0x00AC)
-#define DISPC_GFX_PIXEL_INC		DISPC_REG(0x00B0)
-#define DISPC_GFX_WINDOW_SKIP		DISPC_REG(0x00B4)
-#define DISPC_GFX_TABLE_BA		DISPC_REG(0x00B8)
-
-#define DISPC_DATA_CYCLE1(ch)		DISPC_REG(ch != 2 ? 0x01D4 : 0x03C0)
-#define DISPC_DATA_CYCLE2(ch)		DISPC_REG(ch != 2 ? 0x01D8 : 0x03C4)
-#define DISPC_DATA_CYCLE3(ch)		DISPC_REG(ch != 2 ? 0x01DC : 0x03C8)
-#define DISPC_CPR_COEF_R(ch)		DISPC_REG(ch != 2 ? 0x0220 : 0x03BC)
-#define DISPC_CPR_COEF_G(ch)		DISPC_REG(ch != 2 ? 0x0224 : 0x03B8)
-#define DISPC_CPR_COEF_B(ch)		DISPC_REG(ch != 2 ? 0x0228 : 0x03B4)
-
-#define DISPC_GFX_PRELOAD		DISPC_REG(0x022C)
-
-/* DISPC Video plane, n = 0 for VID1 and n = 1 for VID2 */
-#define DISPC_VID_REG(n, idx)		DISPC_REG(0x00BC + (n)*0x90 + idx)
-
-#define DISPC_VID_BA0(n)		DISPC_VID_REG(n, 0x0000)
-#define DISPC_VID_BA1(n)		DISPC_VID_REG(n, 0x0004)
-#define DISPC_VID_POSITION(n)		DISPC_VID_REG(n, 0x0008)
-#define DISPC_VID_SIZE(n)		DISPC_VID_REG(n, 0x000C)
-#define DISPC_VID_ATTRIBUTES(n)		DISPC_VID_REG(n, 0x0010)
-#define DISPC_VID_FIFO_THRESHOLD(n)	DISPC_VID_REG(n, 0x0014)
-#define DISPC_VID_FIFO_SIZE_STATUS(n)	DISPC_VID_REG(n, 0x0018)
-#define DISPC_VID_ROW_INC(n)		DISPC_VID_REG(n, 0x001C)
-#define DISPC_VID_PIXEL_INC(n)		DISPC_VID_REG(n, 0x0020)
-#define DISPC_VID_FIR(n)		DISPC_VID_REG(n, 0x0024)
-#define DISPC_VID_PICTURE_SIZE(n)	DISPC_VID_REG(n, 0x0028)
-#define DISPC_VID_ACCU0(n)		DISPC_VID_REG(n, 0x002C)
-#define DISPC_VID_ACCU1(n)		DISPC_VID_REG(n, 0x0030)
-
-/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
-#define DISPC_VID_FIR_COEF_H(n, i)	DISPC_REG(0x00F0 + (n)*0x90 + (i)*0x8)
-/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
-#define DISPC_VID_FIR_COEF_HV(n, i)	DISPC_REG(0x00F4 + (n)*0x90 + (i)*0x8)
-/* coef index i = {0, 1, 2, 3, 4} */
-#define DISPC_VID_CONV_COEF(n, i)	DISPC_REG(0x0130 + (n)*0x90 + (i)*0x4)
-/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
-#define DISPC_VID_FIR_COEF_V(n, i)	DISPC_REG(0x01E0 + (n)*0x20 + (i)*0x4)
-
-#define DISPC_VID_PRELOAD(n)		DISPC_REG(0x230 + (n)*0x04)
-
-
 #define DISPC_IRQ_MASK_ERROR            (DISPC_IRQ_GFX_FIFO_UNDERFLOW | \
 					 DISPC_IRQ_OCP_ERR | \
 					 DISPC_IRQ_VID1_FIFO_UNDERFLOW | \
@@ -161,16 +79,18 @@ struct dispc_v_coef {
 	s8 vc00;
 };
 
+enum omap_burst_size {
+	BURST_SIZE_X2 = 0,
+	BURST_SIZE_X4 = 1,
+	BURST_SIZE_X8 = 2,
+};
+
 #define REG_GET(idx, start, end) \
 	FLD_GET(dispc_read_reg(idx), start, end)
 
 #define REG_FLD_MOD(idx, val, start, end)				\
 	dispc_write_reg(idx, FLD_MOD(dispc_read_reg(idx), val, start, end))
 
-static const struct dispc_reg dispc_reg_att[] = { DISPC_GFX_ATTRIBUTES,
-	DISPC_VID_ATTRIBUTES(0),
-	DISPC_VID_ATTRIBUTES(1) };
-
 struct dispc_irq_stats {
 	unsigned long last_reset;
 	unsigned irq_count;
@@ -178,8 +98,14 @@ struct dispc_irq_stats {
 };
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem    *base;
 
+	int		ctx_loss_cnt;
+
+	int irq;
+	struct clk *dss_clk;
+
 	u32	fifo_size[3];
 
 	spinlock_t irq_lock;
@@ -188,6 +114,7 @@ static struct {
 	u32 error_irqs;
 	struct work_struct error_work;
 
+	bool		ctx_valid;
 	u32		ctx[DISPC_SZ_REGS / sizeof(u32)];
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
@@ -196,341 +123,412 @@ static struct {
 #endif
 } dispc;
 
+enum omap_color_component {
+	/* used for all color formats for OMAP3 and earlier
+	 * and for RGB and Y color component on OMAP4
+	 */
+	DISPC_COLOR_COMPONENT_RGB_Y		= 1 << 0,
+	/* used for UV component for
+	 * OMAP_DSS_COLOR_YUV2, OMAP_DSS_COLOR_UYVY, OMAP_DSS_COLOR_NV12
+	 * color formats on OMAP4
+	 */
+	DISPC_COLOR_COMPONENT_UV		= 1 << 1,
+};
+
 static void _omap_dispc_set_irqs(void);
 
-static inline void dispc_write_reg(const struct dispc_reg idx, u32 val)
+static inline void dispc_write_reg(const u16 idx, u32 val)
+{
+	__raw_writel(val, dispc.base + idx);
+}
+
+static inline u32 dispc_read_reg(const u16 idx)
 {
-	__raw_writel(val, dispc.base + idx.idx);
+	return __raw_readl(dispc.base + idx);
 }
 
-static inline u32 dispc_read_reg(const struct dispc_reg idx)
+static int dispc_get_ctx_loss_count(void)
 {
-	return __raw_readl(dispc.base + idx.idx);
+	struct device *dev = &dispc.pdev->dev;
+	struct omap_display_platform_data *pdata = dev->platform_data;
+	struct omap_dss_board_info *board_data = pdata->board_data;
+	int cnt;
+
+	if (!board_data->get_context_loss_count)
+		return -ENOENT;
+
+	cnt = board_data->get_context_loss_count(dev);
+
+	WARN_ONCE(cnt < 0, "get_context_loss_count failed: %d\n", cnt);
+
+	return cnt;
 }
 
 #define SR(reg) \
-	dispc.ctx[(DISPC_##reg).idx / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
+	dispc.ctx[DISPC_##reg / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
 #define RR(reg) \
-	dispc_write_reg(DISPC_##reg, dispc.ctx[(DISPC_##reg).idx / sizeof(u32)])
+	dispc_write_reg(DISPC_##reg, dispc.ctx[DISPC_##reg / sizeof(u32)])
 
-void dispc_save_context(void)
+static void dispc_save_context(void)
 {
-	if (cpu_is_omap24xx())
-		return;
+	int i;
+
+	DSSDBG("dispc_save_context\n");
 
-	SR(SYSCONFIG);
 	SR(IRQENABLE);
 	SR(CONTROL);
 	SR(CONFIG);
-	SR(DEFAULT_COLOR(0));
-	SR(DEFAULT_COLOR(1));
-	SR(TRANS_COLOR(0));
-	SR(TRANS_COLOR(1));
+	SR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD));
+	SR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_DIGIT));
+	SR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD));
+	SR(TRANS_COLOR(OMAP_DSS_CHANNEL_DIGIT));
 	SR(LINE_NUMBER);
-	SR(TIMING_H(0));
-	SR(TIMING_V(0));
-	SR(POL_FREQ(0));
-	SR(DIVISOR(0));
-	SR(GLOBAL_ALPHA);
-	SR(SIZE_DIG);
-	SR(SIZE_LCD(0));
+	SR(TIMING_H(OMAP_DSS_CHANNEL_LCD));
+	SR(TIMING_V(OMAP_DSS_CHANNEL_LCD));
+	SR(POL_FREQ(OMAP_DSS_CHANNEL_LCD));
+	SR(DIVISORo(OMAP_DSS_CHANNEL_LCD));
+	if (dss_has_feature(FEAT_GLOBAL_ALPHA))
+		SR(GLOBAL_ALPHA);
+	SR(SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT));
+	SR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD));
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
 		SR(CONTROL2);
-		SR(DEFAULT_COLOR(2));
-		SR(TRANS_COLOR(2));
-		SR(SIZE_LCD(2));
-		SR(TIMING_H(2));
-		SR(TIMING_V(2));
-		SR(POL_FREQ(2));
-		SR(DIVISOR(2));
+		SR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD2));
+		SR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD2));
+		SR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD2));
+		SR(TIMING_H(OMAP_DSS_CHANNEL_LCD2));
+		SR(TIMING_V(OMAP_DSS_CHANNEL_LCD2));
+		SR(POL_FREQ(OMAP_DSS_CHANNEL_LCD2));
+		SR(DIVISORo(OMAP_DSS_CHANNEL_LCD2));
 		SR(CONFIG2);
 	}
 
-	SR(GFX_BA0);
-	SR(GFX_BA1);
-	SR(GFX_POSITION);
-	SR(GFX_SIZE);
-	SR(GFX_ATTRIBUTES);
-	SR(GFX_FIFO_THRESHOLD);
-	SR(GFX_ROW_INC);
-	SR(GFX_PIXEL_INC);
-	SR(GFX_WINDOW_SKIP);
-	SR(GFX_TABLE_BA);
-
-	SR(DATA_CYCLE1(0));
-	SR(DATA_CYCLE2(0));
-	SR(DATA_CYCLE3(0));
-
-	SR(CPR_COEF_R(0));
-	SR(CPR_COEF_G(0));
-	SR(CPR_COEF_B(0));
+	SR(OVL_BA0(OMAP_DSS_GFX));
+	SR(OVL_BA1(OMAP_DSS_GFX));
+	SR(OVL_POSITION(OMAP_DSS_GFX));
+	SR(OVL_SIZE(OMAP_DSS_GFX));
+	SR(OVL_ATTRIBUTES(OMAP_DSS_GFX));
+	SR(OVL_FIFO_THRESHOLD(OMAP_DSS_GFX));
+	SR(OVL_ROW_INC(OMAP_DSS_GFX));
+	SR(OVL_PIXEL_INC(OMAP_DSS_GFX));
+	SR(OVL_WINDOW_SKIP(OMAP_DSS_GFX));
+	SR(OVL_TABLE_BA(OMAP_DSS_GFX));
+
+	SR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD));
+	SR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD));
+	SR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD));
+
+	if (dss_has_feature(FEAT_CPR)) {
+		SR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD));
+		SR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD));
+		SR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD));
+	}
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
-		SR(CPR_COEF_B(2));
-		SR(CPR_COEF_G(2));
-		SR(CPR_COEF_R(2));
+		if (dss_has_feature(FEAT_CPR)) {
+			SR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD2));
+			SR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD2));
+			SR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD2));
+		}
 
-		SR(DATA_CYCLE1(2));
-		SR(DATA_CYCLE2(2));
-		SR(DATA_CYCLE3(2));
+		SR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD2));
+		SR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD2));
+		SR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD2));
 	}
 
-	SR(GFX_PRELOAD);
+	if (dss_has_feature(FEAT_PRELOAD))
+		SR(OVL_PRELOAD(OMAP_DSS_GFX));
 
 	/* VID1 */
-	SR(VID_BA0(0));
-	SR(VID_BA1(0));
-	SR(VID_POSITION(0));
-	SR(VID_SIZE(0));
-	SR(VID_ATTRIBUTES(0));
-	SR(VID_FIFO_THRESHOLD(0));
-	SR(VID_ROW_INC(0));
-	SR(VID_PIXEL_INC(0));
-	SR(VID_FIR(0));
-	SR(VID_PICTURE_SIZE(0));
-	SR(VID_ACCU0(0));
-	SR(VID_ACCU1(0));
-
-	SR(VID_FIR_COEF_H(0, 0));
-	SR(VID_FIR_COEF_H(0, 1));
-	SR(VID_FIR_COEF_H(0, 2));
-	SR(VID_FIR_COEF_H(0, 3));
-	SR(VID_FIR_COEF_H(0, 4));
-	SR(VID_FIR_COEF_H(0, 5));
-	SR(VID_FIR_COEF_H(0, 6));
-	SR(VID_FIR_COEF_H(0, 7));
-
-	SR(VID_FIR_COEF_HV(0, 0));
-	SR(VID_FIR_COEF_HV(0, 1));
-	SR(VID_FIR_COEF_HV(0, 2));
-	SR(VID_FIR_COEF_HV(0, 3));
-	SR(VID_FIR_COEF_HV(0, 4));
-	SR(VID_FIR_COEF_HV(0, 5));
-	SR(VID_FIR_COEF_HV(0, 6));
-	SR(VID_FIR_COEF_HV(0, 7));
-
-	SR(VID_CONV_COEF(0, 0));
-	SR(VID_CONV_COEF(0, 1));
-	SR(VID_CONV_COEF(0, 2));
-	SR(VID_CONV_COEF(0, 3));
-	SR(VID_CONV_COEF(0, 4));
-
-	SR(VID_FIR_COEF_V(0, 0));
-	SR(VID_FIR_COEF_V(0, 1));
-	SR(VID_FIR_COEF_V(0, 2));
-	SR(VID_FIR_COEF_V(0, 3));
-	SR(VID_FIR_COEF_V(0, 4));
-	SR(VID_FIR_COEF_V(0, 5));
-	SR(VID_FIR_COEF_V(0, 6));
-	SR(VID_FIR_COEF_V(0, 7));
-
-	SR(VID_PRELOAD(0));
+	SR(OVL_BA0(OMAP_DSS_VIDEO1));
+	SR(OVL_BA1(OMAP_DSS_VIDEO1));
+	SR(OVL_POSITION(OMAP_DSS_VIDEO1));
+	SR(OVL_SIZE(OMAP_DSS_VIDEO1));
+	SR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO1));
+	SR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO1));
+	SR(OVL_ROW_INC(OMAP_DSS_VIDEO1));
+	SR(OVL_PIXEL_INC(OMAP_DSS_VIDEO1));
+	SR(OVL_FIR(OMAP_DSS_VIDEO1));
+	SR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO1));
+	SR(OVL_ACCU0(OMAP_DSS_VIDEO1));
+	SR(OVL_ACCU1(OMAP_DSS_VIDEO1));
+
+	for (i = 0; i < 8; i++)
+		SR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, i));
+
+	for (i = 0; i < 8; i++)
+		SR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, i));
+
+	for (i = 0; i < 5; i++)
+		SR(OVL_CONV_COEF(OMAP_DSS_VIDEO1, i));
+
+	if (dss_has_feature(FEAT_FIR_COEF_V)) {
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, i));
+	}
+
+	if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+		SR(OVL_BA0_UV(OMAP_DSS_VIDEO1));
+		SR(OVL_BA1_UV(OMAP_DSS_VIDEO1));
+		SR(OVL_FIR2(OMAP_DSS_VIDEO1));
+		SR(OVL_ACCU2_0(OMAP_DSS_VIDEO1));
+		SR(OVL_ACCU2_1(OMAP_DSS_VIDEO1));
+
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, i));
+
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, i));
+
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, i));
+	}
+	if (dss_has_feature(FEAT_ATTR2))
+		SR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO1));
+
+	if (dss_has_feature(FEAT_PRELOAD))
+		SR(OVL_PRELOAD(OMAP_DSS_VIDEO1));
 
 	/* VID2 */
-	SR(VID_BA0(1));
-	SR(VID_BA1(1));
-	SR(VID_POSITION(1));
-	SR(VID_SIZE(1));
-	SR(VID_ATTRIBUTES(1));
-	SR(VID_FIFO_THRESHOLD(1));
-	SR(VID_ROW_INC(1));
-	SR(VID_PIXEL_INC(1));
-	SR(VID_FIR(1));
-	SR(VID_PICTURE_SIZE(1));
-	SR(VID_ACCU0(1));
-	SR(VID_ACCU1(1));
-
-	SR(VID_FIR_COEF_H(1, 0));
-	SR(VID_FIR_COEF_H(1, 1));
-	SR(VID_FIR_COEF_H(1, 2));
-	SR(VID_FIR_COEF_H(1, 3));
-	SR(VID_FIR_COEF_H(1, 4));
-	SR(VID_FIR_COEF_H(1, 5));
-	SR(VID_FIR_COEF_H(1, 6));
-	SR(VID_FIR_COEF_H(1, 7));
-
-	SR(VID_FIR_COEF_HV(1, 0));
-	SR(VID_FIR_COEF_HV(1, 1));
-	SR(VID_FIR_COEF_HV(1, 2));
-	SR(VID_FIR_COEF_HV(1, 3));
-	SR(VID_FIR_COEF_HV(1, 4));
-	SR(VID_FIR_COEF_HV(1, 5));
-	SR(VID_FIR_COEF_HV(1, 6));
-	SR(VID_FIR_COEF_HV(1, 7));
-
-	SR(VID_CONV_COEF(1, 0));
-	SR(VID_CONV_COEF(1, 1));
-	SR(VID_CONV_COEF(1, 2));
-	SR(VID_CONV_COEF(1, 3));
-	SR(VID_CONV_COEF(1, 4));
-
-	SR(VID_FIR_COEF_V(1, 0));
-	SR(VID_FIR_COEF_V(1, 1));
-	SR(VID_FIR_COEF_V(1, 2));
-	SR(VID_FIR_COEF_V(1, 3));
-	SR(VID_FIR_COEF_V(1, 4));
-	SR(VID_FIR_COEF_V(1, 5));
-	SR(VID_FIR_COEF_V(1, 6));
-	SR(VID_FIR_COEF_V(1, 7));
-
-	SR(VID_PRELOAD(1));
-}
-
-void dispc_restore_context(void)
-{
-	RR(SYSCONFIG);
+	SR(OVL_BA0(OMAP_DSS_VIDEO2));
+	SR(OVL_BA1(OMAP_DSS_VIDEO2));
+	SR(OVL_POSITION(OMAP_DSS_VIDEO2));
+	SR(OVL_SIZE(OMAP_DSS_VIDEO2));
+	SR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO2));
+	SR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO2));
+	SR(OVL_ROW_INC(OMAP_DSS_VIDEO2));
+	SR(OVL_PIXEL_INC(OMAP_DSS_VIDEO2));
+	SR(OVL_FIR(OMAP_DSS_VIDEO2));
+	SR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO2));
+	SR(OVL_ACCU0(OMAP_DSS_VIDEO2));
+	SR(OVL_ACCU1(OMAP_DSS_VIDEO2));
+
+	for (i = 0; i < 8; i++)
+		SR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, i));
+
+	for (i = 0; i < 8; i++)
+		SR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, i));
+
+	for (i = 0; i < 5; i++)
+		SR(OVL_CONV_COEF(OMAP_DSS_VIDEO2, i));
+
+	if (dss_has_feature(FEAT_FIR_COEF_V)) {
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, i));
+	}
+
+	if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+		SR(OVL_BA0_UV(OMAP_DSS_VIDEO2));
+		SR(OVL_BA1_UV(OMAP_DSS_VIDEO2));
+		SR(OVL_FIR2(OMAP_DSS_VIDEO2));
+		SR(OVL_ACCU2_0(OMAP_DSS_VIDEO2));
+		SR(OVL_ACCU2_1(OMAP_DSS_VIDEO2));
+
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, i));
+
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, i));
+
+		for (i = 0; i < 8; i++)
+			SR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, i));
+	}
+	if (dss_has_feature(FEAT_ATTR2))
+		SR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO2));
+
+	if (dss_has_feature(FEAT_PRELOAD))
+		SR(OVL_PRELOAD(OMAP_DSS_VIDEO2));
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV))
+		SR(DIVISOR);
+
+	dispc.ctx_loss_cnt = dispc_get_ctx_loss_count();
+	dispc.ctx_valid = true;
+
+	DSSDBG("context saved, ctx_loss_count %d\n", dispc.ctx_loss_cnt);
+}
+
+static void dispc_restore_context(void)
+{
+	int i, ctx;
+
+	DSSDBG("dispc_restore_context\n");
+
+	if (!dispc.ctx_valid)
+		return;
+
+	ctx = dispc_get_ctx_loss_count();
+
+	if (ctx >= 0 && ctx == dispc.ctx_loss_cnt)
+		return;
+
+	DSSDBG("ctx_loss_count: saved %d, current %d\n",
+			dispc.ctx_loss_cnt, ctx);
+
 	/*RR(IRQENABLE);*/
 	/*RR(CONTROL);*/
 	RR(CONFIG);
-	RR(DEFAULT_COLOR(0));
-	RR(DEFAULT_COLOR(1));
-	RR(TRANS_COLOR(0));
-	RR(TRANS_COLOR(1));
+	RR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD));
+	RR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_DIGIT));
+	RR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD));
+	RR(TRANS_COLOR(OMAP_DSS_CHANNEL_DIGIT));
 	RR(LINE_NUMBER);
-	RR(TIMING_H(0));
-	RR(TIMING_V(0));
-	RR(POL_FREQ(0));
-	RR(DIVISOR(0));
-	RR(GLOBAL_ALPHA);
-	RR(SIZE_DIG);
-	RR(SIZE_LCD(0));
+	RR(TIMING_H(OMAP_DSS_CHANNEL_LCD));
+	RR(TIMING_V(OMAP_DSS_CHANNEL_LCD));
+	RR(POL_FREQ(OMAP_DSS_CHANNEL_LCD));
+	RR(DIVISORo(OMAP_DSS_CHANNEL_LCD));
+	if (dss_has_feature(FEAT_GLOBAL_ALPHA))
+		RR(GLOBAL_ALPHA);
+	RR(SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT));
+	RR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD));
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
-		RR(DEFAULT_COLOR(2));
-		RR(TRANS_COLOR(2));
-		RR(SIZE_LCD(2));
-		RR(TIMING_H(2));
-		RR(TIMING_V(2));
-		RR(POL_FREQ(2));
-		RR(DIVISOR(2));
+		RR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD2));
+		RR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD2));
+		RR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD2));
+		RR(TIMING_H(OMAP_DSS_CHANNEL_LCD2));
+		RR(TIMING_V(OMAP_DSS_CHANNEL_LCD2));
+		RR(POL_FREQ(OMAP_DSS_CHANNEL_LCD2));
+		RR(DIVISORo(OMAP_DSS_CHANNEL_LCD2));
 		RR(CONFIG2);
 	}
 
-	RR(GFX_BA0);
-	RR(GFX_BA1);
-	RR(GFX_POSITION);
-	RR(GFX_SIZE);
-	RR(GFX_ATTRIBUTES);
-	RR(GFX_FIFO_THRESHOLD);
-	RR(GFX_ROW_INC);
-	RR(GFX_PIXEL_INC);
-	RR(GFX_WINDOW_SKIP);
-	RR(GFX_TABLE_BA);
-
-	RR(DATA_CYCLE1(0));
-	RR(DATA_CYCLE2(0));
-	RR(DATA_CYCLE3(0));
-
-	RR(CPR_COEF_R(0));
-	RR(CPR_COEF_G(0));
-	RR(CPR_COEF_B(0));
+	RR(OVL_BA0(OMAP_DSS_GFX));
+	RR(OVL_BA1(OMAP_DSS_GFX));
+	RR(OVL_POSITION(OMAP_DSS_GFX));
+	RR(OVL_SIZE(OMAP_DSS_GFX));
+	RR(OVL_ATTRIBUTES(OMAP_DSS_GFX));
+	RR(OVL_FIFO_THRESHOLD(OMAP_DSS_GFX));
+	RR(OVL_ROW_INC(OMAP_DSS_GFX));
+	RR(OVL_PIXEL_INC(OMAP_DSS_GFX));
+	RR(OVL_WINDOW_SKIP(OMAP_DSS_GFX));
+	RR(OVL_TABLE_BA(OMAP_DSS_GFX));
+
+
+	RR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD));
+	RR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD));
+	RR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD));
+
+	if (dss_has_feature(FEAT_CPR)) {
+		RR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD));
+		RR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD));
+		RR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD));
+	}
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
-		RR(DATA_CYCLE1(2));
-		RR(DATA_CYCLE2(2));
-		RR(DATA_CYCLE3(2));
-
-		RR(CPR_COEF_B(2));
-		RR(CPR_COEF_G(2));
-		RR(CPR_COEF_R(2));
+		RR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD2));
+		RR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD2));
+		RR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD2));
+
+		if (dss_has_feature(FEAT_CPR)) {
+			RR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD2));
+			RR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD2));
+			RR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD2));
+		}
 	}
 
-	RR(GFX_PRELOAD);
+	if (dss_has_feature(FEAT_PRELOAD))
+		RR(OVL_PRELOAD(OMAP_DSS_GFX));
 
 	/* VID1 */
-	RR(VID_BA0(0));
-	RR(VID_BA1(0));
-	RR(VID_POSITION(0));
-	RR(VID_SIZE(0));
-	RR(VID_ATTRIBUTES(0));
-	RR(VID_FIFO_THRESHOLD(0));
-	RR(VID_ROW_INC(0));
-	RR(VID_PIXEL_INC(0));
-	RR(VID_FIR(0));
-	RR(VID_PICTURE_SIZE(0));
-	RR(VID_ACCU0(0));
-	RR(VID_ACCU1(0));
-
-	RR(VID_FIR_COEF_H(0, 0));
-	RR(VID_FIR_COEF_H(0, 1));
-	RR(VID_FIR_COEF_H(0, 2));
-	RR(VID_FIR_COEF_H(0, 3));
-	RR(VID_FIR_COEF_H(0, 4));
-	RR(VID_FIR_COEF_H(0, 5));
-	RR(VID_FIR_COEF_H(0, 6));
-	RR(VID_FIR_COEF_H(0, 7));
-
-	RR(VID_FIR_COEF_HV(0, 0));
-	RR(VID_FIR_COEF_HV(0, 1));
-	RR(VID_FIR_COEF_HV(0, 2));
-	RR(VID_FIR_COEF_HV(0, 3));
-	RR(VID_FIR_COEF_HV(0, 4));
-	RR(VID_FIR_COEF_HV(0, 5));
-	RR(VID_FIR_COEF_HV(0, 6));
-	RR(VID_FIR_COEF_HV(0, 7));
-
-	RR(VID_CONV_COEF(0, 0));
-	RR(VID_CONV_COEF(0, 1));
-	RR(VID_CONV_COEF(0, 2));
-	RR(VID_CONV_COEF(0, 3));
-	RR(VID_CONV_COEF(0, 4));
-
-	RR(VID_FIR_COEF_V(0, 0));
-	RR(VID_FIR_COEF_V(0, 1));
-	RR(VID_FIR_COEF_V(0, 2));
-	RR(VID_FIR_COEF_V(0, 3));
-	RR(VID_FIR_COEF_V(0, 4));
-	RR(VID_FIR_COEF_V(0, 5));
-	RR(VID_FIR_COEF_V(0, 6));
-	RR(VID_FIR_COEF_V(0, 7));
-
-	RR(VID_PRELOAD(0));
+	RR(OVL_BA0(OMAP_DSS_VIDEO1));
+	RR(OVL_BA1(OMAP_DSS_VIDEO1));
+	RR(OVL_POSITION(OMAP_DSS_VIDEO1));
+	RR(OVL_SIZE(OMAP_DSS_VIDEO1));
+	RR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO1));
+	RR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO1));
+	RR(OVL_ROW_INC(OMAP_DSS_VIDEO1));
+	RR(OVL_PIXEL_INC(OMAP_DSS_VIDEO1));
+	RR(OVL_FIR(OMAP_DSS_VIDEO1));
+	RR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO1));
+	RR(OVL_ACCU0(OMAP_DSS_VIDEO1));
+	RR(OVL_ACCU1(OMAP_DSS_VIDEO1));
+
+	for (i = 0; i < 8; i++)
+		RR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, i));
+
+	for (i = 0; i < 8; i++)
+		RR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, i));
+
+	for (i = 0; i < 5; i++)
+		RR(OVL_CONV_COEF(OMAP_DSS_VIDEO1, i));
+
+	if (dss_has_feature(FEAT_FIR_COEF_V)) {
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, i));
+	}
+
+	if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+		RR(OVL_BA0_UV(OMAP_DSS_VIDEO1));
+		RR(OVL_BA1_UV(OMAP_DSS_VIDEO1));
+		RR(OVL_FIR2(OMAP_DSS_VIDEO1));
+		RR(OVL_ACCU2_0(OMAP_DSS_VIDEO1));
+		RR(OVL_ACCU2_1(OMAP_DSS_VIDEO1));
+
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, i));
+
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, i));
+
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, i));
+	}
+	if (dss_has_feature(FEAT_ATTR2))
+		RR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO1));
+
+	if (dss_has_feature(FEAT_PRELOAD))
+		RR(OVL_PRELOAD(OMAP_DSS_VIDEO1));
 
 	/* VID2 */
-	RR(VID_BA0(1));
-	RR(VID_BA1(1));
-	RR(VID_POSITION(1));
-	RR(VID_SIZE(1));
-	RR(VID_ATTRIBUTES(1));
-	RR(VID_FIFO_THRESHOLD(1));
-	RR(VID_ROW_INC(1));
-	RR(VID_PIXEL_INC(1));
-	RR(VID_FIR(1));
-	RR(VID_PICTURE_SIZE(1));
-	RR(VID_ACCU0(1));
-	RR(VID_ACCU1(1));
-
-	RR(VID_FIR_COEF_H(1, 0));
-	RR(VID_FIR_COEF_H(1, 1));
-	RR(VID_FIR_COEF_H(1, 2));
-	RR(VID_FIR_COEF_H(1, 3));
-	RR(VID_FIR_COEF_H(1, 4));
-	RR(VID_FIR_COEF_H(1, 5));
-	RR(VID_FIR_COEF_H(1, 6));
-	RR(VID_FIR_COEF_H(1, 7));
-
-	RR(VID_FIR_COEF_HV(1, 0));
-	RR(VID_FIR_COEF_HV(1, 1));
-	RR(VID_FIR_COEF_HV(1, 2));
-	RR(VID_FIR_COEF_HV(1, 3));
-	RR(VID_FIR_COEF_HV(1, 4));
-	RR(VID_FIR_COEF_HV(1, 5));
-	RR(VID_FIR_COEF_HV(1, 6));
-	RR(VID_FIR_COEF_HV(1, 7));
-
-	RR(VID_CONV_COEF(1, 0));
-	RR(VID_CONV_COEF(1, 1));
-	RR(VID_CONV_COEF(1, 2));
-	RR(VID_CONV_COEF(1, 3));
-	RR(VID_CONV_COEF(1, 4));
-
-	RR(VID_FIR_COEF_V(1, 0));
-	RR(VID_FIR_COEF_V(1, 1));
-	RR(VID_FIR_COEF_V(1, 2));
-	RR(VID_FIR_COEF_V(1, 3));
-	RR(VID_FIR_COEF_V(1, 4));
-	RR(VID_FIR_COEF_V(1, 5));
-	RR(VID_FIR_COEF_V(1, 6));
-	RR(VID_FIR_COEF_V(1, 7));
-
-	RR(VID_PRELOAD(1));
+	RR(OVL_BA0(OMAP_DSS_VIDEO2));
+	RR(OVL_BA1(OMAP_DSS_VIDEO2));
+	RR(OVL_POSITION(OMAP_DSS_VIDEO2));
+	RR(OVL_SIZE(OMAP_DSS_VIDEO2));
+	RR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO2));
+	RR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO2));
+	RR(OVL_ROW_INC(OMAP_DSS_VIDEO2));
+	RR(OVL_PIXEL_INC(OMAP_DSS_VIDEO2));
+	RR(OVL_FIR(OMAP_DSS_VIDEO2));
+	RR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO2));
+	RR(OVL_ACCU0(OMAP_DSS_VIDEO2));
+	RR(OVL_ACCU1(OMAP_DSS_VIDEO2));
+
+	for (i = 0; i < 8; i++)
+		RR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, i));
+
+	for (i = 0; i < 8; i++)
+		RR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, i));
+
+	for (i = 0; i < 5; i++)
+		RR(OVL_CONV_COEF(OMAP_DSS_VIDEO2, i));
+
+	if (dss_has_feature(FEAT_FIR_COEF_V)) {
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, i));
+	}
+
+	if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+		RR(OVL_BA0_UV(OMAP_DSS_VIDEO2));
+		RR(OVL_BA1_UV(OMAP_DSS_VIDEO2));
+		RR(OVL_FIR2(OMAP_DSS_VIDEO2));
+		RR(OVL_ACCU2_0(OMAP_DSS_VIDEO2));
+		RR(OVL_ACCU2_1(OMAP_DSS_VIDEO2));
+
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, i));
+
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, i));
+
+		for (i = 0; i < 8; i++)
+			RR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, i));
+	}
+	if (dss_has_feature(FEAT_ATTR2))
+		RR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO2));
+
+	if (dss_has_feature(FEAT_PRELOAD))
+		RR(OVL_PRELOAD(OMAP_DSS_VIDEO2));
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV))
+		RR(DIVISOR);
 
 	/* enable last, because LCD & DIGIT enable are here */
 	RR(CONTROL);
@@ -544,19 +542,35 @@ void dispc_restore_context(void)
 	 * the context is fully restored
 	 */
 	RR(IRQENABLE);
+
+	DSSDBG("context restored\n");
 }
 
 #undef SR
 #undef RR
 
-static inline void enable_clocks(bool enable)
+int dispc_runtime_get(void)
 {
-	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	int r;
+
+	DSSDBG("dispc_runtime_get\n");
+
+	r = pm_runtime_get_sync(&dispc.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+void dispc_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("dispc_runtime_put\n");
+
+	r = pm_runtime_put(&dispc.pdev->dev);
+	WARN_ON(r < 0);
 }
 
+
 bool dispc_go_busy(enum omap_channel channel)
 {
 	int bit;
@@ -578,8 +592,6 @@ void dispc_go(enum omap_channel channel)
 	int bit;
 	bool enable_bit, go_bit;
 
-	enable_clocks(1);
-
 	if (channel == OMAP_DSS_CHANNEL_LCD ||
 			channel == OMAP_DSS_CHANNEL_LCD2)
 		bit = 0; /* LCDENABLE */
@@ -593,7 +605,7 @@ void dispc_go(enum omap_channel channel)
 		enable_bit = REG_GET(DISPC_CONTROL, bit, bit) == 1;
 
 	if (!enable_bit)
-		goto end;
+		return;
 
 	if (channel == OMAP_DSS_CHANNEL_LCD ||
 			channel == OMAP_DSS_CHANNEL_LCD2)
@@ -608,7 +620,7 @@ void dispc_go(enum omap_channel channel)
 
 	if (go_bit) {
 		DSSERR("GO bit not down for channel %d\n", channel);
-		goto end;
+		return;
 	}
 
 	DSSDBG("GO %s\n", channel == OMAP_DSS_CHANNEL_LCD ? "LCD" :
@@ -618,33 +630,47 @@ void dispc_go(enum omap_channel channel)
 		REG_FLD_MOD(DISPC_CONTROL2, 1, bit, bit);
 	else
 		REG_FLD_MOD(DISPC_CONTROL, 1, bit, bit);
-end:
-	enable_clocks(0);
 }
 
 static void _dispc_write_firh_reg(enum omap_plane plane, int reg, u32 value)
 {
+	dispc_write_reg(DISPC_OVL_FIR_COEF_H(plane, reg), value);
+}
+
+static void _dispc_write_firhv_reg(enum omap_plane plane, int reg, u32 value)
+{
+	dispc_write_reg(DISPC_OVL_FIR_COEF_HV(plane, reg), value);
+}
+
+static void _dispc_write_firv_reg(enum omap_plane plane, int reg, u32 value)
+{
+	dispc_write_reg(DISPC_OVL_FIR_COEF_V(plane, reg), value);
+}
+
+static void _dispc_write_firh2_reg(enum omap_plane plane, int reg, u32 value)
+{
 	BUG_ON(plane == OMAP_DSS_GFX);
 
-	dispc_write_reg(DISPC_VID_FIR_COEF_H(plane-1, reg), value);
+	dispc_write_reg(DISPC_OVL_FIR_COEF_H2(plane, reg), value);
 }
 
-static void _dispc_write_firhv_reg(enum omap_plane plane, int reg, u32 value)
+static void _dispc_write_firhv2_reg(enum omap_plane plane, int reg, u32 value)
 {
 	BUG_ON(plane == OMAP_DSS_GFX);
 
-	dispc_write_reg(DISPC_VID_FIR_COEF_HV(plane-1, reg), value);
+	dispc_write_reg(DISPC_OVL_FIR_COEF_HV2(plane, reg), value);
 }
 
-static void _dispc_write_firv_reg(enum omap_plane plane, int reg, u32 value)
+static void _dispc_write_firv2_reg(enum omap_plane plane, int reg, u32 value)
 {
 	BUG_ON(plane == OMAP_DSS_GFX);
 
-	dispc_write_reg(DISPC_VID_FIR_COEF_V(plane-1, reg), value);
+	dispc_write_reg(DISPC_OVL_FIR_COEF_V2(plane, reg), value);
 }
 
 static void _dispc_set_scale_coef(enum omap_plane plane, int hscaleup,
-		int vscaleup, int five_taps)
+				  int vscaleup, int five_taps,
+				  enum omap_color_component color_comp)
 {
 	/* Coefficients for horizontal up-sampling */
 	static const struct dispc_h_coef coef_hup[8] = {
@@ -742,8 +768,14 @@ static void _dispc_set_scale_coef(enum omap_plane plane, int hscaleup,
 			| FLD_VAL(v_coef[i].vc1, 23, 16)
 			| FLD_VAL(v_coef[i].vc2, 31, 24);
 
-		_dispc_write_firh_reg(plane, i, h);
-		_dispc_write_firhv_reg(plane, i, hv);
+		if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+			_dispc_write_firh_reg(plane, i, h);
+			_dispc_write_firhv_reg(plane, i, hv);
+		} else {
+			_dispc_write_firh2_reg(plane, i, h);
+			_dispc_write_firhv2_reg(plane, i, hv);
+		}
+
 	}
 
 	if (five_taps) {
@@ -751,7 +783,10 @@ static void _dispc_set_scale_coef(enum omap_plane plane, int hscaleup,
 			u32 v;
 			v = FLD_VAL(v_coef[i].vc00, 7, 0)
 				| FLD_VAL(v_coef[i].vc22, 15, 8);
-			_dispc_write_firv_reg(plane, i, v);
+			if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y)
+				_dispc_write_firv_reg(plane, i, v);
+			else
+				_dispc_write_firv2_reg(plane, i, v);
 		}
 	}
 }
@@ -771,72 +806,83 @@ static void _dispc_setup_color_conv_coef(void)
 
 	ct = &ctbl_bt601_5;
 
-	dispc_write_reg(DISPC_VID_CONV_COEF(0, 0), CVAL(ct->rcr, ct->ry));
-	dispc_write_reg(DISPC_VID_CONV_COEF(0, 1), CVAL(ct->gy,	 ct->rcb));
-	dispc_write_reg(DISPC_VID_CONV_COEF(0, 2), CVAL(ct->gcb, ct->gcr));
-	dispc_write_reg(DISPC_VID_CONV_COEF(0, 3), CVAL(ct->bcr, ct->by));
-	dispc_write_reg(DISPC_VID_CONV_COEF(0, 4), CVAL(0,       ct->bcb));
-
-	dispc_write_reg(DISPC_VID_CONV_COEF(1, 0), CVAL(ct->rcr, ct->ry));
-	dispc_write_reg(DISPC_VID_CONV_COEF(1, 1), CVAL(ct->gy,	 ct->rcb));
-	dispc_write_reg(DISPC_VID_CONV_COEF(1, 2), CVAL(ct->gcb, ct->gcr));
-	dispc_write_reg(DISPC_VID_CONV_COEF(1, 3), CVAL(ct->bcr, ct->by));
-	dispc_write_reg(DISPC_VID_CONV_COEF(1, 4), CVAL(0,       ct->bcb));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 0),
+		CVAL(ct->rcr, ct->ry));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 1),
+		CVAL(ct->gy,  ct->rcb));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 2),
+		CVAL(ct->gcb, ct->gcr));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 3),
+		CVAL(ct->bcr, ct->by));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 4),
+		CVAL(0, ct->bcb));
+
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 0),
+		CVAL(ct->rcr, ct->ry));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 1),
+		CVAL(ct->gy, ct->rcb));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 2),
+		CVAL(ct->gcb, ct->gcr));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 3),
+		CVAL(ct->bcr, ct->by));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 4),
+		CVAL(0, ct->bcb));
 
 #undef CVAL
 
-	REG_FLD_MOD(DISPC_VID_ATTRIBUTES(0), ct->full_range, 11, 11);
-	REG_FLD_MOD(DISPC_VID_ATTRIBUTES(1), ct->full_range, 11, 11);
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO1),
+		ct->full_range, 11, 11);
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO2),
+		ct->full_range, 11, 11);
 }
 
 
 static void _dispc_set_plane_ba0(enum omap_plane plane, u32 paddr)
 {
-	const struct dispc_reg ba0_reg[] = { DISPC_GFX_BA0,
-		DISPC_VID_BA0(0),
-		DISPC_VID_BA0(1) };
-
-	dispc_write_reg(ba0_reg[plane], paddr);
+	dispc_write_reg(DISPC_OVL_BA0(plane), paddr);
 }
 
 static void _dispc_set_plane_ba1(enum omap_plane plane, u32 paddr)
 {
-	const struct dispc_reg ba1_reg[] = { DISPC_GFX_BA1,
-				      DISPC_VID_BA1(0),
-				      DISPC_VID_BA1(1) };
+	dispc_write_reg(DISPC_OVL_BA1(plane), paddr);
+}
 
-	dispc_write_reg(ba1_reg[plane], paddr);
+static void _dispc_set_plane_ba0_uv(enum omap_plane plane, u32 paddr)
+{
+	dispc_write_reg(DISPC_OVL_BA0_UV(plane), paddr);
 }
 
-static void _dispc_set_plane_pos(enum omap_plane plane, int x, int y)
+static void _dispc_set_plane_ba1_uv(enum omap_plane plane, u32 paddr)
 {
-	const struct dispc_reg pos_reg[] = { DISPC_GFX_POSITION,
-				      DISPC_VID_POSITION(0),
-				      DISPC_VID_POSITION(1) };
+	dispc_write_reg(DISPC_OVL_BA1_UV(plane), paddr);
+}
 
+static void _dispc_set_plane_pos(enum omap_plane plane, int x, int y)
+{
 	u32 val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
-	dispc_write_reg(pos_reg[plane], val);
+
+	dispc_write_reg(DISPC_OVL_POSITION(plane), val);
 }
 
 static void _dispc_set_pic_size(enum omap_plane plane, int width, int height)
 {
-	const struct dispc_reg siz_reg[] = { DISPC_GFX_SIZE,
-				      DISPC_VID_PICTURE_SIZE(0),
-				      DISPC_VID_PICTURE_SIZE(1) };
 	u32 val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
-	dispc_write_reg(siz_reg[plane], val);
+
+	if (plane == OMAP_DSS_GFX)
+		dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+	else
+		dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
 }
 
 static void _dispc_set_vid_size(enum omap_plane plane, int width, int height)
 {
 	u32 val;
-	const struct dispc_reg vsi_reg[] = { DISPC_VID_SIZE(0),
-				      DISPC_VID_SIZE(1) };
 
 	BUG_ON(plane == OMAP_DSS_GFX);
 
 	val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
-	dispc_write_reg(vsi_reg[plane-1], val);
+
+	dispc_write_reg(DISPC_OVL_SIZE(plane), val);
 }
 
 static void _dispc_set_pre_mult_alpha(enum omap_plane plane, bool enable)
@@ -848,7 +894,7 @@ static void _dispc_set_pre_mult_alpha(enum omap_plane plane, bool enable)
 		plane == OMAP_DSS_VIDEO1)
 		return;
 
-	REG_FLD_MOD(dispc_reg_att[plane], enable ? 1 : 0, 28, 28);
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 28, 28);
 }
 
 static void _dispc_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
@@ -868,64 +914,96 @@ static void _dispc_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
 
 static void _dispc_set_pix_inc(enum omap_plane plane, s32 inc)
 {
-	const struct dispc_reg ri_reg[] = { DISPC_GFX_PIXEL_INC,
-				     DISPC_VID_PIXEL_INC(0),
-				     DISPC_VID_PIXEL_INC(1) };
-
-	dispc_write_reg(ri_reg[plane], inc);
+	dispc_write_reg(DISPC_OVL_PIXEL_INC(plane), inc);
 }
 
 static void _dispc_set_row_inc(enum omap_plane plane, s32 inc)
 {
-	const struct dispc_reg ri_reg[] = { DISPC_GFX_ROW_INC,
-				     DISPC_VID_ROW_INC(0),
-				     DISPC_VID_ROW_INC(1) };
-
-	dispc_write_reg(ri_reg[plane], inc);
+	dispc_write_reg(DISPC_OVL_ROW_INC(plane), inc);
 }
 
 static void _dispc_set_color_mode(enum omap_plane plane,
 		enum omap_color_mode color_mode)
 {
 	u32 m = 0;
-
-	switch (color_mode) {
-	case OMAP_DSS_COLOR_CLUT1:
-		m = 0x0; break;
-	case OMAP_DSS_COLOR_CLUT2:
-		m = 0x1; break;
-	case OMAP_DSS_COLOR_CLUT4:
-		m = 0x2; break;
-	case OMAP_DSS_COLOR_CLUT8:
-		m = 0x3; break;
-	case OMAP_DSS_COLOR_RGB12U:
-		m = 0x4; break;
-	case OMAP_DSS_COLOR_ARGB16:
-		m = 0x5; break;
-	case OMAP_DSS_COLOR_RGB16:
-		m = 0x6; break;
-	case OMAP_DSS_COLOR_RGB24U:
-		m = 0x8; break;
-	case OMAP_DSS_COLOR_RGB24P:
-		m = 0x9; break;
-	case OMAP_DSS_COLOR_YUV2:
-		m = 0xa; break;
-	case OMAP_DSS_COLOR_UYVY:
-		m = 0xb; break;
-	case OMAP_DSS_COLOR_ARGB32:
-		m = 0xc; break;
-	case OMAP_DSS_COLOR_RGBA32:
-		m = 0xd; break;
-	case OMAP_DSS_COLOR_RGBX32:
-		m = 0xe; break;
-	default:
-		BUG(); break;
+	if (plane != OMAP_DSS_GFX) {
+		switch (color_mode) {
+		case OMAP_DSS_COLOR_NV12:
+			m = 0x0; break;
+		case OMAP_DSS_COLOR_RGB12U:
+			m = 0x1; break;
+		case OMAP_DSS_COLOR_RGBA16:
+			m = 0x2; break;
+		case OMAP_DSS_COLOR_RGBX16:
+			m = 0x4; break;
+		case OMAP_DSS_COLOR_ARGB16:
+			m = 0x5; break;
+		case OMAP_DSS_COLOR_RGB16:
+			m = 0x6; break;
+		case OMAP_DSS_COLOR_ARGB16_1555:
+			m = 0x7; break;
+		case OMAP_DSS_COLOR_RGB24U:
+			m = 0x8; break;
+		case OMAP_DSS_COLOR_RGB24P:
+			m = 0x9; break;
+		case OMAP_DSS_COLOR_YUV2:
+			m = 0xa; break;
+		case OMAP_DSS_COLOR_UYVY:
+			m = 0xb; break;
+		case OMAP_DSS_COLOR_ARGB32:
+			m = 0xc; break;
+		case OMAP_DSS_COLOR_RGBA32:
+			m = 0xd; break;
+		case OMAP_DSS_COLOR_RGBX32:
+			m = 0xe; break;
+		case OMAP_DSS_COLOR_XRGB16_1555:
+			m = 0xf; break;
+		default:
+			BUG(); break;
+		}
+	} else {
+		switch (color_mode) {
+		case OMAP_DSS_COLOR_CLUT1:
+			m = 0x0; break;
+		case OMAP_DSS_COLOR_CLUT2:
+			m = 0x1; break;
+		case OMAP_DSS_COLOR_CLUT4:
+			m = 0x2; break;
+		case OMAP_DSS_COLOR_CLUT8:
+			m = 0x3; break;
+		case OMAP_DSS_COLOR_RGB12U:
+			m = 0x4; break;
+		case OMAP_DSS_COLOR_ARGB16:
+			m = 0x5; break;
+		case OMAP_DSS_COLOR_RGB16:
+			m = 0x6; break;
+		case OMAP_DSS_COLOR_ARGB16_1555:
+			m = 0x7; break;
+		case OMAP_DSS_COLOR_RGB24U:
+			m = 0x8; break;
+		case OMAP_DSS_COLOR_RGB24P:
+			m = 0x9; break;
+		case OMAP_DSS_COLOR_YUV2:
+			m = 0xa; break;
+		case OMAP_DSS_COLOR_UYVY:
+			m = 0xb; break;
+		case OMAP_DSS_COLOR_ARGB32:
+			m = 0xc; break;
+		case OMAP_DSS_COLOR_RGBA32:
+			m = 0xd; break;
+		case OMAP_DSS_COLOR_RGBX32:
+			m = 0xe; break;
+		case OMAP_DSS_COLOR_XRGB16_1555:
+			m = 0xf; break;
+		default:
+			BUG(); break;
+		}
 	}
 
-	REG_FLD_MOD(dispc_reg_att[plane], m, 4, 1);
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), m, 4, 1);
 }
 
-static void _dispc_set_channel_out(enum omap_plane plane,
+void dispc_set_channel_out(enum omap_plane plane,
 		enum omap_channel channel)
 {
 	int shift;
@@ -945,7 +1023,7 @@ static void _dispc_set_channel_out(enum omap_plane plane,
 		return;
 	}
 
-	val = dispc_read_reg(dispc_reg_att[plane]);
+	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
 		switch (channel) {
 		case OMAP_DSS_CHANNEL_LCD:
@@ -969,16 +1047,13 @@ static void _dispc_set_channel_out(enum omap_plane plane,
 	} else {
 		val = FLD_MOD(val, channel, shift, shift);
 	}
-	dispc_write_reg(dispc_reg_att[plane], val);
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 }
 
-void dispc_set_burst_size(enum omap_plane plane,
+static void dispc_set_burst_size(enum omap_plane plane,
 		enum omap_burst_size burst_size)
 {
 	int shift;
-	u32 val;
-
-	enable_clocks(1);
 
 	switch (plane) {
 	case OMAP_DSS_GFX:
@@ -993,11 +1068,72 @@ void dispc_set_burst_size(enum omap_plane plane,
 		return;
 	}
 
-	val = dispc_read_reg(dispc_reg_att[plane]);
-	val = FLD_MOD(val, burst_size, shift+1, shift);
-	dispc_write_reg(dispc_reg_att[plane], val);
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), burst_size, shift + 1, shift);
+}
 
-	enable_clocks(0);
+static void dispc_configure_burst_sizes(void)
+{
+	int i;
+	const int burst_size = BURST_SIZE_X8;
+
+	/* Configure burst size always to maximum size */
+	for (i = 0; i < omap_dss_get_num_overlays(); ++i)
+		dispc_set_burst_size(i, burst_size);
+}
+
+u32 dispc_get_burst_size(enum omap_plane plane)
+{
+	unsigned unit = dss_feat_get_burst_size_unit();
+	/* burst multiplier is always x8 (see dispc_configure_burst_sizes()) */
+	return unit * 8;
+}
+
+void dispc_enable_gamma_table(bool enable)
+{
+	/*
+	 * This is partially implemented to support only disabling of
+	 * the gamma table.
+	 */
+	if (enable) {
+		DSSWARN("Gamma table enabling for TV not yet supported");
+		return;
+	}
+
+	REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
+}
+
+void dispc_enable_cpr(enum omap_channel channel, bool enable)
+{
+	u16 reg;
+
+	if (channel == OMAP_DSS_CHANNEL_LCD)
+		reg = DISPC_CONFIG;
+	else if (channel == OMAP_DSS_CHANNEL_LCD2)
+		reg = DISPC_CONFIG2;
+	else
+		return;
+
+	REG_FLD_MOD(reg, enable, 15, 15);
+}
+
+void dispc_set_cpr_coef(enum omap_channel channel,
+		struct omap_dss_cpr_coefs *coefs)
+{
+	u32 coef_r, coef_g, coef_b;
+
+	if (channel != OMAP_DSS_CHANNEL_LCD && channel != OMAP_DSS_CHANNEL_LCD2)
+		return;
+
+	coef_r = FLD_VAL(coefs->rr, 31, 22) | FLD_VAL(coefs->rg, 20, 11) |
+		FLD_VAL(coefs->rb, 9, 0);
+	coef_g = FLD_VAL(coefs->gr, 31, 22) | FLD_VAL(coefs->gg, 20, 11) |
+		FLD_VAL(coefs->gb, 9, 0);
+	coef_b = FLD_VAL(coefs->br, 31, 22) | FLD_VAL(coefs->bg, 20, 11) |
+		FLD_VAL(coefs->bb, 9, 0);
+
+	dispc_write_reg(DISPC_CPR_COEF_R(channel), coef_r);
+	dispc_write_reg(DISPC_CPR_COEF_G(channel), coef_g);
+	dispc_write_reg(DISPC_CPR_COEF_B(channel), coef_b);
 }
 
 static void _dispc_set_vid_color_conv(enum omap_plane plane, bool enable)
@@ -1006,9 +1142,9 @@ static void _dispc_set_vid_color_conv(enum omap_plane plane, bool enable)
 
 	BUG_ON(plane == OMAP_DSS_GFX);
 
-	val = dispc_read_reg(dispc_reg_att[plane]);
+	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 	val = FLD_MOD(val, enable, 9, 9);
-	dispc_write_reg(dispc_reg_att[plane], val);
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 }
 
 void dispc_enable_replication(enum omap_plane plane, bool enable)
@@ -1020,9 +1156,7 @@ void dispc_enable_replication(enum omap_plane plane, bool enable)
 	else
 		bit = 10;
 
-	enable_clocks(1);
-	REG_FLD_MOD(dispc_reg_att[plane], enable, bit, bit);
-	enable_clocks(0);
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, bit, bit);
 }
 
 void dispc_set_lcd_size(enum omap_channel channel, u16 width, u16 height)
@@ -1030,9 +1164,7 @@ void dispc_set_lcd_size(enum omap_channel channel, u16 width, u16 height)
 	u32 val;
 	BUG_ON((width > (1 << 11)) || (height > (1 << 11)));
 	val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
-	enable_clocks(1);
-	dispc_write_reg(DISPC_SIZE_LCD(channel), val);
-	enable_clocks(0);
+	dispc_write_reg(DISPC_SIZE_MGR(channel), val);
 }
 
 void dispc_set_digit_size(u16 width, u16 height)
@@ -1040,30 +1172,25 @@ void dispc_set_digit_size(u16 width, u16 height)
 	u32 val;
 	BUG_ON((width > (1 << 11)) || (height > (1 << 11)));
 	val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
-	enable_clocks(1);
-	dispc_write_reg(DISPC_SIZE_DIG, val);
-	enable_clocks(0);
+	dispc_write_reg(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT), val);
 }
 
 static void dispc_read_plane_fifo_sizes(void)
 {
-	const struct dispc_reg fsz_reg[] = { DISPC_GFX_FIFO_SIZE_STATUS,
-				      DISPC_VID_FIFO_SIZE_STATUS(0),
-				      DISPC_VID_FIFO_SIZE_STATUS(1) };
 	u32 size;
 	int plane;
 	u8 start, end;
+	u32 unit;
 
-	enable_clocks(1);
+	unit = dss_feat_get_buffer_size_unit();
 
 	dss_feat_get_reg_field(FEAT_REG_FIFOSIZE, &start, &end);
 
 	for (plane = 0; plane < ARRAY_SIZE(dispc.fifo_size); ++plane) {
-		size = FLD_GET(dispc_read_reg(fsz_reg[plane]), start, end);
+		size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(plane), start, end);
+		size *= unit;
 		dispc.fifo_size[plane] = size;
 	}
-
-	enable_clocks(0);
 }
 
 u32 dispc_get_plane_fifo_size(enum omap_plane plane)
@@ -1071,129 +1198,164 @@ u32 dispc_get_plane_fifo_size(enum omap_plane plane)
 	return dispc.fifo_size[plane];
 }
 
-void dispc_setup_plane_fifo(enum omap_plane plane, u32 low, u32 high)
+void dispc_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
 {
-	const struct dispc_reg ftrs_reg[] = { DISPC_GFX_FIFO_THRESHOLD,
-				       DISPC_VID_FIFO_THRESHOLD(0),
-				       DISPC_VID_FIFO_THRESHOLD(1) };
 	u8 hi_start, hi_end, lo_start, lo_end;
+	u32 unit;
 
-	enable_clocks(1);
+	unit = dss_feat_get_buffer_size_unit();
 
-	DSSDBG("fifo(%d) low/high old %u/%u, new %u/%u\n",
-			plane,
-			REG_GET(ftrs_reg[plane], 11, 0),
-			REG_GET(ftrs_reg[plane], 27, 16),
-			low, high);
+	WARN_ON(low % unit != 0);
+	WARN_ON(high % unit != 0);
+
+	low /= unit;
+	high /= unit;
 
 	dss_feat_get_reg_field(FEAT_REG_FIFOHIGHTHRESHOLD, &hi_start, &hi_end);
 	dss_feat_get_reg_field(FEAT_REG_FIFOLOWTHRESHOLD, &lo_start, &lo_end);
 
-	dispc_write_reg(ftrs_reg[plane],
+	DSSDBG("fifo(%d) low/high old %u/%u, new %u/%u\n",
+			plane,
+			REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+				lo_start, lo_end),
+			REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+				hi_start, hi_end),
+			low, high);
+
+	dispc_write_reg(DISPC_OVL_FIFO_THRESHOLD(plane),
 			FLD_VAL(high, hi_start, hi_end) |
 			FLD_VAL(low, lo_start, lo_end));
-
-	enable_clocks(0);
 }
 
 void dispc_enable_fifomerge(bool enable)
 {
-	enable_clocks(1);
-
 	DSSDBG("FIFO merge %s\n", enable ? "enabled" : "disabled");
 	REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 14, 14);
-
-	enable_clocks(0);
 }
 
-static void _dispc_set_fir(enum omap_plane plane, int hinc, int vinc)
+static void _dispc_set_fir(enum omap_plane plane,
+				int hinc, int vinc,
+				enum omap_color_component color_comp)
 {
 	u32 val;
-	const struct dispc_reg fir_reg[] = { DISPC_VID_FIR(0),
-				      DISPC_VID_FIR(1) };
-	u8 hinc_start, hinc_end, vinc_start, vinc_end;
-
-	BUG_ON(plane == OMAP_DSS_GFX);
 
-	dss_feat_get_reg_field(FEAT_REG_FIRHINC, &hinc_start, &hinc_end);
-	dss_feat_get_reg_field(FEAT_REG_FIRVINC, &vinc_start, &vinc_end);
+	if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+		u8 hinc_start, hinc_end, vinc_start, vinc_end;
 
-	val = FLD_VAL(vinc, vinc_start, vinc_end) |
-			FLD_VAL(hinc, hinc_start, hinc_end);
+		dss_feat_get_reg_field(FEAT_REG_FIRHINC,
+					&hinc_start, &hinc_end);
+		dss_feat_get_reg_field(FEAT_REG_FIRVINC,
+					&vinc_start, &vinc_end);
+		val = FLD_VAL(vinc, vinc_start, vinc_end) |
+				FLD_VAL(hinc, hinc_start, hinc_end);
 
-	dispc_write_reg(fir_reg[plane-1], val);
+		dispc_write_reg(DISPC_OVL_FIR(plane), val);
+	} else {
+		val = FLD_VAL(vinc, 28, 16) | FLD_VAL(hinc, 12, 0);
+		dispc_write_reg(DISPC_OVL_FIR2(plane), val);
+	}
 }
 
 static void _dispc_set_vid_accu0(enum omap_plane plane, int haccu, int vaccu)
 {
 	u32 val;
-	const struct dispc_reg ac0_reg[] = { DISPC_VID_ACCU0(0),
-				      DISPC_VID_ACCU0(1) };
+	u8 hor_start, hor_end, vert_start, vert_end;
 
-	BUG_ON(plane == OMAP_DSS_GFX);
+	dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+	dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
 
-	val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
-	dispc_write_reg(ac0_reg[plane-1], val);
+	val = FLD_VAL(vaccu, vert_start, vert_end) |
+			FLD_VAL(haccu, hor_start, hor_end);
+
+	dispc_write_reg(DISPC_OVL_ACCU0(plane), val);
 }
 
 static void _dispc_set_vid_accu1(enum omap_plane plane, int haccu, int vaccu)
 {
 	u32 val;
-	const struct dispc_reg ac1_reg[] = { DISPC_VID_ACCU1(0),
-				      DISPC_VID_ACCU1(1) };
+	u8 hor_start, hor_end, vert_start, vert_end;
 
-	BUG_ON(plane == OMAP_DSS_GFX);
+	dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+	dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
 
-	val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
-	dispc_write_reg(ac1_reg[plane-1], val);
+	val = FLD_VAL(vaccu, vert_start, vert_end) |
+			FLD_VAL(haccu, hor_start, hor_end);
+
+	dispc_write_reg(DISPC_OVL_ACCU1(plane), val);
 }
 
+static void _dispc_set_vid_accu2_0(enum omap_plane plane, int haccu, int vaccu)
+{
+	u32 val;
 
-static void _dispc_set_scaling(enum omap_plane plane,
+	val = FLD_VAL(vaccu, 26, 16) | FLD_VAL(haccu, 10, 0);
+	dispc_write_reg(DISPC_OVL_ACCU2_0(plane), val);
+}
+
+static void _dispc_set_vid_accu2_1(enum omap_plane plane, int haccu, int vaccu)
+{
+	u32 val;
+
+	val = FLD_VAL(vaccu, 26, 16) | FLD_VAL(haccu, 10, 0);
+	dispc_write_reg(DISPC_OVL_ACCU2_1(plane), val);
+}
+
+static void _dispc_set_scale_param(enum omap_plane plane,
 		u16 orig_width, u16 orig_height,
 		u16 out_width, u16 out_height,
-		bool ilace, bool five_taps,
-		bool fieldmode)
+		bool five_taps, u8 rotation,
+		enum omap_color_component color_comp)
 {
-	int fir_hinc;
-	int fir_vinc;
+	int fir_hinc, fir_vinc;
 	int hscaleup, vscaleup;
-	int accu0 = 0;
-	int accu1 = 0;
-	u32 l;
-
-	BUG_ON(plane == OMAP_DSS_GFX);
 
 	hscaleup = orig_width <= out_width;
 	vscaleup = orig_height <= out_height;
 
-	_dispc_set_scale_coef(plane, hscaleup, vscaleup, five_taps);
+	_dispc_set_scale_coef(plane, hscaleup, vscaleup, five_taps, color_comp);
 
-	if (!orig_width || orig_width == out_width)
-		fir_hinc = 0;
-	else
-		fir_hinc = 1024 * orig_width / out_width;
+	fir_hinc = 1024 * orig_width / out_width;
+	fir_vinc = 1024 * orig_height / out_height;
 
-	if (!orig_height || orig_height == out_height)
-		fir_vinc = 0;
-	else
-		fir_vinc = 1024 * orig_height / out_height;
+	_dispc_set_fir(plane, fir_hinc, fir_vinc, color_comp);
+}
 
-	_dispc_set_fir(plane, fir_hinc, fir_vinc);
+static void _dispc_set_scaling_common(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool ilace, bool five_taps,
+		bool fieldmode, enum omap_color_mode color_mode,
+		u8 rotation)
+{
+	int accu0 = 0;
+	int accu1 = 0;
+	u32 l;
 
-	l = dispc_read_reg(dispc_reg_att[plane]);
-	l &= ~((0x0f << 5) | (0x3 << 21));
+	_dispc_set_scale_param(plane, orig_width, orig_height,
+				out_width, out_height, five_taps,
+				rotation, DISPC_COLOR_COMPONENT_RGB_Y);
+	l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 
-	l |= fir_hinc ? (1 << 5) : 0;
-	l |= fir_vinc ? (1 << 6) : 0;
+	/* RESIZEENABLE and VERTICALTAPS */
+	l &= ~((0x3 << 5) | (0x1 << 21));
+	l |= (orig_width != out_width) ? (1 << 5) : 0;
+	l |= (orig_height != out_height) ? (1 << 6) : 0;
+	l |= five_taps ? (1 << 21) : 0;
 
-	l |= hscaleup ? 0 : (1 << 7);
-	l |= vscaleup ? 0 : (1 << 8);
+	/* VRESIZECONF and HRESIZECONF */
+	if (dss_has_feature(FEAT_RESIZECONF)) {
+		l &= ~(0x3 << 7);
+		l |= (orig_width <= out_width) ? 0 : (1 << 7);
+		l |= (orig_height <= out_height) ? 0 : (1 << 8);
+	}
 
-	l |= five_taps ? (1 << 21) : 0;
-	l |= five_taps ? (1 << 22) : 0;
+	/* LINEBUFFERSPLIT */
+	if (dss_has_feature(FEAT_LINEBUFFERSPLIT)) {
+		l &= ~(0x1 << 22);
+		l |= five_taps ? (1 << 22) : 0;
+	}
 
-	dispc_write_reg(dispc_reg_att[plane], l);
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
 
 	/*
 	 * field 0 = even field = bottom field
@@ -1201,7 +1363,7 @@ static void _dispc_set_scaling(enum omap_plane plane,
 	 */
 	if (ilace && !fieldmode) {
 		accu1 = 0;
-		accu0 = (fir_vinc / 2) & 0x3ff;
+		accu0 = ((1024 * orig_height / out_height) / 2) & 0x3ff;
 		if (accu0 >= 1024/2) {
 			accu1 = 1024/2;
 			accu0 -= accu1;
@@ -1212,12 +1374,101 @@ static void _dispc_set_scaling(enum omap_plane plane,
 	_dispc_set_vid_accu1(plane, 0, accu1);
 }
 
+static void _dispc_set_scaling_uv(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool ilace, bool five_taps,
+		bool fieldmode, enum omap_color_mode color_mode,
+		u8 rotation)
+{
+	int scale_x = out_width != orig_width;
+	int scale_y = out_height != orig_height;
+
+	if (!dss_has_feature(FEAT_HANDLE_UV_SEPARATE))
+		return;
+	if ((color_mode != OMAP_DSS_COLOR_YUV2 &&
+			color_mode != OMAP_DSS_COLOR_UYVY &&
+			color_mode != OMAP_DSS_COLOR_NV12)) {
+		/* reset chroma resampling for RGB formats  */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
+		return;
+	}
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_NV12:
+		/* UV is subsampled by 2 vertically*/
+		orig_height >>= 1;
+		/* UV is subsampled by 2 horz.*/
+		orig_width >>= 1;
+		break;
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+		/*For YUV422 with 90/270 rotation,
+		 *we don't upsample chroma
+		 */
+		if (rotation == OMAP_DSS_ROT_0 ||
+			rotation == OMAP_DSS_ROT_180)
+			/* UV is subsampled by 2 hrz*/
+			orig_width >>= 1;
+		/* must use FIR for YUV422 if rotated */
+		if (rotation != OMAP_DSS_ROT_0)
+			scale_x = scale_y = true;
+		break;
+	default:
+		BUG();
+	}
+
+	if (out_width != orig_width)
+		scale_x = true;
+	if (out_height != orig_height)
+		scale_y = true;
+
+	_dispc_set_scale_param(plane, orig_width, orig_height,
+			out_width, out_height, five_taps,
+				rotation, DISPC_COLOR_COMPONENT_UV);
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
+		(scale_x || scale_y) ? 1 : 0, 8, 8);
+	/* set H scaling */
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_x ? 1 : 0, 5, 5);
+	/* set V scaling */
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_y ? 1 : 0, 6, 6);
+
+	_dispc_set_vid_accu2_0(plane, 0x80, 0);
+	_dispc_set_vid_accu2_1(plane, 0x80, 0);
+}
+
+static void _dispc_set_scaling(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool ilace, bool five_taps,
+		bool fieldmode, enum omap_color_mode color_mode,
+		u8 rotation)
+{
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	_dispc_set_scaling_common(plane,
+			orig_width, orig_height,
+			out_width, out_height,
+			ilace, five_taps,
+			fieldmode, color_mode,
+			rotation);
+
+	_dispc_set_scaling_uv(plane,
+		orig_width, orig_height,
+		out_width, out_height,
+		ilace, five_taps,
+		fieldmode, color_mode,
+		rotation);
+}
+
 static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
 		bool mirroring, enum omap_color_mode color_mode)
 {
+	bool row_repeat = false;
+	int vidrot = 0;
+
 	if (color_mode == OMAP_DSS_COLOR_YUV2 ||
 			color_mode == OMAP_DSS_COLOR_UYVY) {
-		int vidrot = 0;
 
 		if (mirroring) {
 			switch (rotation) {
@@ -1251,16 +1502,16 @@ static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
 			}
 		}
 
-		REG_FLD_MOD(dispc_reg_att[plane], vidrot, 13, 12);
-
 		if (rotation == OMAP_DSS_ROT_90 || rotation == OMAP_DSS_ROT_270)
-			REG_FLD_MOD(dispc_reg_att[plane], 0x1, 18, 18);
+			row_repeat = true;
 		else
-			REG_FLD_MOD(dispc_reg_att[plane], 0x0, 18, 18);
-	} else {
-		REG_FLD_MOD(dispc_reg_att[plane], 0, 13, 12);
-		REG_FLD_MOD(dispc_reg_att[plane], 0, 18, 18);
+			row_repeat = false;
 	}
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), vidrot, 13, 12);
+	if (dss_has_feature(FEAT_ROWREPEATENABLE))
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane),
+			row_repeat ? 1 : 0, 18, 18);
 }
 
 static int color_mode_to_bpp(enum omap_color_mode color_mode)
@@ -1273,12 +1524,17 @@ static int color_mode_to_bpp(enum omap_color_mode color_mode)
 	case OMAP_DSS_COLOR_CLUT4:
 		return 4;
 	case OMAP_DSS_COLOR_CLUT8:
+	case OMAP_DSS_COLOR_NV12:
 		return 8;
 	case OMAP_DSS_COLOR_RGB12U:
 	case OMAP_DSS_COLOR_RGB16:
 	case OMAP_DSS_COLOR_ARGB16:
 	case OMAP_DSS_COLOR_YUV2:
 	case OMAP_DSS_COLOR_UYVY:
+	case OMAP_DSS_COLOR_RGBA16:
+	case OMAP_DSS_COLOR_RGBX16:
+	case OMAP_DSS_COLOR_ARGB16_1555:
+	case OMAP_DSS_COLOR_XRGB16_1555:
 		return 16;
 	case OMAP_DSS_COLOR_RGB24P:
 		return 24;
@@ -1594,14 +1850,7 @@ static unsigned long calc_fclk(enum omap_channel channel, u16 width,
 	return dispc_pclk_rate(channel) * vf * hf;
 }
 
-void dispc_set_channel_out(enum omap_plane plane, enum omap_channel channel_out)
-{
-	enable_clocks(1);
-	_dispc_set_channel_out(plane, channel_out);
-	enable_clocks(0);
-}
-
-static int _dispc_setup_plane(enum omap_plane plane,
+int dispc_setup_plane(enum omap_plane plane,
 		u32 paddr, u16 screen_width,
 		u16 pos_x, u16 pos_y,
 		u16 width, u16 height,
@@ -1609,9 +1858,9 @@ static int _dispc_setup_plane(enum omap_plane plane,
 		enum omap_color_mode color_mode,
 		bool ilace,
 		enum omap_dss_rotation_type rotation_type,
-		u8 rotation, int mirror,
+		u8 rotation, bool mirror,
 		u8 global_alpha, u8 pre_mult_alpha,
-		enum omap_channel channel)
+		enum omap_channel channel, u32 puv_addr)
 {
 	const int maxdownscale = cpu_is_omap34xx() ? 4 : 2;
 	bool five_taps = 0;
@@ -1623,6 +1872,14 @@ static int _dispc_setup_plane(enum omap_plane plane,
 	u16 frame_height = height;
 	unsigned int field_offset = 0;
 
+	DSSDBG("dispc_setup_plane %d, pa %x, sw %d, %d,%d, %dx%d -> "
+	       "%dx%d, ilace %d, cmode %x, rot %d, mir %d chan %d\n",
+	       plane, paddr, screen_width, pos_x, pos_y,
+	       width, height,
+	       out_width, out_height,
+	       ilace, color_mode,
+	       rotation, mirror, channel);
+
 	if (paddr == 0)
 		return -EINVAL;
 
@@ -1660,7 +1917,8 @@ static int _dispc_setup_plane(enum omap_plane plane,
 			return -EINVAL;
 
 		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
-			color_mode == OMAP_DSS_COLOR_UYVY)
+			color_mode == OMAP_DSS_COLOR_UYVY ||
+			color_mode == OMAP_DSS_COLOR_NV12)
 			cconv = 1;
 
 		/* Must use 5-tap filter? */
@@ -1734,6 +1992,12 @@ static int _dispc_setup_plane(enum omap_plane plane,
 	_dispc_set_plane_ba0(plane, paddr + offset0);
 	_dispc_set_plane_ba1(plane, paddr + offset1);
 
+	if (OMAP_DSS_COLOR_NV12 == color_mode) {
+		_dispc_set_plane_ba0_uv(plane, puv_addr + offset0);
+		_dispc_set_plane_ba1_uv(plane, puv_addr + offset1);
+	}
+
+
 	_dispc_set_row_inc(plane, row_inc);
 	_dispc_set_pix_inc(plane, pix_inc);
 
@@ -1747,7 +2011,8 @@ static int _dispc_setup_plane(enum omap_plane plane,
 	if (plane != OMAP_DSS_GFX) {
 		_dispc_set_scaling(plane, width, height,
 				   out_width, out_height,
-				   ilace, five_taps, fieldmode);
+				   ilace, five_taps, fieldmode,
+				   color_mode, rotation);
 		_dispc_set_vid_size(plane, out_width, out_height);
 		_dispc_set_vid_color_conv(plane, cconv);
 	}
@@ -1760,9 +2025,13 @@ static int _dispc_setup_plane(enum omap_plane plane,
 	return 0;
 }
 
-static void _dispc_enable_plane(enum omap_plane plane, bool enable)
+int dispc_enable_plane(enum omap_plane plane, bool enable)
 {
-	REG_FLD_MOD(dispc_reg_att[plane], enable ? 1 : 0, 0, 0);
+	DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 0, 0);
+
+	return 0;
 }
 
 static void dispc_disable_isr(void *data, u32 mask)
@@ -1786,8 +2055,6 @@ static void dispc_enable_lcd_out(enum omap_channel channel, bool enable)
 	int r;
 	u32 irq;
 
-	enable_clocks(1);
-
 	/* When we disable LCD output, we need to wait until frame is done.
 	 * Otherwise the DSS is still working, and turning off the clocks
 	 * prevents DSS from going to OFF mode */
@@ -1821,8 +2088,6 @@ static void dispc_enable_lcd_out(enum omap_channel channel, bool enable)
 		if (r)
 			DSSERR("failed to unregister FRAMEDONE isr\n");
 	}
-
-	enable_clocks(0);
 }
 
 static void _enable_digit_out(bool enable)
@@ -1835,12 +2100,8 @@ static void dispc_enable_digit_out(bool enable)
 	struct completion frame_done_completion;
 	int r;
 
-	enable_clocks(1);
-
-	if (REG_GET(DISPC_CONTROL, 1, 1) == enable) {
-		enable_clocks(0);
+	if (REG_GET(DISPC_CONTROL, 1, 1) == enable)
 		return;
-	}
 
 	if (enable) {
 		unsigned long flags;
@@ -1892,8 +2153,6 @@ static void dispc_enable_digit_out(bool enable)
 		_omap_dispc_set_irqs();
 		spin_unlock_irqrestore(&dispc.irq_lock, flags);
 	}
-
-	enable_clocks(0);
 }
 
 bool dispc_is_channel_enabled(enum omap_channel channel)
@@ -1924,9 +2183,7 @@ void dispc_lcd_enable_signal_polarity(bool act_high)
 	if (!dss_has_feature(FEAT_LCDENABLEPOL))
 		return;
 
-	enable_clocks(1);
 	REG_FLD_MOD(DISPC_CONTROL, act_high ? 1 : 0, 29, 29);
-	enable_clocks(0);
 }
 
 void dispc_lcd_enable_signal(bool enable)
@@ -1934,9 +2191,7 @@ void dispc_lcd_enable_signal(bool enable)
 	if (!dss_has_feature(FEAT_LCDENABLESIGNAL))
 		return;
 
-	enable_clocks(1);
 	REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 28, 28);
-	enable_clocks(0);
 }
 
 void dispc_pck_free_enable(bool enable)
@@ -1944,19 +2199,15 @@ void dispc_pck_free_enable(bool enable)
 	if (!dss_has_feature(FEAT_PCKFREEENABLE))
 		return;
 
-	enable_clocks(1);
 	REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 27, 27);
-	enable_clocks(0);
 }
 
 void dispc_enable_fifohandcheck(enum omap_channel channel, bool enable)
 {
-	enable_clocks(1);
 	if (channel == OMAP_DSS_CHANNEL_LCD2)
 		REG_FLD_MOD(DISPC_CONFIG2, enable ? 1 : 0, 16, 16);
 	else
 		REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 16, 16);
-	enable_clocks(0);
 }
 
 
@@ -1979,27 +2230,21 @@ void dispc_set_lcd_display_type(enum omap_channel channel,
 		return;
 	}
 
-	enable_clocks(1);
 	if (channel == OMAP_DSS_CHANNEL_LCD2)
 		REG_FLD_MOD(DISPC_CONTROL2, mode, 3, 3);
 	else
 		REG_FLD_MOD(DISPC_CONTROL, mode, 3, 3);
-	enable_clocks(0);
 }
 
 void dispc_set_loadmode(enum omap_dss_load_mode mode)
 {
-	enable_clocks(1);
 	REG_FLD_MOD(DISPC_CONFIG, mode, 2, 1);
-	enable_clocks(0);
 }
 
 
 void dispc_set_default_color(enum omap_channel channel, u32 color)
 {
-	enable_clocks(1);
 	dispc_write_reg(DISPC_DEFAULT_COLOR(channel), color);
-	enable_clocks(0);
 }
 
 u32 dispc_get_default_color(enum omap_channel channel)
@@ -2010,9 +2255,7 @@ u32 dispc_get_default_color(enum omap_channel channel)
 		channel != OMAP_DSS_CHANNEL_LCD &&
 		channel != OMAP_DSS_CHANNEL_LCD2);
 
-	enable_clocks(1);
 	l = dispc_read_reg(DISPC_DEFAULT_COLOR(channel));
-	enable_clocks(0);
 
 	return l;
 }
@@ -2021,7 +2264,6 @@ void dispc_set_trans_key(enum omap_channel ch,
 		enum omap_dss_trans_key_type type,
 		u32 trans_key)
 {
-	enable_clocks(1);
 	if (ch == OMAP_DSS_CHANNEL_LCD)
 		REG_FLD_MOD(DISPC_CONFIG, type, 11, 11);
 	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
@@ -2030,14 +2272,12 @@ void dispc_set_trans_key(enum omap_channel ch,
 		REG_FLD_MOD(DISPC_CONFIG2, type, 11, 11);
 
 	dispc_write_reg(DISPC_TRANS_COLOR(ch), trans_key);
-	enable_clocks(0);
 }
 
 void dispc_get_trans_key(enum omap_channel ch,
 		enum omap_dss_trans_key_type *type,
 		u32 *trans_key)
 {
-	enable_clocks(1);
 	if (type) {
 		if (ch == OMAP_DSS_CHANNEL_LCD)
 			*type = REG_GET(DISPC_CONFIG, 11, 11);
@@ -2051,33 +2291,28 @@ void dispc_get_trans_key(enum omap_channel ch,
 
 	if (trans_key)
 		*trans_key = dispc_read_reg(DISPC_TRANS_COLOR(ch));
-	enable_clocks(0);
 }
 
 void dispc_enable_trans_key(enum omap_channel ch, bool enable)
 {
-	enable_clocks(1);
 	if (ch == OMAP_DSS_CHANNEL_LCD)
 		REG_FLD_MOD(DISPC_CONFIG, enable, 10, 10);
 	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
 		REG_FLD_MOD(DISPC_CONFIG, enable, 12, 12);
 	else /* OMAP_DSS_CHANNEL_LCD2 */
 		REG_FLD_MOD(DISPC_CONFIG2, enable, 10, 10);
-	enable_clocks(0);
 }
 void dispc_enable_alpha_blending(enum omap_channel ch, bool enable)
 {
 	if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
 		return;
 
-	enable_clocks(1);
 	if (ch == OMAP_DSS_CHANNEL_LCD)
 		REG_FLD_MOD(DISPC_CONFIG, enable, 18, 18);
 	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
 		REG_FLD_MOD(DISPC_CONFIG, enable, 19, 19);
 	else /* OMAP_DSS_CHANNEL_LCD2 */
 		REG_FLD_MOD(DISPC_CONFIG2, enable, 18, 18);
-	enable_clocks(0);
 }
 bool dispc_alpha_blending_enabled(enum omap_channel ch)
 {
@@ -2086,7 +2321,6 @@ bool dispc_alpha_blending_enabled(enum omap_channel ch)
 	if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
 		return false;
 
-	enable_clocks(1);
 	if (ch == OMAP_DSS_CHANNEL_LCD)
 		enabled = REG_GET(DISPC_CONFIG, 18, 18);
 	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
@@ -2095,7 +2329,6 @@ bool dispc_alpha_blending_enabled(enum omap_channel ch)
 		enabled = REG_GET(DISPC_CONFIG2, 18, 18);
 	else
 		BUG();
-	enable_clocks(0);
 
 	return enabled;
 }
@@ -2105,7 +2338,6 @@ bool dispc_trans_key_enabled(enum omap_channel ch)
 {
 	bool enabled;
 
-	enable_clocks(1);
 	if (ch == OMAP_DSS_CHANNEL_LCD)
 		enabled = REG_GET(DISPC_CONFIG, 10, 10);
 	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
@@ -2114,7 +2346,6 @@ bool dispc_trans_key_enabled(enum omap_channel ch)
 		enabled = REG_GET(DISPC_CONFIG2, 10, 10);
 	else
 		BUG();
-	enable_clocks(0);
 
 	return enabled;
 }
@@ -2142,12 +2373,10 @@ void dispc_set_tft_data_lines(enum omap_channel channel, u8 data_lines)
 		return;
 	}
 
-	enable_clocks(1);
 	if (channel == OMAP_DSS_CHANNEL_LCD2)
 		REG_FLD_MOD(DISPC_CONTROL2, code, 9, 8);
 	else
 		REG_FLD_MOD(DISPC_CONTROL, code, 9, 8);
-	enable_clocks(0);
 }
 
 void dispc_set_parallel_interface_mode(enum omap_channel channel,
@@ -2179,8 +2408,6 @@ void dispc_set_parallel_interface_mode(enum omap_channel channel,
 		return;
 	}
 
-	enable_clocks(1);
-
 	if (channel == OMAP_DSS_CHANNEL_LCD2) {
 		l = dispc_read_reg(DISPC_CONTROL2);
 		l = FLD_MOD(l, stallmode, 11, 11);
@@ -2192,8 +2419,6 @@ void dispc_set_parallel_interface_mode(enum omap_channel channel,
 		l = FLD_MOD(l, gpout1, 16, 16);
 		dispc_write_reg(DISPC_CONTROL, l);
 	}
-
-	enable_clocks(0);
 }
 
 static bool _dispc_lcd_timings_ok(int hsw, int hfp, int hbp,
@@ -2246,10 +2471,8 @@ static void _dispc_set_lcd_timings(enum omap_channel channel, int hsw,
 			FLD_VAL(vbp, 31, 20);
 	}
 
-	enable_clocks(1);
 	dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
 	dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
-	enable_clocks(0);
 }
 
 /* change name to mode? */
@@ -2292,83 +2515,122 @@ static void dispc_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
 	BUG_ON(lck_div < 1);
 	BUG_ON(pck_div < 2);
 
-	enable_clocks(1);
-	dispc_write_reg(DISPC_DIVISOR(channel),
+	dispc_write_reg(DISPC_DIVISORo(channel),
 			FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
-	enable_clocks(0);
 }
 
 static void dispc_get_lcd_divisor(enum omap_channel channel, int *lck_div,
 		int *pck_div)
 {
 	u32 l;
-	l = dispc_read_reg(DISPC_DIVISOR(channel));
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
 	*lck_div = FLD_GET(l, 23, 16);
 	*pck_div = FLD_GET(l, 7, 0);
 }
 
 unsigned long dispc_fclk_rate(void)
 {
+	struct platform_device *dsidev;
 	unsigned long r = 0;
 
-	if (dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK)
-		r = dss_clk_get_rate(DSS_CLK_FCK1);
-	else
-#ifdef CONFIG_OMAP2_DSS_DSI
-		r = dsi_get_dsi1_pll_rate();
-#else
-	BUG();
-#endif
+	switch (dss_get_dispc_clk_source()) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		r = clk_get_rate(dispc.dss_clk);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		dsidev = dsi_get_dsidev_from_id(0);
+		r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		dsidev = dsi_get_dsidev_from_id(1);
+		r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+		break;
+	default:
+		BUG();
+	}
+
 	return r;
 }
 
 unsigned long dispc_lclk_rate(enum omap_channel channel)
 {
+	struct platform_device *dsidev;
 	int lcd;
 	unsigned long r;
 	u32 l;
 
-	l = dispc_read_reg(DISPC_DIVISOR(channel));
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
 
 	lcd = FLD_GET(l, 23, 16);
 
-	r = dispc_fclk_rate();
+	switch (dss_get_lcd_clk_source(channel)) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		r = clk_get_rate(dispc.dss_clk);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		dsidev = dsi_get_dsidev_from_id(0);
+		r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		dsidev = dsi_get_dsidev_from_id(1);
+		r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+		break;
+	default:
+		BUG();
+	}
 
 	return r / lcd;
 }
 
 unsigned long dispc_pclk_rate(enum omap_channel channel)
 {
-	int lcd, pcd;
+	int pcd;
 	unsigned long r;
 	u32 l;
 
-	l = dispc_read_reg(DISPC_DIVISOR(channel));
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
 
-	lcd = FLD_GET(l, 23, 16);
 	pcd = FLD_GET(l, 7, 0);
 
-	r = dispc_fclk_rate();
+	r = dispc_lclk_rate(channel);
 
-	return r / lcd / pcd;
+	return r / pcd;
 }
 
 void dispc_dump_clocks(struct seq_file *s)
 {
 	int lcd, pcd;
+	u32 l;
+	enum omap_dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
+	enum omap_dss_clk_source lcd_clk_src;
 
-	enable_clocks(1);
+	if (dispc_runtime_get())
+		return;
 
 	seq_printf(s, "- DISPC -\n");
 
-	seq_printf(s, "dispc fclk source = %s\n",
-			dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-			"dss1_alwon_fclk" : "dsi1_pll_fclk");
+	seq_printf(s, "dispc fclk source = %s (%s)\n",
+			dss_get_generic_clk_source_name(dispc_clk_src),
+			dss_feat_get_clk_source_name(dispc_clk_src));
 
 	seq_printf(s, "fck\t\t%-16lu\n", dispc_fclk_rate());
 
+	if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+		seq_printf(s, "- DISPC-CORE-CLK -\n");
+		l = dispc_read_reg(DISPC_DIVISOR);
+		lcd = FLD_GET(l, 23, 16);
+
+		seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+				(dispc_fclk_rate()/lcd), lcd);
+	}
 	seq_printf(s, "- LCD1 -\n");
 
+	lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD);
+
+	seq_printf(s, "lcd1_clk source = %s (%s)\n",
+		dss_get_generic_clk_source_name(lcd_clk_src),
+		dss_feat_get_clk_source_name(lcd_clk_src));
+
 	dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD, &lcd, &pcd);
 
 	seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
@@ -2378,6 +2640,12 @@ void dispc_dump_clocks(struct seq_file *s)
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
 		seq_printf(s, "- LCD2 -\n");
 
+		lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD2);
+
+		seq_printf(s, "lcd2_clk source = %s (%s)\n",
+			dss_get_generic_clk_source_name(lcd_clk_src),
+			dss_feat_get_clk_source_name(lcd_clk_src));
+
 		dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD2, &lcd, &pcd);
 
 		seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
@@ -2385,7 +2653,8 @@ void dispc_dump_clocks(struct seq_file *s)
 		seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
 				dispc_pclk_rate(OMAP_DSS_CHANNEL_LCD2), pcd);
 	}
-	enable_clocks(0);
+
+	dispc_runtime_put();
 }
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
@@ -2438,9 +2707,10 @@ void dispc_dump_irqs(struct seq_file *s)
 
 void dispc_dump_regs(struct seq_file *s)
 {
-#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dispc_read_reg(r))
+#define DUMPREG(r) seq_printf(s, "%-50s %08x\n", #r, dispc_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	if (dispc_runtime_get())
+		return;
 
 	DUMPREG(DISPC_REVISION);
 	DUMPREG(DISPC_SYSCONFIG);
@@ -2450,154 +2720,242 @@ void dispc_dump_regs(struct seq_file *s)
 	DUMPREG(DISPC_CONTROL);
 	DUMPREG(DISPC_CONFIG);
 	DUMPREG(DISPC_CAPABLE);
-	DUMPREG(DISPC_DEFAULT_COLOR(0));
-	DUMPREG(DISPC_DEFAULT_COLOR(1));
-	DUMPREG(DISPC_TRANS_COLOR(0));
-	DUMPREG(DISPC_TRANS_COLOR(1));
+	DUMPREG(DISPC_DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_DEFAULT_COLOR(OMAP_DSS_CHANNEL_DIGIT));
+	DUMPREG(DISPC_TRANS_COLOR(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_TRANS_COLOR(OMAP_DSS_CHANNEL_DIGIT));
 	DUMPREG(DISPC_LINE_STATUS);
 	DUMPREG(DISPC_LINE_NUMBER);
-	DUMPREG(DISPC_TIMING_H(0));
-	DUMPREG(DISPC_TIMING_V(0));
-	DUMPREG(DISPC_POL_FREQ(0));
-	DUMPREG(DISPC_DIVISOR(0));
-	DUMPREG(DISPC_GLOBAL_ALPHA);
-	DUMPREG(DISPC_SIZE_DIG);
-	DUMPREG(DISPC_SIZE_LCD(0));
+	DUMPREG(DISPC_TIMING_H(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_TIMING_V(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_POL_FREQ(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_DIVISORo(OMAP_DSS_CHANNEL_LCD));
+	if (dss_has_feature(FEAT_GLOBAL_ALPHA))
+		DUMPREG(DISPC_GLOBAL_ALPHA);
+	DUMPREG(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT));
+	DUMPREG(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_LCD));
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
 		DUMPREG(DISPC_CONTROL2);
 		DUMPREG(DISPC_CONFIG2);
-		DUMPREG(DISPC_DEFAULT_COLOR(2));
-		DUMPREG(DISPC_TRANS_COLOR(2));
-		DUMPREG(DISPC_TIMING_H(2));
-		DUMPREG(DISPC_TIMING_V(2));
-		DUMPREG(DISPC_POL_FREQ(2));
-		DUMPREG(DISPC_DIVISOR(2));
-		DUMPREG(DISPC_SIZE_LCD(2));
-	}
-
-	DUMPREG(DISPC_GFX_BA0);
-	DUMPREG(DISPC_GFX_BA1);
-	DUMPREG(DISPC_GFX_POSITION);
-	DUMPREG(DISPC_GFX_SIZE);
-	DUMPREG(DISPC_GFX_ATTRIBUTES);
-	DUMPREG(DISPC_GFX_FIFO_THRESHOLD);
-	DUMPREG(DISPC_GFX_FIFO_SIZE_STATUS);
-	DUMPREG(DISPC_GFX_ROW_INC);
-	DUMPREG(DISPC_GFX_PIXEL_INC);
-	DUMPREG(DISPC_GFX_WINDOW_SKIP);
-	DUMPREG(DISPC_GFX_TABLE_BA);
-
-	DUMPREG(DISPC_DATA_CYCLE1(0));
-	DUMPREG(DISPC_DATA_CYCLE2(0));
-	DUMPREG(DISPC_DATA_CYCLE3(0));
-
-	DUMPREG(DISPC_CPR_COEF_R(0));
-	DUMPREG(DISPC_CPR_COEF_G(0));
-	DUMPREG(DISPC_CPR_COEF_B(0));
+		DUMPREG(DISPC_DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_TRANS_COLOR(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_TIMING_H(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_TIMING_V(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_POL_FREQ(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_DIVISORo(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_LCD2));
+	}
+
+	DUMPREG(DISPC_OVL_BA0(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_BA1(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_POSITION(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_SIZE(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_ATTRIBUTES(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_FIFO_THRESHOLD(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_FIFO_SIZE_STATUS(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_ROW_INC(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_PIXEL_INC(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_WINDOW_SKIP(OMAP_DSS_GFX));
+	DUMPREG(DISPC_OVL_TABLE_BA(OMAP_DSS_GFX));
+
+	DUMPREG(DISPC_DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD));
+	DUMPREG(DISPC_DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD));
+
+	if (dss_has_feature(FEAT_CPR)) {
+		DUMPREG(DISPC_CPR_COEF_R(OMAP_DSS_CHANNEL_LCD));
+		DUMPREG(DISPC_CPR_COEF_G(OMAP_DSS_CHANNEL_LCD));
+		DUMPREG(DISPC_CPR_COEF_B(OMAP_DSS_CHANNEL_LCD));
+	}
 	if (dss_has_feature(FEAT_MGR_LCD2)) {
-		DUMPREG(DISPC_DATA_CYCLE1(2));
-		DUMPREG(DISPC_DATA_CYCLE2(2));
-		DUMPREG(DISPC_DATA_CYCLE3(2));
-
-		DUMPREG(DISPC_CPR_COEF_R(2));
-		DUMPREG(DISPC_CPR_COEF_G(2));
-		DUMPREG(DISPC_CPR_COEF_B(2));
-	}
-
-	DUMPREG(DISPC_GFX_PRELOAD);
-
-	DUMPREG(DISPC_VID_BA0(0));
-	DUMPREG(DISPC_VID_BA1(0));
-	DUMPREG(DISPC_VID_POSITION(0));
-	DUMPREG(DISPC_VID_SIZE(0));
-	DUMPREG(DISPC_VID_ATTRIBUTES(0));
-	DUMPREG(DISPC_VID_FIFO_THRESHOLD(0));
-	DUMPREG(DISPC_VID_FIFO_SIZE_STATUS(0));
-	DUMPREG(DISPC_VID_ROW_INC(0));
-	DUMPREG(DISPC_VID_PIXEL_INC(0));
-	DUMPREG(DISPC_VID_FIR(0));
-	DUMPREG(DISPC_VID_PICTURE_SIZE(0));
-	DUMPREG(DISPC_VID_ACCU0(0));
-	DUMPREG(DISPC_VID_ACCU1(0));
-
-	DUMPREG(DISPC_VID_BA0(1));
-	DUMPREG(DISPC_VID_BA1(1));
-	DUMPREG(DISPC_VID_POSITION(1));
-	DUMPREG(DISPC_VID_SIZE(1));
-	DUMPREG(DISPC_VID_ATTRIBUTES(1));
-	DUMPREG(DISPC_VID_FIFO_THRESHOLD(1));
-	DUMPREG(DISPC_VID_FIFO_SIZE_STATUS(1));
-	DUMPREG(DISPC_VID_ROW_INC(1));
-	DUMPREG(DISPC_VID_PIXEL_INC(1));
-	DUMPREG(DISPC_VID_FIR(1));
-	DUMPREG(DISPC_VID_PICTURE_SIZE(1));
-	DUMPREG(DISPC_VID_ACCU0(1));
-	DUMPREG(DISPC_VID_ACCU1(1));
-
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 0));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 1));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 2));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 3));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 5));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 6));
-	DUMPREG(DISPC_VID_FIR_COEF_H(0, 7));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 0));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 1));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 2));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 3));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 5));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 6));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(0, 7));
-	DUMPREG(DISPC_VID_CONV_COEF(0, 0));
-	DUMPREG(DISPC_VID_CONV_COEF(0, 1));
-	DUMPREG(DISPC_VID_CONV_COEF(0, 2));
-	DUMPREG(DISPC_VID_CONV_COEF(0, 3));
-	DUMPREG(DISPC_VID_CONV_COEF(0, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 0));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 1));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 2));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 3));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 5));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 6));
-	DUMPREG(DISPC_VID_FIR_COEF_V(0, 7));
-
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 0));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 1));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 2));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 3));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 5));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 6));
-	DUMPREG(DISPC_VID_FIR_COEF_H(1, 7));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 0));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 1));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 2));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 3));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 5));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 6));
-	DUMPREG(DISPC_VID_FIR_COEF_HV(1, 7));
-	DUMPREG(DISPC_VID_CONV_COEF(1, 0));
-	DUMPREG(DISPC_VID_CONV_COEF(1, 1));
-	DUMPREG(DISPC_VID_CONV_COEF(1, 2));
-	DUMPREG(DISPC_VID_CONV_COEF(1, 3));
-	DUMPREG(DISPC_VID_CONV_COEF(1, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 0));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 1));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 2));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 3));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 4));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 5));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 6));
-	DUMPREG(DISPC_VID_FIR_COEF_V(1, 7));
-
-	DUMPREG(DISPC_VID_PRELOAD(0));
-	DUMPREG(DISPC_VID_PRELOAD(1));
-
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		DUMPREG(DISPC_DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD2));
+		DUMPREG(DISPC_DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD2));
+
+		if (dss_has_feature(FEAT_CPR)) {
+			DUMPREG(DISPC_CPR_COEF_R(OMAP_DSS_CHANNEL_LCD2));
+			DUMPREG(DISPC_CPR_COEF_G(OMAP_DSS_CHANNEL_LCD2));
+			DUMPREG(DISPC_CPR_COEF_B(OMAP_DSS_CHANNEL_LCD2));
+		}
+	}
+
+	if (dss_has_feature(FEAT_PRELOAD))
+		DUMPREG(DISPC_OVL_PRELOAD(OMAP_DSS_GFX));
+
+	DUMPREG(DISPC_OVL_BA0(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_BA1(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_POSITION(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_SIZE(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_FIFO_SIZE_STATUS(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_ROW_INC(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_PIXEL_INC(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_FIR(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_PICTURE_SIZE(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_ACCU0(OMAP_DSS_VIDEO1));
+	DUMPREG(DISPC_OVL_ACCU1(OMAP_DSS_VIDEO1));
+
+	DUMPREG(DISPC_OVL_BA0(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_BA1(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_POSITION(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_SIZE(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_FIFO_SIZE_STATUS(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_ROW_INC(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_PIXEL_INC(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_FIR(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_PICTURE_SIZE(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_ACCU0(OMAP_DSS_VIDEO2));
+	DUMPREG(DISPC_OVL_ACCU1(OMAP_DSS_VIDEO2));
+
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 0));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 1));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 2));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 3));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 4));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 5));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 6));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 7));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 0));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 1));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 2));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 3));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 4));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 5));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 6));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 7));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 0));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 1));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 2));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 3));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 4));
+	if (dss_has_feature(FEAT_FIR_COEF_V)) {
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 7));
+	}
+
+	if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+		DUMPREG(DISPC_OVL_BA0_UV(OMAP_DSS_VIDEO1));
+		DUMPREG(DISPC_OVL_BA1_UV(OMAP_DSS_VIDEO1));
+		DUMPREG(DISPC_OVL_FIR2(OMAP_DSS_VIDEO1));
+		DUMPREG(DISPC_OVL_ACCU2_0(OMAP_DSS_VIDEO1));
+		DUMPREG(DISPC_OVL_ACCU2_1(OMAP_DSS_VIDEO1));
+
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 7));
+
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 7));
+
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 7));
+	}
+	if (dss_has_feature(FEAT_ATTR2))
+		DUMPREG(DISPC_OVL_ATTRIBUTES2(OMAP_DSS_VIDEO1));
+
+
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 0));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 1));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 2));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 3));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 4));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 5));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 6));
+	DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 7));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 0));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 1));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 2));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 3));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 4));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 5));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 6));
+	DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 7));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 0));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 1));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 2));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 3));
+	DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 4));
+
+	if (dss_has_feature(FEAT_FIR_COEF_V)) {
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 7));
+	}
+
+	if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+		DUMPREG(DISPC_OVL_BA0_UV(OMAP_DSS_VIDEO2));
+		DUMPREG(DISPC_OVL_BA1_UV(OMAP_DSS_VIDEO2));
+		DUMPREG(DISPC_OVL_FIR2(OMAP_DSS_VIDEO2));
+		DUMPREG(DISPC_OVL_ACCU2_0(OMAP_DSS_VIDEO2));
+		DUMPREG(DISPC_OVL_ACCU2_1(OMAP_DSS_VIDEO2));
+
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 7));
+
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 7));
+
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 0));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 1));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 2));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 3));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 4));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 5));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 6));
+		DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 7));
+	}
+	if (dss_has_feature(FEAT_ATTR2))
+		DUMPREG(DISPC_OVL_ATTRIBUTES2(OMAP_DSS_VIDEO2));
+
+	if (dss_has_feature(FEAT_PRELOAD)) {
+		DUMPREG(DISPC_OVL_PRELOAD(OMAP_DSS_VIDEO1));
+		DUMPREG(DISPC_OVL_PRELOAD(OMAP_DSS_VIDEO2));
+	}
+
+	dispc_runtime_put();
 #undef DUMPREG
 }
 
@@ -2618,9 +2976,7 @@ static void _dispc_set_pol_freq(enum omap_channel channel, bool onoff, bool rf,
 	l |= FLD_VAL(acbi, 11, 8);
 	l |= FLD_VAL(acb, 7, 0);
 
-	enable_clocks(1);
 	dispc_write_reg(DISPC_POL_FREQ(channel), l);
-	enable_clocks(0);
 }
 
 void dispc_set_pol_freq(enum omap_channel channel,
@@ -2713,8 +3069,8 @@ int dispc_get_clock_div(enum omap_channel channel,
 
 	fck = dispc_fclk_rate();
 
-	cinfo->lck_div = REG_GET(DISPC_DIVISOR(channel), 23, 16);
-	cinfo->pck_div = REG_GET(DISPC_DIVISOR(channel), 7, 0);
+	cinfo->lck_div = REG_GET(DISPC_DIVISORo(channel), 23, 16);
+	cinfo->pck_div = REG_GET(DISPC_DIVISORo(channel), 7, 0);
 
 	cinfo->lck = fck / cinfo->lck_div;
 	cinfo->pck = cinfo->lck / cinfo->pck_div;
@@ -2741,15 +3097,11 @@ static void _omap_dispc_set_irqs(void)
 		mask |= isr_data->mask;
 	}
 
-	enable_clocks(1);
-
 	old_mask = dispc_read_reg(DISPC_IRQENABLE);
 	/* clear the irqstatus for newly enabled irqs */
 	dispc_write_reg(DISPC_IRQSTATUS, (mask ^ old_mask) & mask);
 
 	dispc_write_reg(DISPC_IRQENABLE, mask);
-
-	enable_clocks(0);
 }
 
 int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
@@ -2791,6 +3143,9 @@ int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
 		break;
 	}
 
+	if (ret)
+		goto err;
+
 	_omap_dispc_set_irqs();
 
 	spin_unlock_irqrestore(&dispc.irq_lock, flags);
@@ -2866,10 +3221,10 @@ static void print_irq_status(u32 status)
  * but we presume they are on because we got an IRQ. However,
  * an irq handler may turn the clocks off, so we may not have
  * clock later in the function. */
-void dispc_irq_handler(void)
+static irqreturn_t omap_dispc_irq_handler(int irq, void *arg)
 {
 	int i;
-	u32 irqstatus;
+	u32 irqstatus, irqenable;
 	u32 handledirqs = 0;
 	u32 unhandled_errors;
 	struct omap_dispc_isr_data *isr_data;
@@ -2878,6 +3233,13 @@ void dispc_irq_handler(void)
 	spin_lock(&dispc.irq_lock);
 
 	irqstatus = dispc_read_reg(DISPC_IRQSTATUS);
+	irqenable = dispc_read_reg(DISPC_IRQENABLE);
+
+	/* IRQ is not for us */
+	if (!(irqstatus & irqenable)) {
+		spin_unlock(&dispc.irq_lock);
+		return IRQ_NONE;
+	}
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
 	spin_lock(&dispc.irq_stats_lock);
@@ -2929,6 +3291,8 @@ void dispc_irq_handler(void)
 	}
 
 	spin_unlock(&dispc.irq_lock);
+
+	return IRQ_HANDLED;
 }
 
 static void dispc_error_worker(struct work_struct *work)
@@ -3246,12 +3610,14 @@ static void _omap_dispc_initial_config(void)
 {
 	u32 l;
 
-	l = dispc_read_reg(DISPC_SYSCONFIG);
-	l = FLD_MOD(l, 2, 13, 12);	/* MIDLEMODE: smart standby */
-	l = FLD_MOD(l, 2, 4, 3);	/* SIDLEMODE: smart idle */
-	l = FLD_MOD(l, 1, 2, 2);	/* ENWAKEUP */
-	l = FLD_MOD(l, 1, 0, 0);	/* AUTOIDLE */
-	dispc_write_reg(DISPC_SYSCONFIG, l);
+	/* Exclusively enable DISPC_CORE_CLK and set divider to 1 */
+	if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+		l = dispc_read_reg(DISPC_DIVISOR);
+		/* Use DISPC_DIVISOR.LCD, instead of DISPC_DIVISOR1.LCD */
+		l = FLD_MOD(l, 1, 0, 0);
+		l = FLD_MOD(l, 1, 23, 16);
+		dispc_write_reg(DISPC_DIVISOR, l);
+	}
 
 	/* FUNCGATED */
 	if (dss_has_feature(FEAT_FUNCGATED))
@@ -3267,11 +3633,28 @@ static void _omap_dispc_initial_config(void)
 	dispc_set_loadmode(OMAP_DSS_LOAD_FRAME_ONLY);
 
 	dispc_read_plane_fifo_sizes();
+
+	dispc_configure_burst_sizes();
 }
 
-int dispc_init(void)
+/* DISPC HW IP initialisation */
+static int omap_dispchw_probe(struct platform_device *pdev)
 {
 	u32 rev;
+	int r = 0;
+	struct resource *dispc_mem;
+	struct clk *clk;
+
+	dispc.pdev = pdev;
+
+	clk = clk_get(&pdev->dev, "fck");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get fck\n");
+		r = PTR_ERR(clk);
+		goto err_get_clk;
+	}
+
+	dispc.dss_clk = clk;
 
 	spin_lock_init(&dispc.irq_lock);
 
@@ -3282,80 +3665,116 @@ int dispc_init(void)
 
 	INIT_WORK(&dispc.error_work, dispc_error_worker);
 
-	dispc.base = ioremap(DISPC_BASE, DISPC_SZ_REGS);
+	dispc_mem = platform_get_resource(dispc.pdev, IORESOURCE_MEM, 0);
+	if (!dispc_mem) {
+		DSSERR("can't get IORESOURCE_MEM DISPC\n");
+		r = -EINVAL;
+		goto err_ioremap;
+	}
+	dispc.base = ioremap(dispc_mem->start, resource_size(dispc_mem));
 	if (!dispc.base) {
 		DSSERR("can't ioremap DISPC\n");
-		return -ENOMEM;
+		r = -ENOMEM;
+		goto err_ioremap;
+	}
+	dispc.irq = platform_get_irq(dispc.pdev, 0);
+	if (dispc.irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		r = -ENODEV;
+		goto err_irq;
+	}
+
+	r = request_irq(dispc.irq, omap_dispc_irq_handler, IRQF_SHARED,
+		"OMAP DISPC", dispc.pdev);
+	if (r < 0) {
+		DSSERR("request_irq failed\n");
+		goto err_irq;
 	}
 
-	enable_clocks(1);
+	pm_runtime_enable(&pdev->dev);
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err_runtime_get;
 
 	_omap_dispc_initial_config();
 
 	_omap_dispc_initialize_irq();
 
-	dispc_save_context();
-
 	rev = dispc_read_reg(DISPC_REVISION);
-	printk(KERN_INFO "OMAP DISPC rev %d.%d\n",
+	dev_dbg(&pdev->dev, "OMAP DISPC rev %d.%d\n",
 	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
-	enable_clocks(0);
+	dispc_runtime_put();
 
 	return 0;
+
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	free_irq(dispc.irq, dispc.pdev);
+err_irq:
+	iounmap(dispc.base);
+err_ioremap:
+	clk_put(dispc.dss_clk);
+err_get_clk:
+	return r;
 }
 
-void dispc_exit(void)
+static int omap_dispchw_remove(struct platform_device *pdev)
 {
+	pm_runtime_disable(&pdev->dev);
+
+	clk_put(dispc.dss_clk);
+
+	free_irq(dispc.irq, dispc.pdev);
 	iounmap(dispc.base);
+	return 0;
 }
 
-int dispc_enable_plane(enum omap_plane plane, bool enable)
+static int dispc_runtime_suspend(struct device *dev)
 {
-	DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
-
-	enable_clocks(1);
-	_dispc_enable_plane(plane, enable);
-	enable_clocks(0);
+	dispc_save_context();
+	clk_disable(dispc.dss_clk);
+	dss_runtime_put();
 
 	return 0;
 }
 
-int dispc_setup_plane(enum omap_plane plane,
-		       u32 paddr, u16 screen_width,
-		       u16 pos_x, u16 pos_y,
-		       u16 width, u16 height,
-		       u16 out_width, u16 out_height,
-		       enum omap_color_mode color_mode,
-		       bool ilace,
-		       enum omap_dss_rotation_type rotation_type,
-		       u8 rotation, bool mirror, u8 global_alpha,
-		       u8 pre_mult_alpha, enum omap_channel channel)
+static int dispc_runtime_resume(struct device *dev)
 {
-	int r = 0;
+	int r;
 
-	DSSDBG("dispc_setup_plane %d, pa %x, sw %d, %d,%d, %dx%d -> "
-	       "%dx%d, ilace %d, cmode %x, rot %d, mir %d chan %d\n",
-	       plane, paddr, screen_width, pos_x, pos_y,
-	       width, height,
-	       out_width, out_height,
-	       ilace, color_mode,
-	       rotation, mirror, channel);
+	r = dss_runtime_get();
+	if (r < 0)
+		return r;
 
-	enable_clocks(1);
+	clk_enable(dispc.dss_clk);
+	dispc_restore_context();
 
-	r = _dispc_setup_plane(plane,
-			   paddr, screen_width,
-			   pos_x, pos_y,
-			   width, height,
-			   out_width, out_height,
-			   color_mode, ilace,
-			   rotation_type,
-			   rotation, mirror,
-			   global_alpha,
-			   pre_mult_alpha, channel);
+	return 0;
+}
 
-	enable_clocks(0);
+static const struct dev_pm_ops dispc_pm_ops = {
+	.runtime_suspend = dispc_runtime_suspend,
+	.runtime_resume = dispc_runtime_resume,
+};
 
-	return r;
+static struct platform_driver omap_dispchw_driver = {
+	.probe          = omap_dispchw_probe,
+	.remove         = omap_dispchw_remove,
+	.driver         = {
+		.name   = "omapdss_dispc",
+		.owner  = THIS_MODULE,
+		.pm	= &dispc_pm_ops,
+	},
+};
+
+int dispc_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dispchw_driver);
+}
+
+void dispc_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_dispchw_driver);
 }
diff --git a/drivers/video/omap2/dss/dispc.h b/drivers/video/omap2/dss/dispc.h
new file mode 100644
index 000000000000..6c9ee0a0efb3
--- /dev/null
+++ b/drivers/video/omap2/dss/dispc.h
@@ -0,0 +1,691 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc.h
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Archit Taneja <archit@ti.com>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OMAP2_DISPC_REG_H
+#define __OMAP2_DISPC_REG_H
+
+/* DISPC common registers */
+#define DISPC_REVISION			0x0000
+#define DISPC_SYSCONFIG			0x0010
+#define DISPC_SYSSTATUS			0x0014
+#define DISPC_IRQSTATUS			0x0018
+#define DISPC_IRQENABLE			0x001C
+#define DISPC_CONTROL			0x0040
+#define DISPC_CONFIG			0x0044
+#define DISPC_CAPABLE			0x0048
+#define DISPC_LINE_STATUS		0x005C
+#define DISPC_LINE_NUMBER		0x0060
+#define DISPC_GLOBAL_ALPHA		0x0074
+#define DISPC_CONTROL2			0x0238
+#define DISPC_CONFIG2			0x0620
+#define DISPC_DIVISOR			0x0804
+
+/* DISPC overlay registers */
+#define DISPC_OVL_BA0(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA0_OFFSET(n))
+#define DISPC_OVL_BA1(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA1_OFFSET(n))
+#define DISPC_OVL_BA0_UV(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA0_UV_OFFSET(n))
+#define DISPC_OVL_BA1_UV(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA1_UV_OFFSET(n))
+#define DISPC_OVL_POSITION(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_POS_OFFSET(n))
+#define DISPC_OVL_SIZE(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_SIZE_OFFSET(n))
+#define DISPC_OVL_ATTRIBUTES(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ATTR_OFFSET(n))
+#define DISPC_OVL_ATTRIBUTES2(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_ATTR2_OFFSET(n))
+#define DISPC_OVL_FIFO_THRESHOLD(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIFO_THRESH_OFFSET(n))
+#define DISPC_OVL_FIFO_SIZE_STATUS(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIFO_SIZE_STATUS_OFFSET(n))
+#define DISPC_OVL_ROW_INC(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ROW_INC_OFFSET(n))
+#define DISPC_OVL_PIXEL_INC(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_PIX_INC_OFFSET(n))
+#define DISPC_OVL_WINDOW_SKIP(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_WINDOW_SKIP_OFFSET(n))
+#define DISPC_OVL_TABLE_BA(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_TABLE_BA_OFFSET(n))
+#define DISPC_OVL_FIR(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_OFFSET(n))
+#define DISPC_OVL_FIR2(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_FIR2_OFFSET(n))
+#define DISPC_OVL_PICTURE_SIZE(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_PIC_SIZE_OFFSET(n))
+#define DISPC_OVL_ACCU0(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU0_OFFSET(n))
+#define DISPC_OVL_ACCU1(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU1_OFFSET(n))
+#define DISPC_OVL_ACCU2_0(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU2_0_OFFSET(n))
+#define DISPC_OVL_ACCU2_1(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU2_1_OFFSET(n))
+#define DISPC_OVL_FIR_COEF_H(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_H_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_HV(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_HV_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_H2(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_H2_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_HV2(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_HV2_OFFSET(n, i))
+#define DISPC_OVL_CONV_COEF(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_CONV_COEF_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_V(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_V_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_V2(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_V2_OFFSET(n, i))
+#define DISPC_OVL_PRELOAD(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_PRELOAD_OFFSET(n))
+
+/* DISPC manager/channel specific registers */
+static inline u16 DISPC_DEFAULT_COLOR(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x004C;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		return 0x0050;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03AC;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_TRANS_COLOR(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0054;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		return 0x0058;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03B0;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_TIMING_H(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0064;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x0400;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_TIMING_V(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0068;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x0404;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_POL_FREQ(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x006C;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x0408;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_DIVISORo(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0070;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x040C;
+	default:
+		BUG();
+	}
+}
+
+/* Named as DISPC_SIZE_LCD, DISPC_SIZE_DIGIT and DISPC_SIZE_LCD2 in TRM */
+static inline u16 DISPC_SIZE_MGR(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x007C;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		return 0x0078;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03CC;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_DATA_CYCLE1(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x01D4;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03C0;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_DATA_CYCLE2(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x01D8;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03C4;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_DATA_CYCLE3(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x01DC;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03C8;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_CPR_COEF_R(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0220;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03BC;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_CPR_COEF_G(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0224;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03B8;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_CPR_COEF_B(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0228;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03B4;
+	default:
+		BUG();
+	}
+}
+
+/* DISPC overlay register base addresses */
+static inline u16 DISPC_OVL_BASE(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0080;
+	case OMAP_DSS_VIDEO1:
+		return 0x00BC;
+	case OMAP_DSS_VIDEO2:
+		return 0x014C;
+	default:
+		BUG();
+	}
+}
+
+/* DISPC overlay register offsets */
+static inline u16 DISPC_BA0_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0000;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_BA1_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0004;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_BA0_UV_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0544;
+	case OMAP_DSS_VIDEO2:
+		return 0x04BC;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_BA1_UV_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0548;
+	case OMAP_DSS_VIDEO2:
+		return 0x04C0;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_POS_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0008;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_SIZE_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x000C;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_ATTR_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0020;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0010;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_ATTR2_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0568;
+	case OMAP_DSS_VIDEO2:
+		return 0x04DC;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_FIFO_THRESH_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0024;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0014;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_FIFO_SIZE_STATUS_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0028;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0018;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_ROW_INC_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x002C;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x001C;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_PIX_INC_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0030;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0020;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_WINDOW_SKIP_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0034;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		BUG();
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_TABLE_BA_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0038;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		BUG();
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_FIR_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0024;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_FIR2_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0580;
+	case OMAP_DSS_VIDEO2:
+		return 0x055C;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_PIC_SIZE_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0028;
+	default:
+		BUG();
+	}
+}
+
+
+static inline u16 DISPC_ACCU0_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x002C;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_ACCU2_0_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0584;
+	case OMAP_DSS_VIDEO2:
+		return 0x0560;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_ACCU1_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0030;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_ACCU2_1_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0588;
+	case OMAP_DSS_VIDEO2:
+		return 0x0564;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_H_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0034 + i * 0x8;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_H2_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x058C + i * 0x8;
+	case OMAP_DSS_VIDEO2:
+		return 0x0568 + i * 0x8;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_HV_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0038 + i * 0x8;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_HV2_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0590 + i * 8;
+	case OMAP_DSS_VIDEO2:
+		return 0x056C + i * 0x8;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4,} */
+static inline u16 DISPC_CONV_COEF_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0074 + i * 0x4;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_V_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x0124 + i * 0x4;
+	case OMAP_DSS_VIDEO2:
+		return 0x00B4 + i * 0x4;
+	default:
+		BUG();
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_V2_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+	case OMAP_DSS_VIDEO1:
+		return 0x05CC + i * 0x4;
+	case OMAP_DSS_VIDEO2:
+		return 0x05A8 + i * 0x4;
+	default:
+		BUG();
+	}
+}
+
+static inline u16 DISPC_PRELOAD_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x01AC;
+	case OMAP_DSS_VIDEO1:
+		return 0x0174;
+	case OMAP_DSS_VIDEO2:
+		return 0x00E8;
+	default:
+		BUG();
+	}
+}
+#endif
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 22dd7a474f79..94495e45ec5a 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -25,13 +25,11 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/jiffies.h>
-#include <linux/list.h>
 #include <linux/platform_device.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include "dss.h"
-
-static LIST_HEAD(display_list);
+#include "dss_features.h"
 
 static ssize_t display_enabled_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
@@ -47,9 +45,13 @@ static ssize_t display_enabled_store(struct device *dev,
 		const char *buf, size_t size)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	bool enabled, r;
+	int r, enabled;
 
-	enabled = simple_strtoul(buf, NULL, 10);
+	r = kstrtoint(buf, 0, &enabled);
+	if (r)
+		return r;
+
+	enabled = !!enabled;
 
 	if (enabled != (dssdev->state != OMAP_DSS_DISPLAY_DISABLED)) {
 		if (enabled) {
@@ -64,46 +66,6 @@ static ssize_t display_enabled_store(struct device *dev,
 	return size;
 }
 
-static ssize_t display_upd_mode_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct omap_dss_device *dssdev = to_dss_device(dev);
-	enum omap_dss_update_mode mode = OMAP_DSS_UPDATE_AUTO;
-	if (dssdev->driver->get_update_mode)
-		mode = dssdev->driver->get_update_mode(dssdev);
-	return snprintf(buf, PAGE_SIZE, "%d\n", mode);
-}
-
-static ssize_t display_upd_mode_store(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t size)
-{
-	struct omap_dss_device *dssdev = to_dss_device(dev);
-	int val, r;
-	enum omap_dss_update_mode mode;
-
-	if (!dssdev->driver->set_update_mode)
-		return -EINVAL;
-
-	val = simple_strtoul(buf, NULL, 10);
-
-	switch (val) {
-	case OMAP_DSS_UPDATE_DISABLED:
-	case OMAP_DSS_UPDATE_AUTO:
-	case OMAP_DSS_UPDATE_MANUAL:
-		mode = (enum omap_dss_update_mode)val;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	r = dssdev->driver->set_update_mode(dssdev, mode);
-	if (r)
-		return r;
-
-	return size;
-}
-
 static ssize_t display_tear_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -117,13 +79,16 @@ static ssize_t display_tear_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	unsigned long te;
-	int r;
+	int te, r;
 
 	if (!dssdev->driver->enable_te || !dssdev->driver->get_te)
 		return -ENOENT;
 
-	te = simple_strtoul(buf, NULL, 0);
+	r = kstrtoint(buf, 0, &te);
+	if (r)
+		return r;
+
+	te = !!te;
 
 	r = dssdev->driver->enable_te(dssdev, te);
 	if (r)
@@ -199,13 +164,14 @@ static ssize_t display_rotate_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	unsigned long rot;
-	int r;
+	int rot, r;
 
 	if (!dssdev->driver->set_rotate || !dssdev->driver->get_rotate)
 		return -ENOENT;
 
-	rot = simple_strtoul(buf, NULL, 0);
+	r = kstrtoint(buf, 0, &rot);
+	if (r)
+		return r;
 
 	r = dssdev->driver->set_rotate(dssdev, rot);
 	if (r)
@@ -229,13 +195,16 @@ static ssize_t display_mirror_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	unsigned long mirror;
-	int r;
+	int mirror, r;
 
 	if (!dssdev->driver->set_mirror || !dssdev->driver->get_mirror)
 		return -ENOENT;
 
-	mirror = simple_strtoul(buf, NULL, 0);
+	r = kstrtoint(buf, 0, &mirror);
+	if (r)
+		return r;
+
+	mirror = !!mirror;
 
 	r = dssdev->driver->set_mirror(dssdev, mirror);
 	if (r)
@@ -262,14 +231,15 @@ static ssize_t display_wss_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	unsigned long wss;
+	u32 wss;
 	int r;
 
 	if (!dssdev->driver->get_wss || !dssdev->driver->set_wss)
 		return -ENOENT;
 
-	if (strict_strtoul(buf, 0, &wss))
-		return -EINVAL;
+	r = kstrtou32(buf, 0, &wss);
+	if (r)
+		return r;
 
 	if (wss > 0xfffff)
 		return -EINVAL;
@@ -283,8 +253,6 @@ static ssize_t display_wss_store(struct device *dev,
 
 static DEVICE_ATTR(enabled, S_IRUGO|S_IWUSR,
 		display_enabled_show, display_enabled_store);
-static DEVICE_ATTR(update_mode, S_IRUGO|S_IWUSR,
-		display_upd_mode_show, display_upd_mode_store);
 static DEVICE_ATTR(tear_elim, S_IRUGO|S_IWUSR,
 		display_tear_show, display_tear_store);
 static DEVICE_ATTR(timings, S_IRUGO|S_IWUSR,
@@ -298,7 +266,6 @@ static DEVICE_ATTR(wss, S_IRUGO|S_IWUSR,
 
 static struct device_attribute *display_sysfs_attrs[] = {
 	&dev_attr_enabled,
-	&dev_attr_update_mode,
 	&dev_attr_tear_elim,
 	&dev_attr_timings,
 	&dev_attr_rotate,
@@ -316,16 +283,13 @@ void omapdss_default_get_resolution(struct omap_dss_device *dssdev,
 EXPORT_SYMBOL(omapdss_default_get_resolution);
 
 void default_get_overlay_fifo_thresholds(enum omap_plane plane,
-		u32 fifo_size, enum omap_burst_size *burst_size,
+		u32 fifo_size, u32 burst_size,
 		u32 *fifo_low, u32 *fifo_high)
 {
-	unsigned burst_size_bytes;
-
-	*burst_size = OMAP_DSS_BURST_16x32;
-	burst_size_bytes = 16 * 32 / 8;
+	unsigned buf_unit = dss_feat_get_buffer_size_unit();
 
-	*fifo_high = fifo_size - 1;
-	*fifo_low = fifo_size - burst_size_bytes;
+	*fifo_high = fifo_size - buf_unit;
+	*fifo_low = fifo_size - burst_size;
 }
 
 int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
@@ -345,6 +309,7 @@ int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
 			return 16;
 	case OMAP_DISPLAY_TYPE_VENC:
 	case OMAP_DISPLAY_TYPE_SDI:
+	case OMAP_DISPLAY_TYPE_HDMI:
 		return 24;
 	default:
 		BUG();
@@ -371,6 +336,7 @@ bool dss_use_replication(struct omap_dss_device *dssdev,
 	case OMAP_DISPLAY_TYPE_DPI:
 		bpp = dssdev->phy.dpi.data_lines;
 		break;
+	case OMAP_DISPLAY_TYPE_HDMI:
 	case OMAP_DISPLAY_TYPE_VENC:
 	case OMAP_DISPLAY_TYPE_SDI:
 		bpp = 24;
@@ -396,29 +362,6 @@ void dss_init_device(struct platform_device *pdev,
 	switch (dssdev->type) {
 #ifdef CONFIG_OMAP2_DSS_DPI
 	case OMAP_DISPLAY_TYPE_DPI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_RFBI
-	case OMAP_DISPLAY_TYPE_DBI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_SDI
-	case OMAP_DISPLAY_TYPE_SDI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_DSI
-	case OMAP_DISPLAY_TYPE_DSI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_VENC
-	case OMAP_DISPLAY_TYPE_VENC:
-#endif
-		break;
-	default:
-		DSSERR("Support for display '%s' not compiled in.\n",
-				dssdev->name);
-		return;
-	}
-
-	switch (dssdev->type) {
-#ifdef CONFIG_OMAP2_DSS_DPI
-	case OMAP_DISPLAY_TYPE_DPI:
 		r = dpi_init_display(dssdev);
 		break;
 #endif
@@ -442,8 +385,13 @@ void dss_init_device(struct platform_device *pdev,
 		r = dsi_init_display(dssdev);
 		break;
 #endif
+	case OMAP_DISPLAY_TYPE_HDMI:
+		r = hdmi_init_display(dssdev);
+		break;
 	default:
-		BUG();
+		DSSERR("Support for display '%s' not compiled in.\n",
+				dssdev->name);
+		return;
 	}
 
 	if (r) {
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 75fb0a515430..f053b180ecd7 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -23,23 +23,46 @@
 #define DSS_SUBSYS_NAME "DPI"
 
 #include <linux/kernel.h>
-#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
 
 static struct {
 	struct regulator *vdds_dsi_reg;
+	struct platform_device *dsidev;
 } dpi;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
+static struct platform_device *dpi_get_dsidev(enum omap_dss_clk_source clk)
+{
+	int dsi_module;
+
+	dsi_module = clk == OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC ? 0 : 1;
+
+	return dsi_get_dsidev_from_id(dsi_module);
+}
+
+static bool dpi_use_dsi_pll(struct omap_dss_device *dssdev)
+{
+	if (dssdev->clocks.dispc.dispc_fclk_src ==
+			OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC ||
+			dssdev->clocks.dispc.dispc_fclk_src ==
+			OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC ||
+			dssdev->clocks.dispc.channel.lcd_clk_src ==
+			OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC ||
+			dssdev->clocks.dispc.channel.lcd_clk_src ==
+			OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC)
+		return true;
+	else
+		return false;
+}
+
 static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
 		unsigned long pck_req, unsigned long *fck, int *lck_div,
 		int *pck_div)
@@ -48,28 +71,28 @@ static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
 	struct dispc_clock_info dispc_cinfo;
 	int r;
 
-	r = dsi_pll_calc_clock_div_pck(is_tft, pck_req, &dsi_cinfo,
-			&dispc_cinfo);
+	r = dsi_pll_calc_clock_div_pck(dpi.dsidev, is_tft, pck_req,
+			&dsi_cinfo, &dispc_cinfo);
 	if (r)
 		return r;
 
-	r = dsi_pll_set_clock_div(&dsi_cinfo);
+	r = dsi_pll_set_clock_div(dpi.dsidev, &dsi_cinfo);
 	if (r)
 		return r;
 
-	dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
+	dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
 
 	r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
 	if (r)
 		return r;
 
-	*fck = dsi_cinfo.dsi1_pll_fclk;
+	*fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
 	*lck_div = dispc_cinfo.lck_div;
 	*pck_div = dispc_cinfo.pck_div;
 
 	return 0;
 }
-#else
+
 static int dpi_set_dispc_clk(struct omap_dss_device *dssdev, bool is_tft,
 		unsigned long pck_req, unsigned long *fck, int *lck_div,
 		int *pck_div)
@@ -96,33 +119,29 @@ static int dpi_set_dispc_clk(struct omap_dss_device *dssdev, bool is_tft,
 
 	return 0;
 }
-#endif
 
 static int dpi_set_mode(struct omap_dss_device *dssdev)
 {
 	struct omap_video_timings *t = &dssdev->panel.timings;
-	int lck_div, pck_div;
-	unsigned long fck;
+	int lck_div = 0, pck_div = 0;
+	unsigned long fck = 0;
 	unsigned long pck;
 	bool is_tft;
 	int r = 0;
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-
 	dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
 			dssdev->panel.acbi, dssdev->panel.acb);
 
 	is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	r = dpi_set_dsi_clk(dssdev, is_tft, t->pixel_clock * 1000, &fck,
-			&lck_div, &pck_div);
-#else
-	r = dpi_set_dispc_clk(dssdev, is_tft, t->pixel_clock * 1000, &fck,
-			&lck_div, &pck_div);
-#endif
+	if (dpi_use_dsi_pll(dssdev))
+		r = dpi_set_dsi_clk(dssdev, is_tft, t->pixel_clock * 1000,
+				&fck, &lck_div, &pck_div);
+	else
+		r = dpi_set_dispc_clk(dssdev, is_tft, t->pixel_clock * 1000,
+				&fck, &lck_div, &pck_div);
 	if (r)
-		goto err0;
+		return r;
 
 	pck = fck / lck_div / pck_div / 1000;
 
@@ -136,12 +155,10 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 
 	dispc_set_lcd_timings(dssdev->manager->id, t);
 
-err0:
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
-	return r;
+	return 0;
 }
 
-static int dpi_basic_init(struct omap_dss_device *dssdev)
+static void dpi_basic_init(struct omap_dss_device *dssdev)
 {
 	bool is_tft;
 
@@ -153,8 +170,6 @@ static int dpi_basic_init(struct omap_dss_device *dssdev)
 			OMAP_DSS_LCD_DISPLAY_TFT : OMAP_DSS_LCD_DISPLAY_STN);
 	dispc_set_tft_data_lines(dssdev->manager->id,
 			dssdev->phy.dpi.data_lines);
-
-	return 0;
 }
 
 int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
@@ -164,30 +179,38 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 	r = omap_dss_start_device(dssdev);
 	if (r) {
 		DSSERR("failed to start device\n");
-		goto err0;
+		goto err_start_dev;
 	}
 
 	if (cpu_is_omap34xx()) {
 		r = regulator_enable(dpi.vdds_dsi_reg);
 		if (r)
-			goto err1;
+			goto err_reg_enable;
 	}
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-	r = dpi_basic_init(dssdev);
+	r = dss_runtime_get();
 	if (r)
-		goto err2;
+		goto err_get_dss;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	dss_clk_enable(DSS_CLK_FCK2);
-	r = dsi_pll_init(dssdev, 0, 1);
+	r = dispc_runtime_get();
 	if (r)
-		goto err3;
-#endif
+		goto err_get_dispc;
+
+	dpi_basic_init(dssdev);
+
+	if (dpi_use_dsi_pll(dssdev)) {
+		r = dsi_runtime_get(dpi.dsidev);
+		if (r)
+			goto err_get_dsi;
+
+		r = dsi_pll_init(dpi.dsidev, 0, 1);
+		if (r)
+			goto err_dsi_pll_init;
+	}
+
 	r = dpi_set_mode(dssdev);
 	if (r)
-		goto err4;
+		goto err_set_mode;
 
 	mdelay(2);
 
@@ -195,19 +218,22 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 
 	return 0;
 
-err4:
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	dsi_pll_uninit();
-err3:
-	dss_clk_disable(DSS_CLK_FCK2);
-#endif
-err2:
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+err_set_mode:
+	if (dpi_use_dsi_pll(dssdev))
+		dsi_pll_uninit(dpi.dsidev, true);
+err_dsi_pll_init:
+	if (dpi_use_dsi_pll(dssdev))
+		dsi_runtime_put(dpi.dsidev);
+err_get_dsi:
+	dispc_runtime_put();
+err_get_dispc:
+	dss_runtime_put();
+err_get_dss:
 	if (cpu_is_omap34xx())
 		regulator_disable(dpi.vdds_dsi_reg);
-err1:
+err_reg_enable:
 	omap_dss_stop_device(dssdev);
-err0:
+err_start_dev:
 	return r;
 }
 EXPORT_SYMBOL(omapdss_dpi_display_enable);
@@ -216,13 +242,14 @@ void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 {
 	dssdev->manager->disable(dssdev->manager);
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-	dsi_pll_uninit();
-	dss_clk_disable(DSS_CLK_FCK2);
-#endif
+	if (dpi_use_dsi_pll(dssdev)) {
+		dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+		dsi_pll_uninit(dpi.dsidev, true);
+		dsi_runtime_put(dpi.dsidev);
+	}
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dispc_runtime_put();
+	dss_runtime_put();
 
 	if (cpu_is_omap34xx())
 		regulator_disable(dpi.vdds_dsi_reg);
@@ -234,11 +261,26 @@ EXPORT_SYMBOL(omapdss_dpi_display_disable);
 void dpi_set_timings(struct omap_dss_device *dssdev,
 			struct omap_video_timings *timings)
 {
+	int r;
+
 	DSSDBG("dpi_set_timings\n");
 	dssdev->panel.timings = *timings;
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+		r = dss_runtime_get();
+		if (r)
+			return;
+
+		r = dispc_runtime_get();
+		if (r) {
+			dss_runtime_put();
+			return;
+		}
+
 		dpi_set_mode(dssdev);
 		dispc_go(dssdev->manager->id);
+
+		dispc_runtime_put();
+		dss_runtime_put();
 	}
 }
 EXPORT_SYMBOL(dpi_set_timings);
@@ -251,6 +293,7 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 	int lck_div, pck_div;
 	unsigned long fck;
 	unsigned long pck;
+	struct dispc_clock_info dispc_cinfo;
 
 	if (!dispc_lcd_timings_ok(timings))
 		return -EINVAL;
@@ -260,25 +303,18 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 
 	is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	{
+	if (dpi_use_dsi_pll(dssdev)) {
 		struct dsi_clock_info dsi_cinfo;
-		struct dispc_clock_info dispc_cinfo;
-		r = dsi_pll_calc_clock_div_pck(is_tft,
+		r = dsi_pll_calc_clock_div_pck(dpi.dsidev, is_tft,
 				timings->pixel_clock * 1000,
 				&dsi_cinfo, &dispc_cinfo);
 
 		if (r)
 			return r;
 
-		fck = dsi_cinfo.dsi1_pll_fclk;
-		lck_div = dispc_cinfo.lck_div;
-		pck_div = dispc_cinfo.pck_div;
-	}
-#else
-	{
+		fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
+	} else {
 		struct dss_clock_info dss_cinfo;
-		struct dispc_clock_info dispc_cinfo;
 		r = dss_calc_clock_div(is_tft, timings->pixel_clock * 1000,
 				&dss_cinfo, &dispc_cinfo);
 
@@ -286,10 +322,10 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 			return r;
 
 		fck = dss_cinfo.fck;
-		lck_div = dispc_cinfo.lck_div;
-		pck_div = dispc_cinfo.pck_div;
 	}
-#endif
+
+	lck_div = dispc_cinfo.lck_div;
+	pck_div = dispc_cinfo.pck_div;
 
 	pck = fck / lck_div / pck_div / 1000;
 
@@ -303,19 +339,30 @@ int dpi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("init_display\n");
 
-	return 0;
-}
+	if (cpu_is_omap34xx() && dpi.vdds_dsi_reg == NULL) {
+		struct regulator *vdds_dsi;
 
-int dpi_init(struct platform_device *pdev)
-{
-	if (cpu_is_omap34xx()) {
-		dpi.vdds_dsi_reg = dss_get_vdds_dsi();
-		if (IS_ERR(dpi.vdds_dsi_reg)) {
+		vdds_dsi = dss_get_vdds_dsi();
+
+		if (IS_ERR(vdds_dsi)) {
 			DSSERR("can't get VDDS_DSI regulator\n");
-			return PTR_ERR(dpi.vdds_dsi_reg);
+			return PTR_ERR(vdds_dsi);
 		}
+
+		dpi.vdds_dsi_reg = vdds_dsi;
 	}
 
+	if (dpi_use_dsi_pll(dssdev)) {
+		enum omap_dss_clk_source dispc_fclk_src =
+			dssdev->clocks.dispc.dispc_fclk_src;
+		dpi.dsidev = dpi_get_dsidev(dispc_fclk_src);
+	}
+
+	return 0;
+}
+
+int dpi_init(void)
+{
 	return 0;
 }
 
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index ddf3a0560822..7adbbeb84334 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -33,17 +33,20 @@
 #include <linux/regulator/consumer.h>
 #include <linux/wait.h>
 #include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/clock.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 /*#define VERBOSE_IRQ*/
 #define DSI_CATCH_MISSING_TE
 
-#define DSI_BASE		0x4804FC00
-
 struct dsi_reg { u16 idx; };
 
 #define DSI_REG(idx)		((const struct dsi_reg) { idx })
@@ -57,6 +60,7 @@ struct dsi_reg { u16 idx; };
 #define DSI_IRQSTATUS			DSI_REG(0x0018)
 #define DSI_IRQENABLE			DSI_REG(0x001C)
 #define DSI_CTRL			DSI_REG(0x0040)
+#define DSI_GNQ				DSI_REG(0x0044)
 #define DSI_COMPLEXIO_CFG1		DSI_REG(0x0048)
 #define DSI_COMPLEXIO_IRQ_STATUS	DSI_REG(0x004C)
 #define DSI_COMPLEXIO_IRQ_ENABLE	DSI_REG(0x0050)
@@ -91,6 +95,7 @@ struct dsi_reg { u16 idx; };
 #define DSI_DSIPHY_CFG1			DSI_REG(0x200 + 0x0004)
 #define DSI_DSIPHY_CFG2			DSI_REG(0x200 + 0x0008)
 #define DSI_DSIPHY_CFG5			DSI_REG(0x200 + 0x0014)
+#define DSI_DSIPHY_CFG10		DSI_REG(0x200 + 0x0028)
 
 /* DSI_PLL_CTRL_SCP */
 
@@ -100,11 +105,11 @@ struct dsi_reg { u16 idx; };
 #define DSI_PLL_CONFIGURATION1		DSI_REG(0x300 + 0x000C)
 #define DSI_PLL_CONFIGURATION2		DSI_REG(0x300 + 0x0010)
 
-#define REG_GET(idx, start, end) \
-	FLD_GET(dsi_read_reg(idx), start, end)
+#define REG_GET(dsidev, idx, start, end) \
+	FLD_GET(dsi_read_reg(dsidev, idx), start, end)
 
-#define REG_FLD_MOD(idx, val, start, end) \
-	dsi_write_reg(idx, FLD_MOD(dsi_read_reg(idx), val, start, end))
+#define REG_FLD_MOD(dsidev, idx, val, start, end) \
+	dsi_write_reg(dsidev, idx, FLD_MOD(dsi_read_reg(dsidev, idx), val, start, end))
 
 /* Global interrupts */
 #define DSI_IRQ_VC0		(1 << 0)
@@ -148,31 +153,50 @@ struct dsi_reg { u16 idx; };
 #define DSI_CIO_IRQ_ERRSYNCESC1		(1 << 0)
 #define DSI_CIO_IRQ_ERRSYNCESC2		(1 << 1)
 #define DSI_CIO_IRQ_ERRSYNCESC3		(1 << 2)
+#define DSI_CIO_IRQ_ERRSYNCESC4		(1 << 3)
+#define DSI_CIO_IRQ_ERRSYNCESC5		(1 << 4)
 #define DSI_CIO_IRQ_ERRESC1		(1 << 5)
 #define DSI_CIO_IRQ_ERRESC2		(1 << 6)
 #define DSI_CIO_IRQ_ERRESC3		(1 << 7)
+#define DSI_CIO_IRQ_ERRESC4		(1 << 8)
+#define DSI_CIO_IRQ_ERRESC5		(1 << 9)
 #define DSI_CIO_IRQ_ERRCONTROL1		(1 << 10)
 #define DSI_CIO_IRQ_ERRCONTROL2		(1 << 11)
 #define DSI_CIO_IRQ_ERRCONTROL3		(1 << 12)
+#define DSI_CIO_IRQ_ERRCONTROL4		(1 << 13)
+#define DSI_CIO_IRQ_ERRCONTROL5		(1 << 14)
 #define DSI_CIO_IRQ_STATEULPS1		(1 << 15)
 #define DSI_CIO_IRQ_STATEULPS2		(1 << 16)
 #define DSI_CIO_IRQ_STATEULPS3		(1 << 17)
+#define DSI_CIO_IRQ_STATEULPS4		(1 << 18)
+#define DSI_CIO_IRQ_STATEULPS5		(1 << 19)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP0_1	(1 << 20)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_1	(1 << 21)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP0_2	(1 << 22)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_2	(1 << 23)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP0_3	(1 << 24)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_3	(1 << 25)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_4	(1 << 26)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_4	(1 << 27)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_5	(1 << 28)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_5	(1 << 29)
 #define DSI_CIO_IRQ_ULPSACTIVENOT_ALL0	(1 << 30)
 #define DSI_CIO_IRQ_ULPSACTIVENOT_ALL1	(1 << 31)
 #define DSI_CIO_IRQ_ERROR_MASK \
 	(DSI_CIO_IRQ_ERRSYNCESC1 | DSI_CIO_IRQ_ERRSYNCESC2 | \
-	 DSI_CIO_IRQ_ERRSYNCESC3 | DSI_CIO_IRQ_ERRESC1 | DSI_CIO_IRQ_ERRESC2 | \
-	 DSI_CIO_IRQ_ERRESC3 | DSI_CIO_IRQ_ERRCONTROL1 | \
-	 DSI_CIO_IRQ_ERRCONTROL2 | DSI_CIO_IRQ_ERRCONTROL3 | \
+	 DSI_CIO_IRQ_ERRSYNCESC3 | DSI_CIO_IRQ_ERRSYNCESC4 | \
+	 DSI_CIO_IRQ_ERRSYNCESC5 | \
+	 DSI_CIO_IRQ_ERRESC1 | DSI_CIO_IRQ_ERRESC2 | \
+	 DSI_CIO_IRQ_ERRESC3 | DSI_CIO_IRQ_ERRESC4 | \
+	 DSI_CIO_IRQ_ERRESC5 | \
+	 DSI_CIO_IRQ_ERRCONTROL1 | DSI_CIO_IRQ_ERRCONTROL2 | \
+	 DSI_CIO_IRQ_ERRCONTROL3 | DSI_CIO_IRQ_ERRCONTROL4 | \
+	 DSI_CIO_IRQ_ERRCONTROL5 | \
 	 DSI_CIO_IRQ_ERRCONTENTIONLP0_1 | DSI_CIO_IRQ_ERRCONTENTIONLP1_1 | \
 	 DSI_CIO_IRQ_ERRCONTENTIONLP0_2 | DSI_CIO_IRQ_ERRCONTENTIONLP1_2 | \
-	 DSI_CIO_IRQ_ERRCONTENTIONLP0_3 | DSI_CIO_IRQ_ERRCONTENTIONLP1_3)
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_3 | DSI_CIO_IRQ_ERRCONTENTIONLP1_3 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_4 | DSI_CIO_IRQ_ERRCONTENTIONLP1_4 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_5 | DSI_CIO_IRQ_ERRCONTENTIONLP1_5)
 
 #define DSI_DT_DCS_SHORT_WRITE_0	0x05
 #define DSI_DT_DCS_SHORT_WRITE_1	0x15
@@ -186,13 +210,15 @@ struct dsi_reg { u16 idx; };
 #define DSI_DT_RX_SHORT_READ_1		0x21
 #define DSI_DT_RX_SHORT_READ_2		0x22
 
-#define FINT_MAX 2100000
-#define FINT_MIN 750000
-#define REGN_MAX (1 << 7)
-#define REGM_MAX ((1 << 11) - 1)
-#define REGM3_MAX (1 << 4)
-#define REGM4_MAX (1 << 4)
-#define LP_DIV_MAX ((1 << 13) - 1)
+typedef void (*omap_dsi_isr_t) (void *arg, u32 mask);
+
+#define DSI_MAX_NR_ISRS                2
+
+struct dsi_isr_data {
+	omap_dsi_isr_t	isr;
+	void		*arg;
+	u32		mask;
+};
 
 enum fifo_size {
 	DSI_FIFO_SIZE_0		= 0,
@@ -207,6 +233,19 @@ enum dsi_vc_mode {
 	DSI_VC_MODE_VP,
 };
 
+enum dsi_lane {
+	DSI_CLK_P	= 1 << 0,
+	DSI_CLK_N	= 1 << 1,
+	DSI_DATA1_P	= 1 << 2,
+	DSI_DATA1_N	= 1 << 3,
+	DSI_DATA2_P	= 1 << 4,
+	DSI_DATA2_N	= 1 << 5,
+	DSI_DATA3_P	= 1 << 6,
+	DSI_DATA3_N	= 1 << 7,
+	DSI_DATA4_P	= 1 << 8,
+	DSI_DATA4_N	= 1 << 9,
+};
+
 struct dsi_update_region {
 	u16 x, y, w, h;
 	struct omap_dss_device *device;
@@ -220,18 +259,33 @@ struct dsi_irq_stats {
 	unsigned cio_irqs[32];
 };
 
-static struct
-{
+struct dsi_isr_tables {
+	struct dsi_isr_data isr_table[DSI_MAX_NR_ISRS];
+	struct dsi_isr_data isr_table_vc[4][DSI_MAX_NR_ISRS];
+	struct dsi_isr_data isr_table_cio[DSI_MAX_NR_ISRS];
+};
+
+struct dsi_data {
+	struct platform_device *pdev;
 	void __iomem	*base;
 
+	int irq;
+
+	struct clk *dss_clk;
+	struct clk *sys_clk;
+
+	void (*dsi_mux_pads)(bool enable);
+
 	struct dsi_clock_info current_cinfo;
 
+	bool vdds_dsi_enabled;
 	struct regulator *vdds_dsi_reg;
 
 	struct {
 		enum dsi_vc_mode mode;
 		struct omap_dss_device *dssdev;
 		enum fifo_size fifo_size;
+		int vc_id;
 	} vc[4];
 
 	struct mutex lock;
@@ -239,15 +293,16 @@ static struct
 
 	unsigned pll_locked;
 
-	struct completion bta_completion;
-	void (*bta_callback)(void);
+	spinlock_t irq_lock;
+	struct dsi_isr_tables isr_tables;
+	/* space for a copy used by the interrupt handler */
+	struct dsi_isr_tables isr_tables_copy;
 
 	int update_channel;
 	struct dsi_update_region update_region;
 
 	bool te_enabled;
-
-	struct workqueue_struct *workqueue;
+	bool ulps_enabled;
 
 	void (*framedone_callback)(int, void *);
 	void *framedone_data;
@@ -275,55 +330,106 @@ static struct
 	spinlock_t irq_stats_lock;
 	struct dsi_irq_stats irq_stats;
 #endif
-} dsi;
+	/* DSI PLL Parameter Ranges */
+	unsigned long regm_max, regn_max;
+	unsigned long  regm_dispc_max, regm_dsi_max;
+	unsigned long  fint_min, fint_max;
+	unsigned long lpdiv_max;
+
+	int num_data_lanes;
+
+	unsigned scp_clk_refcount;
+};
+
+struct dsi_packet_sent_handler_data {
+	struct platform_device *dsidev;
+	struct completion *completion;
+};
+
+static struct platform_device *dsi_pdev_map[MAX_NUM_DSI];
 
 #ifdef DEBUG
 static unsigned int dsi_perf;
 module_param_named(dsi_perf, dsi_perf, bool, 0644);
 #endif
 
-static inline void dsi_write_reg(const struct dsi_reg idx, u32 val)
+static inline struct dsi_data *dsi_get_dsidrv_data(struct platform_device *dsidev)
 {
-	__raw_writel(val, dsi.base + idx.idx);
+	return dev_get_drvdata(&dsidev->dev);
 }
 
-static inline u32 dsi_read_reg(const struct dsi_reg idx)
+static inline struct platform_device *dsi_get_dsidev_from_dssdev(struct omap_dss_device *dssdev)
 {
-	return __raw_readl(dsi.base + idx.idx);
+	return dsi_pdev_map[dssdev->phy.dsi.module];
 }
 
+struct platform_device *dsi_get_dsidev_from_id(int module)
+{
+	return dsi_pdev_map[module];
+}
 
-void dsi_save_context(void)
+static int dsi_get_dsidev_id(struct platform_device *dsidev)
 {
+	/* TEMP: Pass 0 as the dsi module index till the time the dsi platform
+	 * device names aren't changed to the form "omapdss_dsi.0",
+	 * "omapdss_dsi.1" and so on */
+	BUG_ON(dsidev->id != -1);
+
+	return 0;
 }
 
-void dsi_restore_context(void)
+static inline void dsi_write_reg(struct platform_device *dsidev,
+		const struct dsi_reg idx, u32 val)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	__raw_writel(val, dsi->base + idx.idx);
+}
+
+static inline u32 dsi_read_reg(struct platform_device *dsidev,
+		const struct dsi_reg idx)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return __raw_readl(dsi->base + idx.idx);
 }
 
-void dsi_bus_lock(void)
+void dsi_bus_lock(struct omap_dss_device *dssdev)
 {
-	down(&dsi.bus_lock);
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	down(&dsi->bus_lock);
 }
 EXPORT_SYMBOL(dsi_bus_lock);
 
-void dsi_bus_unlock(void)
+void dsi_bus_unlock(struct omap_dss_device *dssdev)
 {
-	up(&dsi.bus_lock);
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	up(&dsi->bus_lock);
 }
 EXPORT_SYMBOL(dsi_bus_unlock);
 
-static bool dsi_bus_is_locked(void)
+static bool dsi_bus_is_locked(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->bus_lock.count == 0;
+}
+
+static void dsi_completion_handler(void *data, u32 mask)
 {
-	return dsi.bus_lock.count == 0;
+	complete((struct completion *)data);
 }
 
-static inline int wait_for_bit_change(const struct dsi_reg idx, int bitnum,
-		int value)
+static inline int wait_for_bit_change(struct platform_device *dsidev,
+		const struct dsi_reg idx, int bitnum, int value)
 {
 	int t = 100000;
 
-	while (REG_GET(idx, bitnum, bitnum) != value) {
+	while (REG_GET(dsidev, idx, bitnum, bitnum) != value) {
 		if (--t == 0)
 			return !value;
 	}
@@ -332,18 +438,21 @@ static inline int wait_for_bit_change(const struct dsi_reg idx, int bitnum,
 }
 
 #ifdef DEBUG
-static void dsi_perf_mark_setup(void)
+static void dsi_perf_mark_setup(struct platform_device *dsidev)
 {
-	dsi.perf_setup_time = ktime_get();
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	dsi->perf_setup_time = ktime_get();
 }
 
-static void dsi_perf_mark_start(void)
+static void dsi_perf_mark_start(struct platform_device *dsidev)
 {
-	dsi.perf_start_time = ktime_get();
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	dsi->perf_start_time = ktime_get();
 }
 
-static void dsi_perf_show(const char *name)
+static void dsi_perf_show(struct platform_device *dsidev, const char *name)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	ktime_t t, setup_time, trans_time;
 	u32 total_bytes;
 	u32 setup_us, trans_us, total_us;
@@ -353,21 +462,21 @@ static void dsi_perf_show(const char *name)
 
 	t = ktime_get();
 
-	setup_time = ktime_sub(dsi.perf_start_time, dsi.perf_setup_time);
+	setup_time = ktime_sub(dsi->perf_start_time, dsi->perf_setup_time);
 	setup_us = (u32)ktime_to_us(setup_time);
 	if (setup_us == 0)
 		setup_us = 1;
 
-	trans_time = ktime_sub(t, dsi.perf_start_time);
+	trans_time = ktime_sub(t, dsi->perf_start_time);
 	trans_us = (u32)ktime_to_us(trans_time);
 	if (trans_us == 0)
 		trans_us = 1;
 
 	total_us = setup_us + trans_us;
 
-	total_bytes = dsi.update_region.w *
-		dsi.update_region.h *
-		dsi.update_region.device->ctrl.pixel_size / 8;
+	total_bytes = dsi->update_region.w *
+		dsi->update_region.h *
+		dsi->update_region.device->ctrl.pixel_size / 8;
 
 	printk(KERN_INFO "DSI(%s): %u us + %u us = %u us (%uHz), "
 			"%u bytes, %u kbytes/sec\n",
@@ -380,13 +489,25 @@ static void dsi_perf_show(const char *name)
 			total_bytes * 1000 / total_us);
 }
 #else
-#define dsi_perf_mark_setup()
-#define dsi_perf_mark_start()
-#define dsi_perf_show(x)
+static inline void dsi_perf_mark_setup(struct platform_device *dsidev)
+{
+}
+
+static inline void dsi_perf_mark_start(struct platform_device *dsidev)
+{
+}
+
+static inline void dsi_perf_show(struct platform_device *dsidev,
+		const char *name)
+{
+}
 #endif
 
 static void print_irq_status(u32 status)
 {
+	if (status == 0)
+		return;
+
 #ifndef VERBOSE_IRQ
 	if ((status & ~DSI_IRQ_CHANNEL_MASK) == 0)
 		return;
@@ -422,6 +543,9 @@ static void print_irq_status(u32 status)
 
 static void print_irq_status_vc(int channel, u32 status)
 {
+	if (status == 0)
+		return;
+
 #ifndef VERBOSE_IRQ
 	if ((status & ~DSI_VC_IRQ_PACKET_SENT) == 0)
 		return;
@@ -448,6 +572,9 @@ static void print_irq_status_vc(int channel, u32 status)
 
 static void print_irq_status_cio(u32 status)
 {
+	if (status == 0)
+		return;
+
 	printk(KERN_DEBUG "DSI CIO IRQ 0x%x: ", status);
 
 #define PIS(x) \
@@ -478,192 +605,490 @@ static void print_irq_status_cio(u32 status)
 	printk("\n");
 }
 
-static int debug_irq;
-
-/* called from dss */
-void dsi_irq_handler(void)
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dsi_collect_irq_stats(struct platform_device *dsidev, u32 irqstatus,
+		u32 *vcstatus, u32 ciostatus)
 {
-	u32 irqstatus, vcstatus, ciostatus;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int i;
 
-	irqstatus = dsi_read_reg(DSI_IRQSTATUS);
+	spin_lock(&dsi->irq_stats_lock);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-	spin_lock(&dsi.irq_stats_lock);
-	dsi.irq_stats.irq_count++;
-	dss_collect_irq_stats(irqstatus, dsi.irq_stats.dsi_irqs);
+	dsi->irq_stats.irq_count++;
+	dss_collect_irq_stats(irqstatus, dsi->irq_stats.dsi_irqs);
+
+	for (i = 0; i < 4; ++i)
+		dss_collect_irq_stats(vcstatus[i], dsi->irq_stats.vc_irqs[i]);
+
+	dss_collect_irq_stats(ciostatus, dsi->irq_stats.cio_irqs);
+
+	spin_unlock(&dsi->irq_stats_lock);
+}
+#else
+#define dsi_collect_irq_stats(dsidev, irqstatus, vcstatus, ciostatus)
 #endif
 
+static int debug_irq;
+
+static void dsi_handle_irq_errors(struct platform_device *dsidev, u32 irqstatus,
+		u32 *vcstatus, u32 ciostatus)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+
 	if (irqstatus & DSI_IRQ_ERROR_MASK) {
 		DSSERR("DSI error, irqstatus %x\n", irqstatus);
 		print_irq_status(irqstatus);
-		spin_lock(&dsi.errors_lock);
-		dsi.errors |= irqstatus & DSI_IRQ_ERROR_MASK;
-		spin_unlock(&dsi.errors_lock);
+		spin_lock(&dsi->errors_lock);
+		dsi->errors |= irqstatus & DSI_IRQ_ERROR_MASK;
+		spin_unlock(&dsi->errors_lock);
 	} else if (debug_irq) {
 		print_irq_status(irqstatus);
 	}
 
-#ifdef DSI_CATCH_MISSING_TE
-	if (irqstatus & DSI_IRQ_TE_TRIGGER)
-		del_timer(&dsi.te_timer);
-#endif
+	for (i = 0; i < 4; ++i) {
+		if (vcstatus[i] & DSI_VC_IRQ_ERROR_MASK) {
+			DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
+				       i, vcstatus[i]);
+			print_irq_status_vc(i, vcstatus[i]);
+		} else if (debug_irq) {
+			print_irq_status_vc(i, vcstatus[i]);
+		}
+	}
+
+	if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
+		DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
+		print_irq_status_cio(ciostatus);
+	} else if (debug_irq) {
+		print_irq_status_cio(ciostatus);
+	}
+}
+
+static void dsi_call_isrs(struct dsi_isr_data *isr_array,
+		unsigned isr_array_size, u32 irqstatus)
+{
+	struct dsi_isr_data *isr_data;
+	int i;
+
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+		if (isr_data->isr && isr_data->mask & irqstatus)
+			isr_data->isr(isr_data->arg, irqstatus);
+	}
+}
+
+static void dsi_handle_isrs(struct dsi_isr_tables *isr_tables,
+		u32 irqstatus, u32 *vcstatus, u32 ciostatus)
+{
+	int i;
+
+	dsi_call_isrs(isr_tables->isr_table,
+			ARRAY_SIZE(isr_tables->isr_table),
+			irqstatus);
 
 	for (i = 0; i < 4; ++i) {
-		if ((irqstatus & (1<<i)) == 0)
+		if (vcstatus[i] == 0)
 			continue;
+		dsi_call_isrs(isr_tables->isr_table_vc[i],
+				ARRAY_SIZE(isr_tables->isr_table_vc[i]),
+				vcstatus[i]);
+	}
 
-		vcstatus = dsi_read_reg(DSI_VC_IRQSTATUS(i));
+	if (ciostatus != 0)
+		dsi_call_isrs(isr_tables->isr_table_cio,
+				ARRAY_SIZE(isr_tables->isr_table_cio),
+				ciostatus);
+}
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-		dss_collect_irq_stats(vcstatus, dsi.irq_stats.vc_irqs[i]);
-#endif
+static irqreturn_t omap_dsi_irq_handler(int irq, void *arg)
+{
+	struct platform_device *dsidev;
+	struct dsi_data *dsi;
+	u32 irqstatus, vcstatus[4], ciostatus;
+	int i;
 
-		if (vcstatus & DSI_VC_IRQ_BTA) {
-			complete(&dsi.bta_completion);
+	dsidev = (struct platform_device *) arg;
+	dsi = dsi_get_dsidrv_data(dsidev);
 
-			if (dsi.bta_callback)
-				dsi.bta_callback();
-		}
+	spin_lock(&dsi->irq_lock);
 
-		if (vcstatus & DSI_VC_IRQ_ERROR_MASK) {
-			DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
-				       i, vcstatus);
-			print_irq_status_vc(i, vcstatus);
-		} else if (debug_irq) {
-			print_irq_status_vc(i, vcstatus);
+	irqstatus = dsi_read_reg(dsidev, DSI_IRQSTATUS);
+
+	/* IRQ is not for us */
+	if (!irqstatus) {
+		spin_unlock(&dsi->irq_lock);
+		return IRQ_NONE;
+	}
+
+	dsi_write_reg(dsidev, DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
+	/* flush posted write */
+	dsi_read_reg(dsidev, DSI_IRQSTATUS);
+
+	for (i = 0; i < 4; ++i) {
+		if ((irqstatus & (1 << i)) == 0) {
+			vcstatus[i] = 0;
+			continue;
 		}
 
-		dsi_write_reg(DSI_VC_IRQSTATUS(i), vcstatus);
+		vcstatus[i] = dsi_read_reg(dsidev, DSI_VC_IRQSTATUS(i));
+
+		dsi_write_reg(dsidev, DSI_VC_IRQSTATUS(i), vcstatus[i]);
 		/* flush posted write */
-		dsi_read_reg(DSI_VC_IRQSTATUS(i));
+		dsi_read_reg(dsidev, DSI_VC_IRQSTATUS(i));
 	}
 
 	if (irqstatus & DSI_IRQ_COMPLEXIO_ERR) {
-		ciostatus = dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
+		ciostatus = dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-		dss_collect_irq_stats(ciostatus, dsi.irq_stats.cio_irqs);
+		dsi_write_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS, ciostatus);
+		/* flush posted write */
+		dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
+	} else {
+		ciostatus = 0;
+	}
+
+#ifdef DSI_CATCH_MISSING_TE
+	if (irqstatus & DSI_IRQ_TE_TRIGGER)
+		del_timer(&dsi->te_timer);
 #endif
 
-		dsi_write_reg(DSI_COMPLEXIO_IRQ_STATUS, ciostatus);
-		/* flush posted write */
-		dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
+	/* make a copy and unlock, so that isrs can unregister
+	 * themselves */
+	memcpy(&dsi->isr_tables_copy, &dsi->isr_tables,
+		sizeof(dsi->isr_tables));
 
-		if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
-			DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
-			print_irq_status_cio(ciostatus);
-		} else if (debug_irq) {
-			print_irq_status_cio(ciostatus);
-		}
+	spin_unlock(&dsi->irq_lock);
+
+	dsi_handle_isrs(&dsi->isr_tables_copy, irqstatus, vcstatus, ciostatus);
+
+	dsi_handle_irq_errors(dsidev, irqstatus, vcstatus, ciostatus);
+
+	dsi_collect_irq_stats(dsidev, irqstatus, vcstatus, ciostatus);
+
+	return IRQ_HANDLED;
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_configure_irqs(struct platform_device *dsidev,
+		struct dsi_isr_data *isr_array,
+		unsigned isr_array_size, u32 default_mask,
+		const struct dsi_reg enable_reg,
+		const struct dsi_reg status_reg)
+{
+	struct dsi_isr_data *isr_data;
+	u32 mask;
+	u32 old_mask;
+	int i;
+
+	mask = default_mask;
+
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+
+		if (isr_data->isr == NULL)
+			continue;
+
+		mask |= isr_data->mask;
 	}
 
-	dsi_write_reg(DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
-	/* flush posted write */
-	dsi_read_reg(DSI_IRQSTATUS);
+	old_mask = dsi_read_reg(dsidev, enable_reg);
+	/* clear the irqstatus for newly enabled irqs */
+	dsi_write_reg(dsidev, status_reg, (mask ^ old_mask) & mask);
+	dsi_write_reg(dsidev, enable_reg, mask);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-	spin_unlock(&dsi.irq_stats_lock);
+	/* flush posted writes */
+	dsi_read_reg(dsidev, enable_reg);
+	dsi_read_reg(dsidev, status_reg);
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 mask = DSI_IRQ_ERROR_MASK;
+#ifdef DSI_CATCH_MISSING_TE
+	mask |= DSI_IRQ_TE_TRIGGER;
 #endif
+	_omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table,
+			ARRAY_SIZE(dsi->isr_tables.isr_table), mask,
+			DSI_IRQENABLE, DSI_IRQSTATUS);
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_vc(struct platform_device *dsidev, int vc)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	_omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table_vc[vc],
+			ARRAY_SIZE(dsi->isr_tables.isr_table_vc[vc]),
+			DSI_VC_IRQ_ERROR_MASK,
+			DSI_VC_IRQENABLE(vc), DSI_VC_IRQSTATUS(vc));
 }
 
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_cio(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	_omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table_cio,
+			ARRAY_SIZE(dsi->isr_tables.isr_table_cio),
+			DSI_CIO_IRQ_ERROR_MASK,
+			DSI_COMPLEXIO_IRQ_ENABLE, DSI_COMPLEXIO_IRQ_STATUS);
+}
 
-static void _dsi_initialize_irq(void)
+static void _dsi_initialize_irq(struct platform_device *dsidev)
 {
-	u32 l;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int vc;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	memset(&dsi->isr_tables, 0, sizeof(dsi->isr_tables));
+
+	_omap_dsi_set_irqs(dsidev);
+	for (vc = 0; vc < 4; ++vc)
+		_omap_dsi_set_irqs_vc(dsidev, vc);
+	_omap_dsi_set_irqs_cio(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+}
+
+static int _dsi_register_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+		struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+	struct dsi_isr_data *isr_data;
+	int free_idx;
 	int i;
 
-	/* disable all interrupts */
-	dsi_write_reg(DSI_IRQENABLE, 0);
-	for (i = 0; i < 4; ++i)
-		dsi_write_reg(DSI_VC_IRQENABLE(i), 0);
-	dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, 0);
+	BUG_ON(isr == NULL);
 
-	/* clear interrupt status */
-	l = dsi_read_reg(DSI_IRQSTATUS);
-	dsi_write_reg(DSI_IRQSTATUS, l & ~DSI_IRQ_CHANNEL_MASK);
+	/* check for duplicate entry and find a free slot */
+	free_idx = -1;
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
 
-	for (i = 0; i < 4; ++i) {
-		l = dsi_read_reg(DSI_VC_IRQSTATUS(i));
-		dsi_write_reg(DSI_VC_IRQSTATUS(i), l);
+		if (isr_data->isr == isr && isr_data->arg == arg &&
+				isr_data->mask == mask) {
+			return -EINVAL;
+		}
+
+		if (isr_data->isr == NULL && free_idx == -1)
+			free_idx = i;
 	}
 
-	l = dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
-	dsi_write_reg(DSI_COMPLEXIO_IRQ_STATUS, l);
+	if (free_idx == -1)
+		return -EBUSY;
 
-	/* enable error irqs */
-	l = DSI_IRQ_ERROR_MASK;
-#ifdef DSI_CATCH_MISSING_TE
-	l |= DSI_IRQ_TE_TRIGGER;
-#endif
-	dsi_write_reg(DSI_IRQENABLE, l);
+	isr_data = &isr_array[free_idx];
+	isr_data->isr = isr;
+	isr_data->arg = arg;
+	isr_data->mask = mask;
 
-	l = DSI_VC_IRQ_ERROR_MASK;
-	for (i = 0; i < 4; ++i)
-		dsi_write_reg(DSI_VC_IRQENABLE(i), l);
+	return 0;
+}
+
+static int _dsi_unregister_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+		struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+	struct dsi_isr_data *isr_data;
+	int i;
+
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+		if (isr_data->isr != isr || isr_data->arg != arg ||
+				isr_data->mask != mask)
+			continue;
+
+		isr_data->isr = NULL;
+		isr_data->arg = NULL;
+		isr_data->mask = 0;
+
+		return 0;
+	}
 
-	l = DSI_CIO_IRQ_ERROR_MASK;
-	dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, l);
+	return -EINVAL;
 }
 
-static u32 dsi_get_errors(void)
+static int dsi_register_isr(struct platform_device *dsidev, omap_dsi_isr_t isr,
+		void *arg, u32 mask)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned long flags;
-	u32 e;
-	spin_lock_irqsave(&dsi.errors_lock, flags);
-	e = dsi.errors;
-	dsi.errors = 0;
-	spin_unlock_irqrestore(&dsi.errors_lock, flags);
-	return e;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+			ARRAY_SIZE(dsi->isr_tables.isr_table));
+
+	if (r == 0)
+		_omap_dsi_set_irqs(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
 }
 
-static void dsi_vc_enable_bta_irq(int channel)
+static int dsi_unregister_isr(struct platform_device *dsidev,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
 {
-	u32 l;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+			ARRAY_SIZE(dsi->isr_tables.isr_table));
 
-	dsi_write_reg(DSI_VC_IRQSTATUS(channel), DSI_VC_IRQ_BTA);
+	if (r == 0)
+		_omap_dsi_set_irqs(dsidev);
 
-	l = dsi_read_reg(DSI_VC_IRQENABLE(channel));
-	l |= DSI_VC_IRQ_BTA;
-	dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
 }
 
-static void dsi_vc_disable_bta_irq(int channel)
+static int dsi_register_isr_vc(struct platform_device *dsidev, int channel,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
 {
-	u32 l;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_register_isr(isr, arg, mask,
+			dsi->isr_tables.isr_table_vc[channel],
+			ARRAY_SIZE(dsi->isr_tables.isr_table_vc[channel]));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_vc(dsidev, channel);
 
-	l = dsi_read_reg(DSI_VC_IRQENABLE(channel));
-	l &= ~DSI_VC_IRQ_BTA;
-	dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
 }
 
-/* DSI func clock. this could also be DSI2_PLL_FCLK */
-static inline void enable_clocks(bool enable)
+static int dsi_unregister_isr_vc(struct platform_device *dsidev, int channel,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
 {
-	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_unregister_isr(isr, arg, mask,
+			dsi->isr_tables.isr_table_vc[channel],
+			ARRAY_SIZE(dsi->isr_tables.isr_table_vc[channel]));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_vc(dsidev, channel);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_register_isr_cio(struct platform_device *dsidev,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+			ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_cio(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_unregister_isr_cio(struct platform_device *dsidev,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+			ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_cio(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static u32 dsi_get_errors(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	u32 e;
+	spin_lock_irqsave(&dsi->errors_lock, flags);
+	e = dsi->errors;
+	dsi->errors = 0;
+	spin_unlock_irqrestore(&dsi->errors_lock, flags);
+	return e;
+}
+
+int dsi_runtime_get(struct platform_device *dsidev)
+{
+	int r;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	DSSDBG("dsi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&dsi->pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+void dsi_runtime_put(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+
+	DSSDBG("dsi_runtime_put\n");
+
+	r = pm_runtime_put(&dsi->pdev->dev);
+	WARN_ON(r < 0);
 }
 
 /* source clock for DSI PLL. this could also be PCLKFREE */
-static inline void dsi_enable_pll_clock(bool enable)
+static inline void dsi_enable_pll_clock(struct platform_device *dsidev,
+		bool enable)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
 	if (enable)
-		dss_clk_enable(DSS_CLK_FCK2);
+		clk_enable(dsi->sys_clk);
 	else
-		dss_clk_disable(DSS_CLK_FCK2);
+		clk_disable(dsi->sys_clk);
 
-	if (enable && dsi.pll_locked) {
-		if (wait_for_bit_change(DSI_PLL_STATUS, 1, 1) != 1)
+	if (enable && dsi->pll_locked) {
+		if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 1, 1) != 1)
 			DSSERR("cannot lock PLL when enabling clocks\n");
 	}
 }
 
 #ifdef DEBUG
-static void _dsi_print_reset_status(void)
+static void _dsi_print_reset_status(struct platform_device *dsidev)
 {
 	u32 l;
+	int b0, b1, b2;
 
 	if (!dss_debug)
 		return;
@@ -671,35 +1096,47 @@ static void _dsi_print_reset_status(void)
 	/* A dummy read using the SCP interface to any DSIPHY register is
 	 * required after DSIPHY reset to complete the reset of the DSI complex
 	 * I/O. */
-	l = dsi_read_reg(DSI_DSIPHY_CFG5);
+	l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
 
 	printk(KERN_DEBUG "DSI resets: ");
 
-	l = dsi_read_reg(DSI_PLL_STATUS);
+	l = dsi_read_reg(dsidev, DSI_PLL_STATUS);
 	printk("PLL (%d) ", FLD_GET(l, 0, 0));
 
-	l = dsi_read_reg(DSI_COMPLEXIO_CFG1);
+	l = dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1);
 	printk("CIO (%d) ", FLD_GET(l, 29, 29));
 
-	l = dsi_read_reg(DSI_DSIPHY_CFG5);
-	printk("PHY (%x, %d, %d, %d)\n",
-			FLD_GET(l, 28, 26),
+	if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC)) {
+		b0 = 28;
+		b1 = 27;
+		b2 = 26;
+	} else {
+		b0 = 24;
+		b1 = 25;
+		b2 = 26;
+	}
+
+	l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+	printk("PHY (%x%x%x, %d, %d, %d)\n",
+			FLD_GET(l, b0, b0),
+			FLD_GET(l, b1, b1),
+			FLD_GET(l, b2, b2),
 			FLD_GET(l, 29, 29),
 			FLD_GET(l, 30, 30),
 			FLD_GET(l, 31, 31));
 }
 #else
-#define _dsi_print_reset_status()
+#define _dsi_print_reset_status(x)
 #endif
 
-static inline int dsi_if_enable(bool enable)
+static inline int dsi_if_enable(struct platform_device *dsidev, bool enable)
 {
 	DSSDBG("dsi_if_enable(%d)\n", enable);
 
 	enable = enable ? 1 : 0;
-	REG_FLD_MOD(DSI_CTRL, enable, 0, 0); /* IF_EN */
+	REG_FLD_MOD(dsidev, DSI_CTRL, enable, 0, 0); /* IF_EN */
 
-	if (wait_for_bit_change(DSI_CTRL, 0, enable) != enable) {
+	if (wait_for_bit_change(dsidev, DSI_CTRL, 0, enable) != enable) {
 			DSSERR("Failed to set dsi_if_enable to %d\n", enable);
 			return -EIO;
 	}
@@ -707,31 +1144,39 @@ static inline int dsi_if_enable(bool enable)
 	return 0;
 }
 
-unsigned long dsi_get_dsi1_pll_rate(void)
+unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev)
 {
-	return dsi.current_cinfo.dsi1_pll_fclk;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->current_cinfo.dsi_pll_hsdiv_dispc_clk;
 }
 
-static unsigned long dsi_get_dsi2_pll_rate(void)
+static unsigned long dsi_get_pll_hsdiv_dsi_rate(struct platform_device *dsidev)
 {
-	return dsi.current_cinfo.dsi2_pll_fclk;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->current_cinfo.dsi_pll_hsdiv_dsi_clk;
 }
 
-static unsigned long dsi_get_txbyteclkhs(void)
+static unsigned long dsi_get_txbyteclkhs(struct platform_device *dsidev)
 {
-	return dsi.current_cinfo.clkin4ddr / 16;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->current_cinfo.clkin4ddr / 16;
 }
 
-static unsigned long dsi_fclk_rate(void)
+static unsigned long dsi_fclk_rate(struct platform_device *dsidev)
 {
 	unsigned long r;
+	int dsi_module = dsi_get_dsidev_id(dsidev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
-	if (dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK) {
-		/* DSI FCLK source is DSS1_ALWON_FCK, which is dss1_fck */
-		r = dss_clk_get_rate(DSS_CLK_FCK1);
+	if (dss_get_dsi_clk_source(dsi_module) == OMAP_DSS_CLK_SRC_FCK) {
+		/* DSI FCLK source is DSS_CLK_FCK */
+		r = clk_get_rate(dsi->dss_clk);
 	} else {
-		/* DSI FCLK source is DSI2_PLL_FCLK */
-		r = dsi_get_dsi2_pll_rate();
+		/* DSI FCLK source is dsi_pll_hsdiv_dsi_clk */
+		r = dsi_get_pll_hsdiv_dsi_rate(dsidev);
 	}
 
 	return r;
@@ -739,31 +1184,50 @@ static unsigned long dsi_fclk_rate(void)
 
 static int dsi_set_lp_clk_divisor(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned long dsi_fclk;
 	unsigned lp_clk_div;
 	unsigned long lp_clk;
 
-	lp_clk_div = dssdev->phy.dsi.div.lp_clk_div;
+	lp_clk_div = dssdev->clocks.dsi.lp_clk_div;
 
-	if (lp_clk_div == 0 || lp_clk_div > LP_DIV_MAX)
+	if (lp_clk_div == 0 || lp_clk_div > dsi->lpdiv_max)
 		return -EINVAL;
 
-	dsi_fclk = dsi_fclk_rate();
+	dsi_fclk = dsi_fclk_rate(dsidev);
 
 	lp_clk = dsi_fclk / 2 / lp_clk_div;
 
 	DSSDBG("LP_CLK_DIV %u, LP_CLK %lu\n", lp_clk_div, lp_clk);
-	dsi.current_cinfo.lp_clk = lp_clk;
-	dsi.current_cinfo.lp_clk_div = lp_clk_div;
+	dsi->current_cinfo.lp_clk = lp_clk;
+	dsi->current_cinfo.lp_clk_div = lp_clk_div;
 
-	REG_FLD_MOD(DSI_CLK_CTRL, lp_clk_div, 12, 0);   /* LP_CLK_DIVISOR */
+	/* LP_CLK_DIVISOR */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, lp_clk_div, 12, 0);
 
-	REG_FLD_MOD(DSI_CLK_CTRL, dsi_fclk > 30000000 ? 1 : 0,
-			21, 21);		/* LP_RX_SYNCHRO_ENABLE */
+	/* LP_RX_SYNCHRO_ENABLE */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, dsi_fclk > 30000000 ? 1 : 0, 21, 21);
 
 	return 0;
 }
 
+static void dsi_enable_scp_clk(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->scp_clk_refcount++ == 0)
+		REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dsi_disable_scp_clk(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	WARN_ON(dsi->scp_clk_refcount == 0);
+	if (--dsi->scp_clk_refcount == 0)
+		REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 14, 14); /* CIO_CLK_ICG */
+}
 
 enum dsi_pll_power_state {
 	DSI_PLL_POWER_OFF	= 0x0,
@@ -772,14 +1236,21 @@ enum dsi_pll_power_state {
 	DSI_PLL_POWER_ON_DIV	= 0x3,
 };
 
-static int dsi_pll_power(enum dsi_pll_power_state state)
+static int dsi_pll_power(struct platform_device *dsidev,
+		enum dsi_pll_power_state state)
 {
 	int t = 0;
 
-	REG_FLD_MOD(DSI_CLK_CTRL, state, 31, 30);	/* PLL_PWR_CMD */
+	/* DSI-PLL power command 0x3 is not working */
+	if (dss_has_feature(FEAT_DSI_PLL_PWR_BUG) &&
+			state == DSI_PLL_POWER_ON_DIV)
+		state = DSI_PLL_POWER_ON_ALL;
+
+	/* PLL_PWR_CMD */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, state, 31, 30);
 
 	/* PLL_PWR_STATUS */
-	while (FLD_GET(dsi_read_reg(DSI_CLK_CTRL), 29, 28) != state) {
+	while (FLD_GET(dsi_read_reg(dsidev, DSI_CLK_CTRL), 29, 28) != state) {
 		if (++t > 1000) {
 			DSSERR("Failed to set DSI PLL power mode to %d\n",
 					state);
@@ -795,22 +1266,25 @@ static int dsi_pll_power(enum dsi_pll_power_state state)
 static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
 		struct dsi_clock_info *cinfo)
 {
-	if (cinfo->regn == 0 || cinfo->regn > REGN_MAX)
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (cinfo->regn == 0 || cinfo->regn > dsi->regn_max)
 		return -EINVAL;
 
-	if (cinfo->regm == 0 || cinfo->regm > REGM_MAX)
+	if (cinfo->regm == 0 || cinfo->regm > dsi->regm_max)
 		return -EINVAL;
 
-	if (cinfo->regm3 > REGM3_MAX)
+	if (cinfo->regm_dispc > dsi->regm_dispc_max)
 		return -EINVAL;
 
-	if (cinfo->regm4 > REGM4_MAX)
+	if (cinfo->regm_dsi > dsi->regm_dsi_max)
 		return -EINVAL;
 
-	if (cinfo->use_dss2_fck) {
-		cinfo->clkin = dss_clk_get_rate(DSS_CLK_FCK2);
+	if (cinfo->use_sys_clk) {
+		cinfo->clkin = clk_get_rate(dsi->sys_clk);
 		/* XXX it is unclear if highfreq should be used
-		 * with DSS2_FCK source also */
+		 * with DSS_SYS_CLK source also */
 		cinfo->highfreq = 0;
 	} else {
 		cinfo->clkin = dispc_pclk_rate(dssdev->manager->id);
@@ -823,7 +1297,7 @@ static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
 
 	cinfo->fint = cinfo->clkin / (cinfo->regn * (cinfo->highfreq ? 2 : 1));
 
-	if (cinfo->fint > FINT_MAX || cinfo->fint < FINT_MIN)
+	if (cinfo->fint > dsi->fint_max || cinfo->fint < dsi->fint_min)
 		return -EINVAL;
 
 	cinfo->clkin4ddr = 2 * cinfo->regm * cinfo->fint;
@@ -831,44 +1305,49 @@ static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
 	if (cinfo->clkin4ddr > 1800 * 1000 * 1000)
 		return -EINVAL;
 
-	if (cinfo->regm3 > 0)
-		cinfo->dsi1_pll_fclk = cinfo->clkin4ddr / cinfo->regm3;
+	if (cinfo->regm_dispc > 0)
+		cinfo->dsi_pll_hsdiv_dispc_clk =
+			cinfo->clkin4ddr / cinfo->regm_dispc;
 	else
-		cinfo->dsi1_pll_fclk = 0;
+		cinfo->dsi_pll_hsdiv_dispc_clk = 0;
 
-	if (cinfo->regm4 > 0)
-		cinfo->dsi2_pll_fclk = cinfo->clkin4ddr / cinfo->regm4;
+	if (cinfo->regm_dsi > 0)
+		cinfo->dsi_pll_hsdiv_dsi_clk =
+			cinfo->clkin4ddr / cinfo->regm_dsi;
 	else
-		cinfo->dsi2_pll_fclk = 0;
+		cinfo->dsi_pll_hsdiv_dsi_clk = 0;
 
 	return 0;
 }
 
-int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
-		struct dsi_clock_info *dsi_cinfo,
+int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
+		unsigned long req_pck, struct dsi_clock_info *dsi_cinfo,
 		struct dispc_clock_info *dispc_cinfo)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct dsi_clock_info cur, best;
 	struct dispc_clock_info best_dispc;
 	int min_fck_per_pck;
 	int match = 0;
-	unsigned long dss_clk_fck2;
+	unsigned long dss_sys_clk, max_dss_fck;
+
+	dss_sys_clk = clk_get_rate(dsi->sys_clk);
 
-	dss_clk_fck2 = dss_clk_get_rate(DSS_CLK_FCK2);
+	max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
 
-	if (req_pck == dsi.cache_req_pck &&
-			dsi.cache_cinfo.clkin == dss_clk_fck2) {
+	if (req_pck == dsi->cache_req_pck &&
+			dsi->cache_cinfo.clkin == dss_sys_clk) {
 		DSSDBG("DSI clock info found from cache\n");
-		*dsi_cinfo = dsi.cache_cinfo;
-		dispc_find_clk_divs(is_tft, req_pck, dsi_cinfo->dsi1_pll_fclk,
-				dispc_cinfo);
+		*dsi_cinfo = dsi->cache_cinfo;
+		dispc_find_clk_divs(is_tft, req_pck,
+			dsi_cinfo->dsi_pll_hsdiv_dispc_clk, dispc_cinfo);
 		return 0;
 	}
 
 	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
 	if (min_fck_per_pck &&
-		req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+		req_pck * min_fck_per_pck > max_dss_fck) {
 		DSSERR("Requested pixel clock not possible with the current "
 				"OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
 				"the constraint off.\n");
@@ -882,24 +1361,24 @@ retry:
 	memset(&best_dispc, 0, sizeof(best_dispc));
 
 	memset(&cur, 0, sizeof(cur));
-	cur.clkin = dss_clk_fck2;
-	cur.use_dss2_fck = 1;
+	cur.clkin = dss_sys_clk;
+	cur.use_sys_clk = 1;
 	cur.highfreq = 0;
 
 	/* no highfreq: 0.75MHz < Fint = clkin / regn < 2.1MHz */
 	/* highfreq: 0.75MHz < Fint = clkin / (2*regn) < 2.1MHz */
 	/* To reduce PLL lock time, keep Fint high (around 2 MHz) */
-	for (cur.regn = 1; cur.regn < REGN_MAX; ++cur.regn) {
+	for (cur.regn = 1; cur.regn < dsi->regn_max; ++cur.regn) {
 		if (cur.highfreq == 0)
 			cur.fint = cur.clkin / cur.regn;
 		else
 			cur.fint = cur.clkin / (2 * cur.regn);
 
-		if (cur.fint > FINT_MAX || cur.fint < FINT_MIN)
+		if (cur.fint > dsi->fint_max || cur.fint < dsi->fint_min)
 			continue;
 
 		/* DSIPHY(MHz) = (2 * regm / regn) * (clkin / (highfreq + 1)) */
-		for (cur.regm = 1; cur.regm < REGM_MAX; ++cur.regm) {
+		for (cur.regm = 1; cur.regm < dsi->regm_max; ++cur.regm) {
 			unsigned long a, b;
 
 			a = 2 * cur.regm * (cur.clkin/1000);
@@ -909,30 +1388,32 @@ retry:
 			if (cur.clkin4ddr > 1800 * 1000 * 1000)
 				break;
 
-			/* DSI1_PLL_FCLK(MHz) = DSIPHY(MHz) / regm3  < 173MHz */
-			for (cur.regm3 = 1; cur.regm3 < REGM3_MAX;
-					++cur.regm3) {
+			/* dsi_pll_hsdiv_dispc_clk(MHz) =
+			 * DSIPHY(MHz) / regm_dispc  < 173MHz/186Mhz */
+			for (cur.regm_dispc = 1; cur.regm_dispc <
+					dsi->regm_dispc_max; ++cur.regm_dispc) {
 				struct dispc_clock_info cur_dispc;
-				cur.dsi1_pll_fclk = cur.clkin4ddr / cur.regm3;
+				cur.dsi_pll_hsdiv_dispc_clk =
+					cur.clkin4ddr / cur.regm_dispc;
 
 				/* this will narrow down the search a bit,
 				 * but still give pixclocks below what was
 				 * requested */
-				if (cur.dsi1_pll_fclk  < req_pck)
+				if (cur.dsi_pll_hsdiv_dispc_clk  < req_pck)
 					break;
 
-				if (cur.dsi1_pll_fclk > DISPC_MAX_FCK)
+				if (cur.dsi_pll_hsdiv_dispc_clk > max_dss_fck)
 					continue;
 
 				if (min_fck_per_pck &&
-					cur.dsi1_pll_fclk <
+					cur.dsi_pll_hsdiv_dispc_clk <
 						req_pck * min_fck_per_pck)
 					continue;
 
 				match = 1;
 
 				dispc_find_clk_divs(is_tft, req_pck,
-						cur.dsi1_pll_fclk,
+						cur.dsi_pll_hsdiv_dispc_clk,
 						&cur_dispc);
 
 				if (abs(cur_dispc.pck - req_pck) <
@@ -961,44 +1442,53 @@ found:
 		return -EINVAL;
 	}
 
-	/* DSI2_PLL_FCLK (regm4) is not used */
-	best.regm4 = 0;
-	best.dsi2_pll_fclk = 0;
+	/* dsi_pll_hsdiv_dsi_clk (regm_dsi) is not used */
+	best.regm_dsi = 0;
+	best.dsi_pll_hsdiv_dsi_clk = 0;
 
 	if (dsi_cinfo)
 		*dsi_cinfo = best;
 	if (dispc_cinfo)
 		*dispc_cinfo = best_dispc;
 
-	dsi.cache_req_pck = req_pck;
-	dsi.cache_clk_freq = 0;
-	dsi.cache_cinfo = best;
+	dsi->cache_req_pck = req_pck;
+	dsi->cache_clk_freq = 0;
+	dsi->cache_cinfo = best;
 
 	return 0;
 }
 
-int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
+int dsi_pll_set_clock_div(struct platform_device *dsidev,
+		struct dsi_clock_info *cinfo)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int r = 0;
 	u32 l;
-	int f;
+	int f = 0;
+	u8 regn_start, regn_end, regm_start, regm_end;
+	u8 regm_dispc_start, regm_dispc_end, regm_dsi_start, regm_dsi_end;
 
 	DSSDBGF();
 
-	dsi.current_cinfo.fint = cinfo->fint;
-	dsi.current_cinfo.clkin4ddr = cinfo->clkin4ddr;
-	dsi.current_cinfo.dsi1_pll_fclk = cinfo->dsi1_pll_fclk;
-	dsi.current_cinfo.dsi2_pll_fclk = cinfo->dsi2_pll_fclk;
+	dsi->current_cinfo.use_sys_clk = cinfo->use_sys_clk;
+	dsi->current_cinfo.highfreq = cinfo->highfreq;
+
+	dsi->current_cinfo.fint = cinfo->fint;
+	dsi->current_cinfo.clkin4ddr = cinfo->clkin4ddr;
+	dsi->current_cinfo.dsi_pll_hsdiv_dispc_clk =
+			cinfo->dsi_pll_hsdiv_dispc_clk;
+	dsi->current_cinfo.dsi_pll_hsdiv_dsi_clk =
+			cinfo->dsi_pll_hsdiv_dsi_clk;
 
-	dsi.current_cinfo.regn = cinfo->regn;
-	dsi.current_cinfo.regm = cinfo->regm;
-	dsi.current_cinfo.regm3 = cinfo->regm3;
-	dsi.current_cinfo.regm4 = cinfo->regm4;
+	dsi->current_cinfo.regn = cinfo->regn;
+	dsi->current_cinfo.regm = cinfo->regm;
+	dsi->current_cinfo.regm_dispc = cinfo->regm_dispc;
+	dsi->current_cinfo.regm_dsi = cinfo->regm_dsi;
 
 	DSSDBG("DSI Fint %ld\n", cinfo->fint);
 
 	DSSDBG("clkin (%s) rate %ld, highfreq %d\n",
-			cinfo->use_dss2_fck ? "dss2_fck" : "pclkfree",
+			cinfo->use_sys_clk ? "dss_sys_clk" : "pclkfree",
 			cinfo->clkin,
 			cinfo->highfreq);
 
@@ -1015,63 +1505,79 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
 	DSSDBG("Clock lane freq %ld Hz\n", cinfo->clkin4ddr / 4);
 
-	DSSDBG("regm3 = %d, dsi1_pll_fclk = %lu\n",
-			cinfo->regm3, cinfo->dsi1_pll_fclk);
-	DSSDBG("regm4 = %d, dsi2_pll_fclk = %lu\n",
-			cinfo->regm4, cinfo->dsi2_pll_fclk);
-
-	REG_FLD_MOD(DSI_PLL_CONTROL, 0, 0, 0); /* DSI_PLL_AUTOMODE = manual */
-
-	l = dsi_read_reg(DSI_PLL_CONFIGURATION1);
+	DSSDBG("regm_dispc = %d, %s (%s) = %lu\n", cinfo->regm_dispc,
+		dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+		dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+		cinfo->dsi_pll_hsdiv_dispc_clk);
+	DSSDBG("regm_dsi = %d, %s (%s) = %lu\n", cinfo->regm_dsi,
+		dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+		dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+		cinfo->dsi_pll_hsdiv_dsi_clk);
+
+	dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGN, &regn_start, &regn_end);
+	dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM, &regm_start, &regm_end);
+	dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM_DISPC, &regm_dispc_start,
+			&regm_dispc_end);
+	dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM_DSI, &regm_dsi_start,
+			&regm_dsi_end);
+
+	/* DSI_PLL_AUTOMODE = manual */
+	REG_FLD_MOD(dsidev, DSI_PLL_CONTROL, 0, 0, 0);
+
+	l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION1);
 	l = FLD_MOD(l, 1, 0, 0);		/* DSI_PLL_STOPMODE */
-	l = FLD_MOD(l, cinfo->regn - 1, 7, 1);	/* DSI_PLL_REGN */
-	l = FLD_MOD(l, cinfo->regm, 18, 8);	/* DSI_PLL_REGM */
-	l = FLD_MOD(l, cinfo->regm3 > 0 ? cinfo->regm3 - 1 : 0,
-			22, 19);		/* DSI_CLOCK_DIV */
-	l = FLD_MOD(l, cinfo->regm4 > 0 ? cinfo->regm4 - 1 : 0,
-			26, 23);		/* DSIPROTO_CLOCK_DIV */
-	dsi_write_reg(DSI_PLL_CONFIGURATION1, l);
-
-	BUG_ON(cinfo->fint < 750000 || cinfo->fint > 2100000);
-	if (cinfo->fint < 1000000)
-		f = 0x3;
-	else if (cinfo->fint < 1250000)
-		f = 0x4;
-	else if (cinfo->fint < 1500000)
-		f = 0x5;
-	else if (cinfo->fint < 1750000)
-		f = 0x6;
-	else
-		f = 0x7;
+	/* DSI_PLL_REGN */
+	l = FLD_MOD(l, cinfo->regn - 1, regn_start, regn_end);
+	/* DSI_PLL_REGM */
+	l = FLD_MOD(l, cinfo->regm, regm_start, regm_end);
+	/* DSI_CLOCK_DIV */
+	l = FLD_MOD(l, cinfo->regm_dispc > 0 ? cinfo->regm_dispc - 1 : 0,
+			regm_dispc_start, regm_dispc_end);
+	/* DSIPROTO_CLOCK_DIV */
+	l = FLD_MOD(l, cinfo->regm_dsi > 0 ? cinfo->regm_dsi - 1 : 0,
+			regm_dsi_start, regm_dsi_end);
+	dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION1, l);
+
+	BUG_ON(cinfo->fint < dsi->fint_min || cinfo->fint > dsi->fint_max);
+
+	if (dss_has_feature(FEAT_DSI_PLL_FREQSEL)) {
+		f = cinfo->fint < 1000000 ? 0x3 :
+			cinfo->fint < 1250000 ? 0x4 :
+			cinfo->fint < 1500000 ? 0x5 :
+			cinfo->fint < 1750000 ? 0x6 :
+			0x7;
+	}
 
-	l = dsi_read_reg(DSI_PLL_CONFIGURATION2);
-	l = FLD_MOD(l, f, 4, 1);		/* DSI_PLL_FREQSEL */
-	l = FLD_MOD(l, cinfo->use_dss2_fck ? 0 : 1,
+	l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
+
+	if (dss_has_feature(FEAT_DSI_PLL_FREQSEL))
+		l = FLD_MOD(l, f, 4, 1);	/* DSI_PLL_FREQSEL */
+	l = FLD_MOD(l, cinfo->use_sys_clk ? 0 : 1,
 			11, 11);		/* DSI_PLL_CLKSEL */
 	l = FLD_MOD(l, cinfo->highfreq,
 			12, 12);		/* DSI_PLL_HIGHFREQ */
 	l = FLD_MOD(l, 1, 13, 13);		/* DSI_PLL_REFEN */
 	l = FLD_MOD(l, 0, 14, 14);		/* DSIPHY_CLKINEN */
 	l = FLD_MOD(l, 1, 20, 20);		/* DSI_HSDIVBYPASS */
-	dsi_write_reg(DSI_PLL_CONFIGURATION2, l);
+	dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION2, l);
 
-	REG_FLD_MOD(DSI_PLL_GO, 1, 0, 0);	/* DSI_PLL_GO */
+	REG_FLD_MOD(dsidev, DSI_PLL_GO, 1, 0, 0);	/* DSI_PLL_GO */
 
-	if (wait_for_bit_change(DSI_PLL_GO, 0, 0) != 0) {
+	if (wait_for_bit_change(dsidev, DSI_PLL_GO, 0, 0) != 0) {
 		DSSERR("dsi pll go bit not going down.\n");
 		r = -EIO;
 		goto err;
 	}
 
-	if (wait_for_bit_change(DSI_PLL_STATUS, 1, 1) != 1) {
+	if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 1, 1) != 1) {
 		DSSERR("cannot lock PLL\n");
 		r = -EIO;
 		goto err;
 	}
 
-	dsi.pll_locked = 1;
+	dsi->pll_locked = 1;
 
-	l = dsi_read_reg(DSI_PLL_CONFIGURATION2);
+	l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
 	l = FLD_MOD(l, 0, 0, 0);	/* DSI_PLL_IDLE */
 	l = FLD_MOD(l, 0, 5, 5);	/* DSI_PLL_PLLLPMODE */
 	l = FLD_MOD(l, 0, 6, 6);	/* DSI_PLL_LOWCURRSTBY */
@@ -1086,32 +1592,52 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 	l = FLD_MOD(l, 1, 18, 18);	/* DSI_PROTO_CLOCK_EN */
 	l = FLD_MOD(l, 0, 19, 19);	/* DSI_PROTO_CLOCK_PWDN */
 	l = FLD_MOD(l, 0, 20, 20);	/* DSI_HSDIVBYPASS */
-	dsi_write_reg(DSI_PLL_CONFIGURATION2, l);
+	dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION2, l);
 
 	DSSDBG("PLL config done\n");
 err:
 	return r;
 }
 
-int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
+int dsi_pll_init(struct platform_device *dsidev, bool enable_hsclk,
 		bool enable_hsdiv)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int r = 0;
 	enum dsi_pll_power_state pwstate;
 
 	DSSDBG("PLL init\n");
 
-	enable_clocks(1);
-	dsi_enable_pll_clock(1);
+	if (dsi->vdds_dsi_reg == NULL) {
+		struct regulator *vdds_dsi;
 
-	r = regulator_enable(dsi.vdds_dsi_reg);
-	if (r)
-		goto err0;
+		vdds_dsi = regulator_get(&dsi->pdev->dev, "vdds_dsi");
+
+		if (IS_ERR(vdds_dsi)) {
+			DSSERR("can't get VDDS_DSI regulator\n");
+			return PTR_ERR(vdds_dsi);
+		}
+
+		dsi->vdds_dsi_reg = vdds_dsi;
+	}
+
+	dsi_enable_pll_clock(dsidev, 1);
+	/*
+	 * Note: SCP CLK is not required on OMAP3, but it is required on OMAP4.
+	 */
+	dsi_enable_scp_clk(dsidev);
+
+	if (!dsi->vdds_dsi_enabled) {
+		r = regulator_enable(dsi->vdds_dsi_reg);
+		if (r)
+			goto err0;
+		dsi->vdds_dsi_enabled = true;
+	}
 
 	/* XXX PLL does not come out of reset without this... */
 	dispc_pck_free_enable(1);
 
-	if (wait_for_bit_change(DSI_PLL_STATUS, 0, 1) != 1) {
+	if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 0, 1) != 1) {
 		DSSERR("PLL not coming out of reset.\n");
 		r = -ENODEV;
 		dispc_pck_free_enable(0);
@@ -1131,7 +1657,7 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
 	else
 		pwstate = DSI_PLL_POWER_OFF;
 
-	r = dsi_pll_power(pwstate);
+	r = dsi_pll_power(dsidev, pwstate);
 
 	if (r)
 		goto err1;
@@ -1140,92 +1666,120 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
 
 	return 0;
 err1:
-	regulator_disable(dsi.vdds_dsi_reg);
+	if (dsi->vdds_dsi_enabled) {
+		regulator_disable(dsi->vdds_dsi_reg);
+		dsi->vdds_dsi_enabled = false;
+	}
 err0:
-	enable_clocks(0);
-	dsi_enable_pll_clock(0);
+	dsi_disable_scp_clk(dsidev);
+	dsi_enable_pll_clock(dsidev, 0);
 	return r;
 }
 
-void dsi_pll_uninit(void)
+void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes)
 {
-	enable_clocks(0);
-	dsi_enable_pll_clock(0);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	dsi->pll_locked = 0;
+	dsi_pll_power(dsidev, DSI_PLL_POWER_OFF);
+	if (disconnect_lanes) {
+		WARN_ON(!dsi->vdds_dsi_enabled);
+		regulator_disable(dsi->vdds_dsi_reg);
+		dsi->vdds_dsi_enabled = false;
+	}
+
+	dsi_disable_scp_clk(dsidev);
+	dsi_enable_pll_clock(dsidev, 0);
 
-	dsi.pll_locked = 0;
-	dsi_pll_power(DSI_PLL_POWER_OFF);
-	regulator_disable(dsi.vdds_dsi_reg);
 	DSSDBG("PLL uninit done\n");
 }
 
-void dsi_dump_clocks(struct seq_file *s)
+static void dsi_dump_dsidev_clocks(struct platform_device *dsidev,
+		struct seq_file *s)
 {
-	int clksel;
-	struct dsi_clock_info *cinfo = &dsi.current_cinfo;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clock_info *cinfo = &dsi->current_cinfo;
+	enum omap_dss_clk_source dispc_clk_src, dsi_clk_src;
+	int dsi_module = dsi_get_dsidev_id(dsidev);
 
-	enable_clocks(1);
+	dispc_clk_src = dss_get_dispc_clk_source();
+	dsi_clk_src = dss_get_dsi_clk_source(dsi_module);
 
-	clksel = REG_GET(DSI_PLL_CONFIGURATION2, 11, 11);
+	if (dsi_runtime_get(dsidev))
+		return;
 
-	seq_printf(s,	"- DSI PLL -\n");
+	seq_printf(s,	"- DSI%d PLL -\n", dsi_module + 1);
 
 	seq_printf(s,	"dsi pll source = %s\n",
-			clksel == 0 ?
-			"dss2_alwon_fclk" : "pclkfree");
+			cinfo->use_sys_clk ? "dss_sys_clk" : "pclkfree");
 
 	seq_printf(s,	"Fint\t\t%-16luregn %u\n", cinfo->fint, cinfo->regn);
 
 	seq_printf(s,	"CLKIN4DDR\t%-16luregm %u\n",
 			cinfo->clkin4ddr, cinfo->regm);
 
-	seq_printf(s,	"dsi1_pll_fck\t%-16luregm3 %u\t(%s)\n",
-			cinfo->dsi1_pll_fclk,
-			cinfo->regm3,
-			dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+	seq_printf(s,	"%s (%s)\t%-16luregm_dispc %u\t(%s)\n",
+			dss_get_generic_clk_source_name(dispc_clk_src),
+			dss_feat_get_clk_source_name(dispc_clk_src),
+			cinfo->dsi_pll_hsdiv_dispc_clk,
+			cinfo->regm_dispc,
+			dispc_clk_src == OMAP_DSS_CLK_SRC_FCK ?
 			"off" : "on");
 
-	seq_printf(s,	"dsi2_pll_fck\t%-16luregm4 %u\t(%s)\n",
-			cinfo->dsi2_pll_fclk,
-			cinfo->regm4,
-			dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+	seq_printf(s,	"%s (%s)\t%-16luregm_dsi %u\t(%s)\n",
+			dss_get_generic_clk_source_name(dsi_clk_src),
+			dss_feat_get_clk_source_name(dsi_clk_src),
+			cinfo->dsi_pll_hsdiv_dsi_clk,
+			cinfo->regm_dsi,
+			dsi_clk_src == OMAP_DSS_CLK_SRC_FCK ?
 			"off" : "on");
 
-	seq_printf(s,	"- DSI -\n");
+	seq_printf(s,	"- DSI%d -\n", dsi_module + 1);
 
-	seq_printf(s,	"dsi fclk source = %s\n",
-			dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-			"dss1_alwon_fclk" : "dsi2_pll_fclk");
+	seq_printf(s,	"dsi fclk source = %s (%s)\n",
+			dss_get_generic_clk_source_name(dsi_clk_src),
+			dss_feat_get_clk_source_name(dsi_clk_src));
 
-	seq_printf(s,	"DSI_FCLK\t%lu\n", dsi_fclk_rate());
+	seq_printf(s,	"DSI_FCLK\t%lu\n", dsi_fclk_rate(dsidev));
 
 	seq_printf(s,	"DDR_CLK\t\t%lu\n",
 			cinfo->clkin4ddr / 4);
 
-	seq_printf(s,	"TxByteClkHS\t%lu\n", dsi_get_txbyteclkhs());
+	seq_printf(s,	"TxByteClkHS\t%lu\n", dsi_get_txbyteclkhs(dsidev));
 
 	seq_printf(s,	"LP_CLK\t\t%lu\n", cinfo->lp_clk);
 
-	seq_printf(s,	"VP_CLK\t\t%lu\n"
-			"VP_PCLK\t\t%lu\n",
-			dispc_lclk_rate(OMAP_DSS_CHANNEL_LCD),
-			dispc_pclk_rate(OMAP_DSS_CHANNEL_LCD));
+	dsi_runtime_put(dsidev);
+}
 
-	enable_clocks(0);
+void dsi_dump_clocks(struct seq_file *s)
+{
+	struct platform_device *dsidev;
+	int i;
+
+	for  (i = 0; i < MAX_NUM_DSI; i++) {
+		dsidev = dsi_get_dsidev_from_id(i);
+		if (dsidev)
+			dsi_dump_dsidev_clocks(dsidev, s);
+	}
 }
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-void dsi_dump_irqs(struct seq_file *s)
+static void dsi_dump_dsidev_irqs(struct platform_device *dsidev,
+		struct seq_file *s)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned long flags;
 	struct dsi_irq_stats stats;
+	int dsi_module = dsi_get_dsidev_id(dsidev);
 
-	spin_lock_irqsave(&dsi.irq_stats_lock, flags);
+	spin_lock_irqsave(&dsi->irq_stats_lock, flags);
 
-	stats = dsi.irq_stats;
-	memset(&dsi.irq_stats, 0, sizeof(dsi.irq_stats));
-	dsi.irq_stats.last_reset = jiffies;
+	stats = dsi->irq_stats;
+	memset(&dsi->irq_stats, 0, sizeof(dsi->irq_stats));
+	dsi->irq_stats.last_reset = jiffies;
 
-	spin_unlock_irqrestore(&dsi.irq_stats_lock, flags);
+	spin_unlock_irqrestore(&dsi->irq_stats_lock, flags);
 
 	seq_printf(s, "period %u ms\n",
 			jiffies_to_msecs(jiffies - stats.last_reset));
@@ -1234,7 +1788,7 @@ void dsi_dump_irqs(struct seq_file *s)
 #define PIS(x) \
 	seq_printf(s, "%-20s %10d\n", #x, stats.dsi_irqs[ffs(DSI_IRQ_##x)-1]);
 
-	seq_printf(s, "-- DSI interrupts --\n");
+	seq_printf(s, "-- DSI%d interrupts --\n", dsi_module + 1);
 	PIS(VC0);
 	PIS(VC1);
 	PIS(VC2);
@@ -1300,13 +1854,46 @@ void dsi_dump_irqs(struct seq_file *s)
 	PIS(ULPSACTIVENOT_ALL1);
 #undef PIS
 }
+
+static void dsi1_dump_irqs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(0);
+
+	dsi_dump_dsidev_irqs(dsidev, s);
+}
+
+static void dsi2_dump_irqs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(1);
+
+	dsi_dump_dsidev_irqs(dsidev, s);
+}
+
+void dsi_create_debugfs_files_irq(struct dentry *debugfs_dir,
+		const struct file_operations *debug_fops)
+{
+	struct platform_device *dsidev;
+
+	dsidev = dsi_get_dsidev_from_id(0);
+	if (dsidev)
+		debugfs_create_file("dsi1_irqs", S_IRUGO, debugfs_dir,
+			&dsi1_dump_irqs, debug_fops);
+
+	dsidev = dsi_get_dsidev_from_id(1);
+	if (dsidev)
+		debugfs_create_file("dsi2_irqs", S_IRUGO, debugfs_dir,
+			&dsi2_dump_irqs, debug_fops);
+}
 #endif
 
-void dsi_dump_regs(struct seq_file *s)
+static void dsi_dump_dsidev_regs(struct platform_device *dsidev,
+		struct seq_file *s)
 {
-#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(r))
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(dsidev, r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	if (dsi_runtime_get(dsidev))
+		return;
+	dsi_enable_scp_clk(dsidev);
 
 	DUMPREG(DSI_REVISION);
 	DUMPREG(DSI_SYSCONFIG);
@@ -1378,25 +1965,57 @@ void dsi_dump_regs(struct seq_file *s)
 	DUMPREG(DSI_PLL_CONFIGURATION1);
 	DUMPREG(DSI_PLL_CONFIGURATION2);
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dsi_disable_scp_clk(dsidev);
+	dsi_runtime_put(dsidev);
 #undef DUMPREG
 }
 
-enum dsi_complexio_power_state {
+static void dsi1_dump_regs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(0);
+
+	dsi_dump_dsidev_regs(dsidev, s);
+}
+
+static void dsi2_dump_regs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(1);
+
+	dsi_dump_dsidev_regs(dsidev, s);
+}
+
+void dsi_create_debugfs_files_reg(struct dentry *debugfs_dir,
+		const struct file_operations *debug_fops)
+{
+	struct platform_device *dsidev;
+
+	dsidev = dsi_get_dsidev_from_id(0);
+	if (dsidev)
+		debugfs_create_file("dsi1_regs", S_IRUGO, debugfs_dir,
+			&dsi1_dump_regs, debug_fops);
+
+	dsidev = dsi_get_dsidev_from_id(1);
+	if (dsidev)
+		debugfs_create_file("dsi2_regs", S_IRUGO, debugfs_dir,
+			&dsi2_dump_regs, debug_fops);
+}
+enum dsi_cio_power_state {
 	DSI_COMPLEXIO_POWER_OFF		= 0x0,
 	DSI_COMPLEXIO_POWER_ON		= 0x1,
 	DSI_COMPLEXIO_POWER_ULPS	= 0x2,
 };
 
-static int dsi_complexio_power(enum dsi_complexio_power_state state)
+static int dsi_cio_power(struct platform_device *dsidev,
+		enum dsi_cio_power_state state)
 {
 	int t = 0;
 
 	/* PWR_CMD */
-	REG_FLD_MOD(DSI_COMPLEXIO_CFG1, state, 28, 27);
+	REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG1, state, 28, 27);
 
 	/* PWR_STATUS */
-	while (FLD_GET(dsi_read_reg(DSI_COMPLEXIO_CFG1), 26, 25) != state) {
+	while (FLD_GET(dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1),
+			26, 25) != state) {
 		if (++t > 1000) {
 			DSSERR("failed to set complexio power state to "
 					"%d\n", state);
@@ -1408,9 +2027,70 @@ static int dsi_complexio_power(enum dsi_complexio_power_state state)
 	return 0;
 }
 
-static void dsi_complexio_config(struct omap_dss_device *dssdev)
+/* Number of data lanes present on DSI interface */
+static inline int dsi_get_num_data_lanes(struct platform_device *dsidev)
+{
+	/* DSI on OMAP3 doesn't have register DSI_GNQ, set number
+	 * of data lanes as 2 by default */
+	if (dss_has_feature(FEAT_DSI_GNQ))
+		return REG_GET(dsidev, DSI_GNQ, 11, 9);	/* NB_DATA_LANES */
+	else
+		return 2;
+}
+
+/* Number of data lanes used by the dss device */
+static inline int dsi_get_num_data_lanes_dssdev(struct omap_dss_device *dssdev)
+{
+	int num_data_lanes = 0;
+
+	if (dssdev->phy.dsi.data1_lane != 0)
+		num_data_lanes++;
+	if (dssdev->phy.dsi.data2_lane != 0)
+		num_data_lanes++;
+	if (dssdev->phy.dsi.data3_lane != 0)
+		num_data_lanes++;
+	if (dssdev->phy.dsi.data4_lane != 0)
+		num_data_lanes++;
+
+	return num_data_lanes;
+}
+
+static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
 {
+	int val;
+
+	/* line buffer on OMAP3 is 1024 x 24bits */
+	/* XXX: for some reason using full buffer size causes
+	 * considerable TX slowdown with update sizes that fill the
+	 * whole buffer */
+	if (!dss_has_feature(FEAT_DSI_GNQ))
+		return 1023 * 3;
+
+	val = REG_GET(dsidev, DSI_GNQ, 14, 12); /* VP1_LINE_BUFFER_SIZE */
+
+	switch (val) {
+	case 1:
+		return 512 * 3;		/* 512x24 bits */
+	case 2:
+		return 682 * 3;		/* 682x24 bits */
+	case 3:
+		return 853 * 3;		/* 853x24 bits */
+	case 4:
+		return 1024 * 3;	/* 1024x24 bits */
+	case 5:
+		return 1194 * 3;	/* 1194x24 bits */
+	case 6:
+		return 1365 * 3;	/* 1365x24 bits */
+	default:
+		BUG();
+	}
+}
+
+static void dsi_set_lane_config(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	u32 r;
+	int num_data_lanes_dssdev = dsi_get_num_data_lanes_dssdev(dssdev);
 
 	int clk_lane   = dssdev->phy.dsi.clk_lane;
 	int data1_lane = dssdev->phy.dsi.data1_lane;
@@ -1419,14 +2099,28 @@ static void dsi_complexio_config(struct omap_dss_device *dssdev)
 	int data1_pol  = dssdev->phy.dsi.data1_pol;
 	int data2_pol  = dssdev->phy.dsi.data2_pol;
 
-	r = dsi_read_reg(DSI_COMPLEXIO_CFG1);
+	r = dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1);
 	r = FLD_MOD(r, clk_lane, 2, 0);
 	r = FLD_MOD(r, clk_pol, 3, 3);
 	r = FLD_MOD(r, data1_lane, 6, 4);
 	r = FLD_MOD(r, data1_pol, 7, 7);
 	r = FLD_MOD(r, data2_lane, 10, 8);
 	r = FLD_MOD(r, data2_pol, 11, 11);
-	dsi_write_reg(DSI_COMPLEXIO_CFG1, r);
+	if (num_data_lanes_dssdev > 2) {
+		int data3_lane  = dssdev->phy.dsi.data3_lane;
+		int data3_pol  = dssdev->phy.dsi.data3_pol;
+
+		r = FLD_MOD(r, data3_lane, 14, 12);
+		r = FLD_MOD(r, data3_pol, 15, 15);
+	}
+	if (num_data_lanes_dssdev > 3) {
+		int data4_lane  = dssdev->phy.dsi.data4_lane;
+		int data4_pol  = dssdev->phy.dsi.data4_pol;
+
+		r = FLD_MOD(r, data4_lane, 18, 16);
+		r = FLD_MOD(r, data4_pol, 19, 19);
+	}
+	dsi_write_reg(dsidev, DSI_COMPLEXIO_CFG1, r);
 
 	/* The configuration of the DSI complex I/O (number of data lanes,
 	   position, differential order) should not be changed while
@@ -1440,27 +2134,31 @@ static void dsi_complexio_config(struct omap_dss_device *dssdev)
 	   DSI complex I/O configuration is unknown. */
 
 	/*
-	REG_FLD_MOD(DSI_CTRL, 1, 0, 0);
-	REG_FLD_MOD(DSI_CTRL, 0, 0, 0);
-	REG_FLD_MOD(DSI_CLK_CTRL, 1, 20, 20);
-	REG_FLD_MOD(DSI_CTRL, 1, 0, 0);
+	REG_FLD_MOD(dsidev, DSI_CTRL, 1, 0, 0);
+	REG_FLD_MOD(dsidev, DSI_CTRL, 0, 0, 0);
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20);
+	REG_FLD_MOD(dsidev, DSI_CTRL, 1, 0, 0);
 	*/
 }
 
-static inline unsigned ns2ddr(unsigned ns)
+static inline unsigned ns2ddr(struct platform_device *dsidev, unsigned ns)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
 	/* convert time in ns to ddr ticks, rounding up */
-	unsigned long ddr_clk = dsi.current_cinfo.clkin4ddr / 4;
+	unsigned long ddr_clk = dsi->current_cinfo.clkin4ddr / 4;
 	return (ns * (ddr_clk / 1000 / 1000) + 999) / 1000;
 }
 
-static inline unsigned ddr2ns(unsigned ddr)
+static inline unsigned ddr2ns(struct platform_device *dsidev, unsigned ddr)
 {
-	unsigned long ddr_clk = dsi.current_cinfo.clkin4ddr / 4;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	unsigned long ddr_clk = dsi->current_cinfo.clkin4ddr / 4;
 	return ddr * 1000 * 1000 / (ddr_clk / 1000);
 }
 
-static void dsi_complexio_timings(void)
+static void dsi_cio_timings(struct platform_device *dsidev)
 {
 	u32 r;
 	u32 ths_prepare, ths_prepare_ths_zero, ths_trail, ths_exit;
@@ -1472,185 +2170,335 @@ static void dsi_complexio_timings(void)
 	/* 1 * DDR_CLK = 2 * UI */
 
 	/* min 40ns + 4*UI	max 85ns + 6*UI */
-	ths_prepare = ns2ddr(70) + 2;
+	ths_prepare = ns2ddr(dsidev, 70) + 2;
 
 	/* min 145ns + 10*UI */
-	ths_prepare_ths_zero = ns2ddr(175) + 2;
+	ths_prepare_ths_zero = ns2ddr(dsidev, 175) + 2;
 
 	/* min max(8*UI, 60ns+4*UI) */
-	ths_trail = ns2ddr(60) + 5;
+	ths_trail = ns2ddr(dsidev, 60) + 5;
 
 	/* min 100ns */
-	ths_exit = ns2ddr(145);
+	ths_exit = ns2ddr(dsidev, 145);
 
 	/* tlpx min 50n */
-	tlpx_half = ns2ddr(25);
+	tlpx_half = ns2ddr(dsidev, 25);
 
 	/* min 60ns */
-	tclk_trail = ns2ddr(60) + 2;
+	tclk_trail = ns2ddr(dsidev, 60) + 2;
 
 	/* min 38ns, max 95ns */
-	tclk_prepare = ns2ddr(65);
+	tclk_prepare = ns2ddr(dsidev, 65);
 
 	/* min tclk-prepare + tclk-zero = 300ns */
-	tclk_zero = ns2ddr(260);
+	tclk_zero = ns2ddr(dsidev, 260);
 
 	DSSDBG("ths_prepare %u (%uns), ths_prepare_ths_zero %u (%uns)\n",
-		ths_prepare, ddr2ns(ths_prepare),
-		ths_prepare_ths_zero, ddr2ns(ths_prepare_ths_zero));
+		ths_prepare, ddr2ns(dsidev, ths_prepare),
+		ths_prepare_ths_zero, ddr2ns(dsidev, ths_prepare_ths_zero));
 	DSSDBG("ths_trail %u (%uns), ths_exit %u (%uns)\n",
-			ths_trail, ddr2ns(ths_trail),
-			ths_exit, ddr2ns(ths_exit));
+			ths_trail, ddr2ns(dsidev, ths_trail),
+			ths_exit, ddr2ns(dsidev, ths_exit));
 
 	DSSDBG("tlpx_half %u (%uns), tclk_trail %u (%uns), "
 			"tclk_zero %u (%uns)\n",
-			tlpx_half, ddr2ns(tlpx_half),
-			tclk_trail, ddr2ns(tclk_trail),
-			tclk_zero, ddr2ns(tclk_zero));
+			tlpx_half, ddr2ns(dsidev, tlpx_half),
+			tclk_trail, ddr2ns(dsidev, tclk_trail),
+			tclk_zero, ddr2ns(dsidev, tclk_zero));
 	DSSDBG("tclk_prepare %u (%uns)\n",
-			tclk_prepare, ddr2ns(tclk_prepare));
+			tclk_prepare, ddr2ns(dsidev, tclk_prepare));
 
 	/* program timings */
 
-	r = dsi_read_reg(DSI_DSIPHY_CFG0);
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
 	r = FLD_MOD(r, ths_prepare, 31, 24);
 	r = FLD_MOD(r, ths_prepare_ths_zero, 23, 16);
 	r = FLD_MOD(r, ths_trail, 15, 8);
 	r = FLD_MOD(r, ths_exit, 7, 0);
-	dsi_write_reg(DSI_DSIPHY_CFG0, r);
+	dsi_write_reg(dsidev, DSI_DSIPHY_CFG0, r);
 
-	r = dsi_read_reg(DSI_DSIPHY_CFG1);
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
 	r = FLD_MOD(r, tlpx_half, 22, 16);
 	r = FLD_MOD(r, tclk_trail, 15, 8);
 	r = FLD_MOD(r, tclk_zero, 7, 0);
-	dsi_write_reg(DSI_DSIPHY_CFG1, r);
+	dsi_write_reg(dsidev, DSI_DSIPHY_CFG1, r);
 
-	r = dsi_read_reg(DSI_DSIPHY_CFG2);
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
 	r = FLD_MOD(r, tclk_prepare, 7, 0);
-	dsi_write_reg(DSI_DSIPHY_CFG2, r);
+	dsi_write_reg(dsidev, DSI_DSIPHY_CFG2, r);
 }
 
+static void dsi_cio_enable_lane_override(struct omap_dss_device *dssdev,
+		enum dsi_lane lanes)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int clk_lane   = dssdev->phy.dsi.clk_lane;
+	int data1_lane = dssdev->phy.dsi.data1_lane;
+	int data2_lane = dssdev->phy.dsi.data2_lane;
+	int data3_lane = dssdev->phy.dsi.data3_lane;
+	int data4_lane = dssdev->phy.dsi.data4_lane;
+	int clk_pol    = dssdev->phy.dsi.clk_pol;
+	int data1_pol  = dssdev->phy.dsi.data1_pol;
+	int data2_pol  = dssdev->phy.dsi.data2_pol;
+	int data3_pol  = dssdev->phy.dsi.data3_pol;
+	int data4_pol  = dssdev->phy.dsi.data4_pol;
+
+	u32 l = 0;
+	u8 lptxscp_start = dsi->num_data_lanes == 2 ? 22 : 26;
+
+	if (lanes & DSI_CLK_P)
+		l |= 1 << ((clk_lane - 1) * 2 + (clk_pol ? 0 : 1));
+	if (lanes & DSI_CLK_N)
+		l |= 1 << ((clk_lane - 1) * 2 + (clk_pol ? 1 : 0));
+
+	if (lanes & DSI_DATA1_P)
+		l |= 1 << ((data1_lane - 1) * 2 + (data1_pol ? 0 : 1));
+	if (lanes & DSI_DATA1_N)
+		l |= 1 << ((data1_lane - 1) * 2 + (data1_pol ? 1 : 0));
+
+	if (lanes & DSI_DATA2_P)
+		l |= 1 << ((data2_lane - 1) * 2 + (data2_pol ? 0 : 1));
+	if (lanes & DSI_DATA2_N)
+		l |= 1 << ((data2_lane - 1) * 2 + (data2_pol ? 1 : 0));
+
+	if (lanes & DSI_DATA3_P)
+		l |= 1 << ((data3_lane - 1) * 2 + (data3_pol ? 0 : 1));
+	if (lanes & DSI_DATA3_N)
+		l |= 1 << ((data3_lane - 1) * 2 + (data3_pol ? 1 : 0));
+
+	if (lanes & DSI_DATA4_P)
+		l |= 1 << ((data4_lane - 1) * 2 + (data4_pol ? 0 : 1));
+	if (lanes & DSI_DATA4_N)
+		l |= 1 << ((data4_lane - 1) * 2 + (data4_pol ? 1 : 0));
+	/*
+	 * Bits in REGLPTXSCPDAT4TO0DXDY:
+	 * 17: DY0 18: DX0
+	 * 19: DY1 20: DX1
+	 * 21: DY2 22: DX2
+	 * 23: DY3 24: DX3
+	 * 25: DY4 26: DX4
+	 */
+
+	/* Set the lane override configuration */
+
+	/* REGLPTXSCPDAT4TO0DXDY */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, l, lptxscp_start, 17);
+
+	/* Enable lane override */
 
-static int dsi_complexio_init(struct omap_dss_device *dssdev)
+	/* ENLPTXSCPDAT */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 1, 27, 27);
+}
+
+static void dsi_cio_disable_lane_override(struct platform_device *dsidev)
 {
-	int r = 0;
+	/* Disable lane override */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 27, 27); /* ENLPTXSCPDAT */
+	/* Reset the lane override configuration */
+	/* REGLPTXSCPDAT4TO0DXDY */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 22, 17);
+}
 
-	DSSDBG("dsi_complexio_init\n");
+static int dsi_cio_wait_tx_clk_esc_reset(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	int t;
+	int bits[3];
+	bool in_use[3];
+
+	if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC)) {
+		bits[0] = 28;
+		bits[1] = 27;
+		bits[2] = 26;
+	} else {
+		bits[0] = 24;
+		bits[1] = 25;
+		bits[2] = 26;
+	}
 
-	/* CIO_CLK_ICG, enable L3 clk to CIO */
-	REG_FLD_MOD(DSI_CLK_CTRL, 1, 14, 14);
+	in_use[0] = false;
+	in_use[1] = false;
+	in_use[2] = false;
+
+	if (dssdev->phy.dsi.clk_lane != 0)
+		in_use[dssdev->phy.dsi.clk_lane - 1] = true;
+	if (dssdev->phy.dsi.data1_lane != 0)
+		in_use[dssdev->phy.dsi.data1_lane - 1] = true;
+	if (dssdev->phy.dsi.data2_lane != 0)
+		in_use[dssdev->phy.dsi.data2_lane - 1] = true;
+
+	t = 100000;
+	while (true) {
+		u32 l;
+		int i;
+		int ok;
+
+		l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+
+		ok = 0;
+		for (i = 0; i < 3; ++i) {
+			if (!in_use[i] || (l & (1 << bits[i])))
+				ok++;
+		}
+
+		if (ok == 3)
+			break;
+
+		if (--t == 0) {
+			for (i = 0; i < 3; ++i) {
+				if (!in_use[i] || (l & (1 << bits[i])))
+					continue;
+
+				DSSERR("CIO TXCLKESC%d domain not coming " \
+						"out of reset\n", i);
+			}
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int dsi_cio_init(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+	int num_data_lanes_dssdev = dsi_get_num_data_lanes_dssdev(dssdev);
+	u32 l;
+
+	DSSDBGF();
+
+	if (dsi->dsi_mux_pads)
+		dsi->dsi_mux_pads(true);
+
+	dsi_enable_scp_clk(dsidev);
 
 	/* A dummy read using the SCP interface to any DSIPHY register is
 	 * required after DSIPHY reset to complete the reset of the DSI complex
 	 * I/O. */
-	dsi_read_reg(DSI_DSIPHY_CFG5);
+	dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
 
-	if (wait_for_bit_change(DSI_DSIPHY_CFG5, 30, 1) != 1) {
-		DSSERR("ComplexIO PHY not coming out of reset.\n");
-		r = -ENODEV;
-		goto err;
+	if (wait_for_bit_change(dsidev, DSI_DSIPHY_CFG5, 30, 1) != 1) {
+		DSSERR("CIO SCP Clock domain not coming out of reset.\n");
+		r = -EIO;
+		goto err_scp_clk_dom;
 	}
 
-	dsi_complexio_config(dssdev);
+	dsi_set_lane_config(dssdev);
 
-	r = dsi_complexio_power(DSI_COMPLEXIO_POWER_ON);
+	/* set TX STOP MODE timer to maximum for this operation */
+	l = dsi_read_reg(dsidev, DSI_TIMING1);
+	l = FLD_MOD(l, 1, 15, 15);	/* FORCE_TX_STOP_MODE_IO */
+	l = FLD_MOD(l, 1, 14, 14);	/* STOP_STATE_X16_IO */
+	l = FLD_MOD(l, 1, 13, 13);	/* STOP_STATE_X4_IO */
+	l = FLD_MOD(l, 0x1fff, 12, 0);	/* STOP_STATE_COUNTER_IO */
+	dsi_write_reg(dsidev, DSI_TIMING1, l);
 
-	if (r)
-		goto err;
+	if (dsi->ulps_enabled) {
+		u32 lane_mask = DSI_CLK_P | DSI_DATA1_P | DSI_DATA2_P;
 
-	if (wait_for_bit_change(DSI_COMPLEXIO_CFG1, 29, 1) != 1) {
-		DSSERR("ComplexIO not coming out of reset.\n");
-		r = -ENODEV;
-		goto err;
+		DSSDBG("manual ulps exit\n");
+
+		/* ULPS is exited by Mark-1 state for 1ms, followed by
+		 * stop state. DSS HW cannot do this via the normal
+		 * ULPS exit sequence, as after reset the DSS HW thinks
+		 * that we are not in ULPS mode, and refuses to send the
+		 * sequence. So we need to send the ULPS exit sequence
+		 * manually.
+		 */
+
+		if (num_data_lanes_dssdev > 2)
+			lane_mask |= DSI_DATA3_P;
+
+		if (num_data_lanes_dssdev > 3)
+			lane_mask |= DSI_DATA4_P;
+
+		dsi_cio_enable_lane_override(dssdev, lane_mask);
 	}
 
-	if (wait_for_bit_change(DSI_COMPLEXIO_CFG1, 21, 1) != 1) {
-		DSSERR("ComplexIO LDO power down.\n");
+	r = dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ON);
+	if (r)
+		goto err_cio_pwr;
+
+	if (wait_for_bit_change(dsidev, DSI_COMPLEXIO_CFG1, 29, 1) != 1) {
+		DSSERR("CIO PWR clock domain not coming out of reset.\n");
 		r = -ENODEV;
-		goto err;
+		goto err_cio_pwr_dom;
 	}
 
-	dsi_complexio_timings();
+	dsi_if_enable(dsidev, true);
+	dsi_if_enable(dsidev, false);
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
 
-	/*
-	   The configuration of the DSI complex I/O (number of data lanes,
-	   position, differential order) should not be changed while
-	   DSS.DSI_CLK_CRTRL[20] LP_CLK_ENABLE bit is set to 1. For the
-	   hardware to recognize a new configuration of the complex I/O (done
-	   in DSS.DSI_COMPLEXIO_CFG1 register), it is recommended to follow
-	   this sequence: First set the DSS.DSI_CTRL[0] IF_EN bit to 1, next
-	   reset the DSS.DSI_CTRL[0] IF_EN to 0, then set DSS.DSI_CLK_CTRL[20]
-	   LP_CLK_ENABLE to 1, and finally, set again the DSS.DSI_CTRL[0] IF_EN
-	   bit to 1. If the sequence is not followed, the DSi complex I/O
-	   configuration is undetermined.
-	   */
-	dsi_if_enable(1);
-	dsi_if_enable(0);
-	REG_FLD_MOD(DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
-	dsi_if_enable(1);
-	dsi_if_enable(0);
+	r = dsi_cio_wait_tx_clk_esc_reset(dssdev);
+	if (r)
+		goto err_tx_clk_esc_rst;
 
-	DSSDBG("CIO init done\n");
-err:
-	return r;
-}
+	if (dsi->ulps_enabled) {
+		/* Keep Mark-1 state for 1ms (as per DSI spec) */
+		ktime_t wait = ns_to_ktime(1000 * 1000);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
 
-static void dsi_complexio_uninit(void)
-{
-	dsi_complexio_power(DSI_COMPLEXIO_POWER_OFF);
-}
+		/* Disable the override. The lanes should be set to Mark-11
+		 * state by the HW */
+		dsi_cio_disable_lane_override(dsidev);
+	}
 
-static int _dsi_wait_reset(void)
-{
-	int t = 0;
+	/* FORCE_TX_STOP_MODE_IO */
+	REG_FLD_MOD(dsidev, DSI_TIMING1, 0, 15, 15);
 
-	while (REG_GET(DSI_SYSSTATUS, 0, 0) == 0) {
-		if (++t > 5) {
-			DSSERR("soft reset failed\n");
-			return -ENODEV;
-		}
-		udelay(1);
-	}
+	dsi_cio_timings(dsidev);
+
+	dsi->ulps_enabled = false;
+
+	DSSDBG("CIO init done\n");
 
 	return 0;
-}
 
-static int _dsi_reset(void)
-{
-	/* Soft reset */
-	REG_FLD_MOD(DSI_SYSCONFIG, 1, 1, 1);
-	return _dsi_wait_reset();
+err_tx_clk_esc_rst:
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 20, 20); /* LP_CLK_ENABLE */
+err_cio_pwr_dom:
+	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+err_cio_pwr:
+	if (dsi->ulps_enabled)
+		dsi_cio_disable_lane_override(dsidev);
+err_scp_clk_dom:
+	dsi_disable_scp_clk(dsidev);
+	if (dsi->dsi_mux_pads)
+		dsi->dsi_mux_pads(false);
+	return r;
 }
 
-static void dsi_reset_tx_fifo(int channel)
+static void dsi_cio_uninit(struct platform_device *dsidev)
 {
-	u32 mask;
-	u32 l;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
-	/* set fifosize of the channel to 0, then return the old size */
-	l = dsi_read_reg(DSI_TX_FIFO_VC_SIZE);
-
-	mask = FLD_MASK((8 * channel) + 7, (8 * channel) + 4);
-	dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l & ~mask);
-
-	dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l);
+	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+	dsi_disable_scp_clk(dsidev);
+	if (dsi->dsi_mux_pads)
+		dsi->dsi_mux_pads(false);
 }
 
-static void dsi_config_tx_fifo(enum fifo_size size1, enum fifo_size size2,
+static void dsi_config_tx_fifo(struct platform_device *dsidev,
+		enum fifo_size size1, enum fifo_size size2,
 		enum fifo_size size3, enum fifo_size size4)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 r = 0;
 	int add = 0;
 	int i;
 
-	dsi.vc[0].fifo_size = size1;
-	dsi.vc[1].fifo_size = size2;
-	dsi.vc[2].fifo_size = size3;
-	dsi.vc[3].fifo_size = size4;
+	dsi->vc[0].fifo_size = size1;
+	dsi->vc[1].fifo_size = size2;
+	dsi->vc[2].fifo_size = size3;
+	dsi->vc[3].fifo_size = size4;
 
 	for (i = 0; i < 4; i++) {
 		u8 v;
-		int size = dsi.vc[i].fifo_size;
+		int size = dsi->vc[i].fifo_size;
 
 		if (add + size > 4) {
 			DSSERR("Illegal FIFO configuration\n");
@@ -1663,24 +2511,26 @@ static void dsi_config_tx_fifo(enum fifo_size size1, enum fifo_size size2,
 		add += size;
 	}
 
-	dsi_write_reg(DSI_TX_FIFO_VC_SIZE, r);
+	dsi_write_reg(dsidev, DSI_TX_FIFO_VC_SIZE, r);
 }
 
-static void dsi_config_rx_fifo(enum fifo_size size1, enum fifo_size size2,
+static void dsi_config_rx_fifo(struct platform_device *dsidev,
+		enum fifo_size size1, enum fifo_size size2,
 		enum fifo_size size3, enum fifo_size size4)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 r = 0;
 	int add = 0;
 	int i;
 
-	dsi.vc[0].fifo_size = size1;
-	dsi.vc[1].fifo_size = size2;
-	dsi.vc[2].fifo_size = size3;
-	dsi.vc[3].fifo_size = size4;
+	dsi->vc[0].fifo_size = size1;
+	dsi->vc[1].fifo_size = size2;
+	dsi->vc[2].fifo_size = size3;
+	dsi->vc[3].fifo_size = size4;
 
 	for (i = 0; i < 4; i++) {
 		u8 v;
-		int size = dsi.vc[i].fifo_size;
+		int size = dsi->vc[i].fifo_size;
 
 		if (add + size > 4) {
 			DSSERR("Illegal FIFO configuration\n");
@@ -1693,18 +2543,18 @@ static void dsi_config_rx_fifo(enum fifo_size size1, enum fifo_size size2,
 		add += size;
 	}
 
-	dsi_write_reg(DSI_RX_FIFO_VC_SIZE, r);
+	dsi_write_reg(dsidev, DSI_RX_FIFO_VC_SIZE, r);
 }
 
-static int dsi_force_tx_stop_mode_io(void)
+static int dsi_force_tx_stop_mode_io(struct platform_device *dsidev)
 {
 	u32 r;
 
-	r = dsi_read_reg(DSI_TIMING1);
+	r = dsi_read_reg(dsidev, DSI_TIMING1);
 	r = FLD_MOD(r, 1, 15, 15);	/* FORCE_TX_STOP_MODE_IO */
-	dsi_write_reg(DSI_TIMING1, r);
+	dsi_write_reg(dsidev, DSI_TIMING1, r);
 
-	if (wait_for_bit_change(DSI_TIMING1, 15, 0) != 0) {
+	if (wait_for_bit_change(dsidev, DSI_TIMING1, 15, 0) != 0) {
 		DSSERR("TX_STOP bit not going down\n");
 		return -EIO;
 	}
@@ -1712,16 +2562,135 @@ static int dsi_force_tx_stop_mode_io(void)
 	return 0;
 }
 
-static int dsi_vc_enable(int channel, bool enable)
+static bool dsi_vc_is_enabled(struct platform_device *dsidev, int channel)
+{
+	return REG_GET(dsidev, DSI_VC_CTRL(channel), 0, 0);
+}
+
+static void dsi_packet_sent_handler_vp(void *data, u32 mask)
+{
+	struct dsi_packet_sent_handler_data *vp_data =
+		(struct dsi_packet_sent_handler_data *) data;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(vp_data->dsidev);
+	const int channel = dsi->update_channel;
+	u8 bit = dsi->te_enabled ? 30 : 31;
+
+	if (REG_GET(vp_data->dsidev, DSI_VC_TE(channel), bit, bit) == 0)
+		complete(vp_data->completion);
+}
+
+static int dsi_sync_vc_vp(struct platform_device *dsidev, int channel)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	DECLARE_COMPLETION_ONSTACK(completion);
+	struct dsi_packet_sent_handler_data vp_data = { dsidev, &completion };
+	int r = 0;
+	u8 bit;
+
+	bit = dsi->te_enabled ? 30 : 31;
+
+	r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+		&vp_data, DSI_VC_IRQ_PACKET_SENT);
+	if (r)
+		goto err0;
+
+	/* Wait for completion only if TE_EN/TE_START is still set */
+	if (REG_GET(dsidev, DSI_VC_TE(channel), bit, bit)) {
+		if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(10)) == 0) {
+			DSSERR("Failed to complete previous frame transfer\n");
+			r = -EIO;
+			goto err1;
+		}
+	}
+
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+		&vp_data, DSI_VC_IRQ_PACKET_SENT);
+
+	return 0;
+err1:
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+		&vp_data, DSI_VC_IRQ_PACKET_SENT);
+err0:
+	return r;
+}
+
+static void dsi_packet_sent_handler_l4(void *data, u32 mask)
+{
+	struct dsi_packet_sent_handler_data *l4_data =
+		(struct dsi_packet_sent_handler_data *) data;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(l4_data->dsidev);
+	const int channel = dsi->update_channel;
+
+	if (REG_GET(l4_data->dsidev, DSI_VC_CTRL(channel), 5, 5) == 0)
+		complete(l4_data->completion);
+}
+
+static int dsi_sync_vc_l4(struct platform_device *dsidev, int channel)
+{
+	DECLARE_COMPLETION_ONSTACK(completion);
+	struct dsi_packet_sent_handler_data l4_data = { dsidev, &completion };
+	int r = 0;
+
+	r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+		&l4_data, DSI_VC_IRQ_PACKET_SENT);
+	if (r)
+		goto err0;
+
+	/* Wait for completion only if TX_FIFO_NOT_EMPTY is still set */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 5, 5)) {
+		if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(10)) == 0) {
+			DSSERR("Failed to complete previous l4 transfer\n");
+			r = -EIO;
+			goto err1;
+		}
+	}
+
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+		&l4_data, DSI_VC_IRQ_PACKET_SENT);
+
+	return 0;
+err1:
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+		&l4_data, DSI_VC_IRQ_PACKET_SENT);
+err0:
+	return r;
+}
+
+static int dsi_sync_vc(struct platform_device *dsidev, int channel)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	WARN_ON(in_interrupt());
+
+	if (!dsi_vc_is_enabled(dsidev, channel))
+		return 0;
+
+	switch (dsi->vc[channel].mode) {
+	case DSI_VC_MODE_VP:
+		return dsi_sync_vc_vp(dsidev, channel);
+	case DSI_VC_MODE_L4:
+		return dsi_sync_vc_l4(dsidev, channel);
+	default:
+		BUG();
+	}
+}
+
+static int dsi_vc_enable(struct platform_device *dsidev, int channel,
+		bool enable)
 {
 	DSSDBG("dsi_vc_enable channel %d, enable %d\n",
 			channel, enable);
 
 	enable = enable ? 1 : 0;
 
-	REG_FLD_MOD(DSI_VC_CTRL(channel), enable, 0, 0);
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 0, 0);
 
-	if (wait_for_bit_change(DSI_VC_CTRL(channel), 0, enable) != enable) {
+	if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel),
+		0, enable) != enable) {
 			DSSERR("Failed to set dsi_vc_enable to %d\n", enable);
 			return -EIO;
 	}
@@ -1729,13 +2698,13 @@ static int dsi_vc_enable(int channel, bool enable)
 	return 0;
 }
 
-static void dsi_vc_initial_config(int channel)
+static void dsi_vc_initial_config(struct platform_device *dsidev, int channel)
 {
 	u32 r;
 
 	DSSDBGF("%d", channel);
 
-	r = dsi_read_reg(DSI_VC_CTRL(channel));
+	r = dsi_read_reg(dsidev, DSI_VC_CTRL(channel));
 
 	if (FLD_GET(r, 15, 15)) /* VC_BUSY */
 		DSSERR("VC(%d) busy when trying to configure it!\n",
@@ -1748,87 +2717,107 @@ static void dsi_vc_initial_config(int channel)
 	r = FLD_MOD(r, 1, 7, 7); /* CS_TX_EN */
 	r = FLD_MOD(r, 1, 8, 8); /* ECC_TX_EN */
 	r = FLD_MOD(r, 0, 9, 9); /* MODE_SPEED, high speed on/off */
+	if (dss_has_feature(FEAT_DSI_VC_OCP_WIDTH))
+		r = FLD_MOD(r, 3, 11, 10);	/* OCP_WIDTH = 32 bit */
 
 	r = FLD_MOD(r, 4, 29, 27); /* DMA_RX_REQ_NB = no dma */
 	r = FLD_MOD(r, 4, 23, 21); /* DMA_TX_REQ_NB = no dma */
 
-	dsi_write_reg(DSI_VC_CTRL(channel), r);
-
-	dsi.vc[channel].mode = DSI_VC_MODE_L4;
+	dsi_write_reg(dsidev, DSI_VC_CTRL(channel), r);
 }
 
-static int dsi_vc_config_l4(int channel)
+static int dsi_vc_config_l4(struct platform_device *dsidev, int channel)
 {
-	if (dsi.vc[channel].mode == DSI_VC_MODE_L4)
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->vc[channel].mode == DSI_VC_MODE_L4)
 		return 0;
 
 	DSSDBGF("%d", channel);
 
-	dsi_vc_enable(channel, 0);
+	dsi_sync_vc(dsidev, channel);
+
+	dsi_vc_enable(dsidev, channel, 0);
 
 	/* VC_BUSY */
-	if (wait_for_bit_change(DSI_VC_CTRL(channel), 15, 0) != 0) {
+	if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel), 15, 0) != 0) {
 		DSSERR("vc(%d) busy when trying to config for L4\n", channel);
 		return -EIO;
 	}
 
-	REG_FLD_MOD(DSI_VC_CTRL(channel), 0, 1, 1); /* SOURCE, 0 = L4 */
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 0, 1, 1); /* SOURCE, 0 = L4 */
 
-	dsi_vc_enable(channel, 1);
+	/* DCS_CMD_ENABLE */
+	if (dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC))
+		REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 0, 30, 30);
 
-	dsi.vc[channel].mode = DSI_VC_MODE_L4;
+	dsi_vc_enable(dsidev, channel, 1);
+
+	dsi->vc[channel].mode = DSI_VC_MODE_L4;
 
 	return 0;
 }
 
-static int dsi_vc_config_vp(int channel)
+static int dsi_vc_config_vp(struct platform_device *dsidev, int channel)
 {
-	if (dsi.vc[channel].mode == DSI_VC_MODE_VP)
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->vc[channel].mode == DSI_VC_MODE_VP)
 		return 0;
 
 	DSSDBGF("%d", channel);
 
-	dsi_vc_enable(channel, 0);
+	dsi_sync_vc(dsidev, channel);
+
+	dsi_vc_enable(dsidev, channel, 0);
 
 	/* VC_BUSY */
-	if (wait_for_bit_change(DSI_VC_CTRL(channel), 15, 0) != 0) {
+	if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel), 15, 0) != 0) {
 		DSSERR("vc(%d) busy when trying to config for VP\n", channel);
 		return -EIO;
 	}
 
-	REG_FLD_MOD(DSI_VC_CTRL(channel), 1, 1, 1); /* SOURCE, 1 = video port */
+	/* SOURCE, 1 = video port */
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 1, 1);
+
+	/* DCS_CMD_ENABLE */
+	if (dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC))
+		REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 30, 30);
 
-	dsi_vc_enable(channel, 1);
+	dsi_vc_enable(dsidev, channel, 1);
 
-	dsi.vc[channel].mode = DSI_VC_MODE_VP;
+	dsi->vc[channel].mode = DSI_VC_MODE_VP;
 
 	return 0;
 }
 
 
-void omapdss_dsi_vc_enable_hs(int channel, bool enable)
+void omapdss_dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
+		bool enable)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
 	DSSDBG("dsi_vc_enable_hs(%d, %d)\n", channel, enable);
 
-	WARN_ON(!dsi_bus_is_locked());
+	WARN_ON(!dsi_bus_is_locked(dsidev));
 
-	dsi_vc_enable(channel, 0);
-	dsi_if_enable(0);
+	dsi_vc_enable(dsidev, channel, 0);
+	dsi_if_enable(dsidev, 0);
 
-	REG_FLD_MOD(DSI_VC_CTRL(channel), enable, 9, 9);
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 9, 9);
 
-	dsi_vc_enable(channel, 1);
-	dsi_if_enable(1);
+	dsi_vc_enable(dsidev, channel, 1);
+	dsi_if_enable(dsidev, 1);
 
-	dsi_force_tx_stop_mode_io();
+	dsi_force_tx_stop_mode_io(dsidev);
 }
 EXPORT_SYMBOL(omapdss_dsi_vc_enable_hs);
 
-static void dsi_vc_flush_long_data(int channel)
+static void dsi_vc_flush_long_data(struct platform_device *dsidev, int channel)
 {
-	while (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
+	while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
 		u32 val;
-		val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
+		val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
 		DSSDBG("\t\tb1 %#02x b2 %#02x b3 %#02x b4 %#02x\n",
 				(val >> 0) & 0xff,
 				(val >> 8) & 0xff,
@@ -1874,13 +2863,14 @@ static void dsi_show_rx_ack_with_err(u16 err)
 		DSSERR("\t\tDSI Protocol Violation\n");
 }
 
-static u16 dsi_vc_flush_receive_data(int channel)
+static u16 dsi_vc_flush_receive_data(struct platform_device *dsidev,
+		int channel)
 {
 	/* RX_FIFO_NOT_EMPTY */
-	while (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
+	while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
 		u32 val;
 		u8 dt;
-		val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
+		val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
 		DSSERR("\trawval %#08x\n", val);
 		dt = FLD_GET(val, 5, 0);
 		if (dt == DSI_DT_RX_ACK_WITH_ERR) {
@@ -1895,7 +2885,7 @@ static u16 dsi_vc_flush_receive_data(int channel)
 		} else if (dt == DSI_DT_RX_DCS_LONG_READ) {
 			DSSERR("\tDCS long response, len %d\n",
 					FLD_GET(val, 23, 8));
-			dsi_vc_flush_long_data(channel);
+			dsi_vc_flush_long_data(dsidev, channel);
 		} else {
 			DSSERR("\tunknown datatype 0x%02x\n", dt);
 		}
@@ -1903,74 +2893,90 @@ static u16 dsi_vc_flush_receive_data(int channel)
 	return 0;
 }
 
-static int dsi_vc_send_bta(int channel)
+static int dsi_vc_send_bta(struct platform_device *dsidev, int channel)
 {
-	if (dsi.debug_write || dsi.debug_read)
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->debug_write || dsi->debug_read)
 		DSSDBG("dsi_vc_send_bta %d\n", channel);
 
-	WARN_ON(!dsi_bus_is_locked());
+	WARN_ON(!dsi_bus_is_locked(dsidev));
 
-	if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {	/* RX_FIFO_NOT_EMPTY */
+	/* RX_FIFO_NOT_EMPTY */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
 		DSSERR("rx fifo not empty when sending BTA, dumping data:\n");
-		dsi_vc_flush_receive_data(channel);
+		dsi_vc_flush_receive_data(dsidev, channel);
 	}
 
-	REG_FLD_MOD(DSI_VC_CTRL(channel), 1, 6, 6); /* BTA_EN */
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 6, 6); /* BTA_EN */
 
 	return 0;
 }
 
-int dsi_vc_send_bta_sync(int channel)
+int dsi_vc_send_bta_sync(struct omap_dss_device *dssdev, int channel)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	DECLARE_COMPLETION_ONSTACK(completion);
 	int r = 0;
 	u32 err;
 
-	INIT_COMPLETION(dsi.bta_completion);
+	r = dsi_register_isr_vc(dsidev, channel, dsi_completion_handler,
+			&completion, DSI_VC_IRQ_BTA);
+	if (r)
+		goto err0;
 
-	dsi_vc_enable_bta_irq(channel);
+	r = dsi_register_isr(dsidev, dsi_completion_handler, &completion,
+			DSI_IRQ_ERROR_MASK);
+	if (r)
+		goto err1;
 
-	r = dsi_vc_send_bta(channel);
+	r = dsi_vc_send_bta(dsidev, channel);
 	if (r)
-		goto err;
+		goto err2;
 
-	if (wait_for_completion_timeout(&dsi.bta_completion,
+	if (wait_for_completion_timeout(&completion,
 				msecs_to_jiffies(500)) == 0) {
 		DSSERR("Failed to receive BTA\n");
 		r = -EIO;
-		goto err;
+		goto err2;
 	}
 
-	err = dsi_get_errors();
+	err = dsi_get_errors(dsidev);
 	if (err) {
 		DSSERR("Error while sending BTA: %x\n", err);
 		r = -EIO;
-		goto err;
+		goto err2;
 	}
-err:
-	dsi_vc_disable_bta_irq(channel);
-
+err2:
+	dsi_unregister_isr(dsidev, dsi_completion_handler, &completion,
+			DSI_IRQ_ERROR_MASK);
+err1:
+	dsi_unregister_isr_vc(dsidev, channel, dsi_completion_handler,
+			&completion, DSI_VC_IRQ_BTA);
+err0:
 	return r;
 }
 EXPORT_SYMBOL(dsi_vc_send_bta_sync);
 
-static inline void dsi_vc_write_long_header(int channel, u8 data_type,
-		u16 len, u8 ecc)
+static inline void dsi_vc_write_long_header(struct platform_device *dsidev,
+		int channel, u8 data_type, u16 len, u8 ecc)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 val;
 	u8 data_id;
 
-	WARN_ON(!dsi_bus_is_locked());
+	WARN_ON(!dsi_bus_is_locked(dsidev));
 
-	data_id = data_type | channel << 6;
+	data_id = data_type | dsi->vc[channel].vc_id << 6;
 
 	val = FLD_VAL(data_id, 7, 0) | FLD_VAL(len, 23, 8) |
 		FLD_VAL(ecc, 31, 24);
 
-	dsi_write_reg(DSI_VC_LONG_PACKET_HEADER(channel), val);
+	dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_HEADER(channel), val);
 }
 
-static inline void dsi_vc_write_long_payload(int channel,
-		u8 b1, u8 b2, u8 b3, u8 b4)
+static inline void dsi_vc_write_long_payload(struct platform_device *dsidev,
+		int channel, u8 b1, u8 b2, u8 b3, u8 b4)
 {
 	u32 val;
 
@@ -1979,34 +2985,35 @@ static inline void dsi_vc_write_long_payload(int channel,
 /*	DSSDBG("\twriting %02x, %02x, %02x, %02x (%#010x)\n",
 			b1, b2, b3, b4, val); */
 
-	dsi_write_reg(DSI_VC_LONG_PACKET_PAYLOAD(channel), val);
+	dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_PAYLOAD(channel), val);
 }
 
-static int dsi_vc_send_long(int channel, u8 data_type, u8 *data, u16 len,
-		u8 ecc)
+static int dsi_vc_send_long(struct platform_device *dsidev, int channel,
+		u8 data_type, u8 *data, u16 len, u8 ecc)
 {
 	/*u32 val; */
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int i;
 	u8 *p;
 	int r = 0;
 	u8 b1, b2, b3, b4;
 
-	if (dsi.debug_write)
+	if (dsi->debug_write)
 		DSSDBG("dsi_vc_send_long, %d bytes\n", len);
 
 	/* len + header */
-	if (dsi.vc[channel].fifo_size * 32 * 4 < len + 4) {
+	if (dsi->vc[channel].fifo_size * 32 * 4 < len + 4) {
 		DSSERR("unable to send long packet: packet too long.\n");
 		return -EINVAL;
 	}
 
-	dsi_vc_config_l4(channel);
+	dsi_vc_config_l4(dsidev, channel);
 
-	dsi_vc_write_long_header(channel, data_type, len, ecc);
+	dsi_vc_write_long_header(dsidev, channel, data_type, len, ecc);
 
 	p = data;
 	for (i = 0; i < len >> 2; i++) {
-		if (dsi.debug_write)
+		if (dsi->debug_write)
 			DSSDBG("\tsending full packet %d\n", i);
 
 		b1 = *p++;
@@ -2014,14 +3021,14 @@ static int dsi_vc_send_long(int channel, u8 data_type, u8 *data, u16 len,
 		b3 = *p++;
 		b4 = *p++;
 
-		dsi_vc_write_long_payload(channel, b1, b2, b3, b4);
+		dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, b4);
 	}
 
 	i = len % 4;
 	if (i) {
 		b1 = 0; b2 = 0; b3 = 0;
 
-		if (dsi.debug_write)
+		if (dsi->debug_write)
 			DSSDBG("\tsending remainder bytes %d\n", i);
 
 		switch (i) {
@@ -2039,62 +3046,69 @@ static int dsi_vc_send_long(int channel, u8 data_type, u8 *data, u16 len,
 			break;
 		}
 
-		dsi_vc_write_long_payload(channel, b1, b2, b3, 0);
+		dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, 0);
 	}
 
 	return r;
 }
 
-static int dsi_vc_send_short(int channel, u8 data_type, u16 data, u8 ecc)
+static int dsi_vc_send_short(struct platform_device *dsidev, int channel,
+		u8 data_type, u16 data, u8 ecc)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 r;
 	u8 data_id;
 
-	WARN_ON(!dsi_bus_is_locked());
+	WARN_ON(!dsi_bus_is_locked(dsidev));
 
-	if (dsi.debug_write)
+	if (dsi->debug_write)
 		DSSDBG("dsi_vc_send_short(ch%d, dt %#x, b1 %#x, b2 %#x)\n",
 				channel,
 				data_type, data & 0xff, (data >> 8) & 0xff);
 
-	dsi_vc_config_l4(channel);
+	dsi_vc_config_l4(dsidev, channel);
 
-	if (FLD_GET(dsi_read_reg(DSI_VC_CTRL(channel)), 16, 16)) {
+	if (FLD_GET(dsi_read_reg(dsidev, DSI_VC_CTRL(channel)), 16, 16)) {
 		DSSERR("ERROR FIFO FULL, aborting transfer\n");
 		return -EINVAL;
 	}
 
-	data_id = data_type | channel << 6;
+	data_id = data_type | dsi->vc[channel].vc_id << 6;
 
 	r = (data_id << 0) | (data << 8) | (ecc << 24);
 
-	dsi_write_reg(DSI_VC_SHORT_PACKET_HEADER(channel), r);
+	dsi_write_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel), r);
 
 	return 0;
 }
 
-int dsi_vc_send_null(int channel)
+int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	u8 nullpkg[] = {0, 0, 0, 0};
-	return dsi_vc_send_long(channel, DSI_DT_NULL_PACKET, nullpkg, 4, 0);
+
+	return dsi_vc_send_long(dsidev, channel, DSI_DT_NULL_PACKET, nullpkg,
+		4, 0);
 }
 EXPORT_SYMBOL(dsi_vc_send_null);
 
-int dsi_vc_dcs_write_nosync(int channel, u8 *data, int len)
+int dsi_vc_dcs_write_nosync(struct omap_dss_device *dssdev, int channel,
+		u8 *data, int len)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	int r;
 
 	BUG_ON(len == 0);
 
 	if (len == 1) {
-		r = dsi_vc_send_short(channel, DSI_DT_DCS_SHORT_WRITE_0,
+		r = dsi_vc_send_short(dsidev, channel, DSI_DT_DCS_SHORT_WRITE_0,
 				data[0], 0);
 	} else if (len == 2) {
-		r = dsi_vc_send_short(channel, DSI_DT_DCS_SHORT_WRITE_1,
+		r = dsi_vc_send_short(dsidev, channel, DSI_DT_DCS_SHORT_WRITE_1,
 				data[0] | (data[1] << 8), 0);
 	} else {
 		/* 0x39 = DCS Long Write */
-		r = dsi_vc_send_long(channel, DSI_DT_DCS_LONG_WRITE,
+		r = dsi_vc_send_long(dsidev, channel, DSI_DT_DCS_LONG_WRITE,
 				data, len, 0);
 	}
 
@@ -2102,21 +3116,24 @@ int dsi_vc_dcs_write_nosync(int channel, u8 *data, int len)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_nosync);
 
-int dsi_vc_dcs_write(int channel, u8 *data, int len)
+int dsi_vc_dcs_write(struct omap_dss_device *dssdev, int channel, u8 *data,
+		int len)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	int r;
 
-	r = dsi_vc_dcs_write_nosync(channel, data, len);
+	r = dsi_vc_dcs_write_nosync(dssdev, channel, data, len);
 	if (r)
 		goto err;
 
-	r = dsi_vc_send_bta_sync(channel);
+	r = dsi_vc_send_bta_sync(dssdev, channel);
 	if (r)
 		goto err;
 
-	if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {	/* RX_FIFO_NOT_EMPTY */
+	/* RX_FIFO_NOT_EMPTY */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
 		DSSERR("rx fifo not empty after write, dumping data:\n");
-		dsi_vc_flush_receive_data(channel);
+		dsi_vc_flush_receive_data(dsidev, channel);
 		r = -EIO;
 		goto err;
 	}
@@ -2129,47 +3146,51 @@ err:
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write);
 
-int dsi_vc_dcs_write_0(int channel, u8 dcs_cmd)
+int dsi_vc_dcs_write_0(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd)
 {
-	return dsi_vc_dcs_write(channel, &dcs_cmd, 1);
+	return dsi_vc_dcs_write(dssdev, channel, &dcs_cmd, 1);
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_0);
 
-int dsi_vc_dcs_write_1(int channel, u8 dcs_cmd, u8 param)
+int dsi_vc_dcs_write_1(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+		u8 param)
 {
 	u8 buf[2];
 	buf[0] = dcs_cmd;
 	buf[1] = param;
-	return dsi_vc_dcs_write(channel, buf, 2);
+	return dsi_vc_dcs_write(dssdev, channel, buf, 2);
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_1);
 
-int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
+int dsi_vc_dcs_read(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+		u8 *buf, int buflen)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 val;
 	u8 dt;
 	int r;
 
-	if (dsi.debug_read)
+	if (dsi->debug_read)
 		DSSDBG("dsi_vc_dcs_read(ch%d, dcs_cmd %x)\n", channel, dcs_cmd);
 
-	r = dsi_vc_send_short(channel, DSI_DT_DCS_READ, dcs_cmd, 0);
+	r = dsi_vc_send_short(dsidev, channel, DSI_DT_DCS_READ, dcs_cmd, 0);
 	if (r)
 		goto err;
 
-	r = dsi_vc_send_bta_sync(channel);
+	r = dsi_vc_send_bta_sync(dssdev, channel);
 	if (r)
 		goto err;
 
 	/* RX_FIFO_NOT_EMPTY */
-	if (REG_GET(DSI_VC_CTRL(channel), 20, 20) == 0) {
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20) == 0) {
 		DSSERR("RX fifo empty when trying to read.\n");
 		r = -EIO;
 		goto err;
 	}
 
-	val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
-	if (dsi.debug_read)
+	val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+	if (dsi->debug_read)
 		DSSDBG("\theader: %08x\n", val);
 	dt = FLD_GET(val, 5, 0);
 	if (dt == DSI_DT_RX_ACK_WITH_ERR) {
@@ -2180,7 +3201,7 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
 
 	} else if (dt == DSI_DT_RX_SHORT_READ_1) {
 		u8 data = FLD_GET(val, 15, 8);
-		if (dsi.debug_read)
+		if (dsi->debug_read)
 			DSSDBG("\tDCS short response, 1 byte: %02x\n", data);
 
 		if (buflen < 1) {
@@ -2193,7 +3214,7 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
 		return 1;
 	} else if (dt == DSI_DT_RX_SHORT_READ_2) {
 		u16 data = FLD_GET(val, 23, 8);
-		if (dsi.debug_read)
+		if (dsi->debug_read)
 			DSSDBG("\tDCS short response, 2 byte: %04x\n", data);
 
 		if (buflen < 2) {
@@ -2208,7 +3229,7 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
 	} else if (dt == DSI_DT_RX_DCS_LONG_READ) {
 		int w;
 		int len = FLD_GET(val, 23, 8);
-		if (dsi.debug_read)
+		if (dsi->debug_read)
 			DSSDBG("\tDCS long response, len %d\n", len);
 
 		if (len > buflen) {
@@ -2219,8 +3240,9 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
 		/* two byte checksum ends the packet, not included in len */
 		for (w = 0; w < len + 2;) {
 			int b;
-			val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
-			if (dsi.debug_read)
+			val = dsi_read_reg(dsidev,
+				DSI_VC_SHORT_PACKET_HEADER(channel));
+			if (dsi->debug_read)
 				DSSDBG("\t\t%02x %02x %02x %02x\n",
 						(val >> 0) & 0xff,
 						(val >> 8) & 0xff,
@@ -2251,11 +3273,12 @@ err:
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read);
 
-int dsi_vc_dcs_read_1(int channel, u8 dcs_cmd, u8 *data)
+int dsi_vc_dcs_read_1(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+		u8 *data)
 {
 	int r;
 
-	r = dsi_vc_dcs_read(channel, dcs_cmd, data, 1);
+	r = dsi_vc_dcs_read(dssdev, channel, dcs_cmd, data, 1);
 
 	if (r < 0)
 		return r;
@@ -2267,12 +3290,13 @@ int dsi_vc_dcs_read_1(int channel, u8 dcs_cmd, u8 *data)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_1);
 
-int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u8 *data1, u8 *data2)
+int dsi_vc_dcs_read_2(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+		u8 *data1, u8 *data2)
 {
 	u8 buf[2];
 	int r;
 
-	r = dsi_vc_dcs_read(channel, dcs_cmd, buf, 2);
+	r = dsi_vc_dcs_read(dssdev, channel, dcs_cmd, buf, 2);
 
 	if (r < 0)
 		return r;
@@ -2287,14 +3311,98 @@ int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u8 *data1, u8 *data2)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_2);
 
-int dsi_vc_set_max_rx_packet_size(int channel, u16 len)
+int dsi_vc_set_max_rx_packet_size(struct omap_dss_device *dssdev, int channel,
+		u16 len)
 {
-	return dsi_vc_send_short(channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_send_short(dsidev, channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
 			len, 0);
 }
 EXPORT_SYMBOL(dsi_vc_set_max_rx_packet_size);
 
-static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
+static int dsi_enter_ulps(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	DECLARE_COMPLETION_ONSTACK(completion);
+	int r;
+
+	DSSDBGF();
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	WARN_ON(dsi->ulps_enabled);
+
+	if (dsi->ulps_enabled)
+		return 0;
+
+	if (REG_GET(dsidev, DSI_CLK_CTRL, 13, 13)) {
+		DSSERR("DDR_CLK_ALWAYS_ON enabled when entering ULPS\n");
+		return -EIO;
+	}
+
+	dsi_sync_vc(dsidev, 0);
+	dsi_sync_vc(dsidev, 1);
+	dsi_sync_vc(dsidev, 2);
+	dsi_sync_vc(dsidev, 3);
+
+	dsi_force_tx_stop_mode_io(dsidev);
+
+	dsi_vc_enable(dsidev, 0, false);
+	dsi_vc_enable(dsidev, 1, false);
+	dsi_vc_enable(dsidev, 2, false);
+	dsi_vc_enable(dsidev, 3, false);
+
+	if (REG_GET(dsidev, DSI_COMPLEXIO_CFG2, 16, 16)) {	/* HS_BUSY */
+		DSSERR("HS busy when enabling ULPS\n");
+		return -EIO;
+	}
+
+	if (REG_GET(dsidev, DSI_COMPLEXIO_CFG2, 17, 17)) {	/* LP_BUSY */
+		DSSERR("LP busy when enabling ULPS\n");
+		return -EIO;
+	}
+
+	r = dsi_register_isr_cio(dsidev, dsi_completion_handler, &completion,
+			DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+	if (r)
+		return r;
+
+	/* Assert TxRequestEsc for data lanes and TxUlpsClk for clk lane */
+	/* LANEx_ULPS_SIG2 */
+	REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, (1 << 0) | (1 << 1) | (1 << 2),
+		7, 5);
+
+	if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(1000)) == 0) {
+		DSSERR("ULPS enable timeout\n");
+		r = -EIO;
+		goto err;
+	}
+
+	dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+			DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+
+	/* Reset LANEx_ULPS_SIG2 */
+	REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, (0 << 0) | (0 << 1) | (0 << 2),
+		7, 5);
+
+	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ULPS);
+
+	dsi_if_enable(dsidev, false);
+
+	dsi->ulps_enabled = true;
+
+	return 0;
+
+err:
+	dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+			DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+	return r;
+}
+
+static void dsi_set_lp_rx_timeout(struct platform_device *dsidev,
+		unsigned ticks, bool x4, bool x16)
 {
 	unsigned long fck;
 	unsigned long total_ticks;
@@ -2303,14 +3411,14 @@ static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
 	BUG_ON(ticks > 0x1fff);
 
 	/* ticks in DSI_FCK */
-	fck = dsi_fclk_rate();
+	fck = dsi_fclk_rate(dsidev);
 
-	r = dsi_read_reg(DSI_TIMING2);
+	r = dsi_read_reg(dsidev, DSI_TIMING2);
 	r = FLD_MOD(r, 1, 15, 15);	/* LP_RX_TO */
 	r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);	/* LP_RX_TO_X16 */
 	r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);	/* LP_RX_TO_X4 */
 	r = FLD_MOD(r, ticks, 12, 0);	/* LP_RX_COUNTER */
-	dsi_write_reg(DSI_TIMING2, r);
+	dsi_write_reg(dsidev, DSI_TIMING2, r);
 
 	total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
 
@@ -2320,7 +3428,8 @@ static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
 			(total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
+static void dsi_set_ta_timeout(struct platform_device *dsidev, unsigned ticks,
+		bool x8, bool x16)
 {
 	unsigned long fck;
 	unsigned long total_ticks;
@@ -2329,14 +3438,14 @@ static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
 	BUG_ON(ticks > 0x1fff);
 
 	/* ticks in DSI_FCK */
-	fck = dsi_fclk_rate();
+	fck = dsi_fclk_rate(dsidev);
 
-	r = dsi_read_reg(DSI_TIMING1);
+	r = dsi_read_reg(dsidev, DSI_TIMING1);
 	r = FLD_MOD(r, 1, 31, 31);	/* TA_TO */
 	r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);	/* TA_TO_X16 */
 	r = FLD_MOD(r, x8 ? 1 : 0, 29, 29);	/* TA_TO_X8 */
 	r = FLD_MOD(r, ticks, 28, 16);	/* TA_TO_COUNTER */
-	dsi_write_reg(DSI_TIMING1, r);
+	dsi_write_reg(dsidev, DSI_TIMING1, r);
 
 	total_ticks = ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1);
 
@@ -2346,7 +3455,8 @@ static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
 			(total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
+static void dsi_set_stop_state_counter(struct platform_device *dsidev,
+		unsigned ticks, bool x4, bool x16)
 {
 	unsigned long fck;
 	unsigned long total_ticks;
@@ -2355,14 +3465,14 @@ static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
 	BUG_ON(ticks > 0x1fff);
 
 	/* ticks in DSI_FCK */
-	fck = dsi_fclk_rate();
+	fck = dsi_fclk_rate(dsidev);
 
-	r = dsi_read_reg(DSI_TIMING1);
+	r = dsi_read_reg(dsidev, DSI_TIMING1);
 	r = FLD_MOD(r, 1, 15, 15);	/* FORCE_TX_STOP_MODE_IO */
 	r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);	/* STOP_STATE_X16_IO */
 	r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);	/* STOP_STATE_X4_IO */
 	r = FLD_MOD(r, ticks, 12, 0);	/* STOP_STATE_COUNTER_IO */
-	dsi_write_reg(DSI_TIMING1, r);
+	dsi_write_reg(dsidev, DSI_TIMING1, r);
 
 	total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
 
@@ -2372,7 +3482,8 @@ static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
 			(total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
+static void dsi_set_hs_tx_timeout(struct platform_device *dsidev,
+		unsigned ticks, bool x4, bool x16)
 {
 	unsigned long fck;
 	unsigned long total_ticks;
@@ -2381,14 +3492,14 @@ static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
 	BUG_ON(ticks > 0x1fff);
 
 	/* ticks in TxByteClkHS */
-	fck = dsi_get_txbyteclkhs();
+	fck = dsi_get_txbyteclkhs(dsidev);
 
-	r = dsi_read_reg(DSI_TIMING2);
+	r = dsi_read_reg(dsidev, DSI_TIMING2);
 	r = FLD_MOD(r, 1, 31, 31);	/* HS_TX_TO */
 	r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);	/* HS_TX_TO_X16 */
 	r = FLD_MOD(r, x4 ? 1 : 0, 29, 29);	/* HS_TX_TO_X8 (4 really) */
 	r = FLD_MOD(r, ticks, 28, 16);	/* HS_TX_TO_COUNTER */
-	dsi_write_reg(DSI_TIMING2, r);
+	dsi_write_reg(dsidev, DSI_TIMING2, r);
 
 	total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
 
@@ -2399,24 +3510,25 @@ static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
 }
 static int dsi_proto_config(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	u32 r;
 	int buswidth = 0;
 
-	dsi_config_tx_fifo(DSI_FIFO_SIZE_32,
+	dsi_config_tx_fifo(dsidev, DSI_FIFO_SIZE_32,
 			DSI_FIFO_SIZE_32,
 			DSI_FIFO_SIZE_32,
 			DSI_FIFO_SIZE_32);
 
-	dsi_config_rx_fifo(DSI_FIFO_SIZE_32,
+	dsi_config_rx_fifo(dsidev, DSI_FIFO_SIZE_32,
 			DSI_FIFO_SIZE_32,
 			DSI_FIFO_SIZE_32,
 			DSI_FIFO_SIZE_32);
 
 	/* XXX what values for the timeouts? */
-	dsi_set_stop_state_counter(0x1000, false, false);
-	dsi_set_ta_timeout(0x1fff, true, true);
-	dsi_set_lp_rx_timeout(0x1fff, true, true);
-	dsi_set_hs_tx_timeout(0x1fff, true, true);
+	dsi_set_stop_state_counter(dsidev, 0x1000, false, false);
+	dsi_set_ta_timeout(dsidev, 0x1fff, true, true);
+	dsi_set_lp_rx_timeout(dsidev, 0x1fff, true, true);
+	dsi_set_hs_tx_timeout(dsidev, 0x1fff, true, true);
 
 	switch (dssdev->ctrl.pixel_size) {
 	case 16:
@@ -2432,7 +3544,7 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
 		BUG();
 	}
 
-	r = dsi_read_reg(DSI_CTRL);
+	r = dsi_read_reg(dsidev, DSI_CTRL);
 	r = FLD_MOD(r, 1, 1, 1);	/* CS_RX_EN */
 	r = FLD_MOD(r, 1, 2, 2);	/* ECC_RX_EN */
 	r = FLD_MOD(r, 1, 3, 3);	/* TX_FIFO_ARBITRATION */
@@ -2442,21 +3554,25 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
 	r = FLD_MOD(r, 2, 13, 12);	/* LINE_BUFFER, 2 lines */
 	r = FLD_MOD(r, 1, 14, 14);	/* TRIGGER_RESET_MODE */
 	r = FLD_MOD(r, 1, 19, 19);	/* EOT_ENABLE */
-	r = FLD_MOD(r, 1, 24, 24);	/* DCS_CMD_ENABLE */
-	r = FLD_MOD(r, 0, 25, 25);	/* DCS_CMD_CODE, 1=start, 0=continue */
+	if (!dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC)) {
+		r = FLD_MOD(r, 1, 24, 24);	/* DCS_CMD_ENABLE */
+		/* DCS_CMD_CODE, 1=start, 0=continue */
+		r = FLD_MOD(r, 0, 25, 25);
+	}
 
-	dsi_write_reg(DSI_CTRL, r);
+	dsi_write_reg(dsidev, DSI_CTRL, r);
 
-	dsi_vc_initial_config(0);
-	dsi_vc_initial_config(1);
-	dsi_vc_initial_config(2);
-	dsi_vc_initial_config(3);
+	dsi_vc_initial_config(dsidev, 0);
+	dsi_vc_initial_config(dsidev, 1);
+	dsi_vc_initial_config(dsidev, 2);
+	dsi_vc_initial_config(dsidev, 3);
 
 	return 0;
 }
 
 static void dsi_proto_timings(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	unsigned tlpx, tclk_zero, tclk_prepare, tclk_trail;
 	unsigned tclk_pre, tclk_post;
 	unsigned ths_prepare, ths_prepare_ths_zero, ths_zero;
@@ -2466,32 +3582,27 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 	unsigned ths_eot;
 	u32 r;
 
-	r = dsi_read_reg(DSI_DSIPHY_CFG0);
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
 	ths_prepare = FLD_GET(r, 31, 24);
 	ths_prepare_ths_zero = FLD_GET(r, 23, 16);
 	ths_zero = ths_prepare_ths_zero - ths_prepare;
 	ths_trail = FLD_GET(r, 15, 8);
 	ths_exit = FLD_GET(r, 7, 0);
 
-	r = dsi_read_reg(DSI_DSIPHY_CFG1);
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
 	tlpx = FLD_GET(r, 22, 16) * 2;
 	tclk_trail = FLD_GET(r, 15, 8);
 	tclk_zero = FLD_GET(r, 7, 0);
 
-	r = dsi_read_reg(DSI_DSIPHY_CFG2);
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
 	tclk_prepare = FLD_GET(r, 7, 0);
 
 	/* min 8*UI */
 	tclk_pre = 20;
 	/* min 60ns + 52*UI */
-	tclk_post = ns2ddr(60) + 26;
+	tclk_post = ns2ddr(dsidev, 60) + 26;
 
-	/* ths_eot is 2 for 2 datalanes and 4 for 1 datalane */
-	if (dssdev->phy.dsi.data1_lane != 0 &&
-			dssdev->phy.dsi.data2_lane != 0)
-		ths_eot = 2;
-	else
-		ths_eot = 4;
+	ths_eot = DIV_ROUND_UP(4, dsi_get_num_data_lanes_dssdev(dssdev));
 
 	ddr_clk_pre = DIV_ROUND_UP(tclk_pre + tlpx + tclk_zero + tclk_prepare,
 			4);
@@ -2500,10 +3611,10 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 	BUG_ON(ddr_clk_pre == 0 || ddr_clk_pre > 255);
 	BUG_ON(ddr_clk_post == 0 || ddr_clk_post > 255);
 
-	r = dsi_read_reg(DSI_CLK_TIMING);
+	r = dsi_read_reg(dsidev, DSI_CLK_TIMING);
 	r = FLD_MOD(r, ddr_clk_pre, 15, 8);
 	r = FLD_MOD(r, ddr_clk_post, 7, 0);
-	dsi_write_reg(DSI_CLK_TIMING, r);
+	dsi_write_reg(dsidev, DSI_CLK_TIMING, r);
 
 	DSSDBG("ddr_clk_pre %u, ddr_clk_post %u\n",
 			ddr_clk_pre,
@@ -2517,7 +3628,7 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 
 	r = FLD_VAL(enter_hs_mode_lat, 31, 16) |
 		FLD_VAL(exit_hs_mode_lat, 15, 0);
-	dsi_write_reg(DSI_VM_TIMING7, r);
+	dsi_write_reg(dsidev, DSI_VM_TIMING7, r);
 
 	DSSDBG("enter_hs_mode_lat %u, exit_hs_mode_lat %u\n",
 			enter_hs_mode_lat, exit_hs_mode_lat);
@@ -2527,25 +3638,27 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 #define DSI_DECL_VARS \
 	int __dsi_cb = 0; u32 __dsi_cv = 0;
 
-#define DSI_FLUSH(ch) \
+#define DSI_FLUSH(dsidev, ch) \
 	if (__dsi_cb > 0) { \
 		/*DSSDBG("sending long packet %#010x\n", __dsi_cv);*/ \
-		dsi_write_reg(DSI_VC_LONG_PACKET_PAYLOAD(ch), __dsi_cv); \
+		dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_PAYLOAD(ch), __dsi_cv); \
 		__dsi_cb = __dsi_cv = 0; \
 	}
 
-#define DSI_PUSH(ch, data) \
+#define DSI_PUSH(dsidev, ch, data) \
 	do { \
 		__dsi_cv |= (data) << (__dsi_cb * 8); \
 		/*DSSDBG("cv = %#010x, cb = %d\n", __dsi_cv, __dsi_cb);*/ \
 		if (++__dsi_cb > 3) \
-			DSI_FLUSH(ch); \
+			DSI_FLUSH(dsidev, ch); \
 	} while (0)
 
 static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 			int x, int y, int w, int h)
 {
 	/* Note: supports only 24bit colors in 32bit container */
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int first = 1;
 	int fifo_stalls = 0;
 	int max_dsi_packet_size;
@@ -2584,7 +3697,7 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 	 * in fifo */
 
 	/* When using CPU, max long packet size is TX buffer size */
-	max_dsi_packet_size = dsi.vc[0].fifo_size * 32 * 4;
+	max_dsi_packet_size = dsi->vc[0].fifo_size * 32 * 4;
 
 	/* we seem to get better perf if we divide the tx fifo to half,
 	   and while the other half is being sent, we fill the other half
@@ -2613,35 +3726,36 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 #if 1
 		/* using fifo not empty */
 		/* TX_FIFO_NOT_EMPTY */
-		while (FLD_GET(dsi_read_reg(DSI_VC_CTRL(0)), 5, 5)) {
+		while (FLD_GET(dsi_read_reg(dsidev, DSI_VC_CTRL(0)), 5, 5)) {
 			fifo_stalls++;
 			if (fifo_stalls > 0xfffff) {
 				DSSERR("fifo stalls overflow, pixels left %d\n",
 						pixels_left);
-				dsi_if_enable(0);
+				dsi_if_enable(dsidev, 0);
 				return -EIO;
 			}
 			udelay(1);
 		}
 #elif 1
 		/* using fifo emptiness */
-		while ((REG_GET(DSI_TX_FIFO_VC_EMPTINESS, 7, 0)+1)*4 <
+		while ((REG_GET(dsidev, DSI_TX_FIFO_VC_EMPTINESS, 7, 0)+1)*4 <
 				max_dsi_packet_size) {
 			fifo_stalls++;
 			if (fifo_stalls > 0xfffff) {
 				DSSERR("fifo stalls overflow, pixels left %d\n",
 					       pixels_left);
-				dsi_if_enable(0);
+				dsi_if_enable(dsidev, 0);
 				return -EIO;
 			}
 		}
 #else
-		while ((REG_GET(DSI_TX_FIFO_VC_EMPTINESS, 7, 0)+1)*4 == 0) {
+		while ((REG_GET(dsidev, DSI_TX_FIFO_VC_EMPTINESS,
+				7, 0) + 1) * 4 == 0) {
 			fifo_stalls++;
 			if (fifo_stalls > 0xfffff) {
 				DSSERR("fifo stalls overflow, pixels left %d\n",
 					       pixels_left);
-				dsi_if_enable(0);
+				dsi_if_enable(dsidev, 0);
 				return -EIO;
 			}
 		}
@@ -2650,17 +3764,17 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 
 		pixels_left -= pixels;
 
-		dsi_vc_write_long_header(0, DSI_DT_DCS_LONG_WRITE,
+		dsi_vc_write_long_header(dsidev, 0, DSI_DT_DCS_LONG_WRITE,
 				1 + pixels * bytespp, 0);
 
-		DSI_PUSH(0, dcs_cmd);
+		DSI_PUSH(dsidev, 0, dcs_cmd);
 
 		while (pixels-- > 0) {
 			u32 pix = __raw_readl(data++);
 
-			DSI_PUSH(0, (pix >> 16) & 0xff);
-			DSI_PUSH(0, (pix >> 8) & 0xff);
-			DSI_PUSH(0, (pix >> 0) & 0xff);
+			DSI_PUSH(dsidev, 0, (pix >> 16) & 0xff);
+			DSI_PUSH(dsidev, 0, (pix >> 8) & 0xff);
+			DSI_PUSH(dsidev, 0, (pix >> 0) & 0xff);
 
 			current_x++;
 			if (current_x == x+w) {
@@ -2669,7 +3783,7 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 			}
 		}
 
-		DSI_FLUSH(0);
+		DSI_FLUSH(dsidev, 0);
 	}
 
 	return 0;
@@ -2678,6 +3792,8 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 		u16 x, u16 y, u16 w, u16 h)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned bytespp;
 	unsigned bytespl;
 	unsigned bytespf;
@@ -2686,16 +3802,13 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 	unsigned packet_len;
 	u32 l;
 	int r;
-	const unsigned channel = dsi.update_channel;
-	/* line buffer is 1024 x 24bits */
-	/* XXX: for some reason using full buffer size causes considerable TX
-	 * slowdown with update sizes that fill the whole buffer */
-	const unsigned line_buf_size = 1023 * 3;
+	const unsigned channel = dsi->update_channel;
+	const unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
 
 	DSSDBG("dsi_update_screen_dispc(%d,%d %dx%d)\n",
 			x, y, w, h);
 
-	dsi_vc_config_vp(channel);
+	dsi_vc_config_vp(dsidev, channel);
 
 	bytespp	= dssdev->ctrl.pixel_size / 8;
 	bytespl = w * bytespp;
@@ -2716,15 +3829,16 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 		total_len += (bytespf % packet_payload) + 1;
 
 	l = FLD_VAL(total_len, 23, 0); /* TE_SIZE */
-	dsi_write_reg(DSI_VC_TE(channel), l);
+	dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
 
-	dsi_vc_write_long_header(channel, DSI_DT_DCS_LONG_WRITE, packet_len, 0);
+	dsi_vc_write_long_header(dsidev, channel, DSI_DT_DCS_LONG_WRITE,
+		packet_len, 0);
 
-	if (dsi.te_enabled)
+	if (dsi->te_enabled)
 		l = FLD_MOD(l, 1, 30, 30); /* TE_EN */
 	else
 		l = FLD_MOD(l, 1, 31, 31); /* TE_START */
-	dsi_write_reg(DSI_VC_TE(channel), l);
+	dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
 
 	/* We put SIDLEMODE to no-idle for the duration of the transfer,
 	 * because DSS interrupts are not capable of waking up the CPU and the
@@ -2734,23 +3848,23 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 	 */
 	dispc_disable_sidle();
 
-	dsi_perf_mark_start();
+	dsi_perf_mark_start(dsidev);
 
-	r = queue_delayed_work(dsi.workqueue, &dsi.framedone_timeout_work,
-			msecs_to_jiffies(250));
+	r = schedule_delayed_work(&dsi->framedone_timeout_work,
+		msecs_to_jiffies(250));
 	BUG_ON(r == 0);
 
 	dss_start_update(dssdev);
 
-	if (dsi.te_enabled) {
+	if (dsi->te_enabled) {
 		/* disable LP_RX_TO, so that we can receive TE.  Time to wait
 		 * for TE is longer than the timer allows */
-		REG_FLD_MOD(DSI_TIMING2, 0, 15, 15); /* LP_RX_TO */
+		REG_FLD_MOD(dsidev, DSI_TIMING2, 0, 15, 15); /* LP_RX_TO */
 
-		dsi_vc_send_bta(channel);
+		dsi_vc_send_bta(dsidev, channel);
 
 #ifdef DSI_CATCH_MISSING_TE
-		mod_timer(&dsi.te_timer, jiffies + msecs_to_jiffies(250));
+		mod_timer(&dsi->te_timer, jiffies + msecs_to_jiffies(250));
 #endif
 	}
 }
@@ -2762,40 +3876,28 @@ static void dsi_te_timeout(unsigned long arg)
 }
 #endif
 
-static void dsi_handle_framedone(int error)
+static void dsi_handle_framedone(struct platform_device *dsidev, int error)
 {
-	const int channel = dsi.update_channel;
-
-	cancel_delayed_work(&dsi.framedone_timeout_work);
-
-	dsi_vc_disable_bta_irq(channel);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	/* SIDLEMODE back to smart-idle */
 	dispc_enable_sidle();
 
-	dsi.bta_callback = NULL;
-
-	if (dsi.te_enabled) {
+	if (dsi->te_enabled) {
 		/* enable LP_RX_TO again after the TE */
-		REG_FLD_MOD(DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
-	}
-
-	/* RX_FIFO_NOT_EMPTY */
-	if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
-		DSSERR("Received error during frame transfer:\n");
-		dsi_vc_flush_receive_data(channel);
-		if (!error)
-			error = -EIO;
+		REG_FLD_MOD(dsidev, DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
 	}
 
-	dsi.framedone_callback(error, dsi.framedone_data);
+	dsi->framedone_callback(error, dsi->framedone_data);
 
 	if (!error)
-		dsi_perf_show("DISPC");
+		dsi_perf_show(dsidev, "DISPC");
 }
 
 static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 {
+	struct dsi_data *dsi = container_of(work, struct dsi_data,
+			framedone_timeout_work.work);
 	/* XXX While extremely unlikely, we could get FRAMEDONE interrupt after
 	 * 250ms which would conflict with this timeout work. What should be
 	 * done is first cancel the transfer on the HW, and then cancel the
@@ -2805,66 +3907,34 @@ static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 
 	DSSERR("Framedone not received for 250ms!\n");
 
-	dsi_handle_framedone(-ETIMEDOUT);
-}
-
-static void dsi_framedone_bta_callback(void)
-{
-	dsi_handle_framedone(0);
-
-#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
-	dispc_fake_vsync_irq();
-#endif
+	dsi_handle_framedone(dsi->pdev, -ETIMEDOUT);
 }
 
 static void dsi_framedone_irq_callback(void *data, u32 mask)
 {
-	const int channel = dsi.update_channel;
-	int r;
+	struct omap_dss_device *dssdev = (struct omap_dss_device *) data;
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	/* Note: We get FRAMEDONE when DISPC has finished sending pixels and
 	 * turns itself off. However, DSI still has the pixels in its buffers,
 	 * and is sending the data.
 	 */
 
-	if (dsi.te_enabled) {
-		/* enable LP_RX_TO again after the TE */
-		REG_FLD_MOD(DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
-	}
-
-	/* Send BTA after the frame. We need this for the TE to work, as TE
-	 * trigger is only sent for BTAs without preceding packet. Thus we need
-	 * to BTA after the pixel packets so that next BTA will cause TE
-	 * trigger.
-	 *
-	 * This is not needed when TE is not in use, but we do it anyway to
-	 * make sure that the transfer has been completed. It would be more
-	 * optimal, but more complex, to wait only just before starting next
-	 * transfer.
-	 *
-	 * Also, as there's no interrupt telling when the transfer has been
-	 * done and the channel could be reconfigured, the only way is to
-	 * busyloop until TE_SIZE is zero. With BTA we can do this
-	 * asynchronously.
-	 * */
-
-	dsi.bta_callback = dsi_framedone_bta_callback;
+	__cancel_delayed_work(&dsi->framedone_timeout_work);
 
-	barrier();
+	dsi_handle_framedone(dsidev, 0);
 
-	dsi_vc_enable_bta_irq(channel);
-
-	r = dsi_vc_send_bta(channel);
-	if (r) {
-		DSSERR("BTA after framedone failed\n");
-		dsi_handle_framedone(-EIO);
-	}
+#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
+	dispc_fake_vsync_irq();
+#endif
 }
 
 int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
 				    u16 *x, u16 *y, u16 *w, u16 *h,
 				    bool enlarge_update_area)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	u16 dw, dh;
 
 	dssdev->driver->get_resolution(dssdev, &dw, &dh);
@@ -2884,7 +3954,7 @@ int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
 	if (*w == 0 || *h == 0)
 		return -EINVAL;
 
-	dsi_perf_mark_setup();
+	dsi_perf_mark_setup(dsidev);
 
 	if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
 		dss_setup_partial_planes(dssdev, x, y, w, h,
@@ -2901,7 +3971,10 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 		u16 x, u16 y, u16 w, u16 h,
 		void (*callback)(int, void *), void *data)
 {
-	dsi.update_channel = channel;
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	dsi->update_channel = channel;
 
 	/* OMAP DSS cannot send updates of odd widths.
 	 * omap_dsi_prepare_update() makes the widths even, but add a BUG_ON
@@ -2910,14 +3983,14 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 	BUG_ON(x % 2 == 1);
 
 	if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-		dsi.framedone_callback = callback;
-		dsi.framedone_data = data;
+		dsi->framedone_callback = callback;
+		dsi->framedone_data = data;
 
-		dsi.update_region.x = x;
-		dsi.update_region.y = y;
-		dsi.update_region.w = w;
-		dsi.update_region.h = h;
-		dsi.update_region.device = dssdev;
+		dsi->update_region.x = x;
+		dsi->update_region.y = y;
+		dsi->update_region.w = w;
+		dsi->update_region.h = h;
+		dsi->update_region.device = dssdev;
 
 		dsi_update_screen_dispc(dssdev, x, y, w, h);
 	} else {
@@ -2927,7 +4000,7 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 		if (r)
 			return r;
 
-		dsi_perf_show("L4");
+		dsi_perf_show(dsidev, "L4");
 		callback(0, data);
 	}
 
@@ -2940,9 +4013,13 @@ EXPORT_SYMBOL(omap_dsi_update);
 static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 {
 	int r;
+	u32 irq;
+
+	irq = dssdev->manager->id == OMAP_DSS_CHANNEL_LCD ?
+		DISPC_IRQ_FRAMEDONE : DISPC_IRQ_FRAMEDONE2;
 
-	r = omap_dispc_register_isr(dsi_framedone_irq_callback, NULL,
-			DISPC_IRQ_FRAMEDONE);
+	r = omap_dispc_register_isr(dsi_framedone_irq_callback, (void *) dssdev,
+			irq);
 	if (r) {
 		DSSERR("can't get FRAMEDONE irq\n");
 		return r;
@@ -2975,28 +4052,34 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 
 static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
 {
-	omap_dispc_unregister_isr(dsi_framedone_irq_callback, NULL,
-			DISPC_IRQ_FRAMEDONE);
+	u32 irq;
+
+	irq = dssdev->manager->id == OMAP_DSS_CHANNEL_LCD ?
+		DISPC_IRQ_FRAMEDONE : DISPC_IRQ_FRAMEDONE2;
+
+	omap_dispc_unregister_isr(dsi_framedone_irq_callback, (void *) dssdev,
+			irq);
 }
 
 static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_clock_info cinfo;
 	int r;
 
-	/* we always use DSS2_FCK as input clock */
-	cinfo.use_dss2_fck = true;
-	cinfo.regn  = dssdev->phy.dsi.div.regn;
-	cinfo.regm  = dssdev->phy.dsi.div.regm;
-	cinfo.regm3 = dssdev->phy.dsi.div.regm3;
-	cinfo.regm4 = dssdev->phy.dsi.div.regm4;
+	/* we always use DSS_CLK_SYSCK as input clock */
+	cinfo.use_sys_clk = true;
+	cinfo.regn  = dssdev->clocks.dsi.regn;
+	cinfo.regm  = dssdev->clocks.dsi.regm;
+	cinfo.regm_dispc = dssdev->clocks.dsi.regm_dispc;
+	cinfo.regm_dsi = dssdev->clocks.dsi.regm_dsi;
 	r = dsi_calc_clock_rates(dssdev, &cinfo);
 	if (r) {
 		DSSERR("Failed to calc dsi clocks\n");
 		return r;
 	}
 
-	r = dsi_pll_set_clock_div(&cinfo);
+	r = dsi_pll_set_clock_div(dsidev, &cinfo);
 	if (r) {
 		DSSERR("Failed to set dsi clocks\n");
 		return r;
@@ -3007,14 +4090,15 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 
 static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dispc_clock_info dispc_cinfo;
 	int r;
 	unsigned long long fck;
 
-	fck = dsi_get_dsi1_pll_rate();
+	fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
 
-	dispc_cinfo.lck_div = dssdev->phy.dsi.div.lck_div;
-	dispc_cinfo.pck_div = dssdev->phy.dsi.div.pck_div;
+	dispc_cinfo.lck_div = dssdev->clocks.dispc.channel.lck_div;
+	dispc_cinfo.pck_div = dssdev->clocks.dispc.channel.pck_div;
 
 	r = dispc_calc_clock_rates(fck, &dispc_cinfo);
 	if (r) {
@@ -3033,11 +4117,11 @@ static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
 
 static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	int dsi_module = dsi_get_dsidev_id(dsidev);
 	int r;
 
-	_dsi_print_reset_status();
-
-	r = dsi_pll_init(dssdev, true, true);
+	r = dsi_pll_init(dsidev, true, true);
 	if (r)
 		goto err0;
 
@@ -3045,8 +4129,10 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 	if (r)
 		goto err1;
 
-	dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
-	dss_select_dsi_clk_source(DSS_SRC_DSI2_PLL_FCLK);
+	dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
+	dss_select_dsi_clk_source(dsi_module, dssdev->clocks.dsi.dsi_fclk_src);
+	dss_select_lcd_clk_source(dssdev->manager->id,
+			dssdev->clocks.dispc.channel.lcd_clk_src);
 
 	DSSDBG("PLL OK\n");
 
@@ -3054,256 +4140,522 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 	if (r)
 		goto err2;
 
-	r = dsi_complexio_init(dssdev);
+	r = dsi_cio_init(dssdev);
 	if (r)
 		goto err2;
 
-	_dsi_print_reset_status();
+	_dsi_print_reset_status(dsidev);
 
 	dsi_proto_timings(dssdev);
 	dsi_set_lp_clk_divisor(dssdev);
 
 	if (1)
-		_dsi_print_reset_status();
+		_dsi_print_reset_status(dsidev);
 
 	r = dsi_proto_config(dssdev);
 	if (r)
 		goto err3;
 
 	/* enable interface */
-	dsi_vc_enable(0, 1);
-	dsi_vc_enable(1, 1);
-	dsi_vc_enable(2, 1);
-	dsi_vc_enable(3, 1);
-	dsi_if_enable(1);
-	dsi_force_tx_stop_mode_io();
+	dsi_vc_enable(dsidev, 0, 1);
+	dsi_vc_enable(dsidev, 1, 1);
+	dsi_vc_enable(dsidev, 2, 1);
+	dsi_vc_enable(dsidev, 3, 1);
+	dsi_if_enable(dsidev, 1);
+	dsi_force_tx_stop_mode_io(dsidev);
 
 	return 0;
 err3:
-	dsi_complexio_uninit();
+	dsi_cio_uninit(dsidev);
 err2:
-	dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-	dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+	dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+	dss_select_dsi_clk_source(dsi_module, OMAP_DSS_CLK_SRC_FCK);
 err1:
-	dsi_pll_uninit();
+	dsi_pll_uninit(dsidev, true);
 err0:
 	return r;
 }
 
-static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev)
-{
-	/* disable interface */
-	dsi_if_enable(0);
-	dsi_vc_enable(0, 0);
-	dsi_vc_enable(1, 0);
-	dsi_vc_enable(2, 0);
-	dsi_vc_enable(3, 0);
-
-	dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-	dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-	dsi_complexio_uninit();
-	dsi_pll_uninit();
-}
-
-static int dsi_core_init(void)
+static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
+		bool disconnect_lanes, bool enter_ulps)
 {
-	/* Autoidle */
-	REG_FLD_MOD(DSI_SYSCONFIG, 1, 0, 0);
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int dsi_module = dsi_get_dsidev_id(dsidev);
 
-	/* ENWAKEUP */
-	REG_FLD_MOD(DSI_SYSCONFIG, 1, 2, 2);
+	if (enter_ulps && !dsi->ulps_enabled)
+		dsi_enter_ulps(dsidev);
 
-	/* SIDLEMODE smart-idle */
-	REG_FLD_MOD(DSI_SYSCONFIG, 2, 4, 3);
-
-	_dsi_initialize_irq();
-
-	return 0;
+	/* disable interface */
+	dsi_if_enable(dsidev, 0);
+	dsi_vc_enable(dsidev, 0, 0);
+	dsi_vc_enable(dsidev, 1, 0);
+	dsi_vc_enable(dsidev, 2, 0);
+	dsi_vc_enable(dsidev, 3, 0);
+
+	dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+	dss_select_dsi_clk_source(dsi_module, OMAP_DSS_CLK_SRC_FCK);
+	dsi_cio_uninit(dsidev);
+	dsi_pll_uninit(dsidev, disconnect_lanes);
 }
 
 int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int r = 0;
 
 	DSSDBG("dsi_display_enable\n");
 
-	WARN_ON(!dsi_bus_is_locked());
+	WARN_ON(!dsi_bus_is_locked(dsidev));
 
-	mutex_lock(&dsi.lock);
+	mutex_lock(&dsi->lock);
 
 	r = omap_dss_start_device(dssdev);
 	if (r) {
 		DSSERR("failed to start device\n");
-		goto err0;
+		goto err_start_dev;
 	}
 
-	enable_clocks(1);
-	dsi_enable_pll_clock(1);
-
-	r = _dsi_reset();
+	r = dsi_runtime_get(dsidev);
 	if (r)
-		goto err1;
+		goto err_get_dsi;
+
+	dsi_enable_pll_clock(dsidev, 1);
 
-	dsi_core_init();
+	_dsi_initialize_irq(dsidev);
 
 	r = dsi_display_init_dispc(dssdev);
 	if (r)
-		goto err1;
+		goto err_init_dispc;
 
 	r = dsi_display_init_dsi(dssdev);
 	if (r)
-		goto err2;
+		goto err_init_dsi;
 
-	mutex_unlock(&dsi.lock);
+	mutex_unlock(&dsi->lock);
 
 	return 0;
 
-err2:
+err_init_dsi:
 	dsi_display_uninit_dispc(dssdev);
-err1:
-	enable_clocks(0);
-	dsi_enable_pll_clock(0);
+err_init_dispc:
+	dsi_enable_pll_clock(dsidev, 0);
+	dsi_runtime_put(dsidev);
+err_get_dsi:
 	omap_dss_stop_device(dssdev);
-err0:
-	mutex_unlock(&dsi.lock);
+err_start_dev:
+	mutex_unlock(&dsi->lock);
 	DSSDBG("dsi_display_enable FAILED\n");
 	return r;
 }
 EXPORT_SYMBOL(omapdss_dsi_display_enable);
 
-void omapdss_dsi_display_disable(struct omap_dss_device *dssdev)
+void omapdss_dsi_display_disable(struct omap_dss_device *dssdev,
+		bool disconnect_lanes, bool enter_ulps)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
 	DSSDBG("dsi_display_disable\n");
 
-	WARN_ON(!dsi_bus_is_locked());
+	WARN_ON(!dsi_bus_is_locked(dsidev));
 
-	mutex_lock(&dsi.lock);
+	mutex_lock(&dsi->lock);
+
+	dsi_sync_vc(dsidev, 0);
+	dsi_sync_vc(dsidev, 1);
+	dsi_sync_vc(dsidev, 2);
+	dsi_sync_vc(dsidev, 3);
 
 	dsi_display_uninit_dispc(dssdev);
 
-	dsi_display_uninit_dsi(dssdev);
+	dsi_display_uninit_dsi(dssdev, disconnect_lanes, enter_ulps);
 
-	enable_clocks(0);
-	dsi_enable_pll_clock(0);
+	dsi_runtime_put(dsidev);
+	dsi_enable_pll_clock(dsidev, 0);
 
 	omap_dss_stop_device(dssdev);
 
-	mutex_unlock(&dsi.lock);
+	mutex_unlock(&dsi->lock);
 }
 EXPORT_SYMBOL(omapdss_dsi_display_disable);
 
 int omapdss_dsi_enable_te(struct omap_dss_device *dssdev, bool enable)
 {
-	dsi.te_enabled = enable;
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	dsi->te_enabled = enable;
 	return 0;
 }
 EXPORT_SYMBOL(omapdss_dsi_enable_te);
 
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
-		u32 fifo_size, enum omap_burst_size *burst_size,
+		u32 fifo_size, u32 burst_size,
 		u32 *fifo_low, u32 *fifo_high)
 {
-	unsigned burst_size_bytes;
-
-	*burst_size = OMAP_DSS_BURST_16x32;
-	burst_size_bytes = 16 * 32 / 8;
-
-	*fifo_high = fifo_size - burst_size_bytes;
-	*fifo_low = fifo_size - burst_size_bytes * 2;
+	*fifo_high = fifo_size - burst_size;
+	*fifo_low = fifo_size - burst_size * 2;
 }
 
 int dsi_init_display(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int dsi_module = dsi_get_dsidev_id(dsidev);
+
 	DSSDBG("DSI init\n");
 
 	/* XXX these should be figured out dynamically */
 	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
 		OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
 
-	dsi.vc[0].dssdev = dssdev;
-	dsi.vc[1].dssdev = dssdev;
+	if (dsi->vdds_dsi_reg == NULL) {
+		struct regulator *vdds_dsi;
+
+		vdds_dsi = regulator_get(&dsi->pdev->dev, "vdds_dsi");
+
+		if (IS_ERR(vdds_dsi)) {
+			DSSERR("can't get VDDS_DSI regulator\n");
+			return PTR_ERR(vdds_dsi);
+		}
+
+		dsi->vdds_dsi_reg = vdds_dsi;
+	}
+
+	if (dsi_get_num_data_lanes_dssdev(dssdev) > dsi->num_data_lanes) {
+		DSSERR("DSI%d can't support more than %d data lanes\n",
+			dsi_module + 1, dsi->num_data_lanes);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int omap_dsi_request_vc(struct omap_dss_device *dssdev, int *channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+		if (!dsi->vc[i].dssdev) {
+			dsi->vc[i].dssdev = dssdev;
+			*channel = i;
+			return 0;
+		}
+	}
+
+	DSSERR("cannot get VC for display %s", dssdev->name);
+	return -ENOSPC;
+}
+EXPORT_SYMBOL(omap_dsi_request_vc);
+
+int omap_dsi_set_vc_id(struct omap_dss_device *dssdev, int channel, int vc_id)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (vc_id < 0 || vc_id > 3) {
+		DSSERR("VC ID out of range\n");
+		return -EINVAL;
+	}
+
+	if (channel < 0 || channel > 3) {
+		DSSERR("Virtual Channel out of range\n");
+		return -EINVAL;
+	}
+
+	if (dsi->vc[channel].dssdev != dssdev) {
+		DSSERR("Virtual Channel not allocated to display %s\n",
+			dssdev->name);
+		return -EINVAL;
+	}
+
+	dsi->vc[channel].vc_id = vc_id;
 
 	return 0;
 }
+EXPORT_SYMBOL(omap_dsi_set_vc_id);
+
+void omap_dsi_release_vc(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if ((channel >= 0 && channel <= 3) &&
+		dsi->vc[channel].dssdev == dssdev) {
+		dsi->vc[channel].dssdev = NULL;
+		dsi->vc[channel].vc_id = 0;
+	}
+}
+EXPORT_SYMBOL(omap_dsi_release_vc);
 
-void dsi_wait_dsi1_pll_active(void)
+void dsi_wait_pll_hsdiv_dispc_active(struct platform_device *dsidev)
 {
-	if (wait_for_bit_change(DSI_PLL_STATUS, 7, 1) != 1)
-		DSSERR("DSI1 PLL clock not active\n");
+	if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 7, 1) != 1)
+		DSSERR("%s (%s) not active\n",
+			dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+			dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC));
+}
+
+void dsi_wait_pll_hsdiv_dsi_active(struct platform_device *dsidev)
+{
+	if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 8, 1) != 1)
+		DSSERR("%s (%s) not active\n",
+			dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+			dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI));
+}
+
+static void dsi_calc_clock_param_ranges(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	dsi->regn_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGN);
+	dsi->regm_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM);
+	dsi->regm_dispc_max =
+		dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM_DISPC);
+	dsi->regm_dsi_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM_DSI);
+	dsi->fint_min = dss_feat_get_param_min(FEAT_PARAM_DSIPLL_FINT);
+	dsi->fint_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_FINT);
+	dsi->lpdiv_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_LPDIV);
+}
+
+static int dsi_get_clocks(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct clk *clk;
+
+	clk = clk_get(&dsidev->dev, "fck");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get fck\n");
+		return PTR_ERR(clk);
+	}
+
+	dsi->dss_clk = clk;
+
+	if (cpu_is_omap34xx() || cpu_is_omap3630())
+		clk = clk_get(&dsidev->dev, "dss2_alwon_fck");
+	else
+		clk = clk_get(&dsidev->dev, "sys_clk");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get sys_clk\n");
+		clk_put(dsi->dss_clk);
+		dsi->dss_clk = NULL;
+		return PTR_ERR(clk);
+	}
+
+	dsi->sys_clk = clk;
+
+	return 0;
 }
 
-void dsi_wait_dsi2_pll_active(void)
+static void dsi_put_clocks(struct platform_device *dsidev)
 {
-	if (wait_for_bit_change(DSI_PLL_STATUS, 8, 1) != 1)
-		DSSERR("DSI2 PLL clock not active\n");
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->dss_clk)
+		clk_put(dsi->dss_clk);
+	if (dsi->sys_clk)
+		clk_put(dsi->sys_clk);
 }
 
-int dsi_init(struct platform_device *pdev)
+/* DSI1 HW IP initialisation */
+static int omap_dsi1hw_probe(struct platform_device *dsidev)
 {
+	struct omap_display_platform_data *dss_plat_data;
+	struct omap_dss_board_info *board_info;
 	u32 rev;
-	int r;
+	int r, i, dsi_module = dsi_get_dsidev_id(dsidev);
+	struct resource *dsi_mem;
+	struct dsi_data *dsi;
+
+	dsi = kzalloc(sizeof(*dsi), GFP_KERNEL);
+	if (!dsi) {
+		r = -ENOMEM;
+		goto err_alloc;
+	}
+
+	dsi->pdev = dsidev;
+	dsi_pdev_map[dsi_module] = dsidev;
+	dev_set_drvdata(&dsidev->dev, dsi);
+
+	dss_plat_data = dsidev->dev.platform_data;
+	board_info = dss_plat_data->board_data;
+	dsi->dsi_mux_pads = board_info->dsi_mux_pads;
 
-	spin_lock_init(&dsi.errors_lock);
-	dsi.errors = 0;
+	spin_lock_init(&dsi->irq_lock);
+	spin_lock_init(&dsi->errors_lock);
+	dsi->errors = 0;
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-	spin_lock_init(&dsi.irq_stats_lock);
-	dsi.irq_stats.last_reset = jiffies;
+	spin_lock_init(&dsi->irq_stats_lock);
+	dsi->irq_stats.last_reset = jiffies;
 #endif
 
-	init_completion(&dsi.bta_completion);
+	mutex_init(&dsi->lock);
+	sema_init(&dsi->bus_lock, 1);
 
-	mutex_init(&dsi.lock);
-	sema_init(&dsi.bus_lock, 1);
+	r = dsi_get_clocks(dsidev);
+	if (r)
+		goto err_get_clk;
 
-	dsi.workqueue = create_singlethread_workqueue("dsi");
-	if (dsi.workqueue == NULL)
-		return -ENOMEM;
+	pm_runtime_enable(&dsidev->dev);
 
-	INIT_DELAYED_WORK_DEFERRABLE(&dsi.framedone_timeout_work,
+	INIT_DELAYED_WORK_DEFERRABLE(&dsi->framedone_timeout_work,
 			dsi_framedone_timeout_work_callback);
 
 #ifdef DSI_CATCH_MISSING_TE
-	init_timer(&dsi.te_timer);
-	dsi.te_timer.function = dsi_te_timeout;
-	dsi.te_timer.data = 0;
+	init_timer(&dsi->te_timer);
+	dsi->te_timer.function = dsi_te_timeout;
+	dsi->te_timer.data = 0;
 #endif
-	dsi.base = ioremap(DSI_BASE, DSI_SZ_REGS);
-	if (!dsi.base) {
+	dsi_mem = platform_get_resource(dsi->pdev, IORESOURCE_MEM, 0);
+	if (!dsi_mem) {
+		DSSERR("can't get IORESOURCE_MEM DSI\n");
+		r = -EINVAL;
+		goto err_ioremap;
+	}
+	dsi->base = ioremap(dsi_mem->start, resource_size(dsi_mem));
+	if (!dsi->base) {
 		DSSERR("can't ioremap DSI\n");
 		r = -ENOMEM;
-		goto err1;
+		goto err_ioremap;
+	}
+	dsi->irq = platform_get_irq(dsi->pdev, 0);
+	if (dsi->irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		r = -ENODEV;
+		goto err_get_irq;
 	}
 
-	dsi.vdds_dsi_reg = dss_get_vdds_dsi();
-	if (IS_ERR(dsi.vdds_dsi_reg)) {
-		DSSERR("can't get VDDS_DSI regulator\n");
-		r = PTR_ERR(dsi.vdds_dsi_reg);
-		goto err2;
+	r = request_irq(dsi->irq, omap_dsi_irq_handler, IRQF_SHARED,
+		dev_name(&dsidev->dev), dsi->pdev);
+	if (r < 0) {
+		DSSERR("request_irq failed\n");
+		goto err_get_irq;
+	}
+
+	/* DSI VCs initialization */
+	for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+		dsi->vc[i].mode = DSI_VC_MODE_L4;
+		dsi->vc[i].dssdev = NULL;
+		dsi->vc[i].vc_id = 0;
 	}
 
-	enable_clocks(1);
+	dsi_calc_clock_param_ranges(dsidev);
 
-	rev = dsi_read_reg(DSI_REVISION);
-	printk(KERN_INFO "OMAP DSI rev %d.%d\n",
+	r = dsi_runtime_get(dsidev);
+	if (r)
+		goto err_get_dsi;
+
+	rev = dsi_read_reg(dsidev, DSI_REVISION);
+	dev_dbg(&dsidev->dev, "OMAP DSI rev %d.%d\n",
 	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
-	enable_clocks(0);
+	dsi->num_data_lanes = dsi_get_num_data_lanes(dsidev);
+
+	dsi_runtime_put(dsidev);
 
 	return 0;
-err2:
-	iounmap(dsi.base);
-err1:
-	destroy_workqueue(dsi.workqueue);
+
+err_get_dsi:
+	free_irq(dsi->irq, dsi->pdev);
+err_get_irq:
+	iounmap(dsi->base);
+err_ioremap:
+	pm_runtime_disable(&dsidev->dev);
+err_get_clk:
+	kfree(dsi);
+err_alloc:
 	return r;
 }
 
-void dsi_exit(void)
+static int omap_dsi1hw_remove(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	WARN_ON(dsi->scp_clk_refcount > 0);
+
+	pm_runtime_disable(&dsidev->dev);
+
+	dsi_put_clocks(dsidev);
+
+	if (dsi->vdds_dsi_reg != NULL) {
+		if (dsi->vdds_dsi_enabled) {
+			regulator_disable(dsi->vdds_dsi_reg);
+			dsi->vdds_dsi_enabled = false;
+		}
+
+		regulator_put(dsi->vdds_dsi_reg);
+		dsi->vdds_dsi_reg = NULL;
+	}
+
+	free_irq(dsi->irq, dsi->pdev);
+	iounmap(dsi->base);
+
+	kfree(dsi);
+
+	return 0;
+}
+
+static int dsi_runtime_suspend(struct device *dev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(to_platform_device(dev));
+
+	clk_disable(dsi->dss_clk);
+
+	dispc_runtime_put();
+	dss_runtime_put();
+
+	return 0;
+}
+
+static int dsi_runtime_resume(struct device *dev)
 {
-	iounmap(dsi.base);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(to_platform_device(dev));
+	int r;
+
+	r = dss_runtime_get();
+	if (r)
+		goto err_get_dss;
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err_get_dispc;
+
+	clk_enable(dsi->dss_clk);
 
-	destroy_workqueue(dsi.workqueue);
+	return 0;
 
-	DSSDBG("omap_dsi_exit\n");
+err_get_dispc:
+	dss_runtime_put();
+err_get_dss:
+	return r;
 }
 
+static const struct dev_pm_ops dsi_pm_ops = {
+	.runtime_suspend = dsi_runtime_suspend,
+	.runtime_resume = dsi_runtime_resume,
+};
+
+static struct platform_driver omap_dsi1hw_driver = {
+	.probe          = omap_dsi1hw_probe,
+	.remove         = omap_dsi1hw_remove,
+	.driver         = {
+		.name   = "omapdss_dsi1",
+		.owner  = THIS_MODULE,
+		.pm	= &dsi_pm_ops,
+	},
+};
+
+int dsi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dsi1hw_driver);
+}
+
+void dsi_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_dsi1hw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 77c3621c9171..0f9c3a6457a5 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -26,14 +26,15 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/delay.h>
-#include <linux/interrupt.h>
 #include <linux/seq_file.h>
 #include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
+#include <plat/clock.h>
 #include "dss.h"
-
-#define DSS_BASE			0x48050000
+#include "dss_features.h"
 
 #define DSS_SZ_REGS			SZ_512
 
@@ -46,7 +47,6 @@ struct dss_reg {
 #define DSS_REVISION			DSS_REG(0x0000)
 #define DSS_SYSCONFIG			DSS_REG(0x0010)
 #define DSS_SYSSTATUS			DSS_REG(0x0014)
-#define DSS_IRQSTATUS			DSS_REG(0x0018)
 #define DSS_CONTROL			DSS_REG(0x0040)
 #define DSS_SDI_CONTROL			DSS_REG(0x0044)
 #define DSS_PLL_CONTROL			DSS_REG(0x0048)
@@ -59,22 +59,30 @@ struct dss_reg {
 	dss_write_reg(idx, FLD_MOD(dss_read_reg(idx), val, start, end))
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem    *base;
 
 	struct clk	*dpll4_m4_ck;
+	struct clk	*dss_clk;
 
 	unsigned long	cache_req_pck;
 	unsigned long	cache_prate;
 	struct dss_clock_info cache_dss_cinfo;
 	struct dispc_clock_info cache_dispc_cinfo;
 
-	enum dss_clk_source dsi_clk_source;
-	enum dss_clk_source dispc_clk_source;
+	enum omap_dss_clk_source dsi_clk_source[MAX_NUM_DSI];
+	enum omap_dss_clk_source dispc_clk_source;
+	enum omap_dss_clk_source lcd_clk_source[MAX_DSS_LCD_MANAGERS];
 
+	bool		ctx_valid;
 	u32		ctx[DSS_SZ_REGS / sizeof(u32)];
 } dss;
 
-static int _omap_dss_wait_reset(void);
+static const char * const dss_generic_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "DSI_PLL_HSDIV_DISPC",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "DSI_PLL_HSDIV_DSI",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_FCK",
+};
 
 static inline void dss_write_reg(const struct dss_reg idx, u32 val)
 {
@@ -91,32 +99,39 @@ static inline u32 dss_read_reg(const struct dss_reg idx)
 #define RR(reg) \
 	dss_write_reg(DSS_##reg, dss.ctx[(DSS_##reg).idx / sizeof(u32)])
 
-void dss_save_context(void)
+static void dss_save_context(void)
 {
-	if (cpu_is_omap24xx())
-		return;
+	DSSDBG("dss_save_context\n");
 
-	SR(SYSCONFIG);
 	SR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-	SR(SDI_CONTROL);
-	SR(PLL_CONTROL);
-#endif
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		SR(SDI_CONTROL);
+		SR(PLL_CONTROL);
+	}
+
+	dss.ctx_valid = true;
+
+	DSSDBG("context saved\n");
 }
 
-void dss_restore_context(void)
+static void dss_restore_context(void)
 {
-	if (_omap_dss_wait_reset())
-		DSSERR("DSS not coming out of reset after sleep\n");
+	DSSDBG("dss_restore_context\n");
+
+	if (!dss.ctx_valid)
+		return;
 
-	RR(SYSCONFIG);
 	RR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-	RR(SDI_CONTROL);
-	RR(PLL_CONTROL);
-#endif
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		RR(SDI_CONTROL);
+		RR(PLL_CONTROL);
+	}
+
+	DSSDBG("context restored\n");
 }
 
 #undef SR
@@ -209,125 +224,238 @@ void dss_sdi_disable(void)
 	REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
 }
 
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src)
+{
+	return dss_generic_clk_source_names[clk_src];
+}
+
+
 void dss_dump_clocks(struct seq_file *s)
 {
 	unsigned long dpll4_ck_rate;
 	unsigned long dpll4_m4_ck_rate;
+	const char *fclk_name, *fclk_real_name;
+	unsigned long fclk_rate;
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-	dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-	dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
+	if (dss_runtime_get())
+		return;
 
 	seq_printf(s, "- DSS -\n");
 
-	seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
+	fclk_name = dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+	fclk_real_name = dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+	fclk_rate = clk_get_rate(dss.dss_clk);
 
-	if (cpu_is_omap3630())
-		seq_printf(s, "dss1_alwon_fclk = %lu / %lu  = %lu\n",
-			dpll4_ck_rate,
-			dpll4_ck_rate / dpll4_m4_ck_rate,
-			dss_clk_get_rate(DSS_CLK_FCK1));
-	else
-		seq_printf(s, "dss1_alwon_fclk = %lu / %lu * 2 = %lu\n",
-			dpll4_ck_rate,
-			dpll4_ck_rate / dpll4_m4_ck_rate,
-			dss_clk_get_rate(DSS_CLK_FCK1));
+	if (dss.dpll4_m4_ck) {
+		dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+		dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
+
+		seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
+
+		if (cpu_is_omap3630() || cpu_is_omap44xx())
+			seq_printf(s, "%s (%s) = %lu / %lu  = %lu\n",
+					fclk_name, fclk_real_name,
+					dpll4_ck_rate,
+					dpll4_ck_rate / dpll4_m4_ck_rate,
+					fclk_rate);
+		else
+			seq_printf(s, "%s (%s) = %lu / %lu * 2 = %lu\n",
+					fclk_name, fclk_real_name,
+					dpll4_ck_rate,
+					dpll4_ck_rate / dpll4_m4_ck_rate,
+					fclk_rate);
+	} else {
+		seq_printf(s, "%s (%s) = %lu\n",
+				fclk_name, fclk_real_name,
+				fclk_rate);
+	}
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_runtime_put();
 }
 
 void dss_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dss_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	if (dss_runtime_get())
+		return;
 
 	DUMPREG(DSS_REVISION);
 	DUMPREG(DSS_SYSCONFIG);
 	DUMPREG(DSS_SYSSTATUS);
-	DUMPREG(DSS_IRQSTATUS);
 	DUMPREG(DSS_CONTROL);
-	DUMPREG(DSS_SDI_CONTROL);
-	DUMPREG(DSS_PLL_CONTROL);
-	DUMPREG(DSS_SDI_STATUS);
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		DUMPREG(DSS_SDI_CONTROL);
+		DUMPREG(DSS_PLL_CONTROL);
+		DUMPREG(DSS_SDI_STATUS);
+	}
+
+	dss_runtime_put();
 #undef DUMPREG
 }
 
-void dss_select_dispc_clk_source(enum dss_clk_source clk_src)
+void dss_select_dispc_clk_source(enum omap_dss_clk_source clk_src)
 {
+	struct platform_device *dsidev;
 	int b;
+	u8 start, end;
+
+	switch (clk_src) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		b = 0;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		b = 1;
+		dsidev = dsi_get_dsidev_from_id(0);
+		dsi_wait_pll_hsdiv_dispc_active(dsidev);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		b = 2;
+		dsidev = dsi_get_dsidev_from_id(1);
+		dsi_wait_pll_hsdiv_dispc_active(dsidev);
+		break;
+	default:
+		BUG();
+	}
 
-	BUG_ON(clk_src != DSS_SRC_DSI1_PLL_FCLK &&
-			clk_src != DSS_SRC_DSS1_ALWON_FCLK);
-
-	b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
-
-	if (clk_src == DSS_SRC_DSI1_PLL_FCLK)
-		dsi_wait_dsi1_pll_active();
+	dss_feat_get_reg_field(FEAT_REG_DISPC_CLK_SWITCH, &start, &end);
 
-	REG_FLD_MOD(DSS_CONTROL, b, 0, 0);	/* DISPC_CLK_SWITCH */
+	REG_FLD_MOD(DSS_CONTROL, b, start, end);	/* DISPC_CLK_SWITCH */
 
 	dss.dispc_clk_source = clk_src;
 }
 
-void dss_select_dsi_clk_source(enum dss_clk_source clk_src)
+void dss_select_dsi_clk_source(int dsi_module,
+		enum omap_dss_clk_source clk_src)
 {
+	struct platform_device *dsidev;
 	int b;
 
-	BUG_ON(clk_src != DSS_SRC_DSI2_PLL_FCLK &&
-			clk_src != DSS_SRC_DSS1_ALWON_FCLK);
+	switch (clk_src) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		b = 0;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI:
+		BUG_ON(dsi_module != 0);
+		b = 1;
+		dsidev = dsi_get_dsidev_from_id(0);
+		dsi_wait_pll_hsdiv_dsi_active(dsidev);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI:
+		BUG_ON(dsi_module != 1);
+		b = 1;
+		dsidev = dsi_get_dsidev_from_id(1);
+		dsi_wait_pll_hsdiv_dsi_active(dsidev);
+		break;
+	default:
+		BUG();
+	}
 
-	b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
+	REG_FLD_MOD(DSS_CONTROL, b, 1, 1);	/* DSI_CLK_SWITCH */
 
-	if (clk_src == DSS_SRC_DSI2_PLL_FCLK)
-		dsi_wait_dsi2_pll_active();
+	dss.dsi_clk_source[dsi_module] = clk_src;
+}
 
-	REG_FLD_MOD(DSS_CONTROL, b, 1, 1);	/* DSI_CLK_SWITCH */
+void dss_select_lcd_clk_source(enum omap_channel channel,
+		enum omap_dss_clk_source clk_src)
+{
+	struct platform_device *dsidev;
+	int b, ix, pos;
+
+	if (!dss_has_feature(FEAT_LCD_CLK_SRC))
+		return;
+
+	switch (clk_src) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		b = 0;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		BUG_ON(channel != OMAP_DSS_CHANNEL_LCD);
+		b = 1;
+		dsidev = dsi_get_dsidev_from_id(0);
+		dsi_wait_pll_hsdiv_dispc_active(dsidev);
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		BUG_ON(channel != OMAP_DSS_CHANNEL_LCD2);
+		b = 1;
+		dsidev = dsi_get_dsidev_from_id(1);
+		dsi_wait_pll_hsdiv_dispc_active(dsidev);
+		break;
+	default:
+		BUG();
+	}
 
-	dss.dsi_clk_source = clk_src;
+	pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 12;
+	REG_FLD_MOD(DSS_CONTROL, b, pos, pos);	/* LCDx_CLK_SWITCH */
+
+	ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+	dss.lcd_clk_source[ix] = clk_src;
 }
 
-enum dss_clk_source dss_get_dispc_clk_source(void)
+enum omap_dss_clk_source dss_get_dispc_clk_source(void)
 {
 	return dss.dispc_clk_source;
 }
 
-enum dss_clk_source dss_get_dsi_clk_source(void)
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module)
+{
+	return dss.dsi_clk_source[dsi_module];
+}
+
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
 {
-	return dss.dsi_clk_source;
+	if (dss_has_feature(FEAT_LCD_CLK_SRC)) {
+		int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+		return dss.lcd_clk_source[ix];
+	} else {
+		/* LCD_CLK source is the same as DISPC_FCLK source for
+		 * OMAP2 and OMAP3 */
+		return dss.dispc_clk_source;
+	}
 }
 
 /* calculate clock rates using dividers in cinfo */
 int dss_calc_clock_rates(struct dss_clock_info *cinfo)
 {
-	unsigned long prate;
+	if (dss.dpll4_m4_ck) {
+		unsigned long prate;
+		u16 fck_div_max = 16;
 
-	if (cinfo->fck_div > (cpu_is_omap3630() ? 32 : 16) ||
-						cinfo->fck_div == 0)
-		return -EINVAL;
+		if (cpu_is_omap3630() || cpu_is_omap44xx())
+			fck_div_max = 32;
 
-	prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+		if (cinfo->fck_div > fck_div_max || cinfo->fck_div == 0)
+			return -EINVAL;
 
-	cinfo->fck = prate / cinfo->fck_div;
+		prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+
+		cinfo->fck = prate / cinfo->fck_div;
+	} else {
+		if (cinfo->fck_div != 0)
+			return -EINVAL;
+		cinfo->fck = clk_get_rate(dss.dss_clk);
+	}
 
 	return 0;
 }
 
 int dss_set_clock_div(struct dss_clock_info *cinfo)
 {
-	unsigned long prate;
-	int r;
+	if (dss.dpll4_m4_ck) {
+		unsigned long prate;
+		int r;
 
-	if (cpu_is_omap34xx()) {
 		prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
 		DSSDBG("dpll4_m4 = %ld\n", prate);
 
 		r = clk_set_rate(dss.dpll4_m4_ck, prate / cinfo->fck_div);
 		if (r)
 			return r;
+	} else {
+		if (cinfo->fck_div != 0)
+			return -EINVAL;
 	}
 
 	DSSDBG("fck = %ld (%d)\n", cinfo->fck, cinfo->fck_div);
@@ -337,12 +465,14 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 
 int dss_get_clock_div(struct dss_clock_info *cinfo)
 {
-	cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK1);
+	cinfo->fck = clk_get_rate(dss.dss_clk);
 
-	if (cpu_is_omap34xx()) {
+	if (dss.dpll4_m4_ck) {
 		unsigned long prate;
+
 		prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-		if (cpu_is_omap3630())
+
+		if (cpu_is_omap3630() || cpu_is_omap44xx())
 			cinfo->fck_div = prate / (cinfo->fck);
 		else
 			cinfo->fck_div = prate / (cinfo->fck / 2);
@@ -355,7 +485,7 @@ int dss_get_clock_div(struct dss_clock_info *cinfo)
 
 unsigned long dss_get_dpll4_rate(void)
 {
-	if (cpu_is_omap34xx())
+	if (dss.dpll4_m4_ck)
 		return clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
 	else
 		return 0;
@@ -369,16 +499,18 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 	struct dss_clock_info best_dss;
 	struct dispc_clock_info best_dispc;
 
-	unsigned long fck;
+	unsigned long fck, max_dss_fck;
 
-	u16 fck_div;
+	u16 fck_div, fck_div_max = 16;
 
 	int match = 0;
 	int min_fck_per_pck;
 
 	prate = dss_get_dpll4_rate();
 
-	fck = dss_clk_get_rate(DSS_CLK_FCK1);
+	max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+	fck = clk_get_rate(dss.dss_clk);
 	if (req_pck == dss.cache_req_pck &&
 			((cpu_is_omap34xx() && prate == dss.cache_prate) ||
 			 dss.cache_dss_cinfo.fck == fck)) {
@@ -391,7 +523,7 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
 	if (min_fck_per_pck &&
-		req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+		req_pck * min_fck_per_pck > max_dss_fck) {
 		DSSERR("Requested pixel clock not possible with the current "
 				"OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
 				"the constraint off.\n");
@@ -402,10 +534,10 @@ retry:
 	memset(&best_dss, 0, sizeof(best_dss));
 	memset(&best_dispc, 0, sizeof(best_dispc));
 
-	if (cpu_is_omap24xx()) {
+	if (dss.dpll4_m4_ck == NULL) {
 		struct dispc_clock_info cur_dispc;
 		/* XXX can we change the clock on omap2? */
-		fck = dss_clk_get_rate(DSS_CLK_FCK1);
+		fck = clk_get_rate(dss.dss_clk);
 		fck_div = 1;
 
 		dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
@@ -417,17 +549,19 @@ retry:
 		best_dispc = cur_dispc;
 
 		goto found;
-	} else if (cpu_is_omap34xx()) {
-		for (fck_div = (cpu_is_omap3630() ? 32 : 16);
-					fck_div > 0; --fck_div) {
+	} else {
+		if (cpu_is_omap3630() || cpu_is_omap44xx())
+			fck_div_max = 32;
+
+		for (fck_div = fck_div_max; fck_div > 0; --fck_div) {
 			struct dispc_clock_info cur_dispc;
 
-			if (cpu_is_omap3630())
+			if (fck_div_max == 32)
 				fck = prate / fck_div;
 			else
 				fck = prate / fck_div * 2;
 
-			if (fck > DISPC_MAX_FCK)
+			if (fck > max_dss_fck)
 				continue;
 
 			if (min_fck_per_pck &&
@@ -450,8 +584,6 @@ retry:
 					goto found;
 			}
 		}
-	} else {
-		BUG();
 	}
 
 found:
@@ -482,104 +614,152 @@ found:
 	return 0;
 }
 
-
-
-static irqreturn_t dss_irq_handler_omap2(int irq, void *arg)
+void dss_set_venc_output(enum omap_dss_venc_type type)
 {
-	dispc_irq_handler();
+	int l = 0;
 
-	return IRQ_HANDLED;
+	if (type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+		l = 0;
+	else if (type == OMAP_DSS_VENC_TYPE_SVIDEO)
+		l = 1;
+	else
+		BUG();
+
+	/* venc out selection. 0 = comp, 1 = svideo */
+	REG_FLD_MOD(DSS_CONTROL, l, 6, 6);
 }
 
-static irqreturn_t dss_irq_handler_omap3(int irq, void *arg)
+void dss_set_dac_pwrdn_bgz(bool enable)
 {
-	u32 irqstatus;
-
-	irqstatus = dss_read_reg(DSS_IRQSTATUS);
-
-	if (irqstatus & (1<<0))	/* DISPC_IRQ */
-		dispc_irq_handler();
-#ifdef CONFIG_OMAP2_DSS_DSI
-	if (irqstatus & (1<<1))	/* DSI_IRQ */
-		dsi_irq_handler();
-#endif
+	REG_FLD_MOD(DSS_CONTROL, enable, 5, 5);	/* DAC Power-Down Control */
+}
 
-	return IRQ_HANDLED;
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select hdmi)
+{
+	REG_FLD_MOD(DSS_CONTROL, hdmi, 15, 15);	/* VENC_HDMI_SWITCH */
 }
 
-static int _omap_dss_wait_reset(void)
+static int dss_get_clocks(void)
 {
-	int t = 0;
+	struct clk *clk;
+	int r;
 
-	while (REG_GET(DSS_SYSSTATUS, 0, 0) == 0) {
-		if (++t > 1000) {
-			DSSERR("soft reset failed\n");
-			return -ENODEV;
+	clk = clk_get(&dss.pdev->dev, "fck");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get clock fck\n");
+		r = PTR_ERR(clk);
+		goto err;
+	}
+
+	dss.dss_clk = clk;
+
+	if (cpu_is_omap34xx()) {
+		clk = clk_get(NULL, "dpll4_m4_ck");
+		if (IS_ERR(clk)) {
+			DSSERR("Failed to get dpll4_m4_ck\n");
+			r = PTR_ERR(clk);
+			goto err;
 		}
-		udelay(1);
+	} else if (cpu_is_omap44xx()) {
+		clk = clk_get(NULL, "dpll_per_m5x2_ck");
+		if (IS_ERR(clk)) {
+			DSSERR("Failed to get dpll_per_m5x2_ck\n");
+			r = PTR_ERR(clk);
+			goto err;
+		}
+	} else { /* omap24xx */
+		clk = NULL;
 	}
 
+	dss.dpll4_m4_ck = clk;
+
 	return 0;
+
+err:
+	if (dss.dss_clk)
+		clk_put(dss.dss_clk);
+	if (dss.dpll4_m4_ck)
+		clk_put(dss.dpll4_m4_ck);
+
+	return r;
 }
 
-static int _omap_dss_reset(void)
+static void dss_put_clocks(void)
 {
-	/* Soft reset */
-	REG_FLD_MOD(DSS_SYSCONFIG, 1, 1, 1);
-	return _omap_dss_wait_reset();
+	if (dss.dpll4_m4_ck)
+		clk_put(dss.dpll4_m4_ck);
+	clk_put(dss.dss_clk);
 }
 
-void dss_set_venc_output(enum omap_dss_venc_type type)
+struct clk *dss_get_ick(void)
 {
-	int l = 0;
+	return clk_get(&dss.pdev->dev, "ick");
+}
 
-	if (type == OMAP_DSS_VENC_TYPE_COMPOSITE)
-		l = 0;
-	else if (type == OMAP_DSS_VENC_TYPE_SVIDEO)
-		l = 1;
-	else
-		BUG();
+int dss_runtime_get(void)
+{
+	int r;
 
-	/* venc out selection. 0 = comp, 1 = svideo */
-	REG_FLD_MOD(DSS_CONTROL, l, 6, 6);
+	DSSDBG("dss_runtime_get\n");
+
+	r = pm_runtime_get_sync(&dss.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
 }
 
-void dss_set_dac_pwrdn_bgz(bool enable)
+void dss_runtime_put(void)
 {
-	REG_FLD_MOD(DSS_CONTROL, enable, 5, 5);	/* DAC Power-Down Control */
+	int r;
+
+	DSSDBG("dss_runtime_put\n");
+
+	r = pm_runtime_put(&dss.pdev->dev);
+	WARN_ON(r < 0);
 }
 
-int dss_init(bool skip_init)
+/* DEBUGFS */
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s)
 {
-	int r;
+	dss_dump_clocks(s);
+	dispc_dump_clocks(s);
+#ifdef CONFIG_OMAP2_DSS_DSI
+	dsi_dump_clocks(s);
+#endif
+}
+#endif
+
+/* DSS HW IP initialisation */
+static int omap_dsshw_probe(struct platform_device *pdev)
+{
+	struct resource *dss_mem;
 	u32 rev;
+	int r;
 
-	dss.base = ioremap(DSS_BASE, DSS_SZ_REGS);
+	dss.pdev = pdev;
+
+	dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
+	if (!dss_mem) {
+		DSSERR("can't get IORESOURCE_MEM DSS\n");
+		r = -EINVAL;
+		goto err_ioremap;
+	}
+	dss.base = ioremap(dss_mem->start, resource_size(dss_mem));
 	if (!dss.base) {
 		DSSERR("can't ioremap DSS\n");
 		r = -ENOMEM;
-		goto fail0;
+		goto err_ioremap;
 	}
 
-	if (!skip_init) {
-		/* disable LCD and DIGIT output. This seems to fix the synclost
-		 * problem that we get, if the bootloader starts the DSS and
-		 * the kernel resets it */
-		omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
-
-		/* We need to wait here a bit, otherwise we sometimes start to
-		 * get synclost errors, and after that only power cycle will
-		 * restore DSS functionality. I have no idea why this happens.
-		 * And we have to wait _before_ resetting the DSS, but after
-		 * enabling clocks.
-		 */
-		msleep(50);
-
-		_omap_dss_reset();
-	}
+	r = dss_get_clocks();
+	if (r)
+		goto err_clocks;
 
-	/* autoidle */
-	REG_FLD_MOD(DSS_SYSCONFIG, 1, 0, 0);
+	pm_runtime_enable(&pdev->dev);
+
+	r = dss_runtime_get();
+	if (r)
+		goto err_runtime_get;
 
 	/* Select DPLL */
 	REG_FLD_MOD(DSS_CONTROL, 0, 0, 0);
@@ -589,53 +769,93 @@ int dss_init(bool skip_init)
 	REG_FLD_MOD(DSS_CONTROL, 1, 3, 3);	/* venc clock 4x enable */
 	REG_FLD_MOD(DSS_CONTROL, 0, 2, 2);	/* venc clock mode = normal */
 #endif
-
-	r = request_irq(INT_24XX_DSS_IRQ,
-			cpu_is_omap24xx()
-			? dss_irq_handler_omap2
-			: dss_irq_handler_omap3,
-			0, "OMAP DSS", NULL);
-
-	if (r < 0) {
-		DSSERR("omap2 dss: request_irq failed\n");
-		goto fail1;
+	dss.dsi_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
+	dss.dsi_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
+	dss.dispc_clk_source = OMAP_DSS_CLK_SRC_FCK;
+	dss.lcd_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
+	dss.lcd_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
+
+	r = dpi_init();
+	if (r) {
+		DSSERR("Failed to initialize DPI\n");
+		goto err_dpi;
 	}
 
-	if (cpu_is_omap34xx()) {
-		dss.dpll4_m4_ck = clk_get(NULL, "dpll4_m4_ck");
-		if (IS_ERR(dss.dpll4_m4_ck)) {
-			DSSERR("Failed to get dpll4_m4_ck\n");
-			r = PTR_ERR(dss.dpll4_m4_ck);
-			goto fail2;
-		}
+	r = sdi_init();
+	if (r) {
+		DSSERR("Failed to initialize SDI\n");
+		goto err_sdi;
 	}
 
-	dss.dsi_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
-	dss.dispc_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
-
-	dss_save_context();
-
 	rev = dss_read_reg(DSS_REVISION);
 	printk(KERN_INFO "OMAP DSS rev %d.%d\n",
 			FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
-	return 0;
+	dss_runtime_put();
 
-fail2:
-	free_irq(INT_24XX_DSS_IRQ, NULL);
-fail1:
+	return 0;
+err_sdi:
+	dpi_exit();
+err_dpi:
+	dss_runtime_put();
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	dss_put_clocks();
+err_clocks:
 	iounmap(dss.base);
-fail0:
+err_ioremap:
 	return r;
 }
 
-void dss_exit(void)
+static int omap_dsshw_remove(struct platform_device *pdev)
 {
-	if (cpu_is_omap34xx())
-		clk_put(dss.dpll4_m4_ck);
-
-	free_irq(INT_24XX_DSS_IRQ, NULL);
+	dpi_exit();
+	sdi_exit();
 
 	iounmap(dss.base);
+
+	pm_runtime_disable(&pdev->dev);
+
+	dss_put_clocks();
+
+	return 0;
+}
+
+static int dss_runtime_suspend(struct device *dev)
+{
+	dss_save_context();
+	clk_disable(dss.dss_clk);
+	return 0;
+}
+
+static int dss_runtime_resume(struct device *dev)
+{
+	clk_enable(dss.dss_clk);
+	dss_restore_context();
+	return 0;
+}
+
+static const struct dev_pm_ops dss_pm_ops = {
+	.runtime_suspend = dss_runtime_suspend,
+	.runtime_resume = dss_runtime_resume,
+};
+
+static struct platform_driver omap_dsshw_driver = {
+	.probe          = omap_dsshw_probe,
+	.remove         = omap_dsshw_remove,
+	.driver         = {
+		.name   = "omapdss_dss",
+		.owner  = THIS_MODULE,
+		.pm	= &dss_pm_ops,
+	},
+};
+
+int dss_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dsshw_driver);
 }
 
+void dss_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_dsshw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index b394951120ac..9c94b1152c20 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -97,32 +97,15 @@ extern unsigned int dss_debug;
 #define FLD_MOD(orig, val, start, end) \
 	(((orig) & ~FLD_MASK(start, end)) | FLD_VAL(val, start, end))
 
-#define DISPC_MAX_FCK 173000000
-
-enum omap_burst_size {
-	OMAP_DSS_BURST_4x32 = 0,
-	OMAP_DSS_BURST_8x32 = 1,
-	OMAP_DSS_BURST_16x32 = 2,
-};
-
 enum omap_parallel_interface_mode {
 	OMAP_DSS_PARALLELMODE_BYPASS,		/* MIPI DPI */
 	OMAP_DSS_PARALLELMODE_RFBI,		/* MIPI DBI */
 	OMAP_DSS_PARALLELMODE_DSI,
 };
 
-enum dss_clock {
-	DSS_CLK_ICK	= 1 << 0,
-	DSS_CLK_FCK1	= 1 << 1,
-	DSS_CLK_FCK2	= 1 << 2,
-	DSS_CLK_54M	= 1 << 3,
-	DSS_CLK_96M	= 1 << 4,
-};
-
-enum dss_clk_source {
-	DSS_SRC_DSI1_PLL_FCLK,
-	DSS_SRC_DSI2_PLL_FCLK,
-	DSS_SRC_DSS1_ALWON_FCLK,
+enum dss_hdmi_venc_clk_source_select {
+	DSS_VENC_TV_CLK = 0,
+	DSS_HDMI_M_PCLK = 1,
 };
 
 struct dss_clock_info {
@@ -148,36 +131,42 @@ struct dsi_clock_info {
 	unsigned long fint;
 	unsigned long clkin4ddr;
 	unsigned long clkin;
-	unsigned long dsi1_pll_fclk;
-	unsigned long dsi2_pll_fclk;
-
+	unsigned long dsi_pll_hsdiv_dispc_clk;	/* OMAP3: DSI1_PLL_CLK
+						 * OMAP4: PLLx_CLK1 */
+	unsigned long dsi_pll_hsdiv_dsi_clk;	/* OMAP3: DSI2_PLL_CLK
+						 * OMAP4: PLLx_CLK2 */
 	unsigned long lp_clk;
 
 	/* dividers */
 	u16 regn;
 	u16 regm;
-	u16 regm3;
-	u16 regm4;
-
+	u16 regm_dispc;	/* OMAP3: REGM3
+			 * OMAP4: REGM4 */
+	u16 regm_dsi;	/* OMAP3: REGM4
+			 * OMAP4: REGM5 */
 	u16 lp_clk_div;
 
 	u8 highfreq;
-	bool use_dss2_fck;
+	bool use_sys_clk;
+};
+
+/* HDMI PLL structure */
+struct hdmi_pll_info {
+	u16 regn;
+	u16 regm;
+	u32 regmf;
+	u16 regm2;
+	u16 regsd;
+	u16 dcofreq;
 };
 
 struct seq_file;
 struct platform_device;
 
 /* core */
-void dss_clk_enable(enum dss_clock clks);
-void dss_clk_disable(enum dss_clock clks);
-unsigned long dss_clk_get_rate(enum dss_clock clk);
-int dss_need_ctx_restore(void);
-void dss_dump_clocks(struct seq_file *s);
 struct bus_type *dss_get_bus(void);
 struct regulator *dss_get_vdds_dsi(void);
 struct regulator *dss_get_vdds_sdi(void);
-struct regulator *dss_get_vdda_dac(void);
 
 /* display */
 int dss_suspend_all_devices(void);
@@ -191,7 +180,7 @@ void dss_uninit_device(struct platform_device *pdev,
 bool dss_use_replication(struct omap_dss_device *dssdev,
 		enum omap_color_mode mode);
 void default_get_overlay_fifo_thresholds(enum omap_plane plane,
-		u32 fifo_size, enum omap_burst_size *burst_size,
+		u32 fifo_size, u32 burst_size,
 		u32 *fifo_low, u32 *fifo_high);
 
 /* manager */
@@ -214,22 +203,35 @@ void dss_overlay_setup_l4_manager(struct omap_overlay_manager *mgr);
 void dss_recheck_connections(struct omap_dss_device *dssdev, bool force);
 
 /* DSS */
-int dss_init(bool skip_init);
-void dss_exit(void);
+int dss_init_platform_driver(void);
+void dss_uninit_platform_driver(void);
+
+int dss_runtime_get(void);
+void dss_runtime_put(void);
 
-void dss_save_context(void);
-void dss_restore_context(void);
+struct clk *dss_get_ick(void);
+
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src);
+void dss_dump_clocks(struct seq_file *s);
 
 void dss_dump_regs(struct seq_file *s);
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s);
+#endif
 
 void dss_sdi_init(u8 datapairs);
 int dss_sdi_enable(void);
 void dss_sdi_disable(void);
 
-void dss_select_dispc_clk_source(enum dss_clk_source clk_src);
-void dss_select_dsi_clk_source(enum dss_clk_source clk_src);
-enum dss_clk_source dss_get_dispc_clk_source(void);
-enum dss_clk_source dss_get_dsi_clk_source(void);
+void dss_select_dispc_clk_source(enum omap_dss_clk_source clk_src);
+void dss_select_dsi_clk_source(int dsi_module,
+		enum omap_dss_clk_source clk_src);
+void dss_select_lcd_clk_source(enum omap_channel channel,
+		enum omap_dss_clk_source clk_src);
+enum omap_dss_clk_source dss_get_dispc_clk_source(void);
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module);
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel);
 
 void dss_set_venc_output(enum omap_dss_venc_type type);
 void dss_set_dac_pwrdn_bgz(bool enable);
@@ -244,11 +246,11 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 
 /* SDI */
 #ifdef CONFIG_OMAP2_DSS_SDI
-int sdi_init(bool skip_init);
+int sdi_init(void);
 void sdi_exit(void);
 int sdi_init_display(struct omap_dss_device *display);
 #else
-static inline int sdi_init(bool skip_init)
+static inline int sdi_init(void)
 {
 	return 0;
 }
@@ -259,54 +261,104 @@ static inline void sdi_exit(void)
 
 /* DSI */
 #ifdef CONFIG_OMAP2_DSS_DSI
-int dsi_init(struct platform_device *pdev);
-void dsi_exit(void);
 
-void dsi_dump_clocks(struct seq_file *s);
-void dsi_dump_irqs(struct seq_file *s);
-void dsi_dump_regs(struct seq_file *s);
+struct dentry;
+struct file_operations;
+
+int dsi_init_platform_driver(void);
+void dsi_uninit_platform_driver(void);
+
+int dsi_runtime_get(struct platform_device *dsidev);
+void dsi_runtime_put(struct platform_device *dsidev);
 
-void dsi_save_context(void);
-void dsi_restore_context(void);
+void dsi_dump_clocks(struct seq_file *s);
+void dsi_create_debugfs_files_irq(struct dentry *debugfs_dir,
+		const struct file_operations *debug_fops);
+void dsi_create_debugfs_files_reg(struct dentry *debugfs_dir,
+		const struct file_operations *debug_fops);
 
 int dsi_init_display(struct omap_dss_device *display);
 void dsi_irq_handler(void);
-unsigned long dsi_get_dsi1_pll_rate(void);
-int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo);
-int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
-		struct dsi_clock_info *cinfo,
+unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev);
+int dsi_pll_set_clock_div(struct platform_device *dsidev,
+		struct dsi_clock_info *cinfo);
+int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
+		unsigned long req_pck, struct dsi_clock_info *cinfo,
 		struct dispc_clock_info *dispc_cinfo);
-int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
+int dsi_pll_init(struct platform_device *dsidev, bool enable_hsclk,
 		bool enable_hsdiv);
-void dsi_pll_uninit(void);
+void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes);
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
-		u32 fifo_size, enum omap_burst_size *burst_size,
+		u32 fifo_size, u32 burst_size,
 		u32 *fifo_low, u32 *fifo_high);
-void dsi_wait_dsi1_pll_active(void);
-void dsi_wait_dsi2_pll_active(void);
+void dsi_wait_pll_hsdiv_dispc_active(struct platform_device *dsidev);
+void dsi_wait_pll_hsdiv_dsi_active(struct platform_device *dsidev);
+struct platform_device *dsi_get_dsidev_from_id(int module);
 #else
-static inline int dsi_init(struct platform_device *pdev)
+static inline int dsi_init_platform_driver(void)
+{
+	return 0;
+}
+static inline void dsi_uninit_platform_driver(void)
+{
+}
+static inline int dsi_runtime_get(struct platform_device *dsidev)
 {
 	return 0;
 }
-static inline void dsi_exit(void)
+static inline void dsi_runtime_put(struct platform_device *dsidev)
 {
 }
-static inline void dsi_wait_dsi1_pll_active(void)
+static inline unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev)
+{
+	WARN("%s: DSI not compiled in, returning rate as 0\n", __func__);
+	return 0;
+}
+static inline int dsi_pll_set_clock_div(struct platform_device *dsidev,
+		struct dsi_clock_info *cinfo)
+{
+	WARN("%s: DSI not compiled in\n", __func__);
+	return -ENODEV;
+}
+static inline int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
+		bool is_tft, unsigned long req_pck,
+		struct dsi_clock_info *dsi_cinfo,
+		struct dispc_clock_info *dispc_cinfo)
+{
+	WARN("%s: DSI not compiled in\n", __func__);
+	return -ENODEV;
+}
+static inline int dsi_pll_init(struct platform_device *dsidev,
+		bool enable_hsclk, bool enable_hsdiv)
+{
+	WARN("%s: DSI not compiled in\n", __func__);
+	return -ENODEV;
+}
+static inline void dsi_pll_uninit(struct platform_device *dsidev,
+		bool disconnect_lanes)
+{
+}
+static inline void dsi_wait_pll_hsdiv_dispc_active(struct platform_device *dsidev)
+{
+}
+static inline void dsi_wait_pll_hsdiv_dsi_active(struct platform_device *dsidev)
 {
 }
-static inline void dsi_wait_dsi2_pll_active(void)
+static inline struct platform_device *dsi_get_dsidev_from_id(int module)
 {
+	WARN("%s: DSI not compiled in, returning platform device as NULL\n",
+			__func__);
+	return NULL;
 }
 #endif
 
 /* DPI */
 #ifdef CONFIG_OMAP2_DSS_DPI
-int dpi_init(struct platform_device *pdev);
+int dpi_init(void);
 void dpi_exit(void);
 int dpi_init_display(struct omap_dss_device *dssdev);
 #else
-static inline int dpi_init(struct platform_device *pdev)
+static inline int dpi_init(void)
 {
 	return 0;
 }
@@ -316,16 +368,16 @@ static inline void dpi_exit(void)
 #endif
 
 /* DISPC */
-int dispc_init(void);
-void dispc_exit(void);
+int dispc_init_platform_driver(void);
+void dispc_uninit_platform_driver(void);
 void dispc_dump_clocks(struct seq_file *s);
 void dispc_dump_irqs(struct seq_file *s);
 void dispc_dump_regs(struct seq_file *s);
 void dispc_irq_handler(void);
 void dispc_fake_vsync_irq(void);
 
-void dispc_save_context(void);
-void dispc_restore_context(void);
+int dispc_runtime_get(void);
+void dispc_runtime_put(void);
 
 void dispc_enable_sidle(void);
 void dispc_disable_sidle(void);
@@ -338,10 +390,12 @@ void dispc_enable_fifohandcheck(enum omap_channel channel, bool enable);
 void dispc_set_lcd_size(enum omap_channel channel, u16 width, u16 height);
 void dispc_set_digit_size(u16 width, u16 height);
 u32 dispc_get_plane_fifo_size(enum omap_plane plane);
-void dispc_setup_plane_fifo(enum omap_plane plane, u32 low, u32 high);
+void dispc_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high);
 void dispc_enable_fifomerge(bool enable);
-void dispc_set_burst_size(enum omap_plane plane,
-		enum omap_burst_size burst_size);
+u32 dispc_get_burst_size(enum omap_plane plane);
+void dispc_enable_cpr(enum omap_channel channel, bool enable);
+void dispc_set_cpr_coef(enum omap_channel channel,
+		struct omap_dss_cpr_coefs *coefs);
 
 void dispc_set_plane_ba0(enum omap_plane plane, u32 paddr);
 void dispc_set_plane_ba1(enum omap_plane plane, u32 paddr);
@@ -350,6 +404,7 @@ void dispc_set_plane_size(enum omap_plane plane, u16 width, u16 height);
 void dispc_set_channel_out(enum omap_plane plane,
 		enum omap_channel channel_out);
 
+void dispc_enable_gamma_table(bool enable);
 int dispc_setup_plane(enum omap_plane plane,
 		      u32 paddr, u16 screen_width,
 		      u16 pos_x, u16 pos_y,
@@ -360,7 +415,8 @@ int dispc_setup_plane(enum omap_plane plane,
 		      enum omap_dss_rotation_type rotation_type,
 		      u8 rotation, bool mirror,
 		      u8 global_alpha, u8 pre_mult_alpha,
-		      enum omap_channel channel);
+		      enum omap_channel channel,
+		      u32 puv_addr);
 
 bool dispc_go_busy(enum omap_channel channel);
 void dispc_go(enum omap_channel channel);
@@ -409,39 +465,58 @@ int dispc_get_clock_div(enum omap_channel channel,
 
 /* VENC */
 #ifdef CONFIG_OMAP2_DSS_VENC
-int venc_init(struct platform_device *pdev);
-void venc_exit(void);
+int venc_init_platform_driver(void);
+void venc_uninit_platform_driver(void);
 void venc_dump_regs(struct seq_file *s);
 int venc_init_display(struct omap_dss_device *display);
 #else
-static inline int venc_init(struct platform_device *pdev)
+static inline int venc_init_platform_driver(void)
 {
 	return 0;
 }
-static inline void venc_exit(void)
+static inline void venc_uninit_platform_driver(void)
 {
 }
 #endif
 
+/* HDMI */
+#ifdef CONFIG_OMAP4_DSS_HDMI
+int hdmi_init_platform_driver(void);
+void hdmi_uninit_platform_driver(void);
+int hdmi_init_display(struct omap_dss_device *dssdev);
+#else
+static inline int hdmi_init_display(struct omap_dss_device *dssdev)
+{
+	return 0;
+}
+static inline int hdmi_init_platform_driver(void)
+{
+	return 0;
+}
+static inline void hdmi_uninit_platform_driver(void)
+{
+}
+#endif
+int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev);
+int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
+					struct omap_video_timings *timings);
+int hdmi_panel_init(void);
+void hdmi_panel_exit(void);
+
 /* RFBI */
 #ifdef CONFIG_OMAP2_DSS_RFBI
-int rfbi_init(void);
-void rfbi_exit(void);
+int rfbi_init_platform_driver(void);
+void rfbi_uninit_platform_driver(void);
 void rfbi_dump_regs(struct seq_file *s);
-
-int rfbi_configure(int rfbi_module, int bpp, int lines);
-void rfbi_enable_rfbi(bool enable);
-void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
-		u16 height, void (callback)(void *data), void *data);
-void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t);
-unsigned long rfbi_get_max_tx_rate(void);
 int rfbi_init_display(struct omap_dss_device *display);
 #else
-static inline int rfbi_init(void)
+static inline int rfbi_init_platform_driver(void)
 {
 	return 0;
 }
-static inline void rfbi_exit(void)
+static inline void rfbi_uninit_platform_driver(void)
 {
 }
 #endif
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index cf3ef696e141..b415c4ee621d 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -22,17 +22,21 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
+#include "dss.h"
 #include "dss_features.h"
 
 /* Defines a generic omap register field */
 struct dss_reg_field {
-	enum dss_feat_reg_field id;
 	u8 start, end;
 };
 
+struct dss_param_range {
+	int min, max;
+};
+
 struct omap_dss_features {
 	const struct dss_reg_field *reg_fields;
 	const int num_reg_fields;
@@ -43,29 +47,71 @@ struct omap_dss_features {
 	const int num_ovls;
 	const enum omap_display_type *supported_displays;
 	const enum omap_color_mode *supported_color_modes;
+	const char * const *clksrc_names;
+	const struct dss_param_range *dss_params;
+
+	const u32 buffer_size_unit;
+	const u32 burst_size_unit;
 };
 
 /* This struct is assigned to one of the below during initialization */
-static struct omap_dss_features *omap_current_dss_features;
+static const struct omap_dss_features *omap_current_dss_features;
 
 static const struct dss_reg_field omap2_dss_reg_fields[] = {
-	{ FEAT_REG_FIRHINC, 11, 0 },
-	{ FEAT_REG_FIRVINC, 27, 16 },
-	{ FEAT_REG_FIFOLOWTHRESHOLD, 8, 0 },
-	{ FEAT_REG_FIFOHIGHTHRESHOLD, 24, 16 },
-	{ FEAT_REG_FIFOSIZE, 8, 0 },
+	[FEAT_REG_FIRHINC]			= { 11, 0 },
+	[FEAT_REG_FIRVINC]			= { 27, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 8, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 24, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 8, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 9, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 25, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 0, 0 },
+	[FEAT_REG_DSIPLL_REGN]			= { 0, 0 },
+	[FEAT_REG_DSIPLL_REGM]			= { 0, 0 },
+	[FEAT_REG_DSIPLL_REGM_DISPC]		= { 0, 0 },
+	[FEAT_REG_DSIPLL_REGM_DSI]		= { 0, 0 },
 };
 
 static const struct dss_reg_field omap3_dss_reg_fields[] = {
-	{ FEAT_REG_FIRHINC, 12, 0 },
-	{ FEAT_REG_FIRVINC, 28, 16 },
-	{ FEAT_REG_FIFOLOWTHRESHOLD, 11, 0 },
-	{ FEAT_REG_FIFOHIGHTHRESHOLD, 27, 16 },
-	{ FEAT_REG_FIFOSIZE, 10, 0 },
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 11, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 27, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 10, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 9, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 25, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 0, 0 },
+	[FEAT_REG_DSIPLL_REGN]			= { 7, 1 },
+	[FEAT_REG_DSIPLL_REGM]			= { 18, 8 },
+	[FEAT_REG_DSIPLL_REGM_DISPC]		= { 22, 19 },
+	[FEAT_REG_DSIPLL_REGM_DSI]		= { 26, 23 },
+};
+
+static const struct dss_reg_field omap4_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 15, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 31, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 15, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 10, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 26, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 9, 8 },
+	[FEAT_REG_DSIPLL_REGN]			= { 8, 1 },
+	[FEAT_REG_DSIPLL_REGM]			= { 20, 9 },
+	[FEAT_REG_DSIPLL_REGM_DISPC]		= { 25, 21 },
+	[FEAT_REG_DSIPLL_REGM_DSI]		= { 30, 26 },
 };
 
 static const enum omap_display_type omap2_dss_supported_displays[] = {
 	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3430_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
 	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
 	OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
 
@@ -73,10 +119,10 @@ static const enum omap_display_type omap2_dss_supported_displays[] = {
 	OMAP_DISPLAY_TYPE_VENC,
 };
 
-static const enum omap_display_type omap3_dss_supported_displays[] = {
+static const enum omap_display_type omap3630_dss_supported_displays[] = {
 	/* OMAP_DSS_CHANNEL_LCD */
 	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
-	OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
+	OMAP_DISPLAY_TYPE_DSI,
 
 	/* OMAP_DSS_CHANNEL_DIGIT */
 	OMAP_DISPLAY_TYPE_VENC,
@@ -87,7 +133,7 @@ static const enum omap_display_type omap4_dss_supported_displays[] = {
 	OMAP_DISPLAY_TYPE_DBI | OMAP_DISPLAY_TYPE_DSI,
 
 	/* OMAP_DSS_CHANNEL_DIGIT */
-	OMAP_DISPLAY_TYPE_VENC,
+	OMAP_DISPLAY_TYPE_VENC | OMAP_DISPLAY_TYPE_HDMI,
 
 	/* OMAP_DSS_CHANNEL_LCD2 */
 	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
@@ -134,65 +180,200 @@ static const enum omap_color_mode omap3_dss_supported_color_modes[] = {
 	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32,
 };
 
+static const enum omap_color_mode omap4_dss_supported_color_modes[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
+	OMAP_DSS_COLOR_CLUT4 | OMAP_DSS_COLOR_CLUT8 |
+	OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_ARGB16 |
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_ARGB32 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32 |
+	OMAP_DSS_COLOR_ARGB16_1555,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
+
+       /* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
+};
+
+static const char * const omap2_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "N/A",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "N/A",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_FCLK1",
+};
+
+static const char * const omap3_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "DSI1_PLL_FCLK",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "DSI2_PLL_FCLK",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS1_ALWON_FCLK",
+};
+
+static const char * const omap4_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "PLL1_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "PLL1_CLK2",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_FCLK",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC]	= "PLL2_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "PLL2_CLK2",
+};
+
+static const struct dss_param_range omap2_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 173000000 },
+	[FEAT_PARAM_DSIPLL_REGN]		= { 0, 0 },
+	[FEAT_PARAM_DSIPLL_REGM]		= { 0, 0 },
+	[FEAT_PARAM_DSIPLL_REGM_DISPC]		= { 0, 0 },
+	[FEAT_PARAM_DSIPLL_REGM_DSI]		= { 0, 0 },
+	[FEAT_PARAM_DSIPLL_FINT]		= { 0, 0 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, 0 },
+};
+
+static const struct dss_param_range omap3_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 173000000 },
+	[FEAT_PARAM_DSIPLL_REGN]		= { 0, (1 << 7) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM]		= { 0, (1 << 11) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM_DISPC]		= { 0, (1 << 4) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM_DSI]		= { 0, (1 << 4) - 1 },
+	[FEAT_PARAM_DSIPLL_FINT]		= { 750000, 2100000 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 1, (1 << 13) - 1},
+};
+
+static const struct dss_param_range omap4_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 186000000 },
+	[FEAT_PARAM_DSIPLL_REGN]		= { 0, (1 << 8) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM]		= { 0, (1 << 12) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM_DISPC]		= { 0, (1 << 5) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM_DSI]		= { 0, (1 << 5) - 1 },
+	[FEAT_PARAM_DSIPLL_FINT]		= { 500000, 2500000 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, (1 << 13) - 1 },
+};
+
 /* OMAP2 DSS Features */
-static struct omap_dss_features omap2_dss_features = {
+static const struct omap_dss_features omap2_dss_features = {
 	.reg_fields = omap2_dss_reg_fields,
 	.num_reg_fields = ARRAY_SIZE(omap2_dss_reg_fields),
 
 	.has_feature	=
 		FEAT_LCDENABLEPOL | FEAT_LCDENABLESIGNAL |
-		FEAT_PCKFREEENABLE | FEAT_FUNCGATED,
+		FEAT_PCKFREEENABLE | FEAT_FUNCGATED |
+		FEAT_ROWREPEATENABLE | FEAT_RESIZECONF,
 
 	.num_mgrs = 2,
 	.num_ovls = 3,
 	.supported_displays = omap2_dss_supported_displays,
 	.supported_color_modes = omap2_dss_supported_color_modes,
+	.clksrc_names = omap2_dss_clk_source_names,
+	.dss_params = omap2_dss_param_range,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
 };
 
 /* OMAP3 DSS Features */
-static struct omap_dss_features omap3430_dss_features = {
+static const struct omap_dss_features omap3430_dss_features = {
 	.reg_fields = omap3_dss_reg_fields,
 	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
 
 	.has_feature	=
 		FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
 		FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
-		FEAT_FUNCGATED,
+		FEAT_FUNCGATED | FEAT_ROWREPEATENABLE |
+		FEAT_LINEBUFFERSPLIT | FEAT_RESIZECONF |
+		FEAT_DSI_PLL_FREQSEL | FEAT_DSI_REVERSE_TXCLKESC |
+		FEAT_VENC_REQUIRES_TV_DAC_CLK | FEAT_CPR | FEAT_PRELOAD |
+		FEAT_FIR_COEF_V,
 
 	.num_mgrs = 2,
 	.num_ovls = 3,
-	.supported_displays = omap3_dss_supported_displays,
+	.supported_displays = omap3430_dss_supported_displays,
 	.supported_color_modes = omap3_dss_supported_color_modes,
+	.clksrc_names = omap3_dss_clk_source_names,
+	.dss_params = omap3_dss_param_range,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
 };
 
-static struct omap_dss_features omap3630_dss_features = {
+static const struct omap_dss_features omap3630_dss_features = {
 	.reg_fields = omap3_dss_reg_fields,
 	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
 
 	.has_feature    =
 		FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
 		FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
-		FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED,
+		FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED |
+		FEAT_ROWREPEATENABLE | FEAT_LINEBUFFERSPLIT |
+		FEAT_RESIZECONF | FEAT_DSI_PLL_PWR_BUG |
+		FEAT_DSI_PLL_FREQSEL | FEAT_CPR | FEAT_PRELOAD |
+		FEAT_FIR_COEF_V,
 
 	.num_mgrs = 2,
 	.num_ovls = 3,
-	.supported_displays = omap3_dss_supported_displays,
+	.supported_displays = omap3630_dss_supported_displays,
 	.supported_color_modes = omap3_dss_supported_color_modes,
+	.clksrc_names = omap3_dss_clk_source_names,
+	.dss_params = omap3_dss_param_range,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
 };
 
 /* OMAP4 DSS Features */
-static struct omap_dss_features omap4_dss_features = {
-	.reg_fields = omap3_dss_reg_fields,
-	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+/* For OMAP4430 ES 1.0 revision */
+static const struct omap_dss_features omap4430_es1_0_dss_features  = {
+	.reg_fields = omap4_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
 
 	.has_feature	=
 		FEAT_GLOBAL_ALPHA | FEAT_PRE_MULT_ALPHA |
-		FEAT_MGR_LCD2,
+		FEAT_MGR_LCD2 | FEAT_GLOBAL_ALPHA_VID1 |
+		FEAT_CORE_CLK_DIV | FEAT_LCD_CLK_SRC |
+		FEAT_DSI_DCS_CMD_CONFIG_VC | FEAT_DSI_VC_OCP_WIDTH |
+		FEAT_DSI_GNQ | FEAT_HANDLE_UV_SEPARATE | FEAT_ATTR2 |
+		FEAT_CPR | FEAT_PRELOAD | FEAT_FIR_COEF_V,
 
 	.num_mgrs = 3,
 	.num_ovls = 3,
 	.supported_displays = omap4_dss_supported_displays,
-	.supported_color_modes = omap3_dss_supported_color_modes,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.clksrc_names = omap4_dss_clk_source_names,
+	.dss_params = omap4_dss_param_range,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
+};
+
+/* For all the other OMAP4 versions */
+static const struct omap_dss_features omap4_dss_features = {
+	.reg_fields = omap4_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+	.has_feature	=
+		FEAT_GLOBAL_ALPHA | FEAT_PRE_MULT_ALPHA |
+		FEAT_MGR_LCD2 | FEAT_GLOBAL_ALPHA_VID1 |
+		FEAT_CORE_CLK_DIV | FEAT_LCD_CLK_SRC |
+		FEAT_DSI_DCS_CMD_CONFIG_VC | FEAT_DSI_VC_OCP_WIDTH |
+		FEAT_DSI_GNQ | FEAT_HDMI_CTS_SWMODE |
+		FEAT_HANDLE_UV_SEPARATE | FEAT_ATTR2 | FEAT_CPR |
+		FEAT_PRELOAD | FEAT_FIR_COEF_V,
+
+	.num_mgrs = 3,
+	.num_ovls = 3,
+	.supported_displays = omap4_dss_supported_displays,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.clksrc_names = omap4_dss_clk_source_names,
+	.dss_params = omap4_dss_param_range,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
 };
 
 /* Functions returning values related to a DSS feature */
@@ -206,6 +387,16 @@ int dss_feat_get_num_ovls(void)
 	return omap_current_dss_features->num_ovls;
 }
 
+unsigned long dss_feat_get_param_min(enum dss_range_param param)
+{
+	return omap_current_dss_features->dss_params[param].min;
+}
+
+unsigned long dss_feat_get_param_max(enum dss_range_param param)
+{
+	return omap_current_dss_features->dss_params[param].max;
+}
+
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel)
 {
 	return omap_current_dss_features->supported_displays[channel];
@@ -223,6 +414,21 @@ bool dss_feat_color_mode_supported(enum omap_plane plane,
 			color_mode;
 }
 
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id)
+{
+	return omap_current_dss_features->clksrc_names[id];
+}
+
+u32 dss_feat_get_buffer_size_unit(void)
+{
+	return omap_current_dss_features->buffer_size_unit;
+}
+
+u32 dss_feat_get_burst_size_unit(void)
+{
+	return omap_current_dss_features->burst_size_unit;
+}
+
 /* DSS has_feature check */
 bool dss_has_feature(enum dss_feat_id id)
 {
@@ -246,6 +452,10 @@ void dss_features_init(void)
 		omap_current_dss_features = &omap3630_dss_features;
 	else if (cpu_is_omap34xx())
 		omap_current_dss_features = &omap3430_dss_features;
-	else
+	else if (omap_rev() == OMAP4430_REV_ES1_0)
+		omap_current_dss_features = &omap4430_es1_0_dss_features;
+	else if (cpu_is_omap44xx())
 		omap_current_dss_features = &omap4_dss_features;
+	else
+		DSSWARN("Unsupported OMAP version");
 }
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
index b9c70be92588..b7398cbcda5f 100644
--- a/drivers/video/omap2/dss/dss_features.h
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -22,17 +22,39 @@
 
 #define MAX_DSS_MANAGERS	3
 #define MAX_DSS_OVERLAYS	3
+#define MAX_DSS_LCD_MANAGERS	2
+#define MAX_NUM_DSI		2
 
 /* DSS has feature id */
 enum dss_feat_id {
-	FEAT_GLOBAL_ALPHA	= 1 << 0,
-	FEAT_GLOBAL_ALPHA_VID1	= 1 << 1,
-	FEAT_PRE_MULT_ALPHA	= 1 << 2,
-	FEAT_LCDENABLEPOL	= 1 << 3,
-	FEAT_LCDENABLESIGNAL	= 1 << 4,
-	FEAT_PCKFREEENABLE	= 1 << 5,
-	FEAT_FUNCGATED		= 1 << 6,
-	FEAT_MGR_LCD2		= 1 << 7,
+	FEAT_GLOBAL_ALPHA		= 1 << 0,
+	FEAT_GLOBAL_ALPHA_VID1		= 1 << 1,
+	FEAT_PRE_MULT_ALPHA		= 1 << 2,
+	FEAT_LCDENABLEPOL		= 1 << 3,
+	FEAT_LCDENABLESIGNAL		= 1 << 4,
+	FEAT_PCKFREEENABLE		= 1 << 5,
+	FEAT_FUNCGATED			= 1 << 6,
+	FEAT_MGR_LCD2			= 1 << 7,
+	FEAT_LINEBUFFERSPLIT		= 1 << 8,
+	FEAT_ROWREPEATENABLE		= 1 << 9,
+	FEAT_RESIZECONF			= 1 << 10,
+	/* Independent core clk divider */
+	FEAT_CORE_CLK_DIV		= 1 << 11,
+	FEAT_LCD_CLK_SRC		= 1 << 12,
+	/* DSI-PLL power command 0x3 is not working */
+	FEAT_DSI_PLL_PWR_BUG		= 1 << 13,
+	FEAT_DSI_PLL_FREQSEL		= 1 << 14,
+	FEAT_DSI_DCS_CMD_CONFIG_VC	= 1 << 15,
+	FEAT_DSI_VC_OCP_WIDTH		= 1 << 16,
+	FEAT_DSI_REVERSE_TXCLKESC	= 1 << 17,
+	FEAT_DSI_GNQ			= 1 << 18,
+	FEAT_HDMI_CTS_SWMODE		= 1 << 19,
+	FEAT_HANDLE_UV_SEPARATE         = 1 << 20,
+	FEAT_ATTR2                      = 1 << 21,
+	FEAT_VENC_REQUIRES_TV_DAC_CLK	= 1 << 22,
+	FEAT_CPR			= 1 << 23,
+	FEAT_PRELOAD			= 1 << 24,
+	FEAT_FIR_COEF_V			= 1 << 25,
 };
 
 /* DSS register field id */
@@ -42,15 +64,38 @@ enum dss_feat_reg_field {
 	FEAT_REG_FIFOHIGHTHRESHOLD,
 	FEAT_REG_FIFOLOWTHRESHOLD,
 	FEAT_REG_FIFOSIZE,
+	FEAT_REG_HORIZONTALACCU,
+	FEAT_REG_VERTICALACCU,
+	FEAT_REG_DISPC_CLK_SWITCH,
+	FEAT_REG_DSIPLL_REGN,
+	FEAT_REG_DSIPLL_REGM,
+	FEAT_REG_DSIPLL_REGM_DISPC,
+	FEAT_REG_DSIPLL_REGM_DSI,
+};
+
+enum dss_range_param {
+	FEAT_PARAM_DSS_FCK,
+	FEAT_PARAM_DSIPLL_REGN,
+	FEAT_PARAM_DSIPLL_REGM,
+	FEAT_PARAM_DSIPLL_REGM_DISPC,
+	FEAT_PARAM_DSIPLL_REGM_DSI,
+	FEAT_PARAM_DSIPLL_FINT,
+	FEAT_PARAM_DSIPLL_LPDIV,
 };
 
 /* DSS Feature Functions */
 int dss_feat_get_num_mgrs(void);
 int dss_feat_get_num_ovls(void);
+unsigned long dss_feat_get_param_min(enum dss_range_param param);
+unsigned long dss_feat_get_param_max(enum dss_range_param param);
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane);
 bool dss_feat_color_mode_supported(enum omap_plane plane,
 		enum omap_color_mode color_mode);
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id);
+
+u32 dss_feat_get_buffer_size_unit(void);	/* in bytes */
+u32 dss_feat_get_burst_size_unit(void);		/* in bytes */
 
 bool dss_has_feature(enum dss_feat_id id);
 void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end);
diff --git a/drivers/video/omap2/dss/hdmi.c b/drivers/video/omap2/dss/hdmi.c
new file mode 100644
index 000000000000..256f27a9064a
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -0,0 +1,1845 @@
+/*
+ * hdmi.c
+ *
+ * HDMI interface DSS driver setting for TI's OMAP4 family of processor.
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors: Yong Zhi
+ *	Mythri pk <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <video/omapdss.h>
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+	defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+#include <sound/soc.h>
+#include <sound/pcm_params.h>
+#endif
+
+#include "dss.h"
+#include "hdmi.h"
+#include "dss_features.h"
+
+static struct {
+	struct mutex lock;
+	struct omap_display_platform_data *pdata;
+	struct platform_device *pdev;
+	void __iomem *base_wp;	/* HDMI wrapper */
+	int code;
+	int mode;
+	u8 edid[HDMI_EDID_MAX_LENGTH];
+	u8 edid_set;
+	bool custom_set;
+	struct hdmi_config cfg;
+
+	struct clk *sys_clk;
+	struct clk *hdmi_clk;
+} hdmi;
+
+/*
+ * Logic for the below structure :
+ * user enters the CEA or VESA timings by specifying the HDMI/DVI code.
+ * There is a correspondence between CEA/VESA timing and code, please
+ * refer to section 6.3 in HDMI 1.3 specification for timing code.
+ *
+ * In the below structure, cea_vesa_timings corresponds to all OMAP4
+ * supported CEA and VESA timing values.code_cea corresponds to the CEA
+ * code, It is used to get the timing from cea_vesa_timing array.Similarly
+ * with code_vesa. Code_index is used for back mapping, that is once EDID
+ * is read from the TV, EDID is parsed to find the timing values and then
+ * map it to corresponding CEA or VESA index.
+ */
+
+static const struct hdmi_timings cea_vesa_timings[OMAP_HDMI_TIMINGS_NB] = {
+	{ {640, 480, 25200, 96, 16, 48, 2, 10, 33} , 0 , 0},
+	{ {1280, 720, 74250, 40, 440, 220, 5, 5, 20}, 1, 1},
+	{ {1280, 720, 74250, 40, 110, 220, 5, 5, 20}, 1, 1},
+	{ {720, 480, 27027, 62, 16, 60, 6, 9, 30}, 0, 0},
+	{ {2880, 576, 108000, 256, 48, 272, 5, 5, 39}, 0, 0},
+	{ {1440, 240, 27027, 124, 38, 114, 3, 4, 15}, 0, 0},
+	{ {1440, 288, 27000, 126, 24, 138, 3, 2, 19}, 0, 0},
+	{ {1920, 540, 74250, 44, 528, 148, 5, 2, 15}, 1, 1},
+	{ {1920, 540, 74250, 44, 88, 148, 5, 2, 15}, 1, 1},
+	{ {1920, 1080, 148500, 44, 88, 148, 5, 4, 36}, 1, 1},
+	{ {720, 576, 27000, 64, 12, 68, 5, 5, 39}, 0, 0},
+	{ {1440, 576, 54000, 128, 24, 136, 5, 5, 39}, 0, 0},
+	{ {1920, 1080, 148500, 44, 528, 148, 5, 4, 36}, 1, 1},
+	{ {2880, 480, 108108, 248, 64, 240, 6, 9, 30}, 0, 0},
+	{ {1920, 1080, 74250, 44, 638, 148, 5, 4, 36}, 1, 1},
+	/* VESA From Here */
+	{ {640, 480, 25175, 96, 16, 48, 2 , 11, 31}, 0, 0},
+	{ {800, 600, 40000, 128, 40, 88, 4 , 1, 23}, 1, 1},
+	{ {848, 480, 33750, 112, 16, 112, 8 , 6, 23}, 1, 1},
+	{ {1280, 768, 79500, 128, 64, 192, 7 , 3, 20}, 1, 0},
+	{ {1280, 800, 83500, 128, 72, 200, 6 , 3, 22}, 1, 0},
+	{ {1360, 768, 85500, 112, 64, 256, 6 , 3, 18}, 1, 1},
+	{ {1280, 960, 108000, 112, 96, 312, 3 , 1, 36}, 1, 1},
+	{ {1280, 1024, 108000, 112, 48, 248, 3 , 1, 38}, 1, 1},
+	{ {1024, 768, 65000, 136, 24, 160, 6, 3, 29}, 0, 0},
+	{ {1400, 1050, 121750, 144, 88, 232, 4, 3, 32}, 1, 0},
+	{ {1440, 900, 106500, 152, 80, 232, 6, 3, 25}, 1, 0},
+	{ {1680, 1050, 146250, 176 , 104, 280, 6, 3, 30}, 1, 0},
+	{ {1366, 768, 85500, 143, 70, 213, 3, 3, 24}, 1, 1},
+	{ {1920, 1080, 148500, 44, 148, 80, 5, 4, 36}, 1, 1},
+	{ {1280, 768, 68250, 32, 48, 80, 7, 3, 12}, 0, 1},
+	{ {1400, 1050, 101000, 32, 48, 80, 4, 3, 23}, 0, 1},
+	{ {1680, 1050, 119000, 32, 48, 80, 6, 3, 21}, 0, 1},
+	{ {1280, 800, 79500, 32, 48, 80, 6, 3, 14}, 0, 1},
+	{ {1280, 720, 74250, 40, 110, 220, 5, 5, 20}, 1, 1}
+};
+
+/*
+ * This is a static mapping array which maps the timing values
+ * with corresponding CEA / VESA code
+ */
+static const int code_index[OMAP_HDMI_TIMINGS_NB] = {
+	1, 19, 4, 2, 37, 6, 21, 20, 5, 16, 17, 29, 31, 35, 32,
+	/* <--15 CEA 17--> vesa*/
+	4, 9, 0xE, 0x17, 0x1C, 0x27, 0x20, 0x23, 0x10, 0x2A,
+	0X2F, 0x3A, 0X51, 0X52, 0x16, 0x29, 0x39, 0x1B
+};
+
+/*
+ * This is reverse static mapping which maps the CEA / VESA code
+ * to the corresponding timing values
+ */
+static const int code_cea[39] = {
+	-1,  0,  3,  3,  2,  8,  5,  5, -1, -1,
+	-1, -1, -1, -1, -1, -1,  9, 10, 10,  1,
+	7,   6,  6, -1, -1, -1, -1, -1, -1, 11,
+	11, 12, 14, -1, -1, 13, 13,  4,  4
+};
+
+static const int code_vesa[85] = {
+	-1, -1, -1, -1, 15, -1, -1, -1, -1, 16,
+	-1, -1, -1, -1, 17, -1, 23, -1, -1, -1,
+	-1, -1, 29, 18, -1, -1, -1, 32, 19, -1,
+	-1, -1, 21, -1, -1, 22, -1, -1, -1, 20,
+	-1, 30, 24, -1, -1, -1, -1, 25, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, 31, 26, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+	-1, 27, 28, -1, 33};
+
+static const u8 edid_header[8] = {0x0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0};
+
+static inline void hdmi_write_reg(const struct hdmi_reg idx, u32 val)
+{
+	__raw_writel(val, hdmi.base_wp + idx.idx);
+}
+
+static inline u32 hdmi_read_reg(const struct hdmi_reg idx)
+{
+	return __raw_readl(hdmi.base_wp + idx.idx);
+}
+
+static inline int hdmi_wait_for_bit_change(const struct hdmi_reg idx,
+				int b2, int b1, u32 val)
+{
+	u32 t = 0;
+	while (val != REG_GET(idx, b2, b1)) {
+		udelay(1);
+		if (t++ > 10000)
+			return !val;
+	}
+	return val;
+}
+
+static int hdmi_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+static void hdmi_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_put\n");
+
+	r = pm_runtime_put(&hdmi.pdev->dev);
+	WARN_ON(r < 0);
+}
+
+int hdmi_init_display(struct omap_dss_device *dssdev)
+{
+	DSSDBG("init_display\n");
+
+	return 0;
+}
+
+static int hdmi_pll_init(enum hdmi_clk_refsel refsel, int dcofreq,
+		struct hdmi_pll_info *fmt, u16 sd)
+{
+	u32 r;
+
+	/* PLL start always use manual mode */
+	REG_FLD_MOD(PLLCTRL_PLL_CONTROL, 0x0, 0, 0);
+
+	r = hdmi_read_reg(PLLCTRL_CFG1);
+	r = FLD_MOD(r, fmt->regm, 20, 9); /* CFG1_PLL_REGM */
+	r = FLD_MOD(r, fmt->regn, 8, 1);  /* CFG1_PLL_REGN */
+
+	hdmi_write_reg(PLLCTRL_CFG1, r);
+
+	r = hdmi_read_reg(PLLCTRL_CFG2);
+
+	r = FLD_MOD(r, 0x0, 12, 12); /* PLL_HIGHFREQ divide by 2 */
+	r = FLD_MOD(r, 0x1, 13, 13); /* PLL_REFEN */
+	r = FLD_MOD(r, 0x0, 14, 14); /* PHY_CLKINEN de-assert during locking */
+
+	if (dcofreq) {
+		/* divider programming for frequency beyond 1000Mhz */
+		REG_FLD_MOD(PLLCTRL_CFG3, sd, 17, 10);
+		r = FLD_MOD(r, 0x4, 3, 1); /* 1000MHz and 2000MHz */
+	} else {
+		r = FLD_MOD(r, 0x2, 3, 1); /* 500MHz and 1000MHz */
+	}
+
+	hdmi_write_reg(PLLCTRL_CFG2, r);
+
+	r = hdmi_read_reg(PLLCTRL_CFG4);
+	r = FLD_MOD(r, fmt->regm2, 24, 18);
+	r = FLD_MOD(r, fmt->regmf, 17, 0);
+
+	hdmi_write_reg(PLLCTRL_CFG4, r);
+
+	/* go now */
+	REG_FLD_MOD(PLLCTRL_PLL_GO, 0x1, 0, 0);
+
+	/* wait for bit change */
+	if (hdmi_wait_for_bit_change(PLLCTRL_PLL_GO, 0, 0, 1) != 1) {
+		DSSERR("PLL GO bit not set\n");
+		return -ETIMEDOUT;
+	}
+
+	/* Wait till the lock bit is set in PLL status */
+	if (hdmi_wait_for_bit_change(PLLCTRL_PLL_STATUS, 1, 1, 1) != 1) {
+		DSSWARN("cannot lock PLL\n");
+		DSSWARN("CFG1 0x%x\n",
+			hdmi_read_reg(PLLCTRL_CFG1));
+		DSSWARN("CFG2 0x%x\n",
+			hdmi_read_reg(PLLCTRL_CFG2));
+		DSSWARN("CFG4 0x%x\n",
+			hdmi_read_reg(PLLCTRL_CFG4));
+		return -ETIMEDOUT;
+	}
+
+	DSSDBG("PLL locked!\n");
+
+	return 0;
+}
+
+/* PHY_PWR_CMD */
+static int hdmi_set_phy_pwr(enum hdmi_phy_pwr val)
+{
+	/* Command for power control of HDMI PHY */
+	REG_FLD_MOD(HDMI_WP_PWR_CTRL, val, 7, 6);
+
+	/* Status of the power control of HDMI PHY */
+	if (hdmi_wait_for_bit_change(HDMI_WP_PWR_CTRL, 5, 4, val) != val) {
+		DSSERR("Failed to set PHY power mode to %d\n", val);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+/* PLL_PWR_CMD */
+static int hdmi_set_pll_pwr(enum hdmi_pll_pwr val)
+{
+	/* Command for power control of HDMI PLL */
+	REG_FLD_MOD(HDMI_WP_PWR_CTRL, val, 3, 2);
+
+	/* wait till PHY_PWR_STATUS is set */
+	if (hdmi_wait_for_bit_change(HDMI_WP_PWR_CTRL, 1, 0, val) != val) {
+		DSSERR("Failed to set PHY_PWR_STATUS\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int hdmi_pll_reset(void)
+{
+	/* SYSRESET  controlled by power FSM */
+	REG_FLD_MOD(PLLCTRL_PLL_CONTROL, 0x0, 3, 3);
+
+	/* READ 0x0 reset is in progress */
+	if (hdmi_wait_for_bit_change(PLLCTRL_PLL_STATUS, 0, 0, 1) != 1) {
+		DSSERR("Failed to sysreset PLL\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int hdmi_phy_init(void)
+{
+	u16 r = 0;
+
+	r = hdmi_set_phy_pwr(HDMI_PHYPWRCMD_LDOON);
+	if (r)
+		return r;
+
+	r = hdmi_set_phy_pwr(HDMI_PHYPWRCMD_TXON);
+	if (r)
+		return r;
+
+	/*
+	 * Read address 0 in order to get the SCP reset done completed
+	 * Dummy access performed to make sure reset is done
+	 */
+	hdmi_read_reg(HDMI_TXPHY_TX_CTRL);
+
+	/*
+	 * Write to phy address 0 to configure the clock
+	 * use HFBITCLK write HDMI_TXPHY_TX_CONTROL_FREQOUT field
+	 */
+	REG_FLD_MOD(HDMI_TXPHY_TX_CTRL, 0x1, 31, 30);
+
+	/* Write to phy address 1 to start HDMI line (TXVALID and TMDSCLKEN) */
+	hdmi_write_reg(HDMI_TXPHY_DIGITAL_CTRL, 0xF0000000);
+
+	/* Setup max LDO voltage */
+	REG_FLD_MOD(HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
+
+	/* Write to phy address 3 to change the polarity control */
+	REG_FLD_MOD(HDMI_TXPHY_PAD_CFG_CTRL, 0x1, 27, 27);
+
+	return 0;
+}
+
+static int hdmi_pll_program(struct hdmi_pll_info *fmt)
+{
+	u16 r = 0;
+	enum hdmi_clk_refsel refsel;
+
+	r = hdmi_set_pll_pwr(HDMI_PLLPWRCMD_ALLOFF);
+	if (r)
+		return r;
+
+	r = hdmi_set_pll_pwr(HDMI_PLLPWRCMD_BOTHON_ALLCLKS);
+	if (r)
+		return r;
+
+	r = hdmi_pll_reset();
+	if (r)
+		return r;
+
+	refsel = HDMI_REFSEL_SYSCLK;
+
+	r = hdmi_pll_init(refsel, fmt->dcofreq, fmt, fmt->regsd);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void hdmi_phy_off(void)
+{
+	hdmi_set_phy_pwr(HDMI_PHYPWRCMD_OFF);
+}
+
+static int hdmi_core_ddc_edid(u8 *pedid, int ext)
+{
+	u32 i, j;
+	char checksum = 0;
+	u32 offset = 0;
+
+	/* Turn on CLK for DDC */
+	REG_FLD_MOD(HDMI_CORE_AV_DPD, 0x7, 2, 0);
+
+	/*
+	 * SW HACK : Without the Delay DDC(i2c bus) reads 0 values /
+	 * right shifted values( The behavior is not consistent and seen only
+	 * with some TV's)
+	 */
+	usleep_range(800, 1000);
+
+	if (!ext) {
+		/* Clk SCL Devices */
+		REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0xA, 3, 0);
+
+		/* HDMI_CORE_DDC_STATUS_IN_PROG */
+		if (hdmi_wait_for_bit_change(HDMI_CORE_DDC_STATUS,
+						4, 4, 0) != 0) {
+			DSSERR("Failed to program DDC\n");
+			return -ETIMEDOUT;
+		}
+
+		/* Clear FIFO */
+		REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x9, 3, 0);
+
+		/* HDMI_CORE_DDC_STATUS_IN_PROG */
+		if (hdmi_wait_for_bit_change(HDMI_CORE_DDC_STATUS,
+						4, 4, 0) != 0) {
+			DSSERR("Failed to program DDC\n");
+			return -ETIMEDOUT;
+		}
+
+	} else {
+		if (ext % 2 != 0)
+			offset = 0x80;
+	}
+
+	/* Load Segment Address Register */
+	REG_FLD_MOD(HDMI_CORE_DDC_SEGM, ext/2, 7, 0);
+
+	/* Load Slave Address Register */
+	REG_FLD_MOD(HDMI_CORE_DDC_ADDR, 0xA0 >> 1, 7, 1);
+
+	/* Load Offset Address Register */
+	REG_FLD_MOD(HDMI_CORE_DDC_OFFSET, offset, 7, 0);
+
+	/* Load Byte Count */
+	REG_FLD_MOD(HDMI_CORE_DDC_COUNT1, 0x80, 7, 0);
+	REG_FLD_MOD(HDMI_CORE_DDC_COUNT2, 0x0, 1, 0);
+
+	/* Set DDC_CMD */
+	if (ext)
+		REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x4, 3, 0);
+	else
+		REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x2, 3, 0);
+
+	/* HDMI_CORE_DDC_STATUS_BUS_LOW */
+	if (REG_GET(HDMI_CORE_DDC_STATUS, 6, 6) == 1) {
+		DSSWARN("I2C Bus Low?\n");
+		return -EIO;
+	}
+	/* HDMI_CORE_DDC_STATUS_NO_ACK */
+	if (REG_GET(HDMI_CORE_DDC_STATUS, 5, 5) == 1) {
+		DSSWARN("I2C No Ack\n");
+		return -EIO;
+	}
+
+	i = ext * 128;
+	j = 0;
+	while (((REG_GET(HDMI_CORE_DDC_STATUS, 4, 4) == 1) ||
+			(REG_GET(HDMI_CORE_DDC_STATUS, 2, 2) == 0)) &&
+			j < 128) {
+
+		if (REG_GET(HDMI_CORE_DDC_STATUS, 2, 2) == 0) {
+			/* FIFO not empty */
+			pedid[i++] = REG_GET(HDMI_CORE_DDC_DATA, 7, 0);
+			j++;
+		}
+	}
+
+	for (j = 0; j < 128; j++)
+		checksum += pedid[j];
+
+	if (checksum != 0) {
+		DSSERR("E-EDID checksum failed!!\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int read_edid(u8 *pedid, u16 max_length)
+{
+	int r = 0, n = 0, i = 0;
+	int max_ext_blocks = (max_length / 128) - 1;
+
+	r = hdmi_core_ddc_edid(pedid, 0);
+	if (r) {
+		return r;
+	} else {
+		n = pedid[0x7e];
+
+		/*
+		 * README: need to comply with max_length set by the caller.
+		 * Better implementation should be to allocate necessary
+		 * memory to store EDID according to nb_block field found
+		 * in first block
+		 */
+		if (n > max_ext_blocks)
+			n = max_ext_blocks;
+
+		for (i = 1; i <= n; i++) {
+			r = hdmi_core_ddc_edid(pedid, i);
+			if (r)
+				return r;
+		}
+	}
+	return 0;
+}
+
+static int get_timings_index(void)
+{
+	int code;
+
+	if (hdmi.mode == 0)
+		code = code_vesa[hdmi.code];
+	else
+		code = code_cea[hdmi.code];
+
+	if (code == -1)	{
+		/* HDMI code 4 corresponds to 640 * 480 VGA */
+		hdmi.code = 4;
+		/* DVI mode 1 corresponds to HDMI 0 to DVI */
+		hdmi.mode = HDMI_DVI;
+
+		code = code_vesa[hdmi.code];
+	}
+	return code;
+}
+
+static struct hdmi_cm hdmi_get_code(struct omap_video_timings *timing)
+{
+	int i = 0, code = -1, temp_vsync = 0, temp_hsync = 0;
+	int timing_vsync = 0, timing_hsync = 0;
+	struct omap_video_timings temp;
+	struct hdmi_cm cm = {-1};
+	DSSDBG("hdmi_get_code\n");
+
+	for (i = 0; i < OMAP_HDMI_TIMINGS_NB; i++) {
+		temp = cea_vesa_timings[i].timings;
+		if ((temp.pixel_clock == timing->pixel_clock) &&
+			(temp.x_res == timing->x_res) &&
+			(temp.y_res == timing->y_res)) {
+
+			temp_hsync = temp.hfp + temp.hsw + temp.hbp;
+			timing_hsync = timing->hfp + timing->hsw + timing->hbp;
+			temp_vsync = temp.vfp + temp.vsw + temp.vbp;
+			timing_vsync = timing->vfp + timing->vsw + timing->vbp;
+
+			DSSDBG("temp_hsync = %d , temp_vsync = %d"
+				"timing_hsync = %d, timing_vsync = %d\n",
+				temp_hsync, temp_hsync,
+				timing_hsync, timing_vsync);
+
+			if ((temp_hsync == timing_hsync) &&
+					(temp_vsync == timing_vsync)) {
+				code = i;
+				cm.code = code_index[i];
+				if (code < 14)
+					cm.mode = HDMI_HDMI;
+				else
+					cm.mode = HDMI_DVI;
+				DSSDBG("Hdmi_code = %d mode = %d\n",
+					 cm.code, cm.mode);
+				break;
+			 }
+		}
+	}
+
+	return cm;
+}
+
+static void get_horz_vert_timing_info(int current_descriptor_addrs, u8 *edid ,
+		struct omap_video_timings *timings)
+{
+	/* X and Y resolution */
+	timings->x_res = (((edid[current_descriptor_addrs + 4] & 0xF0) << 4) |
+			 edid[current_descriptor_addrs + 2]);
+	timings->y_res = (((edid[current_descriptor_addrs + 7] & 0xF0) << 4) |
+			 edid[current_descriptor_addrs + 5]);
+
+	timings->pixel_clock = ((edid[current_descriptor_addrs + 1] << 8) |
+				edid[current_descriptor_addrs]);
+
+	timings->pixel_clock = 10 * timings->pixel_clock;
+
+	/* HORIZONTAL FRONT PORCH */
+	timings->hfp = edid[current_descriptor_addrs + 8] |
+			((edid[current_descriptor_addrs + 11] & 0xc0) << 2);
+	/* HORIZONTAL SYNC WIDTH */
+	timings->hsw = edid[current_descriptor_addrs + 9] |
+			((edid[current_descriptor_addrs + 11] & 0x30) << 4);
+	/* HORIZONTAL BACK PORCH */
+	timings->hbp = (((edid[current_descriptor_addrs + 4] & 0x0F) << 8) |
+			edid[current_descriptor_addrs + 3]) -
+			(timings->hfp + timings->hsw);
+	/* VERTICAL FRONT PORCH */
+	timings->vfp = ((edid[current_descriptor_addrs + 10] & 0xF0) >> 4) |
+			((edid[current_descriptor_addrs + 11] & 0x0f) << 2);
+	/* VERTICAL SYNC WIDTH */
+	timings->vsw = (edid[current_descriptor_addrs + 10] & 0x0F) |
+			((edid[current_descriptor_addrs + 11] & 0x03) << 4);
+	/* VERTICAL BACK PORCH */
+	timings->vbp = (((edid[current_descriptor_addrs + 7] & 0x0F) << 8) |
+			edid[current_descriptor_addrs + 6]) -
+			(timings->vfp + timings->vsw);
+
+}
+
+/* Description : This function gets the resolution information from EDID */
+static void get_edid_timing_data(u8 *edid)
+{
+	u8 count;
+	u16 current_descriptor_addrs;
+	struct hdmi_cm cm;
+	struct omap_video_timings edid_timings;
+
+	/* search block 0, there are 4 DTDs arranged in priority order */
+	for (count = 0; count < EDID_SIZE_BLOCK0_TIMING_DESCRIPTOR; count++) {
+		current_descriptor_addrs =
+			EDID_DESCRIPTOR_BLOCK0_ADDRESS +
+			count * EDID_TIMING_DESCRIPTOR_SIZE;
+		get_horz_vert_timing_info(current_descriptor_addrs,
+				edid, &edid_timings);
+		cm = hdmi_get_code(&edid_timings);
+		DSSDBG("Block0[%d] value matches code = %d , mode = %d\n",
+			count, cm.code, cm.mode);
+		if (cm.code == -1) {
+			continue;
+		} else {
+			hdmi.code = cm.code;
+			hdmi.mode = cm.mode;
+			DSSDBG("code = %d , mode = %d\n",
+				hdmi.code, hdmi.mode);
+			return;
+		}
+	}
+	if (edid[0x7e] != 0x00) {
+		for (count = 0; count < EDID_SIZE_BLOCK1_TIMING_DESCRIPTOR;
+			count++) {
+			current_descriptor_addrs =
+			EDID_DESCRIPTOR_BLOCK1_ADDRESS +
+			count * EDID_TIMING_DESCRIPTOR_SIZE;
+			get_horz_vert_timing_info(current_descriptor_addrs,
+						edid, &edid_timings);
+			cm = hdmi_get_code(&edid_timings);
+			DSSDBG("Block1[%d] value matches code = %d, mode = %d",
+				count, cm.code, cm.mode);
+			if (cm.code == -1) {
+				continue;
+			} else {
+				hdmi.code = cm.code;
+				hdmi.mode = cm.mode;
+				DSSDBG("code = %d , mode = %d\n",
+					hdmi.code, hdmi.mode);
+				return;
+			}
+		}
+	}
+
+	DSSINFO("no valid timing found , falling back to VGA\n");
+	hdmi.code = 4; /* setting default value of 640 480 VGA */
+	hdmi.mode = HDMI_DVI;
+}
+
+static void hdmi_read_edid(struct omap_video_timings *dp)
+{
+	int ret = 0, code;
+
+	memset(hdmi.edid, 0, HDMI_EDID_MAX_LENGTH);
+
+	if (!hdmi.edid_set)
+		ret = read_edid(hdmi.edid, HDMI_EDID_MAX_LENGTH);
+
+	if (!ret) {
+		if (!memcmp(hdmi.edid, edid_header, sizeof(edid_header))) {
+			/* search for timings of default resolution */
+			get_edid_timing_data(hdmi.edid);
+			hdmi.edid_set = true;
+		}
+	} else {
+		DSSWARN("failed to read E-EDID\n");
+	}
+
+	if (!hdmi.edid_set) {
+		DSSINFO("fallback to VGA\n");
+		hdmi.code = 4; /* setting default value of 640 480 VGA */
+		hdmi.mode = HDMI_DVI;
+	}
+
+	code = get_timings_index();
+
+	*dp = cea_vesa_timings[code].timings;
+}
+
+static void hdmi_core_init(struct hdmi_core_video_config *video_cfg,
+			struct hdmi_core_infoframe_avi *avi_cfg,
+			struct hdmi_core_packet_enable_repeat *repeat_cfg)
+{
+	DSSDBG("Enter hdmi_core_init\n");
+
+	/* video core */
+	video_cfg->ip_bus_width = HDMI_INPUT_8BIT;
+	video_cfg->op_dither_truc = HDMI_OUTPUTTRUNCATION_8BIT;
+	video_cfg->deep_color_pkt = HDMI_DEEPCOLORPACKECTDISABLE;
+	video_cfg->pkt_mode = HDMI_PACKETMODERESERVEDVALUE;
+	video_cfg->hdmi_dvi = HDMI_DVI;
+	video_cfg->tclk_sel_clkmult = HDMI_FPLL10IDCK;
+
+	/* info frame */
+	avi_cfg->db1_format = 0;
+	avi_cfg->db1_active_info = 0;
+	avi_cfg->db1_bar_info_dv = 0;
+	avi_cfg->db1_scan_info = 0;
+	avi_cfg->db2_colorimetry = 0;
+	avi_cfg->db2_aspect_ratio = 0;
+	avi_cfg->db2_active_fmt_ar = 0;
+	avi_cfg->db3_itc = 0;
+	avi_cfg->db3_ec = 0;
+	avi_cfg->db3_q_range = 0;
+	avi_cfg->db3_nup_scaling = 0;
+	avi_cfg->db4_videocode = 0;
+	avi_cfg->db5_pixel_repeat = 0;
+	avi_cfg->db6_7_line_eoftop = 0 ;
+	avi_cfg->db8_9_line_sofbottom = 0;
+	avi_cfg->db10_11_pixel_eofleft = 0;
+	avi_cfg->db12_13_pixel_sofright = 0;
+
+	/* packet enable and repeat */
+	repeat_cfg->audio_pkt = 0;
+	repeat_cfg->audio_pkt_repeat = 0;
+	repeat_cfg->avi_infoframe = 0;
+	repeat_cfg->avi_infoframe_repeat = 0;
+	repeat_cfg->gen_cntrl_pkt = 0;
+	repeat_cfg->gen_cntrl_pkt_repeat = 0;
+	repeat_cfg->generic_pkt = 0;
+	repeat_cfg->generic_pkt_repeat = 0;
+}
+
+static void hdmi_core_powerdown_disable(void)
+{
+	DSSDBG("Enter hdmi_core_powerdown_disable\n");
+	REG_FLD_MOD(HDMI_CORE_CTRL1, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_release(void)
+{
+	DSSDBG("Enter hdmi_core_swreset_release\n");
+	REG_FLD_MOD(HDMI_CORE_SYS_SRST, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_assert(void)
+{
+	DSSDBG("Enter hdmi_core_swreset_assert\n");
+	REG_FLD_MOD(HDMI_CORE_SYS_SRST, 0x1, 0, 0);
+}
+
+/* DSS_HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_video_config *cfg)
+{
+	u32 r = 0;
+
+	/* sys_ctrl1 default configuration not tunable */
+	r = hdmi_read_reg(HDMI_CORE_CTRL1);
+	r = FLD_MOD(r, HDMI_CORE_CTRL1_VEN_FOLLOWVSYNC, 5, 5);
+	r = FLD_MOD(r, HDMI_CORE_CTRL1_HEN_FOLLOWHSYNC, 4, 4);
+	r = FLD_MOD(r, HDMI_CORE_CTRL1_BSEL_24BITBUS, 2, 2);
+	r = FLD_MOD(r, HDMI_CORE_CTRL1_EDGE_RISINGEDGE, 1, 1);
+	hdmi_write_reg(HDMI_CORE_CTRL1, r);
+
+	REG_FLD_MOD(HDMI_CORE_SYS_VID_ACEN, cfg->ip_bus_width, 7, 6);
+
+	/* Vid_Mode */
+	r = hdmi_read_reg(HDMI_CORE_SYS_VID_MODE);
+
+	/* dither truncation configuration */
+	if (cfg->op_dither_truc > HDMI_OUTPUTTRUNCATION_12BIT) {
+		r = FLD_MOD(r, cfg->op_dither_truc - 3, 7, 6);
+		r = FLD_MOD(r, 1, 5, 5);
+	} else {
+		r = FLD_MOD(r, cfg->op_dither_truc, 7, 6);
+		r = FLD_MOD(r, 0, 5, 5);
+	}
+	hdmi_write_reg(HDMI_CORE_SYS_VID_MODE, r);
+
+	/* HDMI_Ctrl */
+	r = hdmi_read_reg(HDMI_CORE_AV_HDMI_CTRL);
+	r = FLD_MOD(r, cfg->deep_color_pkt, 6, 6);
+	r = FLD_MOD(r, cfg->pkt_mode, 5, 3);
+	r = FLD_MOD(r, cfg->hdmi_dvi, 0, 0);
+	hdmi_write_reg(HDMI_CORE_AV_HDMI_CTRL, r);
+
+	/* TMDS_CTRL */
+	REG_FLD_MOD(HDMI_CORE_SYS_TMDS_CTRL,
+		cfg->tclk_sel_clkmult, 6, 5);
+}
+
+static void hdmi_core_aux_infoframe_avi_config(
+		struct hdmi_core_infoframe_avi info_avi)
+{
+	u32 val;
+	char sum = 0, checksum = 0;
+
+	sum += 0x82 + 0x002 + 0x00D;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_TYPE, 0x082);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_VERS, 0x002);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_LEN, 0x00D);
+
+	val = (info_avi.db1_format << 5) |
+		(info_avi.db1_active_info << 4) |
+		(info_avi.db1_bar_info_dv << 2) |
+		(info_avi.db1_scan_info);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(0), val);
+	sum += val;
+
+	val = (info_avi.db2_colorimetry << 6) |
+		(info_avi.db2_aspect_ratio << 4) |
+		(info_avi.db2_active_fmt_ar);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(1), val);
+	sum += val;
+
+	val = (info_avi.db3_itc << 7) |
+		(info_avi.db3_ec << 4) |
+		(info_avi.db3_q_range << 2) |
+		(info_avi.db3_nup_scaling);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(2), val);
+	sum += val;
+
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(3), info_avi.db4_videocode);
+	sum += info_avi.db4_videocode;
+
+	val = info_avi.db5_pixel_repeat;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(4), val);
+	sum += val;
+
+	val = info_avi.db6_7_line_eoftop & 0x00FF;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(5), val);
+	sum += val;
+
+	val = ((info_avi.db6_7_line_eoftop >> 8) & 0x00FF);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(6), val);
+	sum += val;
+
+	val = info_avi.db8_9_line_sofbottom & 0x00FF;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(7), val);
+	sum += val;
+
+	val = ((info_avi.db8_9_line_sofbottom >> 8) & 0x00FF);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(8), val);
+	sum += val;
+
+	val = info_avi.db10_11_pixel_eofleft & 0x00FF;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(9), val);
+	sum += val;
+
+	val = ((info_avi.db10_11_pixel_eofleft >> 8) & 0x00FF);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(10), val);
+	sum += val;
+
+	val = info_avi.db12_13_pixel_sofright & 0x00FF;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(11), val);
+	sum += val;
+
+	val = ((info_avi.db12_13_pixel_sofright >> 8) & 0x00FF);
+	hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(12), val);
+	sum += val;
+
+	checksum = 0x100 - sum;
+	hdmi_write_reg(HDMI_CORE_AV_AVI_CHSUM, checksum);
+}
+
+static void hdmi_core_av_packet_config(
+		struct hdmi_core_packet_enable_repeat repeat_cfg)
+{
+	/* enable/repeat the infoframe */
+	hdmi_write_reg(HDMI_CORE_AV_PB_CTRL1,
+		(repeat_cfg.audio_pkt << 5) |
+		(repeat_cfg.audio_pkt_repeat << 4) |
+		(repeat_cfg.avi_infoframe << 1) |
+		(repeat_cfg.avi_infoframe_repeat));
+
+	/* enable/repeat the packet */
+	hdmi_write_reg(HDMI_CORE_AV_PB_CTRL2,
+		(repeat_cfg.gen_cntrl_pkt << 3) |
+		(repeat_cfg.gen_cntrl_pkt_repeat << 2) |
+		(repeat_cfg.generic_pkt << 1) |
+		(repeat_cfg.generic_pkt_repeat));
+}
+
+static void hdmi_wp_init(struct omap_video_timings *timings,
+			struct hdmi_video_format *video_fmt,
+			struct hdmi_video_interface *video_int)
+{
+	DSSDBG("Enter hdmi_wp_init\n");
+
+	timings->hbp = 0;
+	timings->hfp = 0;
+	timings->hsw = 0;
+	timings->vbp = 0;
+	timings->vfp = 0;
+	timings->vsw = 0;
+
+	video_fmt->packing_mode = HDMI_PACK_10b_RGB_YUV444;
+	video_fmt->y_res = 0;
+	video_fmt->x_res = 0;
+
+	video_int->vsp = 0;
+	video_int->hsp = 0;
+
+	video_int->interlacing = 0;
+	video_int->tm = 0; /* HDMI_TIMING_SLAVE */
+
+}
+
+static void hdmi_wp_video_start(bool start)
+{
+	REG_FLD_MOD(HDMI_WP_VIDEO_CFG, start, 31, 31);
+}
+
+static void hdmi_wp_video_init_format(struct hdmi_video_format *video_fmt,
+	struct omap_video_timings *timings, struct hdmi_config *param)
+{
+	DSSDBG("Enter hdmi_wp_video_init_format\n");
+
+	video_fmt->y_res = param->timings.timings.y_res;
+	video_fmt->x_res = param->timings.timings.x_res;
+
+	timings->hbp = param->timings.timings.hbp;
+	timings->hfp = param->timings.timings.hfp;
+	timings->hsw = param->timings.timings.hsw;
+	timings->vbp = param->timings.timings.vbp;
+	timings->vfp = param->timings.timings.vfp;
+	timings->vsw = param->timings.timings.vsw;
+}
+
+static void hdmi_wp_video_config_format(
+		struct hdmi_video_format *video_fmt)
+{
+	u32 l = 0;
+
+	REG_FLD_MOD(HDMI_WP_VIDEO_CFG, video_fmt->packing_mode, 10, 8);
+
+	l |= FLD_VAL(video_fmt->y_res, 31, 16);
+	l |= FLD_VAL(video_fmt->x_res, 15, 0);
+	hdmi_write_reg(HDMI_WP_VIDEO_SIZE, l);
+}
+
+static void hdmi_wp_video_config_interface(
+		struct hdmi_video_interface *video_int)
+{
+	u32 r;
+	DSSDBG("Enter hdmi_wp_video_config_interface\n");
+
+	r = hdmi_read_reg(HDMI_WP_VIDEO_CFG);
+	r = FLD_MOD(r, video_int->vsp, 7, 7);
+	r = FLD_MOD(r, video_int->hsp, 6, 6);
+	r = FLD_MOD(r, video_int->interlacing, 3, 3);
+	r = FLD_MOD(r, video_int->tm, 1, 0);
+	hdmi_write_reg(HDMI_WP_VIDEO_CFG, r);
+}
+
+static void hdmi_wp_video_config_timing(
+		struct omap_video_timings *timings)
+{
+	u32 timing_h = 0;
+	u32 timing_v = 0;
+
+	DSSDBG("Enter hdmi_wp_video_config_timing\n");
+
+	timing_h |= FLD_VAL(timings->hbp, 31, 20);
+	timing_h |= FLD_VAL(timings->hfp, 19, 8);
+	timing_h |= FLD_VAL(timings->hsw, 7, 0);
+	hdmi_write_reg(HDMI_WP_VIDEO_TIMING_H, timing_h);
+
+	timing_v |= FLD_VAL(timings->vbp, 31, 20);
+	timing_v |= FLD_VAL(timings->vfp, 19, 8);
+	timing_v |= FLD_VAL(timings->vsw, 7, 0);
+	hdmi_write_reg(HDMI_WP_VIDEO_TIMING_V, timing_v);
+}
+
+static void hdmi_basic_configure(struct hdmi_config *cfg)
+{
+	/* HDMI */
+	struct omap_video_timings video_timing;
+	struct hdmi_video_format video_format;
+	struct hdmi_video_interface video_interface;
+	/* HDMI core */
+	struct hdmi_core_infoframe_avi avi_cfg;
+	struct hdmi_core_video_config v_core_cfg;
+	struct hdmi_core_packet_enable_repeat repeat_cfg;
+
+	hdmi_wp_init(&video_timing, &video_format,
+		&video_interface);
+
+	hdmi_core_init(&v_core_cfg,
+		&avi_cfg,
+		&repeat_cfg);
+
+	hdmi_wp_video_init_format(&video_format,
+			&video_timing, cfg);
+
+	hdmi_wp_video_config_timing(&video_timing);
+
+	/* video config */
+	video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+	hdmi_wp_video_config_format(&video_format);
+
+	video_interface.vsp = cfg->timings.vsync_pol;
+	video_interface.hsp = cfg->timings.hsync_pol;
+	video_interface.interlacing = cfg->interlace;
+	video_interface.tm = 1 ; /* HDMI_TIMING_MASTER_24BIT */
+
+	hdmi_wp_video_config_interface(&video_interface);
+
+	/*
+	 * configure core video part
+	 * set software reset in the core
+	 */
+	hdmi_core_swreset_assert();
+
+	/* power down off */
+	hdmi_core_powerdown_disable();
+
+	v_core_cfg.pkt_mode = HDMI_PACKETMODE24BITPERPIXEL;
+	v_core_cfg.hdmi_dvi = cfg->cm.mode;
+
+	hdmi_core_video_config(&v_core_cfg);
+
+	/* release software reset in the core */
+	hdmi_core_swreset_release();
+
+	/*
+	 * configure packet
+	 * info frame video see doc CEA861-D page 65
+	 */
+	avi_cfg.db1_format = HDMI_INFOFRAME_AVI_DB1Y_RGB;
+	avi_cfg.db1_active_info =
+		HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF;
+	avi_cfg.db1_bar_info_dv = HDMI_INFOFRAME_AVI_DB1B_NO;
+	avi_cfg.db1_scan_info = HDMI_INFOFRAME_AVI_DB1S_0;
+	avi_cfg.db2_colorimetry = HDMI_INFOFRAME_AVI_DB2C_NO;
+	avi_cfg.db2_aspect_ratio = HDMI_INFOFRAME_AVI_DB2M_NO;
+	avi_cfg.db2_active_fmt_ar = HDMI_INFOFRAME_AVI_DB2R_SAME;
+	avi_cfg.db3_itc = HDMI_INFOFRAME_AVI_DB3ITC_NO;
+	avi_cfg.db3_ec = HDMI_INFOFRAME_AVI_DB3EC_XVYUV601;
+	avi_cfg.db3_q_range = HDMI_INFOFRAME_AVI_DB3Q_DEFAULT;
+	avi_cfg.db3_nup_scaling = HDMI_INFOFRAME_AVI_DB3SC_NO;
+	avi_cfg.db4_videocode = cfg->cm.code;
+	avi_cfg.db5_pixel_repeat = HDMI_INFOFRAME_AVI_DB5PR_NO;
+	avi_cfg.db6_7_line_eoftop = 0;
+	avi_cfg.db8_9_line_sofbottom = 0;
+	avi_cfg.db10_11_pixel_eofleft = 0;
+	avi_cfg.db12_13_pixel_sofright = 0;
+
+	hdmi_core_aux_infoframe_avi_config(avi_cfg);
+
+	/* enable/repeat the infoframe */
+	repeat_cfg.avi_infoframe = HDMI_PACKETENABLE;
+	repeat_cfg.avi_infoframe_repeat = HDMI_PACKETREPEATON;
+	/* wakeup */
+	repeat_cfg.audio_pkt = HDMI_PACKETENABLE;
+	repeat_cfg.audio_pkt_repeat = HDMI_PACKETREPEATON;
+	hdmi_core_av_packet_config(repeat_cfg);
+}
+
+static void update_hdmi_timings(struct hdmi_config *cfg,
+		struct omap_video_timings *timings, int code)
+{
+	cfg->timings.timings.x_res = timings->x_res;
+	cfg->timings.timings.y_res = timings->y_res;
+	cfg->timings.timings.hbp = timings->hbp;
+	cfg->timings.timings.hfp = timings->hfp;
+	cfg->timings.timings.hsw = timings->hsw;
+	cfg->timings.timings.vbp = timings->vbp;
+	cfg->timings.timings.vfp = timings->vfp;
+	cfg->timings.timings.vsw = timings->vsw;
+	cfg->timings.timings.pixel_clock = timings->pixel_clock;
+	cfg->timings.vsync_pol = cea_vesa_timings[code].vsync_pol;
+	cfg->timings.hsync_pol = cea_vesa_timings[code].hsync_pol;
+}
+
+static void hdmi_compute_pll(struct omap_dss_device *dssdev, int phy,
+		struct hdmi_pll_info *pi)
+{
+	unsigned long clkin, refclk;
+	u32 mf;
+
+	clkin = clk_get_rate(hdmi.sys_clk) / 10000;
+	/*
+	 * Input clock is predivided by N + 1
+	 * out put of which is reference clk
+	 */
+	pi->regn = dssdev->clocks.hdmi.regn;
+	refclk = clkin / (pi->regn + 1);
+
+	/*
+	 * multiplier is pixel_clk/ref_clk
+	 * Multiplying by 100 to avoid fractional part removal
+	 */
+	pi->regm = (phy * 100 / (refclk)) / 100;
+	pi->regm2 = dssdev->clocks.hdmi.regm2;
+
+	/*
+	 * fractional multiplier is remainder of the difference between
+	 * multiplier and actual phy(required pixel clock thus should be
+	 * multiplied by 2^18(262144) divided by the reference clock
+	 */
+	mf = (phy - pi->regm * refclk) * 262144;
+	pi->regmf = mf / (refclk);
+
+	/*
+	 * Dcofreq should be set to 1 if required pixel clock
+	 * is greater than 1000MHz
+	 */
+	pi->dcofreq = phy > 1000 * 100;
+	pi->regsd = ((pi->regm * clkin / 10) / ((pi->regn + 1) * 250) + 5) / 10;
+
+	DSSDBG("M = %d Mf = %d\n", pi->regm, pi->regmf);
+	DSSDBG("range = %d sd = %d\n", pi->dcofreq, pi->regsd);
+}
+
+static int hdmi_power_on(struct omap_dss_device *dssdev)
+{
+	int r, code = 0;
+	struct hdmi_pll_info pll_data;
+	struct omap_video_timings *p;
+	unsigned long phy;
+
+	r = hdmi_runtime_get();
+	if (r)
+		return r;
+
+	dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 0);
+
+	p = &dssdev->panel.timings;
+
+	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n",
+		dssdev->panel.timings.x_res,
+		dssdev->panel.timings.y_res);
+
+	if (!hdmi.custom_set) {
+		DSSDBG("Read EDID as no EDID is not set on poweron\n");
+		hdmi_read_edid(p);
+	}
+	code = get_timings_index();
+	dssdev->panel.timings = cea_vesa_timings[code].timings;
+	update_hdmi_timings(&hdmi.cfg, p, code);
+
+	phy = p->pixel_clock;
+
+	hdmi_compute_pll(dssdev, phy, &pll_data);
+
+	hdmi_wp_video_start(0);
+
+	/* config the PLL and PHY first */
+	r = hdmi_pll_program(&pll_data);
+	if (r) {
+		DSSDBG("Failed to lock PLL\n");
+		goto err;
+	}
+
+	r = hdmi_phy_init();
+	if (r) {
+		DSSDBG("Failed to start PHY\n");
+		goto err;
+	}
+
+	hdmi.cfg.cm.mode = hdmi.mode;
+	hdmi.cfg.cm.code = hdmi.code;
+	hdmi_basic_configure(&hdmi.cfg);
+
+	/* Make selection of HDMI in DSS */
+	dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+	/* Select the dispc clock source as PRCM clock, to ensure that it is not
+	 * DSI PLL source as the clock selected by DSI PLL might not be
+	 * sufficient for the resolution selected / that can be changed
+	 * dynamically by user. This can be moved to single location , say
+	 * Boardfile.
+	 */
+	dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
+
+	/* bypass TV gamma table */
+	dispc_enable_gamma_table(0);
+
+	/* tv size */
+	dispc_set_digit_size(dssdev->panel.timings.x_res,
+			dssdev->panel.timings.y_res);
+
+	dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 1);
+
+	hdmi_wp_video_start(1);
+
+	return 0;
+err:
+	hdmi_runtime_put();
+	return -EIO;
+}
+
+static void hdmi_power_off(struct omap_dss_device *dssdev)
+{
+	dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 0);
+
+	hdmi_wp_video_start(0);
+	hdmi_phy_off();
+	hdmi_set_pll_pwr(HDMI_PLLPWRCMD_ALLOFF);
+	hdmi_runtime_put();
+
+	hdmi.edid_set = 0;
+}
+
+int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
+					struct omap_video_timings *timings)
+{
+	struct hdmi_cm cm;
+
+	cm = hdmi_get_code(timings);
+	if (cm.code == -1) {
+		DSSERR("Invalid timing entered\n");
+		return -EINVAL;
+	}
+
+	return 0;
+
+}
+
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev)
+{
+	struct hdmi_cm cm;
+
+	hdmi.custom_set = 1;
+	cm = hdmi_get_code(&dssdev->panel.timings);
+	hdmi.code = cm.code;
+	hdmi.mode = cm.mode;
+	omapdss_hdmi_display_enable(dssdev);
+	hdmi.custom_set = 0;
+}
+
+int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+
+	DSSDBG("ENTER hdmi_display_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	r = omap_dss_start_device(dssdev);
+	if (r) {
+		DSSERR("failed to start device\n");
+		goto err0;
+	}
+
+	if (dssdev->platform_enable) {
+		r = dssdev->platform_enable(dssdev);
+		if (r) {
+			DSSERR("failed to enable GPIO's\n");
+			goto err1;
+		}
+	}
+
+	r = hdmi_power_on(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err2;
+	}
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err2:
+	if (dssdev->platform_disable)
+		dssdev->platform_disable(dssdev);
+err1:
+	omap_dss_stop_device(dssdev);
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+	DSSDBG("Enter hdmi_display_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	hdmi_power_off(dssdev);
+
+	if (dssdev->platform_disable)
+		dssdev->platform_disable(dssdev);
+
+	omap_dss_stop_device(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+	defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+static void hdmi_wp_audio_config_format(
+		struct hdmi_audio_format *aud_fmt)
+{
+	u32 r;
+
+	DSSDBG("Enter hdmi_wp_audio_config_format\n");
+
+	r = hdmi_read_reg(HDMI_WP_AUDIO_CFG);
+	r = FLD_MOD(r, aud_fmt->stereo_channels, 26, 24);
+	r = FLD_MOD(r, aud_fmt->active_chnnls_msk, 23, 16);
+	r = FLD_MOD(r, aud_fmt->en_sig_blk_strt_end, 5, 5);
+	r = FLD_MOD(r, aud_fmt->type, 4, 4);
+	r = FLD_MOD(r, aud_fmt->justification, 3, 3);
+	r = FLD_MOD(r, aud_fmt->sample_order, 2, 2);
+	r = FLD_MOD(r, aud_fmt->samples_per_word, 1, 1);
+	r = FLD_MOD(r, aud_fmt->sample_size, 0, 0);
+	hdmi_write_reg(HDMI_WP_AUDIO_CFG, r);
+}
+
+static void hdmi_wp_audio_config_dma(struct hdmi_audio_dma *aud_dma)
+{
+	u32 r;
+
+	DSSDBG("Enter hdmi_wp_audio_config_dma\n");
+
+	r = hdmi_read_reg(HDMI_WP_AUDIO_CFG2);
+	r = FLD_MOD(r, aud_dma->transfer_size, 15, 8);
+	r = FLD_MOD(r, aud_dma->block_size, 7, 0);
+	hdmi_write_reg(HDMI_WP_AUDIO_CFG2, r);
+
+	r = hdmi_read_reg(HDMI_WP_AUDIO_CTRL);
+	r = FLD_MOD(r, aud_dma->mode, 9, 9);
+	r = FLD_MOD(r, aud_dma->fifo_threshold, 8, 0);
+	hdmi_write_reg(HDMI_WP_AUDIO_CTRL, r);
+}
+
+static void hdmi_core_audio_config(struct hdmi_core_audio_config *cfg)
+{
+	u32 r;
+
+	/* audio clock recovery parameters */
+	r = hdmi_read_reg(HDMI_CORE_AV_ACR_CTRL);
+	r = FLD_MOD(r, cfg->use_mclk, 2, 2);
+	r = FLD_MOD(r, cfg->en_acr_pkt, 1, 1);
+	r = FLD_MOD(r, cfg->cts_mode, 0, 0);
+	hdmi_write_reg(HDMI_CORE_AV_ACR_CTRL, r);
+
+	REG_FLD_MOD(HDMI_CORE_AV_N_SVAL1, cfg->n, 7, 0);
+	REG_FLD_MOD(HDMI_CORE_AV_N_SVAL2, cfg->n >> 8, 7, 0);
+	REG_FLD_MOD(HDMI_CORE_AV_N_SVAL3, cfg->n >> 16, 7, 0);
+
+	if (cfg->cts_mode == HDMI_AUDIO_CTS_MODE_SW) {
+		REG_FLD_MOD(HDMI_CORE_AV_CTS_SVAL1, cfg->cts, 7, 0);
+		REG_FLD_MOD(HDMI_CORE_AV_CTS_SVAL2, cfg->cts >> 8, 7, 0);
+		REG_FLD_MOD(HDMI_CORE_AV_CTS_SVAL3, cfg->cts >> 16, 7, 0);
+	} else {
+		/*
+		 * HDMI IP uses this configuration to divide the MCLK to
+		 * update CTS value.
+		 */
+		REG_FLD_MOD(HDMI_CORE_AV_FREQ_SVAL, cfg->mclk_mode, 2, 0);
+
+		/* Configure clock for audio packets */
+		REG_FLD_MOD(HDMI_CORE_AV_AUD_PAR_BUSCLK_1,
+			cfg->aud_par_busclk, 7, 0);
+		REG_FLD_MOD(HDMI_CORE_AV_AUD_PAR_BUSCLK_2,
+			(cfg->aud_par_busclk >> 8), 7, 0);
+		REG_FLD_MOD(HDMI_CORE_AV_AUD_PAR_BUSCLK_3,
+			(cfg->aud_par_busclk >> 16), 7, 0);
+	}
+
+	/* Override of SPDIF sample frequency with value in I2S_CHST4 */
+	REG_FLD_MOD(HDMI_CORE_AV_SPDIF_CTRL, cfg->fs_override, 1, 1);
+
+	/* I2S parameters */
+	REG_FLD_MOD(HDMI_CORE_AV_I2S_CHST4, cfg->freq_sample, 3, 0);
+
+	r = hdmi_read_reg(HDMI_CORE_AV_I2S_IN_CTRL);
+	r = FLD_MOD(r, cfg->i2s_cfg.en_high_bitrate_aud, 7, 7);
+	r = FLD_MOD(r, cfg->i2s_cfg.sck_edge_mode, 6, 6);
+	r = FLD_MOD(r, cfg->i2s_cfg.cbit_order, 5, 5);
+	r = FLD_MOD(r, cfg->i2s_cfg.vbit, 4, 4);
+	r = FLD_MOD(r, cfg->i2s_cfg.ws_polarity, 3, 3);
+	r = FLD_MOD(r, cfg->i2s_cfg.justification, 2, 2);
+	r = FLD_MOD(r, cfg->i2s_cfg.direction, 1, 1);
+	r = FLD_MOD(r, cfg->i2s_cfg.shift, 0, 0);
+	hdmi_write_reg(HDMI_CORE_AV_I2S_IN_CTRL, r);
+
+	r = hdmi_read_reg(HDMI_CORE_AV_I2S_CHST5);
+	r = FLD_MOD(r, cfg->freq_sample, 7, 4);
+	r = FLD_MOD(r, cfg->i2s_cfg.word_length, 3, 1);
+	r = FLD_MOD(r, cfg->i2s_cfg.word_max_length, 0, 0);
+	hdmi_write_reg(HDMI_CORE_AV_I2S_CHST5, r);
+
+	REG_FLD_MOD(HDMI_CORE_AV_I2S_IN_LEN, cfg->i2s_cfg.in_length_bits, 3, 0);
+
+	/* Audio channels and mode parameters */
+	REG_FLD_MOD(HDMI_CORE_AV_HDMI_CTRL, cfg->layout, 2, 1);
+	r = hdmi_read_reg(HDMI_CORE_AV_AUD_MODE);
+	r = FLD_MOD(r, cfg->i2s_cfg.active_sds, 7, 4);
+	r = FLD_MOD(r, cfg->en_dsd_audio, 3, 3);
+	r = FLD_MOD(r, cfg->en_parallel_aud_input, 2, 2);
+	r = FLD_MOD(r, cfg->en_spdif, 1, 1);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_MODE, r);
+}
+
+static void hdmi_core_audio_infoframe_config(
+		struct hdmi_core_infoframe_audio *info_aud)
+{
+	u8 val;
+	u8 sum = 0, checksum = 0;
+
+	/*
+	 * Set audio info frame type, version and length as
+	 * described in HDMI 1.4a Section 8.2.2 specification.
+	 * Checksum calculation is defined in Section 5.3.5.
+	 */
+	hdmi_write_reg(HDMI_CORE_AV_AUDIO_TYPE, 0x84);
+	hdmi_write_reg(HDMI_CORE_AV_AUDIO_VERS, 0x01);
+	hdmi_write_reg(HDMI_CORE_AV_AUDIO_LEN, 0x0a);
+	sum += 0x84 + 0x001 + 0x00a;
+
+	val = (info_aud->db1_coding_type << 4)
+			| (info_aud->db1_channel_count - 1);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(0), val);
+	sum += val;
+
+	val = (info_aud->db2_sample_freq << 2) | info_aud->db2_sample_size;
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(1), val);
+	sum += val;
+
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(2), 0x00);
+
+	val = info_aud->db4_channel_alloc;
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(3), val);
+	sum += val;
+
+	val = (info_aud->db5_downmix_inh << 7) | (info_aud->db5_lsv << 3);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(4), val);
+	sum += val;
+
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(5), 0x00);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(6), 0x00);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(7), 0x00);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(8), 0x00);
+	hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(9), 0x00);
+
+	checksum = 0x100 - sum;
+	hdmi_write_reg(HDMI_CORE_AV_AUDIO_CHSUM, checksum);
+
+	/*
+	 * TODO: Add MPEG and SPD enable and repeat cfg when EDID parsing
+	 * is available.
+	 */
+}
+
+static int hdmi_config_audio_acr(u32 sample_freq, u32 *n, u32 *cts)
+{
+	u32 r;
+	u32 deep_color = 0;
+	u32 pclk = hdmi.cfg.timings.timings.pixel_clock;
+
+	if (n == NULL || cts == NULL)
+		return -EINVAL;
+	/*
+	 * Obtain current deep color configuration. This needed
+	 * to calculate the TMDS clock based on the pixel clock.
+	 */
+	r = REG_GET(HDMI_WP_VIDEO_CFG, 1, 0);
+	switch (r) {
+	case 1: /* No deep color selected */
+		deep_color = 100;
+		break;
+	case 2: /* 10-bit deep color selected */
+		deep_color = 125;
+		break;
+	case 3: /* 12-bit deep color selected */
+		deep_color = 150;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (sample_freq) {
+	case 32000:
+		if ((deep_color == 125) && ((pclk == 54054)
+				|| (pclk == 74250)))
+			*n = 8192;
+		else
+			*n = 4096;
+		break;
+	case 44100:
+		*n = 6272;
+		break;
+	case 48000:
+		if ((deep_color == 125) && ((pclk == 54054)
+				|| (pclk == 74250)))
+			*n = 8192;
+		else
+			*n = 6144;
+		break;
+	default:
+		*n = 0;
+		return -EINVAL;
+	}
+
+	/* Calculate CTS. See HDMI 1.3a or 1.4a specifications */
+	*cts = pclk * (*n / 128) * deep_color / (sample_freq / 10);
+
+	return 0;
+}
+
+static int hdmi_audio_hw_params(struct snd_pcm_substream *substream,
+				    struct snd_pcm_hw_params *params,
+				    struct snd_soc_dai *dai)
+{
+	struct hdmi_audio_format audio_format;
+	struct hdmi_audio_dma audio_dma;
+	struct hdmi_core_audio_config core_cfg;
+	struct hdmi_core_infoframe_audio aud_if_cfg;
+	int err, n, cts;
+	enum hdmi_core_audio_sample_freq sample_freq;
+
+	switch (params_format(params)) {
+	case SNDRV_PCM_FORMAT_S16_LE:
+		core_cfg.i2s_cfg.word_max_length =
+			HDMI_AUDIO_I2S_MAX_WORD_20BITS;
+		core_cfg.i2s_cfg.word_length = HDMI_AUDIO_I2S_CHST_WORD_16_BITS;
+		core_cfg.i2s_cfg.in_length_bits =
+			HDMI_AUDIO_I2S_INPUT_LENGTH_16;
+		core_cfg.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+		audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
+		audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
+		audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+		audio_dma.transfer_size = 0x10;
+		break;
+	case SNDRV_PCM_FORMAT_S24_LE:
+		core_cfg.i2s_cfg.word_max_length =
+			HDMI_AUDIO_I2S_MAX_WORD_24BITS;
+		core_cfg.i2s_cfg.word_length = HDMI_AUDIO_I2S_CHST_WORD_24_BITS;
+		core_cfg.i2s_cfg.in_length_bits =
+			HDMI_AUDIO_I2S_INPUT_LENGTH_24;
+		audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_ONESAMPLE;
+		audio_format.sample_size = HDMI_AUDIO_SAMPLE_24BITS;
+		audio_format.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+		core_cfg.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+		audio_dma.transfer_size = 0x20;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (params_rate(params)) {
+	case 32000:
+		sample_freq = HDMI_AUDIO_FS_32000;
+		break;
+	case 44100:
+		sample_freq = HDMI_AUDIO_FS_44100;
+		break;
+	case 48000:
+		sample_freq = HDMI_AUDIO_FS_48000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = hdmi_config_audio_acr(params_rate(params), &n, &cts);
+	if (err < 0)
+		return err;
+
+	/* Audio wrapper config */
+	audio_format.stereo_channels = HDMI_AUDIO_STEREO_ONECHANNEL;
+	audio_format.active_chnnls_msk = 0x03;
+	audio_format.type = HDMI_AUDIO_TYPE_LPCM;
+	audio_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
+	/* Disable start/stop signals of IEC 60958 blocks */
+	audio_format.en_sig_blk_strt_end = HDMI_AUDIO_BLOCK_SIG_STARTEND_OFF;
+
+	audio_dma.block_size = 0xC0;
+	audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+	audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+	hdmi_wp_audio_config_dma(&audio_dma);
+	hdmi_wp_audio_config_format(&audio_format);
+
+	/*
+	 * I2S config
+	 */
+	core_cfg.i2s_cfg.en_high_bitrate_aud = false;
+	/* Only used with high bitrate audio */
+	core_cfg.i2s_cfg.cbit_order = false;
+	/* Serial data and word select should change on sck rising edge */
+	core_cfg.i2s_cfg.sck_edge_mode = HDMI_AUDIO_I2S_SCK_EDGE_RISING;
+	core_cfg.i2s_cfg.vbit = HDMI_AUDIO_I2S_VBIT_FOR_PCM;
+	/* Set I2S word select polarity */
+	core_cfg.i2s_cfg.ws_polarity = HDMI_AUDIO_I2S_WS_POLARITY_LOW_IS_LEFT;
+	core_cfg.i2s_cfg.direction = HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST;
+	/* Set serial data to word select shift. See Phillips spec. */
+	core_cfg.i2s_cfg.shift = HDMI_AUDIO_I2S_FIRST_BIT_SHIFT;
+	/* Enable one of the four available serial data channels */
+	core_cfg.i2s_cfg.active_sds = HDMI_AUDIO_I2S_SD0_EN;
+
+	/* Core audio config */
+	core_cfg.freq_sample = sample_freq;
+	core_cfg.n = n;
+	core_cfg.cts = cts;
+	if (dss_has_feature(FEAT_HDMI_CTS_SWMODE)) {
+		core_cfg.aud_par_busclk = 0;
+		core_cfg.cts_mode = HDMI_AUDIO_CTS_MODE_SW;
+		core_cfg.use_mclk = false;
+	} else {
+		core_cfg.aud_par_busclk = (((128 * 31) - 1) << 8);
+		core_cfg.cts_mode = HDMI_AUDIO_CTS_MODE_HW;
+		core_cfg.use_mclk = true;
+		core_cfg.mclk_mode = HDMI_AUDIO_MCLK_128FS;
+	}
+	core_cfg.layout = HDMI_AUDIO_LAYOUT_2CH;
+	core_cfg.en_spdif = false;
+	/* Use sample frequency from channel status word */
+	core_cfg.fs_override = true;
+	/* Enable ACR packets */
+	core_cfg.en_acr_pkt = true;
+	/* Disable direct streaming digital audio */
+	core_cfg.en_dsd_audio = false;
+	/* Use parallel audio interface */
+	core_cfg.en_parallel_aud_input = true;
+
+	hdmi_core_audio_config(&core_cfg);
+
+	/*
+	 * Configure packet
+	 * info frame audio see doc CEA861-D page 74
+	 */
+	aud_if_cfg.db1_coding_type = HDMI_INFOFRAME_AUDIO_DB1CT_FROM_STREAM;
+	aud_if_cfg.db1_channel_count = 2;
+	aud_if_cfg.db2_sample_freq = HDMI_INFOFRAME_AUDIO_DB2SF_FROM_STREAM;
+	aud_if_cfg.db2_sample_size = HDMI_INFOFRAME_AUDIO_DB2SS_FROM_STREAM;
+	aud_if_cfg.db4_channel_alloc = 0x00;
+	aud_if_cfg.db5_downmix_inh = false;
+	aud_if_cfg.db5_lsv = 0;
+
+	hdmi_core_audio_infoframe_config(&aud_if_cfg);
+	return 0;
+}
+
+static int hdmi_audio_trigger(struct snd_pcm_substream *substream, int cmd,
+				  struct snd_soc_dai *dai)
+{
+	int err = 0;
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+	case SNDRV_PCM_TRIGGER_RESUME:
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		REG_FLD_MOD(HDMI_CORE_AV_AUD_MODE, 1, 0, 0);
+		REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 1, 31, 31);
+		REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 1, 30, 30);
+		break;
+
+	case SNDRV_PCM_TRIGGER_STOP:
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		REG_FLD_MOD(HDMI_CORE_AV_AUD_MODE, 0, 0, 0);
+		REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 0, 30, 30);
+		REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 0, 31, 31);
+		break;
+	default:
+		err = -EINVAL;
+	}
+	return err;
+}
+
+static int hdmi_audio_startup(struct snd_pcm_substream *substream,
+				  struct snd_soc_dai *dai)
+{
+	if (!hdmi.mode) {
+		pr_err("Current video settings do not support audio.\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static struct snd_soc_codec_driver hdmi_audio_codec_drv = {
+};
+
+static struct snd_soc_dai_ops hdmi_audio_codec_ops = {
+	.hw_params = hdmi_audio_hw_params,
+	.trigger = hdmi_audio_trigger,
+	.startup = hdmi_audio_startup,
+};
+
+static struct snd_soc_dai_driver hdmi_codec_dai_drv = {
+		.name = "hdmi-audio-codec",
+		.playback = {
+			.channels_min = 2,
+			.channels_max = 2,
+			.rates = SNDRV_PCM_RATE_32000 |
+				SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000,
+			.formats = SNDRV_PCM_FMTBIT_S16_LE |
+				SNDRV_PCM_FMTBIT_S24_LE,
+		},
+		.ops = &hdmi_audio_codec_ops,
+};
+#endif
+
+static int hdmi_get_clocks(struct platform_device *pdev)
+{
+	struct clk *clk;
+
+	clk = clk_get(&pdev->dev, "sys_clk");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get sys_clk\n");
+		return PTR_ERR(clk);
+	}
+
+	hdmi.sys_clk = clk;
+
+	clk = clk_get(&pdev->dev, "dss_48mhz_clk");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get hdmi_clk\n");
+		clk_put(hdmi.sys_clk);
+		return PTR_ERR(clk);
+	}
+
+	hdmi.hdmi_clk = clk;
+
+	return 0;
+}
+
+static void hdmi_put_clocks(void)
+{
+	if (hdmi.sys_clk)
+		clk_put(hdmi.sys_clk);
+	if (hdmi.hdmi_clk)
+		clk_put(hdmi.hdmi_clk);
+}
+
+/* HDMI HW IP initialisation */
+static int omapdss_hdmihw_probe(struct platform_device *pdev)
+{
+	struct resource *hdmi_mem;
+	int r;
+
+	hdmi.pdata = pdev->dev.platform_data;
+	hdmi.pdev = pdev;
+
+	mutex_init(&hdmi.lock);
+
+	hdmi_mem = platform_get_resource(hdmi.pdev, IORESOURCE_MEM, 0);
+	if (!hdmi_mem) {
+		DSSERR("can't get IORESOURCE_MEM HDMI\n");
+		return -EINVAL;
+	}
+
+	/* Base address taken from platform */
+	hdmi.base_wp = ioremap(hdmi_mem->start, resource_size(hdmi_mem));
+	if (!hdmi.base_wp) {
+		DSSERR("can't ioremap WP\n");
+		return -ENOMEM;
+	}
+
+	r = hdmi_get_clocks(pdev);
+	if (r) {
+		iounmap(hdmi.base_wp);
+		return r;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	hdmi_panel_init();
+
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+	defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+
+	/* Register ASoC codec DAI */
+	r = snd_soc_register_codec(&pdev->dev, &hdmi_audio_codec_drv,
+					&hdmi_codec_dai_drv, 1);
+	if (r) {
+		DSSERR("can't register ASoC HDMI audio codec\n");
+		return r;
+	}
+#endif
+	return 0;
+}
+
+static int omapdss_hdmihw_remove(struct platform_device *pdev)
+{
+	hdmi_panel_exit();
+
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+	defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+	snd_soc_unregister_codec(&pdev->dev);
+#endif
+
+	pm_runtime_disable(&pdev->dev);
+
+	hdmi_put_clocks();
+
+	iounmap(hdmi.base_wp);
+
+	return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	clk_disable(hdmi.hdmi_clk);
+	clk_disable(hdmi.sys_clk);
+
+	dispc_runtime_put();
+	dss_runtime_put();
+
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dss_runtime_get();
+	if (r < 0)
+		goto err_get_dss;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		goto err_get_dispc;
+
+
+	clk_enable(hdmi.sys_clk);
+	clk_enable(hdmi.hdmi_clk);
+
+	return 0;
+
+err_get_dispc:
+	dss_runtime_put();
+err_get_dss:
+	return r;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume = hdmi_runtime_resume,
+};
+
+static struct platform_driver omapdss_hdmihw_driver = {
+	.probe          = omapdss_hdmihw_probe,
+	.remove         = omapdss_hdmihw_remove,
+	.driver         = {
+		.name   = "omapdss_hdmi",
+		.owner  = THIS_MODULE,
+		.pm	= &hdmi_pm_ops,
+	},
+};
+
+int hdmi_init_platform_driver(void)
+{
+	return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/omap2/dss/hdmi.h b/drivers/video/omap2/dss/hdmi.h
new file mode 100644
index 000000000000..c885f9cb0659
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi.h
@@ -0,0 +1,631 @@
+/*
+ * hdmi.h
+ *
+ * HDMI driver definition for TI OMAP4 processors.
+ *
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _OMAP4_DSS_HDMI_H_
+#define _OMAP4_DSS_HDMI_H_
+
+#include <linux/string.h>
+#include <video/omapdss.h>
+
+#define HDMI_WP		0x0
+#define HDMI_CORE_SYS		0x400
+#define HDMI_CORE_AV		0x900
+#define HDMI_PLLCTRL		0x200
+#define HDMI_PHY		0x300
+
+struct hdmi_reg { u16 idx; };
+
+#define HDMI_REG(idx)			((const struct hdmi_reg) { idx })
+
+/* HDMI Wrapper */
+#define HDMI_WP_REG(idx)			HDMI_REG(HDMI_WP + idx)
+
+#define HDMI_WP_REVISION			HDMI_WP_REG(0x0)
+#define HDMI_WP_SYSCONFIG			HDMI_WP_REG(0x10)
+#define HDMI_WP_IRQSTATUS_RAW			HDMI_WP_REG(0x24)
+#define HDMI_WP_IRQSTATUS			HDMI_WP_REG(0x28)
+#define HDMI_WP_PWR_CTRL			HDMI_WP_REG(0x40)
+#define HDMI_WP_IRQENABLE_SET			HDMI_WP_REG(0x2C)
+#define HDMI_WP_VIDEO_CFG			HDMI_WP_REG(0x50)
+#define HDMI_WP_VIDEO_SIZE			HDMI_WP_REG(0x60)
+#define HDMI_WP_VIDEO_TIMING_H			HDMI_WP_REG(0x68)
+#define HDMI_WP_VIDEO_TIMING_V			HDMI_WP_REG(0x6C)
+#define HDMI_WP_WP_CLK				HDMI_WP_REG(0x70)
+#define HDMI_WP_AUDIO_CFG			HDMI_WP_REG(0x80)
+#define HDMI_WP_AUDIO_CFG2			HDMI_WP_REG(0x84)
+#define HDMI_WP_AUDIO_CTRL			HDMI_WP_REG(0x88)
+#define HDMI_WP_AUDIO_DATA			HDMI_WP_REG(0x8C)
+
+/* HDMI IP Core System */
+#define HDMI_CORE_SYS_REG(idx)			HDMI_REG(HDMI_CORE_SYS + idx)
+
+#define HDMI_CORE_SYS_VND_IDL			HDMI_CORE_SYS_REG(0x0)
+#define HDMI_CORE_SYS_DEV_IDL			HDMI_CORE_SYS_REG(0x8)
+#define HDMI_CORE_SYS_DEV_IDH			HDMI_CORE_SYS_REG(0xC)
+#define HDMI_CORE_SYS_DEV_REV			HDMI_CORE_SYS_REG(0x10)
+#define HDMI_CORE_SYS_SRST			HDMI_CORE_SYS_REG(0x14)
+#define HDMI_CORE_CTRL1			HDMI_CORE_SYS_REG(0x20)
+#define HDMI_CORE_SYS_SYS_STAT			HDMI_CORE_SYS_REG(0x24)
+#define HDMI_CORE_SYS_VID_ACEN			HDMI_CORE_SYS_REG(0x124)
+#define HDMI_CORE_SYS_VID_MODE			HDMI_CORE_SYS_REG(0x128)
+#define HDMI_CORE_SYS_INTR_STATE		HDMI_CORE_SYS_REG(0x1C0)
+#define HDMI_CORE_SYS_INTR1			HDMI_CORE_SYS_REG(0x1C4)
+#define HDMI_CORE_SYS_INTR2			HDMI_CORE_SYS_REG(0x1C8)
+#define HDMI_CORE_SYS_INTR3			HDMI_CORE_SYS_REG(0x1CC)
+#define HDMI_CORE_SYS_INTR4			HDMI_CORE_SYS_REG(0x1D0)
+#define HDMI_CORE_SYS_UMASK1			HDMI_CORE_SYS_REG(0x1D4)
+#define HDMI_CORE_SYS_TMDS_CTRL		HDMI_CORE_SYS_REG(0x208)
+#define HDMI_CORE_SYS_DE_DLY			HDMI_CORE_SYS_REG(0xC8)
+#define HDMI_CORE_SYS_DE_CTRL			HDMI_CORE_SYS_REG(0xCC)
+#define HDMI_CORE_SYS_DE_TOP			HDMI_CORE_SYS_REG(0xD0)
+#define HDMI_CORE_SYS_DE_CNTL			HDMI_CORE_SYS_REG(0xD8)
+#define HDMI_CORE_SYS_DE_CNTH			HDMI_CORE_SYS_REG(0xDC)
+#define HDMI_CORE_SYS_DE_LINL			HDMI_CORE_SYS_REG(0xE0)
+#define HDMI_CORE_SYS_DE_LINH_1		HDMI_CORE_SYS_REG(0xE4)
+#define HDMI_CORE_CTRL1_VEN_FOLLOWVSYNC	0x1
+#define HDMI_CORE_CTRL1_HEN_FOLLOWHSYNC	0x1
+#define HDMI_CORE_CTRL1_BSEL_24BITBUS		0x1
+#define HDMI_CORE_CTRL1_EDGE_RISINGEDGE	0x1
+
+/* HDMI DDC E-DID */
+#define HDMI_CORE_DDC_CMD			HDMI_CORE_SYS_REG(0x3CC)
+#define HDMI_CORE_DDC_STATUS			HDMI_CORE_SYS_REG(0x3C8)
+#define HDMI_CORE_DDC_ADDR			HDMI_CORE_SYS_REG(0x3B4)
+#define HDMI_CORE_DDC_OFFSET			HDMI_CORE_SYS_REG(0x3BC)
+#define HDMI_CORE_DDC_COUNT1			HDMI_CORE_SYS_REG(0x3C0)
+#define HDMI_CORE_DDC_COUNT2			HDMI_CORE_SYS_REG(0x3C4)
+#define HDMI_CORE_DDC_DATA			HDMI_CORE_SYS_REG(0x3D0)
+#define HDMI_CORE_DDC_SEGM			HDMI_CORE_SYS_REG(0x3B8)
+
+/* HDMI IP Core Audio Video */
+#define HDMI_CORE_AV_REG(idx)			HDMI_REG(HDMI_CORE_AV + idx)
+
+#define HDMI_CORE_AV_HDMI_CTRL			HDMI_CORE_AV_REG(0xBC)
+#define HDMI_CORE_AV_DPD			HDMI_CORE_AV_REG(0xF4)
+#define HDMI_CORE_AV_PB_CTRL1			HDMI_CORE_AV_REG(0xF8)
+#define HDMI_CORE_AV_PB_CTRL2			HDMI_CORE_AV_REG(0xFC)
+#define HDMI_CORE_AV_AVI_TYPE			HDMI_CORE_AV_REG(0x100)
+#define HDMI_CORE_AV_AVI_VERS			HDMI_CORE_AV_REG(0x104)
+#define HDMI_CORE_AV_AVI_LEN			HDMI_CORE_AV_REG(0x108)
+#define HDMI_CORE_AV_AVI_CHSUM			HDMI_CORE_AV_REG(0x10C)
+#define HDMI_CORE_AV_AVI_DBYTE(n)		HDMI_CORE_AV_REG(n * 4 + 0x110)
+#define HDMI_CORE_AV_AVI_DBYTE_NELEMS		HDMI_CORE_AV_REG(15)
+#define HDMI_CORE_AV_SPD_DBYTE			HDMI_CORE_AV_REG(0x190)
+#define HDMI_CORE_AV_SPD_DBYTE_NELEMS		HDMI_CORE_AV_REG(27)
+#define HDMI_CORE_AV_AUD_DBYTE(n)		HDMI_CORE_AV_REG(n * 4 + 0x210)
+#define HDMI_CORE_AV_AUD_DBYTE_NELEMS		HDMI_CORE_AV_REG(10)
+#define HDMI_CORE_AV_MPEG_DBYTE		HDMI_CORE_AV_REG(0x290)
+#define HDMI_CORE_AV_MPEG_DBYTE_NELEMS		HDMI_CORE_AV_REG(27)
+#define HDMI_CORE_AV_GEN_DBYTE			HDMI_CORE_AV_REG(0x300)
+#define HDMI_CORE_AV_GEN_DBYTE_NELEMS		HDMI_CORE_AV_REG(31)
+#define HDMI_CORE_AV_GEN2_DBYTE		HDMI_CORE_AV_REG(0x380)
+#define HDMI_CORE_AV_GEN2_DBYTE_NELEMS		HDMI_CORE_AV_REG(31)
+#define HDMI_CORE_AV_ACR_CTRL			HDMI_CORE_AV_REG(0x4)
+#define HDMI_CORE_AV_FREQ_SVAL			HDMI_CORE_AV_REG(0x8)
+#define HDMI_CORE_AV_N_SVAL1			HDMI_CORE_AV_REG(0xC)
+#define HDMI_CORE_AV_N_SVAL2			HDMI_CORE_AV_REG(0x10)
+#define HDMI_CORE_AV_N_SVAL3			HDMI_CORE_AV_REG(0x14)
+#define HDMI_CORE_AV_CTS_SVAL1			HDMI_CORE_AV_REG(0x18)
+#define HDMI_CORE_AV_CTS_SVAL2			HDMI_CORE_AV_REG(0x1C)
+#define HDMI_CORE_AV_CTS_SVAL3			HDMI_CORE_AV_REG(0x20)
+#define HDMI_CORE_AV_CTS_HVAL1			HDMI_CORE_AV_REG(0x24)
+#define HDMI_CORE_AV_CTS_HVAL2			HDMI_CORE_AV_REG(0x28)
+#define HDMI_CORE_AV_CTS_HVAL3			HDMI_CORE_AV_REG(0x2C)
+#define HDMI_CORE_AV_AUD_MODE			HDMI_CORE_AV_REG(0x50)
+#define HDMI_CORE_AV_SPDIF_CTRL		HDMI_CORE_AV_REG(0x54)
+#define HDMI_CORE_AV_HW_SPDIF_FS		HDMI_CORE_AV_REG(0x60)
+#define HDMI_CORE_AV_SWAP_I2S			HDMI_CORE_AV_REG(0x64)
+#define HDMI_CORE_AV_SPDIF_ERTH		HDMI_CORE_AV_REG(0x6C)
+#define HDMI_CORE_AV_I2S_IN_MAP		HDMI_CORE_AV_REG(0x70)
+#define HDMI_CORE_AV_I2S_IN_CTRL		HDMI_CORE_AV_REG(0x74)
+#define HDMI_CORE_AV_I2S_CHST0			HDMI_CORE_AV_REG(0x78)
+#define HDMI_CORE_AV_I2S_CHST1			HDMI_CORE_AV_REG(0x7C)
+#define HDMI_CORE_AV_I2S_CHST2			HDMI_CORE_AV_REG(0x80)
+#define HDMI_CORE_AV_I2S_CHST4			HDMI_CORE_AV_REG(0x84)
+#define HDMI_CORE_AV_I2S_CHST5			HDMI_CORE_AV_REG(0x88)
+#define HDMI_CORE_AV_ASRC			HDMI_CORE_AV_REG(0x8C)
+#define HDMI_CORE_AV_I2S_IN_LEN		HDMI_CORE_AV_REG(0x90)
+#define HDMI_CORE_AV_HDMI_CTRL			HDMI_CORE_AV_REG(0xBC)
+#define HDMI_CORE_AV_AUDO_TXSTAT		HDMI_CORE_AV_REG(0xC0)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_1		HDMI_CORE_AV_REG(0xCC)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_2		HDMI_CORE_AV_REG(0xD0)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_3		HDMI_CORE_AV_REG(0xD4)
+#define HDMI_CORE_AV_TEST_TXCTRL		HDMI_CORE_AV_REG(0xF0)
+#define HDMI_CORE_AV_DPD			HDMI_CORE_AV_REG(0xF4)
+#define HDMI_CORE_AV_PB_CTRL1			HDMI_CORE_AV_REG(0xF8)
+#define HDMI_CORE_AV_PB_CTRL2			HDMI_CORE_AV_REG(0xFC)
+#define HDMI_CORE_AV_AVI_TYPE			HDMI_CORE_AV_REG(0x100)
+#define HDMI_CORE_AV_AVI_VERS			HDMI_CORE_AV_REG(0x104)
+#define HDMI_CORE_AV_AVI_LEN			HDMI_CORE_AV_REG(0x108)
+#define HDMI_CORE_AV_AVI_CHSUM			HDMI_CORE_AV_REG(0x10C)
+#define HDMI_CORE_AV_SPD_TYPE			HDMI_CORE_AV_REG(0x180)
+#define HDMI_CORE_AV_SPD_VERS			HDMI_CORE_AV_REG(0x184)
+#define HDMI_CORE_AV_SPD_LEN			HDMI_CORE_AV_REG(0x188)
+#define HDMI_CORE_AV_SPD_CHSUM			HDMI_CORE_AV_REG(0x18C)
+#define HDMI_CORE_AV_AUDIO_TYPE		HDMI_CORE_AV_REG(0x200)
+#define HDMI_CORE_AV_AUDIO_VERS		HDMI_CORE_AV_REG(0x204)
+#define HDMI_CORE_AV_AUDIO_LEN			HDMI_CORE_AV_REG(0x208)
+#define HDMI_CORE_AV_AUDIO_CHSUM		HDMI_CORE_AV_REG(0x20C)
+#define HDMI_CORE_AV_MPEG_TYPE			HDMI_CORE_AV_REG(0x280)
+#define HDMI_CORE_AV_MPEG_VERS			HDMI_CORE_AV_REG(0x284)
+#define HDMI_CORE_AV_MPEG_LEN			HDMI_CORE_AV_REG(0x288)
+#define HDMI_CORE_AV_MPEG_CHSUM		HDMI_CORE_AV_REG(0x28C)
+#define HDMI_CORE_AV_CP_BYTE1			HDMI_CORE_AV_REG(0x37C)
+#define HDMI_CORE_AV_CEC_ADDR_ID		HDMI_CORE_AV_REG(0x3FC)
+#define HDMI_CORE_AV_SPD_DBYTE_ELSIZE		0x4
+#define HDMI_CORE_AV_GEN2_DBYTE_ELSIZE		0x4
+#define HDMI_CORE_AV_MPEG_DBYTE_ELSIZE		0x4
+#define HDMI_CORE_AV_GEN_DBYTE_ELSIZE		0x4
+
+/* PLL */
+#define HDMI_PLL_REG(idx)			HDMI_REG(HDMI_PLLCTRL + idx)
+
+#define PLLCTRL_PLL_CONTROL			HDMI_PLL_REG(0x0)
+#define PLLCTRL_PLL_STATUS			HDMI_PLL_REG(0x4)
+#define PLLCTRL_PLL_GO				HDMI_PLL_REG(0x8)
+#define PLLCTRL_CFG1				HDMI_PLL_REG(0xC)
+#define PLLCTRL_CFG2				HDMI_PLL_REG(0x10)
+#define PLLCTRL_CFG3				HDMI_PLL_REG(0x14)
+#define PLLCTRL_CFG4				HDMI_PLL_REG(0x20)
+
+/* HDMI PHY */
+#define HDMI_PHY_REG(idx)			HDMI_REG(HDMI_PHY + idx)
+
+#define HDMI_TXPHY_TX_CTRL			HDMI_PHY_REG(0x0)
+#define HDMI_TXPHY_DIGITAL_CTRL		HDMI_PHY_REG(0x4)
+#define HDMI_TXPHY_POWER_CTRL			HDMI_PHY_REG(0x8)
+#define HDMI_TXPHY_PAD_CFG_CTRL		HDMI_PHY_REG(0xC)
+
+/* HDMI EDID Length  */
+#define HDMI_EDID_MAX_LENGTH			256
+#define EDID_TIMING_DESCRIPTOR_SIZE		0x12
+#define EDID_DESCRIPTOR_BLOCK0_ADDRESS		0x36
+#define EDID_DESCRIPTOR_BLOCK1_ADDRESS		0x80
+#define EDID_SIZE_BLOCK0_TIMING_DESCRIPTOR	4
+#define EDID_SIZE_BLOCK1_TIMING_DESCRIPTOR	4
+
+#define OMAP_HDMI_TIMINGS_NB			34
+
+#define REG_FLD_MOD(idx, val, start, end) \
+	hdmi_write_reg(idx, FLD_MOD(hdmi_read_reg(idx), val, start, end))
+#define REG_GET(idx, start, end) \
+	FLD_GET(hdmi_read_reg(idx), start, end)
+
+/* HDMI timing structure */
+struct hdmi_timings {
+	struct omap_video_timings timings;
+	int vsync_pol;
+	int hsync_pol;
+};
+
+enum hdmi_phy_pwr {
+	HDMI_PHYPWRCMD_OFF = 0,
+	HDMI_PHYPWRCMD_LDOON = 1,
+	HDMI_PHYPWRCMD_TXON = 2
+};
+
+enum hdmi_pll_pwr {
+	HDMI_PLLPWRCMD_ALLOFF = 0,
+	HDMI_PLLPWRCMD_PLLONLY = 1,
+	HDMI_PLLPWRCMD_BOTHON_ALLCLKS = 2,
+	HDMI_PLLPWRCMD_BOTHON_NOPHYCLK = 3
+};
+
+enum hdmi_clk_refsel {
+	HDMI_REFSEL_PCLK = 0,
+	HDMI_REFSEL_REF1 = 1,
+	HDMI_REFSEL_REF2 = 2,
+	HDMI_REFSEL_SYSCLK = 3
+};
+
+enum hdmi_core_inputbus_width {
+	HDMI_INPUT_8BIT = 0,
+	HDMI_INPUT_10BIT = 1,
+	HDMI_INPUT_12BIT = 2
+};
+
+enum hdmi_core_dither_trunc {
+	HDMI_OUTPUTTRUNCATION_8BIT = 0,
+	HDMI_OUTPUTTRUNCATION_10BIT = 1,
+	HDMI_OUTPUTTRUNCATION_12BIT = 2,
+	HDMI_OUTPUTDITHER_8BIT = 3,
+	HDMI_OUTPUTDITHER_10BIT = 4,
+	HDMI_OUTPUTDITHER_12BIT = 5
+};
+
+enum hdmi_core_deepcolor_ed {
+	HDMI_DEEPCOLORPACKECTDISABLE = 0,
+	HDMI_DEEPCOLORPACKECTENABLE = 1
+};
+
+enum hdmi_core_packet_mode {
+	HDMI_PACKETMODERESERVEDVALUE = 0,
+	HDMI_PACKETMODE24BITPERPIXEL = 4,
+	HDMI_PACKETMODE30BITPERPIXEL = 5,
+	HDMI_PACKETMODE36BITPERPIXEL = 6,
+	HDMI_PACKETMODE48BITPERPIXEL = 7
+};
+
+enum hdmi_core_hdmi_dvi {
+	HDMI_DVI = 0,
+	HDMI_HDMI = 1
+};
+
+enum hdmi_core_tclkselclkmult {
+	HDMI_FPLL05IDCK = 0,
+	HDMI_FPLL10IDCK = 1,
+	HDMI_FPLL20IDCK = 2,
+	HDMI_FPLL40IDCK = 3
+};
+
+enum hdmi_core_packet_ctrl {
+	HDMI_PACKETENABLE = 1,
+	HDMI_PACKETDISABLE = 0,
+	HDMI_PACKETREPEATON = 1,
+	HDMI_PACKETREPEATOFF = 0
+};
+
+/* INFOFRAME_AVI_ and INFOFRAME_AUDIO_ definitions */
+enum hdmi_core_infoframe {
+	HDMI_INFOFRAME_AVI_DB1Y_RGB = 0,
+	HDMI_INFOFRAME_AVI_DB1Y_YUV422 = 1,
+	HDMI_INFOFRAME_AVI_DB1Y_YUV444 = 2,
+	HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF = 0,
+	HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_ON =  1,
+	HDMI_INFOFRAME_AVI_DB1B_NO = 0,
+	HDMI_INFOFRAME_AVI_DB1B_VERT = 1,
+	HDMI_INFOFRAME_AVI_DB1B_HORI = 2,
+	HDMI_INFOFRAME_AVI_DB1B_VERTHORI = 3,
+	HDMI_INFOFRAME_AVI_DB1S_0 = 0,
+	HDMI_INFOFRAME_AVI_DB1S_1 = 1,
+	HDMI_INFOFRAME_AVI_DB1S_2 = 2,
+	HDMI_INFOFRAME_AVI_DB2C_NO = 0,
+	HDMI_INFOFRAME_AVI_DB2C_ITU601 = 1,
+	HDMI_INFOFRAME_AVI_DB2C_ITU709 = 2,
+	HDMI_INFOFRAME_AVI_DB2C_EC_EXTENDED = 3,
+	HDMI_INFOFRAME_AVI_DB2M_NO = 0,
+	HDMI_INFOFRAME_AVI_DB2M_43 = 1,
+	HDMI_INFOFRAME_AVI_DB2M_169 = 2,
+	HDMI_INFOFRAME_AVI_DB2R_SAME = 8,
+	HDMI_INFOFRAME_AVI_DB2R_43 = 9,
+	HDMI_INFOFRAME_AVI_DB2R_169 = 10,
+	HDMI_INFOFRAME_AVI_DB2R_149 = 11,
+	HDMI_INFOFRAME_AVI_DB3ITC_NO = 0,
+	HDMI_INFOFRAME_AVI_DB3ITC_YES = 1,
+	HDMI_INFOFRAME_AVI_DB3EC_XVYUV601 = 0,
+	HDMI_INFOFRAME_AVI_DB3EC_XVYUV709 = 1,
+	HDMI_INFOFRAME_AVI_DB3Q_DEFAULT = 0,
+	HDMI_INFOFRAME_AVI_DB3Q_LR = 1,
+	HDMI_INFOFRAME_AVI_DB3Q_FR = 2,
+	HDMI_INFOFRAME_AVI_DB3SC_NO = 0,
+	HDMI_INFOFRAME_AVI_DB3SC_HORI = 1,
+	HDMI_INFOFRAME_AVI_DB3SC_VERT = 2,
+	HDMI_INFOFRAME_AVI_DB3SC_HORIVERT = 3,
+	HDMI_INFOFRAME_AVI_DB5PR_NO = 0,
+	HDMI_INFOFRAME_AVI_DB5PR_2 = 1,
+	HDMI_INFOFRAME_AVI_DB5PR_3 = 2,
+	HDMI_INFOFRAME_AVI_DB5PR_4 = 3,
+	HDMI_INFOFRAME_AVI_DB5PR_5 = 4,
+	HDMI_INFOFRAME_AVI_DB5PR_6 = 5,
+	HDMI_INFOFRAME_AVI_DB5PR_7 = 6,
+	HDMI_INFOFRAME_AVI_DB5PR_8 = 7,
+	HDMI_INFOFRAME_AVI_DB5PR_9 = 8,
+	HDMI_INFOFRAME_AVI_DB5PR_10 = 9,
+	HDMI_INFOFRAME_AUDIO_DB1CT_FROM_STREAM = 0,
+	HDMI_INFOFRAME_AUDIO_DB1CT_IEC60958 = 1,
+	HDMI_INFOFRAME_AUDIO_DB1CT_AC3 = 2,
+	HDMI_INFOFRAME_AUDIO_DB1CT_MPEG1 = 3,
+	HDMI_INFOFRAME_AUDIO_DB1CT_MP3 = 4,
+	HDMI_INFOFRAME_AUDIO_DB1CT_MPEG2_MULTICH = 5,
+	HDMI_INFOFRAME_AUDIO_DB1CT_AAC = 6,
+	HDMI_INFOFRAME_AUDIO_DB1CT_DTS = 7,
+	HDMI_INFOFRAME_AUDIO_DB1CT_ATRAC = 8,
+	HDMI_INFOFRAME_AUDIO_DB1CT_ONEBIT = 9,
+	HDMI_INFOFRAME_AUDIO_DB1CT_DOLBY_DIGITAL_PLUS = 10,
+	HDMI_INFOFRAME_AUDIO_DB1CT_DTS_HD = 11,
+	HDMI_INFOFRAME_AUDIO_DB1CT_MAT = 12,
+	HDMI_INFOFRAME_AUDIO_DB1CT_DST = 13,
+	HDMI_INFOFRAME_AUDIO_DB1CT_WMA_PRO = 14,
+	HDMI_INFOFRAME_AUDIO_DB2SF_FROM_STREAM = 0,
+	HDMI_INFOFRAME_AUDIO_DB2SF_32000 = 1,
+	HDMI_INFOFRAME_AUDIO_DB2SF_44100 = 2,
+	HDMI_INFOFRAME_AUDIO_DB2SF_48000 = 3,
+	HDMI_INFOFRAME_AUDIO_DB2SF_88200 = 4,
+	HDMI_INFOFRAME_AUDIO_DB2SF_96000 = 5,
+	HDMI_INFOFRAME_AUDIO_DB2SF_176400 = 6,
+	HDMI_INFOFRAME_AUDIO_DB2SF_192000 = 7,
+	HDMI_INFOFRAME_AUDIO_DB2SS_FROM_STREAM = 0,
+	HDMI_INFOFRAME_AUDIO_DB2SS_16BIT = 1,
+	HDMI_INFOFRAME_AUDIO_DB2SS_20BIT = 2,
+	HDMI_INFOFRAME_AUDIO_DB2SS_24BIT = 3,
+	HDMI_INFOFRAME_AUDIO_DB5_DM_INH_PERMITTED = 0,
+	HDMI_INFOFRAME_AUDIO_DB5_DM_INH_PROHIBITED = 1
+};
+
+enum hdmi_packing_mode {
+	HDMI_PACK_10b_RGB_YUV444 = 0,
+	HDMI_PACK_24b_RGB_YUV444_YUV422 = 1,
+	HDMI_PACK_20b_YUV422 = 2,
+	HDMI_PACK_ALREADYPACKED = 7
+};
+
+enum hdmi_core_audio_sample_freq {
+	HDMI_AUDIO_FS_32000 = 0x3,
+	HDMI_AUDIO_FS_44100 = 0x0,
+	HDMI_AUDIO_FS_48000 = 0x2,
+	HDMI_AUDIO_FS_88200 = 0x8,
+	HDMI_AUDIO_FS_96000 = 0xA,
+	HDMI_AUDIO_FS_176400 = 0xC,
+	HDMI_AUDIO_FS_192000 = 0xE,
+	HDMI_AUDIO_FS_NOT_INDICATED = 0x1
+};
+
+enum hdmi_core_audio_layout {
+	HDMI_AUDIO_LAYOUT_2CH = 0,
+	HDMI_AUDIO_LAYOUT_8CH = 1
+};
+
+enum hdmi_core_cts_mode {
+	HDMI_AUDIO_CTS_MODE_HW = 0,
+	HDMI_AUDIO_CTS_MODE_SW = 1
+};
+
+enum hdmi_stereo_channels {
+	HDMI_AUDIO_STEREO_NOCHANNELS = 0,
+	HDMI_AUDIO_STEREO_ONECHANNEL = 1,
+	HDMI_AUDIO_STEREO_TWOCHANNELS = 2,
+	HDMI_AUDIO_STEREO_THREECHANNELS = 3,
+	HDMI_AUDIO_STEREO_FOURCHANNELS = 4
+};
+
+enum hdmi_audio_type {
+	HDMI_AUDIO_TYPE_LPCM = 0,
+	HDMI_AUDIO_TYPE_IEC = 1
+};
+
+enum hdmi_audio_justify {
+	HDMI_AUDIO_JUSTIFY_LEFT = 0,
+	HDMI_AUDIO_JUSTIFY_RIGHT = 1
+};
+
+enum hdmi_audio_sample_order {
+	HDMI_AUDIO_SAMPLE_RIGHT_FIRST = 0,
+	HDMI_AUDIO_SAMPLE_LEFT_FIRST = 1
+};
+
+enum hdmi_audio_samples_perword {
+	HDMI_AUDIO_ONEWORD_ONESAMPLE = 0,
+	HDMI_AUDIO_ONEWORD_TWOSAMPLES = 1
+};
+
+enum hdmi_audio_sample_size {
+	HDMI_AUDIO_SAMPLE_16BITS = 0,
+	HDMI_AUDIO_SAMPLE_24BITS = 1
+};
+
+enum hdmi_audio_transf_mode {
+	HDMI_AUDIO_TRANSF_DMA = 0,
+	HDMI_AUDIO_TRANSF_IRQ = 1
+};
+
+enum hdmi_audio_blk_strt_end_sig {
+	HDMI_AUDIO_BLOCK_SIG_STARTEND_ON = 0,
+	HDMI_AUDIO_BLOCK_SIG_STARTEND_OFF = 1
+};
+
+enum hdmi_audio_i2s_config {
+	HDMI_AUDIO_I2S_WS_POLARITY_LOW_IS_LEFT = 0,
+	HDMI_AUDIO_I2S_WS_POLARIT_YLOW_IS_RIGHT = 1,
+	HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST = 0,
+	HDMI_AUDIO_I2S_LSB_SHIFTED_FIRST = 1,
+	HDMI_AUDIO_I2S_MAX_WORD_20BITS = 0,
+	HDMI_AUDIO_I2S_MAX_WORD_24BITS = 1,
+	HDMI_AUDIO_I2S_CHST_WORD_NOT_SPECIFIED = 0,
+	HDMI_AUDIO_I2S_CHST_WORD_16_BITS = 1,
+	HDMI_AUDIO_I2S_CHST_WORD_17_BITS = 6,
+	HDMI_AUDIO_I2S_CHST_WORD_18_BITS = 2,
+	HDMI_AUDIO_I2S_CHST_WORD_19_BITS = 4,
+	HDMI_AUDIO_I2S_CHST_WORD_20_BITS_20MAX = 5,
+	HDMI_AUDIO_I2S_CHST_WORD_20_BITS_24MAX = 1,
+	HDMI_AUDIO_I2S_CHST_WORD_21_BITS = 6,
+	HDMI_AUDIO_I2S_CHST_WORD_22_BITS = 2,
+	HDMI_AUDIO_I2S_CHST_WORD_23_BITS = 4,
+	HDMI_AUDIO_I2S_CHST_WORD_24_BITS = 5,
+	HDMI_AUDIO_I2S_SCK_EDGE_FALLING = 0,
+	HDMI_AUDIO_I2S_SCK_EDGE_RISING = 1,
+	HDMI_AUDIO_I2S_VBIT_FOR_PCM = 0,
+	HDMI_AUDIO_I2S_VBIT_FOR_COMPRESSED = 1,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_NA = 0,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_16 = 2,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_17 = 12,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_18 = 4,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_19 = 8,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_20 = 10,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_21 = 13,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_22 = 5,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_23 = 9,
+	HDMI_AUDIO_I2S_INPUT_LENGTH_24 = 11,
+	HDMI_AUDIO_I2S_FIRST_BIT_SHIFT = 0,
+	HDMI_AUDIO_I2S_FIRST_BIT_NO_SHIFT = 1,
+	HDMI_AUDIO_I2S_SD0_EN = 1,
+	HDMI_AUDIO_I2S_SD1_EN = 1 << 1,
+	HDMI_AUDIO_I2S_SD2_EN = 1 << 2,
+	HDMI_AUDIO_I2S_SD3_EN = 1 << 3,
+};
+
+enum hdmi_audio_mclk_mode {
+	HDMI_AUDIO_MCLK_128FS = 0,
+	HDMI_AUDIO_MCLK_256FS = 1,
+	HDMI_AUDIO_MCLK_384FS = 2,
+	HDMI_AUDIO_MCLK_512FS = 3,
+	HDMI_AUDIO_MCLK_768FS = 4,
+	HDMI_AUDIO_MCLK_1024FS = 5,
+	HDMI_AUDIO_MCLK_1152FS = 6,
+	HDMI_AUDIO_MCLK_192FS = 7
+};
+
+struct hdmi_core_video_config {
+	enum hdmi_core_inputbus_width	ip_bus_width;
+	enum hdmi_core_dither_trunc	op_dither_truc;
+	enum hdmi_core_deepcolor_ed	deep_color_pkt;
+	enum hdmi_core_packet_mode	pkt_mode;
+	enum hdmi_core_hdmi_dvi		hdmi_dvi;
+	enum hdmi_core_tclkselclkmult	tclk_sel_clkmult;
+};
+
+/*
+ * Refer to section 8.2 in HDMI 1.3 specification for
+ * details about infoframe databytes
+ */
+struct hdmi_core_infoframe_avi {
+	u8	db1_format;
+		/* Y0, Y1 rgb,yCbCr */
+	u8	db1_active_info;
+		/* A0  Active information Present */
+	u8	db1_bar_info_dv;
+		/* B0, B1 Bar info data valid */
+	u8	db1_scan_info;
+		/* S0, S1 scan information */
+	u8	db2_colorimetry;
+		/* C0, C1 colorimetry */
+	u8	db2_aspect_ratio;
+		/* M0, M1 Aspect ratio (4:3, 16:9) */
+	u8	db2_active_fmt_ar;
+		/* R0...R3 Active format aspect ratio */
+	u8	db3_itc;
+		/* ITC IT content. */
+	u8	db3_ec;
+		/* EC0, EC1, EC2 Extended colorimetry */
+	u8	db3_q_range;
+		/* Q1, Q0 Quantization range */
+	u8	db3_nup_scaling;
+		/* SC1, SC0 Non-uniform picture scaling */
+	u8	db4_videocode;
+		/* VIC0..6 Video format identification */
+	u8	db5_pixel_repeat;
+		/* PR0..PR3 Pixel repetition factor */
+	u16	db6_7_line_eoftop;
+		/* Line number end of top bar */
+	u16	db8_9_line_sofbottom;
+		/* Line number start of bottom bar */
+	u16	db10_11_pixel_eofleft;
+		/* Pixel number end of left bar */
+	u16	db12_13_pixel_sofright;
+		/* Pixel number start of right bar */
+};
+/*
+ * Refer to section 8.2 in HDMI 1.3 specification for
+ * details about infoframe databytes
+ */
+struct hdmi_core_infoframe_audio {
+	u8 db1_coding_type;
+	u8 db1_channel_count;
+	u8 db2_sample_freq;
+	u8 db2_sample_size;
+	u8 db4_channel_alloc;
+	bool db5_downmix_inh;
+	u8 db5_lsv;	/* Level shift values for downmix */
+};
+
+struct hdmi_core_packet_enable_repeat {
+	u32	audio_pkt;
+	u32	audio_pkt_repeat;
+	u32	avi_infoframe;
+	u32	avi_infoframe_repeat;
+	u32	gen_cntrl_pkt;
+	u32	gen_cntrl_pkt_repeat;
+	u32	generic_pkt;
+	u32	generic_pkt_repeat;
+};
+
+struct hdmi_video_format {
+	enum hdmi_packing_mode	packing_mode;
+	u32			y_res;	/* Line per panel */
+	u32			x_res;	/* pixel per line */
+};
+
+struct hdmi_video_interface {
+	int	vsp;	/* Vsync polarity */
+	int	hsp;	/* Hsync polarity */
+	int	interlacing;
+	int	tm;	/* Timing mode */
+};
+
+struct hdmi_cm {
+	int	code;
+	int	mode;
+};
+
+struct hdmi_config {
+	struct hdmi_timings timings;
+	u16	interlace;
+	struct hdmi_cm cm;
+};
+
+struct hdmi_audio_format {
+	enum hdmi_stereo_channels		stereo_channels;
+	u8					active_chnnls_msk;
+	enum hdmi_audio_type			type;
+	enum hdmi_audio_justify			justification;
+	enum hdmi_audio_sample_order		sample_order;
+	enum hdmi_audio_samples_perword		samples_per_word;
+	enum hdmi_audio_sample_size		sample_size;
+	enum hdmi_audio_blk_strt_end_sig	en_sig_blk_strt_end;
+};
+
+struct hdmi_audio_dma {
+	u8				transfer_size;
+	u8				block_size;
+	enum hdmi_audio_transf_mode	mode;
+	u16				fifo_threshold;
+};
+
+struct hdmi_core_audio_i2s_config {
+	u8 word_max_length;
+	u8 word_length;
+	u8 in_length_bits;
+	u8 justification;
+	u8 en_high_bitrate_aud;
+	u8 sck_edge_mode;
+	u8 cbit_order;
+	u8 vbit;
+	u8 ws_polarity;
+	u8 direction;
+	u8 shift;
+	u8 active_sds;
+};
+
+struct hdmi_core_audio_config {
+	struct hdmi_core_audio_i2s_config	i2s_cfg;
+	enum hdmi_core_audio_sample_freq	freq_sample;
+	bool					fs_override;
+	u32					n;
+	u32					cts;
+	u32					aud_par_busclk;
+	enum hdmi_core_audio_layout		layout;
+	enum hdmi_core_cts_mode			cts_mode;
+	bool					use_mclk;
+	enum hdmi_audio_mclk_mode		mclk_mode;
+	bool					en_acr_pkt;
+	bool					en_dsd_audio;
+	bool					en_parallel_aud_input;
+	bool					en_spdif;
+};
+#endif
diff --git a/drivers/video/omap2/dss/hdmi_omap4_panel.c b/drivers/video/omap2/dss/hdmi_omap4_panel.c
new file mode 100644
index 000000000000..7d4f2bd7c506
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi_omap4_panel.c
@@ -0,0 +1,222 @@
+/*
+ * hdmi_omap4_panel.c
+ *
+ * HDMI library support functions for TI OMAP4 processors.
+ *
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors:	Mythri P k <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static struct {
+	struct mutex hdmi_lock;
+} hdmi;
+
+
+static int hdmi_panel_probe(struct omap_dss_device *dssdev)
+{
+	DSSDBG("ENTER hdmi_panel_probe\n");
+
+	dssdev->panel.config = OMAP_DSS_LCD_TFT |
+			OMAP_DSS_LCD_IVS | OMAP_DSS_LCD_IHS;
+
+	/*
+	 * Initialize the timings to 640 * 480
+	 * This is only for framebuffer update not for TV timing setting
+	 * Setting TV timing will be done only on enable
+	 */
+	dssdev->panel.timings.x_res = 640;
+	dssdev->panel.timings.y_res = 480;
+
+	DSSDBG("hdmi_panel_probe x_res= %d y_res = %d\n",
+		dssdev->panel.timings.x_res,
+		dssdev->panel.timings.y_res);
+	return 0;
+}
+
+static void hdmi_panel_remove(struct omap_dss_device *dssdev)
+{
+
+}
+
+static int hdmi_panel_enable(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+	DSSDBG("ENTER hdmi_panel_enable\n");
+
+	mutex_lock(&hdmi.hdmi_lock);
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
+		r = -EINVAL;
+		goto err;
+	}
+
+	r = omapdss_hdmi_display_enable(dssdev);
+	if (r) {
+		DSSERR("failed to power on\n");
+		goto err;
+	}
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+err:
+	mutex_unlock(&hdmi.hdmi_lock);
+
+	return r;
+}
+
+static void hdmi_panel_disable(struct omap_dss_device *dssdev)
+{
+	mutex_lock(&hdmi.hdmi_lock);
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+		omapdss_hdmi_display_disable(dssdev);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+	mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static int hdmi_panel_suspend(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+
+	mutex_lock(&hdmi.hdmi_lock);
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE) {
+		r = -EINVAL;
+		goto err;
+	}
+
+	dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+	omapdss_hdmi_display_disable(dssdev);
+
+err:
+	mutex_unlock(&hdmi.hdmi_lock);
+
+	return r;
+}
+
+static int hdmi_panel_resume(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+
+	mutex_lock(&hdmi.hdmi_lock);
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_SUSPENDED) {
+		r = -EINVAL;
+		goto err;
+	}
+
+	r = omapdss_hdmi_display_enable(dssdev);
+	if (r) {
+		DSSERR("failed to power on\n");
+		goto err;
+	}
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+err:
+	mutex_unlock(&hdmi.hdmi_lock);
+
+	return r;
+}
+
+static void hdmi_get_timings(struct omap_dss_device *dssdev,
+			struct omap_video_timings *timings)
+{
+	mutex_lock(&hdmi.hdmi_lock);
+
+	*timings = dssdev->panel.timings;
+
+	mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static void hdmi_set_timings(struct omap_dss_device *dssdev,
+			struct omap_video_timings *timings)
+{
+	DSSDBG("hdmi_set_timings\n");
+
+	mutex_lock(&hdmi.hdmi_lock);
+
+	dssdev->panel.timings = *timings;
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+		/* turn the hdmi off and on to get new timings to use */
+		omapdss_hdmi_display_disable(dssdev);
+		omapdss_hdmi_display_set_timing(dssdev);
+	}
+
+	mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static int hdmi_check_timings(struct omap_dss_device *dssdev,
+			struct omap_video_timings *timings)
+{
+	int r = 0;
+
+	DSSDBG("hdmi_check_timings\n");
+
+	mutex_lock(&hdmi.hdmi_lock);
+
+	r = omapdss_hdmi_display_check_timing(dssdev, timings);
+	if (r) {
+		DSSERR("Timing cannot be applied\n");
+		goto err;
+	}
+err:
+	mutex_unlock(&hdmi.hdmi_lock);
+	return r;
+}
+
+static struct omap_dss_driver hdmi_driver = {
+	.probe		= hdmi_panel_probe,
+	.remove		= hdmi_panel_remove,
+	.enable		= hdmi_panel_enable,
+	.disable	= hdmi_panel_disable,
+	.suspend	= hdmi_panel_suspend,
+	.resume		= hdmi_panel_resume,
+	.get_timings	= hdmi_get_timings,
+	.set_timings	= hdmi_set_timings,
+	.check_timings	= hdmi_check_timings,
+	.driver			= {
+		.name   = "hdmi_panel",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int hdmi_panel_init(void)
+{
+	mutex_init(&hdmi.hdmi_lock);
+
+	omap_dss_register_driver(&hdmi_driver);
+
+	return 0;
+}
+
+void hdmi_panel_exit(void)
+{
+	omap_dss_unregister_driver(&hdmi_driver);
+
+}
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 172d4e697309..13d72d5c714b 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -29,7 +29,7 @@
 #include <linux/spinlock.h>
 #include <linux/jiffies.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
@@ -275,6 +275,108 @@ static ssize_t manager_alpha_blending_enabled_store(
 	return size;
 }
 
+static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
+		char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", mgr->info.cpr_enable);
+}
+
+static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	int v;
+	int r;
+	bool enable;
+
+	if (!dss_has_feature(FEAT_CPR))
+		return -ENODEV;
+
+	r = kstrtoint(buf, 0, &v);
+	if (r)
+		return r;
+
+	enable = !!v;
+
+	mgr->get_manager_info(mgr, &info);
+
+	if (info.cpr_enable == enable)
+		return size;
+
+	info.cpr_enable = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
+		char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE,
+			"%d %d %d %d %d %d %d %d %d\n",
+			info.cpr_coefs.rr,
+			info.cpr_coefs.rg,
+			info.cpr_coefs.rb,
+			info.cpr_coefs.gr,
+			info.cpr_coefs.gg,
+			info.cpr_coefs.gb,
+			info.cpr_coefs.br,
+			info.cpr_coefs.bg,
+			info.cpr_coefs.bb);
+}
+
+static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	struct omap_dss_cpr_coefs coefs;
+	int r, i;
+	s16 *arr;
+
+	if (!dss_has_feature(FEAT_CPR))
+		return -ENODEV;
+
+	if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
+				&coefs.rr, &coefs.rg, &coefs.rb,
+				&coefs.gr, &coefs.gg, &coefs.gb,
+				&coefs.br, &coefs.bg, &coefs.bb) != 9)
+		return -EINVAL;
+
+	arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
+		coefs.gr, coefs.gg, coefs.gb,
+		coefs.br, coefs.bg, coefs.bb };
+
+	for (i = 0; i < 9; ++i) {
+		if (arr[i] < -512 || arr[i] > 511)
+			return -EINVAL;
+	}
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.cpr_coefs = coefs;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
 struct manager_attribute {
 	struct attribute attr;
 	ssize_t (*show)(struct omap_overlay_manager *, char *);
@@ -300,6 +402,12 @@ static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
 static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
 		manager_alpha_blending_enabled_show,
 		manager_alpha_blending_enabled_store);
+static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
+		manager_cpr_enable_show,
+		manager_cpr_enable_store);
+static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
+		manager_cpr_coef_show,
+		manager_cpr_coef_store);
 
 
 static struct attribute *manager_sysfs_attrs[] = {
@@ -310,6 +418,8 @@ static struct attribute *manager_sysfs_attrs[] = {
 	&manager_attr_trans_key_value.attr,
 	&manager_attr_trans_key_enabled.attr,
 	&manager_attr_alpha_blending_enabled.attr,
+	&manager_attr_cpr_enable.attr,
+	&manager_attr_cpr_coef.attr,
 	NULL
 };
 
@@ -391,32 +501,14 @@ struct overlay_cache_data {
 
 	bool enabled;
 
-	u32 paddr;
-	void __iomem *vaddr;
-	u16 screen_width;
-	u16 width;
-	u16 height;
-	enum omap_color_mode color_mode;
-	u8 rotation;
-	enum omap_dss_rotation_type rotation_type;
-	bool mirror;
-
-	u16 pos_x;
-	u16 pos_y;
-	u16 out_width;	/* if 0, out_width == width */
-	u16 out_height;	/* if 0, out_height == height */
-	u8 global_alpha;
-	u8 pre_mult_alpha;
+	struct omap_overlay_info info;
 
 	enum omap_channel channel;
 	bool replication;
 	bool ilace;
 
-	enum omap_burst_size burst_size;
 	u32 fifo_low;
 	u32 fifo_high;
-
-	bool manual_update;
 };
 
 struct manager_cache_data {
@@ -428,15 +520,8 @@ struct manager_cache_data {
 	 * VSYNC/EVSYNC */
 	bool shadow_dirty;
 
-	u32 default_color;
-
-	enum omap_dss_trans_key_type trans_key_type;
-	u32 trans_key;
-	bool trans_enabled;
-
-	bool alpha_enabled;
+	struct omap_overlay_manager_info info;
 
-	bool manual_upd_display;
 	bool manual_update;
 	bool do_manual_update;
 
@@ -515,6 +600,8 @@ static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
 
 	if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC) {
 		irq = DISPC_IRQ_EVSYNC_ODD;
+	} else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI) {
+		irq = DISPC_IRQ_EVSYNC_EVEN;
 	} else {
 		if (mgr->id == OMAP_DSS_CHANNEL_LCD)
 			irq = DISPC_IRQ_VSYNC;
@@ -536,23 +623,15 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 	if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
-	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+	if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE)
+		return 0;
+
+	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+			|| dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
 		irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
 	} else {
-		if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
-			enum omap_dss_update_mode mode;
-			mode = dssdev->driver->get_update_mode(dssdev);
-			if (mode != OMAP_DSS_UPDATE_AUTO)
-				return 0;
-
-			irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
-				DISPC_IRQ_FRAMEDONE
-				: DISPC_IRQ_FRAMEDONE2;
-		} else {
-			irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
-				DISPC_IRQ_VSYNC
-				: DISPC_IRQ_VSYNC2;
-		}
+		irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
+			DISPC_IRQ_VSYNC : DISPC_IRQ_VSYNC2;
 	}
 
 	mc = &dss_cache.manager_cache[mgr->id];
@@ -613,23 +692,15 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
 	if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
-	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+	if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE)
+		return 0;
+
+	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+			|| dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
 		irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
 	} else {
-		if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
-			enum omap_dss_update_mode mode;
-			mode = dssdev->driver->get_update_mode(dssdev);
-			if (mode != OMAP_DSS_UPDATE_AUTO)
-				return 0;
-
-			irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
-				DISPC_IRQ_FRAMEDONE
-				: DISPC_IRQ_FRAMEDONE2;
-		} else {
-			irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
-				DISPC_IRQ_VSYNC
-				: DISPC_IRQ_VSYNC2;
-		}
+		irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
+			DISPC_IRQ_VSYNC : DISPC_IRQ_VSYNC2;
 	}
 
 	oc = &dss_cache.overlay_cache[ovl->id];
@@ -715,10 +786,12 @@ static bool rectangle_intersects(int x1, int y1, int w1, int h1,
 
 static bool dispc_is_overlay_scaled(struct overlay_cache_data *oc)
 {
-	if (oc->out_width != 0 && oc->width != oc->out_width)
+	struct omap_overlay_info *oi = &oc->info;
+
+	if (oi->out_width != 0 && oi->width != oi->out_width)
 		return true;
 
-	if (oc->out_height != 0 && oc->height != oc->out_height)
+	if (oi->out_height != 0 && oi->height != oi->out_height)
 		return true;
 
 	return false;
@@ -728,6 +801,8 @@ static int configure_overlay(enum omap_plane plane)
 {
 	struct overlay_cache_data *c;
 	struct manager_cache_data *mc;
+	struct omap_overlay_info *oi;
+	struct omap_overlay_manager_info *mi;
 	u16 outw, outh;
 	u16 x, y, w, h;
 	u32 paddr;
@@ -737,6 +812,7 @@ static int configure_overlay(enum omap_plane plane)
 	DSSDBGF("%d", plane);
 
 	c = &dss_cache.overlay_cache[plane];
+	oi = &c->info;
 
 	if (!c->enabled) {
 		dispc_enable_plane(plane, 0);
@@ -744,21 +820,22 @@ static int configure_overlay(enum omap_plane plane)
 	}
 
 	mc = &dss_cache.manager_cache[c->channel];
+	mi = &mc->info;
 
-	x = c->pos_x;
-	y = c->pos_y;
-	w = c->width;
-	h = c->height;
-	outw = c->out_width == 0 ? c->width : c->out_width;
-	outh = c->out_height == 0 ? c->height : c->out_height;
-	paddr = c->paddr;
+	x = oi->pos_x;
+	y = oi->pos_y;
+	w = oi->width;
+	h = oi->height;
+	outw = oi->out_width == 0 ? oi->width : oi->out_width;
+	outh = oi->out_height == 0 ? oi->height : oi->out_height;
+	paddr = oi->paddr;
 
 	orig_w = w;
 	orig_h = h;
 	orig_outw = outw;
 	orig_outh = outh;
 
-	if (c->manual_update && mc->do_manual_update) {
+	if (mc->manual_update && mc->do_manual_update) {
 		unsigned bpp;
 		unsigned scale_x_m = w, scale_x_d = outw;
 		unsigned scale_y_m = h, scale_y_d = outh;
@@ -770,11 +847,18 @@ static int configure_overlay(enum omap_plane plane)
 			return 0;
 		}
 
-		switch (c->color_mode) {
+		switch (oi->color_mode) {
+		case OMAP_DSS_COLOR_NV12:
+			bpp = 8;
+			break;
 		case OMAP_DSS_COLOR_RGB16:
 		case OMAP_DSS_COLOR_ARGB16:
 		case OMAP_DSS_COLOR_YUV2:
 		case OMAP_DSS_COLOR_UYVY:
+		case OMAP_DSS_COLOR_RGBA16:
+		case OMAP_DSS_COLOR_RGBX16:
+		case OMAP_DSS_COLOR_ARGB16_1555:
+		case OMAP_DSS_COLOR_XRGB16_1555:
 			bpp = 16;
 			break;
 
@@ -793,23 +877,23 @@ static int configure_overlay(enum omap_plane plane)
 			BUG();
 		}
 
-		if (mc->x > c->pos_x) {
+		if (mc->x > oi->pos_x) {
 			x = 0;
-			outw -= (mc->x - c->pos_x);
-			paddr += (mc->x - c->pos_x) *
+			outw -= (mc->x - oi->pos_x);
+			paddr += (mc->x - oi->pos_x) *
 				scale_x_m / scale_x_d * bpp / 8;
 		} else {
-			x = c->pos_x - mc->x;
+			x = oi->pos_x - mc->x;
 		}
 
-		if (mc->y > c->pos_y) {
+		if (mc->y > oi->pos_y) {
 			y = 0;
-			outh -= (mc->y - c->pos_y);
-			paddr += (mc->y - c->pos_y) *
+			outh -= (mc->y - oi->pos_y);
+			paddr += (mc->y - oi->pos_y) *
 				scale_y_m / scale_y_d *
-				c->screen_width * bpp / 8;
+				oi->screen_width * bpp / 8;
 		} else {
-			y = c->pos_y - mc->y;
+			y = oi->pos_y - mc->y;
 		}
 
 		if (mc->w < (x + outw))
@@ -828,8 +912,8 @@ static int configure_overlay(enum omap_plane plane)
 		 * the width if the original width was bigger.
 		 */
 		if ((w & 1) &&
-				(c->color_mode == OMAP_DSS_COLOR_YUV2 ||
-				 c->color_mode == OMAP_DSS_COLOR_UYVY)) {
+				(oi->color_mode == OMAP_DSS_COLOR_YUV2 ||
+				 oi->color_mode == OMAP_DSS_COLOR_UYVY)) {
 			if (orig_w > w)
 				w += 1;
 			else
@@ -839,18 +923,19 @@ static int configure_overlay(enum omap_plane plane)
 
 	r = dispc_setup_plane(plane,
 			paddr,
-			c->screen_width,
+			oi->screen_width,
 			x, y,
 			w, h,
 			outw, outh,
-			c->color_mode,
+			oi->color_mode,
 			c->ilace,
-			c->rotation_type,
-			c->rotation,
-			c->mirror,
-			c->global_alpha,
-			c->pre_mult_alpha,
-			c->channel);
+			oi->rotation_type,
+			oi->rotation,
+			oi->mirror,
+			oi->global_alpha,
+			oi->pre_mult_alpha,
+			c->channel,
+			oi->p_uv_addr);
 
 	if (r) {
 		/* this shouldn't happen */
@@ -861,8 +946,7 @@ static int configure_overlay(enum omap_plane plane)
 
 	dispc_enable_replication(plane, c->replication);
 
-	dispc_set_burst_size(plane, c->burst_size);
-	dispc_setup_plane_fifo(plane, c->fifo_low, c->fifo_high);
+	dispc_set_fifo_threshold(plane, c->fifo_low, c->fifo_high);
 
 	dispc_enable_plane(plane, 1);
 
@@ -871,16 +955,21 @@ static int configure_overlay(enum omap_plane plane)
 
 static void configure_manager(enum omap_channel channel)
 {
-	struct manager_cache_data *c;
+	struct omap_overlay_manager_info *mi;
 
 	DSSDBGF("%d", channel);
 
-	c = &dss_cache.manager_cache[channel];
+	/* picking info from the cache */
+	mi = &dss_cache.manager_cache[channel].info;
 
-	dispc_set_default_color(channel, c->default_color);
-	dispc_set_trans_key(channel, c->trans_key_type, c->trans_key);
-	dispc_enable_trans_key(channel, c->trans_enabled);
-	dispc_enable_alpha_blending(channel, c->alpha_enabled);
+	dispc_set_default_color(channel, mi->default_color);
+	dispc_set_trans_key(channel, mi->trans_key_type, mi->trans_key);
+	dispc_enable_trans_key(channel, mi->trans_enabled);
+	dispc_enable_alpha_blending(channel, mi->alpha_enabled);
+	if (dss_has_feature(FEAT_CPR)) {
+		dispc_enable_cpr(channel, mi->cpr_enable);
+		dispc_set_cpr_coef(channel, &mi->cpr_coefs);
+	}
 }
 
 /* configure_dispc() tries to write values from cache to shadow registers.
@@ -915,7 +1004,7 @@ static int configure_dispc(void)
 		if (!oc->dirty)
 			continue;
 
-		if (oc->manual_update && !mc->do_manual_update)
+		if (mc->manual_update && !mc->do_manual_update)
 			continue;
 
 		if (mgr_busy[oc->channel]) {
@@ -963,7 +1052,7 @@ static int configure_dispc(void)
 		/* We don't need GO with manual update display. LCD iface will
 		 * always be turned off after frame, and new settings will be
 		 * taken in to use at next update */
-		if (!mc->manual_upd_display)
+		if (!mc->manual_update)
 			dispc_go(i);
 	}
 
@@ -998,6 +1087,7 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
 {
 	struct overlay_cache_data *oc;
 	struct manager_cache_data *mc;
+	struct omap_overlay_info *oi;
 	const int num_ovls = dss_feat_get_num_ovls();
 	struct omap_overlay_manager *mgr;
 	int i;
@@ -1040,6 +1130,7 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
 			unsigned outw, outh;
 
 			oc = &dss_cache.overlay_cache[i];
+			oi = &oc->info;
 
 			if (oc->channel != mgr->id)
 				continue;
@@ -1055,39 +1146,39 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
 			if (!dispc_is_overlay_scaled(oc))
 				continue;
 
-			outw = oc->out_width == 0 ?
-				oc->width : oc->out_width;
-			outh = oc->out_height == 0 ?
-				oc->height : oc->out_height;
+			outw = oi->out_width == 0 ?
+				oi->width : oi->out_width;
+			outh = oi->out_height == 0 ?
+				oi->height : oi->out_height;
 
 			/* is the overlay outside the update region? */
 			if (!rectangle_intersects(x, y, w, h,
-						oc->pos_x, oc->pos_y,
+						oi->pos_x, oi->pos_y,
 						outw, outh))
 				continue;
 
 			/* if the overlay totally inside the update region? */
-			if (rectangle_subset(oc->pos_x, oc->pos_y, outw, outh,
+			if (rectangle_subset(oi->pos_x, oi->pos_y, outw, outh,
 						x, y, w, h))
 				continue;
 
-			if (x > oc->pos_x)
-				x1 = oc->pos_x;
+			if (x > oi->pos_x)
+				x1 = oi->pos_x;
 			else
 				x1 = x;
 
-			if (y > oc->pos_y)
-				y1 = oc->pos_y;
+			if (y > oi->pos_y)
+				y1 = oi->pos_y;
 			else
 				y1 = y;
 
-			if ((x + w) < (oc->pos_x + outw))
-				x2 = oc->pos_x + outw;
+			if ((x + w) < (oi->pos_x + outw))
+				x2 = oi->pos_x + outw;
 			else
 				x2 = x + w;
 
-			if ((y + h) < (oc->pos_y + outh))
-				y2 = oc->pos_y + outh;
+			if ((y + h) < (oi->pos_y + outh))
+				y2 = oi->pos_y + outh;
 			else
 				y2 = y + h;
 
@@ -1223,6 +1314,10 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 
 	DSSDBG("omap_dss_mgr_apply(%s)\n", mgr->name);
 
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
 	spin_lock_irqsave(&dss_cache.lock, flags);
 
 	/* Configure overlays */
@@ -1262,22 +1357,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 
 		ovl->info_dirty = false;
 		oc->dirty = true;
-
-		oc->paddr = ovl->info.paddr;
-		oc->vaddr = ovl->info.vaddr;
-		oc->screen_width = ovl->info.screen_width;
-		oc->width = ovl->info.width;
-		oc->height = ovl->info.height;
-		oc->color_mode = ovl->info.color_mode;
-		oc->rotation = ovl->info.rotation;
-		oc->rotation_type = ovl->info.rotation_type;
-		oc->mirror = ovl->info.mirror;
-		oc->pos_x = ovl->info.pos_x;
-		oc->pos_y = ovl->info.pos_y;
-		oc->out_width = ovl->info.out_width;
-		oc->out_height = ovl->info.out_height;
-		oc->global_alpha = ovl->info.global_alpha;
-		oc->pre_mult_alpha = ovl->info.pre_mult_alpha;
+		oc->info = ovl->info;
 
 		oc->replication =
 			dss_use_replication(dssdev, ovl->info.color_mode);
@@ -1288,11 +1368,6 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 
 		oc->enabled = true;
 
-		oc->manual_update =
-			dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE &&
-			dssdev->driver->get_update_mode(dssdev) !=
-				OMAP_DSS_UPDATE_AUTO;
-
 		++num_planes_enabled;
 	}
 
@@ -1320,20 +1395,10 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 
 		mgr->info_dirty = false;
 		mc->dirty = true;
-
-		mc->default_color = mgr->info.default_color;
-		mc->trans_key_type = mgr->info.trans_key_type;
-		mc->trans_key = mgr->info.trans_key;
-		mc->trans_enabled = mgr->info.trans_enabled;
-		mc->alpha_enabled = mgr->info.alpha_enabled;
-
-		mc->manual_upd_display =
-			dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
+		mc->info = mgr->info;
 
 		mc->manual_update =
-			dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE &&
-			dssdev->driver->get_update_mode(dssdev) !=
-				OMAP_DSS_UPDATE_AUTO;
+			dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 	}
 
 	/* XXX TODO: Try to get fifomerge working. The problem is that it
@@ -1354,7 +1419,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 	/* Configure overlay fifos */
 	for (i = 0; i < omap_dss_get_num_overlays(); ++i) {
 		struct omap_dss_device *dssdev;
-		u32 size;
+		u32 size, burst_size;
 
 		ovl = omap_dss_get_overlay(i);
 
@@ -1372,19 +1437,22 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 		if (use_fifomerge)
 			size *= 3;
 
+		burst_size = dispc_get_burst_size(ovl->id);
+
 		switch (dssdev->type) {
 		case OMAP_DISPLAY_TYPE_DPI:
 		case OMAP_DISPLAY_TYPE_DBI:
 		case OMAP_DISPLAY_TYPE_SDI:
 		case OMAP_DISPLAY_TYPE_VENC:
+		case OMAP_DISPLAY_TYPE_HDMI:
 			default_get_overlay_fifo_thresholds(ovl->id, size,
-					&oc->burst_size, &oc->fifo_low,
+					burst_size, &oc->fifo_low,
 					&oc->fifo_high);
 			break;
 #ifdef CONFIG_OMAP2_DSS_DSI
 		case OMAP_DISPLAY_TYPE_DSI:
 			dsi_get_overlay_fifo_thresholds(ovl->id, size,
-					&oc->burst_size, &oc->fifo_low,
+					burst_size, &oc->fifo_low,
 					&oc->fifo_high);
 			break;
 #endif
@@ -1394,7 +1462,6 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 	}
 
 	r = 0;
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
 	if (!dss_cache.irq_enabled) {
 		u32 mask;
 
@@ -1407,10 +1474,11 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 		dss_cache.irq_enabled = true;
 	}
 	configure_dispc();
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
 
 	spin_unlock_irqrestore(&dss_cache.lock, flags);
 
+	dispc_runtime_put();
+
 	return r;
 }
 
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 456efef03c20..c84380c53c39 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -31,7 +31,7 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
@@ -84,32 +84,42 @@ static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
 
 	old_mgr = ovl->manager;
 
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
 	/* detach old manager */
 	if (old_mgr) {
 		r = ovl->unset_manager(ovl);
 		if (r) {
 			DSSERR("detach failed\n");
-			return r;
+			goto err;
 		}
 
 		r = old_mgr->apply(old_mgr);
 		if (r)
-			return r;
+			goto err;
 	}
 
 	if (mgr) {
 		r = ovl->set_manager(ovl, mgr);
 		if (r) {
 			DSSERR("Failed to attach overlay\n");
-			return r;
+			goto err;
 		}
 
 		r = mgr->apply(mgr);
 		if (r)
-			return r;
+			goto err;
 	}
 
+	dispc_runtime_put();
+
 	return size;
+
+err:
+	dispc_runtime_put();
+	return r;
 }
 
 static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
@@ -201,12 +211,16 @@ static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
 static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
 		size_t size)
 {
-	int r;
+	int r, enable;
 	struct omap_overlay_info info;
 
 	ovl->get_overlay_info(ovl, &info);
 
-	info.enabled = simple_strtoul(buf, NULL, 10);
+	r = kstrtoint(buf, 0, &enable);
+	if (r)
+		return r;
+
+	info.enabled = !!enable;
 
 	r = ovl->set_overlay_info(ovl, &info);
 	if (r)
@@ -231,8 +245,16 @@ static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
 		const char *buf, size_t size)
 {
 	int r;
+	u8 alpha;
 	struct omap_overlay_info info;
 
+	if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &alpha);
+	if (r)
+		return r;
+
 	ovl->get_overlay_info(ovl, &info);
 
 	/* Video1 plane does not support global alpha
@@ -242,7 +264,7 @@ static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
 			ovl->id == OMAP_DSS_VIDEO1)
 		info.global_alpha = 255;
 	else
-		info.global_alpha = simple_strtoul(buf, NULL, 10);
+		info.global_alpha = alpha;
 
 	r = ovl->set_overlay_info(ovl, &info);
 	if (r)
@@ -268,8 +290,13 @@ static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
 		const char *buf, size_t size)
 {
 	int r;
+	u8 alpha;
 	struct omap_overlay_info info;
 
+	r = kstrtou8(buf, 0, &alpha);
+	if (r)
+		return r;
+
 	ovl->get_overlay_info(ovl, &info);
 
 	/* only GFX and Video2 plane support pre alpha multiplied
@@ -279,7 +306,7 @@ static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
 		ovl->id == OMAP_DSS_VIDEO1)
 		info.pre_mult_alpha = 0;
 	else
-		info.pre_mult_alpha = simple_strtoul(buf, NULL, 10);
+		info.pre_mult_alpha = alpha;
 
 	r = ovl->set_overlay_info(ovl, &info);
 	if (r)
@@ -490,16 +517,19 @@ static int omap_dss_set_manager(struct omap_overlay *ovl,
 
 	ovl->manager = mgr;
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-	/* XXX: on manual update display, in auto update mode, a bug happens
-	 * here. When an overlay is first enabled on LCD, then it's disabled,
-	 * and the manager is changed to TV, we sometimes get SYNC_LOST_DIGIT
-	 * errors. Waiting before changing the channel_out fixes it. I'm
-	 * guessing that the overlay is still somehow being used for the LCD,
-	 * but I don't understand how or why. */
-	msleep(40);
+	/* XXX: When there is an overlay on a DSI manual update display, and
+	 * the overlay is first disabled, then moved to tv, and enabled, we
+	 * seem to get SYNC_LOST_DIGIT error.
+	 *
+	 * Waiting doesn't seem to help, but updating the manual update display
+	 * after disabling the overlay seems to fix this. This hints that the
+	 * overlay is perhaps somehow tied to the LCD output until the output
+	 * is updated.
+	 *
+	 * Userspace workaround for this is to update the LCD after disabling
+	 * the overlay, but before moving the overlay to TV.
+	 */
 	dispc_set_channel_out(ovl->id, mgr->id);
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
 
 	return 0;
 }
@@ -679,7 +709,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
 			lcd2_mgr->set_device(lcd2_mgr, dssdev);
 			mgr = lcd2_mgr;
 		}
-	} else if (dssdev->type != OMAP_DISPLAY_TYPE_VENC) {
+	} else if (dssdev->type != OMAP_DISPLAY_TYPE_VENC
+			&& dssdev->type != OMAP_DISPLAY_TYPE_HDMI) {
 		if (!lcd_mgr->device || force) {
 			if (lcd_mgr->device)
 				lcd_mgr->unset_device(lcd_mgr);
@@ -688,7 +719,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
 		}
 	}
 
-	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+			|| dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
 		if (!tv_mgr->device || force) {
 			if (tv_mgr->device)
 				tv_mgr->unset_device(tv_mgr);
@@ -698,6 +730,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
 	}
 
 	if (mgr) {
+		dispc_runtime_get();
+
 		for (i = 0; i < dss_feat_get_num_ovls(); i++) {
 			struct omap_overlay *ovl;
 			ovl = omap_dss_get_overlay(i);
@@ -707,6 +741,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
 				omap_dss_set_manager(ovl, mgr);
 			}
 		}
+
+		dispc_runtime_put();
 	}
 }
 
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index 10a2ffe02882..39f4c597026a 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -32,12 +32,13 @@
 #include <linux/ktime.h>
 #include <linux/hrtimer.h>
 #include <linux/seq_file.h>
+#include <linux/semaphore.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include "dss.h"
 
-#define RFBI_BASE               0x48050800
-
 struct rfbi_reg { u16 idx; };
 
 #define RFBI_REG(idx)		((const struct rfbi_reg) { idx })
@@ -67,9 +68,6 @@ struct rfbi_reg { u16 idx; };
 #define REG_FLD_MOD(idx, val, start, end) \
 	rfbi_write_reg(idx, FLD_MOD(rfbi_read_reg(idx), val, start, end))
 
-/* To work around an RFBI transfer rate limitation */
-#define OMAP_RFBI_RATE_LIMIT    1
-
 enum omap_rfbi_cycleformat {
 	OMAP_DSS_RFBI_CYCLEFORMAT_1_1 = 0,
 	OMAP_DSS_RFBI_CYCLEFORMAT_2_1 = 1,
@@ -91,15 +89,11 @@ enum omap_rfbi_parallelmode {
 	OMAP_DSS_RFBI_PARALLELMODE_16 = 3,
 };
 
-enum update_cmd {
-	RFBI_CMD_UPDATE = 0,
-	RFBI_CMD_SYNC   = 1,
-};
-
 static int rfbi_convert_timings(struct rfbi_timings *t);
 static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div);
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem	*base;
 
 	unsigned long	l4_khz;
@@ -115,20 +109,9 @@ static struct {
 
 	struct omap_dss_device *dssdev[2];
 
-	struct kfifo      cmd_fifo;
-	spinlock_t        cmd_lock;
-	struct completion cmd_done;
-	atomic_t          cmd_fifo_full;
-	atomic_t          cmd_pending;
+	struct semaphore bus_lock;
 } rfbi;
 
-struct update_region {
-	u16	x;
-	u16     y;
-	u16     w;
-	u16     h;
-};
-
 static inline void rfbi_write_reg(const struct rfbi_reg idx, u32 val)
 {
 	__raw_writel(val, rfbi.base + idx.idx);
@@ -139,17 +122,41 @@ static inline u32 rfbi_read_reg(const struct rfbi_reg idx)
 	return __raw_readl(rfbi.base + idx.idx);
 }
 
-static void rfbi_enable_clocks(bool enable)
+static int rfbi_runtime_get(void)
 {
-	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	int r;
+
+	DSSDBG("rfbi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&rfbi.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
 }
 
+static void rfbi_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("rfbi_runtime_put\n");
+
+	r = pm_runtime_put(&rfbi.pdev->dev);
+	WARN_ON(r < 0);
+}
+
+void rfbi_bus_lock(void)
+{
+	down(&rfbi.bus_lock);
+}
+EXPORT_SYMBOL(rfbi_bus_lock);
+
+void rfbi_bus_unlock(void)
+{
+	up(&rfbi.bus_lock);
+}
+EXPORT_SYMBOL(rfbi_bus_unlock);
+
 void omap_rfbi_write_command(const void *buf, u32 len)
 {
-	rfbi_enable_clocks(1);
 	switch (rfbi.parallelmode) {
 	case OMAP_DSS_RFBI_PARALLELMODE_8:
 	{
@@ -173,13 +180,11 @@ void omap_rfbi_write_command(const void *buf, u32 len)
 	default:
 		BUG();
 	}
-	rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_write_command);
 
 void omap_rfbi_read_data(void *buf, u32 len)
 {
-	rfbi_enable_clocks(1);
 	switch (rfbi.parallelmode) {
 	case OMAP_DSS_RFBI_PARALLELMODE_8:
 	{
@@ -207,13 +212,11 @@ void omap_rfbi_read_data(void *buf, u32 len)
 	default:
 		BUG();
 	}
-	rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_read_data);
 
 void omap_rfbi_write_data(const void *buf, u32 len)
 {
-	rfbi_enable_clocks(1);
 	switch (rfbi.parallelmode) {
 	case OMAP_DSS_RFBI_PARALLELMODE_8:
 	{
@@ -238,7 +241,6 @@ void omap_rfbi_write_data(const void *buf, u32 len)
 		BUG();
 
 	}
-	rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_write_data);
 
@@ -250,8 +252,6 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
 	int horiz_offset = scr_width - w;
 	int i;
 
-	rfbi_enable_clocks(1);
-
 	if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_16 &&
 	   rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_8) {
 		const u16 __iomem *pd = buf;
@@ -296,12 +296,10 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
 	} else {
 		BUG();
 	}
-
-	rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_write_pixels);
 
-void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
+static void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
 		u16 height, void (*callback)(void *data), void *data)
 {
 	u32 l;
@@ -318,8 +316,6 @@ void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
 	rfbi.framedone_callback = callback;
 	rfbi.framedone_callback_data = data;
 
-	rfbi_enable_clocks(1);
-
 	rfbi_write_reg(RFBI_PIXEL_CNT, width * height);
 
 	l = rfbi_read_reg(RFBI_CONTROL);
@@ -338,15 +334,11 @@ static void framedone_callback(void *data, u32 mask)
 
 	REG_FLD_MOD(RFBI_CONTROL, 0, 0, 0);
 
-	rfbi_enable_clocks(0);
-
 	callback = rfbi.framedone_callback;
 	rfbi.framedone_callback = NULL;
 
 	if (callback != NULL)
 		callback(rfbi.framedone_callback_data);
-
-	atomic_set(&rfbi.cmd_pending, 0);
 }
 
 #if 1 /* VERBOSE */
@@ -436,7 +428,7 @@ static int calc_extif_timings(struct rfbi_timings *t)
 }
 
 
-void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
+static void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
 {
 	int r;
 
@@ -448,7 +440,6 @@ void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
 
 	BUG_ON(!t->converted);
 
-	rfbi_enable_clocks(1);
 	rfbi_write_reg(RFBI_ONOFF_TIME(rfbi_module), t->tim[0]);
 	rfbi_write_reg(RFBI_CYCLE_TIME(rfbi_module), t->tim[1]);
 
@@ -457,7 +448,6 @@ void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
 		    (t->tim[2] ? 1 : 0), 4, 4);
 
 	rfbi_print_timings();
-	rfbi_enable_clocks(0);
 }
 
 static int ps_to_rfbi_ticks(int time, int div)
@@ -473,59 +463,6 @@ static int ps_to_rfbi_ticks(int time, int div)
 	return ret;
 }
 
-#ifdef OMAP_RFBI_RATE_LIMIT
-unsigned long rfbi_get_max_tx_rate(void)
-{
-	unsigned long   l4_rate, dss1_rate;
-	int             min_l4_ticks = 0;
-	int             i;
-
-	/* According to TI this can't be calculated so make the
-	 * adjustments for a couple of known frequencies and warn for
-	 * others.
-	 */
-	static const struct {
-		unsigned long l4_clk;           /* HZ */
-		unsigned long dss1_clk;         /* HZ */
-		unsigned long min_l4_ticks;
-	} ftab[] = {
-		{ 55,   132,    7, },           /* 7.86 MPix/s */
-		{ 110,  110,    12, },          /* 9.16 MPix/s */
-		{ 110,  132,    10, },          /* 11   Mpix/s */
-		{ 120,  120,    10, },          /* 12   Mpix/s */
-		{ 133,  133,    10, },          /* 13.3 Mpix/s */
-	};
-
-	l4_rate = rfbi.l4_khz / 1000;
-	dss1_rate = dss_clk_get_rate(DSS_CLK_FCK1) / 1000000;
-
-	for (i = 0; i < ARRAY_SIZE(ftab); i++) {
-		/* Use a window instead of an exact match, to account
-		 * for different DPLL multiplier / divider pairs.
-		 */
-		if (abs(ftab[i].l4_clk - l4_rate) < 3 &&
-		    abs(ftab[i].dss1_clk - dss1_rate) < 3) {
-			min_l4_ticks = ftab[i].min_l4_ticks;
-			break;
-		}
-	}
-	if (i == ARRAY_SIZE(ftab)) {
-		/* Can't be sure, return anyway the maximum not
-		 * rate-limited. This might cause a problem only for the
-		 * tearing synchronisation.
-		 */
-		DSSERR("can't determine maximum RFBI transfer rate\n");
-		return rfbi.l4_khz * 1000;
-	}
-	return rfbi.l4_khz * 1000 / min_l4_ticks;
-}
-#else
-int rfbi_get_max_tx_rate(void)
-{
-	return rfbi.l4_khz * 1000;
-}
-#endif
-
 static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div)
 {
 	*clk_period = 1000000000 / rfbi.l4_khz;
@@ -645,7 +582,6 @@ int omap_rfbi_setup_te(enum omap_rfbi_te_mode mode,
 	DSSDBG("setup_te: mode %d hs %d vs %d hs_inv %d vs_inv %d\n",
 		mode, hs, vs, hs_pol_inv, vs_pol_inv);
 
-	rfbi_enable_clocks(1);
 	rfbi_write_reg(RFBI_HSYNC_WIDTH, hs);
 	rfbi_write_reg(RFBI_VSYNC_WIDTH, vs);
 
@@ -658,7 +594,6 @@ int omap_rfbi_setup_te(enum omap_rfbi_te_mode mode,
 		l &= ~(1 << 20);
 	else
 		l |= 1 << 20;
-	rfbi_enable_clocks(0);
 
 	return 0;
 }
@@ -673,7 +608,6 @@ int omap_rfbi_enable_te(bool enable, unsigned line)
 	if (line > (1 << 11) - 1)
 		return -EINVAL;
 
-	rfbi_enable_clocks(1);
 	l = rfbi_read_reg(RFBI_CONFIG(0));
 	l &= ~(0x3 << 2);
 	if (enable) {
@@ -683,50 +617,12 @@ int omap_rfbi_enable_te(bool enable, unsigned line)
 		rfbi.te_enabled = 0;
 	rfbi_write_reg(RFBI_CONFIG(0), l);
 	rfbi_write_reg(RFBI_LINE_NUMBER, line);
-	rfbi_enable_clocks(0);
 
 	return 0;
 }
 EXPORT_SYMBOL(omap_rfbi_enable_te);
 
-#if 0
-static void rfbi_enable_config(int enable1, int enable2)
-{
-	u32 l;
-	int cs = 0;
-
-	if (enable1)
-		cs |= 1<<0;
-	if (enable2)
-		cs |= 1<<1;
-
-	rfbi_enable_clocks(1);
-
-	l = rfbi_read_reg(RFBI_CONTROL);
-
-	l = FLD_MOD(l, cs, 3, 2);
-	l = FLD_MOD(l, 0, 1, 1);
-
-	rfbi_write_reg(RFBI_CONTROL, l);
-
-
-	l = rfbi_read_reg(RFBI_CONFIG(0));
-	l = FLD_MOD(l, 0, 3, 2); /* TRIGGERMODE: ITE */
-	/*l |= FLD_VAL(2, 8, 7); */ /* L4FORMAT, 2pix/L4 */
-	/*l |= FLD_VAL(0, 8, 7); */ /* L4FORMAT, 1pix/L4 */
-
-	l = FLD_MOD(l, 0, 16, 16); /* A0POLARITY */
-	l = FLD_MOD(l, 1, 20, 20); /* TE_VSYNC_POLARITY */
-	l = FLD_MOD(l, 1, 21, 21); /* HSYNCPOLARITY */
-
-	l = FLD_MOD(l, OMAP_DSS_RFBI_PARALLELMODE_8, 1, 0);
-	rfbi_write_reg(RFBI_CONFIG(0), l);
-
-	rfbi_enable_clocks(0);
-}
-#endif
-
-int rfbi_configure(int rfbi_module, int bpp, int lines)
+static int rfbi_configure(int rfbi_module, int bpp, int lines)
 {
 	u32 l;
 	int cycle1 = 0, cycle2 = 0, cycle3 = 0;
@@ -822,8 +718,6 @@ int rfbi_configure(int rfbi_module, int bpp, int lines)
 		break;
 	}
 
-	rfbi_enable_clocks(1);
-
 	REG_FLD_MOD(RFBI_CONTROL, 0, 3, 2); /* clear CS */
 
 	l = 0;
@@ -857,11 +751,15 @@ int rfbi_configure(int rfbi_module, int bpp, int lines)
 	DSSDBG("RFBI config: bpp %d, lines %d, cycles: 0x%x 0x%x 0x%x\n",
 	       bpp, lines, cycle1, cycle2, cycle3);
 
-	rfbi_enable_clocks(0);
-
 	return 0;
 }
-EXPORT_SYMBOL(rfbi_configure);
+
+int omap_rfbi_configure(struct omap_dss_device *dssdev, int pixel_size,
+		int data_lines)
+{
+	return rfbi_configure(dssdev->phy.rfbi.channel, pixel_size, data_lines);
+}
+EXPORT_SYMBOL(omap_rfbi_configure);
 
 int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
 		u16 *x, u16 *y, u16 *w, u16 *h)
@@ -922,7 +820,8 @@ void rfbi_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, rfbi_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	if (rfbi_runtime_get())
+		return;
 
 	DUMPREG(RFBI_REVISION);
 	DUMPREG(RFBI_SYSCONFIG);
@@ -953,58 +852,18 @@ void rfbi_dump_regs(struct seq_file *s)
 	DUMPREG(RFBI_VSYNC_WIDTH);
 	DUMPREG(RFBI_HSYNC_WIDTH);
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	rfbi_runtime_put();
 #undef DUMPREG
 }
 
-int rfbi_init(void)
-{
-	u32 rev;
-	u32 l;
-
-	spin_lock_init(&rfbi.cmd_lock);
-
-	init_completion(&rfbi.cmd_done);
-	atomic_set(&rfbi.cmd_fifo_full, 0);
-	atomic_set(&rfbi.cmd_pending, 0);
-
-	rfbi.base = ioremap(RFBI_BASE, SZ_256);
-	if (!rfbi.base) {
-		DSSERR("can't ioremap RFBI\n");
-		return -ENOMEM;
-	}
-
-	rfbi_enable_clocks(1);
-
-	msleep(10);
-
-	rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
-
-	/* Enable autoidle and smart-idle */
-	l = rfbi_read_reg(RFBI_SYSCONFIG);
-	l |= (1 << 0) | (2 << 3);
-	rfbi_write_reg(RFBI_SYSCONFIG, l);
-
-	rev = rfbi_read_reg(RFBI_REVISION);
-	printk(KERN_INFO "OMAP RFBI rev %d.%d\n",
-	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-	rfbi_enable_clocks(0);
-
-	return 0;
-}
-
-void rfbi_exit(void)
-{
-	DSSDBG("rfbi_exit\n");
-
-	iounmap(rfbi.base);
-}
-
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
 	int r;
 
+	r = rfbi_runtime_get();
+	if (r)
+		return r;
+
 	r = omap_dss_start_device(dssdev);
 	if (r) {
 		DSSERR("failed to start device\n");
@@ -1038,6 +897,7 @@ int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 err1:
 	omap_dss_stop_device(dssdev);
 err0:
+	rfbi_runtime_put();
 	return r;
 }
 EXPORT_SYMBOL(omapdss_rfbi_display_enable);
@@ -1047,6 +907,8 @@ void omapdss_rfbi_display_disable(struct omap_dss_device *dssdev)
 	omap_dispc_unregister_isr(framedone_callback, NULL,
 			DISPC_IRQ_FRAMEDONE);
 	omap_dss_stop_device(dssdev);
+
+	rfbi_runtime_put();
 }
 EXPORT_SYMBOL(omapdss_rfbi_display_disable);
 
@@ -1056,3 +918,127 @@ int rfbi_init_display(struct omap_dss_device *dssdev)
 	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 	return 0;
 }
+
+/* RFBI HW IP initialisation */
+static int omap_rfbihw_probe(struct platform_device *pdev)
+{
+	u32 rev;
+	struct resource *rfbi_mem;
+	struct clk *clk;
+	int r;
+
+	rfbi.pdev = pdev;
+
+	sema_init(&rfbi.bus_lock, 1);
+
+	rfbi_mem = platform_get_resource(rfbi.pdev, IORESOURCE_MEM, 0);
+	if (!rfbi_mem) {
+		DSSERR("can't get IORESOURCE_MEM RFBI\n");
+		r = -EINVAL;
+		goto err_ioremap;
+	}
+	rfbi.base = ioremap(rfbi_mem->start, resource_size(rfbi_mem));
+	if (!rfbi.base) {
+		DSSERR("can't ioremap RFBI\n");
+		r = -ENOMEM;
+		goto err_ioremap;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	r = rfbi_runtime_get();
+	if (r)
+		goto err_get_rfbi;
+
+	msleep(10);
+
+	if (cpu_is_omap24xx() || cpu_is_omap34xx() || cpu_is_omap3630())
+		clk = dss_get_ick();
+	else
+		clk = clk_get(&pdev->dev, "ick");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get ick\n");
+		r = PTR_ERR(clk);
+		goto err_get_ick;
+	}
+
+	rfbi.l4_khz = clk_get_rate(clk) / 1000;
+
+	clk_put(clk);
+
+	rev = rfbi_read_reg(RFBI_REVISION);
+	dev_dbg(&pdev->dev, "OMAP RFBI rev %d.%d\n",
+	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+	rfbi_runtime_put();
+
+	return 0;
+
+err_get_ick:
+	rfbi_runtime_put();
+err_get_rfbi:
+	pm_runtime_disable(&pdev->dev);
+	iounmap(rfbi.base);
+err_ioremap:
+	return r;
+}
+
+static int omap_rfbihw_remove(struct platform_device *pdev)
+{
+	pm_runtime_disable(&pdev->dev);
+	iounmap(rfbi.base);
+	return 0;
+}
+
+static int rfbi_runtime_suspend(struct device *dev)
+{
+	dispc_runtime_put();
+	dss_runtime_put();
+
+	return 0;
+}
+
+static int rfbi_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dss_runtime_get();
+	if (r < 0)
+		goto err_get_dss;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		goto err_get_dispc;
+
+	return 0;
+
+err_get_dispc:
+	dss_runtime_put();
+err_get_dss:
+	return r;
+}
+
+static const struct dev_pm_ops rfbi_pm_ops = {
+	.runtime_suspend = rfbi_runtime_suspend,
+	.runtime_resume = rfbi_runtime_resume,
+};
+
+static struct platform_driver omap_rfbihw_driver = {
+	.probe          = omap_rfbihw_probe,
+	.remove         = omap_rfbihw_remove,
+	.driver         = {
+		.name   = "omapdss_rfbi",
+		.owner  = THIS_MODULE,
+		.pm	= &rfbi_pm_ops,
+	},
+};
+
+int rfbi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_rfbihw_driver);
+}
+
+void rfbi_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_rfbihw_driver);
+}
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index b64adf7dfc88..3a688c871a45 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -20,17 +20,14 @@
 #define DSS_SUBSYS_NAME "SDI"
 
 #include <linux/kernel.h>
-#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
-#include <plat/cpu.h>
+#include <video/omapdss.h>
 #include "dss.h"
 
 static struct {
-	bool skip_init;
 	bool update_enabled;
 	struct regulator *vdds_sdi_reg;
 } sdi;
@@ -61,16 +58,20 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	r = omap_dss_start_device(dssdev);
 	if (r) {
 		DSSERR("failed to start device\n");
-		goto err0;
+		goto err_start_dev;
 	}
 
 	r = regulator_enable(sdi.vdds_sdi_reg);
 	if (r)
-		goto err1;
+		goto err_reg_enable;
 
-	/* In case of skip_init sdi_init has already enabled the clocks */
-	if (!sdi.skip_init)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	r = dss_runtime_get();
+	if (r)
+		goto err_get_dss;
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err_get_dispc;
 
 	sdi_basic_init(dssdev);
 
@@ -80,16 +81,10 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
 			dssdev->panel.acbi, dssdev->panel.acb);
 
-	if (!sdi.skip_init) {
-		r = dss_calc_clock_div(1, t->pixel_clock * 1000,
-				&dss_cinfo, &dispc_cinfo);
-	} else {
-		r = dss_get_clock_div(&dss_cinfo);
-		r = dispc_get_clock_div(dssdev->manager->id, &dispc_cinfo);
-	}
-
+	r = dss_calc_clock_div(1, t->pixel_clock * 1000,
+			&dss_cinfo, &dispc_cinfo);
 	if (r)
-		goto err2;
+		goto err_calc_clock_div;
 
 	fck = dss_cinfo.fck;
 	lck_div = dispc_cinfo.lck_div;
@@ -110,31 +105,34 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 
 	r = dss_set_clock_div(&dss_cinfo);
 	if (r)
-		goto err2;
+		goto err_set_dss_clock_div;
 
 	r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
 	if (r)
-		goto err2;
-
-	if (!sdi.skip_init) {
-		dss_sdi_init(dssdev->phy.sdi.datapairs);
-		r = dss_sdi_enable();
-		if (r)
-			goto err1;
-		mdelay(2);
-	}
+		goto err_set_dispc_clock_div;
 
-	dssdev->manager->enable(dssdev->manager);
+	dss_sdi_init(dssdev->phy.sdi.datapairs);
+	r = dss_sdi_enable();
+	if (r)
+		goto err_sdi_enable;
+	mdelay(2);
 
-	sdi.skip_init = 0;
+	dssdev->manager->enable(dssdev->manager);
 
 	return 0;
-err2:
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+
+err_sdi_enable:
+err_set_dispc_clock_div:
+err_set_dss_clock_div:
+err_calc_clock_div:
+	dispc_runtime_put();
+err_get_dispc:
+	dss_runtime_put();
+err_get_dss:
 	regulator_disable(sdi.vdds_sdi_reg);
-err1:
+err_reg_enable:
 	omap_dss_stop_device(dssdev);
-err0:
+err_start_dev:
 	return r;
 }
 EXPORT_SYMBOL(omapdss_sdi_display_enable);
@@ -145,7 +143,8 @@ void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 
 	dss_sdi_disable();
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dispc_runtime_put();
+	dss_runtime_put();
 
 	regulator_disable(sdi.vdds_sdi_reg);
 
@@ -157,25 +156,24 @@ int sdi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("SDI init\n");
 
+	if (sdi.vdds_sdi_reg == NULL) {
+		struct regulator *vdds_sdi;
+
+		vdds_sdi = dss_get_vdds_sdi();
+
+		if (IS_ERR(vdds_sdi)) {
+			DSSERR("can't get VDDS_SDI regulator\n");
+			return PTR_ERR(vdds_sdi);
+		}
+
+		sdi.vdds_sdi_reg = vdds_sdi;
+	}
+
 	return 0;
 }
 
-int sdi_init(bool skip_init)
+int sdi_init(void)
 {
-	/* we store this for first display enable, then clear it */
-	sdi.skip_init = skip_init;
-
-	sdi.vdds_sdi_reg = dss_get_vdds_sdi();
-	if (IS_ERR(sdi.vdds_sdi_reg)) {
-		DSSERR("can't get VDDS_SDI regulator\n");
-		return PTR_ERR(sdi.vdds_sdi_reg);
-	}
-	/*
-	 * Enable clocks already here, otherwise there would be a toggle
-	 * of them until sdi_display_enable is called.
-	 */
-	if (skip_init)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
 	return 0;
 }
 
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index eff35050e28a..173c66430dad 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -33,13 +33,13 @@
 #include <linux/seq_file.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
-
-#define VENC_BASE	0x48050C00
+#include "dss_features.h"
 
 /* Venc registers */
 #define VENC_REV_ID				0x00
@@ -289,10 +289,14 @@ const struct omap_video_timings omap_dss_ntsc_timings = {
 EXPORT_SYMBOL(omap_dss_ntsc_timings);
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem *base;
 	struct mutex venc_lock;
 	u32 wss_data;
 	struct regulator *vdda_dac_reg;
+
+	struct clk	*tv_clk;
+	struct clk	*tv_dac_clk;
 } venc;
 
 static inline void venc_write_reg(int idx, u32 val)
@@ -374,18 +378,32 @@ static void venc_reset(void)
 		}
 	}
 
+#ifdef CONFIG_OMAP2_DSS_SLEEP_AFTER_VENC_RESET
 	/* the magical sleep that makes things work */
+	/* XXX more info? What bug this circumvents? */
 	msleep(20);
+#endif
 }
 
-static void venc_enable_clocks(int enable)
+static int venc_runtime_get(void)
 {
-	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-				DSS_CLK_96M);
-	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-				DSS_CLK_96M);
+	int r;
+
+	DSSDBG("venc_runtime_get\n");
+
+	r = pm_runtime_get_sync(&venc.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+static void venc_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("venc_runtime_put\n");
+
+	r = pm_runtime_put(&venc.pdev->dev);
+	WARN_ON(r < 0);
 }
 
 static const struct venc_config *venc_timings_to_config(
@@ -404,8 +422,6 @@ static void venc_power_on(struct omap_dss_device *dssdev)
 {
 	u32 l;
 
-	venc_enable_clocks(1);
-
 	venc_reset();
 	venc_write_config(venc_timings_to_config(&dssdev->panel.timings));
 
@@ -446,8 +462,6 @@ static void venc_power_off(struct omap_dss_device *dssdev)
 		dssdev->platform_disable(dssdev);
 
 	regulator_disable(venc.vdda_dac_reg);
-
-	venc_enable_clocks(0);
 }
 
 
@@ -474,21 +488,32 @@ static int venc_panel_enable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&venc.venc_lock);
 
+	r = omap_dss_start_device(dssdev);
+	if (r) {
+		DSSERR("failed to start device\n");
+		goto err0;
+	}
+
 	if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
 		r = -EINVAL;
 		goto err1;
 	}
 
+	r = venc_runtime_get();
+	if (r)
+		goto err1;
+
 	venc_power_on(dssdev);
 
 	venc.wss_data = 0;
 
 	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
-	/* wait couple of vsyncs until enabling the LCD */
-	msleep(50);
-
+	mutex_unlock(&venc.venc_lock);
+	return 0;
 err1:
+	omap_dss_stop_device(dssdev);
+err0:
 	mutex_unlock(&venc.venc_lock);
 
 	return r;
@@ -511,10 +536,11 @@ static void venc_panel_disable(struct omap_dss_device *dssdev)
 
 	venc_power_off(dssdev);
 
-	/* wait at least 5 vsyncs after disabling the LCD */
-	msleep(100);
+	venc_runtime_put();
 
 	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+	omap_dss_stop_device(dssdev);
 end:
 	mutex_unlock(&venc.venc_lock);
 }
@@ -530,20 +556,6 @@ static int venc_panel_resume(struct omap_dss_device *dssdev)
 	return venc_panel_enable(dssdev);
 }
 
-static enum omap_dss_update_mode venc_get_update_mode(
-		struct omap_dss_device *dssdev)
-{
-	return OMAP_DSS_UPDATE_AUTO;
-}
-
-static int venc_set_update_mode(struct omap_dss_device *dssdev,
-		enum omap_dss_update_mode mode)
-{
-	if (mode != OMAP_DSS_UPDATE_AUTO)
-		return -EINVAL;
-	return 0;
-}
-
 static void venc_get_timings(struct omap_dss_device *dssdev,
 			struct omap_video_timings *timings)
 {
@@ -590,6 +602,7 @@ static u32 venc_get_wss(struct omap_dss_device *dssdev)
 static int venc_set_wss(struct omap_dss_device *dssdev,	u32 wss)
 {
 	const struct venc_config *config;
+	int r;
 
 	DSSDBG("venc_set_wss\n");
 
@@ -600,16 +613,19 @@ static int venc_set_wss(struct omap_dss_device *dssdev,	u32 wss)
 	/* Invert due to VENC_L21_WC_CTL:INV=1 */
 	venc.wss_data = (wss ^ 0xfffff) << 8;
 
-	venc_enable_clocks(1);
+	r = venc_runtime_get();
+	if (r)
+		goto err;
 
 	venc_write_reg(VENC_BSTAMP_WSS_DATA, config->bstamp_wss_data |
 			venc.wss_data);
 
-	venc_enable_clocks(0);
+	venc_runtime_put();
 
+err:
 	mutex_unlock(&venc.venc_lock);
 
-	return 0;
+	return r;
 }
 
 static struct omap_dss_driver venc_driver = {
@@ -624,9 +640,6 @@ static struct omap_dss_driver venc_driver = {
 	.get_resolution	= omapdss_default_get_resolution,
 	.get_recommended_bpp = omapdss_default_get_recommended_bpp,
 
-	.set_update_mode = venc_set_update_mode,
-	.get_update_mode = venc_get_update_mode,
-
 	.get_timings	= venc_get_timings,
 	.set_timings	= venc_set_timings,
 	.check_timings	= venc_check_timings,
@@ -641,50 +654,23 @@ static struct omap_dss_driver venc_driver = {
 };
 /* driver end */
 
-
-
-int venc_init(struct platform_device *pdev)
+int venc_init_display(struct omap_dss_device *dssdev)
 {
-	u8 rev_id;
+	DSSDBG("init_display\n");
 
-	mutex_init(&venc.venc_lock);
+	if (venc.vdda_dac_reg == NULL) {
+		struct regulator *vdda_dac;
 
-	venc.wss_data = 0;
+		vdda_dac = regulator_get(&venc.pdev->dev, "vdda_dac");
 
-	venc.base = ioremap(VENC_BASE, SZ_1K);
-	if (!venc.base) {
-		DSSERR("can't ioremap VENC\n");
-		return -ENOMEM;
-	}
+		if (IS_ERR(vdda_dac)) {
+			DSSERR("can't get VDDA_DAC regulator\n");
+			return PTR_ERR(vdda_dac);
+		}
 
-	venc.vdda_dac_reg = dss_get_vdda_dac();
-	if (IS_ERR(venc.vdda_dac_reg)) {
-		iounmap(venc.base);
-		DSSERR("can't get VDDA_DAC regulator\n");
-		return PTR_ERR(venc.vdda_dac_reg);
+		venc.vdda_dac_reg = vdda_dac;
 	}
 
-	venc_enable_clocks(1);
-
-	rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
-	printk(KERN_INFO "OMAP VENC rev %d\n", rev_id);
-
-	venc_enable_clocks(0);
-
-	return omap_dss_register_driver(&venc_driver);
-}
-
-void venc_exit(void)
-{
-	omap_dss_unregister_driver(&venc_driver);
-
-	iounmap(venc.base);
-}
-
-int venc_init_display(struct omap_dss_device *dssdev)
-{
-	DSSDBG("init_display\n");
-
 	return 0;
 }
 
@@ -692,7 +678,8 @@ void venc_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, venc_read_reg(r))
 
-	venc_enable_clocks(1);
+	if (venc_runtime_get())
+		return;
 
 	DUMPREG(VENC_F_CONTROL);
 	DUMPREG(VENC_VIDOUT_CTRL);
@@ -736,7 +723,180 @@ void venc_dump_regs(struct seq_file *s)
 	DUMPREG(VENC_OUTPUT_CONTROL);
 	DUMPREG(VENC_OUTPUT_TEST);
 
-	venc_enable_clocks(0);
+	venc_runtime_put();
 
 #undef DUMPREG
 }
+
+static int venc_get_clocks(struct platform_device *pdev)
+{
+	struct clk *clk;
+
+	clk = clk_get(&pdev->dev, "fck");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get fck\n");
+		return PTR_ERR(clk);
+	}
+
+	venc.tv_clk = clk;
+
+	if (dss_has_feature(FEAT_VENC_REQUIRES_TV_DAC_CLK)) {
+		if (cpu_is_omap34xx() || cpu_is_omap3630())
+			clk = clk_get(&pdev->dev, "dss_96m_fck");
+		else
+			clk = clk_get(&pdev->dev, "tv_dac_clk");
+		if (IS_ERR(clk)) {
+			DSSERR("can't get tv_dac_clk\n");
+			clk_put(venc.tv_clk);
+			return PTR_ERR(clk);
+		}
+	} else {
+		clk = NULL;
+	}
+
+	venc.tv_dac_clk = clk;
+
+	return 0;
+}
+
+static void venc_put_clocks(void)
+{
+	if (venc.tv_clk)
+		clk_put(venc.tv_clk);
+	if (venc.tv_dac_clk)
+		clk_put(venc.tv_dac_clk);
+}
+
+/* VENC HW IP initialisation */
+static int omap_venchw_probe(struct platform_device *pdev)
+{
+	u8 rev_id;
+	struct resource *venc_mem;
+	int r;
+
+	venc.pdev = pdev;
+
+	mutex_init(&venc.venc_lock);
+
+	venc.wss_data = 0;
+
+	venc_mem = platform_get_resource(venc.pdev, IORESOURCE_MEM, 0);
+	if (!venc_mem) {
+		DSSERR("can't get IORESOURCE_MEM VENC\n");
+		r = -EINVAL;
+		goto err_ioremap;
+	}
+	venc.base = ioremap(venc_mem->start, resource_size(venc_mem));
+	if (!venc.base) {
+		DSSERR("can't ioremap VENC\n");
+		r = -ENOMEM;
+		goto err_ioremap;
+	}
+
+	r = venc_get_clocks(pdev);
+	if (r)
+		goto err_get_clk;
+
+	pm_runtime_enable(&pdev->dev);
+
+	r = venc_runtime_get();
+	if (r)
+		goto err_get_venc;
+
+	rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
+	dev_dbg(&pdev->dev, "OMAP VENC rev %d\n", rev_id);
+
+	venc_runtime_put();
+
+	return omap_dss_register_driver(&venc_driver);
+
+err_get_venc:
+	pm_runtime_disable(&pdev->dev);
+	venc_put_clocks();
+err_get_clk:
+	iounmap(venc.base);
+err_ioremap:
+	return r;
+}
+
+static int omap_venchw_remove(struct platform_device *pdev)
+{
+	if (venc.vdda_dac_reg != NULL) {
+		regulator_put(venc.vdda_dac_reg);
+		venc.vdda_dac_reg = NULL;
+	}
+	omap_dss_unregister_driver(&venc_driver);
+
+	pm_runtime_disable(&pdev->dev);
+	venc_put_clocks();
+
+	iounmap(venc.base);
+	return 0;
+}
+
+static int venc_runtime_suspend(struct device *dev)
+{
+	if (venc.tv_dac_clk)
+		clk_disable(venc.tv_dac_clk);
+	clk_disable(venc.tv_clk);
+
+	dispc_runtime_put();
+	dss_runtime_put();
+
+	return 0;
+}
+
+static int venc_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dss_runtime_get();
+	if (r < 0)
+		goto err_get_dss;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		goto err_get_dispc;
+
+	clk_enable(venc.tv_clk);
+	if (venc.tv_dac_clk)
+		clk_enable(venc.tv_dac_clk);
+
+	return 0;
+
+err_get_dispc:
+	dss_runtime_put();
+err_get_dss:
+	return r;
+}
+
+static const struct dev_pm_ops venc_pm_ops = {
+	.runtime_suspend = venc_runtime_suspend,
+	.runtime_resume = venc_runtime_resume,
+};
+
+static struct platform_driver omap_venchw_driver = {
+	.probe          = omap_venchw_probe,
+	.remove         = omap_venchw_remove,
+	.driver         = {
+		.name   = "omapdss_venc",
+		.owner  = THIS_MODULE,
+		.pm	= &venc_pm_ops,
+	},
+};
+
+int venc_init_platform_driver(void)
+{
+	if (cpu_is_omap44xx())
+		return 0;
+
+	return platform_driver_register(&omap_venchw_driver);
+}
+
+void venc_uninit_platform_driver(void)
+{
+	if (cpu_is_omap44xx())
+		return;
+
+	return platform_driver_unregister(&omap_venchw_driver);
+}
diff --git a/drivers/video/omap2/omapfb/Kconfig b/drivers/video/omap2/omapfb/Kconfig
index 65149b22cf37..aa33386c81ff 100644
--- a/drivers/video/omap2/omapfb/Kconfig
+++ b/drivers/video/omap2/omapfb/Kconfig
@@ -1,5 +1,5 @@
 menuconfig FB_OMAP2
-        tristate "OMAP2/3 frame buffer support (EXPERIMENTAL)"
+        tristate "OMAP2+ frame buffer support (EXPERIMENTAL)"
         depends on FB && OMAP2_DSS
 
 	select OMAP2_VRAM
@@ -8,10 +8,10 @@ menuconfig FB_OMAP2
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
         help
-          Frame buffer driver for OMAP2/3 based boards.
+	  Frame buffer driver for OMAP2+ based boards.
 
 config FB_OMAP2_DEBUG_SUPPORT
-	bool "Debug support for OMAP2/3 FB"
+        bool "Debug support for OMAP2+ FB"
 	default y
 	depends on FB_OMAP2
 	help
diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index 6f435450987e..6b1ac23dbbd3 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -28,7 +28,7 @@
 #include <linux/omapfb.h>
 #include <linux/vmalloc.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/vrfb.h>
 #include <plat/vram.h>
 
@@ -316,67 +316,67 @@ int omapfb_update_window(struct fb_info *fbi,
 }
 EXPORT_SYMBOL(omapfb_update_window);
 
-static int omapfb_set_update_mode(struct fb_info *fbi,
+int omapfb_set_update_mode(struct fb_info *fbi,
 				   enum omapfb_update_mode mode)
 {
 	struct omap_dss_device *display = fb2display(fbi);
-	enum omap_dss_update_mode um;
+	struct omapfb_info *ofbi = FB2OFB(fbi);
+	struct omapfb2_device *fbdev = ofbi->fbdev;
+	struct omapfb_display_data *d;
 	int r;
 
-	if (!display || !display->driver->set_update_mode)
+	if (!display)
 		return -EINVAL;
 
-	switch (mode) {
-	case OMAPFB_UPDATE_DISABLED:
-		um = OMAP_DSS_UPDATE_DISABLED;
-		break;
+	if (mode != OMAPFB_AUTO_UPDATE && mode != OMAPFB_MANUAL_UPDATE)
+		return -EINVAL;
 
-	case OMAPFB_AUTO_UPDATE:
-		um = OMAP_DSS_UPDATE_AUTO;
-		break;
+	omapfb_lock(fbdev);
 
-	case OMAPFB_MANUAL_UPDATE:
-		um = OMAP_DSS_UPDATE_MANUAL;
-		break;
+	d = get_display_data(fbdev, display);
 
-	default:
-		return -EINVAL;
+	if (d->update_mode == mode) {
+		omapfb_unlock(fbdev);
+		return 0;
 	}
 
-	r = display->driver->set_update_mode(display, um);
+	r = 0;
+
+	if (display->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
+		if (mode == OMAPFB_AUTO_UPDATE)
+			omapfb_start_auto_update(fbdev, display);
+		else /* MANUAL_UPDATE */
+			omapfb_stop_auto_update(fbdev, display);
+
+		d->update_mode = mode;
+	} else { /* AUTO_UPDATE */
+		if (mode == OMAPFB_MANUAL_UPDATE)
+			r = -EINVAL;
+	}
+
+	omapfb_unlock(fbdev);
 
 	return r;
 }
 
-static int omapfb_get_update_mode(struct fb_info *fbi,
+int omapfb_get_update_mode(struct fb_info *fbi,
 		enum omapfb_update_mode *mode)
 {
 	struct omap_dss_device *display = fb2display(fbi);
-	enum omap_dss_update_mode m;
+	struct omapfb_info *ofbi = FB2OFB(fbi);
+	struct omapfb2_device *fbdev = ofbi->fbdev;
+	struct omapfb_display_data *d;
 
 	if (!display)
 		return -EINVAL;
 
-	if (!display->driver->get_update_mode) {
-		*mode = OMAPFB_AUTO_UPDATE;
-		return 0;
-	}
+	omapfb_lock(fbdev);
 
-	m = display->driver->get_update_mode(display);
+	d = get_display_data(fbdev, display);
 
-	switch (m) {
-	case OMAP_DSS_UPDATE_DISABLED:
-		*mode = OMAPFB_UPDATE_DISABLED;
-		break;
-	case OMAP_DSS_UPDATE_AUTO:
-		*mode = OMAPFB_AUTO_UPDATE;
-		break;
-	case OMAP_DSS_UPDATE_MANUAL:
-		*mode = OMAPFB_MANUAL_UPDATE;
-		break;
-	default:
-		BUG();
-	}
+	*mode = d->update_mode;
+
+	omapfb_unlock(fbdev);
 
 	return 0;
 }
@@ -895,8 +895,16 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 		p.display_info.xres = xres;
 		p.display_info.yres = yres;
-		p.display_info.width = 0;
-		p.display_info.height = 0;
+
+		if (display->driver->get_dimensions) {
+			u32 w, h;
+			display->driver->get_dimensions(display, &w, &h);
+			p.display_info.width = w;
+			p.display_info.height = h;
+		} else {
+			p.display_info.width = 0;
+			p.display_info.height = 0;
+		}
 
 		if (copy_to_user((void __user *)arg, &p.display_info,
 					sizeof(p.display_info)))
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 4fdab8e9c496..602b71a92d3c 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -30,7 +30,7 @@
 #include <linux/platform_device.h>
 #include <linux/omapfb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/vram.h>
 #include <plat/vrfb.h>
 
@@ -46,6 +46,10 @@ static char *def_vram;
 static int def_vrfb;
 static int def_rotate;
 static int def_mirror;
+static bool auto_update;
+static unsigned int auto_update_freq;
+module_param(auto_update, bool, 0);
+module_param(auto_update_freq, uint, 0644);
 
 #ifdef DEBUG
 unsigned int omapfb_debug;
@@ -702,8 +706,16 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
 			var->xres, var->yres,
 			var->xres_virtual, var->yres_virtual);
 
-	var->height             = -1;
-	var->width              = -1;
+	if (display && display->driver->get_dimensions) {
+		u32 w, h;
+		display->driver->get_dimensions(display, &w, &h);
+		var->width = DIV_ROUND_CLOSEST(w, 1000);
+		var->height = DIV_ROUND_CLOSEST(h, 1000);
+	} else {
+		var->height = -1;
+		var->width = -1;
+	}
+
 	var->grayscale          = 0;
 
 	if (display && display->driver->get_timings) {
@@ -749,35 +761,6 @@ static int omapfb_open(struct fb_info *fbi, int user)
 
 static int omapfb_release(struct fb_info *fbi, int user)
 {
-#if 0
-	struct omapfb_info *ofbi = FB2OFB(fbi);
-	struct omapfb2_device *fbdev = ofbi->fbdev;
-	struct omap_dss_device *display = fb2display(fbi);
-
-	DBG("Closing fb with plane index %d\n", ofbi->id);
-
-	omapfb_lock(fbdev);
-
-	if (display && display->get_update_mode && display->update) {
-		/* XXX this update should be removed, I think. But it's
-		 * good for debugging */
-		if (display->get_update_mode(display) ==
-				OMAP_DSS_UPDATE_MANUAL) {
-			u16 w, h;
-
-			if (display->sync)
-				display->sync(display);
-
-			display->get_resolution(display, &w, &h);
-			display->update(display, 0, 0, w, h);
-		}
-	}
-
-	if (display && display->sync)
-		display->sync(display);
-
-	omapfb_unlock(fbdev);
-#endif
 	return 0;
 }
 
@@ -1263,7 +1246,7 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 	struct omapfb_info *ofbi = FB2OFB(fbi);
 	struct omapfb2_device *fbdev = ofbi->fbdev;
 	struct omap_dss_device *display = fb2display(fbi);
-	int do_update = 0;
+	struct omapfb_display_data *d;
 	int r = 0;
 
 	if (!display)
@@ -1271,6 +1254,8 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 
 	omapfb_lock(fbdev);
 
+	d = get_display_data(fbdev, display);
+
 	switch (blank) {
 	case FB_BLANK_UNBLANK:
 		if (display->state != OMAP_DSS_DISPLAY_SUSPENDED)
@@ -1279,10 +1264,10 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 		if (display->driver->resume)
 			r = display->driver->resume(display);
 
-		if (r == 0 && display->driver->get_update_mode &&
-				display->driver->get_update_mode(display) ==
-				OMAP_DSS_UPDATE_MANUAL)
-			do_update = 1;
+		if ((display->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) &&
+				d->update_mode == OMAPFB_AUTO_UPDATE &&
+				!d->auto_update_work_enabled)
+			omapfb_start_auto_update(fbdev, display);
 
 		break;
 
@@ -1295,6 +1280,9 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 		if (display->state != OMAP_DSS_DISPLAY_ACTIVE)
 			goto exit;
 
+		if (d->auto_update_work_enabled)
+			omapfb_stop_auto_update(fbdev, display);
+
 		if (display->driver->suspend)
 			r = display->driver->suspend(display);
 
@@ -1307,13 +1295,6 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 exit:
 	omapfb_unlock(fbdev);
 
-	if (r == 0 && do_update && display->driver->update) {
-		u16 w, h;
-		display->driver->get_resolution(display, &w, &h);
-
-		r = display->driver->update(display, 0, 0, w, h);
-	}
-
 	return r;
 }
 
@@ -1758,6 +1739,78 @@ err:
 	return r;
 }
 
+static void omapfb_auto_update_work(struct work_struct *work)
+{
+	struct omap_dss_device *dssdev;
+	struct omap_dss_driver *dssdrv;
+	struct omapfb_display_data *d;
+	u16 w, h;
+	unsigned int freq;
+	struct omapfb2_device *fbdev;
+
+	d = container_of(work, struct omapfb_display_data,
+			auto_update_work.work);
+
+	dssdev = d->dssdev;
+	dssdrv = dssdev->driver;
+	fbdev = d->fbdev;
+
+	if (!dssdrv || !dssdrv->update)
+		return;
+
+	if (dssdrv->sync)
+		dssdrv->sync(dssdev);
+
+	dssdrv->get_resolution(dssdev, &w, &h);
+	dssdrv->update(dssdev, 0, 0, w, h);
+
+	freq = auto_update_freq;
+	if (freq == 0)
+		freq = 20;
+	queue_delayed_work(fbdev->auto_update_wq,
+			&d->auto_update_work, HZ / freq);
+}
+
+void omapfb_start_auto_update(struct omapfb2_device *fbdev,
+		struct omap_dss_device *display)
+{
+	struct omapfb_display_data *d;
+
+	if (fbdev->auto_update_wq == NULL) {
+		struct workqueue_struct *wq;
+
+		wq = create_singlethread_workqueue("omapfb_auto_update");
+
+		if (wq == NULL) {
+			dev_err(fbdev->dev, "Failed to create workqueue for "
+					"auto-update\n");
+			return;
+		}
+
+		fbdev->auto_update_wq = wq;
+	}
+
+	d = get_display_data(fbdev, display);
+
+	INIT_DELAYED_WORK(&d->auto_update_work, omapfb_auto_update_work);
+
+	d->auto_update_work_enabled = true;
+
+	omapfb_auto_update_work(&d->auto_update_work.work);
+}
+
+void omapfb_stop_auto_update(struct omapfb2_device *fbdev,
+		struct omap_dss_device *display)
+{
+	struct omapfb_display_data *d;
+
+	d = get_display_data(fbdev, display);
+
+	cancel_delayed_work_sync(&d->auto_update_work);
+
+	d->auto_update_work_enabled = false;
+}
+
 /* initialize fb_info, var, fix to something sane based on the display */
 static int omapfb_fb_init(struct omapfb2_device *fbdev, struct fb_info *fbi)
 {
@@ -1892,10 +1945,21 @@ static void omapfb_free_resources(struct omapfb2_device *fbdev)
 	}
 
 	for (i = 0; i < fbdev->num_displays; i++) {
-		if (fbdev->displays[i]->state != OMAP_DSS_DISPLAY_DISABLED)
-			fbdev->displays[i]->driver->disable(fbdev->displays[i]);
+		struct omap_dss_device *dssdev = fbdev->displays[i].dssdev;
+
+		if (fbdev->displays[i].auto_update_work_enabled)
+			omapfb_stop_auto_update(fbdev, dssdev);
 
-		omap_dss_put_device(fbdev->displays[i]);
+		if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED)
+			dssdev->driver->disable(dssdev);
+
+		omap_dss_put_device(dssdev);
+	}
+
+	if (fbdev->auto_update_wq != NULL) {
+		flush_workqueue(fbdev->auto_update_wq);
+		destroy_workqueue(fbdev->auto_update_wq);
+		fbdev->auto_update_wq = NULL;
 	}
 
 	dev_set_drvdata(fbdev->dev, NULL);
@@ -2030,9 +2094,9 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
 static int omapfb_mode_to_timings(const char *mode_str,
 		struct omap_video_timings *timings, u8 *bpp)
 {
-	struct fb_info fbi;
-	struct fb_var_screeninfo var;
-	struct fb_ops fbops;
+	struct fb_info *fbi;
+	struct fb_var_screeninfo *var;
+	struct fb_ops *fbops;
 	int r;
 
 #ifdef CONFIG_OMAP2_DSS_VENC
@@ -2050,39 +2114,66 @@ static int omapfb_mode_to_timings(const char *mode_str,
 	/* this is quite a hack, but I wanted to use the modedb and for
 	 * that we need fb_info and var, so we create dummy ones */
 
-	memset(&fbi, 0, sizeof(fbi));
-	memset(&var, 0, sizeof(var));
-	memset(&fbops, 0, sizeof(fbops));
-	fbi.fbops = &fbops;
-
-	r = fb_find_mode(&var, &fbi, mode_str, NULL, 0, NULL, 24);
-
-	if (r != 0) {
-		timings->pixel_clock = PICOS2KHZ(var.pixclock);
-		timings->hbp = var.left_margin;
-		timings->hfp = var.right_margin;
-		timings->vbp = var.upper_margin;
-		timings->vfp = var.lower_margin;
-		timings->hsw = var.hsync_len;
-		timings->vsw = var.vsync_len;
-		timings->x_res = var.xres;
-		timings->y_res = var.yres;
-
-		switch (var.bits_per_pixel) {
-		case 16:
-			*bpp = 16;
-			break;
-		case 24:
-		case 32:
-		default:
-			*bpp = 24;
-			break;
-		}
+	*bpp = 0;
+	fbi = NULL;
+	var = NULL;
+	fbops = NULL;
 
-		return 0;
-	} else {
-		return -EINVAL;
+	fbi = kzalloc(sizeof(*fbi), GFP_KERNEL);
+	if (fbi == NULL) {
+		r = -ENOMEM;
+		goto err;
+	}
+
+	var = kzalloc(sizeof(*var), GFP_KERNEL);
+	if (var == NULL) {
+		r = -ENOMEM;
+		goto err;
 	}
+
+	fbops = kzalloc(sizeof(*fbops), GFP_KERNEL);
+	if (fbops == NULL) {
+		r = -ENOMEM;
+		goto err;
+	}
+
+	fbi->fbops = fbops;
+
+	r = fb_find_mode(var, fbi, mode_str, NULL, 0, NULL, 24);
+	if (r == 0) {
+		r = -EINVAL;
+		goto err;
+	}
+
+	timings->pixel_clock = PICOS2KHZ(var->pixclock);
+	timings->hbp = var->left_margin;
+	timings->hfp = var->right_margin;
+	timings->vbp = var->upper_margin;
+	timings->vfp = var->lower_margin;
+	timings->hsw = var->hsync_len;
+	timings->vsw = var->vsync_len;
+	timings->x_res = var->xres;
+	timings->y_res = var->yres;
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		*bpp = 16;
+		break;
+	case 24:
+	case 32:
+	default:
+		*bpp = 24;
+		break;
+	}
+
+	r = 0;
+
+err:
+	kfree(fbi);
+	kfree(var);
+	kfree(fbops);
+
+	return r;
 }
 
 static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
@@ -2090,24 +2181,33 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 {
 	int r;
 	u8 bpp;
-	struct omap_video_timings timings;
+	struct omap_video_timings timings, temp_timings;
+	struct omapfb_display_data *d;
 
 	r = omapfb_mode_to_timings(mode_str, &timings, &bpp);
 	if (r)
 		return r;
 
-	fbdev->bpp_overrides[fbdev->num_bpp_overrides].dssdev = display;
-	fbdev->bpp_overrides[fbdev->num_bpp_overrides].bpp = bpp;
-	++fbdev->num_bpp_overrides;
+	d = get_display_data(fbdev, display);
+	d->bpp_override = bpp;
 
-	if (!display->driver->check_timings || !display->driver->set_timings)
-		return -EINVAL;
+	if (display->driver->check_timings) {
+		r = display->driver->check_timings(display, &timings);
+		if (r)
+			return r;
+	} else {
+		/* If check_timings is not present compare xres and yres */
+		if (display->driver->get_timings) {
+			display->driver->get_timings(display, &temp_timings);
 
-	r = display->driver->check_timings(display, &timings);
-	if (r)
-		return r;
+			if (temp_timings.x_res != timings.x_res ||
+				temp_timings.y_res != timings.y_res)
+				return -EINVAL;
+		}
+	}
 
-	display->driver->set_timings(display, &timings);
+	if (display->driver->set_timings)
+			display->driver->set_timings(display, &timings);
 
 	return 0;
 }
@@ -2115,14 +2215,14 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 static int omapfb_get_recommended_bpp(struct omapfb2_device *fbdev,
 		struct omap_dss_device *dssdev)
 {
-	int i;
+	struct omapfb_display_data *d;
 
 	BUG_ON(dssdev->driver->get_recommended_bpp == NULL);
 
-	for (i = 0; i < fbdev->num_bpp_overrides; ++i) {
-		if (dssdev == fbdev->bpp_overrides[i].dssdev)
-			return fbdev->bpp_overrides[i].bpp;
-	}
+	d = get_display_data(fbdev, dssdev);
+
+	if (d->bpp_override != 0)
+		return d->bpp_override;
 
 	return dssdev->driver->get_recommended_bpp(dssdev);
 }
@@ -2154,9 +2254,9 @@ static int omapfb_parse_def_modes(struct omapfb2_device *fbdev)
 
 		display = NULL;
 		for (i = 0; i < fbdev->num_displays; ++i) {
-			if (strcmp(fbdev->displays[i]->name,
+			if (strcmp(fbdev->displays[i].dssdev->name,
 						display_str) == 0) {
-				display = fbdev->displays[i];
+				display = fbdev->displays[i].dssdev;
 				break;
 			}
 		}
@@ -2176,6 +2276,56 @@ static int omapfb_parse_def_modes(struct omapfb2_device *fbdev)
 	return r;
 }
 
+static int omapfb_init_display(struct omapfb2_device *fbdev,
+		struct omap_dss_device *dssdev)
+{
+	struct omap_dss_driver *dssdrv = dssdev->driver;
+	struct omapfb_display_data *d;
+	int r;
+
+	r = dssdrv->enable(dssdev);
+	if (r) {
+		dev_warn(fbdev->dev, "Failed to enable display '%s'\n",
+				dssdev->name);
+		return r;
+	}
+
+	d = get_display_data(fbdev, dssdev);
+
+	d->fbdev = fbdev;
+
+	if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
+		u16 w, h;
+
+		if (auto_update) {
+			omapfb_start_auto_update(fbdev, dssdev);
+			d->update_mode = OMAPFB_AUTO_UPDATE;
+		} else {
+			d->update_mode = OMAPFB_MANUAL_UPDATE;
+		}
+
+		if (dssdrv->enable_te) {
+			r = dssdrv->enable_te(dssdev, 1);
+			if (r) {
+				dev_err(fbdev->dev, "Failed to set TE\n");
+				return r;
+			}
+		}
+
+		dssdrv->get_resolution(dssdev, &w, &h);
+		r = dssdrv->update(dssdev, 0, 0, w, h);
+		if (r) {
+			dev_err(fbdev->dev,
+					"Failed to update display\n");
+			return r;
+		}
+	} else {
+		d->update_mode = OMAPFB_AUTO_UPDATE;
+	}
+
+	return 0;
+}
+
 static int omapfb_probe(struct platform_device *pdev)
 {
 	struct omapfb2_device *fbdev = NULL;
@@ -2218,6 +2368,8 @@ static int omapfb_probe(struct platform_device *pdev)
 	fbdev->num_displays = 0;
 	dssdev = NULL;
 	for_each_dss_dev(dssdev) {
+		struct omapfb_display_data *d;
+
 		omap_dss_get_device(dssdev);
 
 		if (!dssdev->driver) {
@@ -2225,7 +2377,12 @@ static int omapfb_probe(struct platform_device *pdev)
 			r = -ENODEV;
 		}
 
-		fbdev->displays[fbdev->num_displays++] = dssdev;
+		d = &fbdev->displays[fbdev->num_displays++];
+		d->dssdev = dssdev;
+		if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE)
+			d->update_mode = OMAPFB_MANUAL_UPDATE;
+		else
+			d->update_mode = OMAPFB_AUTO_UPDATE;
 	}
 
 	if (r)
@@ -2275,30 +2432,13 @@ static int omapfb_probe(struct platform_device *pdev)
 	}
 
 	if (def_display) {
-		struct omap_dss_driver *dssdrv = def_display->driver;
-
-		r = def_display->driver->enable(def_display);
+		r = omapfb_init_display(fbdev, def_display);
 		if (r) {
-			dev_warn(fbdev->dev, "Failed to enable display '%s'\n",
-					def_display->name);
+			dev_err(fbdev->dev,
+					"failed to initialize default "
+					"display\n");
 			goto cleanup;
 		}
-
-		if (def_display->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
-			u16 w, h;
-			if (dssdrv->enable_te)
-				dssdrv->enable_te(def_display, 1);
-			if (dssdrv->set_update_mode)
-				dssdrv->set_update_mode(def_display,
-						OMAP_DSS_UPDATE_MANUAL);
-
-			dssdrv->get_resolution(def_display, &w, &h);
-			def_display->driver->update(def_display, 0, 0, w, h);
-		} else {
-			if (dssdrv->set_update_mode)
-				dssdrv->set_update_mode(def_display,
-						OMAP_DSS_UPDATE_AUTO);
-		}
 	}
 
 	DBG("create sysfs for fbs\n");
diff --git a/drivers/video/omap2/omapfb/omapfb-sysfs.c b/drivers/video/omap2/omapfb/omapfb-sysfs.c
index 6f9c72cd6bb0..153bf1aceebc 100644
--- a/drivers/video/omap2/omapfb/omapfb-sysfs.c
+++ b/drivers/video/omap2/omapfb/omapfb-sysfs.c
@@ -29,7 +29,7 @@
 #include <linux/mm.h>
 #include <linux/omapfb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/vrfb.h>
 
 #include "omapfb.h"
@@ -50,10 +50,12 @@ static ssize_t store_rotate_type(struct device *dev,
 	struct fb_info *fbi = dev_get_drvdata(dev);
 	struct omapfb_info *ofbi = FB2OFB(fbi);
 	struct omapfb2_mem_region *rg;
-	enum omap_dss_rotation_type rot_type;
+	int rot_type;
 	int r;
 
-	rot_type = simple_strtoul(buf, NULL, 0);
+	r = kstrtoint(buf, 0, &rot_type);
+	if (r)
+		return r;
 
 	if (rot_type != OMAP_DSS_ROT_DMA && rot_type != OMAP_DSS_ROT_VRFB)
 		return -EINVAL;
@@ -102,14 +104,15 @@ static ssize_t store_mirror(struct device *dev,
 {
 	struct fb_info *fbi = dev_get_drvdata(dev);
 	struct omapfb_info *ofbi = FB2OFB(fbi);
-	unsigned long mirror;
+	int mirror;
 	int r;
 	struct fb_var_screeninfo new_var;
 
-	mirror = simple_strtoul(buf, NULL, 0);
+	r = kstrtoint(buf, 0, &mirror);
+	if (r)
+		return r;
 
-	if (mirror != 0 && mirror != 1)
-		return -EINVAL;
+	mirror = !!mirror;
 
 	if (!lock_fb_info(fbi))
 		return -ENODEV;
@@ -445,7 +448,11 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
 	int r;
 	int i;
 
-	size = PAGE_ALIGN(simple_strtoul(buf, NULL, 0));
+	r = kstrtoul(buf, 0, &size);
+	if (r)
+		return r;
+
+	size = PAGE_ALIGN(size);
 
 	if (!lock_fb_info(fbi))
 		return -ENODEV;
@@ -511,6 +518,39 @@ static ssize_t show_virt(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%p\n", ofbi->region->vaddr);
 }
 
+static ssize_t show_upd_mode(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct fb_info *fbi = dev_get_drvdata(dev);
+	enum omapfb_update_mode mode;
+	int r;
+
+	r = omapfb_get_update_mode(fbi, &mode);
+
+	if (r)
+		return r;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", (unsigned)mode);
+}
+
+static ssize_t store_upd_mode(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct fb_info *fbi = dev_get_drvdata(dev);
+	unsigned mode;
+	int r;
+
+	r = kstrtouint(buf, 0, &mode);
+	if (r)
+		return r;
+
+	r = omapfb_set_update_mode(fbi, mode);
+	if (r)
+		return r;
+
+	return count;
+}
+
 static struct device_attribute omapfb_attrs[] = {
 	__ATTR(rotate_type, S_IRUGO | S_IWUSR, show_rotate_type,
 			store_rotate_type),
@@ -521,6 +561,7 @@ static struct device_attribute omapfb_attrs[] = {
 			store_overlays_rotate),
 	__ATTR(phys_addr, S_IRUGO, show_phys, NULL),
 	__ATTR(virt_addr, S_IRUGO, show_virt, NULL),
+	__ATTR(update_mode, S_IRUGO | S_IWUSR, show_upd_mode, store_upd_mode),
 };
 
 int omapfb_create_sysfs(struct omapfb2_device *fbdev)
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index 1305fc9880ba..fdf0edeccf4e 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -29,13 +29,15 @@
 
 #include <linux/rwsem.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #ifdef DEBUG
 extern unsigned int omapfb_debug;
 #define DBG(format, ...) \
-	if (omapfb_debug) \
-		printk(KERN_DEBUG "OMAPFB: " format, ## __VA_ARGS__)
+	do { \
+		if (omapfb_debug) \
+			printk(KERN_DEBUG "OMAPFB: " format, ## __VA_ARGS__); \
+	} while (0)
 #else
 #define DBG(format, ...)
 #endif
@@ -71,6 +73,15 @@ struct omapfb_info {
 	bool mirror;
 };
 
+struct omapfb_display_data {
+	struct omapfb2_device *fbdev;
+	struct omap_dss_device *dssdev;
+	u8 bpp_override;
+	enum omapfb_update_mode update_mode;
+	bool auto_update_work_enabled;
+	struct delayed_work auto_update_work;
+};
+
 struct omapfb2_device {
 	struct device *dev;
 	struct mutex  mtx;
@@ -84,17 +95,13 @@ struct omapfb2_device {
 	struct omapfb2_mem_region regions[10];
 
 	unsigned num_displays;
-	struct omap_dss_device *displays[10];
+	struct omapfb_display_data displays[10];
 	unsigned num_overlays;
 	struct omap_overlay *overlays[10];
 	unsigned num_managers;
 	struct omap_overlay_manager *managers[10];
 
-	unsigned num_bpp_overrides;
-	struct {
-		struct omap_dss_device *dssdev;
-		u8 bpp;
-	} bpp_overrides[10];
+	struct workqueue_struct *auto_update_wq;
 };
 
 struct omapfb_colormode {
@@ -126,6 +133,13 @@ int dss_mode_to_fb_mode(enum omap_color_mode dssmode,
 int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
 		u16 posx, u16 posy, u16 outw, u16 outh);
 
+void omapfb_start_auto_update(struct omapfb2_device *fbdev,
+		struct omap_dss_device *display);
+void omapfb_stop_auto_update(struct omapfb2_device *fbdev,
+		struct omap_dss_device *display);
+int omapfb_get_update_mode(struct fb_info *fbi, enum omapfb_update_mode *mode);
+int omapfb_set_update_mode(struct fb_info *fbi, enum omapfb_update_mode mode);
+
 /* find the display connected to this fb, if any */
 static inline struct omap_dss_device *fb2display(struct fb_info *fbi)
 {
@@ -141,6 +155,19 @@ static inline struct omap_dss_device *fb2display(struct fb_info *fbi)
 	return NULL;
 }
 
+static inline struct omapfb_display_data *get_display_data(
+		struct omapfb2_device *fbdev, struct omap_dss_device *dssdev)
+{
+	int i;
+
+	for (i = 0; i < fbdev->num_displays; ++i)
+		if (fbdev->displays[i].dssdev == dssdev)
+			return &fbdev->displays[i];
+
+	/* This should never happen */
+	BUG();
+}
+
 static inline void omapfb_lock(struct omapfb2_device *fbdev)
 {
 	mutex_lock(&fbdev->mtx);
diff --git a/drivers/video/p9100.c b/drivers/video/p9100.c
index b6c3fc2db632..d57cc58c5168 100644
--- a/drivers/video/p9100.c
+++ b/drivers/video/p9100.c
@@ -249,7 +249,7 @@ static void p9100_init_fix(struct fb_info *info, int linebytes, struct device_no
 	info->fix.accel = FB_ACCEL_SUN_CGTHREE;
 }
 
-static int __devinit p9100_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit p9100_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -352,7 +352,7 @@ static const struct of_device_id p9100_match[] = {
 };
 MODULE_DEVICE_TABLE(of, p9100_match);
 
-static struct of_platform_driver p9100_driver = {
+static struct platform_driver p9100_driver = {
 	.driver = {
 		.name = "p9100",
 		.owner = THIS_MODULE,
@@ -367,12 +367,12 @@ static int __init p9100_init(void)
 	if (fb_get_options("p9100fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&p9100_driver);
+	return platform_driver_register(&p9100_driver);
 }
 
 static void __exit p9100_exit(void)
 {
-	of_unregister_platform_driver(&p9100_driver);
+	platform_driver_unregister(&p9100_driver);
 }
 
 module_init(p9100_init);
diff --git a/drivers/video/platinumfb.c b/drivers/video/platinumfb.c
index a50e1977b316..f27ae16ead2e 100644
--- a/drivers/video/platinumfb.c
+++ b/drivers/video/platinumfb.c
@@ -533,8 +533,7 @@ static int __init platinumfb_setup(char *options)
 #define invalidate_cache(addr)
 #endif
 
-static int __devinit platinumfb_probe(struct platform_device* odev,
-				      const struct of_device_id *match)
+static int __devinit platinumfb_probe(struct platform_device* odev)
 {
 	struct device_node	*dp = odev->dev.of_node;
 	struct fb_info		*info;
@@ -568,7 +567,7 @@ static int __devinit platinumfb_probe(struct platform_device* odev,
 	 * northbridge and that can fail. Only request framebuffer
 	 */
 	if (!request_mem_region(pinfo->rsrc_fb.start,
-				pinfo->rsrc_fb.end - pinfo->rsrc_fb.start + 1,
+				resource_size(&pinfo->rsrc_fb),
 				"platinumfb framebuffer")) {
 		printk(KERN_ERR "platinumfb: Can't request framebuffer !\n");
 		framebuffer_release(info);
@@ -659,8 +658,7 @@ static int __devexit platinumfb_remove(struct platform_device* odev)
 	iounmap(pinfo->cmap_regs);
 
 	release_mem_region(pinfo->rsrc_fb.start,
-			   pinfo->rsrc_fb.end -
-			   pinfo->rsrc_fb.start + 1);
+			   resource_size(&pinfo->rsrc_fb));
 
 	release_mem_region(pinfo->cmap_regs_phys, 0x1000);
 
@@ -677,7 +675,7 @@ static struct of_device_id platinumfb_match[] =
 	{},
 };
 
-static struct of_platform_driver platinum_driver = 
+static struct platform_driver platinum_driver = 
 {
 	.driver = {
 		.name = "platinumfb",
@@ -697,14 +695,14 @@ static int __init platinumfb_init(void)
 		return -ENODEV;
 	platinumfb_setup(option);
 #endif
-	of_register_platform_driver(&platinum_driver);
+	platform_driver_register(&platinum_driver);
 
 	return 0;
 }
 
 static void __exit platinumfb_exit(void)
 {
-	of_unregister_platform_driver(&platinum_driver);
+	platform_driver_unregister(&platinum_driver);
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/pxa168fb.c b/drivers/video/pxa168fb.c
index 35f61dd0cb3a..18ead6f0184d 100644
--- a/drivers/video/pxa168fb.c
+++ b/drivers/video/pxa168fb.c
@@ -623,19 +623,21 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res == NULL) {
 		dev_err(&pdev->dev, "no IO memory defined\n");
-		return -ENOENT;
+		ret = -ENOENT;
+		goto failed_put_clk;
 	}
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(&pdev->dev, "no IRQ defined\n");
-		return -ENOENT;
+		ret = -ENOENT;
+		goto failed_put_clk;
 	}
 
 	info = framebuffer_alloc(sizeof(struct pxa168fb_info), &pdev->dev);
 	if (info == NULL) {
-		clk_put(clk);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto failed_put_clk;
 	}
 
 	/* Initialize private data */
@@ -660,7 +662,7 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
 	info->fix.ypanstep = 0;
 	info->fix.ywrapstep = 0;
 	info->fix.mmio_start = res->start;
-	info->fix.mmio_len = res->end - res->start + 1;
+	info->fix.mmio_len = resource_size(res);
 	info->fix.accel = FB_ACCEL_NONE;
 	info->fbops = &pxa168fb_ops;
 	info->pseudo_palette = fbi->pseudo_palette;
@@ -671,7 +673,7 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
 	fbi->reg_base = ioremap_nocache(res->start, resource_size(res));
 	if (fbi->reg_base == NULL) {
 		ret = -ENOMEM;
-		goto failed;
+		goto failed_free_info;
 	}
 
 	/*
@@ -683,7 +685,7 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
 						&fbi->fb_start_dma, GFP_KERNEL);
 	if (info->screen_base == NULL) {
 		ret = -ENOMEM;
-		goto failed;
+		goto failed_free_info;
 	}
 
 	info->fix.smem_start = (unsigned long)fbi->fb_start_dma;
@@ -772,8 +774,9 @@ failed_free_clk:
 failed_free_fbmem:
 	dma_free_coherent(fbi->dev, info->fix.smem_len,
 			info->screen_base, fbi->fb_start_dma);
-failed:
+failed_free_info:
 	kfree(info);
+failed_put_clk:
 	clk_put(clk);
 
 	dev_err(&pdev->dev, "frame buffer device init failed with %d\n", ret);
diff --git a/drivers/video/pxa3xx-gcu.c b/drivers/video/pxa3xx-gcu.c
index cf4beb9dc9bb..0283c7021090 100644
--- a/drivers/video/pxa3xx-gcu.c
+++ b/drivers/video/pxa3xx-gcu.c
@@ -25,7 +25,7 @@
 
 /*
  * WARNING: This controller is attached to System Bus 2 of the PXA which
- * needs its arbiter to be enabled explictly (CKENB & 1<<9).
+ * needs its arbiter to be enabled explicitly (CKENB & 1<<9).
  * There is currently no way to do this from Linux, so you need to teach
  * your bootloader for now.
  */
diff --git a/drivers/video/pxafb.c b/drivers/video/pxafb.c
index 825b665245bb..0f4e8c942f9e 100644
--- a/drivers/video/pxafb.c
+++ b/drivers/video/pxafb.c
@@ -627,7 +627,12 @@ static void overlay1fb_enable(struct pxafb_layer *ofb)
 
 static void overlay1fb_disable(struct pxafb_layer *ofb)
 {
-	uint32_t lccr5 = lcd_readl(ofb->fbi, LCCR5);
+	uint32_t lccr5;
+
+	if (!(lcd_readl(ofb->fbi, OVL1C1) & OVLxC1_OEN))
+		return;
+
+	lccr5 = lcd_readl(ofb->fbi, LCCR5);
 
 	lcd_writel(ofb->fbi, OVL1C1, ofb->control[0] & ~OVLxC1_OEN);
 
@@ -685,7 +690,12 @@ static void overlay2fb_enable(struct pxafb_layer *ofb)
 
 static void overlay2fb_disable(struct pxafb_layer *ofb)
 {
-	uint32_t lccr5 = lcd_readl(ofb->fbi, LCCR5);
+	uint32_t lccr5;
+
+	if (!(lcd_readl(ofb->fbi, OVL2C1) & OVLxC1_OEN))
+		return;
+
+	lccr5 = lcd_readl(ofb->fbi, LCCR5);
 
 	lcd_writel(ofb->fbi, OVL2C1, ofb->control[0] & ~OVLxC1_OEN);
 
@@ -720,12 +730,10 @@ static int overlayfb_open(struct fb_info *info, int user)
 	if (user == 0)
 		return -ENODEV;
 
-	/* allow only one user at a time */
-	if (atomic_inc_and_test(&ofb->usage))
-		return -EBUSY;
+	if (ofb->usage++ == 0)
+		/* unblank the base framebuffer */
+		fb_blank(&ofb->fbi->fb, FB_BLANK_UNBLANK);
 
-	/* unblank the base framebuffer */
-	fb_blank(&ofb->fbi->fb, FB_BLANK_UNBLANK);
 	return 0;
 }
 
@@ -733,12 +741,15 @@ static int overlayfb_release(struct fb_info *info, int user)
 {
 	struct pxafb_layer *ofb = (struct pxafb_layer*) info;
 
-	atomic_dec(&ofb->usage);
-	ofb->ops->disable(ofb);
+	if (ofb->usage == 1) {
+		ofb->ops->disable(ofb);
+		ofb->fb.var.height	= -1;
+		ofb->fb.var.width	= -1;
+		ofb->fb.var.xres = ofb->fb.var.xres_virtual = 0;
+		ofb->fb.var.yres = ofb->fb.var.yres_virtual = 0;
 
-	free_pages_exact(ofb->video_mem, ofb->video_mem_size);
-	ofb->video_mem = NULL;
-	ofb->video_mem_size = 0;
+		ofb->usage--;
+	}
 	return 0;
 }
 
@@ -750,7 +761,7 @@ static int overlayfb_check_var(struct fb_var_screeninfo *var,
 	int xpos, ypos, pfor, bpp;
 
 	xpos = NONSTD_TO_XPOS(var->nonstd);
-	ypos = NONSTD_TO_XPOS(var->nonstd);
+	ypos = NONSTD_TO_YPOS(var->nonstd);
 	pfor = NONSTD_TO_PFOR(var->nonstd);
 
 	bpp = pxafb_var_to_bpp(var);
@@ -794,7 +805,7 @@ static int overlayfb_check_var(struct fb_var_screeninfo *var,
 	return 0;
 }
 
-static int overlayfb_map_video_memory(struct pxafb_layer *ofb)
+static int overlayfb_check_video_memory(struct pxafb_layer *ofb)
 {
 	struct fb_var_screeninfo *var = &ofb->fb.var;
 	int pfor = NONSTD_TO_PFOR(var->nonstd);
@@ -812,27 +823,11 @@ static int overlayfb_map_video_memory(struct pxafb_layer *ofb)
 
 	size = PAGE_ALIGN(ofb->fb.fix.line_length * var->yres_virtual);
 
-	/* don't re-allocate if the original video memory is enough */
 	if (ofb->video_mem) {
 		if (ofb->video_mem_size >= size)
 			return 0;
-
-		free_pages_exact(ofb->video_mem, ofb->video_mem_size);
 	}
-
-	ofb->video_mem = alloc_pages_exact(size, GFP_KERNEL | __GFP_ZERO);
-	if (ofb->video_mem == NULL)
-		return -ENOMEM;
-
-	ofb->video_mem_phys = virt_to_phys(ofb->video_mem);
-	ofb->video_mem_size = size;
-
-	mutex_lock(&ofb->fb.mm_lock);
-	ofb->fb.fix.smem_start	= ofb->video_mem_phys;
-	ofb->fb.fix.smem_len	= ofb->fb.fix.line_length * var->yres_virtual;
-	mutex_unlock(&ofb->fb.mm_lock);
-	ofb->fb.screen_base	= ofb->video_mem;
-	return 0;
+	return -EINVAL;
 }
 
 static int overlayfb_set_par(struct fb_info *info)
@@ -841,13 +836,13 @@ static int overlayfb_set_par(struct fb_info *info)
 	struct fb_var_screeninfo *var = &info->var;
 	int xpos, ypos, pfor, bpp, ret;
 
-	ret = overlayfb_map_video_memory(ofb);
+	ret = overlayfb_check_video_memory(ofb);
 	if (ret)
 		return ret;
 
 	bpp  = pxafb_var_to_bpp(var);
 	xpos = NONSTD_TO_XPOS(var->nonstd);
-	ypos = NONSTD_TO_XPOS(var->nonstd);
+	ypos = NONSTD_TO_YPOS(var->nonstd);
 	pfor = NONSTD_TO_PFOR(var->nonstd);
 
 	ofb->control[0] = OVLxC1_PPL(var->xres) | OVLxC1_LPO(var->yres) |
@@ -891,7 +886,7 @@ static void __devinit init_pxafb_overlay(struct pxafb_info *fbi,
 
 	ofb->id = id;
 	ofb->ops = &ofb_ops[id];
-	atomic_set(&ofb->usage, 0);
+	ofb->usage = 0;
 	ofb->fbi = fbi;
 	init_completion(&ofb->branch_done);
 }
@@ -904,29 +899,60 @@ static inline int pxafb_overlay_supported(void)
 	return 0;
 }
 
-static int __devinit pxafb_overlay_init(struct pxafb_info *fbi)
+static int __devinit pxafb_overlay_map_video_memory(struct pxafb_info *pxafb,
+	struct pxafb_layer *ofb)
+{
+	/* We assume that user will use at most video_mem_size for overlay fb,
+	 * anyway, it's useless to use 16bpp main plane and 24bpp overlay
+	 */
+	ofb->video_mem = alloc_pages_exact(PAGE_ALIGN(pxafb->video_mem_size),
+		GFP_KERNEL | __GFP_ZERO);
+	if (ofb->video_mem == NULL)
+		return -ENOMEM;
+
+	ofb->video_mem_phys = virt_to_phys(ofb->video_mem);
+	ofb->video_mem_size = PAGE_ALIGN(pxafb->video_mem_size);
+
+	mutex_lock(&ofb->fb.mm_lock);
+	ofb->fb.fix.smem_start	= ofb->video_mem_phys;
+	ofb->fb.fix.smem_len	= pxafb->video_mem_size;
+	mutex_unlock(&ofb->fb.mm_lock);
+
+	ofb->fb.screen_base	= ofb->video_mem;
+
+	return 0;
+}
+
+static void __devinit pxafb_overlay_init(struct pxafb_info *fbi)
 {
 	int i, ret;
 
 	if (!pxafb_overlay_supported())
-		return 0;
+		return;
 
 	for (i = 0; i < 2; i++) {
-		init_pxafb_overlay(fbi, &fbi->overlay[i], i);
-		ret = register_framebuffer(&fbi->overlay[i].fb);
+		struct pxafb_layer *ofb = &fbi->overlay[i];
+		init_pxafb_overlay(fbi, ofb, i);
+		ret = register_framebuffer(&ofb->fb);
 		if (ret) {
 			dev_err(fbi->dev, "failed to register overlay %d\n", i);
-			return ret;
+			continue;
 		}
+		ret = pxafb_overlay_map_video_memory(fbi, ofb);
+		if (ret) {
+			dev_err(fbi->dev,
+				"failed to map video memory for overlay %d\n",
+				i);
+			unregister_framebuffer(&ofb->fb);
+			continue;
+		}
+		ofb->registered = 1;
 	}
 
 	/* mask all IU/BS/EOF/SOF interrupts */
 	lcd_writel(fbi, LCCR5, ~0);
 
-	/* place overlay(s) on top of base */
-	fbi->lccr0 |= LCCR0_OUC;
 	pr_info("PXA Overlay driver loaded successfully!\n");
-	return 0;
 }
 
 static void __devexit pxafb_overlay_exit(struct pxafb_info *fbi)
@@ -936,8 +962,15 @@ static void __devexit pxafb_overlay_exit(struct pxafb_info *fbi)
 	if (!pxafb_overlay_supported())
 		return;
 
-	for (i = 0; i < 2; i++)
-		unregister_framebuffer(&fbi->overlay[i].fb);
+	for (i = 0; i < 2; i++) {
+		struct pxafb_layer *ofb = &fbi->overlay[i];
+		if (ofb->registered) {
+			if (ofb->video_mem)
+				free_pages_exact(ofb->video_mem,
+					ofb->video_mem_size);
+			unregister_framebuffer(&ofb->fb);
+		}
+	}
 }
 #else
 static inline void pxafb_overlay_init(struct pxafb_info *fbi) {}
@@ -1368,7 +1401,8 @@ static int pxafb_activate_var(struct fb_var_screeninfo *var,
 	    (lcd_readl(fbi, LCCR3) != fbi->reg_lccr3) ||
 	    (lcd_readl(fbi, LCCR4) != fbi->reg_lccr4) ||
 	    (lcd_readl(fbi, FDADR0) != fbi->fdadr[0]) ||
-	    (lcd_readl(fbi, FDADR1) != fbi->fdadr[1]))
+	    ((fbi->lccr0 & LCCR0_SDS) &&
+	    (lcd_readl(fbi, FDADR1) != fbi->fdadr[1])))
 		pxafb_schedule_work(fbi, C_REENABLE);
 
 	return 0;
@@ -1420,7 +1454,8 @@ static void pxafb_enable_controller(struct pxafb_info *fbi)
 	lcd_writel(fbi, LCCR0, fbi->reg_lccr0 & ~LCCR0_ENB);
 
 	lcd_writel(fbi, FDADR0, fbi->fdadr[0]);
-	lcd_writel(fbi, FDADR1, fbi->fdadr[1]);
+	if (fbi->lccr0 & LCCR0_SDS)
+		lcd_writel(fbi, FDADR1, fbi->fdadr[1]);
 	lcd_writel(fbi, LCCR0, fbi->reg_lccr0 | LCCR0_ENB);
 }
 
@@ -1613,7 +1648,10 @@ pxafb_freq_transition(struct notifier_block *nb, unsigned long val, void *data)
 
 	switch (val) {
 	case CPUFREQ_PRECHANGE:
-		set_ctrlr_state(fbi, C_DISABLE_CLKCHANGE);
+#ifdef CONFIG_FB_PXA_OVERLAY
+		if (!(fbi->overlay[0].usage || fbi->overlay[1].usage))
+#endif
+			set_ctrlr_state(fbi, C_DISABLE_CLKCHANGE);
 		break;
 
 	case CPUFREQ_POSTCHANGE:
@@ -1806,6 +1844,12 @@ static struct pxafb_info * __devinit pxafb_init_fbinfo(struct device *dev)
 
 	pxafb_decode_mach_info(fbi, inf);
 
+#ifdef CONFIG_FB_PXA_OVERLAY
+	/* place overlay(s) on top of base */
+	if (pxafb_overlay_supported())
+		fbi->lccr0 |= LCCR0_OUC;
+#endif
+
 	init_waitqueue_head(&fbi->ctrlr_wait);
 	INIT_WORK(&fbi->task, pxafb_task);
 	mutex_init(&fbi->ctrlr_lock);
diff --git a/drivers/video/pxafb.h b/drivers/video/pxafb.h
index 2353521c5c8c..26ba9fa3f737 100644
--- a/drivers/video/pxafb.h
+++ b/drivers/video/pxafb.h
@@ -92,7 +92,8 @@ struct pxafb_layer_ops {
 struct pxafb_layer {
 	struct fb_info		fb;
 	int			id;
-	atomic_t		usage;
+	int			registered;
+	uint32_t		usage;
 	uint32_t		control[2];
 
 	struct pxafb_layer_ops	*ops;
diff --git a/drivers/video/riva/fbdev.c b/drivers/video/riva/fbdev.c
index da388186d617..d8ab7be4fd6b 100644
--- a/drivers/video/riva/fbdev.c
+++ b/drivers/video/riva/fbdev.c
@@ -355,6 +355,7 @@ static void riva_bl_init(struct riva_par *par)
 	snprintf(name, sizeof(name), "rivabl%d", info->node);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
 	props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
 	bd = backlight_device_register(name, info->dev, par, &riva_bl_ops,
 				       &props);
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index 83ce9a04d872..4aecf213c9be 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -48,7 +48,7 @@
 #undef writel
 #define writel(v, r) do { \
 	printk(KERN_DEBUG "%s: %08x => %p\n", __func__, (unsigned int)v, r); \
-	__raw_writel(v, r); } while(0)
+	__raw_writel(v, r); } while (0)
 #endif /* FB_S3C_DEBUG_REGWRITE */
 
 /* irq_flags bits */
@@ -182,6 +182,7 @@ struct s3c_fb_vsync {
 
 /**
  * struct s3c_fb - overall hardware state of the hardware
+ * @slock: The spinlock protection for this data sturcture.
  * @dev: The device that we bound to, for printing, etc.
  * @regs_res: The resource we claimed for the IO registers.
  * @bus_clk: The clk (hclk) feeding our interface and possibly pixclk.
@@ -195,6 +196,7 @@ struct s3c_fb_vsync {
  * @vsync_info: VSYNC-related information (count, queues...)
  */
 struct s3c_fb {
+	spinlock_t		slock;
 	struct device		*dev;
 	struct resource		*regs_res;
 	struct clk		*bus_clk;
@@ -233,13 +235,12 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
 			    struct fb_info *info)
 {
 	struct s3c_fb_win *win = info->par;
-	struct s3c_fb_pd_win *windata = win->windata;
 	struct s3c_fb *sfb = win->parent;
 
 	dev_dbg(sfb->dev, "checking parameters\n");
 
-	var->xres_virtual = max((unsigned int)windata->virtual_x, var->xres);
-	var->yres_virtual = max((unsigned int)windata->virtual_y, var->yres);
+	var->xres_virtual = max(var->xres_virtual, var->xres);
+	var->yres_virtual = max(var->yres_virtual, var->yres);
 
 	if (!s3c_fb_validate_win_bpp(win, var->bits_per_pixel)) {
 		dev_dbg(sfb->dev, "win %d: unsupported bpp %d\n",
@@ -300,6 +301,7 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
 		var->blue.length	= 5;
 		break;
 
+	case 32:
 	case 28:
 	case 25:
 		var->transp.length	= var->bits_per_pixel - 24;
@@ -308,7 +310,6 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
 	case 24:
 		/* our 24bpp is unpacked, so 32bpp */
 		var->bits_per_pixel	= 32;
-	case 32:
 		var->red.offset		= 16;
 		var->red.length		= 8;
 		var->green.offset	= 8;
@@ -518,7 +519,7 @@ static int s3c_fb_set_par(struct fb_info *info)
 
 		data = VIDTCON2_LINEVAL(var->yres - 1) |
 		       VIDTCON2_HOZVAL(var->xres - 1);
-		writel(data, regs +sfb->variant.vidtcon + 8 );
+		writel(data, regs + sfb->variant.vidtcon + 8);
 	}
 
 	/* write the buffer address */
@@ -556,6 +557,13 @@ static int s3c_fb_set_par(struct fb_info *info)
 	vidosd_set_alpha(win, alpha);
 	vidosd_set_size(win, data);
 
+	/* Enable DMA channel for this window */
+	if (sfb->variant.has_shadowcon) {
+		data = readl(sfb->regs + SHADOWCON);
+		data |= SHADOWCON_CHx_ENABLE(win_no);
+		writel(data, sfb->regs + SHADOWCON);
+	}
+
 	data = WINCONx_ENWIN;
 
 	/* note, since we have to round up the bits-per-pixel, we end up
@@ -635,13 +643,6 @@ static int s3c_fb_set_par(struct fb_info *info)
 	writel(data, regs + sfb->variant.wincon + (win_no * 4));
 	writel(0x0, regs + sfb->variant.winmap + (win_no * 4));
 
-	/* Enable DMA channel for this window */
-	if (sfb->variant.has_shadowcon) {
-		data = readl(sfb->regs + SHADOWCON);
-		data |= SHADOWCON_CHx_ENABLE(win_no);
-		writel(data, sfb->regs + SHADOWCON);
-	}
-
 	shadow_protect_win(win, 0);
 
 	return 0;
@@ -947,6 +948,8 @@ static irqreturn_t s3c_fb_irq(int irq, void *dev_id)
 	void __iomem  *regs = sfb->regs;
 	u32 irq_sts_reg;
 
+	spin_lock(&sfb->slock);
+
 	irq_sts_reg = readl(regs + VIDINTCON1);
 
 	if (irq_sts_reg & VIDINTCON1_INT_FRAME) {
@@ -963,6 +966,7 @@ static irqreturn_t s3c_fb_irq(int irq, void *dev_id)
 	 */
 	s3c_fb_disable_irq(sfb);
 
+	spin_unlock(&sfb->slock);
 	return IRQ_HANDLED;
 }
 
@@ -1304,6 +1308,7 @@ static void s3c_fb_clear_win(struct s3c_fb *sfb, int win)
 
 static int __devinit s3c_fb_probe(struct platform_device *pdev)
 {
+	const struct platform_device_id *platid;
 	struct s3c_fb_driverdata *fbdrv;
 	struct device *dev = &pdev->dev;
 	struct s3c_fb_platdata *pd;
@@ -1312,7 +1317,8 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 	int win;
 	int ret = 0;
 
-	fbdrv = (struct s3c_fb_driverdata *)platform_get_device_id(pdev)->driver_data;
+	platid = platform_get_device_id(pdev);
+	fbdrv = (struct s3c_fb_driverdata *)platid->driver_data;
 
 	if (fbdrv->variant.nr_windows > S3C_FB_MAX_WIN) {
 		dev_err(dev, "too many windows, cannot attach\n");
@@ -1337,9 +1343,12 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 	sfb->pdata = pd;
 	sfb->variant = fbdrv->variant;
 
+	spin_lock_init(&sfb->slock);
+
 	sfb->bus_clk = clk_get(dev, "lcd");
 	if (IS_ERR(sfb->bus_clk)) {
 		dev_err(dev, "failed to get bus clock\n");
+		ret = PTR_ERR(sfb->bus_clk);
 		goto err_sfb;
 	}
 
@@ -1439,8 +1448,7 @@ err_ioremap:
 	iounmap(sfb->regs);
 
 err_req_region:
-	release_resource(sfb->regs_res);
-	kfree(sfb->regs_res);
+	release_mem_region(sfb->regs_res->start, resource_size(sfb->regs_res));
 
 err_clk:
 	clk_disable(sfb->bus_clk);
@@ -1476,14 +1484,12 @@ static int __devexit s3c_fb_remove(struct platform_device *pdev)
 	clk_disable(sfb->bus_clk);
 	clk_put(sfb->bus_clk);
 
-	release_resource(sfb->regs_res);
-	kfree(sfb->regs_res);
-
-	kfree(sfb);
+	release_mem_region(sfb->regs_res->start, resource_size(sfb->regs_res));
 
 	pm_runtime_put_sync(sfb->dev);
 	pm_runtime_disable(sfb->dev);
 
+	kfree(sfb);
 	return 0;
 }
 
@@ -1518,7 +1524,8 @@ static int s3c_fb_resume(struct device *dev)
 
 	clk_enable(sfb->bus_clk);
 
-	/* setup registers */
+	/* setup gpio and output polarity controls */
+	pd->setup_gpio();
 	writel(pd->vidcon1, sfb->regs + VIDCON1);
 
 	/* zero all windows before we do anything */
@@ -1546,7 +1553,7 @@ static int s3c_fb_resume(struct device *dev)
 	return 0;
 }
 
-int s3c_fb_runtime_suspend(struct device *dev)
+static int s3c_fb_runtime_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s3c_fb *sfb = platform_get_drvdata(pdev);
@@ -1566,7 +1573,7 @@ int s3c_fb_runtime_suspend(struct device *dev)
 	return 0;
 }
 
-int s3c_fb_runtime_resume(struct device *dev)
+static int s3c_fb_runtime_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s3c_fb *sfb = platform_get_drvdata(pdev);
@@ -1576,7 +1583,8 @@ int s3c_fb_runtime_resume(struct device *dev)
 
 	clk_enable(sfb->bus_clk);
 
-	/* setup registers */
+	/* setup gpio and output polarity controls */
+	pd->setup_gpio();
 	writel(pd->vidcon1, sfb->regs + VIDCON1);
 
 	/* zero all windows before we do anything */
@@ -1620,28 +1628,31 @@ static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
 		.has_osd_c	= 1,
 		.osd_size_off	= 0x8,
 		.palette_sz	= 256,
-		.valid_bpp	= VALID_BPP1248 | VALID_BPP(16) | VALID_BPP(24),
+		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(16) |
+				   VALID_BPP(18) | VALID_BPP(24)),
 	},
 	[1] = {
 		.has_osd_c	= 1,
 		.has_osd_d	= 1,
-		.osd_size_off	= 0x12,
+		.osd_size_off	= 0xc,
 		.has_osd_alpha	= 1,
 		.palette_sz	= 256,
 		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(16) |
 				   VALID_BPP(18) | VALID_BPP(19) |
-				   VALID_BPP(24) | VALID_BPP(25)),
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(28)),
 	},
 	[2] = {
 		.has_osd_c	= 1,
 		.has_osd_d	= 1,
-		.osd_size_off	= 0x12,
+		.osd_size_off	= 0xc,
 		.has_osd_alpha	= 1,
 		.palette_sz	= 16,
 		.palette_16bpp	= 1,
 		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(16) |
 				   VALID_BPP(18) | VALID_BPP(19) |
-				   VALID_BPP(24) | VALID_BPP(25)),
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(28)),
 	},
 	[3] = {
 		.has_osd_c	= 1,
@@ -1650,7 +1661,8 @@ static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
 		.palette_16bpp	= 1,
 		.valid_bpp	= (VALID_BPP124  | VALID_BPP(16) |
 				   VALID_BPP(18) | VALID_BPP(19) |
-				   VALID_BPP(24) | VALID_BPP(25)),
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(28)),
 	},
 	[4] = {
 		.has_osd_c	= 1,
@@ -1659,7 +1671,65 @@ static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
 		.palette_16bpp	= 1,
 		.valid_bpp	= (VALID_BPP(1) | VALID_BPP(2) |
 				   VALID_BPP(16) | VALID_BPP(18) |
-				   VALID_BPP(24) | VALID_BPP(25)),
+				   VALID_BPP(19) | VALID_BPP(24) |
+				   VALID_BPP(25) | VALID_BPP(28)),
+	},
+};
+
+static struct s3c_fb_win_variant s3c_fb_data_s5p_wins[] = {
+	[0] = {
+		.has_osd_c	= 1,
+		.osd_size_off	= 0x8,
+		.palette_sz	= 256,
+		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(13) |
+				   VALID_BPP(15) | VALID_BPP(16) |
+				   VALID_BPP(18) | VALID_BPP(19) |
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(32)),
+	},
+	[1] = {
+		.has_osd_c	= 1,
+		.has_osd_d	= 1,
+		.osd_size_off	= 0xc,
+		.has_osd_alpha	= 1,
+		.palette_sz	= 256,
+		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(13) |
+				   VALID_BPP(15) | VALID_BPP(16) |
+				   VALID_BPP(18) | VALID_BPP(19) |
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(32)),
+	},
+	[2] = {
+		.has_osd_c	= 1,
+		.has_osd_d	= 1,
+		.osd_size_off	= 0xc,
+		.has_osd_alpha	= 1,
+		.palette_sz	= 256,
+		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(13) |
+				   VALID_BPP(15) | VALID_BPP(16) |
+				   VALID_BPP(18) | VALID_BPP(19) |
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(32)),
+	},
+	[3] = {
+		.has_osd_c	= 1,
+		.has_osd_alpha	= 1,
+		.palette_sz	= 256,
+		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(13) |
+				   VALID_BPP(15) | VALID_BPP(16) |
+				   VALID_BPP(18) | VALID_BPP(19) |
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(32)),
+	},
+	[4] = {
+		.has_osd_c	= 1,
+		.has_osd_alpha	= 1,
+		.palette_sz	= 256,
+		.valid_bpp	= (VALID_BPP1248 | VALID_BPP(13) |
+				   VALID_BPP(15) | VALID_BPP(16) |
+				   VALID_BPP(18) | VALID_BPP(19) |
+				   VALID_BPP(24) | VALID_BPP(25) |
+				   VALID_BPP(32)),
 	},
 };
 
@@ -1716,11 +1786,11 @@ static struct s3c_fb_driverdata s3c_fb_data_s5pc100 = {
 
 		.has_prtcon	= 1,
 	},
-	.win[0]	= &s3c_fb_data_64xx_wins[0],
-	.win[1]	= &s3c_fb_data_64xx_wins[1],
-	.win[2]	= &s3c_fb_data_64xx_wins[2],
-	.win[3]	= &s3c_fb_data_64xx_wins[3],
-	.win[4]	= &s3c_fb_data_64xx_wins[4],
+	.win[0]	= &s3c_fb_data_s5p_wins[0],
+	.win[1]	= &s3c_fb_data_s5p_wins[1],
+	.win[2]	= &s3c_fb_data_s5p_wins[2],
+	.win[3]	= &s3c_fb_data_s5p_wins[3],
+	.win[4]	= &s3c_fb_data_s5p_wins[4],
 };
 
 static struct s3c_fb_driverdata s3c_fb_data_s5pv210 = {
@@ -1746,11 +1816,11 @@ static struct s3c_fb_driverdata s3c_fb_data_s5pv210 = {
 
 		.has_shadowcon	= 1,
 	},
-	.win[0]	= &s3c_fb_data_64xx_wins[0],
-	.win[1]	= &s3c_fb_data_64xx_wins[1],
-	.win[2]	= &s3c_fb_data_64xx_wins[2],
-	.win[3]	= &s3c_fb_data_64xx_wins[3],
-	.win[4]	= &s3c_fb_data_64xx_wins[4],
+	.win[0]	= &s3c_fb_data_s5p_wins[0],
+	.win[1]	= &s3c_fb_data_s5p_wins[1],
+	.win[2]	= &s3c_fb_data_s5p_wins[2],
+	.win[3]	= &s3c_fb_data_s5p_wins[3],
+	.win[4]	= &s3c_fb_data_s5p_wins[4],
 };
 
 /* S3C2443/S3C2416 style hardware */
diff --git a/drivers/video/s3c2410fb.c b/drivers/video/s3c2410fb.c
index 61c819e35f7f..0aa13761de6e 100644
--- a/drivers/video/s3c2410fb.c
+++ b/drivers/video/s3c2410fb.c
@@ -867,7 +867,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 		goto dealloc_fb;
 	}
 
-	size = (res->end - res->start) + 1;
+	size = resource_size(res);
 	info->mem = request_mem_region(res->start, size, pdev->name);
 	if (info->mem == NULL) {
 		dev_err(&pdev->dev, "failed to get memory region\n");
@@ -997,8 +997,7 @@ release_irq:
 release_regs:
 	iounmap(info->io);
 release_mem:
-	release_resource(info->mem);
-	kfree(info->mem);
+	release_mem_region(res->start, size);
 dealloc_fb:
 	platform_set_drvdata(pdev, NULL);
 	framebuffer_release(fbinfo);
@@ -1044,8 +1043,7 @@ static int __devexit s3c2410fb_remove(struct platform_device *pdev)
 
 	iounmap(info->io);
 
-	release_resource(info->mem);
-	kfree(info->mem);
+	release_mem_region(info->mem->start, resource_size(info->mem));
 
 	platform_set_drvdata(pdev, NULL);
 	framebuffer_release(fbinfo);
diff --git a/drivers/video/s3fb.c b/drivers/video/s3fb.c
index 75738a928610..4ca5d0c8fe84 100644
--- a/drivers/video/s3fb.c
+++ b/drivers/video/s3fb.c
@@ -25,6 +25,9 @@
 #include <linux/console.h> /* Why should fb driver call console functions? because console_lock() */
 #include <video/vga.h>
 
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
 #ifdef CONFIG_MTRR
 #include <asm/mtrr.h>
 #endif
@@ -36,6 +39,12 @@ struct s3fb_info {
 	struct mutex open_lock;
 	unsigned int ref_count;
 	u32 pseudo_palette[16];
+#ifdef CONFIG_FB_S3_DDC
+	u8 __iomem *mmio;
+	bool ddc_registered;
+	struct i2c_adapter ddc_adapter;
+	struct i2c_algo_bit_data ddc_algo;
+#endif
 };
 
 
@@ -64,15 +73,18 @@ static const struct svga_fb_format s3fb_formats[] = {
 
 static const struct svga_pll s3_pll = {3, 129, 3, 33, 0, 3,
 	35000, 240000, 14318};
+static const struct svga_pll s3_trio3d_pll = {3, 129, 3, 31, 0, 4,
+	230000, 460000, 14318};
 
 static const int s3_memsizes[] = {4096, 0, 3072, 8192, 2048, 6144, 1024, 512};
 
 static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64", "S3 Trio64V+",
 			"S3 Trio64UV+", "S3 Trio64V2/DX", "S3 Trio64V2/GX",
-			"S3 Plato/PX", "S3 Aurora64VP", "S3 Virge",
+			"S3 Plato/PX", "S3 Aurora64V+", "S3 Virge",
 			"S3 Virge/VX", "S3 Virge/DX", "S3 Virge/GX",
-			"S3 Virge/GX2", "S3 Virge/GX2P", "S3 Virge/GX2P",
-			"S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X"};
+			"S3 Virge/GX2", "S3 Virge/GX2+", "",
+			"S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X",
+			"S3 Trio3D"};
 
 #define CHIP_UNKNOWN		0x00
 #define CHIP_732_TRIO32		0x01
@@ -87,12 +99,12 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_988_VIRGE_VX	0x0A
 #define CHIP_375_VIRGE_DX	0x0B
 #define CHIP_385_VIRGE_GX	0x0C
-#define CHIP_356_VIRGE_GX2	0x0D
-#define CHIP_357_VIRGE_GX2P	0x0E
-#define CHIP_359_VIRGE_GX2P	0x0F
+#define CHIP_357_VIRGE_GX2	0x0D
+#define CHIP_359_VIRGE_GX2P	0x0E
 #define CHIP_360_TRIO3D_1X	0x10
 #define CHIP_362_TRIO3D_2X	0x11
 #define CHIP_368_TRIO3D_2X	0x12
+#define CHIP_365_TRIO3D		0x13
 
 #define CHIP_XXX_TRIO		0x80
 #define CHIP_XXX_TRIO64V2_DXGX	0x81
@@ -102,6 +114,9 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_UNDECIDED_FLAG	0x80
 #define CHIP_MASK		0xFF
 
+#define MMIO_OFFSET		0x1000000
+#define MMIO_SIZE		0x10000
+
 /* CRT timing register sets */
 
 static const struct vga_regset s3_h_total_regs[]        = {{0x00, 0, 7}, {0x5D, 0, 0}, VGA_REGSET_END};
@@ -119,9 +134,11 @@ static const struct vga_regset s3_v_sync_start_regs[]   = {{0x10, 0, 7}, {0x07,
 static const struct vga_regset s3_v_sync_end_regs[]     = {{0x11, 0, 3}, VGA_REGSET_END};
 
 static const struct vga_regset s3_line_compare_regs[]   = {{0x18, 0, 7}, {0x07, 4, 4}, {0x09, 6, 6}, {0x5E, 6, 6}, VGA_REGSET_END};
-static const struct vga_regset s3_start_address_regs[]  = {{0x0d, 0, 7}, {0x0c, 0, 7}, {0x31, 4, 5}, {0x51, 0, 1}, VGA_REGSET_END};
+static const struct vga_regset s3_start_address_regs[]  = {{0x0d, 0, 7}, {0x0c, 0, 7}, {0x69, 0, 4}, VGA_REGSET_END};
 static const struct vga_regset s3_offset_regs[]         = {{0x13, 0, 7}, {0x51, 4, 5}, VGA_REGSET_END}; /* set 0x43 bit 2 to 0 */
 
+static const struct vga_regset s3_dtpc_regs[]		= {{0x3B, 0, 7}, {0x5D, 6, 6}, VGA_REGSET_END};
+
 static const struct svga_timing_regs s3_timing_regs     = {
 	s3_h_total_regs, s3_h_display_regs, s3_h_blank_start_regs,
 	s3_h_blank_end_regs, s3_h_sync_start_regs, s3_h_sync_end_regs,
@@ -135,7 +152,7 @@ static const struct svga_timing_regs s3_timing_regs     = {
 /* Module parameters */
 
 
-static char *mode_option __devinitdata = "640x480-8@60";
+static char *mode_option __devinitdata;
 
 #ifdef CONFIG_MTRR
 static int mtrr __devinitdata = 1;
@@ -164,6 +181,119 @@ MODULE_PARM_DESC(fasttext, "Enable S3 fast text mode (1=enable, 0=disable, defau
 
 /* ------------------------------------------------------------------------- */
 
+#ifdef CONFIG_FB_S3_DDC
+
+#define DDC_REG		0xaa		/* Trio 3D/1X/2X */
+#define DDC_MMIO_REG	0xff20		/* all other chips */
+#define DDC_SCL_OUT	(1 << 0)
+#define DDC_SDA_OUT	(1 << 1)
+#define DDC_SCL_IN	(1 << 2)
+#define DDC_SDA_IN	(1 << 3)
+#define DDC_DRIVE_EN	(1 << 4)
+
+static bool s3fb_ddc_needs_mmio(int chip)
+{
+	return !(chip == CHIP_360_TRIO3D_1X  ||
+		 chip == CHIP_362_TRIO3D_2X  ||
+		 chip == CHIP_368_TRIO3D_2X);
+}
+
+static u8 s3fb_ddc_read(struct s3fb_info *par)
+{
+	if (s3fb_ddc_needs_mmio(par->chip))
+		return readb(par->mmio + DDC_MMIO_REG);
+	else
+		return vga_rcrt(par->state.vgabase, DDC_REG);
+}
+
+static void s3fb_ddc_write(struct s3fb_info *par, u8 val)
+{
+	if (s3fb_ddc_needs_mmio(par->chip))
+		writeb(val, par->mmio + DDC_MMIO_REG);
+	else
+		vga_wcrt(par->state.vgabase, DDC_REG, val);
+}
+
+static void s3fb_ddc_setscl(void *data, int val)
+{
+	struct s3fb_info *par = data;
+	unsigned char reg;
+
+	reg = s3fb_ddc_read(par) | DDC_DRIVE_EN;
+	if (val)
+		reg |= DDC_SCL_OUT;
+	else
+		reg &= ~DDC_SCL_OUT;
+	s3fb_ddc_write(par, reg);
+}
+
+static void s3fb_ddc_setsda(void *data, int val)
+{
+	struct s3fb_info *par = data;
+	unsigned char reg;
+
+	reg = s3fb_ddc_read(par) | DDC_DRIVE_EN;
+	if (val)
+		reg |= DDC_SDA_OUT;
+	else
+		reg &= ~DDC_SDA_OUT;
+	s3fb_ddc_write(par, reg);
+}
+
+static int s3fb_ddc_getscl(void *data)
+{
+	struct s3fb_info *par = data;
+
+	return !!(s3fb_ddc_read(par) & DDC_SCL_IN);
+}
+
+static int s3fb_ddc_getsda(void *data)
+{
+	struct s3fb_info *par = data;
+
+	return !!(s3fb_ddc_read(par) & DDC_SDA_IN);
+}
+
+static int __devinit s3fb_setup_ddc_bus(struct fb_info *info)
+{
+	struct s3fb_info *par = info->par;
+
+	strlcpy(par->ddc_adapter.name, info->fix.id,
+		sizeof(par->ddc_adapter.name));
+	par->ddc_adapter.owner		= THIS_MODULE;
+	par->ddc_adapter.class		= I2C_CLASS_DDC;
+	par->ddc_adapter.algo_data	= &par->ddc_algo;
+	par->ddc_adapter.dev.parent	= info->device;
+	par->ddc_algo.setsda		= s3fb_ddc_setsda;
+	par->ddc_algo.setscl		= s3fb_ddc_setscl;
+	par->ddc_algo.getsda		= s3fb_ddc_getsda;
+	par->ddc_algo.getscl		= s3fb_ddc_getscl;
+	par->ddc_algo.udelay		= 10;
+	par->ddc_algo.timeout		= 20;
+	par->ddc_algo.data		= par;
+
+	i2c_set_adapdata(&par->ddc_adapter, par);
+
+	/*
+	 * some Virge cards have external MUX to switch chip I2C bus between
+	 * DDC and extension pins - switch it do DDC
+	 */
+/*	vga_wseq(par->state.vgabase, 0x08, 0x06); - not needed, already unlocked */
+	if (par->chip == CHIP_357_VIRGE_GX2 ||
+	    par->chip == CHIP_359_VIRGE_GX2P)
+		svga_wseq_mask(par->state.vgabase, 0x0d, 0x01, 0x03);
+	else
+		svga_wseq_mask(par->state.vgabase, 0x0d, 0x00, 0x03);
+	/* some Virge need this or the DDC is ignored */
+	svga_wcrt_mask(par->state.vgabase, 0x5c, 0x03, 0x03);
+
+	return i2c_bit_add_bus(&par->ddc_adapter);
+}
+#endif /* CONFIG_FB_S3_DDC */
+
+
+/* ------------------------------------------------------------------------- */
+
 /* Set font in S3 fast text mode */
 
 static void s3fb_settile_fast(struct fb_info *info, struct fb_tilemap *map)
@@ -188,12 +318,19 @@ static void s3fb_settile_fast(struct fb_info *info, struct fb_tilemap *map)
 	}
 }
 
+static void s3fb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+	struct s3fb_info *par = info->par;
+
+	svga_tilecursor(par->state.vgabase, info, cursor);
+}
+
 static struct fb_tile_ops s3fb_tile_ops = {
 	.fb_settile	= svga_settile,
 	.fb_tilecopy	= svga_tilecopy,
 	.fb_tilefill    = svga_tilefill,
 	.fb_tileblit    = svga_tileblit,
-	.fb_tilecursor  = svga_tilecursor,
+	.fb_tilecursor  = s3fb_tilecursor,
 	.fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -202,7 +339,7 @@ static struct fb_tile_ops s3fb_fast_tile_ops = {
 	.fb_tilecopy	= svga_tilecopy,
 	.fb_tilefill    = svga_tilefill,
 	.fb_tileblit    = svga_tileblit,
-	.fb_tilecursor  = svga_tilecursor,
+	.fb_tilecursor  = s3fb_tilecursor,
 	.fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -334,33 +471,36 @@ static void s3_set_pixclock(struct fb_info *info, u32 pixclock)
 	u8 regval;
 	int rv;
 
-	rv = svga_compute_pll(&s3_pll, 1000000000 / pixclock, &m, &n, &r, info->node);
+	rv = svga_compute_pll((par->chip == CHIP_365_TRIO3D) ? &s3_trio3d_pll : &s3_pll,
+			      1000000000 / pixclock, &m, &n, &r, info->node);
 	if (rv < 0) {
 		printk(KERN_ERR "fb%d: cannot set requested pixclock, keeping old value\n", info->node);
 		return;
 	}
 
 	/* Set VGA misc register  */
-	regval = vga_r(NULL, VGA_MIS_R);
-	vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+	regval = vga_r(par->state.vgabase, VGA_MIS_R);
+	vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
 	/* Set S3 clock registers */
-	if (par->chip == CHIP_360_TRIO3D_1X ||
+	if (par->chip == CHIP_357_VIRGE_GX2 ||
+	    par->chip == CHIP_359_VIRGE_GX2P ||
+	    par->chip == CHIP_360_TRIO3D_1X ||
 	    par->chip == CHIP_362_TRIO3D_2X ||
 	    par->chip == CHIP_368_TRIO3D_2X) {
-		vga_wseq(NULL, 0x12, (n - 2) | ((r & 3) << 6));	/* n and two bits of r */
-		vga_wseq(NULL, 0x29, r >> 2); /* remaining highest bit of r */
+		vga_wseq(par->state.vgabase, 0x12, (n - 2) | ((r & 3) << 6));	/* n and two bits of r */
+		vga_wseq(par->state.vgabase, 0x29, r >> 2); /* remaining highest bit of r */
 	} else
-		vga_wseq(NULL, 0x12, (n - 2) | (r << 5));
-	vga_wseq(NULL, 0x13, m - 2);
+		vga_wseq(par->state.vgabase, 0x12, (n - 2) | (r << 5));
+	vga_wseq(par->state.vgabase, 0x13, m - 2);
 
 	udelay(1000);
 
 	/* Activate clock - write 0, 1, 0 to seq/15 bit 5 */
-	regval = vga_rseq (NULL, 0x15); /* | 0x80; */
-	vga_wseq(NULL, 0x15, regval & ~(1<<5));
-	vga_wseq(NULL, 0x15, regval |  (1<<5));
-	vga_wseq(NULL, 0x15, regval & ~(1<<5));
+	regval = vga_rseq (par->state.vgabase, 0x15); /* | 0x80; */
+	vga_wseq(par->state.vgabase, 0x15, regval & ~(1<<5));
+	vga_wseq(par->state.vgabase, 0x15, regval |  (1<<5));
+	vga_wseq(par->state.vgabase, 0x15, regval & ~(1<<5));
 }
 
 
@@ -372,7 +512,10 @@ static int s3fb_open(struct fb_info *info, int user)
 
 	mutex_lock(&(par->open_lock));
 	if (par->ref_count == 0) {
+		void __iomem *vgabase = par->state.vgabase;
+
 		memset(&(par->state), 0, sizeof(struct vgastate));
+		par->state.vgabase = vgabase;
 		par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
 		par->state.num_crtc = 0x70;
 		par->state.num_seq = 0x20;
@@ -470,6 +613,7 @@ static int s3fb_set_par(struct fb_info *info)
 	struct s3fb_info *par = info->par;
 	u32 value, mode, hmul, offset_value, screen_size, multiplex, dbytes;
 	u32 bpp = info->var.bits_per_pixel;
+	u32 htotal, hsstart;
 
 	if (bpp != 0) {
 		info->fix.ypanstep = 1;
@@ -504,99 +648,115 @@ static int s3fb_set_par(struct fb_info *info)
 	info->var.activate = FB_ACTIVATE_NOW;
 
 	/* Unlock registers */
-	vga_wcrt(NULL, 0x38, 0x48);
-	vga_wcrt(NULL, 0x39, 0xA5);
-	vga_wseq(NULL, 0x08, 0x06);
-	svga_wcrt_mask(0x11, 0x00, 0x80);
+	vga_wcrt(par->state.vgabase, 0x38, 0x48);
+	vga_wcrt(par->state.vgabase, 0x39, 0xA5);
+	vga_wseq(par->state.vgabase, 0x08, 0x06);
+	svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
 
 	/* Blank screen and turn off sync */
-	svga_wseq_mask(0x01, 0x20, 0x20);
-	svga_wcrt_mask(0x17, 0x00, 0x80);
+	svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+	svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
 	/* Set default values */
-	svga_set_default_gfx_regs();
-	svga_set_default_atc_regs();
-	svga_set_default_seq_regs();
-	svga_set_default_crt_regs();
-	svga_wcrt_multi(s3_line_compare_regs, 0xFFFFFFFF);
-	svga_wcrt_multi(s3_start_address_regs, 0);
+	svga_set_default_gfx_regs(par->state.vgabase);
+	svga_set_default_atc_regs(par->state.vgabase);
+	svga_set_default_seq_regs(par->state.vgabase);
+	svga_set_default_crt_regs(par->state.vgabase);
+	svga_wcrt_multi(par->state.vgabase, s3_line_compare_regs, 0xFFFFFFFF);
+	svga_wcrt_multi(par->state.vgabase, s3_start_address_regs, 0);
 
 	/* S3 specific initialization */
-	svga_wcrt_mask(0x58, 0x10, 0x10); /* enable linear framebuffer */
-	svga_wcrt_mask(0x31, 0x08, 0x08); /* enable sequencer access to framebuffer above 256 kB */
+	svga_wcrt_mask(par->state.vgabase, 0x58, 0x10, 0x10); /* enable linear framebuffer */
+	svga_wcrt_mask(par->state.vgabase, 0x31, 0x08, 0x08); /* enable sequencer access to framebuffer above 256 kB */
 
-/*	svga_wcrt_mask(0x33, 0x08, 0x08); */ /* DDR ?	*/
-/*	svga_wcrt_mask(0x43, 0x01, 0x01); */ /* DDR ?	*/
-	svga_wcrt_mask(0x33, 0x00, 0x08); /* no DDR ?	*/
-	svga_wcrt_mask(0x43, 0x00, 0x01); /* no DDR ?	*/
+/*	svga_wcrt_mask(par->state.vgabase, 0x33, 0x08, 0x08); */ /* DDR ?	*/
+/*	svga_wcrt_mask(par->state.vgabase, 0x43, 0x01, 0x01); */ /* DDR ?	*/
+	svga_wcrt_mask(par->state.vgabase, 0x33, 0x00, 0x08); /* no DDR ?	*/
+	svga_wcrt_mask(par->state.vgabase, 0x43, 0x00, 0x01); /* no DDR ?	*/
 
-	svga_wcrt_mask(0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
+	svga_wcrt_mask(par->state.vgabase, 0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
 
-/*	svga_wcrt_mask(0x58, 0x03, 0x03); */
+/*	svga_wcrt_mask(par->state.vgabase, 0x58, 0x03, 0x03); */
 
-/*	svga_wcrt_mask(0x53, 0x12, 0x13); */ /* enable MMIO */
-/*	svga_wcrt_mask(0x40, 0x08, 0x08); */ /* enable write buffer */
+/*	svga_wcrt_mask(par->state.vgabase, 0x53, 0x12, 0x13); */ /* enable MMIO */
+/*	svga_wcrt_mask(par->state.vgabase, 0x40, 0x08, 0x08); */ /* enable write buffer */
 
 
 	/* Set the offset register */
 	pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
-	svga_wcrt_multi(s3_offset_regs, offset_value);
+	svga_wcrt_multi(par->state.vgabase, s3_offset_regs, offset_value);
 
-	if (par->chip != CHIP_360_TRIO3D_1X &&
+	if (par->chip != CHIP_357_VIRGE_GX2 &&
+	    par->chip != CHIP_359_VIRGE_GX2P &&
+	    par->chip != CHIP_360_TRIO3D_1X &&
 	    par->chip != CHIP_362_TRIO3D_2X &&
 	    par->chip != CHIP_368_TRIO3D_2X) {
-		vga_wcrt(NULL, 0x54, 0x18); /* M parameter */
-		vga_wcrt(NULL, 0x60, 0xff); /* N parameter */
-		vga_wcrt(NULL, 0x61, 0xff); /* L parameter */
-		vga_wcrt(NULL, 0x62, 0xff); /* L parameter */
+		vga_wcrt(par->state.vgabase, 0x54, 0x18); /* M parameter */
+		vga_wcrt(par->state.vgabase, 0x60, 0xff); /* N parameter */
+		vga_wcrt(par->state.vgabase, 0x61, 0xff); /* L parameter */
+		vga_wcrt(par->state.vgabase, 0x62, 0xff); /* L parameter */
 	}
 
-	vga_wcrt(NULL, 0x3A, 0x35);
-	svga_wattr(0x33, 0x00);
+	vga_wcrt(par->state.vgabase, 0x3A, 0x35);
+	svga_wattr(par->state.vgabase, 0x33, 0x00);
 
 	if (info->var.vmode & FB_VMODE_DOUBLE)
-		svga_wcrt_mask(0x09, 0x80, 0x80);
+		svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
 	else
-		svga_wcrt_mask(0x09, 0x00, 0x80);
+		svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
 	if (info->var.vmode & FB_VMODE_INTERLACED)
-		svga_wcrt_mask(0x42, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x42, 0x20, 0x20);
 	else
-		svga_wcrt_mask(0x42, 0x00, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x42, 0x00, 0x20);
 
 	/* Disable hardware graphics cursor */
-	svga_wcrt_mask(0x45, 0x00, 0x01);
+	svga_wcrt_mask(par->state.vgabase, 0x45, 0x00, 0x01);
 	/* Disable Streams engine */
-	svga_wcrt_mask(0x67, 0x00, 0x0C);
+	svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0x0C);
 
 	mode = svga_match_format(s3fb_formats, &(info->var), &(info->fix));
 
 	/* S3 virge DX hack */
 	if (par->chip == CHIP_375_VIRGE_DX) {
-		vga_wcrt(NULL, 0x86, 0x80);
-		vga_wcrt(NULL, 0x90, 0x00);
+		vga_wcrt(par->state.vgabase, 0x86, 0x80);
+		vga_wcrt(par->state.vgabase, 0x90, 0x00);
 	}
 
 	/* S3 virge VX hack */
 	if (par->chip == CHIP_988_VIRGE_VX) {
-		vga_wcrt(NULL, 0x50, 0x00);
-		vga_wcrt(NULL, 0x67, 0x50);
+		vga_wcrt(par->state.vgabase, 0x50, 0x00);
+		vga_wcrt(par->state.vgabase, 0x67, 0x50);
 
-		vga_wcrt(NULL, 0x63, (mode <= 2) ? 0x90 : 0x09);
-		vga_wcrt(NULL, 0x66, 0x90);
+		vga_wcrt(par->state.vgabase, 0x63, (mode <= 2) ? 0x90 : 0x09);
+		vga_wcrt(par->state.vgabase, 0x66, 0x90);
 	}
 
-	if (par->chip == CHIP_360_TRIO3D_1X ||
+	if (par->chip == CHIP_357_VIRGE_GX2 ||
+	    par->chip == CHIP_359_VIRGE_GX2P ||
+	    par->chip == CHIP_360_TRIO3D_1X ||
 	    par->chip == CHIP_362_TRIO3D_2X ||
-	    par->chip == CHIP_368_TRIO3D_2X) {
+	    par->chip == CHIP_368_TRIO3D_2X ||
+	    par->chip == CHIP_365_TRIO3D    ||
+	    par->chip == CHIP_375_VIRGE_DX  ||
+	    par->chip == CHIP_385_VIRGE_GX) {
 		dbytes = info->var.xres * ((bpp+7)/8);
-		vga_wcrt(NULL, 0x91, (dbytes + 7) / 8);
-		vga_wcrt(NULL, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
+		vga_wcrt(par->state.vgabase, 0x91, (dbytes + 7) / 8);
+		vga_wcrt(par->state.vgabase, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
 
-		vga_wcrt(NULL, 0x66, 0x81);
+		vga_wcrt(par->state.vgabase, 0x66, 0x81);
 	}
 
-	svga_wcrt_mask(0x31, 0x00, 0x40);
+	if (par->chip == CHIP_357_VIRGE_GX2  ||
+	    par->chip == CHIP_359_VIRGE_GX2P ||
+	    par->chip == CHIP_360_TRIO3D_1X ||
+	    par->chip == CHIP_362_TRIO3D_2X ||
+	    par->chip == CHIP_368_TRIO3D_2X)
+		vga_wcrt(par->state.vgabase, 0x34, 0x00);
+	else	/* enable Data Transfer Position Control (DTPC) */
+		vga_wcrt(par->state.vgabase, 0x34, 0x10);
+
+	svga_wcrt_mask(par->state.vgabase, 0x31, 0x00, 0x40);
 	multiplex = 0;
 	hmul = 1;
 
@@ -604,51 +764,53 @@ static int s3fb_set_par(struct fb_info *info)
 	switch (mode) {
 	case 0:
 		pr_debug("fb%d: text mode\n", info->node);
-		svga_set_textmode_vga_regs();
+		svga_set_textmode_vga_regs(par->state.vgabase);
 
 		/* Set additional registers like in 8-bit mode */
-		svga_wcrt_mask(0x50, 0x00, 0x30);
-		svga_wcrt_mask(0x67, 0x00, 0xF0);
+		svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
 		/* Disable enhanced mode */
-		svga_wcrt_mask(0x3A, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
 
 		if (fasttext) {
 			pr_debug("fb%d: high speed text mode set\n", info->node);
-			svga_wcrt_mask(0x31, 0x40, 0x40);
+			svga_wcrt_mask(par->state.vgabase, 0x31, 0x40, 0x40);
 		}
 		break;
 	case 1:
 		pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-		vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
+		vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
 
 		/* Set additional registers like in 8-bit mode */
-		svga_wcrt_mask(0x50, 0x00, 0x30);
-		svga_wcrt_mask(0x67, 0x00, 0xF0);
+		svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
 		/* disable enhanced mode */
-		svga_wcrt_mask(0x3A, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
 		break;
 	case 2:
 		pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
 
 		/* Set additional registers like in 8-bit mode */
-		svga_wcrt_mask(0x50, 0x00, 0x30);
-		svga_wcrt_mask(0x67, 0x00, 0xF0);
+		svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
 		/* disable enhanced mode */
-		svga_wcrt_mask(0x3A, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
 		break;
 	case 3:
 		pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-		svga_wcrt_mask(0x50, 0x00, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
 		if (info->var.pixclock > 20000 ||
+		    par->chip == CHIP_357_VIRGE_GX2 ||
+		    par->chip == CHIP_359_VIRGE_GX2P ||
 		    par->chip == CHIP_360_TRIO3D_1X ||
 		    par->chip == CHIP_362_TRIO3D_2X ||
 		    par->chip == CHIP_368_TRIO3D_2X)
-			svga_wcrt_mask(0x67, 0x00, 0xF0);
+			svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 		else {
-			svga_wcrt_mask(0x67, 0x10, 0xF0);
+			svga_wcrt_mask(par->state.vgabase, 0x67, 0x10, 0xF0);
 			multiplex = 1;
 		}
 		break;
@@ -656,13 +818,24 @@ static int s3fb_set_par(struct fb_info *info)
 		pr_debug("fb%d: 5/5/5 truecolor\n", info->node);
 		if (par->chip == CHIP_988_VIRGE_VX) {
 			if (info->var.pixclock > 20000)
-				svga_wcrt_mask(0x67, 0x20, 0xF0);
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x20, 0xF0);
 			else
-				svga_wcrt_mask(0x67, 0x30, 0xF0);
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+		} else if (par->chip == CHIP_365_TRIO3D) {
+			svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+			if (info->var.pixclock > 8695) {
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+				hmul = 2;
+			} else {
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x20, 0xF0);
+				multiplex = 1;
+			}
 		} else {
-			svga_wcrt_mask(0x50, 0x10, 0x30);
-			svga_wcrt_mask(0x67, 0x30, 0xF0);
-			if (par->chip != CHIP_360_TRIO3D_1X &&
+			svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+			svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+			if (par->chip != CHIP_357_VIRGE_GX2 &&
+			    par->chip != CHIP_359_VIRGE_GX2P &&
+			    par->chip != CHIP_360_TRIO3D_1X &&
 			    par->chip != CHIP_362_TRIO3D_2X &&
 			    par->chip != CHIP_368_TRIO3D_2X)
 				hmul = 2;
@@ -672,13 +845,24 @@ static int s3fb_set_par(struct fb_info *info)
 		pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
 		if (par->chip == CHIP_988_VIRGE_VX) {
 			if (info->var.pixclock > 20000)
-				svga_wcrt_mask(0x67, 0x40, 0xF0);
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x40, 0xF0);
 			else
-				svga_wcrt_mask(0x67, 0x50, 0xF0);
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+		} else if (par->chip == CHIP_365_TRIO3D) {
+			svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+			if (info->var.pixclock > 8695) {
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+				hmul = 2;
+			} else {
+				svga_wcrt_mask(par->state.vgabase, 0x67, 0x40, 0xF0);
+				multiplex = 1;
+			}
 		} else {
-			svga_wcrt_mask(0x50, 0x10, 0x30);
-			svga_wcrt_mask(0x67, 0x50, 0xF0);
-			if (par->chip != CHIP_360_TRIO3D_1X &&
+			svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+			svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+			if (par->chip != CHIP_357_VIRGE_GX2 &&
+			    par->chip != CHIP_359_VIRGE_GX2P &&
+			    par->chip != CHIP_360_TRIO3D_1X &&
 			    par->chip != CHIP_362_TRIO3D_2X &&
 			    par->chip != CHIP_368_TRIO3D_2X)
 				hmul = 2;
@@ -687,12 +871,12 @@ static int s3fb_set_par(struct fb_info *info)
 	case 6:
 		/* VIRGE VX case */
 		pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
-		svga_wcrt_mask(0x67, 0xD0, 0xF0);
+		svga_wcrt_mask(par->state.vgabase, 0x67, 0xD0, 0xF0);
 		break;
 	case 7:
 		pr_debug("fb%d: 8/8/8/8 truecolor\n", info->node);
-		svga_wcrt_mask(0x50, 0x30, 0x30);
-		svga_wcrt_mask(0x67, 0xD0, 0xF0);
+		svga_wcrt_mask(par->state.vgabase, 0x50, 0x30, 0x30);
+		svga_wcrt_mask(par->state.vgabase, 0x67, 0xD0, 0xF0);
 		break;
 	default:
 		printk(KERN_ERR "fb%d: unsupported mode - bug\n", info->node);
@@ -700,25 +884,30 @@ static int s3fb_set_par(struct fb_info *info)
 	}
 
 	if (par->chip != CHIP_988_VIRGE_VX) {
-		svga_wseq_mask(0x15, multiplex ? 0x10 : 0x00, 0x10);
-		svga_wseq_mask(0x18, multiplex ? 0x80 : 0x00, 0x80);
+		svga_wseq_mask(par->state.vgabase, 0x15, multiplex ? 0x10 : 0x00, 0x10);
+		svga_wseq_mask(par->state.vgabase, 0x18, multiplex ? 0x80 : 0x00, 0x80);
 	}
 
 	s3_set_pixclock(info, info->var.pixclock);
-	svga_set_timings(&s3_timing_regs, &(info->var), hmul, 1,
+	svga_set_timings(par->state.vgabase, &s3_timing_regs, &(info->var), hmul, 1,
 			 (info->var.vmode & FB_VMODE_DOUBLE)     ? 2 : 1,
 			 (info->var.vmode & FB_VMODE_INTERLACED) ? 2 : 1,
 			 hmul, info->node);
 
 	/* Set interlaced mode start/end register */
-	value = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
-	value = ((value * hmul) / 8) - 5;
-	vga_wcrt(NULL, 0x3C, (value + 1) / 2);
+	htotal = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
+	htotal = ((htotal * hmul) / 8) - 5;
+	vga_wcrt(par->state.vgabase, 0x3C, (htotal + 1) / 2);
+
+	/* Set Data Transfer Position */
+	hsstart = ((info->var.xres + info->var.right_margin) * hmul) / 8;
+	value = clamp((htotal + hsstart + 1) / 2, hsstart + 4, htotal + 1);
+	svga_wcrt_multi(par->state.vgabase, s3_dtpc_regs, value);
 
 	memset_io(info->screen_base, 0x00, screen_size);
 	/* Device and screen back on */
-	svga_wcrt_mask(0x17, 0x80, 0x80);
-	svga_wseq_mask(0x01, 0x00, 0x20);
+	svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+	svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
 	return 0;
 }
@@ -788,31 +977,33 @@ static int s3fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int s3fb_blank(int blank_mode, struct fb_info *info)
 {
+	struct s3fb_info *par = info->par;
+
 	switch (blank_mode) {
 	case FB_BLANK_UNBLANK:
 		pr_debug("fb%d: unblank\n", info->node);
-		svga_wcrt_mask(0x56, 0x00, 0x06);
-		svga_wseq_mask(0x01, 0x00, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x56, 0x00, 0x06);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 		break;
 	case FB_BLANK_NORMAL:
 		pr_debug("fb%d: blank\n", info->node);
-		svga_wcrt_mask(0x56, 0x00, 0x06);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x56, 0x00, 0x06);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	case FB_BLANK_HSYNC_SUSPEND:
 		pr_debug("fb%d: hsync\n", info->node);
-		svga_wcrt_mask(0x56, 0x02, 0x06);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x56, 0x02, 0x06);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	case FB_BLANK_VSYNC_SUSPEND:
 		pr_debug("fb%d: vsync\n", info->node);
-		svga_wcrt_mask(0x56, 0x04, 0x06);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x56, 0x04, 0x06);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	case FB_BLANK_POWERDOWN:
 		pr_debug("fb%d: sync down\n", info->node);
-		svga_wcrt_mask(0x56, 0x06, 0x06);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x56, 0x06, 0x06);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	}
 
@@ -822,8 +1013,9 @@ static int s3fb_blank(int blank_mode, struct fb_info *info)
 
 /* Pan the display */
 
-static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info) {
-
+static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct s3fb_info *par = info->par;
 	unsigned int offset;
 
 	/* Calculate the offset */
@@ -837,7 +1029,7 @@ static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 	}
 
 	/* Set the offset */
-	svga_wcrt_multi(s3_start_address_regs, offset);
+	svga_wcrt_multi(par->state.vgabase, s3_start_address_regs, offset);
 
 	return 0;
 }
@@ -863,12 +1055,14 @@ static struct fb_ops s3fb_ops = {
 
 /* ------------------------------------------------------------------------- */
 
-static int __devinit s3_identification(int chip)
+static int __devinit s3_identification(struct s3fb_info *par)
 {
+	int chip = par->chip;
+
 	if (chip == CHIP_XXX_TRIO) {
-		u8 cr30 = vga_rcrt(NULL, 0x30);
-		u8 cr2e = vga_rcrt(NULL, 0x2e);
-		u8 cr2f = vga_rcrt(NULL, 0x2f);
+		u8 cr30 = vga_rcrt(par->state.vgabase, 0x30);
+		u8 cr2e = vga_rcrt(par->state.vgabase, 0x2e);
+		u8 cr2f = vga_rcrt(par->state.vgabase, 0x2f);
 
 		if ((cr30 == 0xE0) || (cr30 == 0xE1)) {
 			if (cr2e == 0x10)
@@ -883,7 +1077,7 @@ static int __devinit s3_identification(int chip)
 	}
 
 	if (chip == CHIP_XXX_TRIO64V2_DXGX) {
-		u8 cr6f = vga_rcrt(NULL, 0x6f);
+		u8 cr6f = vga_rcrt(par->state.vgabase, 0x6f);
 
 		if (! (cr6f & 0x01))
 			return CHIP_775_TRIO64V2_DX;
@@ -892,7 +1086,7 @@ static int __devinit s3_identification(int chip)
 	}
 
 	if (chip == CHIP_XXX_VIRGE_DXGX) {
-		u8 cr6f = vga_rcrt(NULL, 0x6f);
+		u8 cr6f = vga_rcrt(par->state.vgabase, 0x6f);
 
 		if (! (cr6f & 0x01))
 			return CHIP_375_VIRGE_DX;
@@ -901,7 +1095,7 @@ static int __devinit s3_identification(int chip)
 	}
 
 	if (chip == CHIP_36X_TRIO3D_1X_2X) {
-		switch (vga_rcrt(NULL, 0x2f)) {
+		switch (vga_rcrt(par->state.vgabase, 0x2f)) {
 		case 0x00:
 			return CHIP_360_TRIO3D_1X;
 		case 0x01:
@@ -919,10 +1113,13 @@ static int __devinit s3_identification(int chip)
 
 static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+	struct pci_bus_region bus_reg;
+	struct resource vga_res;
 	struct fb_info *info;
 	struct s3fb_info *par;
 	int rc;
 	u8 regval, cr38, cr39;
+	bool found = false;
 
 	/* Ignore secondary VGA device because there is no VGA arbitration */
 	if (! svga_primary_device(dev)) {
@@ -968,47 +1165,68 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
 		goto err_iomap;
 	}
 
+	bus_reg.start = 0;
+	bus_reg.end = 64 * 1024;
+
+	vga_res.flags = IORESOURCE_IO;
+
+	pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+	par->state.vgabase = (void __iomem *) vga_res.start;
+
 	/* Unlock regs */
-	cr38 = vga_rcrt(NULL, 0x38);
-	cr39 = vga_rcrt(NULL, 0x39);
-	vga_wseq(NULL, 0x08, 0x06);
-	vga_wcrt(NULL, 0x38, 0x48);
-	vga_wcrt(NULL, 0x39, 0xA5);
+	cr38 = vga_rcrt(par->state.vgabase, 0x38);
+	cr39 = vga_rcrt(par->state.vgabase, 0x39);
+	vga_wseq(par->state.vgabase, 0x08, 0x06);
+	vga_wcrt(par->state.vgabase, 0x38, 0x48);
+	vga_wcrt(par->state.vgabase, 0x39, 0xA5);
 
 	/* Identify chip type */
 	par->chip = id->driver_data & CHIP_MASK;
-	par->rev = vga_rcrt(NULL, 0x2f);
+	par->rev = vga_rcrt(par->state.vgabase, 0x2f);
 	if (par->chip & CHIP_UNDECIDED_FLAG)
-		par->chip = s3_identification(par->chip);
+		par->chip = s3_identification(par);
 
 	/* Find how many physical memory there is on card */
 	/* 0x36 register is accessible even if other registers are locked */
-	regval = vga_rcrt(NULL, 0x36);
+	regval = vga_rcrt(par->state.vgabase, 0x36);
 	if (par->chip == CHIP_360_TRIO3D_1X ||
 	    par->chip == CHIP_362_TRIO3D_2X ||
-	    par->chip == CHIP_368_TRIO3D_2X) {
+	    par->chip == CHIP_368_TRIO3D_2X ||
+	    par->chip == CHIP_365_TRIO3D) {
 		switch ((regval & 0xE0) >> 5) {
 		case 0: /* 8MB -- only 4MB usable for display */
 		case 1: /* 4MB with 32-bit bus */
 		case 2:	/* 4MB */
 			info->screen_size = 4 << 20;
 			break;
+		case 4: /* 2MB on 365 Trio3D */
 		case 6: /* 2MB */
 			info->screen_size = 2 << 20;
 			break;
 		}
+	} else if (par->chip == CHIP_357_VIRGE_GX2 ||
+		   par->chip == CHIP_359_VIRGE_GX2P) {
+		switch ((regval & 0xC0) >> 6) {
+		case 1: /* 4MB */
+			info->screen_size = 4 << 20;
+			break;
+		case 3: /* 2MB */
+			info->screen_size = 2 << 20;
+			break;
+		}
 	} else
 		info->screen_size = s3_memsizes[regval >> 5] << 10;
 	info->fix.smem_len = info->screen_size;
 
 	/* Find MCLK frequency */
-	regval = vga_rseq(NULL, 0x10);
-	par->mclk_freq = ((vga_rseq(NULL, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
+	regval = vga_rseq(par->state.vgabase, 0x10);
+	par->mclk_freq = ((vga_rseq(par->state.vgabase, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
 	par->mclk_freq = par->mclk_freq >> (regval >> 5);
 
 	/* Restore locks */
-	vga_wcrt(NULL, 0x38, cr38);
-	vga_wcrt(NULL, 0x39, cr39);
+	vga_wcrt(par->state.vgabase, 0x38, cr38);
+	vga_wcrt(par->state.vgabase, 0x39, cr39);
 
 	strcpy(info->fix.id, s3_names [par->chip]);
 	info->fix.mmio_start = 0;
@@ -1018,12 +1236,77 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
 	info->fix.ypanstep = 0;
 	info->fix.accel = FB_ACCEL_NONE;
 	info->pseudo_palette = (void*) (par->pseudo_palette);
+	info->var.bits_per_pixel = 8;
+
+#ifdef CONFIG_FB_S3_DDC
+	/* Enable MMIO if needed */
+	if (s3fb_ddc_needs_mmio(par->chip)) {
+		par->mmio = ioremap(info->fix.smem_start + MMIO_OFFSET, MMIO_SIZE);
+		if (par->mmio)
+			svga_wcrt_mask(par->state.vgabase, 0x53, 0x08, 0x08);	/* enable MMIO */
+		else
+			dev_err(info->device, "unable to map MMIO at 0x%lx, disabling DDC",
+				info->fix.smem_start + MMIO_OFFSET);
+	}
+	if (!s3fb_ddc_needs_mmio(par->chip) || par->mmio)
+		if (s3fb_setup_ddc_bus(info) == 0) {
+			u8 *edid = fb_ddc_read(&par->ddc_adapter);
+			par->ddc_registered = true;
+			if (edid) {
+				fb_edid_to_monspecs(edid, &info->monspecs);
+				kfree(edid);
+				if (!info->monspecs.modedb)
+					dev_err(info->device, "error getting mode database\n");
+				else {
+					const struct fb_videomode *m;
+
+					fb_videomode_to_modelist(info->monspecs.modedb,
+								 info->monspecs.modedb_len,
+								 &info->modelist);
+					m = fb_find_best_display(&info->monspecs, &info->modelist);
+					if (m) {
+						fb_videomode_to_var(&info->var, m);
+						/* fill all other info->var's fields */
+						if (s3fb_check_var(&info->var, info) == 0)
+							found = true;
+					}
+				}
+			}
+		}
+#endif
+	if (!mode_option && !found)
+		mode_option = "640x480-8@60";
 
 	/* Prepare startup mode */
-	rc = fb_find_mode(&(info->var), info, mode_option, NULL, 0, NULL, 8);
-	if (! ((rc == 1) || (rc == 2))) {
-		rc = -EINVAL;
-		dev_err(info->device, "mode %s not found\n", mode_option);
+	if (mode_option) {
+		rc = fb_find_mode(&info->var, info, mode_option,
+				   info->monspecs.modedb, info->monspecs.modedb_len,
+				   NULL, info->var.bits_per_pixel);
+		if (!rc || rc == 4) {
+			rc = -EINVAL;
+			dev_err(info->device, "mode %s not found\n", mode_option);
+			fb_destroy_modedb(info->monspecs.modedb);
+			info->monspecs.modedb = NULL;
+			goto err_find_mode;
+		}
+	}
+
+	fb_destroy_modedb(info->monspecs.modedb);
+	info->monspecs.modedb = NULL;
+
+	/* maximize virtual vertical size for fast scrolling */
+	info->var.yres_virtual = info->fix.smem_len * 8 /
+			(info->var.bits_per_pixel * info->var.xres_virtual);
+	if (info->var.yres_virtual < info->var.yres) {
+		dev_err(info->device, "virtual vertical size smaller than real\n");
+		goto err_find_mode;
+	}
+
+	/* maximize virtual vertical size for fast scrolling */
+	info->var.yres_virtual = info->fix.smem_len * 8 /
+			(info->var.bits_per_pixel * info->var.xres_virtual);
+	if (info->var.yres_virtual < info->var.yres) {
+		dev_err(info->device, "virtual vertical size smaller than real\n");
 		goto err_find_mode;
 	}
 
@@ -1044,8 +1327,8 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
 
 	if (par->chip == CHIP_UNKNOWN)
 		printk(KERN_INFO "fb%d: unknown chip, CR2D=%x, CR2E=%x, CRT2F=%x, CRT30=%x\n",
-			info->node, vga_rcrt(NULL, 0x2d), vga_rcrt(NULL, 0x2e),
-			vga_rcrt(NULL, 0x2f), vga_rcrt(NULL, 0x30));
+			info->node, vga_rcrt(par->state.vgabase, 0x2d), vga_rcrt(par->state.vgabase, 0x2e),
+			vga_rcrt(par->state.vgabase, 0x2f), vga_rcrt(par->state.vgabase, 0x30));
 
 	/* Record a reference to the driver data */
 	pci_set_drvdata(dev, info);
@@ -1064,6 +1347,12 @@ err_reg_fb:
 	fb_dealloc_cmap(&info->cmap);
 err_alloc_cmap:
 err_find_mode:
+#ifdef CONFIG_FB_S3_DDC
+	if (par->ddc_registered)
+		i2c_del_adapter(&par->ddc_adapter);
+	if (par->mmio)
+		iounmap(par->mmio);
+#endif
 	pci_iounmap(dev, info->screen_base);
 err_iomap:
 	pci_release_regions(dev);
@@ -1080,12 +1369,11 @@ err_enable_device:
 static void __devexit s3_pci_remove(struct pci_dev *dev)
 {
 	struct fb_info *info = pci_get_drvdata(dev);
+	struct s3fb_info __maybe_unused *par = info->par;
 
 	if (info) {
 
 #ifdef CONFIG_MTRR
-		struct s3fb_info *par = info->par;
-
 		if (par->mtrr_reg >= 0) {
 			mtrr_del(par->mtrr_reg, 0, 0);
 			par->mtrr_reg = -1;
@@ -1095,6 +1383,13 @@ static void __devexit s3_pci_remove(struct pci_dev *dev)
 		unregister_framebuffer(info);
 		fb_dealloc_cmap(&info->cmap);
 
+#ifdef CONFIG_FB_S3_DDC
+		if (par->ddc_registered)
+			i2c_del_adapter(&par->ddc_adapter);
+		if (par->mmio)
+			iounmap(par->mmio);
+#endif
+
 		pci_iounmap(dev, info->screen_base);
 		pci_release_regions(dev);
 /*		pci_disable_device(dev); */
@@ -1188,10 +1483,11 @@ static struct pci_device_id s3_devices[] __devinitdata = {
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x5631), .driver_data = CHIP_325_VIRGE},
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x883D), .driver_data = CHIP_988_VIRGE_VX},
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A01), .driver_data = CHIP_XXX_VIRGE_DXGX},
-	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A10), .driver_data = CHIP_356_VIRGE_GX2},
-	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A11), .driver_data = CHIP_357_VIRGE_GX2P},
+	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A10), .driver_data = CHIP_357_VIRGE_GX2},
+	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A11), .driver_data = CHIP_359_VIRGE_GX2P},
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A12), .driver_data = CHIP_359_VIRGE_GX2P},
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A13), .driver_data = CHIP_36X_TRIO3D_1X_2X},
+	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8904), .driver_data = CHIP_365_TRIO3D},
 
 	{0, 0, 0, 0, 0, 0, 0}
 };
diff --git a/drivers/video/savage/savagefb-i2c.c b/drivers/video/savage/savagefb-i2c.c
index b16e6138fdd4..80fa87e2ae2f 100644
--- a/drivers/video/savage/savagefb-i2c.c
+++ b/drivers/video/savage/savagefb-i2c.c
@@ -159,8 +159,7 @@ static int savage_setup_i2c_bus(struct savagefb_i2c_chan *chan,
 		else
 			dev_warn(&chan->par->pcidev->dev,
 				 "Failed to register I2C bus %s.\n", name);
-	} else
-		chan->par = NULL;
+	}
 
 	return rc;
 }
@@ -170,9 +169,10 @@ void savagefb_create_i2c_busses(struct fb_info *info)
 	struct savagefb_par *par = info->par;
 	par->chan.par	= par;
 
-	switch(info->fix.accel) {
-	case FB_ACCEL_PROSAVAGE_DDRK:
-	case FB_ACCEL_PROSAVAGE_PM:
+	switch (par->chip) {
+	case S3_PROSAVAGE:
+	case S3_PROSAVAGEDDR:
+	case S3_TWISTER:
 		par->chan.reg         = CR_SERIAL2;
 		par->chan.ioaddr      = par->mmio.vbase;
 		par->chan.algo.setsda = prosavage_gpio_setsda;
@@ -180,7 +180,7 @@ void savagefb_create_i2c_busses(struct fb_info *info)
 		par->chan.algo.getsda = prosavage_gpio_getsda;
 		par->chan.algo.getscl = prosavage_gpio_getscl;
 		break;
-	case FB_ACCEL_SAVAGE4:
+	case S3_SAVAGE4:
 		par->chan.reg = CR_SERIAL1;
 		if (par->pcidev->revision > 1 && !(VGArCR(0xa6, par) & 0x40))
 			par->chan.reg = CR_SERIAL2;
@@ -190,8 +190,8 @@ void savagefb_create_i2c_busses(struct fb_info *info)
 		par->chan.algo.getsda = prosavage_gpio_getsda;
 		par->chan.algo.getscl = prosavage_gpio_getscl;
 		break;
-	case FB_ACCEL_SAVAGE2000:
-		par->chan.reg         = 0xff20;
+	case S3_SAVAGE2000:
+		par->chan.reg         = MM_SERIAL1;
 		par->chan.ioaddr      = par->mmio.vbase;
 		par->chan.algo.setsda = savage4_gpio_setsda;
 		par->chan.algo.setscl = savage4_gpio_setscl;
diff --git a/drivers/video/savage/savagefb.h b/drivers/video/savage/savagefb.h
index e4c3f214eb8e..dcaab9012ca2 100644
--- a/drivers/video/savage/savagefb.h
+++ b/drivers/video/savage/savagefb.h
@@ -36,7 +36,6 @@
 #define PCI_CHIP_SAVAGE_IX    0x8c13
 #define PCI_CHIP_PROSAVAGE_PM 0x8a25
 #define PCI_CHIP_PROSAVAGE_KM 0x8a26
- /* Twister is a code name; hope I get the real name soon. */
 #define PCI_CHIP_S3TWISTER_P  0x8d01
 #define PCI_CHIP_S3TWISTER_K  0x8d02
 #define PCI_CHIP_PROSAVAGE_DDR          0x8d03
@@ -52,14 +51,15 @@
 #define PCI_CHIP_SUPSAV_IXCDDR		0x8c2f
 
 
+#define S3_SAVAGE_SERIES(chip)    ((chip>=S3_SAVAGE3D) && (chip<=S3_SAVAGE2000))
 
 #define S3_SAVAGE3D_SERIES(chip)  ((chip>=S3_SAVAGE3D) && (chip<=S3_SAVAGE_MX))
 
-#define S3_SAVAGE4_SERIES(chip)   ((chip==S3_SAVAGE4) || (chip==S3_PROSAVAGE))
+#define S3_SAVAGE4_SERIES(chip)   ((chip>=S3_SAVAGE4) && (chip<=S3_PROSAVAGEDDR))
 
 #define S3_SAVAGE_MOBILE_SERIES(chip)  ((chip==S3_SAVAGE_MX) || (chip==S3_SUPERSAVAGE))
 
-#define S3_SAVAGE_SERIES(chip)    ((chip>=S3_SAVAGE3D) && (chip<=S3_SAVAGE2000))
+#define S3_MOBILE_TWISTER_SERIES(chip) ((chip==S3_TWISTER) || (chip==S3_PROSAVAGEDDR))
 
 /* Chip tags.  These are used to group the adapters into
  * related families.
@@ -71,6 +71,8 @@ typedef enum {
   S3_SAVAGE_MX,
   S3_SAVAGE4,
   S3_PROSAVAGE,
+  S3_TWISTER,
+  S3_PROSAVAGEDDR,
   S3_SUPERSAVAGE,
   S3_SAVAGE2000,
   S3_LAST
@@ -153,7 +155,7 @@ struct savage_reg {
 	unsigned char CRTC[25];       /* Crtc Controller */
 	unsigned char Sequencer[5];   /* Video Sequencer */
 	unsigned char Graphics[9];    /* Video Graphics */
-	unsigned char Attribute[21];  /* Video Atribute */
+	unsigned char Attribute[21];  /* Video Attribute */
 
 	unsigned int mode, refresh;
 	unsigned char SR08, SR0E, SR0F;
diff --git a/drivers/video/savage/savagefb_driver.c b/drivers/video/savage/savagefb_driver.c
index 487911e2926c..4de541ca9c52 100644
--- a/drivers/video/savage/savagefb_driver.c
+++ b/drivers/video/savage/savagefb_driver.c
@@ -328,7 +328,9 @@ SavageSetup2DEngine(struct savagefb_par  *par)
 		savage_out32(0x48C18, savage_in32(0x48C18, par) | 0x0C, par);
 		break;
 	case S3_SAVAGE4:
+	case S3_TWISTER:
 	case S3_PROSAVAGE:
+	case S3_PROSAVAGEDDR:
 	case S3_SUPERSAVAGE:
 		/* Disable BCI */
 		savage_out32(0x48C18, savage_in32(0x48C18, par) & 0x3FF0, par);
@@ -385,7 +387,7 @@ SavageSetup2DEngine(struct savagefb_par  *par)
 	BCI_SEND(GlobalBitmapDescriptor);
 
 	/*
-	 * I don't know why, sending this twice fixes the intial black screen,
+	 * I don't know why, sending this twice fixes the initial black screen,
 	 * prevents X from crashing at least in Toshiba laptops with SavageIX.
 	 * --Tony
 	 */
@@ -1886,6 +1888,8 @@ static int savage_init_hw(struct savagefb_par *par)
 		break;
 
 	case S3_PROSAVAGE:
+	case S3_PROSAVAGEDDR:
+	case S3_TWISTER:
 		videoRam = RamSavageNB[(config1 & 0xE0) >> 5] * 1024;
 		break;
 
@@ -1963,7 +1967,8 @@ static int savage_init_hw(struct savagefb_par *par)
 		}
 	}
 
-	if (S3_SAVAGE_MOBILE_SERIES(par->chip) && !par->crtonly)
+	if ((S3_SAVAGE_MOBILE_SERIES(par->chip) ||
+	     S3_MOBILE_TWISTER_SERIES(par->chip)) && !par->crtonly)
 		par->display_type = DISP_LCD;
 	else if (dvi || (par->chip == S3_SAVAGE4 && par->dvi))
 		par->display_type = DISP_DFP;
@@ -2111,19 +2116,19 @@ static int __devinit savage_init_fb_info(struct fb_info *info,
 		snprintf(info->fix.id, 16, "ProSavageKM");
 		break;
 	case FB_ACCEL_S3TWISTER_P:
-		par->chip = S3_PROSAVAGE;
+		par->chip = S3_TWISTER;
 		snprintf(info->fix.id, 16, "TwisterP");
 		break;
 	case FB_ACCEL_S3TWISTER_K:
-		par->chip = S3_PROSAVAGE;
+		par->chip = S3_TWISTER;
 		snprintf(info->fix.id, 16, "TwisterK");
 		break;
 	case FB_ACCEL_PROSAVAGE_DDR:
-		par->chip = S3_PROSAVAGE;
+		par->chip = S3_PROSAVAGEDDR;
 		snprintf(info->fix.id, 16, "ProSavageDDR");
 		break;
 	case FB_ACCEL_PROSAVAGE_DDRK:
-		par->chip = S3_PROSAVAGE;
+		par->chip = S3_PROSAVAGEDDR;
 		snprintf(info->fix.id, 16, "ProSavage8");
 		break;
 	}
@@ -2211,7 +2216,7 @@ static int __devinit savagefb_probe(struct pci_dev* dev,
 		goto failed_mmio;
 
 	video_len = savage_init_hw(par);
-	/* FIXME: cant be negative */
+	/* FIXME: can't be negative */
 	if (video_len < 0) {
 		err = video_len;
 		goto failed_mmio;
@@ -2232,6 +2237,22 @@ static int __devinit savagefb_probe(struct pci_dev* dev,
 				 &info->modelist);
 #endif
 	info->var = savagefb_var800x600x8;
+	/* if a panel was detected, default to a CVT mode instead */
+	if (par->SavagePanelWidth) {
+		struct fb_videomode cvt_mode;
+
+		memset(&cvt_mode, 0, sizeof(cvt_mode));
+		cvt_mode.xres = par->SavagePanelWidth;
+		cvt_mode.yres = par->SavagePanelHeight;
+		cvt_mode.refresh = 60;
+		/* FIXME: if we know there is only the panel
+		 * we can enable reduced blanking as well */
+		if (fb_find_mode_cvt(&cvt_mode, 0, 0))
+			printk(KERN_WARNING "No CVT mode found for panel\n");
+		else if (fb_find_mode(&info->var, info, NULL, NULL, 0,
+				      &cvt_mode, 0) != 3)
+			info->var = savagefb_var800x600x8;
+	}
 
 	if (mode_option) {
 		fb_find_mode(&info->var, info, mode_option,
diff --git a/drivers/video/sh7760fb.c b/drivers/video/sh7760fb.c
index bea38fce2470..45e47d847163 100644
--- a/drivers/video/sh7760fb.c
+++ b/drivers/video/sh7760fb.c
@@ -459,14 +459,14 @@ static int __devinit sh7760fb_probe(struct platform_device *pdev)
 	}
 
 	par->ioarea = request_mem_region(res->start,
-					 (res->end - res->start), pdev->name);
+					 resource_size(res), pdev->name);
 	if (!par->ioarea) {
 		dev_err(&pdev->dev, "mmio area busy\n");
 		ret = -EBUSY;
 		goto out_fb;
 	}
 
-	par->base = ioremap_nocache(res->start, res->end - res->start + 1);
+	par->base = ioremap_nocache(res->start, resource_size(res));
 	if (!par->base) {
 		dev_err(&pdev->dev, "cannot remap\n");
 		ret = -ENODEV;
@@ -551,8 +551,7 @@ out_unmap:
 		free_irq(par->irq, &par->vsync);
 	iounmap(par->base);
 out_res:
-	release_resource(par->ioarea);
-	kfree(par->ioarea);
+	release_mem_region(res->start, resource_size(res));
 out_fb:
 	framebuffer_release(info);
 	return ret;
@@ -570,8 +569,7 @@ static int __devexit sh7760fb_remove(struct platform_device *dev)
 	if (par->irq >= 0)
 		free_irq(par->irq, par);
 	iounmap(par->base);
-	release_resource(par->ioarea);
-	kfree(par->ioarea);
+	release_mem_region(par->ioarea->start, resource_size(par->ioarea));
 	framebuffer_release(info);
 	platform_set_drvdata(dev, NULL);
 
diff --git a/drivers/video/sh_mobile_hdmi.c b/drivers/video/sh_mobile_hdmi.c
index 2b9e56a6bde4..7d54e2c612f7 100644
--- a/drivers/video/sh_mobile_hdmi.c
+++ b/drivers/video/sh_mobile_hdmi.c
@@ -1127,9 +1127,6 @@ static void sh_hdmi_edid_work_fn(struct work_struct *work)
 		struct fb_info *info = hdmi->info;
 		unsigned long parent_rate = 0, hdmi_rate;
 
-		/* A device has been plugged in */
-		pm_runtime_get_sync(hdmi->dev);
-
 		ret = sh_hdmi_read_edid(hdmi, &hdmi_rate, &parent_rate);
 		if (ret < 0)
 			goto out;
@@ -1187,7 +1184,6 @@ static void sh_hdmi_edid_work_fn(struct work_struct *work)
 		fb_set_suspend(hdmi->info, 1);
 
 		console_unlock();
-		pm_runtime_put(hdmi->dev);
 	}
 
 out:
@@ -1308,7 +1304,7 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 	INIT_DELAYED_WORK(&hdmi->edid_work, sh_hdmi_edid_work_fn);
 
 	pm_runtime_enable(&pdev->dev);
-	pm_runtime_resume(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
 
 	/* Product and revision IDs are 0 in sh-mobile version */
 	dev_info(&pdev->dev, "Detected HDMI controller 0x%x:0x%x\n",
@@ -1336,6 +1332,7 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 ecodec:
 	free_irq(irq, hdmi);
 ereqirq:
+	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 	iounmap(hdmi->base);
 emap:
@@ -1372,6 +1369,7 @@ static int __exit sh_hdmi_remove(struct platform_device *pdev)
 	free_irq(irq, hdmi);
 	/* Wait for already scheduled work */
 	cancel_delayed_work_sync(&hdmi->edid_work);
+	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 	clk_disable(hdmi->hdmi_clk);
 	clk_put(hdmi->hdmi_clk);
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index bf12e53aed5c..b048417247e8 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -21,10 +21,13 @@
 #include <linux/ioctl.h>
 #include <linux/slab.h>
 #include <linux/console.h>
+#include <linux/backlight.h>
+#include <linux/gpio.h>
 #include <video/sh_mobile_lcdc.h>
-#include <asm/atomic.h>
+#include <linux/atomic.h>
 
 #include "sh_mobile_lcdcfb.h"
+#include "sh_mobile_meram.h"
 
 #define SIDE_B_OFFSET 0x1000
 #define MIRROR_OFFSET 0x2000
@@ -67,6 +70,7 @@ static unsigned long lcdc_offs_mainlcd[NR_CH_REGS] = {
 	[LDSM1R] = 0x428,
 	[LDSM2R] = 0x42c,
 	[LDSA1R] = 0x430,
+	[LDSA2R] = 0x434,
 	[LDMLSR] = 0x438,
 	[LDHCNR] = 0x448,
 	[LDHSYNR] = 0x44c,
@@ -140,6 +144,7 @@ struct sh_mobile_lcdc_priv {
 	unsigned long saved_shared_regs[NR_SHARED_REGS];
 	int started;
 	int forced_bpp; /* 2 channel LCDC must share bpp setting */
+	struct sh_mobile_meram_info *meram_dev;
 };
 
 static bool banked(int reg_nr)
@@ -151,6 +156,7 @@ static bool banked(int reg_nr)
 	case LDDFR:
 	case LDSM1R:
 	case LDSA1R:
+	case LDSA2R:
 	case LDMLSR:
 	case LDHCNR:
 	case LDHSYNR:
@@ -463,8 +469,8 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 	struct sh_mobile_lcdc_board_cfg	*board_cfg;
 	unsigned long tmp;
 	int bpp = 0;
-	int k, m;
-	int ret = 0;
+	unsigned long ldddsr;
+	int k, m, ret;
 
 	/* enable clocks before accessing the hardware */
 	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
@@ -533,27 +539,36 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 		lcdc_write_chan(ch, LDPMR, 0);
 
 		board_cfg = &ch->cfg.board_cfg;
-		if (board_cfg->setup_sys)
-			ret = board_cfg->setup_sys(board_cfg->board_data, ch,
-						   &sh_mobile_lcdc_sys_bus_ops);
-		if (ret)
-			return ret;
+		if (board_cfg->setup_sys) {
+			ret = board_cfg->setup_sys(board_cfg->board_data,
+						ch, &sh_mobile_lcdc_sys_bus_ops);
+			if (ret)
+				return ret;
+		}
 	}
 
 	/* word and long word swap */
-	switch (bpp) {
-	case 16:
-		lcdc_write(priv, _LDDDSR, lcdc_read(priv, _LDDDSR) | 6);
-		break;
-	case 24:
-		lcdc_write(priv, _LDDDSR, lcdc_read(priv, _LDDDSR) | 7);
-		break;
-	case 32:
-		lcdc_write(priv, _LDDDSR, lcdc_read(priv, _LDDDSR) | 4);
-		break;
+	ldddsr = lcdc_read(priv, _LDDDSR);
+	if  (priv->ch[0].info->var.nonstd)
+		lcdc_write(priv, _LDDDSR, ldddsr | 7);
+	else {
+		switch (bpp) {
+		case 16:
+			lcdc_write(priv, _LDDDSR, ldddsr | 6);
+			break;
+		case 24:
+			lcdc_write(priv, _LDDDSR, ldddsr | 7);
+			break;
+		case 32:
+			lcdc_write(priv, _LDDDSR, ldddsr | 4);
+			break;
+		}
 	}
 
 	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+		unsigned long base_addr_y;
+		unsigned long base_addr_c = 0;
+		int pitch;
 		ch = &priv->ch[k];
 
 		if (!priv->ch[k].enabled)
@@ -561,24 +576,95 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
 		/* set bpp format in PKF[4:0] */
 		tmp = lcdc_read_chan(ch, LDDFR);
-		tmp &= ~0x0001001f;
-		switch (ch->info->var.bits_per_pixel) {
-		case 16:
-			tmp |= 0x03;
-			break;
-		case 24:
-			tmp |= 0x0b;
-			break;
-		case 32:
-			break;
+		tmp &= ~0x0003031f;
+		if (ch->info->var.nonstd) {
+			tmp |= (ch->info->var.nonstd << 16);
+			switch (ch->info->var.bits_per_pixel) {
+			case 12:
+				break;
+			case 16:
+				tmp |= (0x1 << 8);
+				break;
+			case 24:
+				tmp |= (0x2 << 8);
+				break;
+			}
+		} else {
+			switch (ch->info->var.bits_per_pixel) {
+			case 16:
+				tmp |= 0x03;
+				break;
+			case 24:
+				tmp |= 0x0b;
+				break;
+			case 32:
+				break;
+			}
 		}
 		lcdc_write_chan(ch, LDDFR, tmp);
 
+		base_addr_y = ch->info->fix.smem_start;
+		base_addr_c = base_addr_y +
+				ch->info->var.xres *
+				ch->info->var.yres_virtual;
+		pitch = ch->info->fix.line_length;
+
+		/* test if we can enable meram */
+		if (ch->cfg.meram_cfg && priv->meram_dev &&
+				priv->meram_dev->ops) {
+			struct sh_mobile_meram_cfg *cfg;
+			struct sh_mobile_meram_info *mdev;
+			unsigned long icb_addr_y, icb_addr_c;
+			int icb_pitch;
+			int pf;
+
+			cfg = ch->cfg.meram_cfg;
+			mdev = priv->meram_dev;
+			/* we need to de-init configured ICBs before we
+			 * we can re-initialize them.
+			 */
+			if (ch->meram_enabled)
+				mdev->ops->meram_unregister(mdev, cfg);
+
+			ch->meram_enabled = 0;
+
+			if (ch->info->var.nonstd) {
+				if (ch->info->var.bits_per_pixel == 24)
+					pf = SH_MOBILE_MERAM_PF_NV24;
+				else
+					pf = SH_MOBILE_MERAM_PF_NV;
+			} else {
+				pf = SH_MOBILE_MERAM_PF_RGB;
+			}
+
+			ret = mdev->ops->meram_register(mdev, cfg, pitch,
+						ch->info->var.yres,
+						pf,
+						base_addr_y,
+						base_addr_c,
+						&icb_addr_y,
+						&icb_addr_c,
+						&icb_pitch);
+			if (!ret)  {
+				/* set LDSA1R value */
+				base_addr_y = icb_addr_y;
+				pitch = icb_pitch;
+
+				/* set LDSA2R value if required */
+				if (base_addr_c)
+					base_addr_c = icb_addr_c;
+
+				ch->meram_enabled = 1;
+			}
+		}
+
 		/* point out our frame buffer */
-		lcdc_write_chan(ch, LDSA1R, ch->info->fix.smem_start);
+		lcdc_write_chan(ch, LDSA1R, base_addr_y);
+		if (ch->info->var.nonstd)
+			lcdc_write_chan(ch, LDSA2R, base_addr_c);
 
 		/* set line size */
-		lcdc_write_chan(ch, LDMLSR, ch->info->fix.line_length);
+		lcdc_write_chan(ch, LDMLSR, pitch);
 
 		/* setup deferred io if SYS bus */
 		tmp = ch->cfg.sys_bus_cfg.deferred_io_msec;
@@ -614,10 +700,15 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 			continue;
 
 		board_cfg = &ch->cfg.board_cfg;
-		if (try_module_get(board_cfg->owner) && board_cfg->display_on) {
+		if (board_cfg->display_on && try_module_get(board_cfg->owner)) {
 			board_cfg->display_on(board_cfg->board_data, ch->info);
 			module_put(board_cfg->owner);
 		}
+
+		if (ch->bl) {
+			ch->bl->props.power = FB_BLANK_UNBLANK;
+			backlight_update_status(ch->bl);
+		}
 	}
 
 	return 0;
@@ -648,11 +739,27 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
 			sh_mobile_lcdc_clk_on(priv);
 		}
 
+		if (ch->bl) {
+			ch->bl->props.power = FB_BLANK_POWERDOWN;
+			backlight_update_status(ch->bl);
+		}
+
 		board_cfg = &ch->cfg.board_cfg;
-		if (try_module_get(board_cfg->owner) && board_cfg->display_off) {
+		if (board_cfg->display_off && try_module_get(board_cfg->owner)) {
 			board_cfg->display_off(board_cfg->board_data);
 			module_put(board_cfg->owner);
 		}
+
+		/* disable the meram */
+		if (ch->meram_enabled) {
+			struct sh_mobile_meram_cfg *cfg;
+			struct sh_mobile_meram_info *mdev;
+			cfg = ch->cfg.meram_cfg;
+			mdev = priv->meram_dev;
+			mdev->ops->meram_unregister(mdev, cfg);
+			ch->meram_enabled = 0;
+		}
+
 	}
 
 	/* stop the lcdc */
@@ -804,9 +911,15 @@ static int sh_mobile_fb_pan_display(struct fb_var_screeninfo *var,
 	struct sh_mobile_lcdc_priv *priv = ch->lcdc;
 	unsigned long ldrcntr;
 	unsigned long new_pan_offset;
+	unsigned long base_addr_y, base_addr_c;
+	unsigned long c_offset;
 
-	new_pan_offset = (var->yoffset * info->fix.line_length) +
-		(var->xoffset * (info->var.bits_per_pixel / 8));
+	if (!var->nonstd)
+		new_pan_offset = (var->yoffset * info->fix.line_length) +
+			(var->xoffset * (info->var.bits_per_pixel / 8));
+	else
+		new_pan_offset = (var->yoffset * info->fix.line_length) +
+			(var->xoffset);
 
 	if (new_pan_offset == ch->pan_offset)
 		return 0;	/* No change, do nothing */
@@ -814,7 +927,46 @@ static int sh_mobile_fb_pan_display(struct fb_var_screeninfo *var,
 	ldrcntr = lcdc_read(priv, _LDRCNTR);
 
 	/* Set the source address for the next refresh */
-	lcdc_write_chan_mirror(ch, LDSA1R, ch->dma_handle + new_pan_offset);
+	base_addr_y = ch->dma_handle + new_pan_offset;
+	if (var->nonstd) {
+		/* Set y offset */
+		c_offset = (var->yoffset *
+			info->fix.line_length *
+			(info->var.bits_per_pixel - 8)) / 8;
+		base_addr_c = ch->dma_handle + var->xres * var->yres_virtual +
+			c_offset;
+		/* Set x offset */
+		if (info->var.bits_per_pixel == 24)
+			base_addr_c += 2 * var->xoffset;
+		else
+			base_addr_c += var->xoffset;
+	} else
+		base_addr_c = 0;
+
+	if (!ch->meram_enabled) {
+		lcdc_write_chan_mirror(ch, LDSA1R, base_addr_y);
+		if (base_addr_c)
+			lcdc_write_chan_mirror(ch, LDSA2R, base_addr_c);
+	} else {
+		struct sh_mobile_meram_cfg *cfg;
+		struct sh_mobile_meram_info *mdev;
+		unsigned long icb_addr_y, icb_addr_c;
+		int ret;
+
+		cfg = ch->cfg.meram_cfg;
+		mdev = priv->meram_dev;
+		ret = mdev->ops->meram_update(mdev, cfg,
+					base_addr_y, base_addr_c,
+					&icb_addr_y, &icb_addr_c);
+		if (ret)
+			return ret;
+
+		lcdc_write_chan_mirror(ch, LDSA1R, icb_addr_y);
+		if (icb_addr_c)
+			lcdc_write_chan_mirror(ch, LDSA2R, icb_addr_c);
+
+	}
+
 	if (lcdc_chan_is_sublcd(ch))
 		lcdc_write(ch->lcdc, _LDRCNTR, ldrcntr ^ LDRCNTR_SRS);
 	else
@@ -885,7 +1037,10 @@ static void sh_mobile_fb_reconfig(struct fb_info *info)
 		/* Couldn't reconfigure, hopefully, can continue as before */
 		return;
 
-	info->fix.line_length = mode1.xres * (ch->cfg.bpp / 8);
+	if (info->var.nonstd)
+		info->fix.line_length = mode1.xres;
+	else
+		info->fix.line_length = mode1.xres * (ch->cfg.bpp / 8);
 
 	/*
 	 * fb_set_var() calls the notifier change internally, only if
@@ -965,6 +1120,49 @@ static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *in
 	return 0;
 }
 
+/*
+ * Screen blanking. Behavior is as follows:
+ * FB_BLANK_UNBLANK: screen unblanked, clocks enabled
+ * FB_BLANK_NORMAL: screen blanked, clocks enabled
+ * FB_BLANK_VSYNC,
+ * FB_BLANK_HSYNC,
+ * FB_BLANK_POWEROFF: screen blanked, clocks disabled
+ */
+static int sh_mobile_lcdc_blank(int blank, struct fb_info *info)
+{
+	struct sh_mobile_lcdc_chan *ch = info->par;
+	struct sh_mobile_lcdc_priv *p = ch->lcdc;
+
+	/* blank the screen? */
+	if (blank > FB_BLANK_UNBLANK && ch->blank_status == FB_BLANK_UNBLANK) {
+		struct fb_fillrect rect = {
+			.width = info->var.xres,
+			.height = info->var.yres,
+		};
+		sh_mobile_lcdc_fillrect(info, &rect);
+	}
+	/* turn clocks on? */
+	if (blank <= FB_BLANK_NORMAL && ch->blank_status > FB_BLANK_NORMAL) {
+		sh_mobile_lcdc_clk_on(p);
+	}
+	/* turn clocks off? */
+	if (blank > FB_BLANK_NORMAL && ch->blank_status <= FB_BLANK_NORMAL) {
+		/* make sure the screen is updated with the black fill before
+		 * switching the clocks off. one vsync is not enough since
+		 * blanking may occur in the middle of a refresh. deferred io
+		 * mode will reenable the clocks and update the screen in time,
+		 * so it does not need this. */
+		if (!info->fbdefio) {
+			sh_mobile_wait_for_vsync(info);
+			sh_mobile_wait_for_vsync(info);
+		}
+		sh_mobile_lcdc_clk_off(p);
+	}
+
+	ch->blank_status = blank;
+	return 0;
+}
+
 static struct fb_ops sh_mobile_lcdc_ops = {
 	.owner          = THIS_MODULE,
 	.fb_setcolreg	= sh_mobile_lcdc_setcolreg,
@@ -973,6 +1171,7 @@ static struct fb_ops sh_mobile_lcdc_ops = {
 	.fb_fillrect	= sh_mobile_lcdc_fillrect,
 	.fb_copyarea	= sh_mobile_lcdc_copyarea,
 	.fb_imageblit	= sh_mobile_lcdc_imageblit,
+	.fb_blank	= sh_mobile_lcdc_blank,
 	.fb_pan_display = sh_mobile_fb_pan_display,
 	.fb_ioctl       = sh_mobile_ioctl,
 	.fb_open	= sh_mobile_open,
@@ -980,8 +1179,81 @@ static struct fb_ops sh_mobile_lcdc_ops = {
 	.fb_check_var	= sh_mobile_check_var,
 };
 
-static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp)
+static int sh_mobile_lcdc_update_bl(struct backlight_device *bdev)
 {
+	struct sh_mobile_lcdc_chan *ch = bl_get_data(bdev);
+	struct sh_mobile_lcdc_board_cfg *cfg = &ch->cfg.board_cfg;
+	int brightness = bdev->props.brightness;
+
+	if (bdev->props.power != FB_BLANK_UNBLANK ||
+	    bdev->props.state & (BL_CORE_SUSPENDED | BL_CORE_FBBLANK))
+		brightness = 0;
+
+	return cfg->set_brightness(cfg->board_data, brightness);
+}
+
+static int sh_mobile_lcdc_get_brightness(struct backlight_device *bdev)
+{
+	struct sh_mobile_lcdc_chan *ch = bl_get_data(bdev);
+	struct sh_mobile_lcdc_board_cfg *cfg = &ch->cfg.board_cfg;
+
+	return cfg->get_brightness(cfg->board_data);
+}
+
+static int sh_mobile_lcdc_check_fb(struct backlight_device *bdev,
+				   struct fb_info *info)
+{
+	return (info->bl_dev == bdev);
+}
+
+static struct backlight_ops sh_mobile_lcdc_bl_ops = {
+	.options	= BL_CORE_SUSPENDRESUME,
+	.update_status	= sh_mobile_lcdc_update_bl,
+	.get_brightness	= sh_mobile_lcdc_get_brightness,
+	.check_fb	= sh_mobile_lcdc_check_fb,
+};
+
+static struct backlight_device *sh_mobile_lcdc_bl_probe(struct device *parent,
+					       struct sh_mobile_lcdc_chan *ch)
+{
+	struct backlight_device *bl;
+
+	bl = backlight_device_register(ch->cfg.bl_info.name, parent, ch,
+				       &sh_mobile_lcdc_bl_ops, NULL);
+	if (IS_ERR(bl)) {
+		dev_err(parent, "unable to register backlight device: %ld\n",
+			PTR_ERR(bl));
+		return NULL;
+	}
+
+	bl->props.max_brightness = ch->cfg.bl_info.max_brightness;
+	bl->props.brightness = bl->props.max_brightness;
+	backlight_update_status(bl);
+
+	return bl;
+}
+
+static void sh_mobile_lcdc_bl_remove(struct backlight_device *bdev)
+{
+	backlight_device_unregister(bdev);
+}
+
+static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp,
+				   int nonstd)
+{
+	if (nonstd) {
+		switch (bpp) {
+		case 12:
+		case 16:
+		case 24:
+			var->bits_per_pixel = bpp;
+			var->nonstd = nonstd;
+			return 0;
+		default:
+			return -EINVAL;
+		}
+	}
+
 	switch (bpp) {
 	case 16: /* PKF[4:0] = 00011 - RGB 565 */
 		var->red.offset = 11;
@@ -1104,7 +1376,6 @@ static int sh_mobile_lcdc_notify(struct notifier_block *nb,
 	struct fb_info *info = event->info;
 	struct sh_mobile_lcdc_chan *ch = info->par;
 	struct sh_mobile_lcdc_board_cfg	*board_cfg = &ch->cfg.board_cfg;
-	int ret;
 
 	if (&ch->lcdc->notifier != nb)
 		return NOTIFY_DONE;
@@ -1114,11 +1385,10 @@ static int sh_mobile_lcdc_notify(struct notifier_block *nb,
 
 	switch(action) {
 	case FB_EVENT_SUSPEND:
-		if (try_module_get(board_cfg->owner) && board_cfg->display_off) {
+		if (board_cfg->display_off && try_module_get(board_cfg->owner)) {
 			board_cfg->display_off(board_cfg->board_data);
 			module_put(board_cfg->owner);
 		}
-		pm_runtime_put(info->device);
 		sh_mobile_lcdc_stop(ch->lcdc);
 		break;
 	case FB_EVENT_RESUME:
@@ -1127,14 +1397,12 @@ static int sh_mobile_lcdc_notify(struct notifier_block *nb,
 		mutex_unlock(&ch->open_lock);
 
 		/* HDMI must be enabled before LCDC configuration */
-		if (try_module_get(board_cfg->owner) && board_cfg->display_on) {
+		if (board_cfg->display_on && try_module_get(board_cfg->owner)) {
 			board_cfg->display_on(board_cfg->board_data, info);
 			module_put(board_cfg->owner);
 		}
 
-		ret = sh_mobile_lcdc_start(ch->lcdc);
-		if (!ret)
-			pm_runtime_get_sync(info->device);
+		sh_mobile_lcdc_start(ch->lcdc);
 	}
 
 	return NOTIFY_OK;
@@ -1198,6 +1466,10 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 		init_completion(&ch->vsync_completion);
 		ch->pan_offset = 0;
 
+		/* probe the backlight is there is one defined */
+		if (ch->cfg.bl_info.max_brightness)
+			ch->bl = sh_mobile_lcdc_bl_probe(&pdev->dev, ch);
+
 		switch (pdata->ch[i].chan) {
 		case LCDC_CHAN_MAINLCD:
 			ch->enabled = 1 << 1;
@@ -1232,6 +1504,8 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
+	priv->meram_dev = pdata->meram_dev;
+
 	for (i = 0; i < j; i++) {
 		struct fb_var_screeninfo *var;
 		const struct fb_videomode *lcd_cfg, *max_cfg = NULL;
@@ -1260,6 +1534,14 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 		     k < cfg->num_cfg && lcd_cfg;
 		     k++, lcd_cfg++) {
 			unsigned long size = lcd_cfg->yres * lcd_cfg->xres;
+			/* NV12 buffers must have even number of lines */
+			if ((cfg->nonstd) && cfg->bpp == 12 &&
+					(lcd_cfg->yres & 0x1)) {
+				dev_err(&pdev->dev, "yres must be multiple of 2"
+						" for YCbCr420 mode.\n");
+				error = -EINVAL;
+				goto err1;
+			}
 
 			if (size > max_size) {
 				max_cfg = lcd_cfg;
@@ -1274,7 +1556,11 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 				max_cfg->xres, max_cfg->yres);
 
 		info->fix = sh_mobile_lcdc_fix;
-		info->fix.smem_len = max_size * (cfg->bpp / 8) * 2;
+		info->fix.smem_len = max_size * 2 * cfg->bpp / 8;
+
+		 /* Only pan in 2 line steps for NV12 */
+		if (cfg->nonstd && cfg->bpp == 12)
+			info->fix.ypanstep = 2;
 
 		if (!mode) {
 			mode = &default_720p;
@@ -1292,7 +1578,7 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 		var->yres_virtual = var->yres * 2;
 		var->activate = FB_ACTIVATE_NOW;
 
-		error = sh_mobile_lcdc_set_bpp(var, cfg->bpp);
+		error = sh_mobile_lcdc_set_bpp(var, cfg->bpp, cfg->nonstd);
 		if (error)
 			break;
 
@@ -1316,7 +1602,11 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 		}
 
 		info->fix.smem_start = ch->dma_handle;
-		info->fix.line_length = var->xres * (cfg->bpp / 8);
+		if (var->nonstd)
+			info->fix.line_length = var->xres;
+		else
+			info->fix.line_length = var->xres * (cfg->bpp / 8);
+
 		info->screen_base = buf;
 		info->device = &pdev->dev;
 		ch->display_var = *var;
@@ -1345,6 +1635,8 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 			}
 		}
 
+		info->bl_dev = ch->bl;
+
 		error = register_framebuffer(info);
 		if (error < 0)
 			goto err1;
@@ -1404,6 +1696,11 @@ static int sh_mobile_lcdc_remove(struct platform_device *pdev)
 		framebuffer_release(info);
 	}
 
+	for (i = 0; i < ARRAY_SIZE(priv->ch); i++) {
+		if (priv->ch[i].bl)
+			sh_mobile_lcdc_bl_remove(priv->ch[i].bl);
+	}
+
 	if (priv->dot_clk)
 		clk_put(priv->dot_clk);
 
diff --git a/drivers/video/sh_mobile_lcdcfb.h b/drivers/video/sh_mobile_lcdcfb.h
index 9ecee2fba1d7..aeed6687e6a7 100644
--- a/drivers/video/sh_mobile_lcdcfb.h
+++ b/drivers/video/sh_mobile_lcdcfb.h
@@ -8,7 +8,7 @@
 
 /* per-channel registers */
 enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
-       LDSM2R, LDSA1R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR,
+       LDSM2R, LDSA1R, LDSA2R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR,
        LDHAJR,
        NR_CH_REGS };
 
@@ -16,6 +16,7 @@ enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
 
 struct sh_mobile_lcdc_priv;
 struct fb_info;
+struct backlight_device;
 
 struct sh_mobile_lcdc_chan {
 	struct sh_mobile_lcdc_priv *lcdc;
@@ -26,6 +27,7 @@ struct sh_mobile_lcdc_chan {
 	u32 pseudo_palette[PALETTE_NR];
 	unsigned long saved_ch_regs[NR_CH_REGS];
 	struct fb_info *info;
+	struct backlight_device *bl;
 	dma_addr_t dma_handle;
 	struct fb_deferred_io defio;
 	struct scatterlist *sglist;
@@ -35,7 +37,9 @@ struct sh_mobile_lcdc_chan {
 	struct completion vsync_completion;
 	struct fb_var_screeninfo display_var;
 	int use_count;
+	int blank_status;
 	struct mutex open_lock;		/* protects the use counter */
+	int meram_enabled;
 };
 
 #endif
diff --git a/drivers/video/sh_mobile_meram.c b/drivers/video/sh_mobile_meram.c
new file mode 100644
index 000000000000..cc7d7329dc15
--- /dev/null
+++ b/drivers/video/sh_mobile_meram.c
@@ -0,0 +1,567 @@
+/*
+ * SuperH Mobile MERAM Driver for SuperH Mobile LCDC Driver
+ *
+ * Copyright (c) 2011	Damian Hobson-Garcia <dhobsong@igel.co.jp>
+ *                      Takanari Hayama <taki@igel.co.jp>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include "sh_mobile_meram.h"
+
+/* meram registers */
+#define MExxCTL 0x0
+#define MExxBSIZE 0x4
+#define MExxMNCF 0x8
+#define MExxSARA 0x10
+#define MExxSARB 0x14
+#define MExxSBSIZE 0x18
+
+#define MERAM_MExxCTL_VAL(ctl, next_icb, addr)	\
+	((ctl) | (((next_icb) & 0x1f) << 11) | (((addr) & 0x7ff) << 16))
+#define	MERAM_MExxBSIZE_VAL(a, b, c) \
+	(((a) << 28) | ((b) << 16) | (c))
+
+#define MEVCR1 0x4
+#define MEACTS 0x10
+#define MEQSEL1 0x40
+#define MEQSEL2 0x44
+
+/* settings */
+#define MERAM_SEC_LINE 15
+#define MERAM_LINE_WIDTH 2048
+
+/*
+ * MERAM/ICB access functions
+ */
+
+#define MERAM_ICB_OFFSET(base, idx, off)	\
+	((base) + (0x400 + ((idx) * 0x20) + (off)))
+
+static inline void meram_write_icb(void __iomem *base, int idx, int off,
+	unsigned long val)
+{
+	iowrite32(val, MERAM_ICB_OFFSET(base, idx, off));
+}
+
+static inline unsigned long meram_read_icb(void __iomem *base, int idx, int off)
+{
+	return ioread32(MERAM_ICB_OFFSET(base, idx, off));
+}
+
+static inline void meram_write_reg(void __iomem *base, int off,
+		unsigned long val)
+{
+	iowrite32(val, base + off);
+}
+
+static inline unsigned long meram_read_reg(void __iomem *base, int off)
+{
+	return ioread32(base + off);
+}
+
+/*
+ * register ICB
+ */
+
+#define MERAM_CACHE_START(p)	 ((p) >> 16)
+#define MERAM_CACHE_END(p)	 ((p) & 0xffff)
+#define MERAM_CACHE_SET(o, s)	 ((((o) & 0xffff) << 16) | \
+				  (((o) + (s) - 1) & 0xffff))
+
+/*
+ * check if there's no overlaps in MERAM allocation.
+ */
+
+static inline int meram_check_overlap(struct sh_mobile_meram_priv *priv,
+				      struct sh_mobile_meram_icb *new)
+{
+	int i;
+	int used_start, used_end, meram_start, meram_end;
+
+	/* valid ICB? */
+	if (new->marker_icb & ~0x1f || new->cache_icb & ~0x1f)
+		return 1;
+
+	if (test_bit(new->marker_icb, &priv->used_icb) ||
+			test_bit(new->cache_icb,  &priv->used_icb))
+		return  1;
+
+	for (i = 0; i < priv->used_meram_cache_regions; i++) {
+		used_start = MERAM_CACHE_START(priv->used_meram_cache[i]);
+		used_end   = MERAM_CACHE_END(priv->used_meram_cache[i]);
+		meram_start = new->meram_offset;
+		meram_end   = new->meram_offset + new->meram_size;
+
+		if ((meram_start >= used_start && meram_start < used_end) ||
+			(meram_end > used_start && meram_end < used_end))
+			return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * mark the specified ICB as used
+ */
+
+static inline void meram_mark(struct sh_mobile_meram_priv *priv,
+			      struct sh_mobile_meram_icb *new)
+{
+	int n;
+
+	if (new->marker_icb < 0 || new->cache_icb < 0)
+		return;
+
+	__set_bit(new->marker_icb, &priv->used_icb);
+	__set_bit(new->cache_icb, &priv->used_icb);
+
+	n = priv->used_meram_cache_regions;
+
+	priv->used_meram_cache[n] = MERAM_CACHE_SET(new->meram_offset,
+						    new->meram_size);
+
+	priv->used_meram_cache_regions++;
+}
+
+/*
+ * unmark the specified ICB as used
+ */
+
+static inline void meram_unmark(struct sh_mobile_meram_priv *priv,
+				struct sh_mobile_meram_icb *icb)
+{
+	int i;
+	unsigned long pattern;
+
+	if (icb->marker_icb < 0 || icb->cache_icb < 0)
+		return;
+
+	__clear_bit(icb->marker_icb, &priv->used_icb);
+	__clear_bit(icb->cache_icb, &priv->used_icb);
+
+	pattern = MERAM_CACHE_SET(icb->meram_offset, icb->meram_size);
+	for (i = 0; i < priv->used_meram_cache_regions; i++) {
+		if (priv->used_meram_cache[i] == pattern) {
+			while (i < priv->used_meram_cache_regions - 1) {
+				priv->used_meram_cache[i] =
+					priv->used_meram_cache[i + 1] ;
+				i++;
+			}
+			priv->used_meram_cache[i] = 0;
+			priv->used_meram_cache_regions--;
+			break;
+		}
+	}
+}
+
+/*
+ * is this a YCbCr(NV12, NV16 or NV24) colorspace
+ */
+static inline int is_nvcolor(int cspace)
+{
+	if (cspace == SH_MOBILE_MERAM_PF_NV ||
+			cspace == SH_MOBILE_MERAM_PF_NV24)
+		return 1;
+	return 0;
+}
+
+/*
+ * set the next address to fetch
+ */
+static inline void meram_set_next_addr(struct sh_mobile_meram_priv *priv,
+				       struct sh_mobile_meram_cfg *cfg,
+				       unsigned long base_addr_y,
+				       unsigned long base_addr_c)
+{
+	unsigned long target;
+
+	target = (cfg->current_reg) ? MExxSARA : MExxSARB;
+	cfg->current_reg ^= 1;
+
+	/* set the next address to fetch */
+	meram_write_icb(priv->base, cfg->icb[0].cache_icb,  target,
+			base_addr_y);
+	meram_write_icb(priv->base, cfg->icb[0].marker_icb, target,
+			base_addr_y + cfg->icb[0].cache_unit);
+
+	if (is_nvcolor(cfg->pixelformat)) {
+		meram_write_icb(priv->base, cfg->icb[1].cache_icb,  target,
+				base_addr_c);
+		meram_write_icb(priv->base, cfg->icb[1].marker_icb, target,
+				base_addr_c + cfg->icb[1].cache_unit);
+	}
+}
+
+/*
+ * get the next ICB address
+ */
+static inline void meram_get_next_icb_addr(struct sh_mobile_meram_info *pdata,
+					   struct sh_mobile_meram_cfg *cfg,
+					   unsigned long *icb_addr_y,
+					   unsigned long *icb_addr_c)
+{
+	unsigned long icb_offset;
+
+	if (pdata->addr_mode == SH_MOBILE_MERAM_MODE0)
+		icb_offset = 0x80000000 | (cfg->current_reg << 29);
+	else
+		icb_offset = 0xc0000000 | (cfg->current_reg << 23);
+
+	*icb_addr_y = icb_offset | (cfg->icb[0].marker_icb << 24);
+	if (is_nvcolor(cfg->pixelformat))
+		*icb_addr_c = icb_offset | (cfg->icb[1].marker_icb << 24);
+}
+
+#define MERAM_CALC_BYTECOUNT(x, y) \
+	(((x) * (y) + (MERAM_LINE_WIDTH - 1)) & ~(MERAM_LINE_WIDTH - 1))
+
+/*
+ * initialize MERAM
+ */
+
+static int meram_init(struct sh_mobile_meram_priv *priv,
+		      struct sh_mobile_meram_icb *icb,
+		      int xres, int yres, int *out_pitch)
+{
+	unsigned long total_byte_count = MERAM_CALC_BYTECOUNT(xres, yres);
+	unsigned long bnm;
+	int lcdc_pitch, xpitch, line_cnt;
+	int save_lines;
+
+	/* adjust pitch to 1024, 2048, 4096 or 8192 */
+	lcdc_pitch = (xres - 1) | 1023;
+	lcdc_pitch = lcdc_pitch | (lcdc_pitch >> 1);
+	lcdc_pitch = lcdc_pitch | (lcdc_pitch >> 2);
+	lcdc_pitch += 1;
+
+	/* derive settings */
+	if (lcdc_pitch == 8192 && yres >= 1024) {
+		lcdc_pitch = xpitch = MERAM_LINE_WIDTH;
+		line_cnt = total_byte_count >> 11;
+		*out_pitch = xres;
+		save_lines = (icb->meram_size / 16 / MERAM_SEC_LINE);
+		save_lines *= MERAM_SEC_LINE;
+	} else {
+		xpitch = xres;
+		line_cnt = yres;
+		*out_pitch = lcdc_pitch;
+		save_lines = icb->meram_size / (lcdc_pitch >> 10) / 2;
+		save_lines &= 0xff;
+	}
+	bnm = (save_lines - 1) << 16;
+
+	/* TODO: we better to check if we have enough MERAM buffer size */
+
+	/* set up ICB */
+	meram_write_icb(priv->base, icb->cache_icb,  MExxBSIZE,
+			MERAM_MExxBSIZE_VAL(0x0, line_cnt - 1, xpitch - 1));
+	meram_write_icb(priv->base, icb->marker_icb, MExxBSIZE,
+			MERAM_MExxBSIZE_VAL(0xf, line_cnt - 1, xpitch - 1));
+
+	meram_write_icb(priv->base, icb->cache_icb,  MExxMNCF, bnm);
+	meram_write_icb(priv->base, icb->marker_icb, MExxMNCF, bnm);
+
+	meram_write_icb(priv->base, icb->cache_icb,  MExxSBSIZE, xpitch);
+	meram_write_icb(priv->base, icb->marker_icb, MExxSBSIZE, xpitch);
+
+	/* save a cache unit size */
+	icb->cache_unit = xres * save_lines;
+
+	/*
+	 * Set MERAM for framebuffer
+	 *
+	 * 0x70f:  WD = 0x3, WS=0x1, CM=0x1, MD=FB mode
+	 * we also chain the cache_icb and the marker_icb.
+	 * we also split the allocated MERAM buffer between two ICBs.
+	 */
+	meram_write_icb(priv->base, icb->cache_icb, MExxCTL,
+			MERAM_MExxCTL_VAL(0x70f, icb->marker_icb,
+					  icb->meram_offset));
+	meram_write_icb(priv->base, icb->marker_icb, MExxCTL,
+			MERAM_MExxCTL_VAL(0x70f, icb->cache_icb,
+					  icb->meram_offset +
+					  icb->meram_size / 2));
+
+	return 0;
+}
+
+static void meram_deinit(struct sh_mobile_meram_priv *priv,
+			struct sh_mobile_meram_icb *icb)
+{
+	/* disable ICB */
+	meram_write_icb(priv->base, icb->cache_icb,  MExxCTL, 0);
+	meram_write_icb(priv->base, icb->marker_icb, MExxCTL, 0);
+	icb->cache_unit = 0;
+}
+
+/*
+ * register the ICB
+ */
+
+static int sh_mobile_meram_register(struct sh_mobile_meram_info *pdata,
+				    struct sh_mobile_meram_cfg *cfg,
+				    int xres, int yres, int pixelformat,
+				    unsigned long base_addr_y,
+				    unsigned long base_addr_c,
+				    unsigned long *icb_addr_y,
+				    unsigned long *icb_addr_c,
+				    int *pitch)
+{
+	struct platform_device *pdev;
+	struct sh_mobile_meram_priv *priv;
+	int n, out_pitch;
+	int error = 0;
+
+	if (!pdata || !pdata->priv || !pdata->pdev || !cfg)
+		return -EINVAL;
+
+	if (pixelformat != SH_MOBILE_MERAM_PF_NV &&
+	    pixelformat != SH_MOBILE_MERAM_PF_NV24 &&
+	    pixelformat != SH_MOBILE_MERAM_PF_RGB)
+		return -EINVAL;
+
+	priv = pdata->priv;
+	pdev = pdata->pdev;
+
+	dev_dbg(&pdev->dev, "registering %dx%d (%s) (y=%08lx, c=%08lx)",
+		xres, yres, (!pixelformat) ? "yuv" : "rgb",
+		base_addr_y, base_addr_c);
+
+	mutex_lock(&priv->lock);
+
+	/* we can't handle wider than 8192px */
+	if (xres > 8192) {
+		dev_err(&pdev->dev, "width exceeding the limit (> 8192).");
+		error = -EINVAL;
+		goto err;
+	}
+
+	if (priv->used_meram_cache_regions + 2 > SH_MOBILE_MERAM_ICB_NUM) {
+		dev_err(&pdev->dev, "no more ICB available.");
+		error = -EINVAL;
+		goto err;
+	}
+
+	/* do we have at least one ICB config? */
+	if (cfg->icb[0].marker_icb < 0 || cfg->icb[0].cache_icb < 0) {
+		dev_err(&pdev->dev, "at least one ICB is required.");
+		error = -EINVAL;
+		goto err;
+	}
+
+	/* make sure that there's no overlaps */
+	if (meram_check_overlap(priv, &cfg->icb[0])) {
+		dev_err(&pdev->dev, "conflicting config detected.");
+		error = -EINVAL;
+		goto err;
+	}
+	n = 1;
+
+	/* do the same if we have the second ICB set */
+	if (cfg->icb[1].marker_icb >= 0 && cfg->icb[1].cache_icb >= 0) {
+		if (meram_check_overlap(priv, &cfg->icb[1])) {
+			dev_err(&pdev->dev, "conflicting config detected.");
+			error = -EINVAL;
+			goto err;
+		}
+		n = 2;
+	}
+
+	if (is_nvcolor(pixelformat) && n != 2) {
+		dev_err(&pdev->dev, "requires two ICB sets for planar Y/C.");
+		error =  -EINVAL;
+		goto err;
+	}
+
+	/* we now register the ICB */
+	cfg->pixelformat = pixelformat;
+	meram_mark(priv, &cfg->icb[0]);
+	if (is_nvcolor(pixelformat))
+		meram_mark(priv, &cfg->icb[1]);
+
+	/* initialize MERAM */
+	meram_init(priv, &cfg->icb[0], xres, yres, &out_pitch);
+	*pitch = out_pitch;
+	if (pixelformat == SH_MOBILE_MERAM_PF_NV)
+		meram_init(priv, &cfg->icb[1], xres, (yres + 1) / 2,
+			&out_pitch);
+	else if (pixelformat == SH_MOBILE_MERAM_PF_NV24)
+		meram_init(priv, &cfg->icb[1], 2 * xres, (yres + 1) / 2,
+			&out_pitch);
+
+	cfg->current_reg = 1;
+	meram_set_next_addr(priv, cfg, base_addr_y, base_addr_c);
+	meram_get_next_icb_addr(pdata, cfg, icb_addr_y, icb_addr_c);
+
+	dev_dbg(&pdev->dev, "registered - can access via y=%08lx, c=%08lx",
+		*icb_addr_y, *icb_addr_c);
+
+err:
+	mutex_unlock(&priv->lock);
+	return error;
+}
+
+static int sh_mobile_meram_unregister(struct sh_mobile_meram_info *pdata,
+				      struct sh_mobile_meram_cfg *cfg)
+{
+	struct sh_mobile_meram_priv *priv;
+
+	if (!pdata || !pdata->priv || !cfg)
+		return -EINVAL;
+
+	priv = pdata->priv;
+
+	mutex_lock(&priv->lock);
+
+	/* deinit & unmark */
+	if (is_nvcolor(cfg->pixelformat)) {
+		meram_deinit(priv, &cfg->icb[1]);
+		meram_unmark(priv, &cfg->icb[1]);
+	}
+	meram_deinit(priv, &cfg->icb[0]);
+	meram_unmark(priv, &cfg->icb[0]);
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int sh_mobile_meram_update(struct sh_mobile_meram_info *pdata,
+				  struct sh_mobile_meram_cfg *cfg,
+				  unsigned long base_addr_y,
+				  unsigned long base_addr_c,
+				  unsigned long *icb_addr_y,
+				  unsigned long *icb_addr_c)
+{
+	struct sh_mobile_meram_priv *priv;
+
+	if (!pdata || !pdata->priv || !cfg)
+		return -EINVAL;
+
+	priv = pdata->priv;
+
+	mutex_lock(&priv->lock);
+
+	meram_set_next_addr(priv, cfg, base_addr_y, base_addr_c);
+	meram_get_next_icb_addr(pdata, cfg, icb_addr_y, icb_addr_c);
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static struct sh_mobile_meram_ops sh_mobile_meram_ops = {
+	.module			= THIS_MODULE,
+	.meram_register		= sh_mobile_meram_register,
+	.meram_unregister	= sh_mobile_meram_unregister,
+	.meram_update		= sh_mobile_meram_update,
+};
+
+/*
+ * initialize MERAM
+ */
+
+static int sh_mobile_meram_remove(struct platform_device *pdev);
+
+static int __devinit sh_mobile_meram_probe(struct platform_device *pdev)
+{
+	struct sh_mobile_meram_priv *priv;
+	struct sh_mobile_meram_info *pdata = pdev->dev.platform_data;
+	struct resource *res;
+	int error;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		return -EINVAL;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "cannot get platform resources\n");
+		return -ENOENT;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "cannot allocate device data\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	/* initialize private data */
+	mutex_init(&priv->lock);
+	priv->base = ioremap_nocache(res->start, resource_size(res));
+	if (!priv->base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		error = -EFAULT;
+		goto err;
+	}
+	pdata->ops = &sh_mobile_meram_ops;
+	pdata->priv = priv;
+	pdata->pdev = pdev;
+
+	/* initialize ICB addressing mode */
+	if (pdata->addr_mode == SH_MOBILE_MERAM_MODE1)
+		meram_write_reg(priv->base, MEVCR1, 1 << 29);
+
+	dev_info(&pdev->dev, "sh_mobile_meram initialized.");
+
+	return 0;
+
+err:
+	sh_mobile_meram_remove(pdev);
+
+	return error;
+}
+
+
+static int sh_mobile_meram_remove(struct platform_device *pdev)
+{
+	struct sh_mobile_meram_priv *priv = platform_get_drvdata(pdev);
+
+	if (priv->base)
+		iounmap(priv->base);
+
+	mutex_destroy(&priv->lock);
+
+	kfree(priv);
+
+	return 0;
+}
+
+static struct platform_driver sh_mobile_meram_driver = {
+	.driver	= {
+		.name		= "sh_mobile_meram",
+		.owner		= THIS_MODULE,
+	},
+	.probe		= sh_mobile_meram_probe,
+	.remove		= sh_mobile_meram_remove,
+};
+
+static int __init sh_mobile_meram_init(void)
+{
+	return platform_driver_register(&sh_mobile_meram_driver);
+}
+
+static void __exit sh_mobile_meram_exit(void)
+{
+	platform_driver_unregister(&sh_mobile_meram_driver);
+}
+
+module_init(sh_mobile_meram_init);
+module_exit(sh_mobile_meram_exit);
+
+MODULE_DESCRIPTION("SuperH Mobile MERAM driver");
+MODULE_AUTHOR("Damian Hobson-Garcia / Takanari Hayama");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sh_mobile_meram.h b/drivers/video/sh_mobile_meram.h
new file mode 100644
index 000000000000..82c54fbce8bd
--- /dev/null
+++ b/drivers/video/sh_mobile_meram.h
@@ -0,0 +1,41 @@
+#ifndef __sh_mobile_meram_h__
+#define __sh_mobile_meram_h__
+
+#include <linux/mutex.h>
+#include <video/sh_mobile_meram.h>
+
+/*
+ * MERAM private
+ */
+
+#define MERAM_ICB_Y 0x1
+#define MERAM_ICB_C 0x2
+
+/* MERAM cache size */
+#define SH_MOBILE_MERAM_ICB_NUM		32
+
+#define SH_MOBILE_MERAM_CACHE_OFFSET(p)	((p) >> 16)
+#define SH_MOBILE_MERAM_CACHE_SIZE(p)	((p) & 0xffff)
+
+struct sh_mobile_meram_priv {
+	void __iomem	*base;
+	struct mutex	lock;
+	unsigned long	used_icb;
+	int		used_meram_cache_regions;
+	unsigned long	used_meram_cache[SH_MOBILE_MERAM_ICB_NUM];
+};
+
+int sh_mobile_meram_alloc_icb(const struct sh_mobile_meram_cfg *cfg,
+		   int xres,
+		   int yres,
+		   unsigned int base_addr,
+		   int yuv_mode,
+		   int *marker_icb,
+		   int *out_pitch);
+
+void sh_mobile_meram_free_icb(int marker_icb);
+
+#define SH_MOBILE_MERAM_START(ind, ab) \
+	(0xC0000000 | ((ab & 0x1) << 23) | ((ind & 0x1F) << 24))
+
+#endif /* !__sh_mobile_meram_h__ */
diff --git a/drivers/video/sis/sis.h b/drivers/video/sis/sis.h
index eac7a01925f3..1987f1b7212f 100644
--- a/drivers/video/sis/sis.h
+++ b/drivers/video/sis/sis.h
@@ -495,6 +495,7 @@ struct sis_video_info {
 	unsigned int	refresh_rate;
 
 	unsigned int	chip;
+	unsigned int	chip_real_id;
 	u8		revision_id;
 	int		sisvga_enabled;		/* PCI device was enabled */
 
diff --git a/drivers/video/sis/sis_main.c b/drivers/video/sis/sis_main.c
index 2fb8c5a660fb..75259845933d 100644
--- a/drivers/video/sis/sis_main.c
+++ b/drivers/video/sis/sis_main.c
@@ -4563,6 +4563,11 @@ sisfb_post_sis315330(struct pci_dev *pdev)
 }
 #endif
 
+static inline int sisfb_xgi_is21(struct sis_video_info *ivideo)
+{
+	return ivideo->chip_real_id == XGI_21;
+}
+
 static void __devinit
 sisfb_post_xgi_delay(struct sis_video_info *ivideo, int delay)
 {
@@ -4627,11 +4632,11 @@ sisfb_post_xgi_rwtest(struct sis_video_info *ivideo, int starta,
 	return 1;
 }
 
-static void __devinit
+static int __devinit
 sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
 {
 	unsigned int buswidth, ranksize, channelab, mapsize;
-	int i, j, k, l;
+	int i, j, k, l, status;
 	u8 reg, sr14;
 	static const u8 dramsr13[12 * 5] = {
 		0x02, 0x0e, 0x0b, 0x80, 0x5d,
@@ -4673,7 +4678,7 @@ sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
 		SiS_SetReg(SISSR, 0x13, 0x35);
 		SiS_SetReg(SISSR, 0x14, 0x41);
 		/* TODO */
-		return;
+		return -ENOMEM;
 	}
 
 	/* Non-interleaving */
@@ -4835,6 +4840,7 @@ bail_out:
 
 	j = (ivideo->chip == XGI_20) ? 5 : 9;
 	k = (ivideo->chip == XGI_20) ? 12 : 4;
+	status = -EIO;
 
 	for(i = 0; i < k; i++) {
 
@@ -4868,11 +4874,15 @@ bail_out:
 		SiS_SetRegANDOR(SISSR, 0x14, 0x0f, (reg & 0xf0));
 		sisfb_post_xgi_delay(ivideo, 1);
 
-		if(sisfb_post_xgi_rwtest(ivideo, j, ((reg >> 4) + channelab - 2 + 20), mapsize))
+		if (sisfb_post_xgi_rwtest(ivideo, j, ((reg >> 4) + channelab - 2 + 20), mapsize)) {
+			status = 0;
 			break;
+		}
 	}
 
 	iounmap(ivideo->video_vbase);
+
+	return status;
 }
 
 static void __devinit
@@ -4931,6 +4941,175 @@ sisfb_post_xgi_setclocks(struct sis_video_info *ivideo, u8 regb)
 	sisfb_post_xgi_delay(ivideo, 0x43);
 }
 
+static void __devinit
+sisfb_post_xgi_ddr2_mrs_default(struct sis_video_info *ivideo, u8 regb)
+{
+	unsigned char *bios = ivideo->bios_abase;
+	u8 v1;
+
+	SiS_SetReg(SISSR, 0x28, 0x64);
+	SiS_SetReg(SISSR, 0x29, 0x63);
+	sisfb_post_xgi_delay(ivideo, 15);
+	SiS_SetReg(SISSR, 0x18, 0x00);
+	SiS_SetReg(SISSR, 0x19, 0x20);
+	SiS_SetReg(SISSR, 0x16, 0x00);
+	SiS_SetReg(SISSR, 0x16, 0x80);
+	SiS_SetReg(SISSR, 0x18, 0xc5);
+	SiS_SetReg(SISSR, 0x19, 0x23);
+	SiS_SetReg(SISSR, 0x16, 0x00);
+	SiS_SetReg(SISSR, 0x16, 0x80);
+	sisfb_post_xgi_delay(ivideo, 1);
+	SiS_SetReg(SISCR, 0x97, 0x11);
+	sisfb_post_xgi_setclocks(ivideo, regb);
+	sisfb_post_xgi_delay(ivideo, 0x46);
+	SiS_SetReg(SISSR, 0x18, 0xc5);
+	SiS_SetReg(SISSR, 0x19, 0x23);
+	SiS_SetReg(SISSR, 0x16, 0x00);
+	SiS_SetReg(SISSR, 0x16, 0x80);
+	sisfb_post_xgi_delay(ivideo, 1);
+	SiS_SetReg(SISSR, 0x1b, 0x04);
+	sisfb_post_xgi_delay(ivideo, 1);
+	SiS_SetReg(SISSR, 0x1b, 0x00);
+	sisfb_post_xgi_delay(ivideo, 1);
+	v1 = 0x31;
+	if (ivideo->haveXGIROM) {
+		v1 = bios[0xf0];
+	}
+	SiS_SetReg(SISSR, 0x18, v1);
+	SiS_SetReg(SISSR, 0x19, 0x06);
+	SiS_SetReg(SISSR, 0x16, 0x04);
+	SiS_SetReg(SISSR, 0x16, 0x84);
+	sisfb_post_xgi_delay(ivideo, 1);
+}
+
+static void __devinit
+sisfb_post_xgi_ddr2_mrs_xg21(struct sis_video_info *ivideo)
+{
+	sisfb_post_xgi_setclocks(ivideo, 1);
+
+	SiS_SetReg(SISCR, 0x97, 0x11);
+	sisfb_post_xgi_delay(ivideo, 0x46);
+
+	SiS_SetReg(SISSR, 0x18, 0x00);	/* EMRS2 */
+	SiS_SetReg(SISSR, 0x19, 0x80);
+	SiS_SetReg(SISSR, 0x16, 0x05);
+	SiS_SetReg(SISSR, 0x16, 0x85);
+
+	SiS_SetReg(SISSR, 0x18, 0x00);	/* EMRS3 */
+	SiS_SetReg(SISSR, 0x19, 0xc0);
+	SiS_SetReg(SISSR, 0x16, 0x05);
+	SiS_SetReg(SISSR, 0x16, 0x85);
+
+	SiS_SetReg(SISSR, 0x18, 0x00);	/* EMRS1 */
+	SiS_SetReg(SISSR, 0x19, 0x40);
+	SiS_SetReg(SISSR, 0x16, 0x05);
+	SiS_SetReg(SISSR, 0x16, 0x85);
+
+	SiS_SetReg(SISSR, 0x18, 0x42);	/* MRS1 */
+	SiS_SetReg(SISSR, 0x19, 0x02);
+	SiS_SetReg(SISSR, 0x16, 0x05);
+	SiS_SetReg(SISSR, 0x16, 0x85);
+	sisfb_post_xgi_delay(ivideo, 1);
+
+	SiS_SetReg(SISSR, 0x1b, 0x04);
+	sisfb_post_xgi_delay(ivideo, 1);
+
+	SiS_SetReg(SISSR, 0x1b, 0x00);
+	sisfb_post_xgi_delay(ivideo, 1);
+
+	SiS_SetReg(SISSR, 0x18, 0x42);	/* MRS1 */
+	SiS_SetReg(SISSR, 0x19, 0x00);
+	SiS_SetReg(SISSR, 0x16, 0x05);
+	SiS_SetReg(SISSR, 0x16, 0x85);
+	sisfb_post_xgi_delay(ivideo, 1);
+}
+
+static void __devinit
+sisfb_post_xgi_ddr2(struct sis_video_info *ivideo, u8 regb)
+{
+	unsigned char *bios = ivideo->bios_abase;
+	static const u8 cs158[8] = {
+		0x88, 0xaa, 0x48, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+	static const u8 cs160[8] = {
+		0x44, 0x77, 0x77, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+	static const u8 cs168[8] = {
+		0x48, 0x78, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+	u8 reg;
+	u8 v1;
+	u8 v2;
+	u8 v3;
+
+	SiS_SetReg(SISCR, 0xb0, 0x80); /* DDR2 dual frequency mode */
+	SiS_SetReg(SISCR, 0x82, 0x77);
+	SiS_SetReg(SISCR, 0x86, 0x00);
+	reg = SiS_GetReg(SISCR, 0x86);
+	SiS_SetReg(SISCR, 0x86, 0x88);
+	reg = SiS_GetReg(SISCR, 0x86);
+	v1 = cs168[regb]; v2 = cs160[regb]; v3 = cs158[regb];
+	if (ivideo->haveXGIROM) {
+		v1 = bios[regb + 0x168];
+		v2 = bios[regb + 0x160];
+		v3 = bios[regb + 0x158];
+	}
+	SiS_SetReg(SISCR, 0x86, v1);
+	SiS_SetReg(SISCR, 0x82, 0x77);
+	SiS_SetReg(SISCR, 0x85, 0x00);
+	reg = SiS_GetReg(SISCR, 0x85);
+	SiS_SetReg(SISCR, 0x85, 0x88);
+	reg = SiS_GetReg(SISCR, 0x85);
+	SiS_SetReg(SISCR, 0x85, v2);
+	SiS_SetReg(SISCR, 0x82, v3);
+	SiS_SetReg(SISCR, 0x98, 0x01);
+	SiS_SetReg(SISCR, 0x9a, 0x02);
+	if (sisfb_xgi_is21(ivideo))
+		sisfb_post_xgi_ddr2_mrs_xg21(ivideo);
+	else
+		sisfb_post_xgi_ddr2_mrs_default(ivideo, regb);
+}
+
+static u8 __devinit
+sisfb_post_xgi_ramtype(struct sis_video_info *ivideo)
+{
+	unsigned char *bios = ivideo->bios_abase;
+	u8 ramtype;
+	u8 reg;
+	u8 v1;
+
+	ramtype = 0x00; v1 = 0x10;
+	if (ivideo->haveXGIROM) {
+		ramtype = bios[0x62];
+		v1 = bios[0x1d2];
+	}
+	if (!(ramtype & 0x80)) {
+		if (sisfb_xgi_is21(ivideo)) {
+			SiS_SetRegAND(SISCR, 0xb4, 0xfd); /* GPIO control */
+			SiS_SetRegOR(SISCR, 0x4a, 0x80);  /* GPIOH EN */
+			reg = SiS_GetReg(SISCR, 0x48);
+			SiS_SetRegOR(SISCR, 0xb4, 0x02);
+			ramtype = reg & 0x01;		  /* GPIOH */
+		} else if (ivideo->chip == XGI_20) {
+			SiS_SetReg(SISCR, 0x97, v1);
+			reg = SiS_GetReg(SISCR, 0x97);
+			if (reg & 0x10) {
+				ramtype = (reg & 0x01) << 1;
+			}
+		} else {
+			reg = SiS_GetReg(SISSR, 0x39);
+			ramtype = reg & 0x02;
+			if (!(ramtype)) {
+				reg = SiS_GetReg(SISSR, 0x3a);
+				ramtype = (reg >> 1) & 0x01;
+			}
+		}
+	}
+	ramtype &= 0x07;
+
+	return ramtype;
+}
+
 static int __devinit
 sisfb_post_xgi(struct pci_dev *pdev)
 {
@@ -5213,9 +5392,23 @@ sisfb_post_xgi(struct pci_dev *pdev)
 		SiS_SetReg(SISCR, 0x77, v1);
 	}
 
-	/* RAM type */
-
-	regb = 0;	/* ! */
+	/* RAM type:
+	 *
+	 * 0 == DDR1, 1 == DDR2, 2..7 == reserved?
+	 *
+	 * The code seems to written so that regb should equal ramtype,
+	 * however, so far it has been hardcoded to 0. Enable other values only
+	 * on XGI Z9, as it passes the POST, and add a warning for others.
+	 */
+	ramtype = sisfb_post_xgi_ramtype(ivideo);
+	if (!sisfb_xgi_is21(ivideo) && ramtype) {
+		dev_warn(&pdev->dev,
+			 "RAM type something else than expected: %d\n",
+			 ramtype);
+		regb = 0;
+	} else {
+		regb = ramtype;
+	}
 
 	v1 = 0xff;
 	if(ivideo->haveXGIROM) {
@@ -5367,7 +5560,10 @@ sisfb_post_xgi(struct pci_dev *pdev)
 		}
 	}
 
-	SiS_SetReg(SISSR, 0x17, 0x00);
+	if (regb == 1)
+		SiS_SetReg(SISSR, 0x17, 0x80);		/* DDR2 */
+	else
+		SiS_SetReg(SISSR, 0x17, 0x00);		/* DDR1 */
 	SiS_SetReg(SISSR, 0x1a, 0x87);
 
 	if(ivideo->chip == XGI_20) {
@@ -5375,31 +5571,6 @@ sisfb_post_xgi(struct pci_dev *pdev)
 		SiS_SetReg(SISSR, 0x1c, 0x00);
 	}
 
-	ramtype = 0x00; v1 = 0x10;
-	if(ivideo->haveXGIROM) {
-		ramtype = bios[0x62];
-		v1 = bios[0x1d2];
-	}
-	if(!(ramtype & 0x80)) {
-		if(ivideo->chip == XGI_20) {
-			SiS_SetReg(SISCR, 0x97, v1);
-			reg = SiS_GetReg(SISCR, 0x97);
-			if(reg & 0x10) {
-				ramtype = (reg & 0x01) << 1;
-			}
-		} else {
-			reg = SiS_GetReg(SISSR, 0x39);
-			ramtype = reg & 0x02;
-			if(!(ramtype)) {
-				reg = SiS_GetReg(SISSR, 0x3a);
-				ramtype = (reg >> 1) & 0x01;
-			}
-		}
-	}
-	ramtype &= 0x07;
-
-	regb = 0;	/* ! */
-
 	switch(ramtype) {
 	case 0:
 		sisfb_post_xgi_setclocks(ivideo, regb);
@@ -5485,61 +5656,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
 		SiS_SetReg(SISSR, 0x1b, 0x00);
 		break;
 	case 1:
-		SiS_SetReg(SISCR, 0x82, 0x77);
-		SiS_SetReg(SISCR, 0x86, 0x00);
-		reg = SiS_GetReg(SISCR, 0x86);
-		SiS_SetReg(SISCR, 0x86, 0x88);
-		reg = SiS_GetReg(SISCR, 0x86);
-		v1 = cs168[regb]; v2 = cs160[regb]; v3 = cs158[regb];
-		if(ivideo->haveXGIROM) {
-			v1 = bios[regb + 0x168];
-			v2 = bios[regb + 0x160];
-			v3 = bios[regb + 0x158];
-		}
-		SiS_SetReg(SISCR, 0x86, v1);
-		SiS_SetReg(SISCR, 0x82, 0x77);
-		SiS_SetReg(SISCR, 0x85, 0x00);
-		reg = SiS_GetReg(SISCR, 0x85);
-		SiS_SetReg(SISCR, 0x85, 0x88);
-		reg = SiS_GetReg(SISCR, 0x85);
-		SiS_SetReg(SISCR, 0x85, v2);
-		SiS_SetReg(SISCR, 0x82, v3);
-		SiS_SetReg(SISCR, 0x98, 0x01);
-		SiS_SetReg(SISCR, 0x9a, 0x02);
-
-		SiS_SetReg(SISSR, 0x28, 0x64);
-		SiS_SetReg(SISSR, 0x29, 0x63);
-		sisfb_post_xgi_delay(ivideo, 15);
-		SiS_SetReg(SISSR, 0x18, 0x00);
-		SiS_SetReg(SISSR, 0x19, 0x20);
-		SiS_SetReg(SISSR, 0x16, 0x00);
-		SiS_SetReg(SISSR, 0x16, 0x80);
-		SiS_SetReg(SISSR, 0x18, 0xc5);
-		SiS_SetReg(SISSR, 0x19, 0x23);
-		SiS_SetReg(SISSR, 0x16, 0x00);
-		SiS_SetReg(SISSR, 0x16, 0x80);
-		sisfb_post_xgi_delay(ivideo, 1);
-		SiS_SetReg(SISCR, 0x97, 0x11);
-		sisfb_post_xgi_setclocks(ivideo, regb);
-		sisfb_post_xgi_delay(ivideo, 0x46);
-		SiS_SetReg(SISSR, 0x18, 0xc5);
-		SiS_SetReg(SISSR, 0x19, 0x23);
-		SiS_SetReg(SISSR, 0x16, 0x00);
-		SiS_SetReg(SISSR, 0x16, 0x80);
-		sisfb_post_xgi_delay(ivideo, 1);
-		SiS_SetReg(SISSR, 0x1b, 0x04);
-		sisfb_post_xgi_delay(ivideo, 1);
-		SiS_SetReg(SISSR, 0x1b, 0x00);
-		sisfb_post_xgi_delay(ivideo, 1);
-		v1 = 0x31;
-		if(ivideo->haveXGIROM) {
-			v1 = bios[0xf0];
-		}
-		SiS_SetReg(SISSR, 0x18, v1);
-		SiS_SetReg(SISSR, 0x19, 0x06);
-		SiS_SetReg(SISSR, 0x16, 0x04);
-		SiS_SetReg(SISSR, 0x16, 0x84);
-		sisfb_post_xgi_delay(ivideo, 1);
+		sisfb_post_xgi_ddr2(ivideo, regb);
 		break;
 	default:
 		sisfb_post_xgi_setclocks(ivideo, regb);
@@ -5648,6 +5765,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
 		SiS_SetReg(SISSR, 0x14, bios[regb + 0xe0 + 8]);
 
 	} else {
+		int err;
 
 		/* Set default mode, don't clear screen */
 		ivideo->SiS_Pr.SiS_UseOEM = false;
@@ -5661,10 +5779,16 @@ sisfb_post_xgi(struct pci_dev *pdev)
 
 		/* Disable read-cache */
 		SiS_SetRegAND(SISSR, 0x21, 0xdf);
-		sisfb_post_xgi_ramsize(ivideo);
+		err = sisfb_post_xgi_ramsize(ivideo);
 		/* Enable read-cache */
 		SiS_SetRegOR(SISSR, 0x21, 0x20);
 
+		if (err) {
+			dev_err(&pdev->dev,
+				"%s: RAM size detection failed: %d\n",
+				__func__, err);
+			return 0;
+		}
 	}
 
 #if 0
@@ -5777,6 +5901,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 #endif
 
 	ivideo->chip = chipinfo->chip;
+	ivideo->chip_real_id = chipinfo->chip;
 	ivideo->sisvga_engine = chipinfo->vgaengine;
 	ivideo->hwcursor_size = chipinfo->hwcursor_size;
 	ivideo->CRT2_write_enable = chipinfo->CRT2_write_enable;
@@ -6010,6 +6135,18 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		sisfb_detect_custom_timing(ivideo);
 	}
 
+#ifdef CONFIG_FB_SIS_315
+	if (ivideo->chip == XGI_20) {
+		/* Check if our Z7 chip is actually Z9 */
+		SiS_SetRegOR(SISCR, 0x4a, 0x40);	/* GPIOG EN */
+		reg = SiS_GetReg(SISCR, 0x48);
+		if (reg & 0x02) {			/* GPIOG */
+			ivideo->chip_real_id = XGI_21;
+			dev_info(&pdev->dev, "Z9 detected\n");
+		}
+	}
+#endif
+
 	/* POST card in case this has not been done by the BIOS */
 	if( (!ivideo->sisvga_enabled)
 #if !defined(__i386__) && !defined(__x86_64__)
diff --git a/drivers/video/sis/vgatypes.h b/drivers/video/sis/vgatypes.h
index 12c0dfaf2518..e3f9976cfef0 100644
--- a/drivers/video/sis/vgatypes.h
+++ b/drivers/video/sis/vgatypes.h
@@ -87,6 +87,7 @@ typedef enum _SIS_CHIP_TYPE {
     SIS_341,
     SIS_342,
     XGI_20  = 75,
+    XGI_21,
     XGI_40,
     MAX_SIS_CHIP
 } SIS_CHIP_TYPE;
diff --git a/drivers/video/sm501fb.c b/drivers/video/sm501fb.c
index bcb44a594ebc..6294dca95500 100644
--- a/drivers/video/sm501fb.c
+++ b/drivers/video/sm501fb.c
@@ -41,6 +41,26 @@
 #include <linux/sm501.h>
 #include <linux/sm501-regs.h>
 
+#include "edid.h"
+
+static char *fb_mode = "640x480-16@60";
+static unsigned long default_bpp = 16;
+
+static struct fb_videomode __devinitdata sm501_default_mode = {
+	.refresh	= 60,
+	.xres		= 640,
+	.yres		= 480,
+	.pixclock	= 20833,
+	.left_margin	= 142,
+	.right_margin	= 13,
+	.upper_margin	= 21,
+	.lower_margin	= 1,
+	.hsync_len	= 69,
+	.vsync_len	= 3,
+	.sync		= FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	.vmode		= FB_VMODE_NONINTERLACED
+};
+
 #define NR_PALETTE	256
 
 enum sm501_controller {
@@ -77,6 +97,7 @@ struct sm501fb_info {
 	void __iomem		*regs2d;	/* 2d remapped registers */
 	void __iomem		*fbmem;		/* remapped framebuffer */
 	size_t			 fbmem_len;	/* length of remapped region */
+	u8 *edid_data;
 };
 
 /* per-framebuffer private data */
@@ -117,7 +138,7 @@ static inline int v_total(struct fb_var_screeninfo *var)
 
 static inline void sm501fb_sync_regs(struct sm501fb_info *info)
 {
-	readl(info->regs);
+	smc501_readl(info->regs);
 }
 
 /* sm501_alloc_mem
@@ -244,7 +265,7 @@ static unsigned long sm501fb_ps_to_hz(unsigned long psvalue)
 	return (unsigned long)numerator;
 }
 
-/* sm501fb_hz_to_ps is identical to the oposite transform */
+/* sm501fb_hz_to_ps is identical to the opposite transform */
 
 #define sm501fb_hz_to_ps(x) sm501fb_ps_to_hz(x)
 
@@ -262,7 +283,7 @@ static void sm501fb_setup_gamma(struct sm501fb_info *fbi,
 
 	/* set gamma values */
 	for (offset = 0; offset < 256 * 4; offset += 4) {
-		writel(value, fbi->regs + palette + offset);
+		smc501_writel(value, fbi->regs + palette + offset);
 		value += 0x010101; 	/* Advance RGB by 1,1,1.*/
 	}
 }
@@ -476,7 +497,8 @@ static int sm501fb_set_par_common(struct fb_info *info,
 
 	/* set start of framebuffer to the screen */
 
-	writel(par->screen.sm_addr | SM501_ADDR_FLIP, fbi->regs + head_addr);
+	smc501_writel(par->screen.sm_addr | SM501_ADDR_FLIP,
+			fbi->regs + head_addr);
 
 	/* program CRT clock  */
 
@@ -519,7 +541,7 @@ static void sm501fb_set_par_geometry(struct fb_info *info,
 	reg = info->fix.line_length;
 	reg |= ((var->xres * var->bits_per_pixel)/8) << 16;
 
-	writel(reg, fbi->regs + (par->head == HEAD_CRT ?
+	smc501_writel(reg, fbi->regs + (par->head == HEAD_CRT ?
 		    SM501_DC_CRT_FB_OFFSET :  SM501_DC_PANEL_FB_OFFSET));
 
 	/* program horizontal total */
@@ -527,27 +549,27 @@ static void sm501fb_set_par_geometry(struct fb_info *info,
 	reg  = (h_total(var) - 1) << 16;
 	reg |= (var->xres - 1);
 
-	writel(reg, base + SM501_OFF_DC_H_TOT);
+	smc501_writel(reg, base + SM501_OFF_DC_H_TOT);
 
 	/* program horizontal sync */
 
 	reg  = var->hsync_len << 16;
 	reg |= var->xres + var->right_margin - 1;
 
-	writel(reg, base + SM501_OFF_DC_H_SYNC);
+	smc501_writel(reg, base + SM501_OFF_DC_H_SYNC);
 
 	/* program vertical total */
 
 	reg  = (v_total(var) - 1) << 16;
 	reg |= (var->yres - 1);
 
-	writel(reg, base + SM501_OFF_DC_V_TOT);
+	smc501_writel(reg, base + SM501_OFF_DC_V_TOT);
 
 	/* program vertical sync */
 	reg  = var->vsync_len << 16;
 	reg |= var->yres + var->lower_margin - 1;
 
-	writel(reg, base + SM501_OFF_DC_V_SYNC);
+	smc501_writel(reg, base + SM501_OFF_DC_V_SYNC);
 }
 
 /* sm501fb_pan_crt
@@ -566,15 +588,15 @@ static int sm501fb_pan_crt(struct fb_var_screeninfo *var,
 
 	xoffs = var->xoffset * bytes_pixel;
 
-	reg = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+	reg = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
 	reg &= ~SM501_DC_CRT_CONTROL_PIXEL_MASK;
 	reg |= ((xoffs & 15) / bytes_pixel) << 4;
-	writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
+	smc501_writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
 
 	reg = (par->screen.sm_addr + xoffs +
 	       var->yoffset * info->fix.line_length);
-	writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
+	smc501_writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
 
 	sm501fb_sync_regs(fbi);
 	return 0;
@@ -593,10 +615,10 @@ static int sm501fb_pan_pnl(struct fb_var_screeninfo *var,
 	unsigned long reg;
 
 	reg = var->xoffset | (var->xres_virtual << 16);
-	writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
+	smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
 
 	reg = var->yoffset | (var->yres_virtual << 16);
-	writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
+	smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
 
 	sm501fb_sync_regs(fbi);
 	return 0;
@@ -622,7 +644,7 @@ static int sm501fb_set_par_crt(struct fb_info *info)
 	/* enable CRT DAC - note 0 is on!*/
 	sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
 
-	control = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+	control = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
 	control &= (SM501_DC_CRT_CONTROL_PIXEL_MASK |
 		    SM501_DC_CRT_CONTROL_GAMMA |
@@ -684,7 +706,7 @@ static int sm501fb_set_par_crt(struct fb_info *info)
  out_update:
 	dev_dbg(fbi->dev, "new control is %08lx\n", control);
 
-	writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
+	smc501_writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
 	sm501fb_sync_regs(fbi);
 
 	return 0;
@@ -696,18 +718,18 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 	void __iomem *ctrl_reg = fbi->regs + SM501_DC_PANEL_CONTROL;
 	struct sm501_platdata_fbsub *pd = fbi->pdata->fb_pnl;
 
-	control = readl(ctrl_reg);
+	control = smc501_readl(ctrl_reg);
 
 	if (to && (control & SM501_DC_PANEL_CONTROL_VDD) == 0) {
 		/* enable panel power */
 
 		control |= SM501_DC_PANEL_CONTROL_VDD;	/* FPVDDEN */
-		writel(control, ctrl_reg);
+		smc501_writel(control, ctrl_reg);
 		sm501fb_sync_regs(fbi);
 		mdelay(10);
 
 		control |= SM501_DC_PANEL_CONTROL_DATA;	/* DATA */
-		writel(control, ctrl_reg);
+		smc501_writel(control, ctrl_reg);
 		sm501fb_sync_regs(fbi);
 		mdelay(10);
 
@@ -719,7 +741,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 			else
 				control |= SM501_DC_PANEL_CONTROL_BIAS;
 
-			writel(control, ctrl_reg);
+			smc501_writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
 		}
@@ -730,7 +752,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 			else
 				control |= SM501_DC_PANEL_CONTROL_FPEN;
 
-			writel(control, ctrl_reg);
+			smc501_writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
 		}
@@ -742,7 +764,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 			else
 				control &= ~SM501_DC_PANEL_CONTROL_FPEN;
 
-			writel(control, ctrl_reg);
+			smc501_writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
 		}
@@ -753,18 +775,18 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 			else
 				control &= ~SM501_DC_PANEL_CONTROL_BIAS;
 
-			writel(control, ctrl_reg);
+			smc501_writel(control, ctrl_reg);
 			sm501fb_sync_regs(fbi);
 			mdelay(10);
 		}
 
 		control &= ~SM501_DC_PANEL_CONTROL_DATA;
-		writel(control, ctrl_reg);
+		smc501_writel(control, ctrl_reg);
 		sm501fb_sync_regs(fbi);
 		mdelay(10);
 
 		control &= ~SM501_DC_PANEL_CONTROL_VDD;
-		writel(control, ctrl_reg);
+		smc501_writel(control, ctrl_reg);
 		sm501fb_sync_regs(fbi);
 		mdelay(10);
 	}
@@ -799,7 +821,7 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
 
 	/* update control register */
 
-	control = readl(fbi->regs + SM501_DC_PANEL_CONTROL);
+	control = smc501_readl(fbi->regs + SM501_DC_PANEL_CONTROL);
 	control &= (SM501_DC_PANEL_CONTROL_GAMMA |
 		    SM501_DC_PANEL_CONTROL_VDD  |
 		    SM501_DC_PANEL_CONTROL_DATA |
@@ -833,16 +855,16 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
 		BUG();
 	}
 
-	writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
+	smc501_writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
 
 	/* panel plane top left and bottom right location */
 
-	writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
+	smc501_writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
 
 	reg  = var->xres - 1;
 	reg |= (var->yres - 1) << 16;
 
-	writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
+	smc501_writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
 
 	/* program panel control register */
 
@@ -855,7 +877,7 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
 	if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
 		control |= SM501_DC_PANEL_CONTROL_VSP;
 
-	writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
+	smc501_writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
 	sm501fb_sync_regs(fbi);
 
 	/* ensure the panel interface is not tristated at this point */
@@ -924,7 +946,7 @@ static int sm501fb_setcolreg(unsigned regno,
 			val |= (green >> 8) << 8;
 			val |= blue >> 8;
 
-			writel(val, base + (regno * 4));
+			smc501_writel(val, base + (regno * 4));
 		}
 
 		break;
@@ -980,7 +1002,7 @@ static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 
 	dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
 
-	ctrl = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+	ctrl = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
 	switch (blank_mode) {
 	case FB_BLANK_POWERDOWN:
@@ -1004,7 +1026,7 @@ static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 
 	}
 
-	writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
+	smc501_writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
 	sm501fb_sync_regs(fbi);
 
 	return 0;
@@ -1041,12 +1063,14 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 	if (cursor->image.depth > 1)
 		return -EINVAL;
 
-	hwc_addr = readl(base + SM501_OFF_HWC_ADDR);
+	hwc_addr = smc501_readl(base + SM501_OFF_HWC_ADDR);
 
 	if (cursor->enable)
-		writel(hwc_addr | SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
+		smc501_writel(hwc_addr | SM501_HWC_EN,
+				base + SM501_OFF_HWC_ADDR);
 	else
-		writel(hwc_addr & ~SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
+		smc501_writel(hwc_addr & ~SM501_HWC_EN,
+				base + SM501_OFF_HWC_ADDR);
 
 	/* set data */
 	if (cursor->set & FB_CUR_SETPOS) {
@@ -1060,7 +1084,7 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
 		//y += cursor->image.height;
 
-		writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
+		smc501_writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
 	}
 
 	if (cursor->set & FB_CUR_SETCMAP) {
@@ -1080,8 +1104,8 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
 		dev_dbg(fbi->dev, "fgcol %08lx, bgcol %08lx\n", fg, bg);
 
-		writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
-		writel(fg, base + SM501_OFF_HWC_COLOR_3);
+		smc501_writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
+		smc501_writel(fg, base + SM501_OFF_HWC_COLOR_3);
 	}
 
 	if (cursor->set & FB_CUR_SETSIZE ||
@@ -1102,7 +1126,7 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 			__func__, cursor->image.width, cursor->image.height);
 
 		for (op = 0; op < (64*64*2)/8; op+=4)
-			writel(0x0, dst + op);
+			smc501_writel(0x0, dst + op);
 
 		for (y = 0; y < cursor->image.height; y++) {
 			for (x = 0; x < cursor->image.width; x++) {
@@ -1141,7 +1165,7 @@ static ssize_t sm501fb_crtsrc_show(struct device *dev,
 	struct sm501fb_info *info = dev_get_drvdata(dev);
 	unsigned long ctrl;
 
-	ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+	ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 	ctrl &= SM501_DC_CRT_CONTROL_SEL;
 
 	return snprintf(buf, PAGE_SIZE, "%s\n", ctrl ? "crt" : "panel");
@@ -1172,7 +1196,7 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
 
 	dev_info(dev, "setting crt source to head %d\n", head);
 
-	ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+	ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 
 	if (head == HEAD_CRT) {
 		ctrl |= SM501_DC_CRT_CONTROL_SEL;
@@ -1184,7 +1208,7 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
 		ctrl &= ~SM501_DC_CRT_CONTROL_TE;
 	}
 
-	writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+	smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
 	sm501fb_sync_regs(info);
 
 	return len;
@@ -1205,7 +1229,8 @@ static int sm501fb_show_regs(struct sm501fb_info *info, char *ptr,
 	unsigned int reg;
 
 	for (reg = start; reg < (len + start); reg += 4)
-		ptr += sprintf(ptr, "%08x = %08x\n", reg, readl(mem + reg));
+		ptr += sprintf(ptr, "%08x = %08x\n", reg,
+				smc501_readl(mem + reg));
 
 	return ptr - buf;
 }
@@ -1257,7 +1282,7 @@ static int sm501fb_sync(struct fb_info *info)
 
 	/* wait for the 2d engine to be ready */
 	while ((count > 0) &&
-	       (readl(fbi->regs + SM501_SYSTEM_CONTROL) &
+	       (smc501_readl(fbi->regs + SM501_SYSTEM_CONTROL) &
 		SM501_SYSCTRL_2D_ENGINE_STATUS) != 0)
 		count--;
 
@@ -1312,45 +1337,46 @@ static void sm501fb_copyarea(struct fb_info *info, const struct fb_copyarea *are
 		return;
 
 	/* set the base addresses */
-	writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
-	writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
+	smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
+	smc501_writel(par->screen.sm_addr,
+			fbi->regs2d + SM501_2D_DESTINATION_BASE);
 
 	/* set the window width */
-	writel((info->var.xres << 16) | info->var.xres,
+	smc501_writel((info->var.xres << 16) | info->var.xres,
 	       fbi->regs2d + SM501_2D_WINDOW_WIDTH);
 
 	/* set window stride */
-	writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
+	smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
 	       fbi->regs2d + SM501_2D_PITCH);
 
 	/* set data format */
 	switch (info->var.bits_per_pixel) {
 	case 8:
-		writel(0, fbi->regs2d + SM501_2D_STRETCH);
+		smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
 		break;
 	case 16:
-		writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
+		smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
 		break;
 	case 32:
-		writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
+		smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
 		break;
 	}
 
 	/* 2d compare mask */
-	writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
+	smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
 
 	/* 2d mask */
-	writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
+	smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
 
 	/* source and destination x y */
-	writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
-	writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
+	smc501_writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
+	smc501_writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
 
 	/* w/h */
-	writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
+	smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
 
 	/* do area move */
-	writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
+	smc501_writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
 }
 
 static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
@@ -1372,47 +1398,49 @@ static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rec
 		return;
 
 	/* set the base addresses */
-	writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
-	writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
+	smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
+	smc501_writel(par->screen.sm_addr,
+			fbi->regs2d + SM501_2D_DESTINATION_BASE);
 
 	/* set the window width */
-	writel((info->var.xres << 16) | info->var.xres,
+	smc501_writel((info->var.xres << 16) | info->var.xres,
 	       fbi->regs2d + SM501_2D_WINDOW_WIDTH);
 
 	/* set window stride */
-	writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
+	smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
 	       fbi->regs2d + SM501_2D_PITCH);
 
 	/* set data format */
 	switch (info->var.bits_per_pixel) {
 	case 8:
-		writel(0, fbi->regs2d + SM501_2D_STRETCH);
+		smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
 		break;
 	case 16:
-		writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
+		smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
 		break;
 	case 32:
-		writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
+		smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
 		break;
 	}
 
 	/* 2d compare mask */
-	writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
+	smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
 
 	/* 2d mask */
-	writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
+	smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
 
 	/* colour */
-	writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
+	smc501_writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
 
 	/* x y */
-	writel((rect->dx << 16) | rect->dy, fbi->regs2d + SM501_2D_DESTINATION);
+	smc501_writel((rect->dx << 16) | rect->dy,
+			fbi->regs2d + SM501_2D_DESTINATION);
 
 	/* w/h */
-	writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
+	smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
 
 	/* do rectangle fill */
-	writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
+	smc501_writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
 }
 
 
@@ -1470,11 +1498,12 @@ static int sm501_init_cursor(struct fb_info *fbi, unsigned int reg_base)
 
 	/* initialise the colour registers */
 
-	writel(par->cursor.sm_addr, par->cursor_regs + SM501_OFF_HWC_ADDR);
+	smc501_writel(par->cursor.sm_addr,
+			par->cursor_regs + SM501_OFF_HWC_ADDR);
 
-	writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
-	writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
-	writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
+	smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
+	smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
+	smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
 	sm501fb_sync_regs(info);
 
 	return 0;
@@ -1581,7 +1610,7 @@ static int sm501fb_start(struct sm501fb_info *info,
 
 	/* clear palette ram - undefined at power on */
 	for (k = 0; k < (256 * 3); k++)
-		writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
+		smc501_writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
 
 	/* enable display controller */
 	sm501_unit_power(dev->parent, SM501_GATE_DISPLAY, 1);
@@ -1596,22 +1625,22 @@ static int sm501fb_start(struct sm501fb_info *info,
 	return 0; /* everything is setup */
 
  err_mem_res:
-	release_resource(info->fbmem_res);
-	kfree(info->fbmem_res);
+	release_mem_region(info->fbmem_res->start,
+			   resource_size(info->fbmem_res));
 
  err_regs2d_map:
 	iounmap(info->regs2d);
 
  err_regs2d_res:
-	release_resource(info->regs2d_res);
-	kfree(info->regs2d_res);
+	release_mem_region(info->regs2d_res->start,
+			   resource_size(info->regs2d_res));
 
  err_regs_map:
 	iounmap(info->regs);
 
  err_regs_res:
-	release_resource(info->regs_res);
-	kfree(info->regs_res);
+	release_mem_region(info->regs_res->start,
+			   resource_size(info->regs_res));
 
  err_release:
 	return ret;
@@ -1623,19 +1652,19 @@ static void sm501fb_stop(struct sm501fb_info *info)
 	sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 0);
 
 	iounmap(info->fbmem);
-	release_resource(info->fbmem_res);
-	kfree(info->fbmem_res);
+	release_mem_region(info->fbmem_res->start,
+			   resource_size(info->fbmem_res));
 
 	iounmap(info->regs2d);
-	release_resource(info->regs2d_res);
-	kfree(info->regs2d_res);
+	release_mem_region(info->regs2d_res->start,
+			   resource_size(info->regs2d_res));
 
 	iounmap(info->regs);
-	release_resource(info->regs_res);
-	kfree(info->regs_res);
+	release_mem_region(info->regs_res->start,
+			   resource_size(info->regs_res));
 }
 
-static int sm501fb_init_fb(struct fb_info *fb,
+static int __devinit sm501fb_init_fb(struct fb_info *fb,
 			   enum sm501_controller head,
 			   const char *fbname)
 {
@@ -1649,20 +1678,20 @@ static int sm501fb_init_fb(struct fb_info *fb,
 	switch (head) {
 	case HEAD_CRT:
 		pd = info->pdata->fb_crt;
-		ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+		ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 		enable = (ctrl & SM501_DC_CRT_CONTROL_ENABLE) ? 1 : 0;
 
 		/* ensure we set the correct source register */
 		if (info->pdata->fb_route != SM501_FB_CRT_PANEL) {
 			ctrl |= SM501_DC_CRT_CONTROL_SEL;
-			writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+			smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
 		}
 
 		break;
 
 	case HEAD_PANEL:
 		pd = info->pdata->fb_pnl;
-		ctrl = readl(info->regs + SM501_DC_PANEL_CONTROL);
+		ctrl = smc501_readl(info->regs + SM501_DC_PANEL_CONTROL);
 		enable = (ctrl & SM501_DC_PANEL_CONTROL_EN) ? 1 : 0;
 		break;
 
@@ -1680,7 +1709,7 @@ static int sm501fb_init_fb(struct fb_info *fb,
 
 	if (head == HEAD_CRT && info->pdata->fb_route == SM501_FB_CRT_PANEL) {
 		ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
-		writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+		smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
 		enable = 0;
 	}
 
@@ -1690,7 +1719,7 @@ static int sm501fb_init_fb(struct fb_info *fb,
 	       (head == HEAD_CRT) ? &sm501fb_ops_crt : &sm501fb_ops_pnl,
 	       sizeof(struct fb_ops));
 
-	/* update ops dependant on what we've been passed */
+	/* update ops dependent on what we've been passed */
 
 	if ((pd->flags & SM501FB_FLAG_USE_HWCURSOR) == 0)
 		par->ops.fb_cursor = NULL;
@@ -1700,6 +1729,15 @@ static int sm501fb_init_fb(struct fb_info *fb,
 		FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT |
 		FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
 
+#if defined(CONFIG_OF)
+#ifdef __BIG_ENDIAN
+	if (of_get_property(info->dev->parent->of_node, "little-endian", NULL))
+		fb->flags |= FBINFO_FOREIGN_ENDIAN;
+#else
+	if (of_get_property(info->dev->parent->of_node, "big-endian", NULL))
+		fb->flags |= FBINFO_FOREIGN_ENDIAN;
+#endif
+#endif
 	/* fixed data */
 
 	fb->fix.type		= FB_TYPE_PACKED_PIXELS;
@@ -1717,9 +1755,16 @@ static int sm501fb_init_fb(struct fb_info *fb,
 	fb->var.vmode		= FB_VMODE_NONINTERLACED;
 	fb->var.bits_per_pixel  = 16;
 
+	if (info->edid_data) {
+			/* Now build modedb from EDID */
+			fb_edid_to_monspecs(info->edid_data, &fb->monspecs);
+			fb_videomode_to_modelist(fb->monspecs.modedb,
+						 fb->monspecs.modedb_len,
+						 &fb->modelist);
+	}
+
 	if (enable && (pd->flags & SM501FB_FLAG_USE_INIT_MODE) && 0) {
 		/* TODO read the mode from the current display */
-
 	} else {
 		if (pd->def_mode) {
 			dev_info(info->dev, "using supplied mode\n");
@@ -1729,12 +1774,37 @@ static int sm501fb_init_fb(struct fb_info *fb,
 			fb->var.xres_virtual = fb->var.xres;
 			fb->var.yres_virtual = fb->var.yres;
 		} else {
-			ret = fb_find_mode(&fb->var, fb,
+			if (info->edid_data) {
+				ret = fb_find_mode(&fb->var, fb, fb_mode,
+					fb->monspecs.modedb,
+					fb->monspecs.modedb_len,
+					&sm501_default_mode, default_bpp);
+				/* edid_data is no longer needed, free it */
+				kfree(info->edid_data);
+			} else {
+				ret = fb_find_mode(&fb->var, fb,
 					   NULL, NULL, 0, NULL, 8);
+			}
 
-			if (ret == 0 || ret == 4) {
-				dev_err(info->dev,
-					"failed to get initial mode\n");
+			switch (ret) {
+			case 1:
+				dev_info(info->dev, "using mode specified in "
+						"@mode\n");
+				break;
+			case 2:
+				dev_info(info->dev, "using mode specified in "
+					"@mode with ignored refresh rate\n");
+				break;
+			case 3:
+				dev_info(info->dev, "using mode default "
+					"mode\n");
+				break;
+			case 4:
+				dev_info(info->dev, "using mode from list\n");
+				break;
+			default:
+				dev_info(info->dev, "ret = %d\n", ret);
+				dev_info(info->dev, "failed to find mode\n");
 				return -EINVAL;
 			}
 		}
@@ -1875,8 +1945,32 @@ static int __devinit sm501fb_probe(struct platform_device *pdev)
 	}
 
 	if (info->pdata == NULL) {
-		dev_info(dev, "using default configuration data\n");
+		int found = 0;
+#if defined(CONFIG_OF)
+		struct device_node *np = pdev->dev.parent->of_node;
+		const u8 *prop;
+		const char *cp;
+		int len;
+
 		info->pdata = &sm501fb_def_pdata;
+		if (np) {
+			/* Get EDID */
+			cp = of_get_property(np, "mode", &len);
+			if (cp)
+				strcpy(fb_mode, cp);
+			prop = of_get_property(np, "edid", &len);
+			if (prop && len == EDID_LENGTH) {
+				info->edid_data = kmemdup(prop, EDID_LENGTH,
+							  GFP_KERNEL);
+				if (info->edid_data)
+					found = 1;
+			}
+		}
+#endif
+		if (!found) {
+			dev_info(dev, "using default configuration data\n");
+			info->pdata = &sm501fb_def_pdata;
+		}
 	}
 
 	/* probe for the presence of each panel */
@@ -2085,7 +2179,7 @@ static int sm501fb_suspend(struct platform_device *pdev, pm_message_t state)
 	struct sm501fb_info *info = platform_get_drvdata(pdev);
 
 	/* store crt control to resume with */
-	info->pm_crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+	info->pm_crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 
 	sm501fb_suspend_fb(info, HEAD_CRT);
 	sm501fb_suspend_fb(info, HEAD_PANEL);
@@ -2109,10 +2203,10 @@ static int sm501fb_resume(struct platform_device *pdev)
 
 	/* restore the items we want to be saved for crt control */
 
-	crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+	crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 	crt_ctrl &= ~SM501_CRT_CTRL_SAVE;
 	crt_ctrl |= info->pm_crt_ctrl & SM501_CRT_CTRL_SAVE;
-	writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
+	smc501_writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
 
 	sm501fb_resume_fb(info, HEAD_CRT);
 	sm501fb_resume_fb(info, HEAD_PANEL);
@@ -2149,6 +2243,11 @@ static void __exit sm501fb_cleanup(void)
 module_init(sm501fb_init);
 module_exit(sm501fb_cleanup);
 
+module_param_named(mode, fb_mode, charp, 0);
+MODULE_PARM_DESC(mode,
+	"Specify resolution as \"<xres>x<yres>[-<bpp>][@<refresh>]\" ");
+module_param_named(bpp, default_bpp, ulong, 0);
+MODULE_PARM_DESC(bpp, "Specify bit-per-pixel if not specified mode");
 MODULE_AUTHOR("Ben Dooks, Vincent Sanders");
 MODULE_DESCRIPTION("SM501 Framebuffer driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sstfb.c b/drivers/video/sstfb.c
index 2ab704118c44..2301c275d63a 100644
--- a/drivers/video/sstfb.c
+++ b/drivers/video/sstfb.c
@@ -221,7 +221,7 @@ static int __sst_wait_idle(u8 __iomem *vbase)
 	while(1) {
 		if (__sst_read(vbase, STATUS) & STATUS_FBI_BUSY) {
 			f_dddprintk("status: busy\n");
-/* FIXME basicaly, this is a busy wait. maybe not that good. oh well;
+/* FIXME basically, this is a busy wait. maybe not that good. oh well;
  * this is a small loop after all.
  * Or maybe we should use mdelay() or udelay() here instead ? */
 			count = 0;
@@ -501,7 +501,7 @@ static int sstfb_set_par(struct fb_info *info)
 	}
 
 	if (IS_VOODOO2(par)) {
-		/* voodoo2 has 32 pixel wide tiles , BUT stange things
+		/* voodoo2 has 32 pixel wide tiles , BUT strange things
 		   happen with odd number of tiles */
 		par->tiles_in_X = (info->var.xres + 63 ) / 64 * 2;
 	} else {
@@ -920,11 +920,11 @@ static int __devinit sst_detect_ti(struct fb_info *info)
  * we get the 1st byte (M value) of preset f1,f7 and fB
  * why those 3 ? mmmh... for now, i'll do it the glide way...
  * and ask questions later. anyway, it seems that all the freq registers are
- * realy at their default state (cf specs) so i ask again, why those 3 regs ?
+ * really at their default state (cf specs) so i ask again, why those 3 regs ?
  * mmmmh.. it seems that's much more ugly than i thought. we use f0 and fA for
  * pll programming, so in fact, we *hope* that the f1, f7 & fB won't be
  * touched...
- * is it realy safe ? how can i reset this ramdac ? geee...
+ * is it really safe ? how can i reset this ramdac ? geee...
  */
 static int __devinit sst_detect_ics(struct fb_info *info)
 {
diff --git a/drivers/video/sticore.h b/drivers/video/sticore.h
index 7fe5be4bc70e..addf7b615ef8 100644
--- a/drivers/video/sticore.h
+++ b/drivers/video/sticore.h
@@ -79,7 +79,7 @@ struct sti_glob_cfg_ext {
 	 u8 curr_mon;			/* current monitor configured */
 	 u8 friendly_boot;		/* in friendly boot mode */
 	s16 power;			/* power calculation (in Watts) */
-	s32 freq_ref;			/* frequency refrence */
+	s32 freq_ref;			/* frequency reference */
 	u32 sti_mem_addr;		/* pointer to global sti memory (size=sti_mem_request) */
 	u32 future_ptr; 		/* pointer to future data */
 };
diff --git a/drivers/video/sunxvr1000.c b/drivers/video/sunxvr1000.c
index 5dbe06af2226..b7f27acaf817 100644
--- a/drivers/video/sunxvr1000.c
+++ b/drivers/video/sunxvr1000.c
@@ -111,8 +111,7 @@ static int __devinit gfb_set_fbinfo(struct gfb_info *gp)
         return 0;
 }
 
-static int __devinit gfb_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit gfb_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -198,7 +197,7 @@ static const struct of_device_id gfb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ffb_match);
 
-static struct of_platform_driver gfb_driver = {
+static struct platform_driver gfb_driver = {
 	.probe		= gfb_probe,
 	.remove		= __devexit_p(gfb_remove),
 	.driver = {
@@ -213,12 +212,12 @@ static int __init gfb_init(void)
 	if (fb_get_options("gfb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&gfb_driver);
+	return platform_driver_register(&gfb_driver);
 }
 
 static void __exit gfb_exit(void)
 {
-	of_unregister_platform_driver(&gfb_driver);
+	platform_driver_unregister(&gfb_driver);
 }
 
 module_init(gfb_init);
diff --git a/drivers/video/svgalib.c b/drivers/video/svgalib.c
index fdb45674e2f6..33df9ec91795 100644
--- a/drivers/video/svgalib.c
+++ b/drivers/video/svgalib.c
@@ -20,12 +20,12 @@
 
 
 /* Write a CRT register value spread across multiple registers */
-void svga_wcrt_multi(const struct vga_regset *regset, u32 value) {
-
+void svga_wcrt_multi(void __iomem *regbase, const struct vga_regset *regset, u32 value)
+{
 	u8 regval, bitval, bitnum;
 
 	while (regset->regnum != VGA_REGSET_END_VAL) {
-		regval = vga_rcrt(NULL, regset->regnum);
+		regval = vga_rcrt(regbase, regset->regnum);
 		bitnum = regset->lowbit;
 		while (bitnum <= regset->highbit) {
 			bitval = 1 << bitnum;
@@ -34,18 +34,18 @@ void svga_wcrt_multi(const struct vga_regset *regset, u32 value) {
 			bitnum ++;
 			value = value >> 1;
 		}
-		vga_wcrt(NULL, regset->regnum, regval);
+		vga_wcrt(regbase, regset->regnum, regval);
 		regset ++;
 	}
 }
 
 /* Write a sequencer register value spread across multiple registers */
-void svga_wseq_multi(const struct vga_regset *regset, u32 value) {
-
+void svga_wseq_multi(void __iomem *regbase, const struct vga_regset *regset, u32 value)
+{
 	u8 regval, bitval, bitnum;
 
 	while (regset->regnum != VGA_REGSET_END_VAL) {
-		regval = vga_rseq(NULL, regset->regnum);
+		regval = vga_rseq(regbase, regset->regnum);
 		bitnum = regset->lowbit;
 		while (bitnum <= regset->highbit) {
 			bitval = 1 << bitnum;
@@ -54,7 +54,7 @@ void svga_wseq_multi(const struct vga_regset *regset, u32 value) {
 			bitnum ++;
 			value = value >> 1;
 		}
-		vga_wseq(NULL, regset->regnum, regval);
+		vga_wseq(regbase, regset->regnum, regval);
 		regset ++;
 	}
 }
@@ -75,95 +75,95 @@ static unsigned int svga_regset_size(const struct vga_regset *regset)
 
 
 /* Set graphics controller registers to sane values */
-void svga_set_default_gfx_regs(void)
+void svga_set_default_gfx_regs(void __iomem *regbase)
 {
 	/* All standard GFX registers (GR00 - GR08) */
-	vga_wgfx(NULL, VGA_GFX_SR_VALUE, 0x00);
-	vga_wgfx(NULL, VGA_GFX_SR_ENABLE, 0x00);
-	vga_wgfx(NULL, VGA_GFX_COMPARE_VALUE, 0x00);
-	vga_wgfx(NULL, VGA_GFX_DATA_ROTATE, 0x00);
-	vga_wgfx(NULL, VGA_GFX_PLANE_READ, 0x00);
-	vga_wgfx(NULL, VGA_GFX_MODE, 0x00);
-/*	vga_wgfx(NULL, VGA_GFX_MODE, 0x20); */
-/*	vga_wgfx(NULL, VGA_GFX_MODE, 0x40); */
-	vga_wgfx(NULL, VGA_GFX_MISC, 0x05);
-/*	vga_wgfx(NULL, VGA_GFX_MISC, 0x01); */
-	vga_wgfx(NULL, VGA_GFX_COMPARE_MASK, 0x0F);
-	vga_wgfx(NULL, VGA_GFX_BIT_MASK, 0xFF);
+	vga_wgfx(regbase, VGA_GFX_SR_VALUE, 0x00);
+	vga_wgfx(regbase, VGA_GFX_SR_ENABLE, 0x00);
+	vga_wgfx(regbase, VGA_GFX_COMPARE_VALUE, 0x00);
+	vga_wgfx(regbase, VGA_GFX_DATA_ROTATE, 0x00);
+	vga_wgfx(regbase, VGA_GFX_PLANE_READ, 0x00);
+	vga_wgfx(regbase, VGA_GFX_MODE, 0x00);
+/*	vga_wgfx(regbase, VGA_GFX_MODE, 0x20); */
+/*	vga_wgfx(regbase, VGA_GFX_MODE, 0x40); */
+	vga_wgfx(regbase, VGA_GFX_MISC, 0x05);
+/*	vga_wgfx(regbase, VGA_GFX_MISC, 0x01); */
+	vga_wgfx(regbase, VGA_GFX_COMPARE_MASK, 0x0F);
+	vga_wgfx(regbase, VGA_GFX_BIT_MASK, 0xFF);
 }
 
 /* Set attribute controller registers to sane values */
-void svga_set_default_atc_regs(void)
+void svga_set_default_atc_regs(void __iomem *regbase)
 {
 	u8 count;
 
-	vga_r(NULL, 0x3DA);
-	vga_w(NULL, VGA_ATT_W, 0x00);
+	vga_r(regbase, 0x3DA);
+	vga_w(regbase, VGA_ATT_W, 0x00);
 
 	/* All standard ATC registers (AR00 - AR14) */
 	for (count = 0; count <= 0xF; count ++)
-		svga_wattr(count, count);
+		svga_wattr(regbase, count, count);
 
-	svga_wattr(VGA_ATC_MODE, 0x01);
-/*	svga_wattr(VGA_ATC_MODE, 0x41); */
-	svga_wattr(VGA_ATC_OVERSCAN, 0x00);
-	svga_wattr(VGA_ATC_PLANE_ENABLE, 0x0F);
-	svga_wattr(VGA_ATC_PEL, 0x00);
-	svga_wattr(VGA_ATC_COLOR_PAGE, 0x00);
+	svga_wattr(regbase, VGA_ATC_MODE, 0x01);
+/*	svga_wattr(regbase, VGA_ATC_MODE, 0x41); */
+	svga_wattr(regbase, VGA_ATC_OVERSCAN, 0x00);
+	svga_wattr(regbase, VGA_ATC_PLANE_ENABLE, 0x0F);
+	svga_wattr(regbase, VGA_ATC_PEL, 0x00);
+	svga_wattr(regbase, VGA_ATC_COLOR_PAGE, 0x00);
 
-	vga_r(NULL, 0x3DA);
-	vga_w(NULL, VGA_ATT_W, 0x20);
+	vga_r(regbase, 0x3DA);
+	vga_w(regbase, VGA_ATT_W, 0x20);
 }
 
 /* Set sequencer registers to sane values */
-void svga_set_default_seq_regs(void)
+void svga_set_default_seq_regs(void __iomem *regbase)
 {
 	/* Standard sequencer registers (SR01 - SR04), SR00 is not set */
-	vga_wseq(NULL, VGA_SEQ_CLOCK_MODE, VGA_SR01_CHAR_CLK_8DOTS);
-	vga_wseq(NULL, VGA_SEQ_PLANE_WRITE, VGA_SR02_ALL_PLANES);
-	vga_wseq(NULL, VGA_SEQ_CHARACTER_MAP, 0x00);
-/*	vga_wseq(NULL, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE | VGA_SR04_CHN_4M); */
-	vga_wseq(NULL, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE);
+	vga_wseq(regbase, VGA_SEQ_CLOCK_MODE, VGA_SR01_CHAR_CLK_8DOTS);
+	vga_wseq(regbase, VGA_SEQ_PLANE_WRITE, VGA_SR02_ALL_PLANES);
+	vga_wseq(regbase, VGA_SEQ_CHARACTER_MAP, 0x00);
+/*	vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE | VGA_SR04_CHN_4M); */
+	vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE);
 }
 
 /* Set CRTC registers to sane values */
-void svga_set_default_crt_regs(void)
+void svga_set_default_crt_regs(void __iomem *regbase)
 {
 	/* Standard CRT registers CR03 CR08 CR09 CR14 CR17 */
-	svga_wcrt_mask(0x03, 0x80, 0x80);	/* Enable vertical retrace EVRA */
-	vga_wcrt(NULL, VGA_CRTC_PRESET_ROW, 0);
-	svga_wcrt_mask(VGA_CRTC_MAX_SCAN, 0, 0x1F);
-	vga_wcrt(NULL, VGA_CRTC_UNDERLINE, 0);
-	vga_wcrt(NULL, VGA_CRTC_MODE, 0xE3);
+	svga_wcrt_mask(regbase, 0x03, 0x80, 0x80);	/* Enable vertical retrace EVRA */
+	vga_wcrt(regbase, VGA_CRTC_PRESET_ROW, 0);
+	svga_wcrt_mask(regbase, VGA_CRTC_MAX_SCAN, 0, 0x1F);
+	vga_wcrt(regbase, VGA_CRTC_UNDERLINE, 0);
+	vga_wcrt(regbase, VGA_CRTC_MODE, 0xE3);
 }
 
-void svga_set_textmode_vga_regs(void)
+void svga_set_textmode_vga_regs(void __iomem *regbase)
 {
-	/* svga_wseq_mask(0x1, 0x00, 0x01); */   /* Switch 8/9 pixel per char */
-	vga_wseq(NULL, VGA_SEQ_MEMORY_MODE,	VGA_SR04_EXT_MEM);
-	vga_wseq(NULL, VGA_SEQ_PLANE_WRITE,	0x03);
+	/* svga_wseq_mask(regbase, 0x1, 0x00, 0x01); */   /* Switch 8/9 pixel per char */
+	vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM);
+	vga_wseq(regbase, VGA_SEQ_PLANE_WRITE, 0x03);
 
-	vga_wcrt(NULL, VGA_CRTC_MAX_SCAN,	0x0f); /* 0x4f */
-	vga_wcrt(NULL, VGA_CRTC_UNDERLINE,	0x1f);
-	svga_wcrt_mask(VGA_CRTC_MODE,		0x23, 0x7f);
+	vga_wcrt(regbase, VGA_CRTC_MAX_SCAN, 0x0f); /* 0x4f */
+	vga_wcrt(regbase, VGA_CRTC_UNDERLINE, 0x1f);
+	svga_wcrt_mask(regbase, VGA_CRTC_MODE, 0x23, 0x7f);
 
-	vga_wcrt(NULL, VGA_CRTC_CURSOR_START,	0x0d);
-	vga_wcrt(NULL, VGA_CRTC_CURSOR_END,	0x0e);
-	vga_wcrt(NULL, VGA_CRTC_CURSOR_HI,	0x00);
-	vga_wcrt(NULL, VGA_CRTC_CURSOR_LO,	0x00);
+	vga_wcrt(regbase, VGA_CRTC_CURSOR_START, 0x0d);
+	vga_wcrt(regbase, VGA_CRTC_CURSOR_END, 0x0e);
+	vga_wcrt(regbase, VGA_CRTC_CURSOR_HI, 0x00);
+	vga_wcrt(regbase, VGA_CRTC_CURSOR_LO, 0x00);
 
-	vga_wgfx(NULL, VGA_GFX_MODE,		0x10); /* Odd/even memory mode */
-	vga_wgfx(NULL, VGA_GFX_MISC,		0x0E); /* Misc graphics register - text mode enable */
-	vga_wgfx(NULL, VGA_GFX_COMPARE_MASK,	0x00);
+	vga_wgfx(regbase, VGA_GFX_MODE, 0x10); /* Odd/even memory mode */
+	vga_wgfx(regbase, VGA_GFX_MISC, 0x0E); /* Misc graphics register - text mode enable */
+	vga_wgfx(regbase, VGA_GFX_COMPARE_MASK, 0x00);
 
-	vga_r(NULL, 0x3DA);
-	vga_w(NULL, VGA_ATT_W, 0x00);
+	vga_r(regbase, 0x3DA);
+	vga_w(regbase, VGA_ATT_W, 0x00);
 
-	svga_wattr(0x10, 0x0C);			/* Attribute Mode Control Register - text mode, blinking and line graphics */
-	svga_wattr(0x13, 0x08);			/* Horizontal Pixel Panning Register  */
+	svga_wattr(regbase, 0x10, 0x0C);			/* Attribute Mode Control Register - text mode, blinking and line graphics */
+	svga_wattr(regbase, 0x13, 0x08);			/* Horizontal Pixel Panning Register  */
 
-	vga_r(NULL, 0x3DA);
-	vga_w(NULL, VGA_ATT_W, 0x20);
+	vga_r(regbase, 0x3DA);
+	vga_w(regbase, VGA_ATT_W, 0x20);
 }
 
 #if 0
@@ -299,7 +299,7 @@ void svga_tileblit(struct fb_info *info, struct fb_tileblit *blit)
 }
 
 /* Set cursor in text (tileblit) mode */
-void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+void svga_tilecursor(void __iomem *regbase, struct fb_info *info, struct fb_tilecursor *cursor)
 {
 	u8 cs = 0x0d;
 	u8 ce = 0x0e;
@@ -310,7 +310,7 @@ void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
 	if (! cursor -> mode)
 		return;
 
-	svga_wcrt_mask(0x0A, 0x20, 0x20); /* disable cursor */
+	svga_wcrt_mask(regbase, 0x0A, 0x20, 0x20); /* disable cursor */
 
 	if (cursor -> shape == FB_TILE_CURSOR_NONE)
 		return;
@@ -334,11 +334,11 @@ void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
 	}
 
 	/* set cursor position */
-	vga_wcrt(NULL, 0x0E, pos >> 8);
-	vga_wcrt(NULL, 0x0F, pos & 0xFF);
+	vga_wcrt(regbase, 0x0E, pos >> 8);
+	vga_wcrt(regbase, 0x0F, pos & 0xFF);
 
-	vga_wcrt(NULL, 0x0B, ce); /* set cursor end */
-	vga_wcrt(NULL, 0x0A, cs); /* set cursor start and enable it */
+	vga_wcrt(regbase, 0x0B, ce); /* set cursor end */
+	vga_wcrt(regbase, 0x0A, cs); /* set cursor start and enable it */
 }
 
 int svga_get_tilemax(struct fb_info *info)
@@ -507,8 +507,9 @@ int svga_check_timings(const struct svga_timing_regs *tm, struct fb_var_screenin
 }
 
 /* Set CRT timing registers */
-void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninfo *var,
-			u32 hmul, u32 hdiv, u32 vmul, u32 vdiv, u32 hborder, int node)
+void svga_set_timings(void __iomem *regbase, const struct svga_timing_regs *tm,
+		      struct fb_var_screeninfo *var,
+		      u32 hmul, u32 hdiv, u32 vmul, u32 vdiv, u32 hborder, int node)
 {
 	u8 regval;
 	u32 value;
@@ -516,66 +517,66 @@ void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninf
 	value = var->xres + var->left_margin + var->right_margin + var->hsync_len;
 	value = (value * hmul) / hdiv;
 	pr_debug("fb%d: horizontal total      : %d\n", node, value);
-	svga_wcrt_multi(tm->h_total_regs, (value / 8) - 5);
+	svga_wcrt_multi(regbase, tm->h_total_regs, (value / 8) - 5);
 
 	value = var->xres;
 	value = (value * hmul) / hdiv;
 	pr_debug("fb%d: horizontal display    : %d\n", node, value);
-	svga_wcrt_multi(tm->h_display_regs, (value / 8) - 1);
+	svga_wcrt_multi(regbase, tm->h_display_regs, (value / 8) - 1);
 
 	value = var->xres;
 	value = (value * hmul) / hdiv;
 	pr_debug("fb%d: horizontal blank start: %d\n", node, value);
-	svga_wcrt_multi(tm->h_blank_start_regs, (value / 8) - 1 + hborder);
+	svga_wcrt_multi(regbase, tm->h_blank_start_regs, (value / 8) - 1 + hborder);
 
 	value = var->xres + var->left_margin + var->right_margin + var->hsync_len;
 	value = (value * hmul) / hdiv;
 	pr_debug("fb%d: horizontal blank end  : %d\n", node, value);
-	svga_wcrt_multi(tm->h_blank_end_regs, (value / 8) - 1 - hborder);
+	svga_wcrt_multi(regbase, tm->h_blank_end_regs, (value / 8) - 1 - hborder);
 
 	value = var->xres + var->right_margin;
 	value = (value * hmul) / hdiv;
 	pr_debug("fb%d: horizontal sync start : %d\n", node, value);
-	svga_wcrt_multi(tm->h_sync_start_regs, (value / 8));
+	svga_wcrt_multi(regbase, tm->h_sync_start_regs, (value / 8));
 
 	value = var->xres + var->right_margin + var->hsync_len;
 	value = (value * hmul) / hdiv;
 	pr_debug("fb%d: horizontal sync end   : %d\n", node, value);
-	svga_wcrt_multi(tm->h_sync_end_regs, (value / 8));
+	svga_wcrt_multi(regbase, tm->h_sync_end_regs, (value / 8));
 
 	value = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
 	value = (value * vmul) / vdiv;
 	pr_debug("fb%d: vertical total        : %d\n", node, value);
-	svga_wcrt_multi(tm->v_total_regs, value - 2);
+	svga_wcrt_multi(regbase, tm->v_total_regs, value - 2);
 
 	value = var->yres;
 	value = (value * vmul) / vdiv;
 	pr_debug("fb%d: vertical display      : %d\n", node, value);
-	svga_wcrt_multi(tm->v_display_regs, value - 1);
+	svga_wcrt_multi(regbase, tm->v_display_regs, value - 1);
 
 	value = var->yres;
 	value = (value * vmul) / vdiv;
 	pr_debug("fb%d: vertical blank start  : %d\n", node, value);
-	svga_wcrt_multi(tm->v_blank_start_regs, value);
+	svga_wcrt_multi(regbase, tm->v_blank_start_regs, value);
 
 	value = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
 	value = (value * vmul) / vdiv;
 	pr_debug("fb%d: vertical blank end    : %d\n", node, value);
-	svga_wcrt_multi(tm->v_blank_end_regs, value - 2);
+	svga_wcrt_multi(regbase, tm->v_blank_end_regs, value - 2);
 
 	value = var->yres + var->lower_margin;
 	value = (value * vmul) / vdiv;
 	pr_debug("fb%d: vertical sync start   : %d\n", node, value);
-	svga_wcrt_multi(tm->v_sync_start_regs, value);
+	svga_wcrt_multi(regbase, tm->v_sync_start_regs, value);
 
 	value = var->yres + var->lower_margin + var->vsync_len;
 	value = (value * vmul) / vdiv;
 	pr_debug("fb%d: vertical sync end     : %d\n", node, value);
-	svga_wcrt_multi(tm->v_sync_end_regs, value);
+	svga_wcrt_multi(regbase, tm->v_sync_end_regs, value);
 
 	/* Set horizontal and vertical sync pulse polarity in misc register */
 
-	regval = vga_r(NULL, VGA_MIS_R);
+	regval = vga_r(regbase, VGA_MIS_R);
 	if (var->sync & FB_SYNC_HOR_HIGH_ACT) {
 		pr_debug("fb%d: positive horizontal sync\n", node);
 		regval = regval & ~0x80;
@@ -590,7 +591,7 @@ void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninf
 		pr_debug("fb%d: negative vertical sync\n\n", node);
 		regval = regval | 0x40;
 	}
-	vga_w(NULL, VGA_MIS_W, regval);
+	vga_w(regbase, VGA_MIS_W, regval);
 }
 
 
diff --git a/drivers/video/tcx.c b/drivers/video/tcx.c
index 77ad27955cf0..07c66e946634 100644
--- a/drivers/video/tcx.c
+++ b/drivers/video/tcx.c
@@ -362,8 +362,7 @@ static void tcx_unmap_regs(struct platform_device *op, struct fb_info *info,
 			   info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit tcx_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit tcx_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -481,6 +480,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
 	tcx_unmap_regs(op, info, par);
+	framebuffer_release(info);
 
 out_err:
 	return err;
@@ -511,7 +511,7 @@ static const struct of_device_id tcx_match[] = {
 };
 MODULE_DEVICE_TABLE(of, tcx_match);
 
-static struct of_platform_driver tcx_driver = {
+static struct platform_driver tcx_driver = {
 	.driver = {
 		.name = "tcx",
 		.owner = THIS_MODULE,
@@ -526,12 +526,12 @@ static int __init tcx_init(void)
 	if (fb_get_options("tcxfb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&tcx_driver);
+	return platform_driver_register(&tcx_driver);
 }
 
 static void __exit tcx_exit(void)
 {
-	of_unregister_platform_driver(&tcx_driver);
+	platform_driver_unregister(&tcx_driver);
 }
 
 module_init(tcx_init);
diff --git a/drivers/video/tdfxfb.c b/drivers/video/tdfxfb.c
index 3ee5e63cfa4f..a99b994c9b6b 100644
--- a/drivers/video/tdfxfb.c
+++ b/drivers/video/tdfxfb.c
@@ -877,12 +877,12 @@ static void tdfxfb_fillrect(struct fb_info *info,
 	else
 		tdfx_rop = TDFX_ROP_XOR;
 
-	/* asume always rect->height < 4096 */
+	/* assume always rect->height < 4096 */
 	if (dy + rect->height > 4095) {
 		dstbase = stride * dy;
 		dy = 0;
 	}
-	/* asume always rect->width < 4096 */
+	/* assume always rect->width < 4096 */
 	if (dx + rect->width > 4095) {
 		dstbase += dx * bpp >> 3;
 		dx = 0;
@@ -915,22 +915,22 @@ static void tdfxfb_copyarea(struct fb_info *info,
 	u32 dstbase = 0;
 	u32 srcbase = 0;
 
-	/* asume always area->height < 4096 */
+	/* assume always area->height < 4096 */
 	if (sy + area->height > 4095) {
 		srcbase = stride * sy;
 		sy = 0;
 	}
-	/* asume always area->width < 4096 */
+	/* assume always area->width < 4096 */
 	if (sx + area->width > 4095) {
 		srcbase += sx * bpp >> 3;
 		sx = 0;
 	}
-	/* asume always area->height < 4096 */
+	/* assume always area->height < 4096 */
 	if (dy + area->height > 4095) {
 		dstbase = stride * dy;
 		dy = 0;
 	}
-	/* asume always area->width < 4096 */
+	/* assume always area->width < 4096 */
 	if (dx + area->width > 4095) {
 		dstbase += dx * bpp >> 3;
 		dx = 0;
@@ -1003,12 +1003,12 @@ static void tdfxfb_imageblit(struct fb_info *info, const struct fb_image *image)
 #else
 	srcfmt = 0x400000;
 #endif
-	/* asume always image->height < 4096 */
+	/* assume always image->height < 4096 */
 	if (dy + image->height > 4095) {
 		dstbase = stride * dy;
 		dy = 0;
 	}
-	/* asume always image->width < 4096 */
+	/* assume always image->width < 4096 */
 	if (dx + image->width > 4095) {
 		dstbase += dx * bpp >> 3;
 		dx = 0;
@@ -1124,7 +1124,7 @@ static int tdfxfb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 		 * lower half (least significant 64 bits) of a 128 bit word
 		 * and pattern 1 the upper half. If you examine the data of
 		 * the cursor image the graphics card uses then from the
-		 * begining you see line one of pattern 0, line one of
+		 * beginning you see line one of pattern 0, line one of
 		 * pattern 1, line two of pattern 0, line two of pattern 1,
 		 * etc etc. The linear stride for the cursor is always 16 bytes
 		 * (128 bits) which is the maximum cursor width times two for
diff --git a/drivers/video/tmiofb.c b/drivers/video/tmiofb.c
index dfef88c803d4..cd1c4dcef8fd 100644
--- a/drivers/video/tmiofb.c
+++ b/drivers/video/tmiofb.c
@@ -250,8 +250,7 @@ static irqreturn_t tmiofb_irq(int irq, void *__info)
  */
 static int tmiofb_hw_stop(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data = dev->dev.platform_data;
 	struct fb_info *info = platform_get_drvdata(dev);
 	struct tmiofb_par *par = info->par;
 
@@ -268,7 +267,7 @@ static int tmiofb_hw_stop(struct platform_device *dev)
  */
 static int tmiofb_hw_init(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct fb_info *info = platform_get_drvdata(dev);
 	struct tmiofb_par *par = info->par;
 	const struct resource *nlcr = &cell->resources[0];
@@ -312,8 +311,7 @@ static int tmiofb_hw_init(struct platform_device *dev)
  */
 static void tmiofb_hw_mode(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data = dev->dev.platform_data;
 	struct fb_info *info = platform_get_drvdata(dev);
 	struct fb_videomode *mode = info->mode;
 	struct tmiofb_par *par = info->par;
@@ -361,7 +359,7 @@ tmiofb_acc_wait(struct fb_info *info, unsigned int ccs)
 {
 	struct tmiofb_par *par = info->par;
 	/*
-	 * This code can be called whith interrupts disabled.
+	 * This code can be called with interrupts disabled.
 	 * So instead of relaying on irq to trigger the event,
 	 * poll the state till the necessary command is executed.
 	 */
@@ -559,9 +557,7 @@ static int tmiofb_ioctl(struct fb_info *fbi,
 static struct fb_videomode *
 tmiofb_find_mode(struct fb_info *info, struct fb_var_screeninfo *var)
 {
-	struct mfd_cell *cell =
-		info->device->platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data = info->device->platform_data;
 	struct fb_videomode *best = NULL;
 	int i;
 
@@ -581,9 +577,7 @@ static int tmiofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 
 	struct fb_videomode *mode;
-	struct mfd_cell *cell =
-		info->device->platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data = info->device->platform_data;
 
 	mode = tmiofb_find_mode(info, var);
 	if (!mode || var->bits_per_pixel > 16)
@@ -683,8 +677,8 @@ static struct fb_ops tmiofb_ops = {
 
 static int __devinit tmiofb_probe(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
+	struct tmio_fb_data *data = dev->dev.platform_data;
 	struct resource *ccr = platform_get_resource(dev, IORESOURCE_MEM, 1);
 	struct resource *lcr = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	struct resource *vram = platform_get_resource(dev, IORESOURCE_MEM, 2);
@@ -811,7 +805,7 @@ err_ioremap_ccr:
 
 static int __devexit tmiofb_remove(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct fb_info *info = platform_get_drvdata(dev);
 	int irq = platform_get_irq(dev, 0);
 	struct tmiofb_par *par;
@@ -941,7 +935,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 #ifdef CONFIG_FB_TMIO_ACCELL
 	struct tmiofb_par *par = info->par;
 #endif
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	int retval = 0;
 
 	console_lock();
@@ -973,7 +967,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 static int tmiofb_resume(struct platform_device *dev)
 {
 	struct fb_info *info = platform_get_drvdata(dev);
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	int retval = 0;
 
 	console_lock();
diff --git a/drivers/video/udlfb.c b/drivers/video/udlfb.c
index 2c8364e9b632..087fc9960bb9 100644
--- a/drivers/video/udlfb.c
+++ b/drivers/video/udlfb.c
@@ -27,6 +27,7 @@
 #include <linux/fb.h>
 #include <linux/vmalloc.h>
 #include <linux/slab.h>
+#include <linux/prefetch.h>
 #include <linux/delay.h>
 #include <video/udlfb.h>
 #include "edid.h"
@@ -769,7 +770,7 @@ static int dlfb_ops_ioctl(struct fb_info *info, unsigned int cmd,
 
 		/*
 		 * If we have a damage-aware client, turn fb_defio "off"
-		 * To avoid perf imact of unecessary page fault handling.
+		 * To avoid perf imact of unnecessary page fault handling.
 		 * Done by resetting the delay for this fb_info to a very
 		 * long period. Pages will become writable and stay that way.
 		 * Reset to normal value when all clients have closed this fb.
@@ -1231,8 +1232,12 @@ static int dlfb_setup_modes(struct dlfb_data *dev,
 			if (dlfb_is_valid_mode(&info->monspecs.modedb[i], info))
 				fb_add_videomode(&info->monspecs.modedb[i],
 					&info->modelist);
-			else /* if we've removed top/best mode */
-				info->monspecs.misc &= ~FB_MISC_1ST_DETAIL;
+			else {
+				if (i == 0)
+					/* if we've removed top/best mode */
+					info->monspecs.misc
+						&= ~FB_MISC_1ST_DETAIL;
+			}
 		}
 
 		default_vmode = fb_find_best_display(&info->monspecs,
@@ -1586,10 +1591,19 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 		goto error;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(fb_device_attrs); i++)
-		device_create_file(info->dev, &fb_device_attrs[i]);
+	for (i = 0; i < ARRAY_SIZE(fb_device_attrs); i++) {
+		retval = device_create_file(info->dev, &fb_device_attrs[i]);
+		if (retval) {
+			pr_err("device_create_file failed %d\n", retval);
+			goto err_del_attrs;
+		}
+	}
 
-	device_create_bin_file(info->dev, &edid_attr);
+	retval = device_create_bin_file(info->dev, &edid_attr);
+	if (retval) {
+		pr_err("device_create_bin_file failed %d\n", retval);
+		goto err_del_attrs;
+	}
 
 	pr_info("DisplayLink USB device /dev/fb%d attached. %dx%d resolution."
 			" Using %dK framebuffer memory\n", info->node,
@@ -1598,6 +1612,10 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 			info->fix.smem_len * 2 : info->fix.smem_len) >> 10);
 	return 0;
 
+err_del_attrs:
+	for (i -= 1; i >= 0; i--)
+		device_remove_file(info->dev, &fb_device_attrs[i]);
+
 error:
 	if (dev) {
 
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index 52ec0959d462..7f8472cc993b 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -73,7 +73,7 @@ static void uvesafb_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *ns
 	struct uvesafb_task *utask;
 	struct uvesafb_ktask *task;
 
-	if (!cap_raised(nsp->eff_cap, CAP_SYS_ADMIN))
+	if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
 		return;
 
 	if (msg->seq >= UVESAFB_TASKS_MAX)
@@ -1552,8 +1552,7 @@ static void __devinit uvesafb_init_mtrr(struct fb_info *info)
 			int rc;
 
 			/* Find the largest power-of-two */
-			while (temp_size & (temp_size - 1))
-				temp_size &= (temp_size - 1);
+			temp_size = roundup_pow_of_two(temp_size);
 
 			/* Try and find a power of two to add */
 			do {
@@ -1566,6 +1565,28 @@ static void __devinit uvesafb_init_mtrr(struct fb_info *info)
 #endif /* CONFIG_MTRR */
 }
 
+static void __devinit uvesafb_ioremap(struct fb_info *info)
+{
+#ifdef CONFIG_X86
+	switch (mtrr) {
+	case 1: /* uncachable */
+		info->screen_base = ioremap_nocache(info->fix.smem_start, info->fix.smem_len);
+		break;
+	case 2: /* write-back */
+		info->screen_base = ioremap_cache(info->fix.smem_start, info->fix.smem_len);
+		break;
+	case 3: /* write-combining */
+		info->screen_base = ioremap_wc(info->fix.smem_start, info->fix.smem_len);
+		break;
+	case 4: /* write-through */
+	default:
+		info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+		break;
+	}
+#else
+	info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+#endif /* CONFIG_X86 */
+}
 
 static ssize_t uvesafb_show_vbe_ver(struct device *dev,
 		struct device_attribute *attr, char *buf)
@@ -1736,15 +1757,22 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 
 	uvesafb_init_info(info, mode);
 
+	if (!request_region(0x3c0, 32, "uvesafb")) {
+		printk(KERN_ERR "uvesafb: request region 0x3c0-0x3e0 failed\n");
+		err = -EIO;
+		goto out_mode;
+	}
+
 	if (!request_mem_region(info->fix.smem_start, info->fix.smem_len,
 				"uvesafb")) {
 		printk(KERN_ERR "uvesafb: cannot reserve video memory at "
 				"0x%lx\n", info->fix.smem_start);
 		err = -EIO;
-		goto out_mode;
+		goto out_reg;
 	}
 
-	info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+	uvesafb_init_mtrr(info);
+	uvesafb_ioremap(info);
 
 	if (!info->screen_base) {
 		printk(KERN_ERR
@@ -1755,20 +1783,13 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 		goto out_mem;
 	}
 
-	if (!request_region(0x3c0, 32, "uvesafb")) {
-		printk(KERN_ERR "uvesafb: request region 0x3c0-0x3e0 failed\n");
-		err = -EIO;
-		goto out_unmap;
-	}
-
-	uvesafb_init_mtrr(info);
 	platform_set_drvdata(dev, info);
 
 	if (register_framebuffer(info) < 0) {
 		printk(KERN_ERR
 			"uvesafb: failed to register framebuffer device\n");
 		err = -EINVAL;
-		goto out_reg;
+		goto out_unmap;
 	}
 
 	printk(KERN_INFO "uvesafb: framebuffer at 0x%lx, mapped to 0x%p, "
@@ -1785,12 +1806,12 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 
 	return 0;
 
-out_reg:
-	release_region(0x3c0, 32);
 out_unmap:
 	iounmap(info->screen_base);
 out_mem:
 	release_mem_region(info->fix.smem_start, info->fix.smem_len);
+out_reg:
+	release_region(0x3c0, 32);
 out_mode:
 	if (!list_empty(&info->modelist))
 		fb_destroy_modelist(&info->modelist);
diff --git a/drivers/video/vermilion/vermilion.c b/drivers/video/vermilion/vermilion.c
index 931a567f9aff..970e43d13f52 100644
--- a/drivers/video/vermilion/vermilion.c
+++ b/drivers/video/vermilion/vermilion.c
@@ -891,8 +891,7 @@ static int vmlfb_set_par(struct fb_info *info)
 	int ret;
 
 	mutex_lock(&vml_mutex);
-	list_del(&vinfo->head);
-	list_add(&vinfo->head, (subsys) ? &global_has_mode : &global_no_mode);
+	list_move(&vinfo->head, (subsys) ? &global_has_mode : &global_no_mode);
 	ret = vmlfb_set_par_locked(vinfo);
 
 	mutex_unlock(&vml_mutex);
diff --git a/drivers/video/vermilion/vermilion.h b/drivers/video/vermilion/vermilion.h
index 7491abfcf1fc..43d11ec197fc 100644
--- a/drivers/video/vermilion/vermilion.h
+++ b/drivers/video/vermilion/vermilion.h
@@ -31,7 +31,7 @@
 
 #include <linux/kernel.h>
 #include <linux/pci.h>
-#include <asm/atomic.h>
+#include <linux/atomic.h>
 #include <linux/mutex.h>
 
 #define VML_DEVICE_GPU 0x5002
diff --git a/drivers/video/vesafb.c b/drivers/video/vesafb.c
index 6a069d047914..501b3406c6d5 100644
--- a/drivers/video/vesafb.c
+++ b/drivers/video/vesafb.c
@@ -175,6 +175,7 @@ static int vesafb_setcolreg(unsigned regno, unsigned red, unsigned green,
 
 static void vesafb_destroy(struct fb_info *info)
 {
+	fb_dealloc_cmap(&info->cmap);
 	if (info->screen_base)
 		iounmap(info->screen_base);
 	release_mem_region(info->apertures->ranges[0].base, info->apertures->ranges[0].size);
@@ -303,19 +304,6 @@ static int __init vesafb_probe(struct platform_device *dev)
 	info->apertures->ranges[0].base = screen_info.lfb_base;
 	info->apertures->ranges[0].size = size_total;
 
-	info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
-	if (!info->screen_base) {
-		printk(KERN_ERR
-		       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
-			vesafb_fix.smem_len, vesafb_fix.smem_start);
-		err = -EIO;
-		goto err;
-	}
-
-	printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, "
-	       "using %dk, total %dk\n",
-	       vesafb_fix.smem_start, info->screen_base,
-	       size_remap/1024, size_total/1024);
 	printk(KERN_INFO "vesafb: mode is %dx%dx%d, linelength=%d, pages=%d\n",
 	       vesafb_defined.xres, vesafb_defined.yres, vesafb_defined.bits_per_pixel, vesafb_fix.line_length, screen_info.pages);
 
@@ -438,8 +426,7 @@ static int __init vesafb_probe(struct platform_device *dev)
 			int rc;
 
 			/* Find the largest power-of-two */
-			while (temp_size & (temp_size - 1))
-				temp_size &= (temp_size - 1);
+			temp_size = roundup_pow_of_two(temp_size);
 
 			/* Try and find a power of two to add */
 			do {
@@ -451,6 +438,34 @@ static int __init vesafb_probe(struct platform_device *dev)
 	}
 #endif
 	
+	switch (mtrr) {
+	case 1: /* uncachable */
+		info->screen_base = ioremap_nocache(vesafb_fix.smem_start, vesafb_fix.smem_len);
+		break;
+	case 2: /* write-back */
+		info->screen_base = ioremap_cache(vesafb_fix.smem_start, vesafb_fix.smem_len);
+		break;
+	case 3: /* write-combining */
+		info->screen_base = ioremap_wc(vesafb_fix.smem_start, vesafb_fix.smem_len);
+		break;
+	case 4: /* write-through */
+	default:
+		info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
+		break;
+	}
+	if (!info->screen_base) {
+		printk(KERN_ERR
+		       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
+			vesafb_fix.smem_len, vesafb_fix.smem_start);
+		err = -EIO;
+		goto err;
+	}
+
+	printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, "
+	       "using %dk, total %dk\n",
+	       vesafb_fix.smem_start, info->screen_base,
+	       size_remap/1024, size_total/1024);
+
 	info->fbops = &vesafb_ops;
 	info->var = vesafb_defined;
 	info->fix = vesafb_fix;
diff --git a/drivers/video/vga16fb.c b/drivers/video/vga16fb.c
index 28ccab44a391..305c975b1787 100644
--- a/drivers/video/vga16fb.c
+++ b/drivers/video/vga16fb.c
@@ -152,7 +152,7 @@ static inline int setop(int op)
 }
 
 /* Set the Enable Set/Reset Register and return its old value.  
-   The code here always uses value 0xf for thsi register. */
+   The code here always uses value 0xf for this register. */
 static inline int setsr(int sr)
 {
 	int oldsr;
@@ -1265,9 +1265,11 @@ static void vga16fb_imageblit(struct fb_info *info, const struct fb_image *image
 
 static void vga16fb_destroy(struct fb_info *info)
 {
+	struct platform_device *dev = container_of(info->device, struct platform_device, dev);
 	iounmap(info->screen_base);
 	fb_dealloc_cmap(&info->cmap);
 	/* XXX unshare VGA regions */
+	platform_set_drvdata(dev, NULL);
 	framebuffer_release(info);
 }
 
diff --git a/drivers/video/via/via-gpio.c b/drivers/video/via/via-gpio.c
index c2a0a1cfd3b3..ab5341814c74 100644
--- a/drivers/video/via/via-gpio.c
+++ b/drivers/video/via/via-gpio.c
@@ -145,7 +145,7 @@ static int via_gpio_get(struct gpio_chip *chip, unsigned int nr)
 }
 
 
-static struct viafb_gpio_cfg gpio_config = {
+static struct viafb_gpio_cfg viafb_gpio_config = {
 	.gpio_chip = {
 		.label = "VIAFB onboard GPIO",
 		.owner = THIS_MODULE,
@@ -183,8 +183,8 @@ static int viafb_gpio_resume(void *private)
 {
 	int i;
 
-	for (i = 0; i < gpio_config.gpio_chip.ngpio; i += 2)
-		viafb_gpio_enable(gpio_config.active_gpios[i]);
+	for (i = 0; i < viafb_gpio_config.gpio_chip.ngpio; i += 2)
+		viafb_gpio_enable(viafb_gpio_config.active_gpios[i]);
 	return 0;
 }
 
@@ -201,9 +201,9 @@ int viafb_gpio_lookup(const char *name)
 {
 	int i;
 
-	for (i = 0; i < gpio_config.gpio_chip.ngpio; i++)
-		if (!strcmp(name, gpio_config.active_gpios[i]->vg_name))
-			return gpio_config.gpio_chip.base + i;
+	for (i = 0; i < viafb_gpio_config.gpio_chip.ngpio; i++)
+		if (!strcmp(name, viafb_gpio_config.active_gpios[i]->vg_name))
+			return viafb_gpio_config.gpio_chip.base + i;
 	return -1;
 }
 EXPORT_SYMBOL_GPL(viafb_gpio_lookup);
@@ -229,14 +229,15 @@ static __devinit int viafb_gpio_probe(struct platform_device *platdev)
 		for (gpio = viafb_all_gpios;
 		     gpio < viafb_all_gpios + VIAFB_NUM_GPIOS; gpio++)
 			if (gpio->vg_port_index == port_cfg[i].ioport_index) {
-				gpio_config.active_gpios[ngpio] = gpio;
-				gpio_config.gpio_names[ngpio] = gpio->vg_name;
+				viafb_gpio_config.active_gpios[ngpio] = gpio;
+				viafb_gpio_config.gpio_names[ngpio] =
+					gpio->vg_name;
 				ngpio++;
 			}
 	}
-	gpio_config.gpio_chip.ngpio = ngpio;
-	gpio_config.gpio_chip.names = gpio_config.gpio_names;
-	gpio_config.vdev = vdev;
+	viafb_gpio_config.gpio_chip.ngpio = ngpio;
+	viafb_gpio_config.gpio_chip.names = viafb_gpio_config.gpio_names;
+	viafb_gpio_config.vdev = vdev;
 	if (ngpio == 0) {
 		printk(KERN_INFO "viafb: no GPIOs configured\n");
 		return 0;
@@ -245,18 +246,18 @@ static __devinit int viafb_gpio_probe(struct platform_device *platdev)
 	 * Enable the ports.  They come in pairs, with a single
 	 * enable bit for both.
 	 */
-	spin_lock_irqsave(&gpio_config.vdev->reg_lock, flags);
+	spin_lock_irqsave(&viafb_gpio_config.vdev->reg_lock, flags);
 	for (i = 0; i < ngpio; i += 2)
-		viafb_gpio_enable(gpio_config.active_gpios[i]);
-	spin_unlock_irqrestore(&gpio_config.vdev->reg_lock, flags);
+		viafb_gpio_enable(viafb_gpio_config.active_gpios[i]);
+	spin_unlock_irqrestore(&viafb_gpio_config.vdev->reg_lock, flags);
 	/*
 	 * Get registered.
 	 */
-	gpio_config.gpio_chip.base = -1;  /* Dynamic */
-	ret = gpiochip_add(&gpio_config.gpio_chip);
+	viafb_gpio_config.gpio_chip.base = -1;  /* Dynamic */
+	ret = gpiochip_add(&viafb_gpio_config.gpio_chip);
 	if (ret) {
 		printk(KERN_ERR "viafb: failed to add gpios (%d)\n", ret);
-		gpio_config.gpio_chip.ngpio = 0;
+		viafb_gpio_config.gpio_chip.ngpio = 0;
 	}
 #ifdef CONFIG_PM
 	viafb_pm_register(&viafb_gpio_pm_hooks);
@@ -277,8 +278,8 @@ static int viafb_gpio_remove(struct platform_device *platdev)
 	/*
 	 * Get unregistered.
 	 */
-	if (gpio_config.gpio_chip.ngpio > 0) {
-		ret = gpiochip_remove(&gpio_config.gpio_chip);
+	if (viafb_gpio_config.gpio_chip.ngpio > 0) {
+		ret = gpiochip_remove(&viafb_gpio_config.gpio_chip);
 		if (ret) { /* Somebody still using it? */
 			printk(KERN_ERR "Viafb: GPIO remove failed\n");
 			return ret;
@@ -287,11 +288,11 @@ static int viafb_gpio_remove(struct platform_device *platdev)
 	/*
 	 * Disable the ports.
 	 */
-	spin_lock_irqsave(&gpio_config.vdev->reg_lock, flags);
-	for (i = 0; i < gpio_config.gpio_chip.ngpio; i += 2)
-		viafb_gpio_disable(gpio_config.active_gpios[i]);
-	gpio_config.gpio_chip.ngpio = 0;
-	spin_unlock_irqrestore(&gpio_config.vdev->reg_lock, flags);
+	spin_lock_irqsave(&viafb_gpio_config.vdev->reg_lock, flags);
+	for (i = 0; i < viafb_gpio_config.gpio_chip.ngpio; i += 2)
+		viafb_gpio_disable(viafb_gpio_config.active_gpios[i]);
+	viafb_gpio_config.gpio_chip.ngpio = 0;
+	spin_unlock_irqrestore(&viafb_gpio_config.vdev->reg_lock, flags);
 	return ret;
 }
 
diff --git a/drivers/video/via/via_utility.c b/drivers/video/via/via_utility.c
index d05ccb62b55f..35458a5eadc8 100644
--- a/drivers/video/via/via_utility.c
+++ b/drivers/video/via/via_utility.c
@@ -174,7 +174,7 @@ void viafb_set_gamma_table(int bpp, unsigned int *gamma_table)
 	}
 
 	/* If adjust Gamma value in SAMM, fill IGA1,
-	   IGA2 Gamma table simultanous. */
+	   IGA2 Gamma table simultaneous. */
 	/* Switch to IGA2 Gamma Table */
 	if ((active_device_amount > 1) &&
 		!((viaparinfo->chip_info->gfx_chip_name ==
diff --git a/drivers/video/via/via_utility.h b/drivers/video/via/via_utility.h
index 1670ba82143f..f23be1708c14 100644
--- a/drivers/video/via/via_utility.h
+++ b/drivers/video/via/via_utility.h
@@ -21,7 +21,7 @@
 #ifndef __VIAUTILITY_H__
 #define __VIAUTILITY_H__
 
-/* These functions are used to get infomation about device's state */
+/* These functions are used to get information about device's state */
 void viafb_get_device_support_state(u32 *support_state);
 void viafb_get_device_connect_state(u32 *connect_state);
 bool viafb_lcd_get_support_expand_state(u32 xres, u32 yres);
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index 38567849a9d6..a1c4a39485f9 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -2012,7 +2012,7 @@ static int __init viafb_setup(void)
 int __init viafb_init(void)
 {
 	u32 dummy_x, dummy_y;
-	int r;
+	int r = 0;
 
 	if (machine_is_olpc())
 		/* Apply XO-1.5-specific configuration. */
@@ -2033,9 +2033,9 @@ int __init viafb_init(void)
 		return -EINVAL;
 
 	printk(KERN_INFO
-       "VIA Graphics Intergration Chipset framebuffer %d.%d initializing\n",
+       "VIA Graphics Integration Chipset framebuffer %d.%d initializing\n",
 	       VERSION_MAJOR, VERSION_MINOR);
-	return 0;
+	return r;
 }
 
 void __exit viafb_exit(void)
diff --git a/drivers/video/via/viafbdev.h b/drivers/video/via/viafbdev.h
index 59e40d1bd614..d9440635d1d4 100644
--- a/drivers/video/via/viafbdev.h
+++ b/drivers/video/via/viafbdev.h
@@ -90,9 +90,6 @@ extern int viafb_LCD_ON;
 extern int viafb_DVI_ON;
 extern int viafb_hotplug;
 
-extern int strict_strtoul(const char *cp, unsigned int base,
-	unsigned long *res);
-
 u8 viafb_gpio_i2c_read_lvds(struct lvds_setting_information
 	*plvds_setting_info, struct lvds_chip_information
 	*plvds_chip_info, u8 index);
diff --git a/drivers/video/vt8623fb.c b/drivers/video/vt8623fb.c
index a2965ab92cfb..f9b3e3dc2421 100644
--- a/drivers/video/vt8623fb.c
+++ b/drivers/video/vt8623fb.c
@@ -121,13 +121,19 @@ MODULE_PARM_DESC(mtrr, "Enable write-combining with MTRR (1=enable, 0=disable, d
 
 /* ------------------------------------------------------------------------- */
 
+static void vt8623fb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+	struct vt8623fb_info *par = info->par;
+
+	svga_tilecursor(par->state.vgabase, info, cursor);
+}
 
 static struct fb_tile_ops vt8623fb_tile_ops = {
 	.fb_settile	= svga_settile,
 	.fb_tilecopy	= svga_tilecopy,
 	.fb_tilefill    = svga_tilefill,
 	.fb_tileblit    = svga_tileblit,
-	.fb_tilecursor  = svga_tilecursor,
+	.fb_tilecursor  = vt8623fb_tilecursor,
 	.fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -253,6 +259,7 @@ static void vt8623fb_fillrect(struct fb_info *info, const struct fb_fillrect *re
 
 static void vt8623_set_pixclock(struct fb_info *info, u32 pixclock)
 {
+	struct vt8623fb_info *par = info->par;
 	u16 m, n, r;
 	u8 regval;
 	int rv;
@@ -264,18 +271,18 @@ static void vt8623_set_pixclock(struct fb_info *info, u32 pixclock)
 	}
 
 	/* Set VGA misc register  */
-	regval = vga_r(NULL, VGA_MIS_R);
-	vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+	regval = vga_r(par->state.vgabase, VGA_MIS_R);
+	vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
 	/* Set clock registers */
-	vga_wseq(NULL, 0x46, (n  | (r << 6)));
-	vga_wseq(NULL, 0x47, m);
+	vga_wseq(par->state.vgabase, 0x46, (n  | (r << 6)));
+	vga_wseq(par->state.vgabase, 0x47, m);
 
 	udelay(1000);
 
 	/* PLL reset */
-	svga_wseq_mask(0x40, 0x02, 0x02);
-	svga_wseq_mask(0x40, 0x00, 0x02);
+	svga_wseq_mask(par->state.vgabase, 0x40, 0x02, 0x02);
+	svga_wseq_mask(par->state.vgabase, 0x40, 0x00, 0x02);
 }
 
 
@@ -285,7 +292,10 @@ static int vt8623fb_open(struct fb_info *info, int user)
 
 	mutex_lock(&(par->open_lock));
 	if (par->ref_count == 0) {
+		void __iomem *vgabase = par->state.vgabase;
+
 		memset(&(par->state), 0, sizeof(struct vgastate));
+		par->state.vgabase = vgabase;
 		par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
 		par->state.num_crtc = 0xA2;
 		par->state.num_seq = 0x50;
@@ -373,6 +383,7 @@ static int vt8623fb_check_var(struct fb_var_screeninfo *var, struct fb_info *inf
 static int vt8623fb_set_par(struct fb_info *info)
 {
 	u32 mode, offset_value, fetch_value, screen_size;
+	struct vt8623fb_info *par = info->par;
 	u32 bpp = info->var.bits_per_pixel;
 
 	if (bpp != 0) {
@@ -414,82 +425,82 @@ static int vt8623fb_set_par(struct fb_info *info)
 	info->var.activate = FB_ACTIVATE_NOW;
 
 	/* Unlock registers */
-	svga_wseq_mask(0x10, 0x01, 0x01);
-	svga_wcrt_mask(0x11, 0x00, 0x80);
-	svga_wcrt_mask(0x47, 0x00, 0x01);
+	svga_wseq_mask(par->state.vgabase, 0x10, 0x01, 0x01);
+	svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
+	svga_wcrt_mask(par->state.vgabase, 0x47, 0x00, 0x01);
 
 	/* Device, screen and sync off */
-	svga_wseq_mask(0x01, 0x20, 0x20);
-	svga_wcrt_mask(0x36, 0x30, 0x30);
-	svga_wcrt_mask(0x17, 0x00, 0x80);
+	svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+	svga_wcrt_mask(par->state.vgabase, 0x36, 0x30, 0x30);
+	svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
 	/* Set default values */
-	svga_set_default_gfx_regs();
-	svga_set_default_atc_regs();
-	svga_set_default_seq_regs();
-	svga_set_default_crt_regs();
-	svga_wcrt_multi(vt8623_line_compare_regs, 0xFFFFFFFF);
-	svga_wcrt_multi(vt8623_start_address_regs, 0);
+	svga_set_default_gfx_regs(par->state.vgabase);
+	svga_set_default_atc_regs(par->state.vgabase);
+	svga_set_default_seq_regs(par->state.vgabase);
+	svga_set_default_crt_regs(par->state.vgabase);
+	svga_wcrt_multi(par->state.vgabase, vt8623_line_compare_regs, 0xFFFFFFFF);
+	svga_wcrt_multi(par->state.vgabase, vt8623_start_address_regs, 0);
 
-	svga_wcrt_multi(vt8623_offset_regs, offset_value);
-	svga_wseq_multi(vt8623_fetch_count_regs, fetch_value);
+	svga_wcrt_multi(par->state.vgabase, vt8623_offset_regs, offset_value);
+	svga_wseq_multi(par->state.vgabase, vt8623_fetch_count_regs, fetch_value);
 
 	/* Clear H/V Skew */
-	svga_wcrt_mask(0x03, 0x00, 0x60);
-	svga_wcrt_mask(0x05, 0x00, 0x60);
+	svga_wcrt_mask(par->state.vgabase, 0x03, 0x00, 0x60);
+	svga_wcrt_mask(par->state.vgabase, 0x05, 0x00, 0x60);
 
 	if (info->var.vmode & FB_VMODE_DOUBLE)
-		svga_wcrt_mask(0x09, 0x80, 0x80);
+		svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
 	else
-		svga_wcrt_mask(0x09, 0x00, 0x80);
+		svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
-	svga_wseq_mask(0x1E, 0xF0, 0xF0); // DI/DVP bus
-	svga_wseq_mask(0x2A, 0x0F, 0x0F); // DI/DVP bus
-	svga_wseq_mask(0x16, 0x08, 0xBF); // FIFO read threshold
-	vga_wseq(NULL, 0x17, 0x1F);       // FIFO depth
-	vga_wseq(NULL, 0x18, 0x4E);
-	svga_wseq_mask(0x1A, 0x08, 0x08); // enable MMIO ?
+	svga_wseq_mask(par->state.vgabase, 0x1E, 0xF0, 0xF0); // DI/DVP bus
+	svga_wseq_mask(par->state.vgabase, 0x2A, 0x0F, 0x0F); // DI/DVP bus
+	svga_wseq_mask(par->state.vgabase, 0x16, 0x08, 0xBF); // FIFO read threshold
+	vga_wseq(par->state.vgabase, 0x17, 0x1F);       // FIFO depth
+	vga_wseq(par->state.vgabase, 0x18, 0x4E);
+	svga_wseq_mask(par->state.vgabase, 0x1A, 0x08, 0x08); // enable MMIO ?
 
-	vga_wcrt(NULL, 0x32, 0x00);
-	vga_wcrt(NULL, 0x34, 0x00);
-	vga_wcrt(NULL, 0x6A, 0x80);
-	vga_wcrt(NULL, 0x6A, 0xC0);
+	vga_wcrt(par->state.vgabase, 0x32, 0x00);
+	vga_wcrt(par->state.vgabase, 0x34, 0x00);
+	vga_wcrt(par->state.vgabase, 0x6A, 0x80);
+	vga_wcrt(par->state.vgabase, 0x6A, 0xC0);
 
-	vga_wgfx(NULL, 0x20, 0x00);
-	vga_wgfx(NULL, 0x21, 0x00);
-	vga_wgfx(NULL, 0x22, 0x00);
+	vga_wgfx(par->state.vgabase, 0x20, 0x00);
+	vga_wgfx(par->state.vgabase, 0x21, 0x00);
+	vga_wgfx(par->state.vgabase, 0x22, 0x00);
 
 	/* Set SR15 according to number of bits per pixel */
 	mode = svga_match_format(vt8623fb_formats, &(info->var), &(info->fix));
 	switch (mode) {
 	case 0:
 		pr_debug("fb%d: text mode\n", info->node);
-		svga_set_textmode_vga_regs();
-		svga_wseq_mask(0x15, 0x00, 0xFE);
-		svga_wcrt_mask(0x11, 0x60, 0x70);
+		svga_set_textmode_vga_regs(par->state.vgabase);
+		svga_wseq_mask(par->state.vgabase, 0x15, 0x00, 0xFE);
+		svga_wcrt_mask(par->state.vgabase, 0x11, 0x60, 0x70);
 		break;
 	case 1:
 		pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-		vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
-		svga_wseq_mask(0x15, 0x20, 0xFE);
-		svga_wcrt_mask(0x11, 0x00, 0x70);
+		vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
+		svga_wseq_mask(par->state.vgabase, 0x15, 0x20, 0xFE);
+		svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x70);
 		break;
 	case 2:
 		pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
-		svga_wseq_mask(0x15, 0x00, 0xFE);
-		svga_wcrt_mask(0x11, 0x00, 0x70);
+		svga_wseq_mask(par->state.vgabase, 0x15, 0x00, 0xFE);
+		svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x70);
 		break;
 	case 3:
 		pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-		svga_wseq_mask(0x15, 0x22, 0xFE);
+		svga_wseq_mask(par->state.vgabase, 0x15, 0x22, 0xFE);
 		break;
 	case 4:
 		pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
-		svga_wseq_mask(0x15, 0xB6, 0xFE);
+		svga_wseq_mask(par->state.vgabase, 0x15, 0xB6, 0xFE);
 		break;
 	case 5:
 		pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
-		svga_wseq_mask(0x15, 0xAE, 0xFE);
+		svga_wseq_mask(par->state.vgabase, 0x15, 0xAE, 0xFE);
 		break;
 	default:
 		printk(KERN_ERR "vt8623fb: unsupported mode - bug\n");
@@ -497,16 +508,16 @@ static int vt8623fb_set_par(struct fb_info *info)
 	}
 
 	vt8623_set_pixclock(info, info->var.pixclock);
-	svga_set_timings(&vt8623_timing_regs, &(info->var), 1, 1,
+	svga_set_timings(par->state.vgabase, &vt8623_timing_regs, &(info->var), 1, 1,
 			 (info->var.vmode & FB_VMODE_DOUBLE) ? 2 : 1, 1,
 			 1, info->node);
 
 	memset_io(info->screen_base, 0x00, screen_size);
 
 	/* Device and screen back on */
-	svga_wcrt_mask(0x17, 0x80, 0x80);
-	svga_wcrt_mask(0x36, 0x00, 0x30);
-	svga_wseq_mask(0x01, 0x00, 0x20);
+	svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+	svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+	svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
 	return 0;
 }
@@ -569,31 +580,33 @@ static int vt8623fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int vt8623fb_blank(int blank_mode, struct fb_info *info)
 {
+	struct vt8623fb_info *par = info->par;
+
 	switch (blank_mode) {
 	case FB_BLANK_UNBLANK:
 		pr_debug("fb%d: unblank\n", info->node);
-		svga_wcrt_mask(0x36, 0x00, 0x30);
-		svga_wseq_mask(0x01, 0x00, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 		break;
 	case FB_BLANK_NORMAL:
 		pr_debug("fb%d: blank\n", info->node);
-		svga_wcrt_mask(0x36, 0x00, 0x30);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	case FB_BLANK_HSYNC_SUSPEND:
 		pr_debug("fb%d: DPMS standby (hsync off)\n", info->node);
-		svga_wcrt_mask(0x36, 0x10, 0x30);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x36, 0x10, 0x30);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	case FB_BLANK_VSYNC_SUSPEND:
 		pr_debug("fb%d: DPMS suspend (vsync off)\n", info->node);
-		svga_wcrt_mask(0x36, 0x20, 0x30);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x36, 0x20, 0x30);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	case FB_BLANK_POWERDOWN:
 		pr_debug("fb%d: DPMS off (no sync)\n", info->node);
-		svga_wcrt_mask(0x36, 0x30, 0x30);
-		svga_wseq_mask(0x01, 0x20, 0x20);
+		svga_wcrt_mask(par->state.vgabase, 0x36, 0x30, 0x30);
+		svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
 		break;
 	}
 
@@ -603,6 +616,7 @@ static int vt8623fb_blank(int blank_mode, struct fb_info *info)
 
 static int vt8623fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+	struct vt8623fb_info *par = info->par;
 	unsigned int offset;
 
 	/* Calculate the offset */
@@ -616,7 +630,7 @@ static int vt8623fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *i
 	}
 
 	/* Set the offset */
-	svga_wcrt_multi(vt8623_start_address_regs, offset);
+	svga_wcrt_multi(par->state.vgabase, vt8623_start_address_regs, offset);
 
 	return 0;
 }
@@ -647,6 +661,8 @@ static struct fb_ops vt8623fb_ops = {
 
 static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+	struct pci_bus_region bus_reg;
+	struct resource vga_res;
 	struct fb_info *info;
 	struct vt8623fb_info *par;
 	unsigned int memsize1, memsize2;
@@ -705,9 +721,18 @@ static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_devi
 		goto err_iomap_2;
 	}
 
+	bus_reg.start = 0;
+	bus_reg.end = 64 * 1024;
+
+	vga_res.flags = IORESOURCE_IO;
+
+	pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+	par->state.vgabase = (void __iomem *) vga_res.start;
+
 	/* Find how many physical memory there is on card */
-	memsize1 = (vga_rseq(NULL, 0x34) + 1) >> 1;
-	memsize2 = vga_rseq(NULL, 0x39) << 2;
+	memsize1 = (vga_rseq(par->state.vgabase, 0x34) + 1) >> 1;
+	memsize2 = vga_rseq(par->state.vgabase, 0x39) << 2;
 
 	if ((16 <= memsize1) && (memsize1 <= 64) && (memsize1 == memsize2))
 		info->screen_size = memsize1 << 20;
diff --git a/drivers/video/w100fb.c b/drivers/video/w100fb.c
index d8b12c32e3ef..c8be8af0cc6d 100644
--- a/drivers/video/w100fb.c
+++ b/drivers/video/w100fb.c
@@ -1306,7 +1306,7 @@ static void w100_init_lcd(struct w100fb_par *par)
 	union graphic_v_disp_u graphic_v_disp;
 	union crtc_total_u crtc_total;
 
-	/* w3200 doesnt like undefined bits being set so zero register values first */
+	/* w3200 doesn't like undefined bits being set so zero register values first */
 
 	active_h_disp.val = 0;
 	active_h_disp.f.active_h_start=mode->left_margin;
diff --git a/drivers/video/xen-fbfront.c b/drivers/video/xen-fbfront.c
index a20218c2fda8..beac52fc1c0e 100644
--- a/drivers/video/xen-fbfront.c
+++ b/drivers/video/xen-fbfront.c
@@ -395,10 +395,9 @@ static int __devinit xenfb_probe(struct xenbus_device *dev,
 	spin_lock_init(&info->dirty_lock);
 	spin_lock_init(&info->resize_lock);
 
-	info->fb = vmalloc(fb_size);
+	info->fb = vzalloc(fb_size);
 	if (info->fb == NULL)
 		goto error_nomem;
-	memset(info->fb, 0, fb_size);
 
 	info->nr_pages = (fb_size + PAGE_SIZE - 1) >> PAGE_SHIFT;
 
diff --git a/drivers/video/xilinxfb.c b/drivers/video/xilinxfb.c
index 68bd23476c64..77dea015ff69 100644
--- a/drivers/video/xilinxfb.c
+++ b/drivers/video/xilinxfb.c
@@ -404,8 +404,7 @@ static int xilinxfb_release(struct device *dev)
  * OF bus binding
  */
 
-static int __devinit
-xilinxfb_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit xilinxfb_of_probe(struct platform_device *op)
 {
 	const u32 *prop;
 	u32 *p;
@@ -418,8 +417,6 @@ xilinxfb_of_probe(struct platform_device *op, const struct of_device_id *match)
 	/* Copy with the default pdata (not a ptr reference!) */
 	pdata = xilinx_fb_default_pdata;
 
-	dev_dbg(&op->dev, "xilinxfb_of_probe(%p, %p)\n", op, match);
-
 	/* Allocate the driver data region */
 	drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
 	if (!drvdata) {
@@ -505,7 +502,7 @@ static struct of_device_id xilinxfb_of_match[] __devinitdata = {
 };
 MODULE_DEVICE_TABLE(of, xilinxfb_of_match);
 
-static struct of_platform_driver xilinxfb_of_driver = {
+static struct platform_driver xilinxfb_of_driver = {
 	.probe = xilinxfb_of_probe,
 	.remove = __devexit_p(xilinxfb_of_remove),
 	.driver = {
@@ -523,13 +520,13 @@ static struct of_platform_driver xilinxfb_of_driver = {
 static int __init
 xilinxfb_init(void)
 {
-	return of_register_platform_driver(&xilinxfb_of_driver);
+	return platform_driver_register(&xilinxfb_of_driver);
 }
 
 static void __exit
 xilinxfb_cleanup(void)
 {
-	of_unregister_platform_driver(&xilinxfb_of_driver);
+	platform_driver_unregister(&xilinxfb_of_driver);
 }
 
 module_init(xilinxfb_init);