103 files changed, 5655 insertions, 1364 deletions

diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 162a0b7ee4c2..27c1fb4b1e0d 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -17,6 +17,8 @@ source "drivers/gpu/vga/Kconfig"
 
 source "drivers/gpu/drm/Kconfig"
 
+source "drivers/gpu/stub/Kconfig"
+
 config VGASTATE
        tristate
        default n
@@ -49,7 +51,7 @@ menuconfig FB
           You need an utility program called fbset to make full use of frame
           buffer devices. Please read <file:Documentation/fb/framebuffer.txt>
           and the Framebuffer-HOWTO at
-          <http://www.munted.org.uk/programming/Framebuffer-HOWTO-1.2.html> for more
+          <http://www.munted.org.uk/programming/Framebuffer-HOWTO-1.3.html> for more
           information.
 
           Say Y here and to the driver for your graphics board below if you
@@ -955,7 +957,7 @@ config FB_EPSON1355
           Build in support for the SED1355 Epson Research Embedded RAMDAC
           LCD/CRT Controller (since redesignated as the S1D13505) as a
           framebuffer.  Product specs at
-          <http://www.erd.epson.com/vdc/html/products.htm>.
+          <http://vdc.epson.com/>.
 
 config FB_S1D13XXX
         tristate "Epson S1D13XXX framebuffer support"
@@ -966,7 +968,7 @@ config FB_S1D13XXX
         help
           Support for S1D13XXX framebuffer device family (currently only
           working with S1D13806). Product specs at
-          <http://www.erd.epson.com/vdc/html/legacy_13xxx.htm>
+          <http://vdc.epson.com/>
 
 config FB_ATMEL
         tristate "AT91/AT32 LCD Controller support"
@@ -1323,7 +1325,7 @@ config FB_RADEON
           don't need to choose this to run the Radeon in plain VGA mode.
 
           There is a product page at
-          http://apps.ati.com/ATIcompare/
+          http://products.amd.com/en-us/GraphicCardResult.aspx
 
 config FB_RADEON_I2C
         bool "DDC/I2C for ATI Radeon support"
@@ -1395,7 +1397,7 @@ config FB_ATY_CT
           Say Y here to support use of ATI's 64-bit Rage boards (or other
           boards based on the Mach64 CT, VT, GT, and LT chipsets) as a
           framebuffer device.  The ATI product support page for these boards
-          is at <http://support.ati.com/products/pc/mach64/>.
+          is at <http://support.ati.com/products/pc/mach64/mach64.html>.
 
 config FB_ATY_GENERIC_LCD
         bool "Mach64 generic LCD support (EXPERIMENTAL)"
@@ -1871,6 +1873,7 @@ config FB_MBX_DEBUG
 config FB_FSL_DIU
         tristate "Freescale DIU framebuffer support"
         depends on FB && FSL_SOC
+        select FB_MODE_HELPERS
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
@@ -1895,6 +1898,13 @@ config FB_W100
 
           If unsure, say N.
 
+config SH_MIPI_DSI
+        tristate
+        depends on (SUPERH || ARCH_SHMOBILE) && HAVE_CLK
+
+config SH_LCD_MIPI_DSI
+        bool
+
 config FB_SH_MOBILE_LCDC
         tristate "SuperH Mobile LCDC framebuffer support"
         depends on FB && (SUPERH || ARCH_SHMOBILE) && HAVE_CLK
@@ -1903,9 +1913,20 @@ config FB_SH_MOBILE_LCDC
         select FB_SYS_IMAGEBLIT
         select FB_SYS_FOPS
         select FB_DEFERRED_IO
+        select SH_MIPI_DSI if SH_LCD_MIPI_DSI
         ---help---
           Frame buffer driver for the on-chip SH-Mobile LCD controller.
 
+config FB_SH_MOBILE_HDMI
+        tristate "SuperH Mobile HDMI controller support"
+        depends on FB_SH_MOBILE_LCDC
+        select FB_MODE_HELPERS
+        select SOUND
+        select SND
+        select SND_SOC
+        ---help---
+          Driver for the on-chip SH-Mobile HDMI controller.
+
 config FB_TMIO
         tristate "Toshiba Mobile IO FrameBuffer support"
         depends on FB && MFD_CORE
@@ -1930,7 +1951,7 @@ config FB_TMIO_ACCELL
 
 config FB_S3C
         tristate "Samsung S3C framebuffer support"
-        depends on FB && ARCH_S3C64XX
+        depends on FB && S3C_DEV_FB
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
@@ -2229,6 +2250,15 @@ config FB_BROADSHEET
           and could also have been called by other names when coupled with
           a bridge adapter.
 
+config FB_JZ4740
+        tristate "JZ4740 LCD framebuffer support"
+        depends on FB && MACH_JZ4740
+        select FB_SYS_FILLRECT
+        select FB_SYS_COPYAREA
+        select FB_SYS_IMAGEBLIT
+        help
+          Framebuffer support for the JZ4740 SoC.
+
 source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
 
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index ddc2af2ba45b..485e8ed1318c 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -123,6 +123,8 @@ obj-$(CONFIG_FB_IBM_GXT4500)	  += gxt4500.o
 obj-$(CONFIG_FB_PS3)              += ps3fb.o
 obj-$(CONFIG_FB_SM501)            += sm501fb.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_LCDC)   += sh_mobile_lcdcfb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
 obj-y                             += omap2/
@@ -131,6 +133,7 @@ obj-$(CONFIG_FB_CARMINE)          += carminefb.o
 obj-$(CONFIG_FB_MB862XX)          += mb862xx/
 obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
+obj-$(CONFIG_FB_JZ4740)           += jz4740_fb.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
diff --git a/drivers/video/amba-clcd.c b/drivers/video/amba-clcd.c
index afe21e6eb544..1c2c68356ea7 100644
--- a/drivers/video/amba-clcd.c
+++ b/drivers/video/amba-clcd.c
@@ -80,7 +80,10 @@ static void clcdfb_disable(struct clcd_fb *fb)
         /*
          * Disable CLCD clock source.
          */
-        clk_disable(fb->clk);
+        if (fb->clk_enabled) {
+                fb->clk_enabled = false;
+                clk_disable(fb->clk);
+        }
 }
 
 static void clcdfb_enable(struct clcd_fb *fb, u32 cntl)
@@ -88,7 +91,10 @@ static void clcdfb_enable(struct clcd_fb *fb, u32 cntl)
         /*
          * Enable the CLCD clock source.
          */
-        clk_enable(fb->clk);
+        if (!fb->clk_enabled) {
+                fb->clk_enabled = true;
+                clk_enable(fb->clk);
+        }
 
         /*
          * Bring up by first enabling..
diff --git a/drivers/video/arcfb.c b/drivers/video/arcfb.c
index f3d7440f0072..3ec4923c2d84 100644
--- a/drivers/video/arcfb.c
+++ b/drivers/video/arcfb.c
@@ -2,7 +2,6 @@
  * linux/drivers/video/arcfb.c -- FB driver for Arc monochrome LCD board
  *
  * Copyright (C) 2005, Jaya Kumar <jayalk@intworks.biz>
- * http://www.intworks.biz/arclcd
  *
  * 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
diff --git a/drivers/video/atafb.c b/drivers/video/atafb.c
index f3aada20fa02..5b2b5ef4edba 100644
--- a/drivers/video/atafb.c
+++ b/drivers/video/atafb.c
@@ -1718,11 +1718,9 @@ static int falcon_setcolreg(unsigned int regno, unsigned int red,
                         (((red & 0xe000) >> 13) | ((red & 0x1000) >> 12) << 8) |
                         (((green & 0xe000) >> 13) | ((green & 0x1000) >> 12) << 4) |
                         ((blue & 0xe000) >> 13) | ((blue & 0x1000) >> 12);
-#ifdef ATAFB_FALCON
                 ((u32 *)info->pseudo_palette)[regno] = ((red & 0xf800) |
                                                        ((green & 0xfc00) >> 5) |
                                                        ((blue & 0xf800) >> 11));
-#endif
         }
         return 0;
 }
diff --git a/drivers/video/aty/atyfb_base.c b/drivers/video/aty/atyfb_base.c
index f8d69ad36830..5bf91236c701 100644
--- a/drivers/video/aty/atyfb_base.c
+++ b/drivers/video/aty/atyfb_base.c
@@ -2970,7 +2970,8 @@ static int __devinit atyfb_setup_sparc(struct pci_dev *pdev,
         struct atyfb_par *par = info->par;
         struct device_node *dp;
         char prop[128];
-        int node, len, i, j, ret;
+        phandle node;
+        int len, i, j, ret;
         u32 mem, chip_id;
 
         /*
diff --git a/drivers/video/aty/radeon_i2c.c b/drivers/video/aty/radeon_i2c.c
index 359fc64e761a..78d1f4cd1fe0 100644
--- a/drivers/video/aty/radeon_i2c.c
+++ b/drivers/video/aty/radeon_i2c.c
@@ -7,7 +7,6 @@
 
 
 #include <linux/i2c.h>
-#include <linux/i2c-id.h>
 #include <linux/i2c-algo-bit.h>
 
 #include <asm/io.h>
diff --git a/drivers/video/au1100fb.c b/drivers/video/au1100fb.c
index 40f61320ce16..34b2fc472fe8 100644
--- a/drivers/video/au1100fb.c
+++ b/drivers/video/au1100fb.c
@@ -95,7 +95,7 @@ struct fb_bitfield rgb_bitfields[][4] =
         { { 8, 4, 0 },  { 4, 4, 0 }, { 0, 4, 0 }, { 0, 0, 0 } },
 };
 
-static struct fb_fix_screeninfo au1100fb_fix __initdata = {
+static struct fb_fix_screeninfo au1100fb_fix __devinitdata = {
         .id             = "AU1100 FB",
         .xpanstep       = 1,
         .ypanstep       = 1,
@@ -103,7 +103,7 @@ static struct fb_fix_screeninfo au1100fb_fix __initdata = {
         .accel          = FB_ACCEL_NONE,
 };
 
-static struct fb_var_screeninfo au1100fb_var __initdata = {
+static struct fb_var_screeninfo au1100fb_var __devinitdata = {
         .activate       = FB_ACTIVATE_NOW,
         .height         = -1,
         .width          = -1,
@@ -458,7 +458,7 @@ static struct fb_ops au1100fb_ops =
 
 /* AU1100 LCD controller device driver */
 
-static int __init au1100fb_drv_probe(struct platform_device *dev)
+static int __devinit au1100fb_drv_probe(struct platform_device *dev)
 {
         struct au1100fb_device *fbdev = NULL;
         struct resource *regs_res;
diff --git a/drivers/video/au1200fb.c b/drivers/video/au1200fb.c
index e77e8e4280fb..4ea187d93768 100644
--- a/drivers/video/au1200fb.c
+++ b/drivers/video/au1200fb.c
@@ -1079,7 +1079,7 @@ static int au1200fb_fb_check_var(struct fb_var_screeninfo *var,
          * clock can only be obtain by dividing this value by an even integer.
          * Fallback to a slower pixel clock if necessary. */
         pixclock = max((u32)(PICOS2KHZ(var->pixclock) * 1000), fbi->monspecs.dclkmin);
-        pixclock = min(pixclock, min(fbi->monspecs.dclkmax, (u32)AU1200_LCD_MAX_CLK/2));
+        pixclock = min3(pixclock, fbi->monspecs.dclkmax, (u32)AU1200_LCD_MAX_CLK/2);
 
         if (AU1200_LCD_MAX_CLK % pixclock) {
                 int diff = AU1200_LCD_MAX_CLK % pixclock;
diff --git a/drivers/video/backlight/locomolcd.c b/drivers/video/backlight/locomolcd.c
index 7571bc26071e..d2f59015d517 100644
--- a/drivers/video/backlight/locomolcd.c
+++ b/drivers/video/backlight/locomolcd.c
@@ -2,7 +2,7 @@
  * Backlight control code for Sharp Zaurus SL-5500
  *
  * Copyright 2005 John Lenz <lenz@cs.wisc.edu>
- * Maintainer: Pavel Machek <pavel@suse.cz> (unless John wants to :-)
+ * Maintainer: Pavel Machek <pavel@ucw.cz> (unless John wants to :-)
  * GPL v2
  *
  * This driver assumes single CPU. That's okay, because collie is
@@ -246,6 +246,6 @@ static void __exit locomolcd_exit(void)
 module_init(locomolcd_init);
 module_exit(locomolcd_exit);
 
-MODULE_AUTHOR("John Lenz <lenz@cs.wisc.edu>, Pavel Machek <pavel@suse.cz>");
+MODULE_AUTHOR("John Lenz <lenz@cs.wisc.edu>, Pavel Machek <pavel@ucw.cz>");
 MODULE_DESCRIPTION("Collie LCD driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index b020ba7f1cf2..e7d0f525041e 100644
--- a/drivers/video/bf54x-lq043fb.c
+++ b/drivers/video/bf54x-lq043fb.c
@@ -241,12 +241,12 @@ static int request_ports(struct bfin_bf54xfb_info *fbi)
         u16 disp = fbi->mach_info->disp;
 
         if (gpio_request(disp, DRIVER_NAME)) {
-                printk(KERN_ERR "Requesting GPIO %d faild\n", disp);
+                printk(KERN_ERR "Requesting GPIO %d failed\n", disp);
                 return -EFAULT;
         }
 
         if (peripheral_request_list(eppi_req_18, DRIVER_NAME)) {
-                printk(KERN_ERR "Requesting Peripherals faild\n");
+                printk(KERN_ERR "Requesting Peripherals failed\n");
                 gpio_free(disp);
                 return -EFAULT;
         }
@@ -256,7 +256,7 @@ static int request_ports(struct bfin_bf54xfb_info *fbi)
                 u16 eppi_req_24[] = EPPI0_24;
 
                 if (peripheral_request_list(eppi_req_24, DRIVER_NAME)) {
-                        printk(KERN_ERR "Requesting Peripherals faild\n");
+                        printk(KERN_ERR "Requesting Peripherals failed\n");
                         peripheral_free_list(eppi_req_18);
                         gpio_free(disp);
                         return -EFAULT;
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 7a50272eaab9..3cf77676947c 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -192,7 +192,7 @@ static int bfin_t350mcqb_request_ports(int action)
 {
         if (action) {
                 if (peripheral_request_list(ppi0_req_8, DRIVER_NAME)) {
-                        printk(KERN_ERR "Requesting Peripherals faild\n");
+                        printk(KERN_ERR "Requesting Peripherals failed\n");
                         return -EFAULT;
                 }
         } else
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 09f1b9b462f4..4dc13467281d 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 of_device *op, const struct of_device_id *match)
+static int __devinit bw2_probe(struct platform_device *op, const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -350,7 +350,7 @@ out_err:
         return err;
 }
 
-static int __devexit bw2_remove(struct of_device *op)
+static int __devexit bw2_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct bw2_par *par = info->par;
@@ -390,12 +390,12 @@ static int __init bw2_init(void)
         if (fb_get_options("bw2fb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&bw2_driver, &of_bus_type);
+        return of_register_platform_driver(&bw2_driver);
 }
 
 static void __exit bw2_exit(void)
 {
-        of_unregister_driver(&bw2_driver);
+        of_unregister_platform_driver(&bw2_driver);
 }
 
 module_init(bw2_init);
diff --git a/drivers/video/carminefb.c b/drivers/video/carminefb.c
index d8345fcc4fe3..6b19136aa181 100644
--- a/drivers/video/carminefb.c
+++ b/drivers/video/carminefb.c
@@ -432,7 +432,7 @@ static int init_hardware(struct carmine_hw *hw)
         u32 loops;
         u32 ret;
 
-        /* Initalize Carmine */
+        /* Initialize Carmine */
         /* Sets internal clock */
         c_set_hw_reg(hw, CARMINE_CTL_REG + CARMINE_CTL_REG_CLOCK_ENABLE,
                         CARMINE_DFLT_IP_CLOCK_ENABLE);
diff --git a/drivers/video/cg14.c b/drivers/video/cg14.c
index e5dc2241194f..24249535ac86 100644
--- a/drivers/video/cg14.c
+++ b/drivers/video/cg14.c
@@ -446,7 +446,7 @@ static struct sbus_mmap_map __cg14_mmap_map[CG14_MMAP_ENTRIES] __devinitdata = {
         { .size = 0 }
 };
 
-static void cg14_unmap_regs(struct of_device *op, struct fb_info *info,
+static void cg14_unmap_regs(struct platform_device *op, struct fb_info *info,
                             struct cg14_par *par)
 {
         if (par->regs)
@@ -463,7 +463,7 @@ static void cg14_unmap_regs(struct of_device *op, struct fb_info *info,
                            info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit cg14_probe(struct of_device *op, const struct of_device_id *match)
+static int __devinit cg14_probe(struct platform_device *op, const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -570,7 +570,7 @@ out_err:
         return err;
 }
 
-static int __devexit cg14_remove(struct of_device *op)
+static int __devexit cg14_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct cg14_par *par = info->par;
@@ -610,12 +610,12 @@ static int __init cg14_init(void)
         if (fb_get_options("cg14fb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&cg14_driver, &of_bus_type);
+        return of_register_platform_driver(&cg14_driver);
 }
 
 static void __exit cg14_exit(void)
 {
-        of_unregister_driver(&cg14_driver);
+        of_unregister_platform_driver(&cg14_driver);
 }
 
 module_init(cg14_init);
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index 558d73a948a0..09c0c3c42482 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -346,7 +346,7 @@ static int __devinit cg3_do_default_mode(struct cg3_par *par)
         return 0;
 }
 
-static int __devinit cg3_probe(struct of_device *op,
+static int __devinit cg3_probe(struct platform_device *op,
                                const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
@@ -433,7 +433,7 @@ out_err:
         return err;
 }
 
-static int __devexit cg3_remove(struct of_device *op)
+static int __devexit cg3_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct cg3_par *par = info->par;
@@ -477,12 +477,12 @@ static int __init cg3_init(void)
         if (fb_get_options("cg3fb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&cg3_driver, &of_bus_type);
+        return of_register_platform_driver(&cg3_driver);
 }
 
 static void __exit cg3_exit(void)
 {
-        of_unregister_driver(&cg3_driver);
+        of_unregister_platform_driver(&cg3_driver);
 }
 
 module_init(cg3_init);
diff --git a/drivers/video/cg6.c b/drivers/video/cg6.c
index 480d761a27a8..2b5a97058b08 100644
--- a/drivers/video/cg6.c
+++ b/drivers/video/cg6.c
@@ -718,7 +718,7 @@ static void __devinit cg6_chip_init(struct fb_info *info)
         sbus_writel(info->var.yres - 1, &fbc->clipmaxy);
 }
 
-static void cg6_unmap_regs(struct of_device *op, struct fb_info *info,
+static void cg6_unmap_regs(struct platform_device *op, struct fb_info *info,
                            struct cg6_par *par)
 {
         if (par->fbc)
@@ -737,7 +737,7 @@ static void cg6_unmap_regs(struct of_device *op, struct fb_info *info,
                            info->fix.smem_len);
 }
 
-static int __devinit cg6_probe(struct of_device *op,
+static int __devinit cg6_probe(struct platform_device *op,
                                 const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
@@ -827,7 +827,7 @@ out_err:
         return err;
 }
 
-static int __devexit cg6_remove(struct of_device *op)
+static int __devexit cg6_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct cg6_par *par = info->par;
@@ -870,12 +870,12 @@ static int __init cg6_init(void)
         if (fb_get_options("cg6fb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&cg6_driver, &of_bus_type);
+        return of_register_platform_driver(&cg6_driver);
 }
 
 static void __exit cg6_exit(void)
 {
-        of_unregister_driver(&cg6_driver);
+        of_unregister_platform_driver(&cg6_driver);
 }
 
 module_init(cg6_init);
diff --git a/drivers/video/console/Kconfig b/drivers/video/console/Kconfig
index 8e8f18d29d7a..5a35f22372b9 100644
--- a/drivers/video/console/Kconfig
+++ b/drivers/video/console/Kconfig
@@ -6,7 +6,7 @@ menu "Console display driver support"
 
 config VGA_CONSOLE
         bool "VGA text console" if EMBEDDED || !X86
-        depends on !ARCH_ACORN && !ARCH_EBSA110 && !4xx && !8xx && !SPARC && !M68K && !PARISC && !FRV && !ARCH_VERSATILE && !SUPERH && !BLACKFIN && !AVR32 && !MN10300
+        depends on !4xx && !8xx && !SPARC && !M68K && !PARISC && !FRV && !SUPERH && !BLACKFIN && !AVR32 && !MN10300 && (!ARM || ARCH_FOOTBRIDGE || ARCH_INTEGRATOR || ARCH_NETWINDER)
         default y
         help
           Saying Y here will allow you to use Linux in text mode through a
diff --git a/drivers/video/console/bitblit.c b/drivers/video/console/bitblit.c
index af88651b0735..28b1a834906b 100644
--- a/drivers/video/console/bitblit.c
+++ b/drivers/video/console/bitblit.c
@@ -22,7 +22,7 @@
 /*
  * Accelerated handlers.
  */
-static inline void update_attr(u8 *dst, u8 *src, int attribute,
+static void update_attr(u8 *dst, u8 *src, int attribute,
                                struct vc_data *vc)
 {
         int i, offset = (vc->vc_font.height < 10) ? 1 : 2;
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index b0a3fa00706d..7ccc967831f0 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -283,10 +283,11 @@ static inline int fbcon_is_inactive(struct vc_data *vc, struct fb_info *info)
         struct fbcon_ops *ops = info->fbcon_par;
 
         return (info->state != FBINFO_STATE_RUNNING ||
-                vc->vc_mode != KD_TEXT || ops->graphics);
+                vc->vc_mode != KD_TEXT || ops->graphics) &&
+                !vt_force_oops_output(vc);
 }
 
-static inline int get_color(struct vc_data *vc, struct fb_info *info,
+static int get_color(struct vc_data *vc, struct fb_info *info,
               u16 c, int is_fg)
 {
         int depth = fb_get_color_depth(&info->var, &info->fix);
@@ -1073,6 +1074,7 @@ static void fbcon_init(struct vc_data *vc, int init)
         if (p->userfont)
                 charcnt = FNTCHARCNT(p->fontdata);
 
+        vc->vc_panic_force_write = !!(info->flags & FBINFO_CAN_FORCE_OUTPUT);
         vc->vc_can_do_color = (fb_get_color_depth(&info->var, &info->fix)!=1);
         vc->vc_complement_mask = vc->vc_can_do_color ? 0x7700 : 0x0800;
         if (charcnt == 256) {
@@ -2342,6 +2344,30 @@ static int fbcon_blank(struct vc_data *vc, int blank, int mode_switch)
         return 0;
 }
 
+static int fbcon_debug_enter(struct vc_data *vc)
+{
+        struct fb_info *info = registered_fb[con2fb_map[vc->vc_num]];
+        struct fbcon_ops *ops = info->fbcon_par;
+
+        ops->save_graphics = ops->graphics;
+        ops->graphics = 0;
+        if (info->fbops->fb_debug_enter)
+                info->fbops->fb_debug_enter(info);
+        fbcon_set_palette(vc, color_table);
+        return 0;
+}
+
+static int fbcon_debug_leave(struct vc_data *vc)
+{
+        struct fb_info *info = registered_fb[con2fb_map[vc->vc_num]];
+        struct fbcon_ops *ops = info->fbcon_par;
+
+        ops->graphics = ops->save_graphics;
+        if (info->fbops->fb_debug_leave)
+                info->fbops->fb_debug_leave(info);
+        return 0;
+}
+
 static int fbcon_get_font(struct vc_data *vc, struct console_font *font)
 {
         u8 *fontdata = vc->vc_font.data;
@@ -3276,6 +3302,8 @@ static const struct consw fb_con = {
         .con_screen_pos         = fbcon_screen_pos,
         .con_getxy              = fbcon_getxy,
         .con_resize             = fbcon_resize,
+        .con_debug_enter        = fbcon_debug_enter,
+        .con_debug_leave        = fbcon_debug_leave,
 };
 
 static struct notifier_block fbcon_event_notifier = {
@@ -3480,7 +3508,7 @@ static void fbcon_exit(void)
         softback_buf = 0UL;
 
         for (i = 0; i < FB_MAX; i++) {
-                int pending;
+                int pending = 0;
 
                 mapped = 0;
                 info = registered_fb[i];
@@ -3488,7 +3516,8 @@ static void fbcon_exit(void)
                 if (info == NULL)
                         continue;
 
-                pending = cancel_work_sync(&info->queue);
+                if (info->queue.func)
+                        pending = cancel_work_sync(&info->queue);
                 DPRINTK("fbcon: %s pending work\n", (pending ? "canceled" :
                         "no"));
 
diff --git a/drivers/video/console/fbcon.h b/drivers/video/console/fbcon.h
index 89a346880ec0..6bd2e0c7f209 100644
--- a/drivers/video/console/fbcon.h
+++ b/drivers/video/console/fbcon.h
@@ -74,6 +74,7 @@ struct fbcon_ops {
         int    cursor_reset;
         int    blank_state;
         int    graphics;
+        int    save_graphics; /* for debug enter/leave */
         int    flags;
         int    rotate;
         int    cur_rotate;
diff --git a/drivers/video/console/fbcon_ccw.c b/drivers/video/console/fbcon_ccw.c
index d135671d9961..41b32ae23dac 100644
--- a/drivers/video/console/fbcon_ccw.c
+++ b/drivers/video/console/fbcon_ccw.c
@@ -22,7 +22,7 @@
  * Rotation 270 degrees
  */
 
-static inline void ccw_update_attr(u8 *dst, u8 *src, int attribute,
+static void ccw_update_attr(u8 *dst, u8 *src, int attribute,
                                   struct vc_data *vc)
 {
         int i, j, offset = (vc->vc_font.height < 10) ? 1 : 2;
diff --git a/drivers/video/console/fbcon_cw.c b/drivers/video/console/fbcon_cw.c
index 126110f8454f..6a737827beb1 100644
--- a/drivers/video/console/fbcon_cw.c
+++ b/drivers/video/console/fbcon_cw.c
@@ -22,7 +22,7 @@
  * Rotation 90 degrees
  */
 
-static inline void cw_update_attr(u8 *dst, u8 *src, int attribute,
+static void cw_update_attr(u8 *dst, u8 *src, int attribute,
                                   struct vc_data *vc)
 {
         int i, j, offset = (vc->vc_font.height < 10) ? 1 : 2;
diff --git a/drivers/video/console/fbcon_ud.c b/drivers/video/console/fbcon_ud.c
index 93a3e7381b50..ff0872c0498b 100644
--- a/drivers/video/console/fbcon_ud.c
+++ b/drivers/video/console/fbcon_ud.c
@@ -22,7 +22,7 @@
  * Rotation 180 degrees
  */
 
-static inline void ud_update_attr(u8 *dst, u8 *src, int attribute,
+static void ud_update_attr(u8 *dst, u8 *src, int attribute,
                                   struct vc_data *vc)
 {
         int i, offset = (vc->vc_font.height < 10) ? 1 : 2;
diff --git a/drivers/video/console/vgacon.c b/drivers/video/console/vgacon.c
index 182dd6f8aadd..54e32c513610 100644
--- a/drivers/video/console/vgacon.c
+++ b/drivers/video/console/vgacon.c
@@ -1108,7 +1108,6 @@ static int vgacon_do_font_op(struct vgastate *state,char *arg,int set,int ch512)
                 charmap += 4 * cmapsz;
 #endif
 
-        unlock_kernel();
         spin_lock_irq(&vga_lock);
         /* First, the Sequencer */
         vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
@@ -1192,7 +1191,6 @@ static int vgacon_do_font_op(struct vgastate *state,char *arg,int set,int ch512)
                 vga_wattr(state->vgabase, VGA_AR_ENABLE_DISPLAY, 0);    
         }
         spin_unlock_irq(&vga_lock);
-        lock_kernel();
         return 0;
 }
 
diff --git a/drivers/video/controlfb.c b/drivers/video/controlfb.c
index 49fcbe8f18ac..c225dcce89e7 100644
--- a/drivers/video/controlfb.c
+++ b/drivers/video/controlfb.c
@@ -40,6 +40,8 @@
 #include <linux/vmalloc.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/pci.h>
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index 3a561df2e8a2..0c1afd13ddd3 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -388,6 +388,7 @@ cyber2000fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
                 pseudo_val |= convert_bitfield(red, &var->red);
                 pseudo_val |= convert_bitfield(green, &var->green);
                 pseudo_val |= convert_bitfield(blue, &var->blue);
+                ret = 0;
                 break;
         }
 
@@ -436,6 +437,8 @@ static void cyber2000fb_write_ramdac_ctrl(struct cfb_info *cfb)
         cyber2000fb_writeb(i | 4, 0x3cf, cfb);
         cyber2000fb_writeb(val, 0x3c6, cfb);
         cyber2000fb_writeb(i, 0x3cf, cfb);
+        /* prevent card lock-up observed on x86 with CyberPro 2000 */
+        cyber2000fb_readb(0x3cf, cfb);
 }
 
 static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
diff --git a/drivers/video/efifb.c b/drivers/video/efifb.c
index 4a56f46af40a..70477c2e4b61 100644
--- a/drivers/video/efifb.c
+++ b/drivers/video/efifb.c
@@ -13,10 +13,10 @@
 #include <linux/platform_device.h>
 #include <linux/screen_info.h>
 #include <linux/dmi.h>
-
+#include <linux/pci.h>
 #include <video/vga.h>
 
-static struct fb_var_screeninfo efifb_defined __initdata = {
+static struct fb_var_screeninfo efifb_defined __devinitdata = {
         .activate               = FB_ACTIVATE_NOW,
         .height                 = -1,
         .width                  = -1,
@@ -27,7 +27,7 @@ static struct fb_var_screeninfo efifb_defined __initdata = {
         .vmode                  = FB_VMODE_NONINTERLACED,
 };
 
-static struct fb_fix_screeninfo efifb_fix __initdata = {
+static struct fb_fix_screeninfo efifb_fix __devinitdata = {
         .id                     = "EFI VGA",
         .type                   = FB_TYPE_PACKED_PIXELS,
         .accel                  = FB_ACCEL_NONE,
@@ -39,17 +39,31 @@ enum {
         M_I20,          /* 20-Inch iMac */
         M_I20_SR,       /* 20-Inch iMac (Santa Rosa) */
         M_I24,          /* 24-Inch iMac */
+        M_I24_8_1,      /* 24-Inch iMac, 8,1th gen */
+        M_I24_10_1,     /* 24-Inch iMac, 10,1th gen */
+        M_I27_11_1,     /* 27-Inch iMac, 11,1th gen */
         M_MINI,         /* Mac Mini */
+        M_MINI_3_1,     /* Mac Mini, 3,1th gen */
+        M_MINI_4_1,     /* Mac Mini, 4,1th gen */
         M_MB,           /* MacBook */
         M_MB_2,         /* MacBook, 2nd rev. */
         M_MB_3,         /* MacBook, 3rd rev. */
+        M_MB_5_1,       /* MacBook, 5th rev. */
+        M_MB_6_1,       /* MacBook, 6th rev. */
+        M_MB_7_1,       /* MacBook, 7th rev. */
         M_MB_SR,        /* MacBook, 2nd gen, (Santa Rosa) */
         M_MBA,          /* MacBook Air */
         M_MBP,          /* MacBook Pro */
         M_MBP_2,        /* MacBook Pro 2nd gen */
+        M_MBP_2_2,      /* MacBook Pro 2,2nd gen */
         M_MBP_SR,       /* MacBook Pro (Santa Rosa) */
         M_MBP_4,        /* MacBook Pro, 4th gen */
         M_MBP_5_1,    /* MacBook Pro, 5,1th gen */
+        M_MBP_5_2,      /* MacBook Pro, 5,2th gen */
+        M_MBP_5_3,      /* MacBook Pro, 5,3rd gen */
+        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_UNKNOWN       /* placeholder */
 };
 
@@ -59,19 +73,33 @@ static struct efifb_dmi_info {
         int stride;
         int width;
         int height;
-} dmi_list[] = {
+} 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 }
 };
 
@@ -83,7 +111,7 @@ static int set_system(const struct dmi_system_id *id);
                 DMI_MATCH(DMI_PRODUCT_NAME, name) },            \
           &dmi_list[enumid] }
 
-static struct dmi_system_id __initdata dmi_system_table[] = {
+static const struct dmi_system_id dmi_system_table[] __initconst = {
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "iMac4,1", M_I17),
         /* At least one of these two will be right; maybe both? */
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "iMac5,1", M_I20),
@@ -92,7 +120,12 @@ static struct dmi_system_id __initdata dmi_system_table[] = {
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "iMac6,1", M_I24),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "iMac6,1", M_I24),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "iMac7,1", M_I20_SR),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "iMac8,1", M_I24_8_1),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "iMac10,1", M_I24_10_1),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "iMac11,1", M_I27_11_1),
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "Macmini1,1", M_MINI),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "Macmini3,1", M_MINI_3_1),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "Macmini4,1", M_MINI_4_1),
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBook1,1", M_MB),
         /* At least one of these two will be right; maybe both? */
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBook2,1", M_MB),
@@ -101,14 +134,23 @@ static struct dmi_system_id __initdata dmi_system_table[] = {
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBook3,1", M_MB),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook3,1", M_MB),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook4,1", M_MB),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook5,1", M_MB_5_1),
+        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 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),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro2,1", M_MBP_2),
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro3,1", M_MBP_SR),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro3,1", M_MBP_SR),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro4,1", M_MBP_4),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro5,1", M_MBP_5_1),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro5,2", M_MBP_5_2),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro5,3", M_MBP_5_3),
+        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),
         {},
 };
 
@@ -116,7 +158,7 @@ static int set_system(const struct dmi_system_id *id)
 {
         struct efifb_dmi_info *info = id->driver_data;
         if (info->base == 0)
-                return -ENODEV;
+                return 0;
 
         printk(KERN_INFO "efifb: dmi detected %s - framebuffer at %p "
                          "(%dx%d, stride %d)\n", id->ident,
@@ -124,18 +166,55 @@ static int set_system(const struct dmi_system_id *id)
                          info->stride);
 
         /* Trust the bootloader over the DMI tables */
-        if (screen_info.lfb_base == 0)
+        if (screen_info.lfb_base == 0) {
+#if defined(CONFIG_PCI)
+                struct pci_dev *dev = NULL;
+                int found_bar = 0;
+#endif
                 screen_info.lfb_base = info->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;
-        if (screen_info.orig_video_isVGA == 0)
-                screen_info.orig_video_isVGA = VIDEO_TYPE_EFI;
 
-        return 0;
+#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;
+                                }
+                        }
+                }
+                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;
+                if (screen_info.orig_video_isVGA == 0)
+                        screen_info.orig_video_isVGA = VIDEO_TYPE_EFI;
+        } else {
+                screen_info.lfb_linelength = 0;
+                screen_info.lfb_width = 0;
+                screen_info.lfb_height = 0;
+                screen_info.orig_video_isVGA = 0;
+                return 0;
+        }
+        return 1;
 }
 
 static int efifb_setcolreg(unsigned regno, unsigned red, unsigned green,
diff --git a/drivers/video/epson1355fb.c b/drivers/video/epson1355fb.c
index db9713b49ce9..a268cbf1cbea 100644
--- a/drivers/video/epson1355fb.c
+++ b/drivers/video/epson1355fb.c
@@ -4,7 +4,7 @@
  * Epson Research S1D13505 Embedded RAMDAC LCD/CRT Controller
  *   (previously known as SED1355)
  *
- * Cf. http://www.erd.epson.com/vdc/html/S1D13505.html
+ * Cf. http://vdc.epson.com/
  *
  *
  * Copyright (C) Hewlett-Packard Company.  All rights reserved.
diff --git a/drivers/video/fbcvt.c b/drivers/video/fbcvt.c
index 7293eaccd81b..7cb715dfc0e1 100644
--- a/drivers/video/fbcvt.c
+++ b/drivers/video/fbcvt.c
@@ -5,7 +5,7 @@
  *
  *      Based from the VESA(TM) Coordinated Video Timing Generator by
  *      Graham Loveridge April 9, 2003 available at
- *      http://www.vesa.org/public/CVT/CVTd6r1.xls
+ *      http://www.elo.utfsm.cl/~elo212/docs/CVTd6r1.xls
  *
  * 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
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 731fce64df9d..0e6aa3d96a42 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -697,9 +697,9 @@ fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
         struct inode *inode = file->f_path.dentry->d_inode;
         int fbidx = iminor(inode);
         struct fb_info *info = registered_fb[fbidx];
-        u32 *buffer, *dst;
-        u32 __iomem *src;
-        int c, i, cnt = 0, err = 0;
+        u8 *buffer, *dst;
+        u8 __iomem *src;
+        int c, cnt = 0, err = 0;
         unsigned long total_size;
 
         if (!info || ! info->screen_base)
@@ -730,7 +730,7 @@ fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
         if (!buffer)
                 return -ENOMEM;
 
-        src = (u32 __iomem *) (info->screen_base + p);
+        src = (u8 __iomem *) (info->screen_base + p);
 
         if (info->fbops->fb_sync)
                 info->fbops->fb_sync(info);
@@ -738,17 +738,9 @@ fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
         while (count) {
                 c  = (count > PAGE_SIZE) ? PAGE_SIZE : count;
                 dst = buffer;
-                for (i = c >> 2; i--; )
-                        *dst++ = fb_readl(src++);
-                if (c & 3) {
-                        u8 *dst8 = (u8 *) dst;
-                        u8 __iomem *src8 = (u8 __iomem *) src;
-
-                        for (i = c & 3; i--;)
-                                *dst8++ = fb_readb(src8++);
-
-                        src = (u32 __iomem *) src8;
-                }
+                fb_memcpy_fromfb(dst, src, c);
+                dst += c;
+                src += c;
 
                 if (copy_to_user(buf, buffer, c)) {
                         err = -EFAULT;
@@ -772,9 +764,9 @@ fb_write(struct file *file, const char __user *buf, size_t count, loff_t *ppos)
         struct inode *inode = file->f_path.dentry->d_inode;
         int fbidx = iminor(inode);
         struct fb_info *info = registered_fb[fbidx];
-        u32 *buffer, *src;
-        u32 __iomem *dst;
-        int c, i, cnt = 0, err = 0;
+        u8 *buffer, *src;
+        u8 __iomem *dst;
+        int c, cnt = 0, err = 0;
         unsigned long total_size;
 
         if (!info || !info->screen_base)
@@ -811,7 +803,7 @@ fb_write(struct file *file, const char __user *buf, size_t count, loff_t *ppos)
         if (!buffer)
                 return -ENOMEM;
 
-        dst = (u32 __iomem *) (info->screen_base + p);
+        dst = (u8 __iomem *) (info->screen_base + p);
 
         if (info->fbops->fb_sync)
                 info->fbops->fb_sync(info);
@@ -825,19 +817,9 @@ fb_write(struct file *file, const char __user *buf, size_t count, loff_t *ppos)
                         break;
                 }
 
-                for (i = c >> 2; i--; )
-                        fb_writel(*src++, dst++);
-
-                if (c & 3) {
-                        u8 *src8 = (u8 *) src;
-                        u8 __iomem *dst8 = (u8 __iomem *) dst;
-
-                        for (i = c & 3; i--; )
-                                fb_writeb(*src8++, dst8++);
-
-                        dst = (u32 __iomem *) dst8;
-                }
-
+                fb_memcpy_tofb(dst, src, c);
+                dst += c;
+                src += c;
                 *ppos += c;
                 buf += c;
                 cnt += c;
@@ -877,13 +859,13 @@ fb_pan_display(struct fb_info *info, struct fb_var_screeninfo *var)
 
         if ((err = info->fbops->fb_pan_display(var, info)))
                 return err;
-        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;
+        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;
 }
 
 static int fb_check_caps(struct fb_info *info, struct fb_var_screeninfo *var,
@@ -1362,6 +1344,7 @@ fb_mmap(struct file *file, struct vm_area_struct * vma)
         vma->vm_pgoff = off >> PAGE_SHIFT;
         /* This is an IO map - tell maydump to skip this VMA */
         vma->vm_flags |= VM_IO | VM_RESERVED;
+        vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
         fb_pgprotect(file, vma, off);
         if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
                              vma->vm_end - vma->vm_start, vma->vm_page_prot))
@@ -1438,6 +1421,7 @@ static const struct file_operations fb_fops = {
 #ifdef CONFIG_FB_DEFERRED_IO
         .fsync =        fb_deferred_io_fsync,
 #endif
+        .llseek =       default_llseek,
 };
 
 struct class *fb_class;
@@ -1786,7 +1770,7 @@ static int ofonly __read_mostly;
 int fb_get_options(char *name, char **option)
 {
         char *opt, *options = NULL;
-        int opt_len, retval = 0;
+        int retval = 0;
         int name_len = strlen(name), i;
 
         if (name_len && ofonly && strncmp(name, "offb", 4))
@@ -1796,8 +1780,7 @@ int fb_get_options(char *name, char **option)
                 for (i = 0; i < FB_MAX; i++) {
                         if (video_options[i] == NULL)
                                 continue;
-                        opt_len = strlen(video_options[i]);
-                        if (!opt_len)
+                        if (!video_options[i][0])
                                 continue;
                         opt = video_options[i];
                         if (!strncmp(name, opt, name_len) &&
diff --git a/drivers/video/ffb.c b/drivers/video/ffb.c
index 95c0227f47fc..6739b2af3bc0 100644
--- a/drivers/video/ffb.c
+++ b/drivers/video/ffb.c
@@ -893,7 +893,7 @@ static void ffb_init_fix(struct fb_info *info)
         info->fix.accel = FB_ACCEL_SUN_CREATOR;
 }
 
-static int __devinit ffb_probe(struct of_device *op,
+static int __devinit ffb_probe(struct platform_device *op,
                                const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
@@ -1023,7 +1023,7 @@ out_err:
         return err;
 }
 
-static int __devexit ffb_remove(struct of_device *op)
+static int __devexit ffb_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct ffb_par *par = info->par;
@@ -1067,12 +1067,12 @@ static int __init ffb_init(void)
         if (fb_get_options("ffb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&ffb_driver, &of_bus_type);
+        return of_register_platform_driver(&ffb_driver);
 }
 
 static void __exit ffb_exit(void)
 {
-        of_unregister_driver(&ffb_driver);
+        of_unregister_platform_driver(&ffb_driver);
 }
 
 module_init(ffb_init);
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 27455ce298b7..8bbbf08fa3ce 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -34,7 +34,8 @@
 #include <linux/of_platform.h>
 
 #include <sysdev/fsl_soc.h>
-#include "fsl-diu-fb.h"
+#include <linux/fsl-diu-fb.h>
+#include "edid.h"
 
 /*
  * These parameters give default parameters
@@ -217,6 +218,7 @@ struct mfb_info {
         int x_aoi_d;            /* aoi display x offset to physical screen */
         int y_aoi_d;            /* aoi display y offset to physical screen */
         struct fsl_diu_data *parent;
+        u8 *edid_data;
 };
 
 
@@ -317,6 +319,17 @@ static void fsl_diu_free(void *virt, size_t size)
                 free_pages_exact(virt, size);
 }
 
+/*
+ * Workaround for failed writing desc register of planes.
+ * Needed with MPC5121 DIU rev 2.0 silicon.
+ */
+void wr_reg_wa(u32 *reg, u32 val)
+{
+        do {
+                out_be32(reg, val);
+        } while (in_be32(reg) != val);
+}
+
 static int fsl_diu_enable_panel(struct fb_info *info)
 {
         struct mfb_info *pmfbi, *cmfbi, *mfbi = info->par;
@@ -330,7 +343,7 @@ static int fsl_diu_enable_panel(struct fb_info *info)
                 switch (mfbi->index) {
                 case 0:                         /* plane 0 */
                         if (hw->desc[0] != ad->paddr)
-                                out_be32(&hw->desc[0], ad->paddr);
+                                wr_reg_wa(&hw->desc[0], ad->paddr);
                         break;
                 case 1:                         /* plane 1 AOI 0 */
                         cmfbi = machine_data->fsl_diu_info[2]->par;
@@ -340,7 +353,7 @@ static int fsl_diu_enable_panel(struct fb_info *info)
                                                 cpu_to_le32(cmfbi->ad->paddr);
                                 else
                                         ad->next_ad = 0;
-                                out_be32(&hw->desc[1], ad->paddr);
+                                wr_reg_wa(&hw->desc[1], ad->paddr);
                         }
                         break;
                 case 3:                         /* plane 2 AOI 0 */
@@ -351,14 +364,14 @@ static int fsl_diu_enable_panel(struct fb_info *info)
                                                 cpu_to_le32(cmfbi->ad->paddr);
                                 else
                                         ad->next_ad = 0;
-                                out_be32(&hw->desc[2], ad->paddr);
+                                wr_reg_wa(&hw->desc[2], ad->paddr);
                         }
                         break;
                 case 2:                         /* plane 1 AOI 1 */
                         pmfbi = machine_data->fsl_diu_info[1]->par;
                         ad->next_ad = 0;
                         if (hw->desc[1] == machine_data->dummy_ad->paddr)
-                                out_be32(&hw->desc[1], ad->paddr);
+                                wr_reg_wa(&hw->desc[1], ad->paddr);
                         else                                    /* AOI0 open */
                                 pmfbi->ad->next_ad = cpu_to_le32(ad->paddr);
                         break;
@@ -366,7 +379,7 @@ static int fsl_diu_enable_panel(struct fb_info *info)
                         pmfbi = machine_data->fsl_diu_info[3]->par;
                         ad->next_ad = 0;
                         if (hw->desc[2] == machine_data->dummy_ad->paddr)
-                                out_be32(&hw->desc[2], ad->paddr);
+                                wr_reg_wa(&hw->desc[2], ad->paddr);
                         else                            /* AOI0 was open */
                                 pmfbi->ad->next_ad = cpu_to_le32(ad->paddr);
                         break;
@@ -390,27 +403,24 @@ static int fsl_diu_disable_panel(struct fb_info *info)
         switch (mfbi->index) {
         case 0:                                 /* plane 0 */
                 if (hw->desc[0] != machine_data->dummy_ad->paddr)
-                        out_be32(&hw->desc[0],
-                                machine_data->dummy_ad->paddr);
+                        wr_reg_wa(&hw->desc[0], machine_data->dummy_ad->paddr);
                 break;
         case 1:                                 /* plane 1 AOI 0 */
                 cmfbi = machine_data->fsl_diu_info[2]->par;
                 if (cmfbi->count > 0)   /* AOI1 is open */
-                        out_be32(&hw->desc[1], cmfbi->ad->paddr);
+                        wr_reg_wa(&hw->desc[1], cmfbi->ad->paddr);
                                         /* move AOI1 to the first */
                 else                    /* AOI1 was closed */
-                        out_be32(&hw->desc[1],
-                                machine_data->dummy_ad->paddr);
+                        wr_reg_wa(&hw->desc[1], machine_data->dummy_ad->paddr);
                                         /* close AOI 0 */
                 break;
         case 3:                                 /* plane 2 AOI 0 */
                 cmfbi = machine_data->fsl_diu_info[4]->par;
                 if (cmfbi->count > 0)   /* AOI1 is open */
-                        out_be32(&hw->desc[2], cmfbi->ad->paddr);
+                        wr_reg_wa(&hw->desc[2], cmfbi->ad->paddr);
                                         /* move AOI1 to the first */
                 else                    /* AOI1 was closed */
-                        out_be32(&hw->desc[2],
-                                machine_data->dummy_ad->paddr);
+                        wr_reg_wa(&hw->desc[2], machine_data->dummy_ad->paddr);
                                         /* close AOI 0 */
                 break;
         case 2:                                 /* plane 1 AOI 1 */
@@ -421,7 +431,7 @@ static int fsl_diu_disable_panel(struct fb_info *info)
                                         /* AOI0 is open, must be the first */
                                 pmfbi->ad->next_ad = 0;
                 } else                  /* AOI1 is the first in the chain */
-                        out_be32(&hw->desc[1], machine_data->dummy_ad->paddr);
+                        wr_reg_wa(&hw->desc[1], machine_data->dummy_ad->paddr);
                                         /* close AOI 1 */
                 break;
         case 4:                                 /* plane 2 AOI 1 */
@@ -432,7 +442,7 @@ static int fsl_diu_disable_panel(struct fb_info *info)
                                 /* AOI0 is open, must be the first */
                                 pmfbi->ad->next_ad = 0;
                 } else          /* AOI1 is the first in the chain */
-                        out_be32(&hw->desc[2], machine_data->dummy_ad->paddr);
+                        wr_reg_wa(&hw->desc[2], machine_data->dummy_ad->paddr);
                                 /* close AOI 1 */
                 break;
         default:
@@ -1100,6 +1110,10 @@ static int fsl_diu_open(struct fb_info *info, int user)
         struct mfb_info *mfbi = info->par;
         int res = 0;
 
+        /* free boot splash memory on first /dev/fb0 open */
+        if (!mfbi->index && diu_ops.release_bootmem)
+                diu_ops.release_bootmem();
+
         spin_lock(&diu_lock);
         mfbi->count++;
         if (mfbi->count == 1) {
@@ -1173,18 +1187,30 @@ static int __devinit install_fb(struct fb_info *info)
         int rc;
         struct mfb_info *mfbi = info->par;
         const char *aoi_mode, *init_aoi_mode = "320x240";
+        struct fb_videomode *db = fsl_diu_mode_db;
+        unsigned int dbsize = ARRAY_SIZE(fsl_diu_mode_db);
+        int has_default_mode = 1;
 
         if (init_fbinfo(info))
                 return -EINVAL;
 
-        if (mfbi->index == 0)   /* plane 0 */
+        if (mfbi->index == 0) { /* plane 0 */
+                if (mfbi->edid_data) {
+                        /* Now build modedb from EDID */
+                        fb_edid_to_monspecs(mfbi->edid_data, &info->monspecs);
+                        fb_videomode_to_modelist(info->monspecs.modedb,
+                                                 info->monspecs.modedb_len,
+                                                 &info->modelist);
+                        db = info->monspecs.modedb;
+                        dbsize = info->monspecs.modedb_len;
+                }
                 aoi_mode = fb_mode;
-        else
+        } else {
                 aoi_mode = init_aoi_mode;
+        }
         pr_debug("mode used = %s\n", aoi_mode);
-        rc = fb_find_mode(&info->var, info, aoi_mode, fsl_diu_mode_db,
-             ARRAY_SIZE(fsl_diu_mode_db), &fsl_diu_default_mode, default_bpp);
-
+        rc = fb_find_mode(&info->var, info, aoi_mode, db, dbsize,
+                          &fsl_diu_default_mode, default_bpp);
         switch (rc) {
         case 1:
                 pr_debug("using mode specified in @mode\n");
@@ -1202,10 +1228,50 @@ static int __devinit install_fb(struct fb_info *info)
         default:
                 pr_debug("rc = %d\n", rc);
                 pr_debug("failed to find mode\n");
-                return -EINVAL;
+                /*
+                 * For plane 0 we continue and look into
+                 * driver's internal modedb.
+                 */
+                if (mfbi->index == 0 && mfbi->edid_data)
+                        has_default_mode = 0;
+                else
+                        return -EINVAL;
                 break;
         }
 
+        if (!has_default_mode) {
+                rc = fb_find_mode(&info->var, info, aoi_mode, fsl_diu_mode_db,
+                                  ARRAY_SIZE(fsl_diu_mode_db),
+                                  &fsl_diu_default_mode,
+                                  default_bpp);
+                if (rc > 0 && rc < 5)
+                        has_default_mode = 1;
+        }
+
+        /* Still not found, use preferred mode from database if any */
+        if (!has_default_mode && info->monspecs.modedb) {
+                struct fb_monspecs *specs = &info->monspecs;
+                struct fb_videomode *modedb = &specs->modedb[0];
+
+                /*
+                 * Get preferred timing. If not found,
+                 * first mode in database will be used.
+                 */
+                if (specs->misc & FB_MISC_1ST_DETAIL) {
+                        int i;
+
+                        for (i = 0; i < specs->modedb_len; i++) {
+                                if (specs->modedb[i].flag & FB_MODE_IS_FIRST) {
+                                        modedb = &specs->modedb[i];
+                                        break;
+                                }
+                        }
+                }
+
+                info->var.bits_per_pixel = default_bpp;
+                fb_videomode_to_var(&info->var, modedb);
+        }
+
         pr_debug("xres_virtual %d\n", info->var.xres_virtual);
         pr_debug("bits_per_pixel %d\n", info->var.bits_per_pixel);
 
@@ -1244,6 +1310,9 @@ static void uninstall_fb(struct fb_info *info)
         if (!mfbi->registered)
                 return;
 
+        if (mfbi->index == 0)
+                kfree(mfbi->edid_data);
+
         unregister_framebuffer(info);
         unmap_video_memory(info);
         if (&info->cmap)
@@ -1324,7 +1393,7 @@ static void free_irq_local(int irq)
  * Power management hooks. Note that we won't be called from IRQ context,
  * unlike the blank functions above, so we may sleep.
  */
-static int fsl_diu_suspend(struct of_device *ofdev, pm_message_t state)
+static int fsl_diu_suspend(struct platform_device *ofdev, pm_message_t state)
 {
         struct fsl_diu_data *machine_data;
 
@@ -1334,7 +1403,7 @@ static int fsl_diu_suspend(struct of_device *ofdev, pm_message_t state)
         return 0;
 }
 
-static int fsl_diu_resume(struct of_device *ofdev)
+static int fsl_diu_resume(struct platform_device *ofdev)
 {
         struct fsl_diu_data *machine_data;
 
@@ -1418,7 +1487,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 of_device *ofdev,
+static int __devinit fsl_diu_probe(struct platform_device *ofdev,
         const struct of_device_id *match)
 {
         struct device_node *np = ofdev->dev.of_node;
@@ -1427,6 +1496,7 @@ static int __devinit fsl_diu_probe(struct of_device *ofdev,
         int ret, i, error = 0;
         struct resource res;
         struct fsl_diu_data *machine_data;
+        int diu_mode;
 
         machine_data = kzalloc(sizeof(struct fsl_diu_data), GFP_KERNEL);
         if (!machine_data)
@@ -1443,6 +1513,17 @@ static int __devinit fsl_diu_probe(struct of_device *ofdev,
                 mfbi = machine_data->fsl_diu_info[i]->par;
                 memcpy(mfbi, &mfb_template[i], sizeof(struct mfb_info));
                 mfbi->parent = machine_data;
+
+                if (mfbi->index == 0) {
+                        const u8 *prop;
+                        int len;
+
+                        /* Get EDID */
+                        prop = of_get_property(np, "edid", &len);
+                        if (prop && len == EDID_LENGTH)
+                                mfbi->edid_data = kmemdup(prop, EDID_LENGTH,
+                                                          GFP_KERNEL);
+                }
         }
 
         ret = of_address_to_resource(np, 0, &res);
@@ -1463,7 +1544,9 @@ static int __devinit fsl_diu_probe(struct of_device *ofdev,
                 goto error2;
         }
 
-        out_be32(&dr.diu_reg->diu_mode, 0);             /* disable DIU anyway*/
+        diu_mode = in_be32(&dr.diu_reg->diu_mode);
+        if (diu_mode != MFB_MODE1)
+                out_be32(&dr.diu_reg->diu_mode, 0);     /* disable DIU */
 
         /* Get the IRQ of the DIU */
         machine_data->irq = irq_of_parse_and_map(np, 0);
@@ -1511,7 +1594,13 @@ static int __devinit fsl_diu_probe(struct of_device *ofdev,
         machine_data->dummy_ad->offset_xyd = 0;
         machine_data->dummy_ad->next_ad = 0;
 
-        out_be32(&dr.diu_reg->desc[0], machine_data->dummy_ad->paddr);
+        /*
+         * Let DIU display splash screen if it was pre-initialized
+         * by the bootloader, set dummy area descriptor otherwise.
+         */
+        if (diu_mode != MFB_MODE1)
+                out_be32(&dr.diu_reg->desc[0], machine_data->dummy_ad->paddr);
+
         out_be32(&dr.diu_reg->desc[1], machine_data->dummy_ad->paddr);
         out_be32(&dr.diu_reg->desc[2], machine_data->dummy_ad->paddr);
 
@@ -1578,7 +1667,7 @@ error2:
 }
 
 
-static int fsl_diu_remove(struct of_device *ofdev)
+static int fsl_diu_remove(struct platform_device *ofdev)
 {
         struct fsl_diu_data *machine_data;
         int i;
diff --git a/drivers/video/fsl-diu-fb.h b/drivers/video/fsl-diu-fb.h
deleted file mode 100644
index fc295d7ea463..000000000000
--- a/drivers/video/fsl-diu-fb.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/*
- * Copyright 2008 Freescale Semiconductor, Inc. All Rights Reserved.
- *
- *  Freescale DIU Frame Buffer device driver
- *
- *  Authors: Hongjun Chen <hong-jun.chen@freescale.com>
- *           Paul Widmer <paul.widmer@freescale.com>
- *           Srikanth Srinivasan <srikanth.srinivasan@freescale.com>
- *           York Sun <yorksun@freescale.com>
- *
- *   Based on imxfb.c Copyright (C) 2004 S.Hauer, Pengutronix
- *
- * 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.
- *
- */
-
-#ifndef __FSL_DIU_FB_H__
-#define __FSL_DIU_FB_H__
-
-/* Arbitrary threshold to determine the allocation method
- * See mpc8610fb_set_par(), map_video_memory(), and unmap_video_memory()
- */
-#define MEM_ALLOC_THRESHOLD (1024*768*4+32)
-/* Minimum value that the pixel clock can be set to in pico seconds
- * This is determined by platform clock/3 where the minimum platform
- * clock is 533MHz. This gives 5629 pico seconds.
- */
-#define MIN_PIX_CLK 5629
-#define MAX_PIX_CLK 96096
-
-#include <linux/types.h>
-
-struct mfb_alpha {
-        int enable;
-        int alpha;
-};
-
-struct mfb_chroma_key {
-        int enable;
-        __u8  red_max;
-        __u8  green_max;
-        __u8  blue_max;
-        __u8  red_min;
-        __u8  green_min;
-        __u8  blue_min;
-};
-
-struct aoi_display_offset {
-        int x_aoi_d;
-        int y_aoi_d;
-};
-
-#define MFB_SET_CHROMA_KEY      _IOW('M', 1, struct mfb_chroma_key)
-#define MFB_WAIT_FOR_VSYNC      _IOW('F', 0x20, u_int32_t)
-#define MFB_SET_BRIGHTNESS      _IOW('M', 3, __u8)
-
-#define MFB_SET_ALPHA           0x80014d00
-#define MFB_GET_ALPHA           0x40014d00
-#define MFB_SET_AOID            0x80084d04
-#define MFB_GET_AOID            0x40084d04
-#define MFB_SET_PIXFMT          0x80014d08
-#define MFB_GET_PIXFMT          0x40014d08
-
-#define FBIOGET_GWINFO          0x46E0
-#define FBIOPUT_GWINFO          0x46E1
-
-#ifdef __KERNEL__
-#include <linux/spinlock.h>
-
-/*
- * These are the fields of area descriptor(in DDR memory) for every plane
- */
-struct diu_ad {
-        /* Word 0(32-bit) in DDR memory */
-/*      __u16 comp; */
-/*      __u16 pixel_s:2; */
-/*      __u16 pallete:1; */
-/*      __u16 red_c:2; */
-/*      __u16 green_c:2; */
-/*      __u16 blue_c:2; */
-/*      __u16 alpha_c:3; */
-/*      __u16 byte_f:1; */
-/*      __u16 res0:3; */
-
-        __be32 pix_fmt; /* hard coding pixel format */
-
-        /* Word 1(32-bit) in DDR memory */
-        __le32 addr;
-
-        /* Word 2(32-bit) in DDR memory */
-/*      __u32 delta_xs:11; */
-/*      __u32 res1:1; */
-/*      __u32 delta_ys:11; */
-/*      __u32 res2:1; */
-/*      __u32 g_alpha:8; */
-        __le32 src_size_g_alpha;
-
-        /* Word 3(32-bit) in DDR memory */
-/*      __u32 delta_xi:11; */
-/*      __u32 res3:5; */
-/*      __u32 delta_yi:11; */
-/*      __u32 res4:3; */
-/*      __u32 flip:2; */
-        __le32 aoi_size;
-
-        /* Word 4(32-bit) in DDR memory */
-        /*__u32 offset_xi:11;
-        __u32 res5:5;
-        __u32 offset_yi:11;
-        __u32 res6:5;
-        */
-        __le32 offset_xyi;
-
-        /* Word 5(32-bit) in DDR memory */
-        /*__u32 offset_xd:11;
-        __u32 res7:5;
-        __u32 offset_yd:11;
-        __u32 res8:5; */
-        __le32 offset_xyd;
-
-
-        /* Word 6(32-bit) in DDR memory */
-        __u8 ckmax_r;
-        __u8 ckmax_g;
-        __u8 ckmax_b;
-        __u8 res9;
-
-        /* Word 7(32-bit) in DDR memory */
-        __u8 ckmin_r;
-        __u8 ckmin_g;
-        __u8 ckmin_b;
-        __u8 res10;
-/*      __u32 res10:8; */
-
-        /* Word 8(32-bit) in DDR memory */
-        __le32 next_ad;
-
-        /* Word 9(32-bit) in DDR memory, just for 64-bit aligned */
-        __u32 paddr;
-} __attribute__ ((packed));
-
-/* DIU register map */
-struct diu {
-        __be32 desc[3];
-        __be32 gamma;
-        __be32 pallete;
-        __be32 cursor;
-        __be32 curs_pos;
-        __be32 diu_mode;
-        __be32 bgnd;
-        __be32 bgnd_wb;
-        __be32 disp_size;
-        __be32 wb_size;
-        __be32 wb_mem_addr;
-        __be32 hsyn_para;
-        __be32 vsyn_para;
-        __be32 syn_pol;
-        __be32 thresholds;
-        __be32 int_status;
-        __be32 int_mask;
-        __be32 colorbar[8];
-        __be32 filling;
-        __be32 plut;
-} __attribute__ ((packed));
-
-struct diu_hw {
-        struct diu *diu_reg;
-        spinlock_t reg_lock;
-
-        __u32 mode;             /* DIU operation mode */
-};
-
-struct diu_addr {
-        __u8 __iomem *vaddr;    /* Virtual address */
-        dma_addr_t paddr;       /* Physical address */
-        __u32      offset;
-};
-
-struct diu_pool {
-        struct diu_addr ad;
-        struct diu_addr gamma;
-        struct diu_addr pallete;
-        struct diu_addr cursor;
-};
-
-#define FSL_DIU_BASE_OFFSET     0x2C000 /* Offset of DIU */
-#define INT_LCDC                64      /* DIU interrupt number */
-
-#define FSL_AOI_NUM     6       /* 5 AOIs and one dummy AOI */
-                                /* 1 for plane 0, 2 for plane 1&2 each */
-
-/* Minimum X and Y resolutions */
-#define MIN_XRES        64
-#define MIN_YRES        64
-
-/* HW cursor parameters */
-#define MAX_CURS                32
-
-/* Modes of operation of DIU */
-#define MFB_MODE0       0       /* DIU off */
-#define MFB_MODE1       1       /* All three planes output to display */
-#define MFB_MODE2       2       /* Plane 1 to display, planes 2+3 written back*/
-#define MFB_MODE3       3       /* All three planes written back to memory */
-#define MFB_MODE4       4       /* Color bar generation */
-
-/* INT_STATUS/INT_MASK field descriptions */
-#define INT_VSYNC       0x01    /* Vsync interrupt  */
-#define INT_VSYNC_WB    0x02    /* Vsync interrupt for write back operation */
-#define INT_UNDRUN      0x04    /* Under run exception interrupt */
-#define INT_PARERR      0x08    /* Display parameters error interrupt */
-#define INT_LS_BF_VS    0x10    /* Lines before vsync. interrupt */
-
-/* Panels'operation modes */
-#define MFB_TYPE_OUTPUT 0       /* Panel output to display */
-#define MFB_TYPE_OFF    1       /* Panel off */
-#define MFB_TYPE_WB     2       /* Panel written back to memory */
-#define MFB_TYPE_TEST   3       /* Panel generate color bar */
-
-#endif /* __KERNEL__ */
-#endif /* __FSL_DIU_FB_H__ */
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index 7d8c55d7fd28..933899dca33a 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -91,10 +91,10 @@ static uint32_t pseudo_palette[16];
 static uint32_t gbe_cmap[256];
 static int gbe_turned_on; /* 0 turned off, 1 turned on */
 
-static char *mode_option __initdata = NULL;
+static char *mode_option __devinitdata = NULL;
 
 /* default CRT mode */
-static struct fb_var_screeninfo default_var_CRT __initdata = {
+static struct fb_var_screeninfo default_var_CRT __devinitdata = {
         /* 640x480, 60 Hz, Non-Interlaced (25.175 MHz dotclock) */
         .xres           = 640,
         .yres           = 480,
@@ -125,7 +125,7 @@ static struct fb_var_screeninfo default_var_CRT __initdata = {
 };
 
 /* default LCD mode */
-static struct fb_var_screeninfo default_var_LCD __initdata = {
+static struct fb_var_screeninfo default_var_LCD __devinitdata = {
         /* 1600x1024, 8 bpp */
         .xres           = 1600,
         .yres           = 1024,
@@ -157,7 +157,7 @@ static struct fb_var_screeninfo default_var_LCD __initdata = {
 
 /* default modedb mode */
 /* 640x480, 60 Hz, Non-Interlaced (25.172 MHz dotclock) */
-static struct fb_videomode default_mode_CRT __initdata = {
+static struct fb_videomode default_mode_CRT __devinitdata = {
         .refresh        = 60,
         .xres           = 640,
         .yres           = 480,
@@ -172,7 +172,7 @@ static struct fb_videomode default_mode_CRT __initdata = {
         .vmode          = FB_VMODE_NONINTERLACED,
 };
 /* 1600x1024 SGI flatpanel 1600sw */
-static struct fb_videomode default_mode_LCD __initdata = {
+static struct fb_videomode default_mode_LCD __devinitdata = {
         /* 1600x1024, 8 bpp */
         .xres           = 1600,
         .yres           = 1024,
@@ -186,8 +186,8 @@ static struct fb_videomode default_mode_LCD __initdata = {
         .vmode          = FB_VMODE_NONINTERLACED,
 };
 
-static struct fb_videomode *default_mode __initdata = &default_mode_CRT;
-static struct fb_var_screeninfo *default_var __initdata = &default_var_CRT;
+static struct fb_videomode *default_mode __devinitdata = &default_mode_CRT;
+static struct fb_var_screeninfo *default_var __devinitdata = &default_var_CRT;
 
 static int flat_panel_enabled = 0;
 
@@ -1098,7 +1098,7 @@ static void gbefb_create_sysfs(struct device *dev)
  * Initialization
  */
 
-static int __init gbefb_setup(char *options)
+static int __devinit gbefb_setup(char *options)
 {
         char *this_opt;
 
@@ -1143,8 +1143,10 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
                 return -ENOMEM;
 
 #ifndef MODULE
-        if (fb_get_options("gbefb", &options))
-                return -ENODEV;
+        if (fb_get_options("gbefb", &options)) {
+                ret = -ENODEV;
+                goto out_release_framebuffer;
+        }
         gbefb_setup(options);
 #endif
 
diff --git a/drivers/video/i810/i810.h b/drivers/video/i810/i810.h
index 328ae6c673ec..f37de60ecc59 100644
--- a/drivers/video/i810/i810.h
+++ b/drivers/video/i810/i810.h
@@ -17,7 +17,6 @@
 #include <linux/agp_backend.h>
 #include <linux/fb.h>
 #include <linux/i2c.h>
-#include <linux/i2c-id.h>
 #include <linux/i2c-algo-bit.h>
 #include <video/vga.h>
 
diff --git a/drivers/video/igafb.c b/drivers/video/igafb.c
index 15d200109446..d885c770eb84 100644
--- a/drivers/video/igafb.c
+++ b/drivers/video/igafb.c
@@ -368,7 +368,7 @@ static int __init iga_init(struct fb_info *info, struct iga_par *par)
         return 1;
 }
 
-int __init igafb_init(void)
+static int __init igafb_init(void)
 {
         struct fb_info *info;
         struct pci_dev *pdev;
@@ -531,6 +531,7 @@ int __init igafb_init(void)
                 iounmap(info->screen_base);
                 kfree(par->mmap_map);
                 kfree(info);
+                return -ENODEV;
         }
 
 #ifdef CONFIG_SPARC
@@ -556,7 +557,7 @@ int __init igafb_init(void)
         return 0;
 }
 
-int __init igafb_setup(char *options)
+static int __init igafb_setup(char *options)
 {
     char *this_opt;
 
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index b4b6deceed15..5c363d026f64 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -40,6 +40,12 @@
  */
 #define DEBUG_VAR 1
 
+#if defined(CONFIG_BACKLIGHT_CLASS_DEVICE) || \
+        (defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE) && \
+                defined(CONFIG_FB_IMX_MODULE))
+#define PWMR_BACKLIGHT_AVAILABLE
+#endif
+
 #define DRIVER_NAME "imx-fb"
 
 #define LCDC_SSA        0x00
@@ -175,6 +181,9 @@ struct imxfb_info {
 
         struct imx_fb_videomode *mode;
         int                     num_modes;
+#ifdef PWMR_BACKLIGHT_AVAILABLE
+        struct backlight_device *bl;
+#endif
 
         void (*lcd_power)(int);
         void (*backlight_power)(int);
@@ -449,6 +458,73 @@ static int imxfb_set_par(struct fb_info *info)
         return 0;
 }
 
+#ifdef PWMR_BACKLIGHT_AVAILABLE
+static int imxfb_bl_get_brightness(struct backlight_device *bl)
+{
+        struct imxfb_info *fbi = bl_get_data(bl);
+
+        return readl(fbi->regs + LCDC_PWMR) & 0xFF;
+}
+
+static int imxfb_bl_update_status(struct backlight_device *bl)
+{
+        struct imxfb_info *fbi = bl_get_data(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;
+
+        fbi->pwmr = (fbi->pwmr & ~0xFF) | brightness;
+
+        if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+                clk_enable(fbi->clk);
+        writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
+        if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+                clk_disable(fbi->clk);
+
+        return 0;
+}
+
+static const struct backlight_ops imxfb_lcdc_bl_ops = {
+        .update_status = imxfb_bl_update_status,
+        .get_brightness = imxfb_bl_get_brightness,
+};
+
+static void imxfb_init_backlight(struct imxfb_info *fbi)
+{
+        struct backlight_properties props;
+        struct backlight_device *bl;
+
+        if (fbi->bl)
+                return;
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = 0xff;
+        writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
+
+        bl = backlight_device_register("imxfb-bl", &fbi->pdev->dev, fbi,
+                                       &imxfb_lcdc_bl_ops, &props);
+        if (IS_ERR(bl)) {
+                dev_err(&fbi->pdev->dev, "error %ld on backlight register\n",
+                                PTR_ERR(bl));
+                return;
+        }
+
+        fbi->bl = bl;
+        bl->props.power = FB_BLANK_UNBLANK;
+        bl->props.fb_blank = FB_BLANK_UNBLANK;
+        bl->props.brightness = imxfb_bl_get_brightness(bl);
+}
+
+static void imxfb_exit_backlight(struct imxfb_info *fbi)
+{
+        if (fbi->bl)
+                backlight_device_unregister(fbi->bl);
+}
+#endif
+
 static void imxfb_enable_controller(struct imxfb_info *fbi)
 {
         pr_debug("Enabling LCD controller\n");
@@ -579,7 +655,9 @@ static int imxfb_activate_var(struct fb_var_screeninfo *var, struct fb_info *inf
                         fbi->regs + LCDC_SIZE);
 
         writel(fbi->pcr, fbi->regs + LCDC_PCR);
+#ifndef PWMR_BACKLIGHT_AVAILABLE
         writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
+#endif
         writel(fbi->lscr1, fbi->regs + LCDC_LSCR1);
         writel(fbi->dmacr, fbi->regs + LCDC_DMACR);
 
@@ -779,6 +857,10 @@ static int __init imxfb_probe(struct platform_device *pdev)
         }
 
         imxfb_enable_controller(fbi);
+        fbi->pdev = pdev;
+#ifdef PWMR_BACKLIGHT_AVAILABLE
+        imxfb_init_backlight(fbi);
+#endif
 
         return 0;
 
@@ -816,6 +898,9 @@ static int __devexit imxfb_remove(struct platform_device *pdev)
 
         imxfb_disable_controller(fbi);
 
+#ifdef PWMR_BACKLIGHT_AVAILABLE
+        imxfb_exit_backlight(fbi);
+#endif
         unregister_framebuffer(info);
 
         pdata = pdev->dev.platform_data;
diff --git a/drivers/video/intelfb/intelfb_i2c.c b/drivers/video/intelfb/intelfb_i2c.c
index 487f2be47460..3300bd31d9d7 100644
--- a/drivers/video/intelfb/intelfb_i2c.c
+++ b/drivers/video/intelfb/intelfb_i2c.c
@@ -32,7 +32,6 @@ USE OR OTHER DEALINGS IN THE SOFTWARE.
 #include <linux/fb.h>
 
 #include <linux/i2c.h>
-#include <linux/i2c-id.h>
 #include <linux/i2c-algo-bit.h>
 
 #include <asm/io.h>
diff --git a/drivers/video/jz4740_fb.c b/drivers/video/jz4740_fb.c
new file mode 100644
index 000000000000..670ecaa0385a
--- /dev/null
+++ b/drivers/video/jz4740_fb.c
@@ -0,0 +1,847 @@
+/*
+ *  Copyright (C) 2009-2010, Lars-Peter Clausen <lars@metafoo.de>
+ *      JZ4740 SoC LCD framebuffer driver
+ *
+ *  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.
+ *
+ *  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.,
+ *  675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+
+#include <linux/console.h>
+#include <linux/fb.h>
+
+#include <linux/dma-mapping.h>
+
+#include <asm/mach-jz4740/jz4740_fb.h>
+#include <asm/mach-jz4740/gpio.h>
+
+#define JZ_REG_LCD_CFG          0x00
+#define JZ_REG_LCD_VSYNC        0x04
+#define JZ_REG_LCD_HSYNC        0x08
+#define JZ_REG_LCD_VAT          0x0C
+#define JZ_REG_LCD_DAH          0x10
+#define JZ_REG_LCD_DAV          0x14
+#define JZ_REG_LCD_PS           0x18
+#define JZ_REG_LCD_CLS          0x1C
+#define JZ_REG_LCD_SPL          0x20
+#define JZ_REG_LCD_REV          0x24
+#define JZ_REG_LCD_CTRL         0x30
+#define JZ_REG_LCD_STATE        0x34
+#define JZ_REG_LCD_IID          0x38
+#define JZ_REG_LCD_DA0          0x40
+#define JZ_REG_LCD_SA0          0x44
+#define JZ_REG_LCD_FID0         0x48
+#define JZ_REG_LCD_CMD0         0x4C
+#define JZ_REG_LCD_DA1          0x50
+#define JZ_REG_LCD_SA1          0x54
+#define JZ_REG_LCD_FID1         0x58
+#define JZ_REG_LCD_CMD1         0x5C
+
+#define JZ_LCD_CFG_SLCD                 BIT(31)
+#define JZ_LCD_CFG_PS_DISABLE           BIT(23)
+#define JZ_LCD_CFG_CLS_DISABLE          BIT(22)
+#define JZ_LCD_CFG_SPL_DISABLE          BIT(21)
+#define JZ_LCD_CFG_REV_DISABLE          BIT(20)
+#define JZ_LCD_CFG_HSYNCM               BIT(19)
+#define JZ_LCD_CFG_PCLKM                BIT(18)
+#define JZ_LCD_CFG_INV                  BIT(17)
+#define JZ_LCD_CFG_SYNC_DIR             BIT(16)
+#define JZ_LCD_CFG_PS_POLARITY          BIT(15)
+#define JZ_LCD_CFG_CLS_POLARITY         BIT(14)
+#define JZ_LCD_CFG_SPL_POLARITY         BIT(13)
+#define JZ_LCD_CFG_REV_POLARITY         BIT(12)
+#define JZ_LCD_CFG_HSYNC_ACTIVE_LOW     BIT(11)
+#define JZ_LCD_CFG_PCLK_FALLING_EDGE    BIT(10)
+#define JZ_LCD_CFG_DE_ACTIVE_LOW        BIT(9)
+#define JZ_LCD_CFG_VSYNC_ACTIVE_LOW     BIT(8)
+#define JZ_LCD_CFG_18_BIT               BIT(7)
+#define JZ_LCD_CFG_PDW                  (BIT(5) | BIT(4))
+#define JZ_LCD_CFG_MODE_MASK 0xf
+
+#define JZ_LCD_CTRL_BURST_4             (0x0 << 28)
+#define JZ_LCD_CTRL_BURST_8             (0x1 << 28)
+#define JZ_LCD_CTRL_BURST_16            (0x2 << 28)
+#define JZ_LCD_CTRL_RGB555              BIT(27)
+#define JZ_LCD_CTRL_OFUP                BIT(26)
+#define JZ_LCD_CTRL_FRC_GRAYSCALE_16    (0x0 << 24)
+#define JZ_LCD_CTRL_FRC_GRAYSCALE_4     (0x1 << 24)
+#define JZ_LCD_CTRL_FRC_GRAYSCALE_2     (0x2 << 24)
+#define JZ_LCD_CTRL_PDD_MASK            (0xff << 16)
+#define JZ_LCD_CTRL_EOF_IRQ             BIT(13)
+#define JZ_LCD_CTRL_SOF_IRQ             BIT(12)
+#define JZ_LCD_CTRL_OFU_IRQ             BIT(11)
+#define JZ_LCD_CTRL_IFU0_IRQ            BIT(10)
+#define JZ_LCD_CTRL_IFU1_IRQ            BIT(9)
+#define JZ_LCD_CTRL_DD_IRQ              BIT(8)
+#define JZ_LCD_CTRL_QDD_IRQ             BIT(7)
+#define JZ_LCD_CTRL_REVERSE_ENDIAN      BIT(6)
+#define JZ_LCD_CTRL_LSB_FISRT           BIT(5)
+#define JZ_LCD_CTRL_DISABLE             BIT(4)
+#define JZ_LCD_CTRL_ENABLE              BIT(3)
+#define JZ_LCD_CTRL_BPP_1               0x0
+#define JZ_LCD_CTRL_BPP_2               0x1
+#define JZ_LCD_CTRL_BPP_4               0x2
+#define JZ_LCD_CTRL_BPP_8               0x3
+#define JZ_LCD_CTRL_BPP_15_16           0x4
+#define JZ_LCD_CTRL_BPP_18_24           0x5
+
+#define JZ_LCD_CMD_SOF_IRQ BIT(15)
+#define JZ_LCD_CMD_EOF_IRQ BIT(16)
+#define JZ_LCD_CMD_ENABLE_PAL BIT(12)
+
+#define JZ_LCD_SYNC_MASK 0x3ff
+
+#define JZ_LCD_STATE_DISABLED BIT(0)
+
+struct jzfb_framedesc {
+        uint32_t next;
+        uint32_t addr;
+        uint32_t id;
+        uint32_t cmd;
+} __packed;
+
+struct jzfb {
+        struct fb_info *fb;
+        struct platform_device *pdev;
+        void __iomem *base;
+        struct resource *mem;
+        struct jz4740_fb_platform_data *pdata;
+
+        size_t vidmem_size;
+        void *vidmem;
+        dma_addr_t vidmem_phys;
+        struct jzfb_framedesc *framedesc;
+        dma_addr_t framedesc_phys;
+
+        struct clk *ldclk;
+        struct clk *lpclk;
+
+        unsigned is_enabled:1;
+        struct mutex lock;
+
+        uint32_t pseudo_palette[16];
+};
+
+static const struct fb_fix_screeninfo jzfb_fix __devinitdata = {
+        .id             = "JZ4740 FB",
+        .type           = FB_TYPE_PACKED_PIXELS,
+        .visual         = FB_VISUAL_TRUECOLOR,
+        .xpanstep       = 0,
+        .ypanstep       = 0,
+        .ywrapstep      = 0,
+        .accel          = FB_ACCEL_NONE,
+};
+
+static const struct jz_gpio_bulk_request jz_lcd_ctrl_pins[] = {
+        JZ_GPIO_BULK_PIN(LCD_PCLK),
+        JZ_GPIO_BULK_PIN(LCD_HSYNC),
+        JZ_GPIO_BULK_PIN(LCD_VSYNC),
+        JZ_GPIO_BULK_PIN(LCD_DE),
+        JZ_GPIO_BULK_PIN(LCD_PS),
+        JZ_GPIO_BULK_PIN(LCD_REV),
+        JZ_GPIO_BULK_PIN(LCD_CLS),
+        JZ_GPIO_BULK_PIN(LCD_SPL),
+};
+
+static const struct jz_gpio_bulk_request jz_lcd_data_pins[] = {
+        JZ_GPIO_BULK_PIN(LCD_DATA0),
+        JZ_GPIO_BULK_PIN(LCD_DATA1),
+        JZ_GPIO_BULK_PIN(LCD_DATA2),
+        JZ_GPIO_BULK_PIN(LCD_DATA3),
+        JZ_GPIO_BULK_PIN(LCD_DATA4),
+        JZ_GPIO_BULK_PIN(LCD_DATA5),
+        JZ_GPIO_BULK_PIN(LCD_DATA6),
+        JZ_GPIO_BULK_PIN(LCD_DATA7),
+        JZ_GPIO_BULK_PIN(LCD_DATA8),
+        JZ_GPIO_BULK_PIN(LCD_DATA9),
+        JZ_GPIO_BULK_PIN(LCD_DATA10),
+        JZ_GPIO_BULK_PIN(LCD_DATA11),
+        JZ_GPIO_BULK_PIN(LCD_DATA12),
+        JZ_GPIO_BULK_PIN(LCD_DATA13),
+        JZ_GPIO_BULK_PIN(LCD_DATA14),
+        JZ_GPIO_BULK_PIN(LCD_DATA15),
+        JZ_GPIO_BULK_PIN(LCD_DATA16),
+        JZ_GPIO_BULK_PIN(LCD_DATA17),
+};
+
+static unsigned int jzfb_num_ctrl_pins(struct jzfb *jzfb)
+{
+        unsigned int num;
+
+        switch (jzfb->pdata->lcd_type) {
+        case JZ_LCD_TYPE_GENERIC_16_BIT:
+                num = 4;
+                break;
+        case JZ_LCD_TYPE_GENERIC_18_BIT:
+                num = 4;
+                break;
+        case JZ_LCD_TYPE_8BIT_SERIAL:
+                num = 3;
+                break;
+        case JZ_LCD_TYPE_SPECIAL_TFT_1:
+        case JZ_LCD_TYPE_SPECIAL_TFT_2:
+        case JZ_LCD_TYPE_SPECIAL_TFT_3:
+                num = 8;
+                break;
+        default:
+                num = 0;
+                break;
+        }
+        return num;
+}
+
+static unsigned int jzfb_num_data_pins(struct jzfb *jzfb)
+{
+        unsigned int num;
+
+        switch (jzfb->pdata->lcd_type) {
+        case JZ_LCD_TYPE_GENERIC_16_BIT:
+                num = 16;
+                break;
+        case JZ_LCD_TYPE_GENERIC_18_BIT:
+                num = 18;
+                break;
+        case JZ_LCD_TYPE_8BIT_SERIAL:
+                num = 8;
+                break;
+        case JZ_LCD_TYPE_SPECIAL_TFT_1:
+        case JZ_LCD_TYPE_SPECIAL_TFT_2:
+        case JZ_LCD_TYPE_SPECIAL_TFT_3:
+                if (jzfb->pdata->bpp == 18)
+                        num = 18;
+                else
+                        num = 16;
+                break;
+        default:
+                num = 0;
+                break;
+        }
+        return num;
+}
+
+/* Based on CNVT_TOHW macro from skeletonfb.c */
+static inline uint32_t jzfb_convert_color_to_hw(unsigned val,
+        struct fb_bitfield *bf)
+{
+        return (((val << bf->length) + 0x7FFF - val) >> 16) << bf->offset;
+}
+
+static int jzfb_setcolreg(unsigned regno, unsigned red, unsigned green,
+                        unsigned blue, unsigned transp, struct fb_info *fb)
+{
+        uint32_t color;
+
+        if (regno >= 16)
+                return -EINVAL;
+
+        color = jzfb_convert_color_to_hw(red, &fb->var.red);
+        color |= jzfb_convert_color_to_hw(green, &fb->var.green);
+        color |= jzfb_convert_color_to_hw(blue, &fb->var.blue);
+        color |= jzfb_convert_color_to_hw(transp, &fb->var.transp);
+
+        ((uint32_t *)(fb->pseudo_palette))[regno] = color;
+
+        return 0;
+}
+
+static int jzfb_get_controller_bpp(struct jzfb *jzfb)
+{
+        switch (jzfb->pdata->bpp) {
+        case 18:
+        case 24:
+                return 32;
+        case 15:
+                return 16;
+        default:
+                return jzfb->pdata->bpp;
+        }
+}
+
+static struct fb_videomode *jzfb_get_mode(struct jzfb *jzfb,
+        struct fb_var_screeninfo *var)
+{
+        size_t i;
+        struct fb_videomode *mode = jzfb->pdata->modes;
+
+        for (i = 0; i < jzfb->pdata->num_modes; ++i, ++mode) {
+                if (mode->xres == var->xres && mode->yres == var->yres)
+                        return mode;
+        }
+
+        return NULL;
+}
+
+static int jzfb_check_var(struct fb_var_screeninfo *var, struct fb_info *fb)
+{
+        struct jzfb *jzfb = fb->par;
+        struct fb_videomode *mode;
+
+        if (var->bits_per_pixel != jzfb_get_controller_bpp(jzfb) &&
+                var->bits_per_pixel != jzfb->pdata->bpp)
+                return -EINVAL;
+
+        mode = jzfb_get_mode(jzfb, var);
+        if (mode == NULL)
+                return -EINVAL;
+
+        fb_videomode_to_var(var, mode);
+
+        switch (jzfb->pdata->bpp) {
+        case 8:
+                break;
+        case 15:
+                var->red.offset = 10;
+                var->red.length = 5;
+                var->green.offset = 6;
+                var->green.length = 5;
+                var->blue.offset = 0;
+                var->blue.length = 5;
+                break;
+        case 16:
+                var->red.offset = 11;
+                var->red.length = 5;
+                var->green.offset = 5;
+                var->green.length = 6;
+                var->blue.offset = 0;
+                var->blue.length = 5;
+                break;
+        case 18:
+                var->red.offset = 16;
+                var->red.length = 6;
+                var->green.offset = 8;
+                var->green.length = 6;
+                var->blue.offset = 0;
+                var->blue.length = 6;
+                var->bits_per_pixel = 32;
+                break;
+        case 32:
+        case 24:
+                var->transp.offset = 24;
+                var->transp.length = 8;
+                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->bits_per_pixel = 32;
+                break;
+        default:
+                break;
+        }
+
+        return 0;
+}
+
+static int jzfb_set_par(struct fb_info *info)
+{
+        struct jzfb *jzfb = info->par;
+        struct jz4740_fb_platform_data *pdata = jzfb->pdata;
+        struct fb_var_screeninfo *var = &info->var;
+        struct fb_videomode *mode;
+        uint16_t hds, vds;
+        uint16_t hde, vde;
+        uint16_t ht, vt;
+        uint32_t ctrl;
+        uint32_t cfg;
+        unsigned long rate;
+
+        mode = jzfb_get_mode(jzfb, var);
+        if (mode == NULL)
+                return -EINVAL;
+
+        if (mode == info->mode)
+                return 0;
+
+        info->mode = mode;
+
+        hds = mode->hsync_len + mode->left_margin;
+        hde = hds + mode->xres;
+        ht = hde + mode->right_margin;
+
+        vds = mode->vsync_len + mode->upper_margin;
+        vde = vds + mode->yres;
+        vt = vde + mode->lower_margin;
+
+        ctrl = JZ_LCD_CTRL_OFUP | JZ_LCD_CTRL_BURST_16;
+
+        switch (pdata->bpp) {
+        case 1:
+                ctrl |= JZ_LCD_CTRL_BPP_1;
+                break;
+        case 2:
+                ctrl |= JZ_LCD_CTRL_BPP_2;
+                break;
+        case 4:
+                ctrl |= JZ_LCD_CTRL_BPP_4;
+                break;
+        case 8:
+                ctrl |= JZ_LCD_CTRL_BPP_8;
+        break;
+        case 15:
+                ctrl |= JZ_LCD_CTRL_RGB555; /* Falltrough */
+        case 16:
+                ctrl |= JZ_LCD_CTRL_BPP_15_16;
+                break;
+        case 18:
+        case 24:
+        case 32:
+                ctrl |= JZ_LCD_CTRL_BPP_18_24;
+                break;
+        default:
+                break;
+        }
+
+        cfg = pdata->lcd_type & 0xf;
+
+        if (!(mode->sync & FB_SYNC_HOR_HIGH_ACT))
+                cfg |= JZ_LCD_CFG_HSYNC_ACTIVE_LOW;
+
+        if (!(mode->sync & FB_SYNC_VERT_HIGH_ACT))
+                cfg |= JZ_LCD_CFG_VSYNC_ACTIVE_LOW;
+
+        if (pdata->pixclk_falling_edge)
+                cfg |= JZ_LCD_CFG_PCLK_FALLING_EDGE;
+
+        if (pdata->date_enable_active_low)
+                cfg |= JZ_LCD_CFG_DE_ACTIVE_LOW;
+
+        if (pdata->lcd_type == JZ_LCD_TYPE_GENERIC_18_BIT)
+                cfg |= JZ_LCD_CFG_18_BIT;
+
+        if (mode->pixclock) {
+                rate = PICOS2KHZ(mode->pixclock) * 1000;
+                mode->refresh = rate / vt / ht;
+        } else {
+                if (pdata->lcd_type == JZ_LCD_TYPE_8BIT_SERIAL)
+                        rate = mode->refresh * (vt + 2 * mode->xres) * ht;
+                else
+                        rate = mode->refresh * vt * ht;
+
+                mode->pixclock = KHZ2PICOS(rate / 1000);
+        }
+
+        mutex_lock(&jzfb->lock);
+        if (!jzfb->is_enabled)
+                clk_enable(jzfb->ldclk);
+        else
+                ctrl |= JZ_LCD_CTRL_ENABLE;
+
+        switch (pdata->lcd_type) {
+        case JZ_LCD_TYPE_SPECIAL_TFT_1:
+        case JZ_LCD_TYPE_SPECIAL_TFT_2:
+        case JZ_LCD_TYPE_SPECIAL_TFT_3:
+                writel(pdata->special_tft_config.spl, jzfb->base + JZ_REG_LCD_SPL);
+                writel(pdata->special_tft_config.cls, jzfb->base + JZ_REG_LCD_CLS);
+                writel(pdata->special_tft_config.ps, jzfb->base + JZ_REG_LCD_PS);
+                writel(pdata->special_tft_config.ps, jzfb->base + JZ_REG_LCD_REV);
+                break;
+        default:
+                cfg |= JZ_LCD_CFG_PS_DISABLE;
+                cfg |= JZ_LCD_CFG_CLS_DISABLE;
+                cfg |= JZ_LCD_CFG_SPL_DISABLE;
+                cfg |= JZ_LCD_CFG_REV_DISABLE;
+                break;
+        }
+
+        writel(mode->hsync_len, jzfb->base + JZ_REG_LCD_HSYNC);
+        writel(mode->vsync_len, jzfb->base + JZ_REG_LCD_VSYNC);
+
+        writel((ht << 16) | vt, jzfb->base + JZ_REG_LCD_VAT);
+
+        writel((hds << 16) | hde, jzfb->base + JZ_REG_LCD_DAH);
+        writel((vds << 16) | vde, jzfb->base + JZ_REG_LCD_DAV);
+
+        writel(cfg, jzfb->base + JZ_REG_LCD_CFG);
+
+        writel(ctrl, jzfb->base + JZ_REG_LCD_CTRL);
+
+        if (!jzfb->is_enabled)
+                clk_disable(jzfb->ldclk);
+
+        mutex_unlock(&jzfb->lock);
+
+        clk_set_rate(jzfb->lpclk, rate);
+        clk_set_rate(jzfb->ldclk, rate * 3);
+
+        return 0;
+}
+
+static void jzfb_enable(struct jzfb *jzfb)
+{
+        uint32_t ctrl;
+
+        clk_enable(jzfb->ldclk);
+
+        jz_gpio_bulk_resume(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
+        jz_gpio_bulk_resume(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+
+        writel(0, jzfb->base + JZ_REG_LCD_STATE);
+
+        writel(jzfb->framedesc->next, jzfb->base + JZ_REG_LCD_DA0);
+
+        ctrl = readl(jzfb->base + JZ_REG_LCD_CTRL);
+        ctrl |= JZ_LCD_CTRL_ENABLE;
+        ctrl &= ~JZ_LCD_CTRL_DISABLE;
+        writel(ctrl, jzfb->base + JZ_REG_LCD_CTRL);
+}
+
+static void jzfb_disable(struct jzfb *jzfb)
+{
+        uint32_t ctrl;
+
+        ctrl = readl(jzfb->base + JZ_REG_LCD_CTRL);
+        ctrl |= JZ_LCD_CTRL_DISABLE;
+        writel(ctrl, jzfb->base + JZ_REG_LCD_CTRL);
+        do {
+                ctrl = readl(jzfb->base + JZ_REG_LCD_STATE);
+        } while (!(ctrl & JZ_LCD_STATE_DISABLED));
+
+        jz_gpio_bulk_suspend(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
+        jz_gpio_bulk_suspend(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+
+        clk_disable(jzfb->ldclk);
+}
+
+static int jzfb_blank(int blank_mode, struct fb_info *info)
+{
+        struct jzfb *jzfb = info->par;
+
+        switch (blank_mode) {
+        case FB_BLANK_UNBLANK:
+                mutex_lock(&jzfb->lock);
+                if (jzfb->is_enabled) {
+                        mutex_unlock(&jzfb->lock);
+                        return 0;
+                }
+
+                jzfb_enable(jzfb);
+                jzfb->is_enabled = 1;
+
+                mutex_unlock(&jzfb->lock);
+                break;
+        default:
+                mutex_lock(&jzfb->lock);
+                if (!jzfb->is_enabled) {
+                        mutex_unlock(&jzfb->lock);
+                        return 0;
+                }
+
+                jzfb_disable(jzfb);
+                jzfb->is_enabled = 0;
+
+                mutex_unlock(&jzfb->lock);
+                break;
+        }
+
+        return 0;
+}
+
+static int jzfb_alloc_devmem(struct jzfb *jzfb)
+{
+        int max_videosize = 0;
+        struct fb_videomode *mode = jzfb->pdata->modes;
+        void *page;
+        int i;
+
+        for (i = 0; i < jzfb->pdata->num_modes; ++mode, ++i) {
+                if (max_videosize < mode->xres * mode->yres)
+                        max_videosize = mode->xres * mode->yres;
+        }
+
+        max_videosize *= jzfb_get_controller_bpp(jzfb) >> 3;
+
+        jzfb->framedesc = dma_alloc_coherent(&jzfb->pdev->dev,
+                                        sizeof(*jzfb->framedesc),
+                                        &jzfb->framedesc_phys, GFP_KERNEL);
+
+        if (!jzfb->framedesc)
+                return -ENOMEM;
+
+        jzfb->vidmem_size = PAGE_ALIGN(max_videosize);
+        jzfb->vidmem = dma_alloc_coherent(&jzfb->pdev->dev,
+                                        jzfb->vidmem_size,
+                                        &jzfb->vidmem_phys, GFP_KERNEL);
+
+        if (!jzfb->vidmem)
+                goto err_free_framedesc;
+
+        for (page = jzfb->vidmem;
+                 page < jzfb->vidmem + PAGE_ALIGN(jzfb->vidmem_size);
+                 page += PAGE_SIZE) {
+                SetPageReserved(virt_to_page(page));
+        }
+
+        jzfb->framedesc->next = jzfb->framedesc_phys;
+        jzfb->framedesc->addr = jzfb->vidmem_phys;
+        jzfb->framedesc->id = 0xdeafbead;
+        jzfb->framedesc->cmd = 0;
+        jzfb->framedesc->cmd |= max_videosize / 4;
+
+        return 0;
+
+err_free_framedesc:
+        dma_free_coherent(&jzfb->pdev->dev, sizeof(*jzfb->framedesc),
+                                jzfb->framedesc, jzfb->framedesc_phys);
+        return -ENOMEM;
+}
+
+static void jzfb_free_devmem(struct jzfb *jzfb)
+{
+        dma_free_coherent(&jzfb->pdev->dev, jzfb->vidmem_size,
+                                jzfb->vidmem, jzfb->vidmem_phys);
+        dma_free_coherent(&jzfb->pdev->dev, sizeof(*jzfb->framedesc),
+                                jzfb->framedesc, jzfb->framedesc_phys);
+}
+
+static struct  fb_ops jzfb_ops = {
+        .owner = THIS_MODULE,
+        .fb_check_var = jzfb_check_var,
+        .fb_set_par = jzfb_set_par,
+        .fb_blank = jzfb_blank,
+        .fb_fillrect    = sys_fillrect,
+        .fb_copyarea    = sys_copyarea,
+        .fb_imageblit   = sys_imageblit,
+        .fb_setcolreg = jzfb_setcolreg,
+};
+
+static int __devinit jzfb_probe(struct platform_device *pdev)
+{
+        int ret;
+        struct jzfb *jzfb;
+        struct fb_info *fb;
+        struct jz4740_fb_platform_data *pdata = pdev->dev.platform_data;
+        struct resource *mem;
+
+        if (!pdata) {
+                dev_err(&pdev->dev, "Missing platform data\n");
+                return -ENXIO;
+        }
+
+        mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!mem) {
+                dev_err(&pdev->dev, "Failed to get register memory resource\n");
+                return -ENXIO;
+        }
+
+        mem = request_mem_region(mem->start, resource_size(mem), pdev->name);
+        if (!mem) {
+                dev_err(&pdev->dev, "Failed to request register memory region\n");
+                return -EBUSY;
+        }
+
+        fb = framebuffer_alloc(sizeof(struct jzfb), &pdev->dev);
+        if (!fb) {
+                dev_err(&pdev->dev, "Failed to allocate framebuffer device\n");
+                ret = -ENOMEM;
+                goto err_release_mem_region;
+        }
+
+        fb->fbops = &jzfb_ops;
+        fb->flags = FBINFO_DEFAULT;
+
+        jzfb = fb->par;
+        jzfb->pdev = pdev;
+        jzfb->pdata = pdata;
+        jzfb->mem = mem;
+
+        jzfb->ldclk = clk_get(&pdev->dev, "lcd");
+        if (IS_ERR(jzfb->ldclk)) {
+                ret = PTR_ERR(jzfb->ldclk);
+                dev_err(&pdev->dev, "Failed to get lcd clock: %d\n", ret);
+                goto err_framebuffer_release;
+        }
+
+        jzfb->lpclk = clk_get(&pdev->dev, "lcd_pclk");
+        if (IS_ERR(jzfb->lpclk)) {
+                ret = PTR_ERR(jzfb->lpclk);
+                dev_err(&pdev->dev, "Failed to get lcd pixel clock: %d\n", ret);
+                goto err_put_ldclk;
+        }
+
+        jzfb->base = ioremap(mem->start, resource_size(mem));
+        if (!jzfb->base) {
+                dev_err(&pdev->dev, "Failed to ioremap register memory region\n");
+                ret = -EBUSY;
+                goto err_put_lpclk;
+        }
+
+        platform_set_drvdata(pdev, jzfb);
+
+        mutex_init(&jzfb->lock);
+
+        fb_videomode_to_modelist(pdata->modes, pdata->num_modes,
+                                 &fb->modelist);
+        fb_videomode_to_var(&fb->var, pdata->modes);
+        fb->var.bits_per_pixel = pdata->bpp;
+        jzfb_check_var(&fb->var, fb);
+
+        ret = jzfb_alloc_devmem(jzfb);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to allocate video memory\n");
+                goto err_iounmap;
+        }
+
+        fb->fix = jzfb_fix;
+        fb->fix.line_length = fb->var.bits_per_pixel * fb->var.xres / 8;
+        fb->fix.mmio_start = mem->start;
+        fb->fix.mmio_len = resource_size(mem);
+        fb->fix.smem_start = jzfb->vidmem_phys;
+        fb->fix.smem_len =  fb->fix.line_length * fb->var.yres;
+        fb->screen_base = jzfb->vidmem;
+        fb->pseudo_palette = jzfb->pseudo_palette;
+
+        fb_alloc_cmap(&fb->cmap, 256, 0);
+
+        clk_enable(jzfb->ldclk);
+        jzfb->is_enabled = 1;
+
+        writel(jzfb->framedesc->next, jzfb->base + JZ_REG_LCD_DA0);
+
+        fb->mode = NULL;
+        jzfb_set_par(fb);
+
+        jz_gpio_bulk_request(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
+        jz_gpio_bulk_request(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+
+        ret = register_framebuffer(fb);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to register framebuffer: %d\n", ret);
+                goto err_free_devmem;
+        }
+
+        jzfb->fb = fb;
+
+        return 0;
+
+err_free_devmem:
+        jz_gpio_bulk_free(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
+        jz_gpio_bulk_free(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+
+        fb_dealloc_cmap(&fb->cmap);
+        jzfb_free_devmem(jzfb);
+err_iounmap:
+        iounmap(jzfb->base);
+err_put_lpclk:
+        clk_put(jzfb->lpclk);
+err_put_ldclk:
+        clk_put(jzfb->ldclk);
+err_framebuffer_release:
+        framebuffer_release(fb);
+err_release_mem_region:
+        release_mem_region(mem->start, resource_size(mem));
+        return ret;
+}
+
+static int __devexit jzfb_remove(struct platform_device *pdev)
+{
+        struct jzfb *jzfb = platform_get_drvdata(pdev);
+
+        jzfb_blank(FB_BLANK_POWERDOWN, jzfb->fb);
+
+        jz_gpio_bulk_free(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
+        jz_gpio_bulk_free(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+
+        iounmap(jzfb->base);
+        release_mem_region(jzfb->mem->start, resource_size(jzfb->mem));
+
+        fb_dealloc_cmap(&jzfb->fb->cmap);
+        jzfb_free_devmem(jzfb);
+
+        platform_set_drvdata(pdev, NULL);
+
+        clk_put(jzfb->lpclk);
+        clk_put(jzfb->ldclk);
+
+        framebuffer_release(jzfb->fb);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int jzfb_suspend(struct device *dev)
+{
+        struct jzfb *jzfb = dev_get_drvdata(dev);
+
+        acquire_console_sem();
+        fb_set_suspend(jzfb->fb, 1);
+        release_console_sem();
+
+        mutex_lock(&jzfb->lock);
+        if (jzfb->is_enabled)
+                jzfb_disable(jzfb);
+        mutex_unlock(&jzfb->lock);
+
+        return 0;
+}
+
+static int jzfb_resume(struct device *dev)
+{
+        struct jzfb *jzfb = dev_get_drvdata(dev);
+        clk_enable(jzfb->ldclk);
+
+        mutex_lock(&jzfb->lock);
+        if (jzfb->is_enabled)
+                jzfb_enable(jzfb);
+        mutex_unlock(&jzfb->lock);
+
+        acquire_console_sem();
+        fb_set_suspend(jzfb->fb, 0);
+        release_console_sem();
+
+        return 0;
+}
+
+static const struct dev_pm_ops jzfb_pm_ops = {
+        .suspend        = jzfb_suspend,
+        .resume         = jzfb_resume,
+        .poweroff       = jzfb_suspend,
+        .restore        = jzfb_resume,
+};
+
+#define JZFB_PM_OPS (&jzfb_pm_ops)
+
+#else
+#define JZFB_PM_OPS NULL
+#endif
+
+static struct platform_driver jzfb_driver = {
+        .probe = jzfb_probe,
+        .remove = __devexit_p(jzfb_remove),
+        .driver = {
+                .name = "jz4740-fb",
+                .pm = JZFB_PM_OPS,
+        },
+};
+
+static int __init jzfb_init(void)
+{
+        return platform_driver_register(&jzfb_driver);
+}
+module_init(jzfb_init);
+
+static void __exit jzfb_exit(void)
+{
+        platform_driver_unregister(&jzfb_driver);
+}
+module_exit(jzfb_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("JZ4740 SoC LCD framebuffer driver");
+MODULE_ALIAS("platform:jz4740-fb");
diff --git a/drivers/video/leo.c b/drivers/video/leo.c
index 9e8bf7d5e249..b599e5e36ced 100644
--- a/drivers/video/leo.c
+++ b/drivers/video/leo.c
@@ -529,7 +529,7 @@ static void leo_fixup_var_rgb(struct fb_var_screeninfo *var)
         var->transp.length = 0;
 }
 
-static void leo_unmap_regs(struct of_device *op, struct fb_info *info,
+static void leo_unmap_regs(struct platform_device *op, struct fb_info *info,
                            struct leo_par *par)
 {
         if (par->lc_ss0_usr)
@@ -547,7 +547,7 @@ static void leo_unmap_regs(struct of_device *op, struct fb_info *info,
                 of_iounmap(&op->resource[0], info->screen_base, 0x800000);
 }
 
-static int __devinit leo_probe(struct of_device *op,
+static int __devinit leo_probe(struct platform_device *op,
                                const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
@@ -637,7 +637,7 @@ out_err:
         return err;
 }
 
-static int __devexit leo_remove(struct of_device *op)
+static int __devexit leo_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct leo_par *par = info->par;
@@ -677,12 +677,12 @@ static int __init leo_init(void)
         if (fb_get_options("leofb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&leo_driver, &of_bus_type);
+        return of_register_platform_driver(&leo_driver);
 }
 
 static void __exit leo_exit(void)
 {
-        of_unregister_driver(&leo_driver);
+        of_unregister_platform_driver(&leo_driver);
 }
 
 module_init(leo_init);
diff --git a/drivers/video/matrox/i2c-matroxfb.c b/drivers/video/matrox/i2c-matroxfb.c
index 403b14445a78..0fb280ead3dc 100644
--- a/drivers/video/matrox/i2c-matroxfb.c
+++ b/drivers/video/matrox/i2c-matroxfb.c
@@ -191,7 +191,7 @@ static void* i2c_matroxfb_probe(struct matrox_fb_info* minfo) {
                         };
 
                         i2c_new_probed_device(&m2info->maven.adapter,
-                                              &maven_info, addr_list);
+                                              &maven_info, addr_list, NULL);
                 }
         }
         return m2info;
diff --git a/drivers/video/matrox/matroxfb_DAC1064.c b/drivers/video/matrox/matroxfb_DAC1064.c
index f9fa0fd00292..1717623aabc0 100644
--- a/drivers/video/matrox/matroxfb_DAC1064.c
+++ b/drivers/video/matrox/matroxfb_DAC1064.c
@@ -869,12 +869,9 @@ static int MGAG100_preinit(struct matrox_fb_info *minfo)
         minfo->capable.plnwt = minfo->devflags.accelerator == FB_ACCEL_MATROX_MGAG100
                         ? minfo->devflags.sgram : 1;
 
-#ifdef CONFIG_FB_MATROX_G
         if (minfo->devflags.g450dac) {
                 minfo->outputs[0].output = &g450out;
-        } else
-#endif
-        {
+        } else {
                 minfo->outputs[0].output = &m1064;
         }
         minfo->outputs[0].src = minfo->outputs[0].default_src;
diff --git a/drivers/video/matrox/matroxfb_base.h b/drivers/video/matrox/matroxfb_base.h
index f3a4e15672d9..f96a471cb1a8 100644
--- a/drivers/video/matrox/matroxfb_base.h
+++ b/drivers/video/matrox/matroxfb_base.h
@@ -151,13 +151,13 @@ static inline void mga_writel(vaddr_t va, unsigned int offs, u_int32_t value) {
 static inline void mga_memcpy_toio(vaddr_t va, const void* src, int len) {
 #if defined(__alpha__) || defined(__i386__) || defined(__x86_64__)
         /*
-         * memcpy_toio works for us if:
+         * iowrite32_rep works for us if:
          *  (1) Copies data as 32bit quantities, not byte after byte,
          *  (2) Performs LE ordered stores, and
          *  (3) It copes with unaligned source (destination is guaranteed to be page
          *      aligned and length is guaranteed to be multiple of 4).
          */
-        memcpy_toio(va.vaddr, src, len);
+        iowrite32_rep(va.vaddr, src, len >> 2);
 #else
         u_int32_t __iomem* addr = va.vaddr;
 
diff --git a/drivers/video/matrox/matroxfb_maven.c b/drivers/video/matrox/matroxfb_maven.c
index 1e3e8f19783e..31b8f67477b7 100644
--- a/drivers/video/matrox/matroxfb_maven.c
+++ b/drivers/video/matrox/matroxfb_maven.c
@@ -280,7 +280,7 @@ static int matroxfb_PLL_mavenclock(const struct matrox_pll_features2* pll,
         return fxtal * (*feed) / (*in) * ctl->den;
 }
 
-static unsigned int matroxfb_mavenclock(const struct matrox_pll_ctl* ctl,
+static int matroxfb_mavenclock(const struct matrox_pll_ctl *ctl,
                 unsigned int htotal, unsigned int vtotal,
                 unsigned int* in, unsigned int* feed, unsigned int* post,
                 unsigned int* htotal2) {
diff --git a/drivers/video/mb862xx/mb862xxfb.c b/drivers/video/mb862xx/mb862xxfb.c
index 4e2b8cc3d460..b1c4374cf940 100644
--- a/drivers/video/mb862xx/mb862xxfb.c
+++ b/drivers/video/mb862xx/mb862xxfb.c
@@ -550,7 +550,7 @@ static int mb862xx_gdc_init(struct mb862xxfb_par *par)
         return 0;
 }
 
-static int __devinit of_platform_mb862xx_probe(struct of_device *ofdev,
+static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev,
                                                const struct of_device_id *id)
 {
         struct device_node *np = ofdev->dev.of_node;
@@ -669,7 +669,7 @@ fbrel:
         return ret;
 }
 
-static int __devexit of_platform_mb862xx_remove(struct of_device *ofdev)
+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;
diff --git a/drivers/video/mbx/mbxdebugfs.c b/drivers/video/mbx/mbxdebugfs.c
index ecad96524570..12dec7634c55 100644
--- a/drivers/video/mbx/mbxdebugfs.c
+++ b/drivers/video/mbx/mbxdebugfs.c
@@ -175,36 +175,42 @@ static const struct file_operations sysconf_fops = {
         .read = sysconf_read_file,
         .write = write_file_dummy,
         .open = open_file_generic,
+        .llseek = default_llseek,
 };
 
 static const struct file_operations clock_fops = {
         .read = clock_read_file,
         .write = write_file_dummy,
         .open = open_file_generic,
+        .llseek = default_llseek,
 };
 
 static const struct file_operations display_fops = {
         .read = display_read_file,
         .write = write_file_dummy,
         .open = open_file_generic,
+        .llseek = default_llseek,
 };
 
 static const struct file_operations gsctl_fops = {
         .read = gsctl_read_file,
         .write = write_file_dummy,
         .open = open_file_generic,
+        .llseek = default_llseek,
 };
 
 static const struct file_operations sdram_fops = {
         .read = sdram_read_file,
         .write = write_file_dummy,
         .open = open_file_generic,
+        .llseek = default_llseek,
 };
 
 static const struct file_operations misc_fops = {
         .read = misc_read_file,
         .write = write_file_dummy,
         .open = open_file_generic,
+        .llseek = default_llseek,
 };
 
 static void __devinit mbxfb_debugfs_init(struct fb_info *fbi)
diff --git a/drivers/video/metronomefb.c b/drivers/video/metronomefb.c
index 9b3d6e4584cc..63ed3b72b01c 100644
--- a/drivers/video/metronomefb.c
+++ b/drivers/video/metronomefb.c
@@ -10,7 +10,7 @@
  * Layout is based on skeletonfb.c by James Simmons and Geert Uytterhoeven.
  *
  * This work was made possible by help and equipment support from E-Ink
- * Corporation. http://support.eink.com/community
+ * Corporation. http://www.eink.com/
  *
  * This driver is written to be used with the Metronome display controller.
  * It is intended to be architecture independent. A board specific driver
diff --git a/drivers/video/msm/mddi.c b/drivers/video/msm/mddi.c
index c1ff271017a9..7c316c34dfca 100644
--- a/drivers/video/msm/mddi.c
+++ b/drivers/video/msm/mddi.c
@@ -187,10 +187,8 @@ static void mddi_wait_interrupt(struct mddi_info *mddi, uint32_t intmask);
 
 static void mddi_handle_rev_data_avail(struct mddi_info *mddi)
 {
-        union mddi_rev *rev = mddi->rev_data;
         uint32_t rev_data_count;
         uint32_t rev_crc_err_count;
-        int i;
         struct reg_read_info *ri;
         size_t prev_offset;
         uint16_t length;
@@ -670,7 +668,7 @@ static int __init mddi_rev_data_setup(struct mddi_info *mddi)
         return 0;
 }
 
-static int __init mddi_probe(struct platform_device *pdev)
+static int __devinit mddi_probe(struct platform_device *pdev)
 {
         struct msm_mddi_platform_data *pdata = pdev->dev.platform_data;
         struct mddi_info *mddi = &mddi_info[pdev->id];
diff --git a/drivers/video/msm/mdp.c b/drivers/video/msm/mdp.c
index 19c01c6208e8..3c28db03ad39 100644
--- a/drivers/video/msm/mdp.c
+++ b/drivers/video/msm/mdp.c
@@ -258,7 +258,6 @@ int get_img(struct mdp_img *img, struct fb_info *info,
 {
         int put_needed, ret = 0;
         struct file *file;
-        unsigned long vstart;
 
         file = fget_light(img->memory_id, &put_needed);
         if (file == NULL)
diff --git a/drivers/video/offb.c b/drivers/video/offb.c
index 46dda7d8aaee..cb163a5397be 100644
--- a/drivers/video/offb.c
+++ b/drivers/video/offb.c
@@ -19,13 +19,14 @@
 #include <linux/mm.h>
 #include <linux/vmalloc.h>
 #include <linux/delay.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/interrupt.h>
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/ioport.h>
 #include <linux/pci.h>
 #include <asm/io.h>
-#include <asm/prom.h>
 
 #ifdef CONFIG_PPC64
 #include <asm/pci-bridge.h>
diff --git a/drivers/video/omap/blizzard.c b/drivers/video/omap/blizzard.c
index 2ffb34af4c59..87785c215a52 100644
--- a/drivers/video/omap/blizzard.c
+++ b/drivers/video/omap/blizzard.c
@@ -1590,7 +1590,7 @@ static int blizzard_init(struct omapfb_device *fbdev, int ext_mode,
         blizzard.auto_update_window.width = fbdev->panel->x_res;
         blizzard.auto_update_window.height = fbdev->panel->y_res;
         blizzard.auto_update_window.out_x = 0;
-        blizzard.auto_update_window.out_x = 0;
+        blizzard.auto_update_window.out_y = 0;
         blizzard.auto_update_window.out_width = fbdev->panel->x_res;
         blizzard.auto_update_window.out_height = fbdev->panel->y_res;
         blizzard.auto_update_window.format = 0;
diff --git a/drivers/video/omap/lcd_apollon.c b/drivers/video/omap/lcd_apollon.c
index 2be94eb3bbf5..10459d8bd9a0 100644
--- a/drivers/video/omap/lcd_apollon.c
+++ b/drivers/video/omap/lcd_apollon.c
@@ -25,7 +25,6 @@
 #include <linux/platform_device.h>
 
 #include <mach/gpio.h>
-#include <plat/mux.h>
 
 #include "omapfb.h"
 
@@ -34,8 +33,6 @@
 static int apollon_panel_init(struct lcd_panel *panel,
                                 struct omapfb_device *fbdev)
 {
-        /* configure LCD PWR_EN */
-        omap_cfg_reg(M21_242X_GPIO11);
         return 0;
 }
 
diff --git a/drivers/video/omap/lcd_omap3beagle.c b/drivers/video/omap/lcd_omap3beagle.c
index ca75cc2a87a5..d7c6c3e0afc6 100644
--- a/drivers/video/omap/lcd_omap3beagle.c
+++ b/drivers/video/omap/lcd_omap3beagle.c
@@ -25,8 +25,6 @@
 #include <linux/gpio.h>
 #include <linux/i2c/twl.h>
 
-#include <plat/mux.h>
-#include <plat/mux.h>
 #include <asm/mach-types.h>
 
 #include "omapfb.h"
diff --git a/drivers/video/omap2/displays/Kconfig b/drivers/video/omap2/displays/Kconfig
index 881c9f77c75a..12327bbfdbbb 100644
--- a/drivers/video/omap2/displays/Kconfig
+++ b/drivers/video/omap2/displays/Kconfig
@@ -40,7 +40,7 @@ config PANEL_TPO_TD043MTEA1
 
 config PANEL_ACX565AKM
         tristate "ACX565AKM Panel"
-        depends on OMAP2_DSS_SDI
+        depends on OMAP2_DSS_SDI && SPI
         select BACKLIGHT_CLASS_DEVICE
         help
           This is the LCD panel used on Nokia N900
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index 1f8eb70e2937..e77310653207 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -587,12 +587,15 @@ static int acx_panel_power_on(struct omap_dss_device *dssdev)
 
         dev_dbg(&dssdev->dev, "%s\n", __func__);
 
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
         mutex_lock(&md->mutex);
 
         r = omapdss_sdi_display_enable(dssdev);
         if (r) {
                 pr_err("%s sdi enable failed\n", __func__);
-                return r;
+                goto fail_unlock;
         }
 
         /*FIXME tweak me */
@@ -633,6 +636,8 @@ static int acx_panel_power_on(struct omap_dss_device *dssdev)
         return acx565akm_bl_update_status(md->bl_dev);
 fail:
         omapdss_sdi_display_disable(dssdev);
+fail_unlock:
+        mutex_unlock(&md->mutex);
         return r;
 }
 
@@ -642,6 +647,9 @@ static void acx_panel_power_off(struct omap_dss_device *dssdev)
 
         dev_dbg(&dssdev->dev, "%s\n", __func__);
 
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
         mutex_lock(&md->mutex);
 
         if (!md->enabled) {
diff --git a/drivers/video/omap2/displays/panel-generic.c b/drivers/video/omap2/displays/panel-generic.c
index 300eff5de1b4..395a68de3990 100644
--- a/drivers/video/omap2/displays/panel-generic.c
+++ b/drivers/video/omap2/displays/panel-generic.c
@@ -39,6 +39,9 @@ static int generic_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;
@@ -58,6 +61,9 @@ err0:
 
 static void generic_panel_power_off(struct omap_dss_device *dssdev)
 {
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
         if (dssdev->platform_disable)
                 dssdev->platform_disable(dssdev);
 
diff --git a/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c b/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c
index 10267461991c..0c6896cea2d0 100644
--- a/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c
+++ b/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c
@@ -43,6 +43,9 @@ static int sharp_lq_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;
@@ -65,6 +68,9 @@ err0:
 
 static void sharp_lq_panel_power_off(struct omap_dss_device *dssdev)
 {
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
         if (dssdev->platform_disable)
                 dssdev->platform_disable(dssdev);
 
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index 7d9eb2b1f5af..9a138f650e05 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -135,6 +135,9 @@ static int sharp_ls_power_on(struct omap_dss_device *dssdev)
 {
         int r = 0;
 
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
@@ -157,6 +160,9 @@ err0:
 
 static void sharp_ls_power_off(struct omap_dss_device *dssdev)
 {
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
         if (dssdev->platform_disable)
                 dssdev->platform_disable(dssdev);
 
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index aaf5d308a046..e1c765d11419 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -28,12 +28,13 @@
 #include <linux/fb.h>
 #include <linux/interrupt.h>
 #include <linux/gpio.h>
-#include <linux/completion.h>
 #include <linux/workqueue.h>
 #include <linux/slab.h>
+#include <linux/regulator/consumer.h>
 #include <linux/mutex.h>
 
 #include <plat/display.h>
+#include <plat/nokia-dsi-panel.h>
 
 /* DSI Virtual channel. Hardcoded for now. */
 #define TCH 0
@@ -62,11 +63,136 @@
 #define DCS_GET_ID2             0xdb
 #define DCS_GET_ID3             0xdc
 
-/* #define TAAL_USE_ESD_CHECK */
 #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);
 
+struct panel_regulator {
+        struct regulator *regulator;
+        const char *name;
+        int min_uV;
+        int max_uV;
+};
+
+static void free_regulators(struct panel_regulator *regulators, int n)
+{
+        int i;
+
+        for (i = 0; i < n; i++) {
+                /* disable/put in reverse order */
+                regulator_disable(regulators[n - i - 1].regulator);
+                regulator_put(regulators[n - i - 1].regulator);
+        }
+}
+
+static int init_regulators(struct omap_dss_device *dssdev,
+                        struct panel_regulator *regulators, int n)
+{
+        int r, i, v;
+
+        for (i = 0; i < n; i++) {
+                struct regulator *reg;
+
+                reg = regulator_get(&dssdev->dev, regulators[i].name);
+                if (IS_ERR(reg)) {
+                        dev_err(&dssdev->dev, "failed to get regulator %s\n",
+                                regulators[i].name);
+                        r = PTR_ERR(reg);
+                        goto err;
+                }
+
+                /* FIXME: better handling of fixed vs. variable regulators */
+                v = regulator_get_voltage(reg);
+                if (v < regulators[i].min_uV || v > regulators[i].max_uV) {
+                        r = regulator_set_voltage(reg, regulators[i].min_uV,
+                                                regulators[i].max_uV);
+                        if (r) {
+                                dev_err(&dssdev->dev,
+                                        "failed to set regulator %s voltage\n",
+                                        regulators[i].name);
+                                regulator_put(reg);
+                                goto err;
+                        }
+                }
+
+                r = regulator_enable(reg);
+                if (r) {
+                        dev_err(&dssdev->dev, "failed to enable regulator %s\n",
+                                regulators[i].name);
+                        regulator_put(reg);
+                        goto err;
+                }
+
+                regulators[i].regulator = reg;
+        }
+
+        return 0;
+
+err:
+        free_regulators(regulators, i);
+
+        return r;
+}
+
+/**
+ * struct panel_config - panel configuration
+ * @name: panel name
+ * @type: panel type
+ * @timings: panel resolution
+ * @sleep: various panel specific delays, passed to msleep() if non-zero
+ * @reset_sequence: reset sequence timings, passed to udelay() if non-zero
+ * @regulators: array of panel regulators
+ * @num_regulators: number of regulators in the array
+ */
+struct panel_config {
+        const char *name;
+        int type;
+
+        struct omap_video_timings timings;
+
+        struct {
+                unsigned int sleep_in;
+                unsigned int sleep_out;
+                unsigned int hw_reset;
+                unsigned int enable_te;
+        } sleep;
+
+        struct {
+                unsigned int high;
+                unsigned int low;
+        } reset_sequence;
+
+        struct panel_regulator *regulators;
+        int num_regulators;
+};
+
+enum {
+        PANEL_TAAL,
+};
+
+static struct panel_config panel_configs[] = {
+        {
+                .name           = "taal",
+                .type           = PANEL_TAAL,
+                .timings        = {
+                        .x_res          = 864,
+                        .y_res          = 480,
+                },
+                .sleep          = {
+                        .sleep_in       = 5,
+                        .sleep_out      = 5,
+                        .hw_reset       = 5,
+                        .enable_te      = 100, /* possible panel bug */
+                },
+                .reset_sequence = {
+                        .high           = 10,
+                        .low            = 10,
+                },
+        },
+};
+
 struct taal_data {
         struct mutex lock;
 
@@ -84,8 +210,15 @@ struct taal_data {
         bool mirror;
 
         bool te_enabled;
-        bool use_ext_te;
-        struct completion te_completion;
+
+        atomic_t do_update;
+        struct {
+                u16 x;
+                u16 y;
+                u16 w;
+                u16 h;
+        } update_region;
+        struct delayed_work te_timeout_work;
 
         bool use_dsi_bl;
 
@@ -96,8 +229,16 @@ struct taal_data {
 
         struct workqueue_struct *esd_wq;
         struct delayed_work esd_work;
+
+        struct panel_config *panel_config;
 };
 
+static inline struct nokia_dsi_panel_data
+*get_panel_data(const struct omap_dss_device *dssdev)
+{
+        return (struct nokia_dsi_panel_data *) dssdev->data;
+}
+
 static void taal_esd_work(struct work_struct *work);
 
 static void hw_guard_start(struct taal_data *td, int guard_msec)
@@ -159,7 +300,8 @@ static int taal_sleep_in(struct taal_data *td)
 
         hw_guard_start(td, 120);
 
-        msleep(5);
+        if (td->panel_config->sleep.sleep_in)
+                msleep(td->panel_config->sleep.sleep_in);
 
         return 0;
 }
@@ -176,7 +318,8 @@ static int taal_sleep_out(struct taal_data *td)
 
         hw_guard_start(td, 120);
 
-        msleep(5);
+        if (td->panel_config->sleep.sleep_out)
+                msleep(td->panel_config->sleep.sleep_out);
 
         return 0;
 }
@@ -279,6 +422,7 @@ static int taal_bl_update_status(struct backlight_device *dev)
 {
         struct omap_dss_device *dssdev = dev_get_drvdata(&dev->dev);
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
         int level;
 
@@ -290,24 +434,26 @@ static int taal_bl_update_status(struct backlight_device *dev)
 
         dev_dbg(&dssdev->dev, "update brightness to %d\n", level);
 
+        mutex_lock(&td->lock);
+
         if (td->use_dsi_bl) {
                 if (td->enabled) {
                         dsi_bus_lock();
                         r = taal_dcs_write_1(DCS_BRIGHTNESS, level);
                         dsi_bus_unlock();
-                        if (r)
-                                return r;
+                } else {
+                        r = 0;
                 }
         } else {
-                if (!dssdev->set_backlight)
-                        return -EINVAL;
-
-                r = dssdev->set_backlight(dssdev, level);
-                if (r)
-                        return r;
+                if (!panel_data->set_backlight)
+                        r = -EINVAL;
+                else
+                        r = panel_data->set_backlight(dssdev, level);
         }
 
-        return 0;
+        mutex_unlock(&td->lock);
+
+        return r;
 }
 
 static int taal_bl_get_intensity(struct backlight_device *dev)
@@ -344,16 +490,6 @@ static void taal_get_resolution(struct omap_dss_device *dssdev,
         }
 }
 
-static irqreturn_t taal_te_isr(int irq, void *data)
-{
-        struct omap_dss_device *dssdev = data;
-        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-
-        complete_all(&td->te_completion);
-
-        return IRQ_HANDLED;
-}
-
 static ssize_t taal_num_errors_show(struct device *dev,
                 struct device_attribute *attr, char *buf)
 {
@@ -362,6 +498,8 @@ static ssize_t taal_num_errors_show(struct device *dev,
         u8 errors;
         int r;
 
+        mutex_lock(&td->lock);
+
         if (td->enabled) {
                 dsi_bus_lock();
                 r = taal_dcs_read_1(DCS_READ_NUM_ERRORS, &errors);
@@ -370,6 +508,8 @@ static ssize_t taal_num_errors_show(struct device *dev,
                 r = -ENODEV;
         }
 
+        mutex_unlock(&td->lock);
+
         if (r)
                 return r;
 
@@ -384,6 +524,8 @@ static ssize_t taal_hw_revision_show(struct device *dev,
         u8 id1, id2, id3;
         int r;
 
+        mutex_lock(&td->lock);
+
         if (td->enabled) {
                 dsi_bus_lock();
                 r = taal_get_id(&id1, &id2, &id3);
@@ -392,6 +534,8 @@ static ssize_t taal_hw_revision_show(struct device *dev,
                 r = -ENODEV;
         }
 
+        mutex_unlock(&td->lock);
+
         if (r)
                 return r;
 
@@ -441,6 +585,8 @@ static ssize_t store_cabc_mode(struct device *dev,
         if (i == ARRAY_SIZE(cabc_modes))
                 return -EINVAL;
 
+        mutex_lock(&td->lock);
+
         if (td->enabled) {
                 dsi_bus_lock();
                 if (!td->cabc_broken)
@@ -450,6 +596,8 @@ static ssize_t store_cabc_mode(struct device *dev,
 
         td->cabc_mode = i;
 
+        mutex_unlock(&td->lock);
+
         return count;
 }
 
@@ -488,47 +636,93 @@ static struct attribute_group taal_attr_group = {
         .attrs = taal_attrs,
 };
 
+static void taal_hw_reset(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);
+
+        if (panel_data->reset_gpio == -1)
+                return;
+
+        gpio_set_value(panel_data->reset_gpio, 1);
+        if (td->panel_config->reset_sequence.high)
+                udelay(td->panel_config->reset_sequence.high);
+        /* reset the panel */
+        gpio_set_value(panel_data->reset_gpio, 0);
+        /* assert reset */
+        if (td->panel_config->reset_sequence.low)
+                udelay(td->panel_config->reset_sequence.low);
+        gpio_set_value(panel_data->reset_gpio, 1);
+        /* wait after releasing reset */
+        if (td->panel_config->sleep.hw_reset)
+                msleep(td->panel_config->sleep.hw_reset);
+}
+
 static int taal_probe(struct omap_dss_device *dssdev)
 {
         struct backlight_properties props;
         struct taal_data *td;
         struct backlight_device *bldev;
-        int r;
-
-        const struct omap_video_timings taal_panel_timings = {
-                .x_res          = 864,
-                .y_res          = 480,
-        };
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+        struct panel_config *panel_config = NULL;
+        int r, i;
 
         dev_dbg(&dssdev->dev, "probe\n");
 
+        if (!panel_data || !panel_data->name) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(panel_configs); i++) {
+                if (strcmp(panel_data->name, panel_configs[i].name) == 0) {
+                        panel_config = &panel_configs[i];
+                        break;
+                }
+        }
+
+        if (!panel_config) {
+                r = -EINVAL;
+                goto err;
+        }
+
         dssdev->panel.config = OMAP_DSS_LCD_TFT;
-        dssdev->panel.timings = taal_panel_timings;
+        dssdev->panel.timings = panel_config->timings;
         dssdev->ctrl.pixel_size = 24;
 
         td = kzalloc(sizeof(*td), GFP_KERNEL);
         if (!td) {
                 r = -ENOMEM;
-                goto err0;
+                goto err;
         }
         td->dssdev = dssdev;
+        td->panel_config = panel_config;
 
         mutex_init(&td->lock);
 
+        atomic_set(&td->do_update, 0);
+
+        r = init_regulators(dssdev, panel_config->regulators,
+                        panel_config->num_regulators);
+        if (r)
+                goto err_reg;
+
         td->esd_wq = create_singlethread_workqueue("taal_esd");
         if (td->esd_wq == NULL) {
                 dev_err(&dssdev->dev, "can't create ESD workqueue\n");
                 r = -ENOMEM;
-                goto err1;
+                goto err_wq;
         }
         INIT_DELAYED_WORK_DEFERRABLE(&td->esd_work, taal_esd_work);
 
         dev_set_drvdata(&dssdev->dev, td);
 
+        taal_hw_reset(dssdev);
+
         /* if no platform set_backlight() defined, presume DSI backlight
          * control */
         memset(&props, 0, sizeof(struct backlight_properties));
-        if (!dssdev->set_backlight)
+        if (!panel_data->set_backlight)
                 td->use_dsi_bl = true;
 
         if (td->use_dsi_bl)
@@ -539,7 +733,7 @@ static int taal_probe(struct omap_dss_device *dssdev)
                                           &taal_bl_ops, &props);
         if (IS_ERR(bldev)) {
                 r = PTR_ERR(bldev);
-                goto err2;
+                goto err_bl;
         }
 
         td->bldev = bldev;
@@ -553,13 +747,13 @@ static int taal_probe(struct omap_dss_device *dssdev)
 
         taal_bl_update_status(bldev);
 
-        if (dssdev->phy.dsi.ext_te) {
-                int gpio = dssdev->phy.dsi.ext_te_gpio;
+        if (panel_data->use_ext_te) {
+                int gpio = panel_data->ext_te_gpio;
 
                 r = gpio_request(gpio, "taal irq");
                 if (r) {
                         dev_err(&dssdev->dev, "GPIO request failed\n");
-                        goto err3;
+                        goto err_gpio;
                 }
 
                 gpio_direction_input(gpio);
@@ -571,49 +765,52 @@ static int taal_probe(struct omap_dss_device *dssdev)
                 if (r) {
                         dev_err(&dssdev->dev, "IRQ request failed\n");
                         gpio_free(gpio);
-                        goto err3;
+                        goto err_irq;
                 }
 
-                init_completion(&td->te_completion);
+                INIT_DELAYED_WORK_DEFERRABLE(&td->te_timeout_work,
+                                        taal_te_timeout_work_callback);
 
-                td->use_ext_te = true;
+                dev_dbg(&dssdev->dev, "Using GPIO TE\n");
         }
 
         r = sysfs_create_group(&dssdev->dev.kobj, &taal_attr_group);
         if (r) {
                 dev_err(&dssdev->dev, "failed to create sysfs files\n");
-                goto err4;
+                goto err_sysfs;
         }
 
         return 0;
-err4:
-        if (td->use_ext_te) {
-                int gpio = dssdev->phy.dsi.ext_te_gpio;
-                free_irq(gpio_to_irq(gpio), dssdev);
-                gpio_free(gpio);
-        }
-err3:
+err_sysfs:
+        if (panel_data->use_ext_te)
+                free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
+err_irq:
+        if (panel_data->use_ext_te)
+                gpio_free(panel_data->ext_te_gpio);
+err_gpio:
         backlight_device_unregister(bldev);
-err2:
-        cancel_delayed_work_sync(&td->esd_work);
+err_bl:
         destroy_workqueue(td->esd_wq);
-err1:
+err_wq:
+        free_regulators(panel_config->regulators, panel_config->num_regulators);
+err_reg:
         kfree(td);
-err0:
+err:
         return r;
 }
 
 static void 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);
         struct backlight_device *bldev;
 
         dev_dbg(&dssdev->dev, "remove\n");
 
         sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
 
-        if (td->use_ext_te) {
-                int gpio = dssdev->phy.dsi.ext_te_gpio;
+        if (panel_data->use_ext_te) {
+                int gpio = panel_data->ext_te_gpio;
                 free_irq(gpio_to_irq(gpio), dssdev);
                 gpio_free(gpio);
         }
@@ -623,9 +820,15 @@ static void taal_remove(struct omap_dss_device *dssdev)
         taal_bl_update_status(bldev);
         backlight_device_unregister(bldev);
 
-        cancel_delayed_work_sync(&td->esd_work);
+        cancel_delayed_work(&td->esd_work);
         destroy_workqueue(td->esd_wq);
 
+        /* reset, to be sure that the panel is in a valid state */
+        taal_hw_reset(dssdev);
+
+        free_regulators(td->panel_config->regulators,
+                        td->panel_config->num_regulators);
+
         kfree(td);
 }
 
@@ -635,23 +838,14 @@ static int taal_power_on(struct omap_dss_device *dssdev)
         u8 id1, id2, id3;
         int r;
 
-        if (dssdev->platform_enable) {
-                r = dssdev->platform_enable(dssdev);
-                if (r)
-                        return r;
-        }
-
-        /* it seems we have to wait a bit until taal is ready */
-        msleep(5);
-
-        dsi_bus_lock();
-
         r = omapdss_dsi_display_enable(dssdev);
         if (r) {
                 dev_err(&dssdev->dev, "failed to enable DSI\n");
                 goto err0;
         }
 
+        taal_hw_reset(dssdev);
+
         omapdss_dsi_vc_enable_hs(TCH, false);
 
         r = taal_sleep_out(td);
@@ -662,34 +856,47 @@ static int taal_power_on(struct omap_dss_device *dssdev)
         if (r)
                 goto err;
 
-        /* on early revisions CABC is broken */
-        if (id2 == 0x00 || id2 == 0xff || id2 == 0x81)
+        /* on early Taal revisions CABC is broken */
+        if (td->panel_config->type == PANEL_TAAL &&
+                (id2 == 0x00 || id2 == 0xff || id2 == 0x81))
                 td->cabc_broken = true;
 
-        taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
-        taal_dcs_write_1(DCS_CTRL_DISPLAY, (1<<2) | (1<<5)); /* BL | BCTRL */
+        r = taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
+        if (r)
+                goto err;
+
+        r = taal_dcs_write_1(DCS_CTRL_DISPLAY,
+                        (1<<2) | (1<<5));       /* BL | BCTRL */
+        if (r)
+                goto err;
+
+        r = taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+        if (r)
+                goto err;
 
-        taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+        r = taal_set_addr_mode(td->rotate, td->mirror);
+        if (r)
+                goto err;
 
-        taal_set_addr_mode(td->rotate, td->mirror);
-        if (!td->cabc_broken)
-                taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+        if (!td->cabc_broken) {
+                r = taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+                if (r)
+                        goto err;
+        }
 
-        taal_dcs_write_0(DCS_DISPLAY_ON);
+        r = taal_dcs_write_0(DCS_DISPLAY_ON);
+        if (r)
+                goto err;
 
         r = _taal_enable_te(dssdev, td->te_enabled);
         if (r)
                 goto err;
 
-#ifdef TAAL_USE_ESD_CHECK
-        queue_delayed_work(td->esd_wq, &td->esd_work, TAAL_ESD_CHECK_PERIOD);
-#endif
-
         td->enabled = 1;
 
         if (!td->intro_printed) {
-                dev_info(&dssdev->dev, "revision %02x.%02x.%02x\n",
-                                id1, id2, id3);
+                dev_info(&dssdev->dev, "%s panel revision %02x.%02x.%02x\n",
+                        td->panel_config->name, id1, id2, id3);
                 if (td->cabc_broken)
                         dev_info(&dssdev->dev,
                                         "old Taal version, CABC disabled\n");
@@ -698,46 +905,44 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 
         omapdss_dsi_vc_enable_hs(TCH, true);
 
-        dsi_bus_unlock();
-
         return 0;
 err:
+        dev_err(&dssdev->dev, "error while enabling panel, issuing HW reset\n");
+
+        taal_hw_reset(dssdev);
+
         omapdss_dsi_display_disable(dssdev);
 err0:
-        dsi_bus_unlock();
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
         return r;
 }
 
 static void taal_power_off(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        int r;
 
-        dsi_bus_lock();
-
-        cancel_delayed_work(&td->esd_work);
-
-        taal_dcs_write_0(DCS_DISPLAY_OFF);
-        taal_sleep_in(td);
+        r = taal_dcs_write_0(DCS_DISPLAY_OFF);
+        if (!r) {
+                r = taal_sleep_in(td);
+                /* HACK: wait a bit so that the message goes through */
+                msleep(10);
+        }
 
-        /* wait a bit so that the message goes through */
-        msleep(10);
+        if (r) {
+                dev_err(&dssdev->dev,
+                                "error disabling panel, issuing HW reset\n");
+                taal_hw_reset(dssdev);
+        }
 
         omapdss_dsi_display_disable(dssdev);
 
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
         td->enabled = 0;
-
-        dsi_bus_unlock();
 }
 
 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");
@@ -749,10 +954,19 @@ static int taal_enable(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        dsi_bus_lock();
+
         r = taal_power_on(dssdev);
+
+        dsi_bus_unlock();
+
         if (r)
                 goto err;
 
+        if (panel_data->use_esd_check)
+                queue_delayed_work(td->esd_wq, &td->esd_work,
+                                TAAL_ESD_CHECK_PERIOD);
+
         dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
         mutex_unlock(&td->lock);
@@ -772,9 +986,15 @@ static void taal_disable(struct omap_dss_device *dssdev)
 
         mutex_lock(&td->lock);
 
+        cancel_delayed_work(&td->esd_work);
+
+        dsi_bus_lock();
+
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 taal_power_off(dssdev);
 
+        dsi_bus_unlock();
+
         dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 
         mutex_unlock(&td->lock);
@@ -794,7 +1014,14 @@ static int taal_suspend(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        cancel_delayed_work(&td->esd_work);
+
+        dsi_bus_lock();
+
         taal_power_off(dssdev);
+
+        dsi_bus_unlock();
+
         dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
         mutex_unlock(&td->lock);
@@ -808,6 +1035,7 @@ 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");
@@ -819,8 +1047,20 @@ static int taal_resume(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        dsi_bus_lock();
+
         r = taal_power_on(dssdev);
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        dsi_bus_unlock();
+
+        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);
+        }
 
         mutex_unlock(&td->lock);
 
@@ -837,10 +1077,52 @@ static void taal_framedone_cb(int err, void *data)
         dsi_bus_unlock();
 }
 
+static irqreturn_t taal_te_isr(int irq, void *data)
+{
+        struct omap_dss_device *dssdev = data;
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        int old;
+        int r;
+
+        old = atomic_cmpxchg(&td->do_update, 1, 0);
+
+        if (old) {
+                cancel_delayed_work(&td->te_timeout_work);
+
+                r = omap_dsi_update(dssdev, TCH,
+                                td->update_region.x,
+                                td->update_region.y,
+                                td->update_region.w,
+                                td->update_region.h,
+                                taal_framedone_cb, dssdev);
+                if (r)
+                        goto err;
+        }
+
+        return IRQ_HANDLED;
+err:
+        dev_err(&dssdev->dev, "start update failed\n");
+        dsi_bus_unlock();
+        return IRQ_HANDLED;
+}
+
+static void taal_te_timeout_work_callback(struct work_struct *work)
+{
+        struct taal_data *td = container_of(work, struct taal_data,
+                                        te_timeout_work.work);
+        struct omap_dss_device *dssdev = td->dssdev;
+
+        dev_err(&dssdev->dev, "TE not received for 250ms!\n");
+
+        atomic_set(&td->do_update, 0);
+        dsi_bus_unlock();
+}
+
 static int taal_update(struct omap_dss_device *dssdev,
                                     u16 x, u16 y, u16 w, u16 h)
 {
         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, "update %d, %d, %d x %d\n", x, y, w, h);
@@ -853,7 +1135,7 @@ static int taal_update(struct omap_dss_device *dssdev,
                 goto err;
         }
 
-        r = omap_dsi_prepare_update(dssdev, &x, &y, &w, &h);
+        r = omap_dsi_prepare_update(dssdev, &x, &y, &w, &h, true);
         if (r)
                 goto err;
 
@@ -861,10 +1143,21 @@ static int taal_update(struct omap_dss_device *dssdev,
         if (r)
                 goto err;
 
-        r = omap_dsi_update(dssdev, TCH, x, y, w, h,
-                        taal_framedone_cb, dssdev);
-        if (r)
-                goto err;
+        if (td->te_enabled && panel_data->use_ext_te) {
+                td->update_region.x = x;
+                td->update_region.y = y;
+                td->update_region.w = w;
+                td->update_region.h = h;
+                barrier();
+                schedule_delayed_work(&td->te_timeout_work,
+                                msecs_to_jiffies(250));
+                atomic_set(&td->do_update, 1);
+        } else {
+                r = omap_dsi_update(dssdev, TCH, x, y, w, h,
+                                taal_framedone_cb, dssdev);
+                if (r)
+                        goto err;
+        }
 
         /* note: no bus_unlock here. unlock is in framedone_cb */
         mutex_unlock(&td->lock);
@@ -894,20 +1187,19 @@ static int taal_sync(struct omap_dss_device *dssdev)
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
-        td->te_enabled = enable;
-
         if (enable)
                 r = taal_dcs_write_1(DCS_TEAR_ON, 0);
         else
                 r = taal_dcs_write_0(DCS_TEAR_OFF);
 
-        omapdss_dsi_enable_te(dssdev, enable);
+        if (!panel_data->use_ext_te)
+                omapdss_dsi_enable_te(dssdev, enable);
 
-        /* XXX for some reason, DSI TE breaks if we don't wait here.
-         * Panel bug? Needs more studying */
-        msleep(100);
+        if (td->panel_config->sleep.enable_te)
+                msleep(td->panel_config->sleep.enable_te);
 
         return r;
 }
@@ -918,10 +1210,26 @@ static int taal_enable_te(struct omap_dss_device *dssdev, bool enable)
         int r;
 
         mutex_lock(&td->lock);
+
+        if (td->te_enabled == enable)
+                goto end;
+
         dsi_bus_lock();
 
-        r = _taal_enable_te(dssdev, enable);
+        if (td->enabled) {
+                r = _taal_enable_te(dssdev, enable);
+                if (r)
+                        goto err;
+        }
+
+        td->te_enabled = enable;
+
+        dsi_bus_unlock();
+end:
+        mutex_unlock(&td->lock);
 
+        return 0;
+err:
         dsi_bus_unlock();
         mutex_unlock(&td->lock);
 
@@ -948,6 +1256,10 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
         dev_dbg(&dssdev->dev, "rotate %d\n", rotate);
 
         mutex_lock(&td->lock);
+
+        if (td->rotate == rotate)
+                goto end;
+
         dsi_bus_lock();
 
         if (td->enabled) {
@@ -959,6 +1271,7 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
         td->rotate = rotate;
 
         dsi_bus_unlock();
+end:
         mutex_unlock(&td->lock);
         return 0;
 err:
@@ -987,6 +1300,10 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
         dev_dbg(&dssdev->dev, "mirror %d\n", enable);
 
         mutex_lock(&td->lock);
+
+        if (td->mirror == enable)
+                goto end;
+
         dsi_bus_lock();
         if (td->enabled) {
                 r = taal_set_addr_mode(td->rotate, enable);
@@ -997,6 +1314,7 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
         td->mirror = enable;
 
         dsi_bus_unlock();
+end:
         mutex_unlock(&td->lock);
         return 0;
 err:
@@ -1024,23 +1342,30 @@ static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
         int r;
 
         mutex_lock(&td->lock);
+
+        if (!td->enabled) {
+                r = -ENODEV;
+                goto err1;
+        }
+
         dsi_bus_lock();
 
         r = taal_dcs_read_1(DCS_GET_ID1, &id1);
         if (r)
-                goto err;
+                goto err2;
         r = taal_dcs_read_1(DCS_GET_ID2, &id2);
         if (r)
-                goto err;
+                goto err2;
         r = taal_dcs_read_1(DCS_GET_ID3, &id3);
         if (r)
-                goto err;
+                goto err2;
 
         dsi_bus_unlock();
         mutex_unlock(&td->lock);
         return 0;
-err:
+err2:
         dsi_bus_unlock();
+err1:
         mutex_unlock(&td->lock);
         return r;
 }
@@ -1128,6 +1453,7 @@ static void taal_esd_work(struct work_struct *work)
         struct taal_data *td = container_of(work, struct taal_data,
                         esd_work.work);
         struct omap_dss_device *dssdev = td->dssdev;
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         u8 state1, state2;
         int r;
 
@@ -1168,7 +1494,7 @@ 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->use_ext_te && td->te_enabled) {
+        if (td->te_enabled && panel_data->use_ext_te) {
                 r = taal_dcs_write_1(DCS_TEAR_ON, 0);
                 if (r)
                         goto err;
@@ -1184,6 +1510,7 @@ err:
         dev_err(&dssdev->dev, "performing LCD reset\n");
 
         taal_power_off(dssdev);
+        taal_hw_reset(dssdev);
         taal_power_on(dssdev);
 
         dsi_bus_unlock();
diff --git a/drivers/video/omap2/displays/panel-toppoly-tdo35s.c b/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
index fa434ca6e4b7..526e906c8a6c 100644
--- a/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
+++ b/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
@@ -46,6 +46,9 @@ static int toppoly_tdo_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;
@@ -65,6 +68,9 @@ err0:
 
 static void toppoly_tdo_panel_power_off(struct omap_dss_device *dssdev)
 {
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
         if (dssdev->platform_disable)
                 dssdev->platform_disable(dssdev);
 
@@ -73,8 +79,12 @@ static void toppoly_tdo_panel_power_off(struct omap_dss_device *dssdev)
 
 static int toppoly_tdo_panel_probe(struct omap_dss_device *dssdev)
 {
-        dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-                OMAP_DSS_LCD_IHS;
+        dssdev->panel.config = OMAP_DSS_LCD_TFT |
+                               OMAP_DSS_LCD_IVS |
+                               OMAP_DSS_LCD_IHS |
+                               OMAP_DSS_LCD_IPC |
+                               OMAP_DSS_LCD_ONOFF;
+
         dssdev->panel.timings = toppoly_tdo_panel_timings;
 
         return 0;
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index e866e76b13d0..dbe9d43b4850 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -269,6 +269,9 @@ static int tpo_td043_power_on(struct omap_dss_device *dssdev)
         int nreset_gpio = dssdev->reset_gpio;
         int r;
 
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
         r = omapdss_dpi_display_enable(dssdev);
         if (r)
                 goto err0;
@@ -308,6 +311,9 @@ static void tpo_td043_power_off(struct omap_dss_device *dssdev)
         struct tpo_td043_device *tpo_td043 = dev_get_drvdata(&dssdev->dev);
         int nreset_gpio = dssdev->reset_gpio;
 
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
         tpo_td043_write(tpo_td043->spi, 3,
                         TPO_R03_VAL_STANDBY | TPO_R03_EN_PWM);
 
diff --git a/drivers/video/omap2/dss/Makefile b/drivers/video/omap2/dss/Makefile
index d71b5d9d71b1..7db17b5e570c 100644
--- a/drivers/video/omap2/dss/Makefile
+++ b/drivers/video/omap2/dss/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_OMAP2_DSS) += omapdss.o
-omapdss-y := core.o dss.o dispc.o display.o manager.o overlay.o
+omapdss-y := core.o dss.o dss_features.o dispc.o display.o manager.o overlay.o
 omapdss-$(CONFIG_OMAP2_DSS_DPI) += dpi.o
 omapdss-$(CONFIG_OMAP2_DSS_RFBI) += rfbi.o
 omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index b3a498f22d36..8e89f6049280 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -37,6 +37,7 @@
 #include <plat/clock.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static struct {
         struct platform_device *pdev;
@@ -502,6 +503,8 @@ static int omap_dss_probe(struct platform_device *pdev)
 
         core.pdev = pdev;
 
+        dss_features_init();
+
         dss_init_overlay_managers(pdev);
         dss_init_overlays(pdev);
 
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index e777e352dbcd..fa40fa59a9ac 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -31,6 +31,7 @@
 #include <linux/seq_file.h>
 #include <linux/delay.h>
 #include <linux/workqueue.h>
+#include <linux/hardirq.h>
 
 #include <plat/sram.h>
 #include <plat/clock.h>
@@ -38,6 +39,7 @@
 #include <plat/display.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 /* DISPC */
 #define DISPC_BASE                      0x48050400
@@ -138,6 +140,22 @@ struct omap_dispc_isr_data {
         u32                     mask;
 };
 
+struct dispc_h_coef {
+        s8 hc4;
+        s8 hc3;
+        u8 hc2;
+        s8 hc1;
+        s8 hc0;
+};
+
+struct dispc_v_coef {
+        s8 vc22;
+        s8 vc2;
+        u8 vc1;
+        s8 vc0;
+        s8 vc00;
+};
+
 #define REG_GET(idx, start, end) \
         FLD_GET(dispc_read_reg(idx), start, end)
 
@@ -335,7 +353,7 @@ void dispc_save_context(void)
 void dispc_restore_context(void)
 {
         RR(SYSCONFIG);
-        RR(IRQENABLE);
+        /*RR(IRQENABLE);*/
         /*RR(CONTROL);*/
         RR(CONFIG);
         RR(DEFAULT_COLOR0);
@@ -472,6 +490,15 @@ void dispc_restore_context(void)
 
         /* enable last, because LCD & DIGIT enable are here */
         RR(CONTROL);
+
+        /* clear spurious SYNC_LOST_DIGIT interrupts */
+        dispc_write_reg(DISPC_IRQSTATUS, DISPC_IRQ_SYNC_LOST_DIGIT);
+
+        /*
+         * enable last so IRQs won't trigger before
+         * the context is fully restored
+         */
+        RR(IRQENABLE);
 }
 
 #undef SR
@@ -554,106 +581,77 @@ static void _dispc_set_scale_coef(enum omap_plane plane, int hscaleup,
                 int vscaleup, int five_taps)
 {
         /* Coefficients for horizontal up-sampling */
-        static const u32 coef_hup[8] = {
-                0x00800000,
-                0x0D7CF800,
-                0x1E70F5FF,
-                0x335FF5FE,
-                0xF74949F7,
-                0xF55F33FB,
-                0xF5701EFE,
-                0xF87C0DFF,
+        static const struct dispc_h_coef coef_hup[8] = {
+                {  0,   0, 128,   0,  0 },
+                { -1,  13, 124,  -8,  0 },
+                { -2,  30, 112, -11, -1 },
+                { -5,  51,  95, -11, -2 },
+                {  0,  -9,  73,  73, -9 },
+                { -2, -11,  95,  51, -5 },
+                { -1, -11, 112,  30, -2 },
+                {  0,  -8, 124,  13, -1 },
         };
 
-        /* Coefficients for horizontal down-sampling */
-        static const u32 coef_hdown[8] = {
-                0x24382400,
-                0x28371FFE,
-                0x2C361BFB,
-                0x303516F9,
-                0x11343311,
-                0x1635300C,
-                0x1B362C08,
-                0x1F372804,
+        /* Coefficients for vertical up-sampling */
+        static const struct dispc_v_coef coef_vup_3tap[8] = {
+                { 0,  0, 128,  0, 0 },
+                { 0,  3, 123,  2, 0 },
+                { 0, 12, 111,  5, 0 },
+                { 0, 32,  89,  7, 0 },
+                { 0,  0,  64, 64, 0 },
+                { 0,  7,  89, 32, 0 },
+                { 0,  5, 111, 12, 0 },
+                { 0,  2, 123,  3, 0 },
         };
 
-        /* Coefficients for horizontal and vertical up-sampling */
-        static const u32 coef_hvup[2][8] = {
-                {
-                0x00800000,
-                0x037B02FF,
-                0x0C6F05FE,
-                0x205907FB,
-                0x00404000,
-                0x075920FE,
-                0x056F0CFF,
-                0x027B0300,
-                },
-                {
-                0x00800000,
-                0x0D7CF8FF,
-                0x1E70F5FE,
-                0x335FF5FB,
-                0xF7404000,
-                0xF55F33FE,
-                0xF5701EFF,
-                0xF87C0D00,
-                },
+        static const struct dispc_v_coef coef_vup_5tap[8] = {
+                {  0,   0, 128,   0,  0 },
+                { -1,  13, 124,  -8,  0 },
+                { -2,  30, 112, -11, -1 },
+                { -5,  51,  95, -11, -2 },
+                {  0,  -9,  73,  73, -9 },
+                { -2, -11,  95,  51, -5 },
+                { -1, -11, 112,  30, -2 },
+                {  0,  -8, 124,  13, -1 },
         };
 
-        /* Coefficients for horizontal and vertical down-sampling */
-        static const u32 coef_hvdown[2][8] = {
-                {
-                0x24382400,
-                0x28391F04,
-                0x2D381B08,
-                0x3237170C,
-                0x123737F7,
-                0x173732F9,
-                0x1B382DFB,
-                0x1F3928FE,
-                },
-                {
-                0x24382400,
-                0x28371F04,
-                0x2C361B08,
-                0x3035160C,
-                0x113433F7,
-                0x163530F9,
-                0x1B362CFB,
-                0x1F3728FE,
-                },
+        /* Coefficients for horizontal down-sampling */
+        static const struct dispc_h_coef coef_hdown[8] = {
+                {   0, 36, 56, 36,  0 },
+                {   4, 40, 55, 31, -2 },
+                {   8, 44, 54, 27, -5 },
+                {  12, 48, 53, 22, -7 },
+                {  -9, 17, 52, 51, 17 },
+                {  -7, 22, 53, 48, 12 },
+                {  -5, 27, 54, 44,  8 },
+                {  -2, 31, 55, 40,  4 },
         };
 
-        /* Coefficients for vertical up-sampling */
-        static const u32 coef_vup[8] = {
-                0x00000000,
-                0x0000FF00,
-                0x0000FEFF,
-                0x0000FBFE,
-                0x000000F7,
-                0x0000FEFB,
-                0x0000FFFE,
-                0x000000FF,
+        /* Coefficients for vertical down-sampling */
+        static const struct dispc_v_coef coef_vdown_3tap[8] = {
+                { 0, 36, 56, 36, 0 },
+                { 0, 40, 57, 31, 0 },
+                { 0, 45, 56, 27, 0 },
+                { 0, 50, 55, 23, 0 },
+                { 0, 18, 55, 55, 0 },
+                { 0, 23, 55, 50, 0 },
+                { 0, 27, 56, 45, 0 },
+                { 0, 31, 57, 40, 0 },
         };
 
-
-        /* Coefficients for vertical down-sampling */
-        static const u32 coef_vdown[8] = {
-                0x00000000,
-                0x000004FE,
-                0x000008FB,
-                0x00000CF9,
-                0x0000F711,
-                0x0000F90C,
-                0x0000FB08,
-                0x0000FE04,
+        static const struct dispc_v_coef coef_vdown_5tap[8] = {
+                {   0, 36, 56, 36,  0 },
+                {   4, 40, 55, 31, -2 },
+                {   8, 44, 54, 27, -5 },
+                {  12, 48, 53, 22, -7 },
+                {  -9, 17, 52, 51, 17 },
+                {  -7, 22, 53, 48, 12 },
+                {  -5, 27, 54, 44,  8 },
+                {  -2, 31, 55, 40,  4 },
         };
 
-        const u32 *h_coef;
-        const u32 *hv_coef;
-        const u32 *hv_coef_mod;
-        const u32 *v_coef;
+        const struct dispc_h_coef *h_coef;
+        const struct dispc_v_coef *v_coef;
         int i;
 
         if (hscaleup)
@@ -661,47 +659,34 @@ static void _dispc_set_scale_coef(enum omap_plane plane, int hscaleup,
         else
                 h_coef = coef_hdown;
 
-        if (vscaleup) {
-                hv_coef = coef_hvup[five_taps];
-                v_coef = coef_vup;
-
-                if (hscaleup)
-                        hv_coef_mod = NULL;
-                else
-                        hv_coef_mod = coef_hvdown[five_taps];
-        } else {
-                hv_coef = coef_hvdown[five_taps];
-                v_coef = coef_vdown;
-
-                if (hscaleup)
-                        hv_coef_mod = coef_hvup[five_taps];
-                else
-                        hv_coef_mod = NULL;
-        }
+        if (vscaleup)
+                v_coef = five_taps ? coef_vup_5tap : coef_vup_3tap;
+        else
+                v_coef = five_taps ? coef_vdown_5tap : coef_vdown_3tap;
 
         for (i = 0; i < 8; i++) {
                 u32 h, hv;
 
-                h = h_coef[i];
-
-                hv = hv_coef[i];
-
-                if (hv_coef_mod) {
-                        hv &= 0xffffff00;
-                        hv |= (hv_coef_mod[i] & 0xff);
-                }
+                h = FLD_VAL(h_coef[i].hc0, 7, 0)
+                        | FLD_VAL(h_coef[i].hc1, 15, 8)
+                        | FLD_VAL(h_coef[i].hc2, 23, 16)
+                        | FLD_VAL(h_coef[i].hc3, 31, 24);
+                hv = FLD_VAL(h_coef[i].hc4, 7, 0)
+                        | FLD_VAL(v_coef[i].vc0, 15, 8)
+                        | 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 (!five_taps)
-                return;
-
-        for (i = 0; i < 8; i++) {
-                u32 v;
-                v = v_coef[i];
-                _dispc_write_firv_reg(plane, i, v);
+        if (five_taps) {
+                for (i = 0; i < 8; i++) {
+                        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);
+                }
         }
 }
 
@@ -790,12 +775,12 @@ static void _dispc_set_vid_size(enum omap_plane plane, int width, int height)
 
 static void _dispc_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
 {
-
-        BUG_ON(plane == OMAP_DSS_VIDEO1);
-
-        if (cpu_is_omap24xx())
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
                 return;
 
+        BUG_ON(!dss_has_feature(FEAT_GLOBAL_ALPHA_VID1) &&
+                        plane == OMAP_DSS_VIDEO1);
+
         if (plane == OMAP_DSS_GFX)
                 REG_FLD_MOD(DISPC_GLOBAL_ALPHA, global_alpha, 7, 0);
         else if (plane == OMAP_DSS_VIDEO2)
@@ -965,17 +950,14 @@ static void dispc_read_plane_fifo_sizes(void)
                                       DISPC_VID_FIFO_SIZE_STATUS(1) };
         u32 size;
         int plane;
+        u8 start, end;
 
         enable_clocks(1);
 
-        for (plane = 0; plane < ARRAY_SIZE(dispc.fifo_size); ++plane) {
-                if (cpu_is_omap24xx())
-                        size = FLD_GET(dispc_read_reg(fsz_reg[plane]), 8, 0);
-                else if (cpu_is_omap34xx())
-                        size = FLD_GET(dispc_read_reg(fsz_reg[plane]), 10, 0);
-                else
-                        BUG();
+        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);
                 dispc.fifo_size[plane] = size;
         }
 
@@ -992,6 +974,8 @@ void dispc_setup_plane_fifo(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;
+
         enable_clocks(1);
 
         DSSDBG("fifo(%d) low/high old %u/%u, new %u/%u\n",
@@ -1000,12 +984,12 @@ void dispc_setup_plane_fifo(enum omap_plane plane, u32 low, u32 high)
                         REG_GET(ftrs_reg[plane], 27, 16),
                         low, high);
 
-        if (cpu_is_omap24xx())
-                dispc_write_reg(ftrs_reg[plane],
-                                FLD_VAL(high, 24, 16) | FLD_VAL(low, 8, 0));
-        else
-                dispc_write_reg(ftrs_reg[plane],
-                                FLD_VAL(high, 27, 16) | FLD_VAL(low, 11, 0));
+        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],
+                        FLD_VAL(high, hi_start, hi_end) |
+                        FLD_VAL(low, lo_start, lo_end));
 
         enable_clocks(0);
 }
@@ -1025,13 +1009,16 @@ static void _dispc_set_fir(enum omap_plane plane, int hinc, int vinc)
         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);
 
-        if (cpu_is_omap24xx())
-                val = FLD_VAL(vinc, 27, 16) | FLD_VAL(hinc, 11, 0);
-        else
-                val = FLD_VAL(vinc, 28, 16) | FLD_VAL(hinc, 12, 0);
+        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);
 }
 
@@ -1557,6 +1544,8 @@ static int _dispc_setup_plane(enum omap_plane plane,
                 case OMAP_DSS_COLOR_ARGB16:
                 case OMAP_DSS_COLOR_ARGB32:
                 case OMAP_DSS_COLOR_RGBA32:
+                        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
+                                return -EINVAL;
                 case OMAP_DSS_COLOR_RGBX32:
                         if (cpu_is_omap24xx())
                                 return -EINVAL;
@@ -1597,9 +1586,10 @@ static int _dispc_setup_plane(enum omap_plane plane,
                 case OMAP_DSS_COLOR_ARGB16:
                 case OMAP_DSS_COLOR_ARGB32:
                 case OMAP_DSS_COLOR_RGBA32:
-                        if (cpu_is_omap24xx())
+                        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
                                 return -EINVAL;
-                        if (plane == OMAP_DSS_VIDEO1)
+                        if (!dss_has_feature(FEAT_GLOBAL_ALPHA_VID1) &&
+                                        plane == OMAP_DSS_VIDEO1)
                                 return -EINVAL;
                         break;
 
@@ -1992,7 +1982,7 @@ void dispc_enable_trans_key(enum omap_channel ch, bool enable)
 }
 void dispc_enable_alpha_blending(enum omap_channel ch, bool enable)
 {
-        if (cpu_is_omap24xx())
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
                 return;
 
         enable_clocks(1);
@@ -2006,7 +1996,7 @@ bool dispc_alpha_blending_enabled(enum omap_channel ch)
 {
         bool enabled;
 
-        if (cpu_is_omap24xx())
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
                 return false;
 
         enable_clocks(1);
@@ -3019,7 +3009,7 @@ void dispc_fake_vsync_irq(void)
         u32 irqstatus = DISPC_IRQ_VSYNC;
         int i;
 
-        local_irq_disable();
+        WARN_ON(!in_interrupt());
 
         for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
                 struct omap_dispc_isr_data *isr_data;
@@ -3031,8 +3021,6 @@ void dispc_fake_vsync_irq(void)
                 if (isr_data->mask & irqstatus)
                         isr_data->isr(isr_data->arg, irqstatus);
         }
-
-        local_irq_enable();
 }
 #endif
 
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index ef8c8529dda2..22dd7a474f79 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -82,6 +82,9 @@ static ssize_t display_upd_mode_store(struct 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) {
@@ -343,7 +346,6 @@ int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
         case OMAP_DISPLAY_TYPE_VENC:
         case OMAP_DISPLAY_TYPE_SDI:
                 return 24;
-                return 24;
         default:
                 BUG();
         }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 3af207b2bde3..aa4f7a5fae29 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -165,6 +165,14 @@ struct dsi_reg { u16 idx; };
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_3  (1 << 25)
 #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_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)
 
 #define DSI_DT_DCS_SHORT_WRITE_0        0x05
 #define DSI_DT_DCS_SHORT_WRITE_1        0x15
@@ -232,13 +240,15 @@ static struct
         unsigned pll_locked;
 
         struct completion bta_completion;
+        void (*bta_callback)(void);
 
         int update_channel;
         struct dsi_update_region update_region;
 
         bool te_enabled;
 
-        struct work_struct framedone_work;
+        struct workqueue_struct *workqueue;
+
         void (*framedone_callback)(int, void *);
         void *framedone_data;
 
@@ -509,9 +519,13 @@ void dsi_irq_handler(void)
                 dss_collect_irq_stats(vcstatus, dsi.irq_stats.vc_irqs[i]);
 #endif
 
-                if (vcstatus & DSI_VC_IRQ_BTA)
+                if (vcstatus & DSI_VC_IRQ_BTA) {
                         complete(&dsi.bta_completion);
 
+                        if (dsi.bta_callback)
+                                dsi.bta_callback();
+                }
+
                 if (vcstatus & DSI_VC_IRQ_ERROR_MASK) {
                         DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
                                        i, vcstatus);
@@ -536,8 +550,12 @@ void dsi_irq_handler(void)
                 /* flush posted write */
                 dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
 
-                DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
-                print_irq_status_cio(ciostatus);
+                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);
+                }
         }
 
         dsi_write_reg(DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
@@ -584,11 +602,8 @@ static void _dsi_initialize_irq(void)
         for (i = 0; i < 4; ++i)
                 dsi_write_reg(DSI_VC_IRQENABLE(i), l);
 
-        /* XXX zonda responds incorrectly, causing control error:
-           Exit from LP-ESC mode to LP11 uses wrong transition states on the
-           data lines LP0 and LN0. */
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE,
-                        -1 & (~DSI_CIO_IRQ_ERRCONTROL2));
+        l = DSI_CIO_IRQ_ERROR_MASK;
+        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, l);
 }
 
 static u32 dsi_get_errors(void)
@@ -1098,6 +1113,7 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
         if (wait_for_bit_change(DSI_PLL_STATUS, 0, 1) != 1) {
                 DSSERR("PLL not coming out of reset.\n");
                 r = -ENODEV;
+                dispc_pck_free_enable(0);
                 goto err1;
         }
 
@@ -1740,42 +1756,52 @@ static void dsi_vc_initial_config(int channel)
         dsi.vc[channel].mode = DSI_VC_MODE_L4;
 }
 
-static void dsi_vc_config_l4(int channel)
+static int dsi_vc_config_l4(int channel)
 {
         if (dsi.vc[channel].mode == DSI_VC_MODE_L4)
-                return;
+                return 0;
 
         DSSDBGF("%d", channel);
 
         dsi_vc_enable(channel, 0);
 
-        if (REG_GET(DSI_VC_CTRL(channel), 15, 15)) /* VC_BUSY */
+        /* VC_BUSY */
+        if (wait_for_bit_change(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 */
 
         dsi_vc_enable(channel, 1);
 
         dsi.vc[channel].mode = DSI_VC_MODE_L4;
+
+        return 0;
 }
 
-static void dsi_vc_config_vp(int channel)
+static int dsi_vc_config_vp(int channel)
 {
         if (dsi.vc[channel].mode == DSI_VC_MODE_VP)
-                return;
+                return 0;
 
         DSSDBGF("%d", channel);
 
         dsi_vc_enable(channel, 0);
 
-        if (REG_GET(DSI_VC_CTRL(channel), 15, 15)) /* VC_BUSY */
+        /* VC_BUSY */
+        if (wait_for_bit_change(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 */
 
         dsi_vc_enable(channel, 1);
 
         dsi.vc[channel].mode = DSI_VC_MODE_VP;
+
+        return 0;
 }
 
 
@@ -1854,19 +1880,19 @@ static u16 dsi_vc_flush_receive_data(int channel)
                 u32 val;
                 u8 dt;
                 val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
-                DSSDBG("\trawval %#08x\n", val);
+                DSSERR("\trawval %#08x\n", val);
                 dt = FLD_GET(val, 5, 0);
                 if (dt == DSI_DT_RX_ACK_WITH_ERR) {
                         u16 err = FLD_GET(val, 23, 8);
                         dsi_show_rx_ack_with_err(err);
                 } else if (dt == DSI_DT_RX_SHORT_READ_1) {
-                        DSSDBG("\tDCS short response, 1 byte: %#x\n",
+                        DSSERR("\tDCS short response, 1 byte: %#x\n",
                                         FLD_GET(val, 23, 8));
                 } else if (dt == DSI_DT_RX_SHORT_READ_2) {
-                        DSSDBG("\tDCS short response, 2 byte: %#x\n",
+                        DSSERR("\tDCS short response, 2 byte: %#x\n",
                                         FLD_GET(val, 23, 8));
                 } else if (dt == DSI_DT_RX_DCS_LONG_READ) {
-                        DSSDBG("\tDCS long response, len %d\n",
+                        DSSERR("\tDCS long response, len %d\n",
                                         FLD_GET(val, 23, 8));
                         dsi_vc_flush_long_data(channel);
                 } else {
@@ -2087,6 +2113,13 @@ int dsi_vc_dcs_write(int channel, u8 *data, int len)
         if (r)
                 goto err;
 
+        if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {    /* RX_FIFO_NOT_EMPTY */
+                DSSERR("rx fifo not empty after write, dumping data:\n");
+                dsi_vc_flush_receive_data(channel);
+                r = -EIO;
+                goto err;
+        }
+
         return 0;
 err:
         DSSERR("dsi_vc_dcs_write(ch %d, cmd 0x%02x, len %d) failed\n",
@@ -2233,11 +2266,12 @@ 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, u16 *data)
+int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u8 *data1, u8 *data2)
 {
+        u8 buf[2];
         int r;
 
-        r = dsi_vc_dcs_read(channel, dcs_cmd, (u8 *)data, 2);
+        r = dsi_vc_dcs_read(channel, dcs_cmd, buf, 2);
 
         if (r < 0)
                 return r;
@@ -2245,231 +2279,122 @@ int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u16 *data)
         if (r != 2)
                 return -EIO;
 
+        *data1 = buf[0];
+        *data2 = buf[1];
+
         return 0;
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_2);
 
 int dsi_vc_set_max_rx_packet_size(int channel, u16 len)
 {
-        int r;
-        r = dsi_vc_send_short(channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
+        return dsi_vc_send_short(channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
                         len, 0);
-
-        if (r)
-                return r;
-
-        r = dsi_vc_send_bta_sync(channel);
-
-        return r;
 }
 EXPORT_SYMBOL(dsi_vc_set_max_rx_packet_size);
 
-static void dsi_set_lp_rx_timeout(unsigned long ns)
+static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
 {
-        u32 r;
-        unsigned x4, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
 
-        /* ticks in DSI_FCK */
+        BUG_ON(ticks > 0x1fff);
 
+        /* ticks in DSI_FCK */
         fck = dsi_fclk_rate();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x4 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 4;
-                x4 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x4 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (4 * 16);
-                x4 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("LP_TX_TO over limit, setting it to max\n");
-                ticks = 0x1fff;
-                x4 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING2);
         r = FLD_MOD(r, 1, 15, 15);      /* LP_RX_TO */
-        r = FLD_MOD(r, x16, 14, 14);    /* LP_RX_TO_X16 */
-        r = FLD_MOD(r, x4, 13, 13);     /* LP_RX_TO_X4 */
+        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);
 
-        DSSDBG("LP_RX_TO %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x4 ? " x4" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("LP_RX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_ta_timeout(unsigned long ns)
+static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
 {
-        u32 r;
-        unsigned x8, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
+
+        BUG_ON(ticks > 0x1fff);
 
         /* ticks in DSI_FCK */
         fck = dsi_fclk_rate();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x8 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 8;
-                x8 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x8 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (8 * 16);
-                x8 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("TA_TO over limit, setting it to max\n");
-                ticks = 0x1fff;
-                x8 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING1);
         r = FLD_MOD(r, 1, 31, 31);      /* TA_TO */
-        r = FLD_MOD(r, x16, 30, 30);    /* TA_TO_X16 */
-        r = FLD_MOD(r, x8, 29, 29);     /* TA_TO_X8 */
+        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);
 
-        DSSDBG("TA_TO %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x8 ? " x8" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1);
+
+        DSSDBG("TA_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x8 ? " x8" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_stop_state_counter(unsigned long ns)
+static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
 {
-        u32 r;
-        unsigned x4, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
 
-        /* ticks in DSI_FCK */
+        BUG_ON(ticks > 0x1fff);
 
+        /* ticks in DSI_FCK */
         fck = dsi_fclk_rate();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x4 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 4;
-                x4 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x4 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (4 * 16);
-                x4 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("STOP_STATE_COUNTER_IO over limit, "
-                                "setting it to max\n");
-                ticks = 0x1fff;
-                x4 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING1);
         r = FLD_MOD(r, 1, 15, 15);      /* FORCE_TX_STOP_MODE_IO */
-        r = FLD_MOD(r, x16, 14, 14);    /* STOP_STATE_X16_IO */
-        r = FLD_MOD(r, x4, 13, 13);     /* STOP_STATE_X4_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);
 
-        DSSDBG("STOP_STATE_COUNTER %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x4 ? " x4" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("STOP_STATE_COUNTER %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_hs_tx_timeout(unsigned long ns)
+static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
 {
-        u32 r;
-        unsigned x4, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
 
-        /* ticks in TxByteClkHS */
+        BUG_ON(ticks > 0x1fff);
 
+        /* ticks in TxByteClkHS */
         fck = dsi_get_txbyteclkhs();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x4 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 4;
-                x4 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x4 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (4 * 16);
-                x4 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("HS_TX_TO over limit, setting it to max\n");
-                ticks = 0x1fff;
-                x4 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING2);
         r = FLD_MOD(r, 1, 31, 31);      /* HS_TX_TO */
-        r = FLD_MOD(r, x16, 30, 30);    /* HS_TX_TO_X16 */
-        r = FLD_MOD(r, x4, 29, 29);     /* HS_TX_TO_X8 (4 really) */
+        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);
 
-        DSSDBG("HS_TX_TO %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x4 ? " x4" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("HS_TX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 static int dsi_proto_config(struct omap_dss_device *dssdev)
 {
@@ -2487,10 +2412,10 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
                         DSI_FIFO_SIZE_32);
 
         /* XXX what values for the timeouts? */
-        dsi_set_stop_state_counter(1000);
-        dsi_set_ta_timeout(6400000);
-        dsi_set_lp_rx_timeout(48000);
-        dsi_set_hs_tx_timeout(1000000);
+        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);
 
         switch (dssdev->ctrl.pixel_size) {
         case 16:
@@ -2759,6 +2684,7 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
         unsigned packet_payload;
         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
@@ -2809,8 +2735,9 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 
         dsi_perf_mark_start();
 
-        schedule_delayed_work(&dsi.framedone_timeout_work,
+        r = queue_delayed_work(dsi.workqueue, &dsi.framedone_timeout_work,
                         msecs_to_jiffies(250));
+        BUG_ON(r == 0);
 
         dss_start_update(dssdev);
 
@@ -2834,62 +2761,70 @@ static void dsi_te_timeout(unsigned long arg)
 }
 #endif
 
-static void dsi_framedone_timeout_work_callback(struct work_struct *work)
+static void dsi_handle_framedone(int error)
 {
-        int r;
         const int channel = dsi.update_channel;
 
-        DSSERR("Framedone not received for 250ms!\n");
+        cancel_delayed_work(&dsi.framedone_timeout_work);
+
+        dsi_vc_disable_bta_irq(channel);
 
         /* SIDLEMODE back to smart-idle */
         dispc_enable_sidle();
 
+        dsi.bta_callback = NULL;
+
         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. */
-        r = dsi_vc_send_bta_sync(channel);
-        if (r)
-                DSSERR("BTA after framedone failed\n");
-
         /* 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;
         }
 
-        dsi.framedone_callback(-ETIMEDOUT, dsi.framedone_data);
+        dsi.framedone_callback(error, dsi.framedone_data);
+
+        if (!error)
+                dsi_perf_show("DISPC");
 }
 
-static void dsi_framedone_irq_callback(void *data, u32 mask)
+static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 {
-        /* 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.
-         */
+        /* 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
+         * possibly scheduled framedone work. However, cancelling the transfer
+         * on the HW is buggy, and would probably require resetting the whole
+         * DSI */
 
-        /* SIDLEMODE back to smart-idle */
-        dispc_enable_sidle();
+        DSSERR("Framedone not received for 250ms!\n");
 
-        schedule_work(&dsi.framedone_work);
+        dsi_handle_framedone(-ETIMEDOUT);
 }
 
-static void dsi_handle_framedone(void)
+static void dsi_framedone_bta_callback(void)
+{
+        dsi_handle_framedone(0);
+
+#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
+        dispc_fake_vsync_irq();
+#endif
+}
+
+static void dsi_framedone_irq_callback(void *data, u32 mask)
 {
-        int r;
         const int channel = dsi.update_channel;
+        int r;
 
-        DSSDBG("FRAMEDONE\n");
+        /* 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 */
@@ -2904,37 +2839,30 @@ static void dsi_handle_framedone(void)
          * 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. */
-        r = dsi_vc_send_bta_sync(channel);
-        if (r)
-                DSSERR("BTA after framedone failed\n");
-
-        /* 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);
-        }
-
-#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
-        dispc_fake_vsync_irq();
-#endif
-}
-
-static void dsi_framedone_work_callback(struct work_struct *work)
-{
-        DSSDBGF();
+         * 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.
+         * */
 
-        cancel_delayed_work_sync(&dsi.framedone_timeout_work);
+        dsi.bta_callback = dsi_framedone_bta_callback;
 
-        dsi_handle_framedone();
+        barrier();
 
-        dsi_perf_show("DISPC");
+        dsi_vc_enable_bta_irq(channel);
 
-        dsi.framedone_callback(0, dsi.framedone_data);
+        r = dsi_vc_send_bta(channel);
+        if (r) {
+                DSSERR("BTA after framedone failed\n");
+                dsi_handle_framedone(-EIO);
+        }
 }
 
 int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
-                                    u16 *x, u16 *y, u16 *w, u16 *h)
+                                    u16 *x, u16 *y, u16 *w, u16 *h,
+                                    bool enlarge_update_area)
 {
         u16 dw, dh;
 
@@ -2958,7 +2886,8 @@ int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
         dsi_perf_mark_setup();
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-                dss_setup_partial_planes(dssdev, x, y, w, h);
+                dss_setup_partial_planes(dssdev, x, y, w, h,
+                                enlarge_update_area);
                 dispc_set_lcd_size(*w, *h);
         }
 
@@ -2973,6 +2902,12 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 {
         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
+         * here to make sure we catch erroneous updates. Otherwise we'll only
+         * see rather obscure HW error happening, as DSS halts. */
+        BUG_ON(x % 2 == 1);
+
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
                 dsi.framedone_callback = callback;
                 dsi.framedone_data = data;
@@ -2985,7 +2920,12 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 
                 dsi_update_screen_dispc(dssdev, x, y, w, h);
         } else {
-                dsi_update_screen_l4(dssdev, x, y, w, h);
+                int r;
+
+                r = dsi_update_screen_l4(dssdev, x, y, w, h);
+                if (r)
+                        return r;
+
                 dsi_perf_show("L4");
                 callback(0, data);
         }
@@ -3048,8 +2988,10 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
         cinfo.regm3 = dssdev->phy.dsi.div.regm3;
         cinfo.regm4 = dssdev->phy.dsi.div.regm4;
         r = dsi_calc_clock_rates(&cinfo);
-        if (r)
+        if (r) {
+                DSSERR("Failed to calc dsi clocks\n");
                 return r;
+        }
 
         r = dsi_pll_set_clock_div(&cinfo);
         if (r) {
@@ -3147,6 +3089,13 @@ err0:
 
 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();
@@ -3257,7 +3206,7 @@ void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
         burst_size_bytes = 16 * 32 / 8;
 
         *fifo_high = fifo_size - burst_size_bytes;
-        *fifo_low = fifo_size - burst_size_bytes * 8;
+        *fifo_low = fifo_size - burst_size_bytes * 2;
 }
 
 int dsi_init_display(struct omap_dss_device *dssdev)
@@ -3274,6 +3223,18 @@ int dsi_init_display(struct omap_dss_device *dssdev)
         return 0;
 }
 
+void dsi_wait_dsi1_pll_active(void)
+{
+        if (wait_for_bit_change(DSI_PLL_STATUS, 7, 1) != 1)
+                DSSERR("DSI1 PLL clock not active\n");
+}
+
+void dsi_wait_dsi2_pll_active(void)
+{
+        if (wait_for_bit_change(DSI_PLL_STATUS, 8, 1) != 1)
+                DSSERR("DSI2 PLL clock not active\n");
+}
+
 int dsi_init(struct platform_device *pdev)
 {
         u32 rev;
@@ -3292,7 +3253,10 @@ int dsi_init(struct platform_device *pdev)
         mutex_init(&dsi.lock);
         sema_init(&dsi.bus_lock, 1);
 
-        INIT_WORK(&dsi.framedone_work, dsi_framedone_work_callback);
+        dsi.workqueue = create_singlethread_workqueue("dsi");
+        if (dsi.workqueue == NULL)
+                return -ENOMEM;
+
         INIT_DELAYED_WORK_DEFERRABLE(&dsi.framedone_timeout_work,
                         dsi_framedone_timeout_work_callback);
 
@@ -3310,7 +3274,6 @@ int dsi_init(struct platform_device *pdev)
 
         dsi.vdds_dsi_reg = dss_get_vdds_dsi();
         if (IS_ERR(dsi.vdds_dsi_reg)) {
-                iounmap(dsi.base);
                 DSSERR("can't get VDDS_DSI regulator\n");
                 r = PTR_ERR(dsi.vdds_dsi_reg);
                 goto err2;
@@ -3328,6 +3291,7 @@ int dsi_init(struct platform_device *pdev)
 err2:
         iounmap(dsi.base);
 err1:
+        destroy_workqueue(dsi.workqueue);
         return r;
 }
 
@@ -3335,6 +3299,8 @@ void dsi_exit(void)
 {
         iounmap(dsi.base);
 
+        destroy_workqueue(dsi.workqueue);
+
         DSSDBG("omap_dsi_exit\n");
 }
 
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 24b18258654f..77c3621c9171 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -265,6 +265,9 @@ void dss_select_dispc_clk_source(enum dss_clk_source clk_src)
 
         b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
 
+        if (clk_src == DSS_SRC_DSI1_PLL_FCLK)
+                dsi_wait_dsi1_pll_active();
+
         REG_FLD_MOD(DSS_CONTROL, b, 0, 0);      /* DISPC_CLK_SWITCH */
 
         dss.dispc_clk_source = clk_src;
@@ -279,6 +282,9 @@ void dss_select_dsi_clk_source(enum dss_clk_source clk_src)
 
         b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
 
+        if (clk_src == DSS_SRC_DSI2_PLL_FCLK)
+                dsi_wait_dsi2_pll_active();
+
         REG_FLD_MOD(DSS_CONTROL, b, 1, 1);      /* DSI_CLK_SWITCH */
 
         dss.dsi_clk_source = clk_src;
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index 786f433fd571..5c7940d5f282 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -199,7 +199,8 @@ int dss_init_overlay_managers(struct platform_device *pdev);
 void dss_uninit_overlay_managers(struct platform_device *pdev);
 int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl);
 void dss_setup_partial_planes(struct omap_dss_device *dssdev,
-                                u16 *x, u16 *y, u16 *w, u16 *h);
+                                u16 *x, u16 *y, u16 *w, u16 *h,
+                                bool enlarge_update_area);
 void dss_start_update(struct omap_dss_device *dssdev);
 
 /* overlay */
@@ -281,6 +282,8 @@ void dsi_pll_uninit(void);
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
                 u32 fifo_size, enum omap_burst_size *burst_size,
                 u32 *fifo_low, u32 *fifo_high);
+void dsi_wait_dsi1_pll_active(void);
+void dsi_wait_dsi2_pll_active(void);
 #else
 static inline int dsi_init(struct platform_device *pdev)
 {
@@ -289,6 +292,12 @@ static inline int dsi_init(struct platform_device *pdev)
 static inline void dsi_exit(void)
 {
 }
+static inline void dsi_wait_dsi1_pll_active(void)
+{
+}
+static inline void dsi_wait_dsi2_pll_active(void)
+{
+}
 #endif
 
 /* DPI */
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
new file mode 100644
index 000000000000..867f68de125f
--- /dev/null
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -0,0 +1,191 @@
+/*
+ * linux/drivers/video/omap2/dss/dss_features.c
+ *
+ * Copyright (C) 2010 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/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#include <plat/display.h>
+#include <plat/cpu.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 omap_dss_features {
+        const struct dss_reg_field *reg_fields;
+        const int num_reg_fields;
+
+        const u32 has_feature;
+
+        const int num_mgrs;
+        const int num_ovls;
+        const enum omap_display_type *supported_displays;
+        const enum omap_color_mode *supported_color_modes;
+};
+
+/* This struct is assigned to one of the below during initialization */
+static 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 },
+};
+
+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 },
+};
+
+static const enum omap_display_type omap2_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_color_mode omap2_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_RGB16 |
+        OMAP_DSS_COLOR_RGB24U | OMAP_DSS_COLOR_RGB24P,
+
+        /* OMAP_DSS_VIDEO1 */
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_YUV2 |
+        OMAP_DSS_COLOR_UYVY,
+
+        /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_YUV2 |
+        OMAP_DSS_COLOR_UYVY,
+};
+
+static const enum omap_color_mode omap3_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_VIDEO1 */
+        OMAP_DSS_COLOR_RGB24U | OMAP_DSS_COLOR_RGB24P |
+        OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_RGB16 |
+        OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_UYVY,
+
+        /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_ARGB16 |
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_YUV2 |
+        OMAP_DSS_COLOR_UYVY | OMAP_DSS_COLOR_ARGB32 |
+        OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32,
+};
+
+/* OMAP2 DSS Features */
+static struct omap_dss_features omap2_dss_features = {
+        .reg_fields = omap2_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap2_dss_reg_fields),
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap2_dss_supported_displays,
+        .supported_color_modes = omap2_dss_supported_color_modes,
+};
+
+/* OMAP3 DSS Features */
+static struct omap_dss_features omap3_dss_features = {
+        .reg_fields = omap3_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+        .has_feature    = FEAT_GLOBAL_ALPHA,
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3_dss_supported_displays,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+};
+
+/* Functions returning values related to a DSS feature */
+int dss_feat_get_num_mgrs(void)
+{
+        return omap_current_dss_features->num_mgrs;
+}
+
+int dss_feat_get_num_ovls(void)
+{
+        return omap_current_dss_features->num_ovls;
+}
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel)
+{
+        return omap_current_dss_features->supported_displays[channel];
+}
+
+enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
+{
+        return omap_current_dss_features->supported_color_modes[plane];
+}
+
+/* DSS has_feature check */
+bool dss_has_feature(enum dss_feat_id id)
+{
+        return omap_current_dss_features->has_feature & id;
+}
+
+void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end)
+{
+        if (id >= omap_current_dss_features->num_reg_fields)
+                BUG();
+
+        *start = omap_current_dss_features->reg_fields[id].start;
+        *end = omap_current_dss_features->reg_fields[id].end;
+}
+
+void dss_features_init(void)
+{
+        if (cpu_is_omap24xx())
+                omap_current_dss_features = &omap2_dss_features;
+        else
+                omap_current_dss_features = &omap3_dss_features;
+}
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
new file mode 100644
index 000000000000..cb231eaa9b31
--- /dev/null
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -0,0 +1,50 @@
+/*
+ * linux/drivers/video/omap2/dss/dss_features.h
+ *
+ * Copyright (C) 2010 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_DSS_FEATURES_H
+#define __OMAP2_DSS_FEATURES_H
+
+#define MAX_DSS_MANAGERS        2
+#define MAX_DSS_OVERLAYS        3
+
+/* DSS has feature id */
+enum dss_feat_id {
+        FEAT_GLOBAL_ALPHA       = 1 << 0,
+        FEAT_GLOBAL_ALPHA_VID1  = 1 << 1,
+};
+
+/* DSS register field id */
+enum dss_feat_reg_field {
+        FEAT_REG_FIRHINC,
+        FEAT_REG_FIRVINC,
+        FEAT_REG_FIFOHIGHTHRESHOLD,
+        FEAT_REG_FIFOLOWTHRESHOLD,
+        FEAT_REG_FIFOSIZE,
+};
+
+/* DSS Feature Functions */
+int dss_feat_get_num_mgrs(void);
+int dss_feat_get_num_ovls(void);
+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_has_feature(enum dss_feat_id id);
+void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end);
+void dss_features_init(void);
+#endif
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 9e1fbe531bf0..545e9b9a4d92 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -33,6 +33,7 @@
 #include <plat/cpu.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static int num_managers;
 static struct list_head manager_list;
@@ -440,12 +441,16 @@ struct manager_cache_data {
 
         /* manual update region */
         u16 x, y, w, h;
+
+        /* enlarge the update area if the update area contains scaled
+         * overlays */
+        bool enlarge_update_area;
 };
 
 static struct {
         spinlock_t lock;
-        struct overlay_cache_data overlay_cache[3];
-        struct manager_cache_data manager_cache[2];
+        struct overlay_cache_data overlay_cache[MAX_DSS_OVERLAYS];
+        struct manager_cache_data manager_cache[MAX_DSS_MANAGERS];
 
         bool irq_enabled;
 } dss_cache;
@@ -525,7 +530,7 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
         int i;
         struct omap_dss_device *dssdev = mgr->device;
 
-        if (!dssdev)
+        if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
         if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
@@ -596,11 +601,14 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
         int r;
         int i;
 
-        if (!ovl->manager || !ovl->manager->device)
+        if (!ovl->manager)
                 return 0;
 
         dssdev = ovl->manager->device;
 
+        if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
         if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
                 irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
                 channel = OMAP_DSS_CHANNEL_DIGIT;
@@ -718,6 +726,7 @@ static int configure_overlay(enum omap_plane plane)
         u16 x, y, w, h;
         u32 paddr;
         int r;
+        u16 orig_w, orig_h, orig_outw, orig_outh;
 
         DSSDBGF("%d", plane);
 
@@ -738,8 +747,16 @@ static int configure_overlay(enum omap_plane plane)
         outh = c->out_height == 0 ? c->height : c->out_height;
         paddr = c->paddr;
 
+        orig_w = w;
+        orig_h = h;
+        orig_outw = outw;
+        orig_outh = outh;
+
         if (c->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;
+
                 /* If the overlay is outside the update region, disable it */
                 if (!rectangle_intersects(mc->x, mc->y, mc->w, mc->h,
                                         x, y, outw, outh)) {
@@ -770,38 +787,47 @@ static int configure_overlay(enum omap_plane plane)
                         BUG();
                 }
 
-                if (dispc_is_overlay_scaled(c)) {
-                        /* If the overlay is scaled, the update area has
-                         * already been enlarged to cover the whole overlay. We
-                         * only need to adjust x/y here */
-                        x = c->pos_x - mc->x;
-                        y = c->pos_y - mc->y;
+                if (mc->x > c->pos_x) {
+                        x = 0;
+                        outw -= (mc->x - c->pos_x);
+                        paddr += (mc->x - c->pos_x) *
+                                scale_x_m / scale_x_d * bpp / 8;
                 } else {
-                        if (mc->x > c->pos_x) {
-                                x = 0;
-                                w -= (mc->x - c->pos_x);
-                                paddr += (mc->x - c->pos_x) * bpp / 8;
-                        } else {
-                                x = c->pos_x - mc->x;
-                        }
-
-                        if (mc->y > c->pos_y) {
-                                y = 0;
-                                h -= (mc->y - c->pos_y);
-                                paddr += (mc->y - c->pos_y) * c->screen_width *
-                                        bpp / 8;
-                        } else {
-                                y = c->pos_y - mc->y;
-                        }
-
-                        if (mc->w < (x+w))
-                                w -= (x+w) - (mc->w);
+                        x = c->pos_x - mc->x;
+                }
 
-                        if (mc->h < (y+h))
-                                h -= (y+h) - (mc->h);
+                if (mc->y > c->pos_y) {
+                        y = 0;
+                        outh -= (mc->y - c->pos_y);
+                        paddr += (mc->y - c->pos_y) *
+                                scale_y_m / scale_y_d *
+                                c->screen_width * bpp / 8;
+                } else {
+                        y = c->pos_y - mc->y;
+                }
 
-                        outw = w;
-                        outh = h;
+                if (mc->w < (x + outw))
+                        outw -= (x + outw) - (mc->w);
+
+                if (mc->h < (y + outh))
+                        outh -= (y + outh) - (mc->h);
+
+                w = w * outw / orig_outw;
+                h = h * outh / orig_outh;
+
+                /* YUV mode overlay's input width has to be even and the
+                 * algorithm above may adjust the width to be odd.
+                 *
+                 * Here we adjust the width if needed, preferring to increase
+                 * 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)) {
+                        if (orig_w > w)
+                                w += 1;
+                        else
+                                w -= 1;
                 }
         }
 
@@ -857,12 +883,12 @@ static int configure_dispc(void)
 {
         struct overlay_cache_data *oc;
         struct manager_cache_data *mc;
-        const int num_ovls = ARRAY_SIZE(dss_cache.overlay_cache);
-        const int num_mgrs = ARRAY_SIZE(dss_cache.manager_cache);
+        const int num_ovls = dss_feat_get_num_ovls();
+        const int num_mgrs = dss_feat_get_num_mgrs();
         int i;
         int r;
-        bool mgr_busy[2];
-        bool mgr_go[2];
+        bool mgr_busy[MAX_DSS_MANAGERS];
+        bool mgr_go[MAX_DSS_MANAGERS];
         bool busy;
 
         r = 0;
@@ -960,15 +986,16 @@ static void make_even(u16 *x, u16 *w)
 /* Configure dispc for partial update. Return possibly modified update
  * area */
 void dss_setup_partial_planes(struct omap_dss_device *dssdev,
-                u16 *xi, u16 *yi, u16 *wi, u16 *hi)
+                u16 *xi, u16 *yi, u16 *wi, u16 *hi, bool enlarge_update_area)
 {
         struct overlay_cache_data *oc;
         struct manager_cache_data *mc;
-        const int num_ovls = ARRAY_SIZE(dss_cache.overlay_cache);
+        const int num_ovls = dss_feat_get_num_ovls();
         struct omap_overlay_manager *mgr;
         int i;
         u16 x, y, w, h;
         unsigned long flags;
+        bool area_changed;
 
         x = *xi;
         y = *yi;
@@ -989,73 +1016,91 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
 
         spin_lock_irqsave(&dss_cache.lock, flags);
 
-        /* We need to show the whole overlay if it is scaled. So look for
-         * those, and make the update area larger if found.
-         * Also mark the overlay cache dirty */
-        for (i = 0; i < num_ovls; ++i) {
-                unsigned x1, y1, x2, y2;
-                unsigned outw, outh;
+        /*
+         * Execute the outer loop until the inner loop has completed
+         * once without increasing the update area. This will ensure that
+         * all scaled overlays end up completely within the update area.
+         */
+        do {
+                area_changed = false;
 
-                oc = &dss_cache.overlay_cache[i];
+                /* We need to show the whole overlay if it is scaled. So look
+                 * for those, and make the update area larger if found.
+                 * Also mark the overlay cache dirty */
+                for (i = 0; i < num_ovls; ++i) {
+                        unsigned x1, y1, x2, y2;
+                        unsigned outw, outh;
 
-                if (oc->channel != mgr->id)
-                        continue;
+                        oc = &dss_cache.overlay_cache[i];
 
-                oc->dirty = true;
+                        if (oc->channel != mgr->id)
+                                continue;
 
-                if (!oc->enabled)
-                        continue;
+                        oc->dirty = true;
 
-                if (!dispc_is_overlay_scaled(oc))
-                        continue;
+                        if (!enlarge_update_area)
+                                continue;
 
-                outw = oc->out_width == 0 ? oc->width : oc->out_width;
-                outh = oc->out_height == 0 ? oc->height : oc->out_height;
+                        if (!oc->enabled)
+                                continue;
 
-                /* is the overlay outside the update region? */
-                if (!rectangle_intersects(x, y, w, h,
-                                        oc->pos_x, oc->pos_y,
-                                        outw, outh))
-                        continue;
+                        if (!dispc_is_overlay_scaled(oc))
+                                continue;
 
-                /* if the overlay totally inside the update region? */
-                if (rectangle_subset(oc->pos_x, oc->pos_y, outw, outh,
-                                        x, y, w, h))
-                        continue;
+                        outw = oc->out_width == 0 ?
+                                oc->width : oc->out_width;
+                        outh = oc->out_height == 0 ?
+                                oc->height : oc->out_height;
+
+                        /* is the overlay outside the update region? */
+                        if (!rectangle_intersects(x, y, w, h,
+                                                oc->pos_x, oc->pos_y,
+                                                outw, outh))
+                                continue;
+
+                        /* if the overlay totally inside the update region? */
+                        if (rectangle_subset(oc->pos_x, oc->pos_y, outw, outh,
+                                                x, y, w, h))
+                                continue;
 
-                if (x > oc->pos_x)
-                        x1 = oc->pos_x;
-                else
-                        x1 = x;
+                        if (x > oc->pos_x)
+                                x1 = oc->pos_x;
+                        else
+                                x1 = x;
 
-                if (y > oc->pos_y)
-                        y1 = oc->pos_y;
-                else
-                        y1 = y;
+                        if (y > oc->pos_y)
+                                y1 = oc->pos_y;
+                        else
+                                y1 = y;
 
-                if ((x + w) < (oc->pos_x + outw))
-                        x2 = oc->pos_x + outw;
-                else
-                        x2 = x + w;
+                        if ((x + w) < (oc->pos_x + outw))
+                                x2 = oc->pos_x + outw;
+                        else
+                                x2 = x + w;
 
-                if ((y + h) < (oc->pos_y + outh))
-                        y2 = oc->pos_y + outh;
-                else
-                        y2 = y + h;
+                        if ((y + h) < (oc->pos_y + outh))
+                                y2 = oc->pos_y + outh;
+                        else
+                                y2 = y + h;
 
-                x = x1;
-                y = y1;
-                w = x2 - x1;
-                h = y2 - y1;
+                        x = x1;
+                        y = y1;
+                        w = x2 - x1;
+                        h = y2 - y1;
 
-                make_even(&x, &w);
+                        make_even(&x, &w);
 
-                DSSDBG("changing upd area due to ovl(%d) scaling %d,%d %dx%d\n",
+                        DSSDBG("changing upd area due to ovl(%d) "
+                               "scaling %d,%d %dx%d\n",
                                 i, x, y, w, h);
-        }
+
+                        area_changed = true;
+                }
+        } while (area_changed);
 
         mc = &dss_cache.manager_cache[mgr->id];
         mc->do_manual_update = true;
+        mc->enlarge_update_area = enlarge_update_area;
         mc->x = x;
         mc->y = y;
         mc->w = w;
@@ -1077,8 +1122,8 @@ void dss_start_update(struct omap_dss_device *dssdev)
 {
         struct manager_cache_data *mc;
         struct overlay_cache_data *oc;
-        const int num_ovls = ARRAY_SIZE(dss_cache.overlay_cache);
-        const int num_mgrs = ARRAY_SIZE(dss_cache.manager_cache);
+        const int num_ovls = dss_feat_get_num_ovls();
+        const int num_mgrs = dss_feat_get_num_mgrs();
         struct omap_overlay_manager *mgr;
         int i;
 
@@ -1107,10 +1152,10 @@ static void dss_apply_irq_handler(void *data, u32 mask)
 {
         struct manager_cache_data *mc;
         struct overlay_cache_data *oc;
-        const int num_ovls = ARRAY_SIZE(dss_cache.overlay_cache);
-        const int num_mgrs = ARRAY_SIZE(dss_cache.manager_cache);
+        const int num_ovls = dss_feat_get_num_ovls();
+        const int num_mgrs = dss_feat_get_num_mgrs();
         int i, r;
-        bool mgr_busy[2];
+        bool mgr_busy[MAX_DSS_MANAGERS];
 
         mgr_busy[0] = dispc_go_busy(0);
         mgr_busy[1] = dispc_go_busy(1);
@@ -1417,7 +1462,7 @@ int dss_init_overlay_managers(struct platform_device *pdev)
 
         num_managers = 0;
 
-        for (i = 0; i < 2; ++i) {
+        for (i = 0; i < dss_feat_get_num_mgrs(); ++i) {
                 struct omap_overlay_manager *mgr;
                 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
 
@@ -1427,14 +1472,10 @@ int dss_init_overlay_managers(struct platform_device *pdev)
                 case 0:
                         mgr->name = "lcd";
                         mgr->id = OMAP_DSS_CHANNEL_LCD;
-                        mgr->supported_displays =
-                                OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
-                                OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI;
                         break;
                 case 1:
                         mgr->name = "tv";
                         mgr->id = OMAP_DSS_CHANNEL_DIGIT;
-                        mgr->supported_displays = OMAP_DISPLAY_TYPE_VENC;
                         break;
                 }
 
@@ -1450,6 +1491,8 @@ int dss_init_overlay_managers(struct platform_device *pdev)
                 mgr->disable = &dss_mgr_disable;
 
                 mgr->caps = OMAP_DSS_OVL_MGR_CAP_DISPC;
+                mgr->supported_displays =
+                        dss_feat_get_supported_displays(mgr->id);
 
                 dss_overlay_setup_dispc_manager(mgr);
 
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 82336583adef..75642c22cac7 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -35,6 +35,7 @@
 #include <plat/cpu.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static int num_overlays;
 static struct list_head overlay_list;
@@ -65,7 +66,7 @@ static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
                 for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
                         mgr = omap_dss_get_overlay_manager(i);
 
-                        if (strncmp(buf, mgr->name, len) == 0)
+                        if (sysfs_streq(buf, mgr->name))
                                 break;
 
                         mgr = NULL;
@@ -237,7 +238,8 @@ static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
         /* Video1 plane does not support global alpha
          * to always make it 255 completely opaque
          */
-        if (ovl->id == OMAP_DSS_VIDEO1)
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA_VID1) &&
+                        ovl->id == OMAP_DSS_VIDEO1)
                 info.global_alpha = 255;
         else
                 info.global_alpha = simple_strtoul(buf, NULL, 10);
@@ -510,11 +512,11 @@ static void omap_dss_add_overlay(struct omap_overlay *overlay)
         list_add_tail(&overlay->list, &overlay_list);
 }
 
-static struct omap_overlay *dispc_overlays[3];
+static struct omap_overlay *dispc_overlays[MAX_DSS_OVERLAYS];
 
 void dss_overlay_setup_dispc_manager(struct omap_overlay_manager *mgr)
 {
-        mgr->num_overlays = 3;
+        mgr->num_overlays = dss_feat_get_num_ovls();
         mgr->overlays = dispc_overlays;
 }
 
@@ -535,7 +537,7 @@ void dss_init_overlays(struct platform_device *pdev)
 
         num_overlays = 0;
 
-        for (i = 0; i < 3; ++i) {
+        for (i = 0; i < dss_feat_get_num_ovls(); ++i) {
                 struct omap_overlay *ovl;
                 ovl = kzalloc(sizeof(*ovl), GFP_KERNEL);
 
@@ -545,18 +547,12 @@ void dss_init_overlays(struct platform_device *pdev)
                 case 0:
                         ovl->name = "gfx";
                         ovl->id = OMAP_DSS_GFX;
-                        ovl->supported_modes = cpu_is_omap34xx() ?
-                                OMAP_DSS_COLOR_GFX_OMAP3 :
-                                OMAP_DSS_COLOR_GFX_OMAP2;
                         ovl->caps = OMAP_DSS_OVL_CAP_DISPC;
                         ovl->info.global_alpha = 255;
                         break;
                 case 1:
                         ovl->name = "vid1";
                         ovl->id = OMAP_DSS_VIDEO1;
-                        ovl->supported_modes = cpu_is_omap34xx() ?
-                                OMAP_DSS_COLOR_VID1_OMAP3 :
-                                OMAP_DSS_COLOR_VID_OMAP2;
                         ovl->caps = OMAP_DSS_OVL_CAP_SCALE |
                                 OMAP_DSS_OVL_CAP_DISPC;
                         ovl->info.global_alpha = 255;
@@ -564,9 +560,6 @@ void dss_init_overlays(struct platform_device *pdev)
                 case 2:
                         ovl->name = "vid2";
                         ovl->id = OMAP_DSS_VIDEO2;
-                        ovl->supported_modes = cpu_is_omap34xx() ?
-                                OMAP_DSS_COLOR_VID2_OMAP3 :
-                                OMAP_DSS_COLOR_VID_OMAP2;
                         ovl->caps = OMAP_DSS_OVL_CAP_SCALE |
                                 OMAP_DSS_OVL_CAP_DISPC;
                         ovl->info.global_alpha = 255;
@@ -579,6 +572,9 @@ void dss_init_overlays(struct platform_device *pdev)
                 ovl->get_overlay_info = &dss_ovl_get_overlay_info;
                 ovl->wait_for_go = &dss_ovl_wait_for_go;
 
+                ovl->supported_modes =
+                        dss_feat_get_supported_color_modes(ovl->id);
+
                 omap_dss_add_overlay(ovl);
 
                 r = kobject_init_and_add(&ovl->kobj, &overlay_ktype,
@@ -651,7 +647,7 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
         }
 
         if (mgr) {
-                for (i = 0; i < 3; i++) {
+                for (i = 0; i < dss_feat_get_num_ovls(); i++) {
                         struct omap_overlay *ovl;
                         ovl = omap_dss_get_overlay(i);
                         if (!ovl->manager || force) {
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index cc23f53cc62d..bbe62464e92d 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -886,7 +886,7 @@ int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
                 return -EINVAL;
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-                dss_setup_partial_planes(dssdev, x, y, w, h);
+                dss_setup_partial_planes(dssdev, x, y, w, h, true);
                 dispc_set_lcd_size(*w, *h);
         }
 
diff --git a/drivers/video/omap2/omapfb/Kconfig b/drivers/video/omap2/omapfb/Kconfig
index 43496d6c377f..65149b22cf37 100644
--- a/drivers/video/omap2/omapfb/Kconfig
+++ b/drivers/video/omap2/omapfb/Kconfig
@@ -3,7 +3,7 @@ menuconfig FB_OMAP2
         depends on FB && OMAP2_DSS
 
         select OMAP2_VRAM
-        select OMAP2_VRFB
+        select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index 9c7361871d78..6f435450987e 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -34,12 +34,37 @@
 
 #include "omapfb.h"
 
+static u8 get_mem_idx(struct omapfb_info *ofbi)
+{
+        if (ofbi->id == ofbi->region->id)
+                return 0;
+
+        return OMAPFB_MEM_IDX_ENABLED | ofbi->region->id;
+}
+
+static struct omapfb2_mem_region *get_mem_region(struct omapfb_info *ofbi,
+                                                 u8 mem_idx)
+{
+        struct omapfb2_device *fbdev = ofbi->fbdev;
+
+        if (mem_idx & OMAPFB_MEM_IDX_ENABLED)
+                mem_idx &= OMAPFB_MEM_IDX_MASK;
+        else
+                mem_idx = ofbi->id;
+
+        if (mem_idx >= fbdev->num_fbs)
+                return NULL;
+
+        return &fbdev->regions[mem_idx];
+}
+
 static int omapfb_setup_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omap_overlay *ovl;
-        struct omap_overlay_info info;
+        struct omap_overlay_info old_info;
+        struct omapfb2_mem_region *old_rg, *new_rg;
         int r = 0;
 
         DBG("omapfb_setup_plane\n");
@@ -52,36 +77,106 @@ static int omapfb_setup_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
         /* XXX uses only the first overlay */
         ovl = ofbi->overlays[0];
 
-        if (pi->enabled && !ofbi->region.size) {
+        old_rg = ofbi->region;
+        new_rg = get_mem_region(ofbi, pi->mem_idx);
+        if (!new_rg) {
+                r = -EINVAL;
+                goto out;
+        }
+
+        /* Take the locks in a specific order to keep lockdep happy */
+        if (old_rg->id < new_rg->id) {
+                omapfb_get_mem_region(old_rg);
+                omapfb_get_mem_region(new_rg);
+        } else if (new_rg->id < old_rg->id) {
+                omapfb_get_mem_region(new_rg);
+                omapfb_get_mem_region(old_rg);
+        } else
+                omapfb_get_mem_region(old_rg);
+
+        if (pi->enabled && !new_rg->size) {
                 /*
                  * This plane's memory was freed, can't enable it
                  * until it's reallocated.
                  */
                 r = -EINVAL;
-                goto out;
+                goto put_mem;
         }
 
-        ovl->get_overlay_info(ovl, &info);
+        ovl->get_overlay_info(ovl, &old_info);
 
-        info.pos_x = pi->pos_x;
-        info.pos_y = pi->pos_y;
-        info.out_width = pi->out_width;
-        info.out_height = pi->out_height;
-        info.enabled = pi->enabled;
+        if (old_rg != new_rg) {
+                ofbi->region = new_rg;
+                set_fb_fix(fbi);
+        }
 
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                goto out;
+        if (pi->enabled) {
+                struct omap_overlay_info info;
 
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
+                r = omapfb_setup_overlay(fbi, ovl, pi->pos_x, pi->pos_y,
+                        pi->out_width, pi->out_height);
                 if (r)
-                        goto out;
+                        goto undo;
+
+                ovl->get_overlay_info(ovl, &info);
+
+                if (!info.enabled) {
+                        info.enabled = pi->enabled;
+                        r = ovl->set_overlay_info(ovl, &info);
+                        if (r)
+                                goto undo;
+                }
+        } else {
+                struct omap_overlay_info info;
+
+                ovl->get_overlay_info(ovl, &info);
+
+                info.enabled = pi->enabled;
+                info.pos_x = pi->pos_x;
+                info.pos_y = pi->pos_y;
+                info.out_width = pi->out_width;
+                info.out_height = pi->out_height;
+
+                r = ovl->set_overlay_info(ovl, &info);
+                if (r)
+                        goto undo;
         }
 
-out:
-        if (r)
-                dev_err(fbdev->dev, "setup_plane failed\n");
+        if (ovl->manager)
+                ovl->manager->apply(ovl->manager);
+
+        /* Release the locks in a specific order to keep lockdep happy */
+        if (old_rg->id > new_rg->id) {
+                omapfb_put_mem_region(old_rg);
+                omapfb_put_mem_region(new_rg);
+        } else if (new_rg->id > old_rg->id) {
+                omapfb_put_mem_region(new_rg);
+                omapfb_put_mem_region(old_rg);
+        } else
+                omapfb_put_mem_region(old_rg);
+
+        return 0;
+
+ undo:
+        if (old_rg != new_rg) {
+                ofbi->region = old_rg;
+                set_fb_fix(fbi);
+        }
+
+        ovl->set_overlay_info(ovl, &old_info);
+ put_mem:
+        /* Release the locks in a specific order to keep lockdep happy */
+        if (old_rg->id > new_rg->id) {
+                omapfb_put_mem_region(old_rg);
+                omapfb_put_mem_region(new_rg);
+        } else if (new_rg->id > old_rg->id) {
+                omapfb_put_mem_region(new_rg);
+                omapfb_put_mem_region(old_rg);
+        } else
+                omapfb_put_mem_region(old_rg);
+ out:
+        dev_err(fbdev->dev, "setup_plane failed\n");
+
         return r;
 }
 
@@ -92,8 +187,8 @@ static int omapfb_query_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
         if (ofbi->num_overlays != 1) {
                 memset(pi, 0, sizeof(*pi));
         } else {
-                struct omap_overlay_info *ovli;
                 struct omap_overlay *ovl;
+                struct omap_overlay_info *ovli;
 
                 ovl = ofbi->overlays[0];
                 ovli = &ovl->info;
@@ -103,6 +198,7 @@ static int omapfb_query_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
                 pi->enabled = ovli->enabled;
                 pi->channel_out = 0; /* xxx */
                 pi->mirror = 0;
+                pi->mem_idx = get_mem_idx(ofbi);
                 pi->out_width = ovli->out_width;
                 pi->out_height = ovli->out_height;
         }
@@ -115,7 +211,7 @@ static int omapfb_setup_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omapfb2_mem_region *rg;
-        int r, i;
+        int r = 0, i;
         size_t size;
 
         if (mi->type > OMAPFB_MEMTYPE_MAX)
@@ -123,22 +219,44 @@ static int omapfb_setup_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
 
         size = PAGE_ALIGN(mi->size);
 
-        rg = &ofbi->region;
+        rg = ofbi->region;
 
-        for (i = 0; i < ofbi->num_overlays; i++) {
-                if (ofbi->overlays[i]->info.enabled)
-                        return -EBUSY;
+        down_write_nested(&rg->lock, rg->id);
+        atomic_inc(&rg->lock_count);
+
+        if (atomic_read(&rg->map_count)) {
+                r = -EBUSY;
+                goto out;
+        }
+
+        for (i = 0; i < fbdev->num_fbs; i++) {
+                struct omapfb_info *ofbi2 = FB2OFB(fbdev->fbs[i]);
+                int j;
+
+                if (ofbi2->region != rg)
+                        continue;
+
+                for (j = 0; j < ofbi2->num_overlays; j++) {
+                        if (ofbi2->overlays[j]->info.enabled) {
+                                r = -EBUSY;
+                                goto out;
+                        }
+                }
         }
 
         if (rg->size != size || rg->type != mi->type) {
                 r = omapfb_realloc_fbmem(fbi, size, mi->type);
                 if (r) {
                         dev_err(fbdev->dev, "realloc fbmem failed\n");
-                        return r;
+                        goto out;
                 }
         }
 
-        return 0;
+ out:
+        atomic_dec(&rg->lock_count);
+        up_write(&rg->lock);
+
+        return r;
 }
 
 static int omapfb_query_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
@@ -146,12 +264,14 @@ static int omapfb_query_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_mem_region *rg;
 
-        rg = &ofbi->region;
+        rg = omapfb_get_mem_region(ofbi->region);
         memset(mi, 0, sizeof(*mi));
 
         mi->size = rg->size;
         mi->type = rg->type;
 
+        omapfb_put_mem_region(rg);
+
         return 0;
 }
 
@@ -490,6 +610,7 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
                 struct omapfb_vram_info         vram_info;
                 struct omapfb_tearsync_info     tearsync_info;
                 struct omapfb_display_info      display_info;
+                u32                             crt;
         } p;
 
         int r = 0;
@@ -648,6 +769,17 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
                         r = -EFAULT;
                 break;
 
+        case FBIO_WAITFORVSYNC:
+                if (get_user(p.crt, (__u32 __user *)arg)) {
+                        r = -EFAULT;
+                        break;
+                }
+                if (p.crt != 0) {
+                        r = -ENODEV;
+                        break;
+                }
+                /* FALLTHROUGH */
+
         case OMAPFB_WAITFORVSYNC:
                 DBG("ioctl WAITFORVSYNC\n");
                 if (!display) {
@@ -738,7 +870,7 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
                         break;
                 }
 
-                if (!display->driver->enable_te) {
+                if (!display || !display->driver->enable_te) {
                         r = -ENODEV;
                         break;
                 }
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 4b4506da96da..6a704f176c22 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -157,7 +157,7 @@ static void fill_fb(struct fb_info *fbi)
 
 static unsigned omapfb_get_vrfb_offset(const struct omapfb_info *ofbi, int rot)
 {
-        const struct vrfb *vrfb = &ofbi->region.vrfb;
+        const struct vrfb *vrfb = &ofbi->region->vrfb;
         unsigned offset;
 
         switch (rot) {
@@ -185,27 +185,27 @@ static unsigned omapfb_get_vrfb_offset(const struct omapfb_info *ofbi, int rot)
 static u32 omapfb_get_region_rot_paddr(const struct omapfb_info *ofbi, int rot)
 {
         if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB) {
-                return ofbi->region.vrfb.paddr[rot]
+                return ofbi->region->vrfb.paddr[rot]
                         + omapfb_get_vrfb_offset(ofbi, rot);
         } else {
-                return ofbi->region.paddr;
+                return ofbi->region->paddr;
         }
 }
 
 static u32 omapfb_get_region_paddr(const struct omapfb_info *ofbi)
 {
         if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
-                return ofbi->region.vrfb.paddr[0];
+                return ofbi->region->vrfb.paddr[0];
         else
-                return ofbi->region.paddr;
+                return ofbi->region->paddr;
 }
 
 static void __iomem *omapfb_get_region_vaddr(const struct omapfb_info *ofbi)
 {
         if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
-                return ofbi->region.vrfb.vaddr[0];
+                return ofbi->region->vrfb.vaddr[0];
         else
-                return ofbi->region.vaddr;
+                return ofbi->region->vaddr;
 }
 
 static struct omapfb_colormode omapfb_colormodes[] = {
@@ -450,7 +450,7 @@ static int check_vrfb_fb_size(unsigned long region_size,
 static int check_fb_size(const struct omapfb_info *ofbi,
                 struct fb_var_screeninfo *var)
 {
-        unsigned long max_frame_size = ofbi->region.size;
+        unsigned long max_frame_size = ofbi->region->size;
         int bytespp = var->bits_per_pixel >> 3;
         unsigned long line_size = var->xres_virtual * bytespp;
 
@@ -497,7 +497,7 @@ static int check_fb_size(const struct omapfb_info *ofbi,
 static int setup_vrfb_rotation(struct fb_info *fbi)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        struct omapfb2_mem_region *rg = &ofbi->region;
+        struct omapfb2_mem_region *rg = ofbi->region;
         struct vrfb *vrfb = &rg->vrfb;
         struct fb_var_screeninfo *var = &fbi->var;
         struct fb_fix_screeninfo *fix = &fbi->fix;
@@ -558,9 +558,9 @@ static int setup_vrfb_rotation(struct fb_info *fbi)
                 return r;
 
         /* used by open/write in fbmem.c */
-        fbi->screen_base = ofbi->region.vrfb.vaddr[0];
+        fbi->screen_base = ofbi->region->vrfb.vaddr[0];
 
-        fix->smem_start = ofbi->region.vrfb.paddr[0];
+        fix->smem_start = ofbi->region->vrfb.paddr[0];
 
         switch (var->nonstd) {
         case OMAPFB_COLOR_YUV422:
@@ -599,7 +599,7 @@ void set_fb_fix(struct fb_info *fbi)
         struct fb_fix_screeninfo *fix = &fbi->fix;
         struct fb_var_screeninfo *var = &fbi->var;
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        struct omapfb2_mem_region *rg = &ofbi->region;
+        struct omapfb2_mem_region *rg = ofbi->region;
 
         DBG("set_fb_fix\n");
 
@@ -668,8 +668,7 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
 
         DBG("check_fb_var %d\n", ofbi->id);
 
-        if (ofbi->region.size == 0)
-                return 0;
+        WARN_ON(!atomic_read(&ofbi->region->lock_count));
 
         r = fb_mode_to_dss_mode(var, &mode);
         if (r) {
@@ -684,13 +683,14 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
                 }
         }
 
-        if (var->rotate < 0 || var->rotate > 3)
+        if (var->rotate > 3)
                 return -EINVAL;
 
         if (check_fb_res_bounds(var))
                 return -EINVAL;
 
-        if (check_fb_size(ofbi, var))
+        /* When no memory is allocated ignore the size check */
+        if (ofbi->region->size != 0 && check_fb_size(ofbi, var))
                 return -EINVAL;
 
         if (var->xres + var->xoffset > var->xres_virtual)
@@ -714,10 +714,10 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
                 var->pixclock = timings.pixel_clock != 0 ?
                         KHZ2PICOS(timings.pixel_clock) :
                         0;
-                var->left_margin = timings.hfp;
-                var->right_margin = timings.hbp;
-                var->upper_margin = timings.vfp;
-                var->lower_margin = timings.vbp;
+                var->left_margin = timings.hbp;
+                var->right_margin = timings.hfp;
+                var->upper_margin = timings.vbp;
+                var->lower_margin = timings.vfp;
                 var->hsync_len = timings.hsw;
                 var->vsync_len = timings.vsw;
         } else {
@@ -822,9 +822,43 @@ static unsigned calc_rotation_offset_vrfb(const struct fb_var_screeninfo *var,
         return offset;
 }
 
+static void omapfb_calc_addr(const struct omapfb_info *ofbi,
+                             const struct fb_var_screeninfo *var,
+                             const struct fb_fix_screeninfo *fix,
+                             int rotation, u32 *paddr, void __iomem **vaddr)
+{
+        u32 data_start_p;
+        void __iomem *data_start_v;
+        int offset;
+
+        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB) {
+                data_start_p = omapfb_get_region_rot_paddr(ofbi, rotation);
+                data_start_v = NULL;
+        } else {
+                data_start_p = omapfb_get_region_paddr(ofbi);
+                data_start_v = omapfb_get_region_vaddr(ofbi);
+        }
+
+        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
+                offset = calc_rotation_offset_vrfb(var, fix, rotation);
+        else
+                offset = calc_rotation_offset_dma(var, fix, rotation);
+
+        data_start_p += offset;
+        data_start_v += offset;
+
+        if (offset)
+                DBG("offset %d, %d = %d\n",
+                    var->xoffset, var->yoffset, offset);
+
+        DBG("paddr %x, vaddr %p\n", data_start_p, data_start_v);
+
+        *paddr = data_start_p;
+        *vaddr = data_start_v;
+}
 
 /* setup overlay according to the fb */
-static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
+int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
                 u16 posx, u16 posy, u16 outw, u16 outh)
 {
         int r = 0;
@@ -832,9 +866,8 @@ static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
         struct fb_var_screeninfo *var = &fbi->var;
         struct fb_fix_screeninfo *fix = &fbi->fix;
         enum omap_color_mode mode = 0;
-        int offset;
-        u32 data_start_p;
-        void __iomem *data_start_v;
+        u32 data_start_p = 0;
+        void __iomem *data_start_v = NULL;
         struct omap_overlay_info info;
         int xres, yres;
         int screen_width;
@@ -842,6 +875,8 @@ static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
         int rotation = var->rotate;
         int i;
 
+        WARN_ON(!atomic_read(&ofbi->region->lock_count));
+
         for (i = 0; i < ofbi->num_overlays; i++) {
                 if (ovl != ofbi->overlays[i])
                         continue;
@@ -861,28 +896,9 @@ static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
                 yres = var->yres;
         }
 
-
-        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB) {
-                data_start_p = omapfb_get_region_rot_paddr(ofbi, rotation);
-                data_start_v = NULL;
-        } else {
-                data_start_p = omapfb_get_region_paddr(ofbi);
-                data_start_v = omapfb_get_region_vaddr(ofbi);
-        }
-
-        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
-                offset = calc_rotation_offset_vrfb(var, fix, rotation);
-        else
-                offset = calc_rotation_offset_dma(var, fix, rotation);
-
-        data_start_p += offset;
-        data_start_v += offset;
-
-        if (offset)
-                DBG("offset %d, %d = %d\n",
-                                var->xoffset, var->yoffset, offset);
-
-        DBG("paddr %x, vaddr %p\n", data_start_p, data_start_v);
+        if (ofbi->region->size)
+                omapfb_calc_addr(ofbi, var, fix, rotation,
+                                 &data_start_p, &data_start_v);
 
         r = fb_mode_to_dss_mode(var, &mode);
         if (r) {
@@ -954,12 +970,14 @@ int omapfb_apply_changes(struct fb_info *fbi, int init)
                 fill_fb(fbi);
 #endif
 
+        WARN_ON(!atomic_read(&ofbi->region->lock_count));
+
         for (i = 0; i < ofbi->num_overlays; i++) {
                 ovl = ofbi->overlays[i];
 
                 DBG("apply_changes, fb %d, ovl %d\n", ofbi->id, ovl->id);
 
-                if (ofbi->region.size == 0) {
+                if (ofbi->region->size == 0) {
                         /* the fb is not available. disable the overlay */
                         omapfb_overlay_enable(ovl, 0);
                         if (!init && ovl->manager)
@@ -1007,36 +1025,48 @@ err:
  * DO NOT MODIFY PAR */
 static int omapfb_check_var(struct fb_var_screeninfo *var, struct fb_info *fbi)
 {
+        struct omapfb_info *ofbi = FB2OFB(fbi);
         int r;
 
         DBG("check_var(%d)\n", FB2OFB(fbi)->id);
 
+        omapfb_get_mem_region(ofbi->region);
+
         r = check_fb_var(fbi, var);
 
+        omapfb_put_mem_region(ofbi->region);
+
         return r;
 }
 
 /* set the video mode according to info->var */
 static int omapfb_set_par(struct fb_info *fbi)
 {
+        struct omapfb_info *ofbi = FB2OFB(fbi);
         int r;
 
         DBG("set_par(%d)\n", FB2OFB(fbi)->id);
 
+        omapfb_get_mem_region(ofbi->region);
+
         set_fb_fix(fbi);
 
         r = setup_vrfb_rotation(fbi);
         if (r)
-                return r;
+                goto out;
 
         r = omapfb_apply_changes(fbi, 0);
 
+ out:
+        omapfb_put_mem_region(ofbi->region);
+
         return r;
 }
 
 static int omapfb_pan_display(struct fb_var_screeninfo *var,
                 struct fb_info *fbi)
 {
+        struct omapfb_info *ofbi = FB2OFB(fbi);
         struct fb_var_screeninfo new_var;
         int r;
 
@@ -1052,23 +1082,31 @@ static int omapfb_pan_display(struct fb_var_screeninfo *var,
 
         fbi->var = new_var;
 
+        omapfb_get_mem_region(ofbi->region);
+
         r = omapfb_apply_changes(fbi, 0);
 
+        omapfb_put_mem_region(ofbi->region);
+
         return r;
 }
 
 static void mmap_user_open(struct vm_area_struct *vma)
 {
-        struct omapfb_info *ofbi = (struct omapfb_info *)vma->vm_private_data;
+        struct omapfb2_mem_region *rg = vma->vm_private_data;
 
-        atomic_inc(&ofbi->map_count);
+        omapfb_get_mem_region(rg);
+        atomic_inc(&rg->map_count);
+        omapfb_put_mem_region(rg);
 }
 
 static void mmap_user_close(struct vm_area_struct *vma)
 {
-        struct omapfb_info *ofbi = (struct omapfb_info *)vma->vm_private_data;
+        struct omapfb2_mem_region *rg = vma->vm_private_data;
 
-        atomic_dec(&ofbi->map_count);
+        omapfb_get_mem_region(rg);
+        atomic_dec(&rg->map_count);
+        omapfb_put_mem_region(rg);
 }
 
 static struct vm_operations_struct mmap_user_ops = {
@@ -1080,9 +1118,11 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct fb_fix_screeninfo *fix = &fbi->fix;
+        struct omapfb2_mem_region *rg;
         unsigned long off;
         unsigned long start;
         u32 len;
+        int r = -EINVAL;
 
         if (vma->vm_end - vma->vm_start == 0)
                 return 0;
@@ -1090,12 +1130,14 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
                 return -EINVAL;
         off = vma->vm_pgoff << PAGE_SHIFT;
 
+        rg = omapfb_get_mem_region(ofbi->region);
+
         start = omapfb_get_region_paddr(ofbi);
         len = fix->smem_len;
         if (off >= len)
-                return -EINVAL;
+                goto error;
         if ((vma->vm_end - vma->vm_start + off) > len)
-                return -EINVAL;
+                goto error;
 
         off += start;
 
@@ -1105,13 +1147,25 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
         vma->vm_flags |= VM_IO | VM_RESERVED;
         vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
         vma->vm_ops = &mmap_user_ops;
-        vma->vm_private_data = ofbi;
+        vma->vm_private_data = rg;
         if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-                             vma->vm_end - vma->vm_start, vma->vm_page_prot))
-                return -EAGAIN;
+                               vma->vm_end - vma->vm_start,
+                               vma->vm_page_prot)) {
+                r = -EAGAIN;
+                goto error;
+        }
+
         /* vm_ops.open won't be called for mmap itself. */
-        atomic_inc(&ofbi->map_count);
+        atomic_inc(&rg->map_count);
+
+        omapfb_put_mem_region(rg);
+
         return 0;
+
+ error:
+        omapfb_put_mem_region(ofbi->region);
+
+        return r;
 }
 
 /* Store a single color palette entry into a pseudo palette or the hardware
@@ -1154,11 +1208,6 @@ static int _setcolreg(struct fb_info *fbi, u_int regno, u_int red, u_int green,
                 if (r != 0)
                         break;
 
-                if (regno < 0) {
-                        r = -EINVAL;
-                        break;
-                }
-
                 if (regno < 16) {
                         u16 pal;
                         pal = ((red >> (16 - var->red.length)) <<
@@ -1217,6 +1266,9 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
         int do_update = 0;
         int r = 0;
 
+        if (!display)
+                return -EINVAL;
+
         omapfb_lock(fbdev);
 
         switch (blank) {
@@ -1300,7 +1352,9 @@ static void omapfb_free_fbmem(struct fb_info *fbi)
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omapfb2_mem_region *rg;
 
-        rg = &ofbi->region;
+        rg = ofbi->region;
+
+        WARN_ON(atomic_read(&rg->map_count));
 
         if (rg->paddr)
                 if (omap_vram_free(rg->paddr, rg->size))
@@ -1355,8 +1409,15 @@ static int omapfb_alloc_fbmem(struct fb_info *fbi, unsigned long size,
         void __iomem *vaddr;
         int r;
 
-        rg = &ofbi->region;
-        memset(rg, 0, sizeof(*rg));
+        rg = ofbi->region;
+
+        rg->paddr = 0;
+        rg->vaddr = NULL;
+        memset(&rg->vrfb, 0, sizeof rg->vrfb);
+        rg->size = 0;
+        rg->type = 0;
+        rg->alloc = false;
+        rg->map = false;
 
         size = PAGE_ALIGN(size);
 
@@ -1609,7 +1670,7 @@ static int omapfb_allocate_all_fbs(struct omapfb2_device *fbdev)
         for (i = 0; i < fbdev->num_fbs; i++) {
                 struct omapfb_info *ofbi = FB2OFB(fbdev->fbs[i]);
                 struct omapfb2_mem_region *rg;
-                rg = &ofbi->region;
+                rg = ofbi->region;
 
                 DBG("region%d phys %08x virt %p size=%lu\n",
                                 i,
@@ -1626,7 +1687,7 @@ int omapfb_realloc_fbmem(struct fb_info *fbi, unsigned long size, int type)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omap_dss_device *display = fb2display(fbi);
-        struct omapfb2_mem_region *rg = &ofbi->region;
+        struct omapfb2_mem_region *rg = ofbi->region;
         unsigned long old_size = rg->size;
         unsigned long old_paddr = rg->paddr;
         int old_type = rg->type;
@@ -1709,7 +1770,7 @@ static int omapfb_fb_init(struct omapfb2_device *fbdev, struct fb_info *fbi)
         fbi->flags = FBINFO_FLAG_DEFAULT;
         fbi->pseudo_palette = fbdev->pseudo_palette;
 
-        if (ofbi->region.size == 0) {
+        if (ofbi->region->size == 0) {
                 clear_fb_info(fbi);
                 return 0;
         }
@@ -1871,6 +1932,10 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
                 ofbi->fbdev = fbdev;
                 ofbi->id = i;
 
+                ofbi->region = &fbdev->regions[i];
+                ofbi->region->id = i;
+                init_rwsem(&ofbi->region->lock);
+
                 /* assign these early, so that fb alloc can use them */
                 ofbi->rotation_type = def_vrfb ? OMAP_DSS_ROT_VRFB :
                         OMAP_DSS_ROT_DMA;
@@ -1900,7 +1965,13 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
 
         /* setup fb_infos */
         for (i = 0; i < fbdev->num_fbs; i++) {
-                r = omapfb_fb_init(fbdev, fbdev->fbs[i]);
+                struct fb_info *fbi = fbdev->fbs[i];
+                struct omapfb_info *ofbi = FB2OFB(fbi);
+
+                omapfb_get_mem_region(ofbi->region);
+                r = omapfb_fb_init(fbdev, fbi);
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r) {
                         dev_err(fbdev->dev, "failed to setup fb_info\n");
                         return r;
@@ -1921,20 +1992,19 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
         DBG("framebuffers registered\n");
 
         for (i = 0; i < fbdev->num_fbs; i++) {
-                r = omapfb_apply_changes(fbdev->fbs[i], 1);
+                struct fb_info *fbi = fbdev->fbs[i];
+                struct omapfb_info *ofbi = FB2OFB(fbi);
+
+                omapfb_get_mem_region(ofbi->region);
+                r = omapfb_apply_changes(fbi, 1);
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r) {
                         dev_err(fbdev->dev, "failed to change mode\n");
                         return r;
                 }
         }
 
-        DBG("create sysfs for fbs\n");
-        r = omapfb_create_sysfs(fbdev);
-        if (r) {
-                dev_err(fbdev->dev, "failed to create sysfs entries\n");
-                return r;
-        }
-
         /* Enable fb0 */
         if (fbdev->num_fbs > 0) {
                 struct omapfb_info *ofbi = FB2OFB(fbdev->fbs[0]);
@@ -1968,11 +2038,11 @@ static int omapfb_mode_to_timings(const char *mode_str,
 #ifdef CONFIG_OMAP2_DSS_VENC
         if (strcmp(mode_str, "pal") == 0) {
                 *timings = omap_dss_pal_timings;
-                *bpp = 0;
+                *bpp = 24;
                 return 0;
         } else if (strcmp(mode_str, "ntsc") == 0) {
                 *timings = omap_dss_ntsc_timings;
-                *bpp = 0;
+                *bpp = 24;
                 return 0;
         }
 #endif
@@ -1989,10 +2059,10 @@ static int omapfb_mode_to_timings(const char *mode_str,
 
         if (r != 0) {
                 timings->pixel_clock = PICOS2KHZ(var.pixclock);
-                timings->hfp = var.left_margin;
-                timings->hbp = var.right_margin;
-                timings->vfp = var.upper_margin;
-                timings->vbp = var.lower_margin;
+                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;
@@ -2128,6 +2198,16 @@ static int omapfb_probe(struct platform_device *pdev)
                 goto err0;
         }
 
+        /* TODO : Replace cpu check with omap_has_vrfb once HAS_FEATURE
+        *        available for OMAP2 and OMAP3
+        */
+        if (def_vrfb && !cpu_is_omap24xx() && !cpu_is_omap34xx()) {
+                def_vrfb = 0;
+                dev_warn(&pdev->dev, "VRFB is not supported on this hardware, "
+                                "ignoring the module parameter vrfb=y\n");
+        }
+
+
         mutex_init(&fbdev->mtx);
 
         fbdev->dev = &pdev->dev;
@@ -2220,6 +2300,13 @@ static int omapfb_probe(struct platform_device *pdev)
                 }
         }
 
+        DBG("create sysfs for fbs\n");
+        r = omapfb_create_sysfs(fbdev);
+        if (r) {
+                dev_err(fbdev->dev, "failed to create sysfs entries\n");
+                goto cleanup;
+        }
+
         return 0;
 
 cleanup:
diff --git a/drivers/video/omap2/omapfb/omapfb-sysfs.c b/drivers/video/omap2/omapfb/omapfb-sysfs.c
index 5179219128bd..6f9c72cd6bb0 100644
--- a/drivers/video/omap2/omapfb/omapfb-sysfs.c
+++ b/drivers/video/omap2/omapfb/omapfb-sysfs.c
@@ -49,6 +49,7 @@ 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 r;
 
@@ -64,9 +65,11 @@ static ssize_t store_rotate_type(struct device *dev,
         if (rot_type == ofbi->rotation_type)
                 goto out;
 
-        if (ofbi->region.size) {
+        rg = omapfb_get_mem_region(ofbi->region);
+
+        if (rg->size) {
                 r = -EBUSY;
-                goto out;
+                goto put_region;
         }
 
         ofbi->rotation_type = rot_type;
@@ -75,6 +78,8 @@ static ssize_t store_rotate_type(struct device *dev,
          * Since the VRAM for this FB is not allocated at the moment we don't
          * need to do any further parameter checking at this point.
          */
+put_region:
+        omapfb_put_mem_region(rg);
 out:
         unlock_fb_info(fbi);
 
@@ -97,7 +102,7 @@ static ssize_t store_mirror(struct device *dev,
 {
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        bool mirror;
+        unsigned long mirror;
         int r;
         struct fb_var_screeninfo new_var;
 
@@ -111,6 +116,8 @@ static ssize_t store_mirror(struct device *dev,
 
         ofbi->mirror = mirror;
 
+        omapfb_get_mem_region(ofbi->region);
+
         memcpy(&new_var, &fbi->var, sizeof(new_var));
         r = check_fb_var(fbi, &new_var);
         if (r)
@@ -125,6 +132,8 @@ static ssize_t store_mirror(struct device *dev,
 
         r = count;
 out:
+        omapfb_put_mem_region(ofbi->region);
+
         unlock_fb_info(fbi);
 
         return r;
@@ -263,11 +272,15 @@ static ssize_t store_overlays(struct device *dev, struct device_attribute *attr,
 
                 DBG("detaching %d\n", ofbi->overlays[i]->id);
 
+                omapfb_get_mem_region(ofbi->region);
+
                 omapfb_overlay_enable(ovl, 0);
 
                 if (ovl->manager)
                         ovl->manager->apply(ovl->manager);
 
+                omapfb_put_mem_region(ofbi->region);
+
                 for (t = i + 1; t < ofbi->num_overlays; t++) {
                         ofbi->rotation[t-1] = ofbi->rotation[t];
                         ofbi->overlays[t-1] = ofbi->overlays[t];
@@ -300,7 +313,12 @@ static ssize_t store_overlays(struct device *dev, struct device_attribute *attr,
         }
 
         if (added) {
+                omapfb_get_mem_region(ofbi->region);
+
                 r = omapfb_apply_changes(fbi, 0);
+
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r)
                         goto out;
         }
@@ -388,7 +406,12 @@ static ssize_t store_overlays_rotate(struct device *dev,
                 for (i = 0; i < num_ovls; ++i)
                         ofbi->rotation[i] = rotation[i];
 
+                omapfb_get_mem_region(ofbi->region);
+
                 r = omapfb_apply_changes(fbi, 0);
+
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r)
                         goto out;
 
@@ -408,7 +431,7 @@ static ssize_t show_size(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
 
-        return snprintf(buf, PAGE_SIZE, "%lu\n", ofbi->region.size);
+        return snprintf(buf, PAGE_SIZE, "%lu\n", ofbi->region->size);
 }
 
 static ssize_t store_size(struct device *dev, struct device_attribute *attr,
@@ -416,6 +439,8 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
 {
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
+        struct omapfb2_device *fbdev = ofbi->fbdev;
+        struct omapfb2_mem_region *rg;
         unsigned long size;
         int r;
         int i;
@@ -425,15 +450,33 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
         if (!lock_fb_info(fbi))
                 return -ENODEV;
 
-        for (i = 0; i < ofbi->num_overlays; i++) {
-                if (ofbi->overlays[i]->info.enabled) {
-                        r = -EBUSY;
-                        goto out;
+        rg = ofbi->region;
+
+        down_write_nested(&rg->lock, rg->id);
+        atomic_inc(&rg->lock_count);
+
+        if (atomic_read(&rg->map_count)) {
+                r = -EBUSY;
+                goto out;
+        }
+
+        for (i = 0; i < fbdev->num_fbs; i++) {
+                struct omapfb_info *ofbi2 = FB2OFB(fbdev->fbs[i]);
+                int j;
+
+                if (ofbi2->region != rg)
+                        continue;
+
+                for (j = 0; j < ofbi2->num_overlays; j++) {
+                        if (ofbi2->overlays[j]->info.enabled) {
+                                r = -EBUSY;
+                                goto out;
+                        }
                 }
         }
 
-        if (size != ofbi->region.size) {
-                r = omapfb_realloc_fbmem(fbi, size, ofbi->region.type);
+        if (size != ofbi->region->size) {
+                r = omapfb_realloc_fbmem(fbi, size, ofbi->region->type);
                 if (r) {
                         dev_err(dev, "realloc fbmem failed\n");
                         goto out;
@@ -442,6 +485,9 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
 
         r = count;
 out:
+        atomic_dec(&rg->lock_count);
+        up_write(&rg->lock);
+
         unlock_fb_info(fbi);
 
         return r;
@@ -453,7 +499,7 @@ static ssize_t show_phys(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
 
-        return snprintf(buf, PAGE_SIZE, "%0x\n", ofbi->region.paddr);
+        return snprintf(buf, PAGE_SIZE, "%0x\n", ofbi->region->paddr);
 }
 
 static ssize_t show_virt(struct device *dev,
@@ -462,7 +508,7 @@ static ssize_t show_virt(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
 
-        return snprintf(buf, PAGE_SIZE, "%p\n", ofbi->region.vaddr);
+        return snprintf(buf, PAGE_SIZE, "%p\n", ofbi->region->vaddr);
 }
 
 static struct device_attribute omapfb_attrs[] = {
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index cd54fdbfd8bb..1305fc9880ba 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -27,6 +27,8 @@
 #define DEBUG
 #endif
 
+#include <linux/rwsem.h>
+
 #include <plat/display.h>
 
 #ifdef DEBUG
@@ -44,6 +46,7 @@ extern unsigned int omapfb_debug;
 #define OMAPFB_MAX_OVL_PER_FB 3
 
 struct omapfb2_mem_region {
+        int             id;
         u32             paddr;
         void __iomem    *vaddr;
         struct vrfb     vrfb;
@@ -51,13 +54,15 @@ struct omapfb2_mem_region {
         u8              type;           /* OMAPFB_PLANE_MEM_* */
         bool            alloc;          /* allocated by the driver */
         bool            map;            /* kernel mapped by the driver */
+        atomic_t        map_count;
+        struct rw_semaphore lock;
+        atomic_t        lock_count;
 };
 
 /* appended to fb_info */
 struct omapfb_info {
         int id;
-        struct omapfb2_mem_region region;
-        atomic_t map_count;
+        struct omapfb2_mem_region *region;
         int num_overlays;
         struct omap_overlay *overlays[OMAPFB_MAX_OVL_PER_FB];
         struct omapfb2_device *fbdev;
@@ -76,6 +81,7 @@ struct omapfb2_device {
 
         unsigned num_fbs;
         struct fb_info *fbs[10];
+        struct omapfb2_mem_region regions[10];
 
         unsigned num_displays;
         struct omap_dss_device *displays[10];
@@ -117,6 +123,9 @@ int omapfb_update_window(struct fb_info *fbi,
 int dss_mode_to_fb_mode(enum omap_color_mode dssmode,
                         struct fb_var_screeninfo *var);
 
+int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
+                u16 posx, u16 posy, u16 outw, u16 outh);
+
 /* find the display connected to this fb, if any */
 static inline struct omap_dss_device *fb2display(struct fb_info *fbi)
 {
@@ -148,8 +157,24 @@ static inline int omapfb_overlay_enable(struct omap_overlay *ovl,
         struct omap_overlay_info info;
 
         ovl->get_overlay_info(ovl, &info);
+        if (info.enabled == enable)
+                return 0;
         info.enabled = enable;
         return ovl->set_overlay_info(ovl, &info);
 }
 
+static inline struct omapfb2_mem_region *
+omapfb_get_mem_region(struct omapfb2_mem_region *rg)
+{
+        down_read_nested(&rg->lock, rg->id);
+        atomic_inc(&rg->lock_count);
+        return rg;
+}
+
+static inline void omapfb_put_mem_region(struct omapfb2_mem_region *rg)
+{
+        atomic_dec(&rg->lock_count);
+        up_read(&rg->lock);
+}
+
 #endif
diff --git a/drivers/video/omap2/vram.c b/drivers/video/omap2/vram.c
index 3b1237ad85ed..fed2a72bc6b6 100644
--- a/drivers/video/omap2/vram.c
+++ b/drivers/video/omap2/vram.c
@@ -25,7 +25,7 @@
 #include <linux/list.h>
 #include <linux/slab.h>
 #include <linux/seq_file.h>
-#include <linux/bootmem.h>
+#include <linux/memblock.h>
 #include <linux/completion.h>
 #include <linux/debugfs.h>
 #include <linux/jiffies.h>
@@ -525,10 +525,8 @@ early_param("vram", omap_vram_early_vram);
  * Called from map_io. We need to call to this early enough so that we
  * can reserve the fixed SDRAM regions before VM could get hold of them.
  */
-void __init omap_vram_reserve_sdram(void)
+void __init omap_vram_reserve_sdram_memblock(void)
 {
-        struct bootmem_data     *bdata;
-        unsigned long           sdram_start, sdram_size;
         u32 paddr;
         u32 size = 0;
 
@@ -555,29 +553,24 @@ void __init omap_vram_reserve_sdram(void)
 
         size = PAGE_ALIGN(size);
 
-        bdata = NODE_DATA(0)->bdata;
-        sdram_start = bdata->node_min_pfn << PAGE_SHIFT;
-        sdram_size = (bdata->node_low_pfn << PAGE_SHIFT) - sdram_start;
-
         if (paddr) {
-                if ((paddr & ~PAGE_MASK) || paddr < sdram_start ||
-                                paddr + size > sdram_start + sdram_size) {
+                if ((paddr & ~PAGE_MASK) ||
+                    !memblock_is_region_memory(paddr, size)) {
                         pr_err("Illegal SDRAM region for VRAM\n");
                         return;
                 }
 
-                if (reserve_bootmem(paddr, size, BOOTMEM_EXCLUSIVE) < 0) {
-                        pr_err("FB: failed to reserve VRAM\n");
+                if (memblock_is_region_reserved(paddr, size)) {
+                        pr_err("FB: failed to reserve VRAM - busy\n");
                         return;
                 }
-        } else {
-                if (size > sdram_size) {
-                        pr_err("Illegal SDRAM size for VRAM\n");
+
+                if (memblock_reserve(paddr, size) < 0) {
+                        pr_err("FB: failed to reserve VRAM - no memory\n");
                         return;
                 }
-
-                paddr = virt_to_phys(alloc_bootmem_pages(size));
-                BUG_ON(paddr & ~PAGE_MASK);
+        } else {
+                paddr = memblock_alloc_base(size, PAGE_SIZE, MEMBLOCK_REAL_LIMIT);
         }
 
         omap_vram_add_region(paddr, size);
diff --git a/drivers/video/p9100.c b/drivers/video/p9100.c
index 6552751e81aa..b6c3fc2db632 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 of_device *op, const struct of_device_id *match)
+static int __devinit p9100_probe(struct platform_device *op, const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -326,7 +326,7 @@ out_err:
         return err;
 }
 
-static int __devexit p9100_remove(struct of_device *op)
+static int __devexit p9100_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct p9100_par *par = info->par;
@@ -367,12 +367,12 @@ static int __init p9100_init(void)
         if (fb_get_options("p9100fb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&p9100_driver, &of_bus_type);
+        return of_register_platform_driver(&p9100_driver);
 }
 
 static void __exit p9100_exit(void)
 {
-        of_unregister_driver(&p9100_driver);
+        of_unregister_platform_driver(&p9100_driver);
 }
 
 module_init(p9100_init);
diff --git a/drivers/video/platinumfb.c b/drivers/video/platinumfb.c
index 72a1f4c04732..a50e1977b316 100644
--- a/drivers/video/platinumfb.c
+++ b/drivers/video/platinumfb.c
@@ -533,7 +533,7 @@ static int __init platinumfb_setup(char *options)
 #define invalidate_cache(addr)
 #endif
 
-static int __devinit platinumfb_probe(struct of_device* odev,
+static int __devinit platinumfb_probe(struct platform_device* odev,
                                       const struct of_device_id *match)
 {
         struct device_node      *dp = odev->dev.of_node;
@@ -646,7 +646,7 @@ static int __devinit platinumfb_probe(struct of_device* odev,
         return rc;
 }
 
-static int __devexit platinumfb_remove(struct of_device* odev)
+static int __devexit platinumfb_remove(struct platform_device* odev)
 {
         struct fb_info          *info = dev_get_drvdata(&odev->dev);
         struct fb_info_platinum *pinfo = info->par;
diff --git a/drivers/video/pmag-ba-fb.c b/drivers/video/pmag-ba-fb.c
index 0f361b6100d2..0c69fa20251b 100644
--- a/drivers/video/pmag-ba-fb.c
+++ b/drivers/video/pmag-ba-fb.c
@@ -44,7 +44,7 @@ struct pmagbafb_par {
 };
 
 
-static struct fb_var_screeninfo pmagbafb_defined __initdata = {
+static struct fb_var_screeninfo pmagbafb_defined __devinitdata = {
         .xres           = 1024,
         .yres           = 864,
         .xres_virtual   = 1024,
@@ -68,7 +68,7 @@ static struct fb_var_screeninfo pmagbafb_defined __initdata = {
         .vmode          = FB_VMODE_NONINTERLACED,
 };
 
-static struct fb_fix_screeninfo pmagbafb_fix __initdata = {
+static struct fb_fix_screeninfo pmagbafb_fix __devinitdata = {
         .id             = "PMAG-BA",
         .smem_len       = (1024 * 1024),
         .type           = FB_TYPE_PACKED_PIXELS,
@@ -142,7 +142,7 @@ static void __init pmagbafb_erase_cursor(struct fb_info *info)
 }
 
 
-static int __init pmagbafb_probe(struct device *dev)
+static int __devinit pmagbafb_probe(struct device *dev)
 {
         struct tc_dev *tdev = to_tc_dev(dev);
         resource_size_t start, len;
diff --git a/drivers/video/pmagb-b-fb.c b/drivers/video/pmagb-b-fb.c
index 2de0806421b4..22fcb9a3d5c0 100644
--- a/drivers/video/pmagb-b-fb.c
+++ b/drivers/video/pmagb-b-fb.c
@@ -45,7 +45,7 @@ struct pmagbbfb_par {
 };
 
 
-static struct fb_var_screeninfo pmagbbfb_defined __initdata = {
+static struct fb_var_screeninfo pmagbbfb_defined __devinitdata = {
         .bits_per_pixel = 8,
         .red.length     = 8,
         .green.length   = 8,
@@ -58,7 +58,7 @@ static struct fb_var_screeninfo pmagbbfb_defined __initdata = {
         .vmode          = FB_VMODE_NONINTERLACED,
 };
 
-static struct fb_fix_screeninfo pmagbbfb_fix __initdata = {
+static struct fb_fix_screeninfo pmagbbfb_fix __devinitdata = {
         .id             = "PMAGB-BA",
         .smem_len       = (2048 * 1024),
         .type           = FB_TYPE_PACKED_PIXELS,
@@ -148,7 +148,7 @@ static void __init pmagbbfb_erase_cursor(struct fb_info *info)
 /*
  * Set up screen parameters.
  */
-static void __init pmagbbfb_screen_setup(struct fb_info *info)
+static void __devinit pmagbbfb_screen_setup(struct fb_info *info)
 {
         struct pmagbbfb_par *par = info->par;
 
@@ -180,9 +180,9 @@ static void __init pmagbbfb_screen_setup(struct fb_info *info)
 /*
  * Determine oscillator configuration.
  */
-static void __init pmagbbfb_osc_setup(struct fb_info *info)
+static void __devinit pmagbbfb_osc_setup(struct fb_info *info)
 {
-        static unsigned int pmagbbfb_freqs[] __initdata = {
+        static unsigned int pmagbbfb_freqs[] __devinitdata = {
                 130808, 119843, 104000, 92980, 74370, 72800,
                 69197, 66000, 65000, 50350, 36000, 32000, 25175
         };
@@ -247,7 +247,7 @@ static void __init pmagbbfb_osc_setup(struct fb_info *info)
 };
 
 
-static int __init pmagbbfb_probe(struct device *dev)
+static int __devinit pmagbbfb_probe(struct device *dev)
 {
         struct tc_dev *tdev = to_tc_dev(dev);
         resource_size_t start, len;
diff --git a/drivers/video/pxa168fb.c b/drivers/video/pxa168fb.c
index c91a7f70f7b0..cea6403ae71c 100644
--- a/drivers/video/pxa168fb.c
+++ b/drivers/video/pxa168fb.c
@@ -298,8 +298,8 @@ static void set_dma_control0(struct pxa168fb_info *fbi)
          * Set bit to enable graphics DMA.
          */
         x = readl(fbi->reg_base + LCD_SPU_DMA_CTRL0);
-        x |= fbi->active ? 0x00000100 : 0;
-        fbi->active = 0;
+        x &= ~CFG_GRA_ENA_MASK;
+        x |= fbi->active ? CFG_GRA_ENA(1) : CFG_GRA_ENA(0);
 
         /*
          * If we are in a pseudo-color mode, we need to enable
@@ -559,7 +559,7 @@ static struct fb_ops pxa168fb_ops = {
         .fb_imageblit   = cfb_imageblit,
 };
 
-static int __init pxa168fb_init_mode(struct fb_info *info,
+static int __devinit pxa168fb_init_mode(struct fb_info *info,
                               struct pxa168fb_mach_info *mi)
 {
         struct pxa168fb_info *fbi = info->par;
@@ -599,7 +599,7 @@ static int __init pxa168fb_init_mode(struct fb_info *info,
         return ret;
 }
 
-static int __init pxa168fb_probe(struct platform_device *pdev)
+static int __devinit pxa168fb_probe(struct platform_device *pdev)
 {
         struct pxa168fb_mach_info *mi;
         struct fb_info *info = 0;
@@ -784,20 +784,67 @@ failed:
         return ret;
 }
 
+static int __devexit pxa168fb_remove(struct platform_device *pdev)
+{
+        struct pxa168fb_info *fbi = platform_get_drvdata(pdev);
+        struct fb_info *info;
+        int irq;
+        unsigned int data;
+
+        if (!fbi)
+                return 0;
+
+        /* disable DMA transfer */
+        data = readl(fbi->reg_base + LCD_SPU_DMA_CTRL0);
+        data &= ~CFG_GRA_ENA_MASK;
+        writel(data, fbi->reg_base + LCD_SPU_DMA_CTRL0);
+
+        info = fbi->info;
+
+        unregister_framebuffer(info);
+
+        writel(GRA_FRAME_IRQ0_ENA(0x0), fbi->reg_base + SPU_IRQ_ENA);
+
+        if (info->cmap.len)
+                fb_dealloc_cmap(&info->cmap);
+
+        irq = platform_get_irq(pdev, 0);
+        free_irq(irq, fbi);
+
+        dma_free_writecombine(fbi->dev, PAGE_ALIGN(info->fix.smem_len),
+                                info->screen_base, info->fix.smem_start);
+
+        iounmap(fbi->reg_base);
+
+        clk_disable(fbi->clk);
+        clk_put(fbi->clk);
+
+        framebuffer_release(info);
+
+        return 0;
+}
+
 static struct platform_driver pxa168fb_driver = {
         .driver         = {
                 .name   = "pxa168-fb",
                 .owner  = THIS_MODULE,
         },
         .probe          = pxa168fb_probe,
+        .remove         = __devexit_p(pxa168fb_remove),
 };
 
-static int __devinit pxa168fb_init(void)
+static int __init pxa168fb_init(void)
 {
         return platform_driver_register(&pxa168fb_driver);
 }
 module_init(pxa168fb_init);
 
+static void __exit pxa168fb_exit(void)
+{
+        platform_driver_unregister(&pxa168fb_driver);
+}
+module_exit(pxa168fb_exit);
+
 MODULE_AUTHOR("Lennert Buytenhek <buytenh@marvell.com> "
               "Green Wan <gwan@marvell.com>");
 MODULE_DESCRIPTION("Framebuffer driver for PXA168/910");
diff --git a/drivers/video/q40fb.c b/drivers/video/q40fb.c
index fc32c323a381..f5a39f5aa900 100644
--- a/drivers/video/q40fb.c
+++ b/drivers/video/q40fb.c
@@ -28,7 +28,7 @@
 
 #define Q40_PHYS_SCREEN_ADDR 0xFE800000
 
-static struct fb_fix_screeninfo q40fb_fix __initdata = {
+static struct fb_fix_screeninfo q40fb_fix __devinitdata = {
         .id             = "Q40",
         .smem_len       = 1024*1024,
         .type           = FB_TYPE_PACKED_PIXELS,
@@ -37,7 +37,7 @@ static struct fb_fix_screeninfo q40fb_fix __initdata = {
         .accel          = FB_ACCEL_NONE,
 };
 
-static struct fb_var_screeninfo q40fb_var __initdata = {
+static struct fb_var_screeninfo q40fb_var __devinitdata = {
         .xres           = 1024,
         .yres           = 512,
         .xres_virtual   = 1024,
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index 9682ecc60e12..f9aca9d13d1b 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -1,7 +1,7 @@
 /* linux/drivers/video/s3c-fb.c
  *
  * Copyright 2008 Openmoko Inc.
- * Copyright 2008 Simtec Electronics
+ * Copyright 2008-2010 Simtec Electronics
  *      Ben Dooks <ben@simtec.co.uk>
  *      http://armlinux.simtec.co.uk/
  *
@@ -9,7 +9,7 @@
  *
  * 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.
+ * published by the Free Software FoundatIon.
 */
 
 #include <linux/kernel.h>
@@ -21,9 +21,11 @@
 #include <linux/clk.h>
 #include <linux/fb.h>
 #include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/interrupt.h>
 
 #include <mach/map.h>
-#include <mach/regs-fb.h>
+#include <plat/regs-fb-v4.h>
 #include <plat/fb.h>
 
 /* This driver will export a number of framebuffer interfaces depending
@@ -36,9 +38,9 @@
  * output timings and as the control for the output power-down state.
 */
 
-/* note, some of the functions that get called are derived from including
- * <mach/regs-fb.h> as they are specific to the architecture that the code
- * is being built for.
+/* note, the previous use of <mach/regs-fb.h> to get platform specific data
+ * has been replaced by using the platform device name to pick the correct
+ * configuration data for the system.
 */
 
 #ifdef CONFIG_FB_S3C_DEBUG_REGWRITE
@@ -48,13 +50,108 @@
         __raw_writel(v, r); } while(0)
 #endif /* FB_S3C_DEBUG_REGWRITE */
 
+/* irq_flags bits */
+#define S3C_FB_VSYNC_IRQ_EN     0
+
+#define VSYNC_TIMEOUT_MSEC 50
+
 struct s3c_fb;
 
+#define VALID_BPP(x) (1 << ((x) - 1))
+
+#define OSD_BASE(win, variant) ((variant).osd + ((win) * (variant).osd_stride))
+#define VIDOSD_A(win, variant) (OSD_BASE(win, variant) + 0x00)
+#define VIDOSD_B(win, variant) (OSD_BASE(win, variant) + 0x04)
+#define VIDOSD_C(win, variant) (OSD_BASE(win, variant) + 0x08)
+#define VIDOSD_D(win, variant) (OSD_BASE(win, variant) + 0x0C)
+
+/**
+ * struct s3c_fb_variant - fb variant information
+ * @is_2443: Set if S3C2443/S3C2416 style hardware.
+ * @nr_windows: The number of windows.
+ * @vidtcon: The base for the VIDTCONx registers
+ * @wincon: The base for the WINxCON registers.
+ * @winmap: The base for the WINxMAP registers.
+ * @keycon: The abse for the WxKEYCON registers.
+ * @buf_start: Offset of buffer start registers.
+ * @buf_size: Offset of buffer size registers.
+ * @buf_end: Offset of buffer end registers.
+ * @osd: The base for the OSD registers.
+ * @palette: Address of palette memory, or 0 if none.
+ * @has_prtcon: Set if has PRTCON register.
+ * @has_shadowcon: Set if has SHADOWCON register.
+ */
+struct s3c_fb_variant {
+        unsigned int    is_2443:1;
+        unsigned short  nr_windows;
+        unsigned short  vidtcon;
+        unsigned short  wincon;
+        unsigned short  winmap;
+        unsigned short  keycon;
+        unsigned short  buf_start;
+        unsigned short  buf_end;
+        unsigned short  buf_size;
+        unsigned short  osd;
+        unsigned short  osd_stride;
+        unsigned short  palette[S3C_FB_MAX_WIN];
+
+        unsigned int    has_prtcon:1;
+        unsigned int    has_shadowcon:1;
+};
+
+/**
+ * struct s3c_fb_win_variant
+ * @has_osd_c: Set if has OSD C register.
+ * @has_osd_d: Set if has OSD D register.
+ * @has_osd_alpha: Set if can change alpha transparency for a window.
+ * @palette_sz: Size of palette in entries.
+ * @palette_16bpp: Set if palette is 16bits wide.
+ * @osd_size_off: If != 0, supports setting up OSD for a window; the appropriate
+ *                register is located at the given offset from OSD_BASE.
+ * @valid_bpp: 1 bit per BPP setting to show valid bits-per-pixel.
+ *
+ * valid_bpp bit x is set if (x+1)BPP is supported.
+ */
+struct s3c_fb_win_variant {
+        unsigned int    has_osd_c:1;
+        unsigned int    has_osd_d:1;
+        unsigned int    has_osd_alpha:1;
+        unsigned int    palette_16bpp:1;
+        unsigned short  osd_size_off;
+        unsigned short  palette_sz;
+        u32             valid_bpp;
+};
+
+/**
+ * struct s3c_fb_driverdata - per-device type driver data for init time.
+ * @variant: The variant information for this driver.
+ * @win: The window information for each window.
+ */
+struct s3c_fb_driverdata {
+        struct s3c_fb_variant   variant;
+        struct s3c_fb_win_variant *win[S3C_FB_MAX_WIN];
+};
+
+/**
+ * struct s3c_fb_palette - palette information
+ * @r: Red bitfield.
+ * @g: Green bitfield.
+ * @b: Blue bitfield.
+ * @a: Alpha bitfield.
+ */
+struct s3c_fb_palette {
+        struct fb_bitfield      r;
+        struct fb_bitfield      g;
+        struct fb_bitfield      b;
+        struct fb_bitfield      a;
+};
+
 /**
  * struct s3c_fb_win - per window private data for each framebuffer.
  * @windata: The platform data supplied for the window configuration.
  * @parent: The hardware that this window is part of.
  * @fbinfo: Pointer pack to the framebuffer info for this window.
+ * @varint: The variant information for this window.
  * @palette_buffer: Buffer/cache to hold palette entries.
  * @pseudo_palette: For use in TRUECOLOUR modes for entries 0..15/
  * @index: The window number of this window.
@@ -65,6 +162,7 @@ struct s3c_fb_win {
         struct s3c_fb           *parent;
         struct fb_info          *fbinfo;
         struct s3c_fb_palette    palette;
+        struct s3c_fb_win_variant variant;
 
         u32                     *palette_buffer;
         u32                      pseudo_palette[16];
@@ -72,37 +170,54 @@ struct s3c_fb_win {
 };
 
 /**
+ * struct s3c_fb_vsync - vsync information
+ * @wait:       a queue for processes waiting for vsync
+ * @count:      vsync interrupt count
+ */
+struct s3c_fb_vsync {
+        wait_queue_head_t       wait;
+        unsigned int            count;
+};
+
+/**
  * struct s3c_fb - overall hardware state of the hardware
  * @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.
  * @regs: The mapped hardware registers.
+ * @variant: Variant information for this hardware.
  * @enabled: A bitmask of enabled hardware windows.
  * @pdata: The platform configuration data passed with the device.
  * @windows: The hardware windows that have been claimed.
+ * @irq_no: IRQ line number
+ * @irq_flags: irq flags
+ * @vsync_info: VSYNC-related information (count, queues...)
  */
 struct s3c_fb {
         struct device           *dev;
         struct resource         *regs_res;
         struct clk              *bus_clk;
         void __iomem            *regs;
+        struct s3c_fb_variant    variant;
 
         unsigned char            enabled;
 
         struct s3c_fb_platdata  *pdata;
         struct s3c_fb_win       *windows[S3C_FB_MAX_WIN];
+
+        int                      irq_no;
+        unsigned long            irq_flags;
+        struct s3c_fb_vsync      vsync_info;
 };
 
 /**
- * s3c_fb_win_has_palette() - determine if a mode has a palette
- * @win: The window number being queried.
- * @bpp: The number of bits per pixel to test.
- *
- * Work out if the given window supports palletised data at the specified bpp.
+ * s3c_fb_validate_win_bpp - validate the bits-per-pixel for this mode.
+ * @win: The device window.
+ * @bpp: The bit depth.
  */
-static int s3c_fb_win_has_palette(unsigned int win, unsigned int bpp)
+static bool s3c_fb_validate_win_bpp(struct s3c_fb_win *win, unsigned int bpp)
 {
-        return s3c_fb_win_pal_size(win) <= (1 << bpp);
+        return win->variant.valid_bpp & VALID_BPP(bpp);
 }
 
 /**
@@ -125,7 +240,7 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
         var->xres_virtual = max((unsigned int)windata->virtual_x, var->xres);
         var->yres_virtual = max((unsigned int)windata->virtual_y, var->yres);
 
-        if (!s3c_fb_validate_win_bpp(win->index, var->bits_per_pixel)) {
+        if (!s3c_fb_validate_win_bpp(win, var->bits_per_pixel)) {
                 dev_dbg(sfb->dev, "win %d: unsupported bpp %d\n",
                         win->index, var->bits_per_pixel);
                 return -EINVAL;
@@ -140,7 +255,7 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
         case 2:
         case 4:
         case 8:
-                if (!s3c_fb_win_has_palette(win->index, var->bits_per_pixel)) {
+                if (sfb->variant.palette[win->index] != 0) {
                         /* non palletised, A:1,R:2,G:3,B:2 mode */
                         var->red.offset         = 4;
                         var->green.offset       = 2;
@@ -255,6 +370,66 @@ static int s3c_fb_align_word(unsigned int bpp, unsigned int pix)
 }
 
 /**
+ * vidosd_set_size() - set OSD size for a window
+ *
+ * @win: the window to set OSD size for
+ * @size: OSD size register value
+ */
+static void vidosd_set_size(struct s3c_fb_win *win, u32 size)
+{
+        struct s3c_fb *sfb = win->parent;
+
+        /* OSD can be set up if osd_size_off != 0 for this window */
+        if (win->variant.osd_size_off)
+                writel(size, sfb->regs + OSD_BASE(win->index, sfb->variant)
+                                + win->variant.osd_size_off);
+}
+
+/**
+ * vidosd_set_alpha() - set alpha transparency for a window
+ *
+ * @win: the window to set OSD size for
+ * @alpha: alpha register value
+ */
+static void vidosd_set_alpha(struct s3c_fb_win *win, u32 alpha)
+{
+        struct s3c_fb *sfb = win->parent;
+
+        if (win->variant.has_osd_alpha)
+                writel(alpha, sfb->regs + VIDOSD_C(win->index, sfb->variant));
+}
+
+/**
+ * shadow_protect_win() - disable updating values from shadow registers at vsync
+ *
+ * @win: window to protect registers for
+ * @protect: 1 to protect (disable updates)
+ */
+static void shadow_protect_win(struct s3c_fb_win *win, bool protect)
+{
+        struct s3c_fb *sfb = win->parent;
+        u32 reg;
+
+        if (protect) {
+                if (sfb->variant.has_prtcon) {
+                        writel(PRTCON_PROTECT, sfb->regs + PRTCON);
+                } else if (sfb->variant.has_shadowcon) {
+                        reg = readl(sfb->regs + SHADOWCON);
+                        writel(reg | SHADOWCON_WINx_PROTECT(win->index),
+                                sfb->regs + SHADOWCON);
+                }
+        } else {
+                if (sfb->variant.has_prtcon) {
+                        writel(0, sfb->regs + PRTCON);
+                } else if (sfb->variant.has_shadowcon) {
+                        reg = readl(sfb->regs + SHADOWCON);
+                        writel(reg & ~SHADOWCON_WINx_PROTECT(win->index),
+                                sfb->regs + SHADOWCON);
+                }
+        }
+}
+
+/**
  * s3c_fb_set_par() - framebuffer request to set new framebuffer state.
  * @info: The framebuffer to change.
  *
@@ -266,14 +441,17 @@ static int s3c_fb_set_par(struct fb_info *info)
         struct s3c_fb_win *win = info->par;
         struct s3c_fb *sfb = win->parent;
         void __iomem *regs = sfb->regs;
+        void __iomem *buf = regs;
         int win_no = win->index;
-        u32 osdc_data = 0;
+        u32 alpha = 0;
         u32 data;
         u32 pagewidth;
         int clkdiv;
 
         dev_dbg(sfb->dev, "setting framebuffer parameters\n");
 
+        shadow_protect_win(win, 1);
+
         switch (var->bits_per_pixel) {
         case 32:
         case 24:
@@ -282,7 +460,7 @@ static int s3c_fb_set_par(struct fb_info *info)
                 info->fix.visual = FB_VISUAL_TRUECOLOR;
                 break;
         case 8:
-                if (s3c_fb_win_has_palette(win_no, 8))
+                if (win->variant.palette_sz >= 256)
                         info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
                 else
                         info->fix.visual = FB_VISUAL_TRUECOLOR;
@@ -297,12 +475,15 @@ static int s3c_fb_set_par(struct fb_info *info)
 
         info->fix.line_length = (var->xres_virtual * var->bits_per_pixel) / 8;
 
+        info->fix.xpanstep = info->var.xres_virtual > info->var.xres ? 1 : 0;
+        info->fix.ypanstep = info->var.yres_virtual > info->var.yres ? 1 : 0;
+
         /* disable the window whilst we update it */
         writel(0, regs + WINCON(win_no));
 
-        /* use window 0 as the basis for the lcd output timings */
+        /* use platform specified window as the basis for the lcd timings */
 
-        if (win_no == 0) {
+        if (win_no == sfb->pdata->default_win) {
                 clkdiv = s3c_fb_calc_pixclk(sfb, var->pixclock);
 
                 data = sfb->pdata->vidcon0;
@@ -315,6 +496,9 @@ static int s3c_fb_set_par(struct fb_info *info)
 
                 /* write the timing data to the panel */
 
+                if (sfb->variant.is_2443)
+                        data |= (1 << 5);
+
                 data |= VIDCON0_ENVID | VIDCON0_ENVID_F;
                 writel(data, regs + VIDCON0);
 
@@ -322,53 +506,54 @@ static int s3c_fb_set_par(struct fb_info *info)
                        VIDTCON0_VFPD(var->lower_margin - 1) |
                        VIDTCON0_VSPW(var->vsync_len - 1);
 
-                writel(data, regs + VIDTCON0);
+                writel(data, regs + sfb->variant.vidtcon);
 
                 data = VIDTCON1_HBPD(var->left_margin - 1) |
                        VIDTCON1_HFPD(var->right_margin - 1) |
                        VIDTCON1_HSPW(var->hsync_len - 1);
 
-                writel(data, regs + VIDTCON1);
+                /* VIDTCON1 */
+                writel(data, regs + sfb->variant.vidtcon + 4);
 
                 data = VIDTCON2_LINEVAL(var->yres - 1) |
                        VIDTCON2_HOZVAL(var->xres - 1);
-                writel(data, regs + VIDTCON2);
+                writel(data, regs +sfb->variant.vidtcon + 8 );
         }
 
         /* write the buffer address */
 
-        writel(info->fix.smem_start, regs + VIDW_BUF_START(win_no));
+        /* start and end registers stride is 8 */
+        buf = regs + win_no * 8;
+
+        writel(info->fix.smem_start, buf + sfb->variant.buf_start);
 
         data = info->fix.smem_start + info->fix.line_length * var->yres;
-        writel(data, regs + VIDW_BUF_END(win_no));
+        writel(data, buf + sfb->variant.buf_end);
 
         pagewidth = (var->xres * var->bits_per_pixel) >> 3;
         data = VIDW_BUF_SIZE_OFFSET(info->fix.line_length - pagewidth) |
                VIDW_BUF_SIZE_PAGEWIDTH(pagewidth);
-        writel(data, regs + VIDW_BUF_SIZE(win_no));
+        writel(data, regs + sfb->variant.buf_size + (win_no * 4));
 
         /* write 'OSD' registers to control position of framebuffer */
 
         data = VIDOSDxA_TOPLEFT_X(0) | VIDOSDxA_TOPLEFT_Y(0);
-        writel(data, regs + VIDOSD_A(win_no));
+        writel(data, regs + VIDOSD_A(win_no, sfb->variant));
 
         data = VIDOSDxB_BOTRIGHT_X(s3c_fb_align_word(var->bits_per_pixel,
                                                      var->xres - 1)) |
                VIDOSDxB_BOTRIGHT_Y(var->yres - 1);
 
-        writel(data, regs + VIDOSD_B(win_no));
+        writel(data, regs + VIDOSD_B(win_no, sfb->variant));
 
         data = var->xres * var->yres;
 
-        osdc_data = VIDISD14C_ALPHA1_R(0xf) |
+        alpha = VIDISD14C_ALPHA1_R(0xf) |
                 VIDISD14C_ALPHA1_G(0xf) |
                 VIDISD14C_ALPHA1_B(0xf);
 
-        if (s3c_fb_has_osd_d(win_no)) {
-                writel(data, regs + VIDOSD_D(win_no));
-                writel(osdc_data, regs + VIDOSD_C(win_no));
-        } else
-                writel(data, regs + VIDOSD_C(win_no));
+        vidosd_set_alpha(win, alpha);
+        vidosd_set_size(win, data);
 
         data = WINCONx_ENWIN;
 
@@ -424,13 +609,15 @@ static int s3c_fb_set_par(struct fb_info *info)
                 else
                         data |= WINCON0_BPPMODE_24BPP_888;
 
+                data |= WINCONx_WSWP;
                 data |= WINCONx_BURSTLEN_16WORD;
                 break;
         }
 
-        /* It has no color key control register for window0 */
+        /* Enable the colour keying for the window below this one */
         if (win_no > 0) {
                 u32 keycon0_data = 0, keycon1_data = 0;
+                void __iomem *keycon = regs + sfb->variant.keycon;
 
                 keycon0_data = ~(WxKEYCON0_KEYBL_EN |
                                 WxKEYCON0_KEYEN_F |
@@ -438,12 +625,23 @@ static int s3c_fb_set_par(struct fb_info *info)
 
                 keycon1_data = WxKEYCON1_COLVAL(0xffffff);
 
-                writel(keycon0_data, regs + WxKEYCONy(win_no-1, 0));
-                writel(keycon1_data, regs + WxKEYCONy(win_no-1, 1));
+                keycon += (win_no - 1) * 8;
+
+                writel(keycon0_data, keycon + WKEYCON0);
+                writel(keycon1_data, keycon + WKEYCON1);
+        }
+
+        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);
         }
 
-        writel(data, regs + WINCON(win_no));
-        writel(0x0, regs + WINxMAP(win_no));
+        shadow_protect_win(win, 0);
 
         return 0;
 }
@@ -470,7 +668,7 @@ static void s3c_fb_update_palette(struct s3c_fb *sfb,
         void __iomem *palreg;
         u32 palcon;
 
-        palreg = sfb->regs + s3c_fb_pal_reg(win->index, reg);
+        palreg = sfb->regs + sfb->variant.palette[win->index];
 
         dev_dbg(sfb->dev, "%s: win %d, reg %d (%p): %08x\n",
                 __func__, win->index, reg, palreg, value);
@@ -480,10 +678,10 @@ static void s3c_fb_update_palette(struct s3c_fb *sfb,
         palcon = readl(sfb->regs + WPALCON);
         writel(palcon | WPALCON_PAL_UPDATE, sfb->regs + WPALCON);
 
-        if (s3c_fb_pal_is16(win->index))
-                writew(value, palreg);
+        if (win->variant.palette_16bpp)
+                writew(value, palreg + (reg * 2));
         else
-                writel(value, palreg);
+                writel(value, palreg + (reg * 4));
 
         writel(palcon, sfb->regs + WPALCON);
 }
@@ -532,7 +730,7 @@ static int s3c_fb_setcolreg(unsigned regno,
                 break;
 
         case FB_VISUAL_PSEUDOCOLOR:
-                if (regno < s3c_fb_win_pal_size(win->index)) {
+                if (regno < win->variant.palette_sz) {
                         val  = chan_to_field(red, &win->palette.r);
                         val |= chan_to_field(green, &win->palette.g);
                         val |= chan_to_field(blue, &win->palette.b);
@@ -591,7 +789,7 @@ static int s3c_fb_blank(int blank_mode, struct fb_info *info)
 
         dev_dbg(sfb->dev, "blank mode %d\n", blank_mode);
 
-        wincon = readl(sfb->regs + WINCON(index));
+        wincon = readl(sfb->regs + sfb->variant.wincon + (index * 4));
 
         switch (blank_mode) {
         case FB_BLANK_POWERDOWN:
@@ -602,11 +800,11 @@ static int s3c_fb_blank(int blank_mode, struct fb_info *info)
         case FB_BLANK_NORMAL:
                 /* disable the DMA and display 0x0 (black) */
                 writel(WINxMAP_MAP | WINxMAP_MAP_COLOUR(0x0),
-                       sfb->regs + WINxMAP(index));
+                       sfb->regs + sfb->variant.winmap + (index * 4));
                 break;
 
         case FB_BLANK_UNBLANK:
-                writel(0x0, sfb->regs + WINxMAP(index));
+                writel(0x0, sfb->regs + sfb->variant.winmap + (index * 4));
                 wincon |= WINCONx_ENWIN;
                 sfb->enabled |= (1 << index);
                 break;
@@ -617,7 +815,7 @@ static int s3c_fb_blank(int blank_mode, struct fb_info *info)
                 return 1;
         }
 
-        writel(wincon, sfb->regs + WINCON(index));
+        writel(wincon, sfb->regs + sfb->variant.wincon + (index * 4));
 
         /* Check the enabled state to see if we need to be running the
          * main LCD interface, as if there are no active windows then
@@ -636,12 +834,185 @@ static int s3c_fb_blank(int blank_mode, struct fb_info *info)
         /* we're stuck with this until we can do something about overriding
          * the power control using the blanking event for a single fb.
          */
-        if (index == 0)
+        if (index == sfb->pdata->default_win)
                 s3c_fb_enable(sfb, blank_mode != FB_BLANK_POWERDOWN ? 1 : 0);
 
         return 0;
 }
 
+/**
+ * s3c_fb_pan_display() - Pan the display.
+ *
+ * Note that the offsets can be written to the device at any time, as their
+ * values are latched at each vsync automatically. This also means that only
+ * the last call to this function will have any effect on next vsync, but
+ * there is no need to sleep waiting for it to prevent tearing.
+ *
+ * @var: The screen information to verify.
+ * @info: The framebuffer device.
+ */
+static int s3c_fb_pan_display(struct fb_var_screeninfo *var,
+                              struct fb_info *info)
+{
+        struct s3c_fb_win *win  = info->par;
+        struct s3c_fb *sfb      = win->parent;
+        void __iomem *buf       = sfb->regs + win->index * 8;
+        unsigned int start_boff, end_boff;
+
+        /* Offset in bytes to the start of the displayed area */
+        start_boff = var->yoffset * info->fix.line_length;
+        /* X offset depends on the current bpp */
+        if (info->var.bits_per_pixel >= 8) {
+                start_boff += var->xoffset * (info->var.bits_per_pixel >> 3);
+        } else {
+                switch (info->var.bits_per_pixel) {
+                case 4:
+                        start_boff += var->xoffset >> 1;
+                        break;
+                case 2:
+                        start_boff += var->xoffset >> 2;
+                        break;
+                case 1:
+                        start_boff += var->xoffset >> 3;
+                        break;
+                default:
+                        dev_err(sfb->dev, "invalid bpp\n");
+                        return -EINVAL;
+                }
+        }
+        /* Offset in bytes to the end of the displayed area */
+        end_boff = start_boff + var->yres * info->fix.line_length;
+
+        /* Temporarily turn off per-vsync update from shadow registers until
+         * both start and end addresses are updated to prevent corruption */
+        shadow_protect_win(win, 1);
+
+        writel(info->fix.smem_start + start_boff, buf + sfb->variant.buf_start);
+        writel(info->fix.smem_start + end_boff, buf + sfb->variant.buf_end);
+
+        shadow_protect_win(win, 0);
+
+        return 0;
+}
+
+/**
+ * s3c_fb_enable_irq() - enable framebuffer interrupts
+ * @sfb: main hardware state
+ */
+static void s3c_fb_enable_irq(struct s3c_fb *sfb)
+{
+        void __iomem *regs = sfb->regs;
+        u32 irq_ctrl_reg;
+
+        if (!test_and_set_bit(S3C_FB_VSYNC_IRQ_EN, &sfb->irq_flags)) {
+                /* IRQ disabled, enable it */
+                irq_ctrl_reg = readl(regs + VIDINTCON0);
+
+                irq_ctrl_reg |= VIDINTCON0_INT_ENABLE;
+                irq_ctrl_reg |= VIDINTCON0_INT_FRAME;
+
+                irq_ctrl_reg &= ~VIDINTCON0_FRAMESEL0_MASK;
+                irq_ctrl_reg |= VIDINTCON0_FRAMESEL0_VSYNC;
+                irq_ctrl_reg &= ~VIDINTCON0_FRAMESEL1_MASK;
+                irq_ctrl_reg |= VIDINTCON0_FRAMESEL1_NONE;
+
+                writel(irq_ctrl_reg, regs + VIDINTCON0);
+        }
+}
+
+/**
+ * s3c_fb_disable_irq() - disable framebuffer interrupts
+ * @sfb: main hardware state
+ */
+static void s3c_fb_disable_irq(struct s3c_fb *sfb)
+{
+        void __iomem *regs = sfb->regs;
+        u32 irq_ctrl_reg;
+
+        if (test_and_clear_bit(S3C_FB_VSYNC_IRQ_EN, &sfb->irq_flags)) {
+                /* IRQ enabled, disable it */
+                irq_ctrl_reg = readl(regs + VIDINTCON0);
+
+                irq_ctrl_reg &= ~VIDINTCON0_INT_FRAME;
+                irq_ctrl_reg &= ~VIDINTCON0_INT_ENABLE;
+
+                writel(irq_ctrl_reg, regs + VIDINTCON0);
+        }
+}
+
+static irqreturn_t s3c_fb_irq(int irq, void *dev_id)
+{
+        struct s3c_fb *sfb = dev_id;
+        void __iomem  *regs = sfb->regs;
+        u32 irq_sts_reg;
+
+        irq_sts_reg = readl(regs + VIDINTCON1);
+
+        if (irq_sts_reg & VIDINTCON1_INT_FRAME) {
+
+                /* VSYNC interrupt, accept it */
+                writel(VIDINTCON1_INT_FRAME, regs + VIDINTCON1);
+
+                sfb->vsync_info.count++;
+                wake_up_interruptible(&sfb->vsync_info.wait);
+        }
+
+        /* We only support waiting for VSYNC for now, so it's safe
+         * to always disable irqs here.
+         */
+        s3c_fb_disable_irq(sfb);
+
+        return IRQ_HANDLED;
+}
+
+/**
+ * s3c_fb_wait_for_vsync() - sleep until next VSYNC interrupt or timeout
+ * @sfb: main hardware state
+ * @crtc: head index.
+ */
+static int s3c_fb_wait_for_vsync(struct s3c_fb *sfb, u32 crtc)
+{
+        unsigned long count;
+        int ret;
+
+        if (crtc != 0)
+                return -ENODEV;
+
+        count = sfb->vsync_info.count;
+        s3c_fb_enable_irq(sfb);
+        ret = wait_event_interruptible_timeout(sfb->vsync_info.wait,
+                                       count != sfb->vsync_info.count,
+                                       msecs_to_jiffies(VSYNC_TIMEOUT_MSEC));
+        if (ret == 0)
+                return -ETIMEDOUT;
+
+        return 0;
+}
+
+static int s3c_fb_ioctl(struct fb_info *info, unsigned int cmd,
+                        unsigned long arg)
+{
+        struct s3c_fb_win *win = info->par;
+        struct s3c_fb *sfb = win->parent;
+        int ret;
+        u32 crtc;
+
+        switch (cmd) {
+        case FBIO_WAITFORVSYNC:
+                if (get_user(crtc, (u32 __user *)arg)) {
+                        ret = -EFAULT;
+                        break;
+                }
+
+                ret = s3c_fb_wait_for_vsync(sfb, crtc);
+                break;
+        default:
+                ret = -ENOTTY;
+        }
+
+        return ret;
+}
+
 static struct fb_ops s3c_fb_ops = {
         .owner          = THIS_MODULE,
         .fb_check_var   = s3c_fb_check_var,
@@ -651,9 +1022,33 @@ static struct fb_ops s3c_fb_ops = {
         .fb_fillrect    = cfb_fillrect,
         .fb_copyarea    = cfb_copyarea,
         .fb_imageblit   = cfb_imageblit,
+        .fb_pan_display = s3c_fb_pan_display,
+        .fb_ioctl       = s3c_fb_ioctl,
 };
 
 /**
+ * s3c_fb_missing_pixclock() - calculates pixel clock
+ * @mode: The video mode to change.
+ *
+ * Calculate the pixel clock when none has been given through platform data.
+ */
+static void __devinit s3c_fb_missing_pixclock(struct fb_videomode *mode)
+{
+        u64 pixclk = 1000000000000ULL;
+        u32 div;
+
+        div  = mode->left_margin + mode->hsync_len + mode->right_margin +
+               mode->xres;
+        div *= mode->upper_margin + mode->vsync_len + mode->lower_margin +
+               mode->yres;
+        div *= mode->refresh ? : 60;
+
+        do_div(pixclk, div);
+
+        mode->pixclock = pixclk;
+}
+
+/**
  * s3c_fb_alloc_memory() - allocate display memory for framebuffer window
  * @sfb: The base resources for the hardware.
  * @win: The window to initialise memory for.
@@ -711,7 +1106,8 @@ static void s3c_fb_free_memory(struct s3c_fb *sfb, struct s3c_fb_win *win)
 {
         struct fb_info *fbi = win->fbinfo;
 
-        dma_free_writecombine(sfb->dev, PAGE_ALIGN(fbi->fix.smem_len),
+        if (fbi->screen_base)
+                dma_free_writecombine(sfb->dev, PAGE_ALIGN(fbi->fix.smem_len),
                               fbi->screen_base, fbi->fix.smem_start);
 }
 
@@ -724,9 +1120,18 @@ static void s3c_fb_free_memory(struct s3c_fb *sfb, struct s3c_fb_win *win)
  */
 static void s3c_fb_release_win(struct s3c_fb *sfb, struct s3c_fb_win *win)
 {
+        u32 data;
+
         if (win->fbinfo) {
+                if (sfb->variant.has_shadowcon) {
+                        data = readl(sfb->regs + SHADOWCON);
+                        data &= ~SHADOWCON_CHx_ENABLE(win->index);
+                        data &= ~SHADOWCON_CHx_LOCAL_ENABLE(win->index);
+                        writel(data, sfb->regs + SHADOWCON);
+                }
                 unregister_framebuffer(win->fbinfo);
-                fb_dealloc_cmap(&win->fbinfo->cmap);
+                if (win->fbinfo->cmap.len)
+                        fb_dealloc_cmap(&win->fbinfo->cmap);
                 s3c_fb_free_memory(sfb, win);
                 framebuffer_release(win->fbinfo);
         }
@@ -735,12 +1140,14 @@ static void s3c_fb_release_win(struct s3c_fb *sfb, struct s3c_fb_win *win)
 /**
  * s3c_fb_probe_win() - register an hardware window
  * @sfb: The base resources for the hardware
+ * @variant: The variant information for this window.
  * @res: Pointer to where to place the resultant window.
  *
  * Allocate and do the basic initialisation for one of the hardware's graphics
  * windows.
  */
 static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
+                                      struct s3c_fb_win_variant *variant,
                                       struct s3c_fb_win **res)
 {
         struct fb_var_screeninfo *var;
@@ -751,9 +1158,11 @@ static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
         int palette_size;
         int ret;
 
-        dev_dbg(sfb->dev, "probing window %d\n", win_no);
+        dev_dbg(sfb->dev, "probing window %d, variant %p\n", win_no, variant);
+
+        init_waitqueue_head(&sfb->vsync_info.wait);
 
-        palette_size = s3c_fb_win_pal_size(win_no);
+        palette_size = variant->palette_sz * 4;
 
         fbinfo = framebuffer_alloc(sizeof(struct s3c_fb_win) +
                                    palette_size * sizeof(u32), sfb->dev);
@@ -770,7 +1179,9 @@ static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
         WARN_ON(windata->win_mode.yres == 0);
 
         win = fbinfo->par;
+        *res = win;
         var = &fbinfo->var;
+        win->variant = *variant;
         win->fbinfo = fbinfo;
         win->parent = sfb;
         win->windata = windata;
@@ -784,7 +1195,24 @@ static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
         }
 
         /* setup the r/b/g positions for the window's palette */
-        s3c_fb_init_palette(win_no, &win->palette);
+        if (win->variant.palette_16bpp) {
+                /* Set RGB 5:6:5 as default */
+                win->palette.r.offset = 11;
+                win->palette.r.length = 5;
+                win->palette.g.offset = 5;
+                win->palette.g.length = 6;
+                win->palette.b.offset = 0;
+                win->palette.b.length = 5;
+
+        } else {
+                /* Set 8bpp or 8bpp and 1bit alpha */
+                win->palette.r.offset = 16;
+                win->palette.r.length = 8;
+                win->palette.g.offset = 8;
+                win->palette.g.length = 8;
+                win->palette.b.offset = 0;
+                win->palette.b.length = 8;
+        }
 
         /* setup the initial video mode from the window */
         fb_videomode_to_var(&fbinfo->var, initmode);
@@ -808,7 +1236,7 @@ static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
 
         /* create initial colour map */
 
-        ret = fb_alloc_cmap(&fbinfo->cmap, s3c_fb_win_pal_size(win_no), 1);
+        ret = fb_alloc_cmap(&fbinfo->cmap, win->variant.palette_sz, 1);
         if (ret == 0)
                 fb_set_cmap(&fbinfo->cmap, fbinfo);
         else
@@ -826,7 +1254,6 @@ static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
                 return ret;
         }
 
-        *res = win;
         dev_info(sfb->dev, "window %d: fb %s\n", win_no, fbinfo->fix.id);
 
         return 0;
@@ -842,18 +1269,19 @@ static int __devinit s3c_fb_probe_win(struct s3c_fb *sfb, unsigned int win_no,
 static void s3c_fb_clear_win(struct s3c_fb *sfb, int win)
 {
         void __iomem *regs = sfb->regs;
-
-        writel(0, regs + WINCON(win));
-        writel(0xffffff, regs + WxKEYCONy(win, 0));
-        writel(0xffffff, regs + WxKEYCONy(win, 1));
-
-        writel(0, regs + VIDOSD_A(win));
-        writel(0, regs + VIDOSD_B(win));
-        writel(0, regs + VIDOSD_C(win));
+        u32 reg;
+
+        writel(0, regs + sfb->variant.wincon + (win * 4));
+        writel(0, regs + VIDOSD_A(win, sfb->variant));
+        writel(0, regs + VIDOSD_B(win, sfb->variant));
+        writel(0, regs + VIDOSD_C(win, sfb->variant));
+        reg = readl(regs + SHADOWCON);
+        writel(reg & ~SHADOWCON_WINx_PROTECT(win), regs + SHADOWCON);
 }
 
 static int __devinit s3c_fb_probe(struct platform_device *pdev)
 {
+        struct s3c_fb_driverdata *fbdrv;
         struct device *dev = &pdev->dev;
         struct s3c_fb_platdata *pd;
         struct s3c_fb *sfb;
@@ -861,6 +1289,13 @@ 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;
+
+        if (fbdrv->variant.nr_windows > S3C_FB_MAX_WIN) {
+                dev_err(dev, "too many windows, cannot attach\n");
+                return -EINVAL;
+        }
+
         pd = pdev->dev.platform_data;
         if (!pd) {
                 dev_err(dev, "no platform data specified\n");
@@ -873,8 +1308,11 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
                 return -ENOMEM;
         }
 
+        dev_dbg(dev, "allocate new framebuffer %p\n", sfb);
+
         sfb->dev = dev;
         sfb->pdata = pd;
+        sfb->variant = fbdrv->variant;
 
         sfb->bus_clk = clk_get(dev, "lcd");
         if (IS_ERR(sfb->bus_clk)) {
@@ -906,6 +1344,20 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
                 goto err_req_region;
         }
 
+        res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+        if (!res) {
+                dev_err(dev, "failed to acquire irq resource\n");
+                ret = -ENOENT;
+                goto err_ioremap;
+        }
+        sfb->irq_no = res->start;
+        ret = request_irq(sfb->irq_no, s3c_fb_irq,
+                          0, "s3c_fb", sfb);
+        if (ret) {
+                dev_err(dev, "irq request failed\n");
+                goto err_ioremap;
+        }
+
         dev_dbg(dev, "got resources (regs %p), probing windows\n", sfb->regs);
 
         /* setup gpio and output polarity controls */
@@ -916,21 +1368,34 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 
         /* zero all windows before we do anything */
 
-        for (win = 0; win < S3C_FB_MAX_WIN; win++)
+        for (win = 0; win < fbdrv->variant.nr_windows; win++)
                 s3c_fb_clear_win(sfb, win);
 
+        /* initialise colour key controls */
+        for (win = 0; win < (fbdrv->variant.nr_windows - 1); win++) {
+                void __iomem *regs = sfb->regs + sfb->variant.keycon;
+
+                regs += (win * 8);
+                writel(0xffffff, regs + WKEYCON0);
+                writel(0xffffff, regs + WKEYCON1);
+        }
+
         /* we have the register setup, start allocating framebuffers */
 
-        for (win = 0; win < S3C_FB_MAX_WIN; win++) {
+        for (win = 0; win < fbdrv->variant.nr_windows; win++) {
                 if (!pd->win[win])
                         continue;
 
-                ret = s3c_fb_probe_win(sfb, win, &sfb->windows[win]);
+                if (!pd->win[win]->win_mode.pixclock)
+                        s3c_fb_missing_pixclock(&pd->win[win]->win_mode);
+
+                ret = s3c_fb_probe_win(sfb, win, fbdrv->win[win],
+                                       &sfb->windows[win]);
                 if (ret < 0) {
                         dev_err(dev, "failed to create window %d\n", win);
                         for (; win >= 0; win--)
                                 s3c_fb_release_win(sfb, sfb->windows[win]);
-                        goto err_ioremap;
+                        goto err_irq;
                 }
         }
 
@@ -938,6 +1403,9 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 
         return 0;
 
+err_irq:
+        free_irq(sfb->irq_no, sfb);
+
 err_ioremap:
         iounmap(sfb->regs);
 
@@ -970,6 +1438,8 @@ static int __devexit s3c_fb_remove(struct platform_device *pdev)
                 if (sfb->windows[win])
                         s3c_fb_release_win(sfb, sfb->windows[win]);
 
+        free_irq(sfb->irq_no, sfb);
+
         iounmap(sfb->regs);
 
         clk_disable(sfb->bus_clk);
@@ -1016,9 +1486,17 @@ static int s3c_fb_resume(struct platform_device *pdev)
         writel(pd->vidcon1, sfb->regs + VIDCON1);
 
         /* zero all windows before we do anything */
-        for (win_no = 0; win_no < S3C_FB_MAX_WIN; win_no++)
+        for (win_no = 0; win_no < sfb->variant.nr_windows; win_no++)
                 s3c_fb_clear_win(sfb, win_no);
 
+        for (win_no = 0; win_no < sfb->variant.nr_windows - 1; win_no++) {
+                void __iomem *regs = sfb->regs + sfb->variant.keycon;
+
+                regs += (win_no * 8);
+                writel(0xffffff, regs + WKEYCON0);
+                writel(0xffffff, regs + WKEYCON1);
+        }
+
         /* restore framebuffers */
         for (win_no = 0; win_no < S3C_FB_MAX_WIN; win_no++) {
                 win = sfb->windows[win_no];
@@ -1036,11 +1514,208 @@ static int s3c_fb_resume(struct platform_device *pdev)
 #define s3c_fb_resume  NULL
 #endif
 
+
+#define VALID_BPP124 (VALID_BPP(1) | VALID_BPP(2) | VALID_BPP(4))
+#define VALID_BPP1248 (VALID_BPP124 | VALID_BPP(8))
+
+static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
+        [0] = {
+                .has_osd_c      = 1,
+                .osd_size_off   = 0x8,
+                .palette_sz     = 256,
+                .valid_bpp      = VALID_BPP1248 | VALID_BPP(16) | VALID_BPP(24),
+        },
+        [1] = {
+                .has_osd_c      = 1,
+                .has_osd_d      = 1,
+                .osd_size_off   = 0x12,
+                .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)),
+        },
+        [2] = {
+                .has_osd_c      = 1,
+                .has_osd_d      = 1,
+                .osd_size_off   = 0x12,
+                .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)),
+        },
+        [3] = {
+                .has_osd_c      = 1,
+                .has_osd_alpha  = 1,
+                .palette_sz     = 16,
+                .palette_16bpp  = 1,
+                .valid_bpp      = (VALID_BPP124  | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(19) |
+                                   VALID_BPP(24) | VALID_BPP(25)),
+        },
+        [4] = {
+                .has_osd_c      = 1,
+                .has_osd_alpha  = 1,
+                .palette_sz     = 4,
+                .palette_16bpp  = 1,
+                .valid_bpp      = (VALID_BPP(1) | VALID_BPP(2) |
+                                   VALID_BPP(16) | VALID_BPP(18) |
+                                   VALID_BPP(24) | VALID_BPP(25)),
+        },
+};
+
+static struct s3c_fb_driverdata s3c_fb_data_64xx = {
+        .variant = {
+                .nr_windows     = 5,
+                .vidtcon        = VIDTCON0,
+                .wincon         = WINCON(0),
+                .winmap         = WINxMAP(0),
+                .keycon         = WKEYCON,
+                .osd            = VIDOSD_BASE,
+                .osd_stride     = 16,
+                .buf_start      = VIDW_BUF_START(0),
+                .buf_size       = VIDW_BUF_SIZE(0),
+                .buf_end        = VIDW_BUF_END(0),
+
+                .palette = {
+                        [0] = 0x400,
+                        [1] = 0x800,
+                        [2] = 0x300,
+                        [3] = 0x320,
+                        [4] = 0x340,
+                },
+
+                .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],
+};
+
+static struct s3c_fb_driverdata s3c_fb_data_s5pc100 = {
+        .variant = {
+                .nr_windows     = 5,
+                .vidtcon        = VIDTCON0,
+                .wincon         = WINCON(0),
+                .winmap         = WINxMAP(0),
+                .keycon         = WKEYCON,
+                .osd            = VIDOSD_BASE,
+                .osd_stride     = 16,
+                .buf_start      = VIDW_BUF_START(0),
+                .buf_size       = VIDW_BUF_SIZE(0),
+                .buf_end        = VIDW_BUF_END(0),
+
+                .palette = {
+                        [0] = 0x2400,
+                        [1] = 0x2800,
+                        [2] = 0x2c00,
+                        [3] = 0x3000,
+                        [4] = 0x3400,
+                },
+
+                .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],
+};
+
+static struct s3c_fb_driverdata s3c_fb_data_s5pv210 = {
+        .variant = {
+                .nr_windows     = 5,
+                .vidtcon        = VIDTCON0,
+                .wincon         = WINCON(0),
+                .winmap         = WINxMAP(0),
+                .keycon         = WKEYCON,
+                .osd            = VIDOSD_BASE,
+                .osd_stride     = 16,
+                .buf_start      = VIDW_BUF_START(0),
+                .buf_size       = VIDW_BUF_SIZE(0),
+                .buf_end        = VIDW_BUF_END(0),
+
+                .palette = {
+                        [0] = 0x2400,
+                        [1] = 0x2800,
+                        [2] = 0x2c00,
+                        [3] = 0x3000,
+                        [4] = 0x3400,
+                },
+
+                .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],
+};
+
+/* S3C2443/S3C2416 style hardware */
+static struct s3c_fb_driverdata s3c_fb_data_s3c2443 = {
+        .variant = {
+                .nr_windows     = 2,
+                .is_2443        = 1,
+
+                .vidtcon        = 0x08,
+                .wincon         = 0x14,
+                .winmap         = 0xd0,
+                .keycon         = 0xb0,
+                .osd            = 0x28,
+                .osd_stride     = 12,
+                .buf_start      = 0x64,
+                .buf_size       = 0x94,
+                .buf_end        = 0x7c,
+
+                .palette = {
+                        [0] = 0x400,
+                        [1] = 0x800,
+                },
+        },
+        .win[0] = &(struct s3c_fb_win_variant) {
+                .palette_sz     = 256,
+                .valid_bpp      = VALID_BPP1248 | VALID_BPP(16) | VALID_BPP(24),
+        },
+        .win[1] = &(struct s3c_fb_win_variant) {
+                .has_osd_c      = 1,
+                .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(28)),
+        },
+};
+
+static struct platform_device_id s3c_fb_driver_ids[] = {
+        {
+                .name           = "s3c-fb",
+                .driver_data    = (unsigned long)&s3c_fb_data_64xx,
+        }, {
+                .name           = "s5pc100-fb",
+                .driver_data    = (unsigned long)&s3c_fb_data_s5pc100,
+        }, {
+                .name           = "s5pv210-fb",
+                .driver_data    = (unsigned long)&s3c_fb_data_s5pv210,
+        }, {
+                .name           = "s3c2443-fb",
+                .driver_data    = (unsigned long)&s3c_fb_data_s3c2443,
+        },
+        {},
+};
+MODULE_DEVICE_TABLE(platform, s3c_fb_driver_ids);
+
 static struct platform_driver s3c_fb_driver = {
         .probe          = s3c_fb_probe,
         .remove         = __devexit_p(s3c_fb_remove),
         .suspend        = s3c_fb_suspend,
         .resume         = s3c_fb_resume,
+        .id_table       = s3c_fb_driver_ids,
         .driver         = {
                 .name   = "s3c-fb",
                 .owner  = THIS_MODULE,
diff --git a/drivers/video/savage/savagefb-i2c.c b/drivers/video/savage/savagefb-i2c.c
index ed371c868b3a..b16e6138fdd4 100644
--- a/drivers/video/savage/savagefb-i2c.c
+++ b/drivers/video/savage/savagefb-i2c.c
@@ -181,6 +181,15 @@ void savagefb_create_i2c_busses(struct fb_info *info)
                 par->chan.algo.getscl = prosavage_gpio_getscl;
                 break;
         case FB_ACCEL_SAVAGE4:
+                par->chan.reg = CR_SERIAL1;
+                if (par->pcidev->revision > 1 && !(VGArCR(0xa6, par) & 0x40))
+                        par->chan.reg = CR_SERIAL2;
+                par->chan.ioaddr      = par->mmio.vbase;
+                par->chan.algo.setsda = prosavage_gpio_setsda;
+                par->chan.algo.setscl = prosavage_gpio_setscl;
+                par->chan.algo.getsda = prosavage_gpio_getsda;
+                par->chan.algo.getscl = prosavage_gpio_getscl;
+                break;
         case FB_ACCEL_SAVAGE2000:
                 par->chan.reg         = 0xff20;
                 par->chan.ioaddr      = par->mmio.vbase;
diff --git a/drivers/video/savage/savagefb.h b/drivers/video/savage/savagefb.h
index 8bfdfc3c5234..e4c3f214eb8e 100644
--- a/drivers/video/savage/savagefb.h
+++ b/drivers/video/savage/savagefb.h
@@ -13,7 +13,6 @@
 #define __SAVAGEFB_H__
 
 #include <linux/i2c.h>
-#include <linux/i2c-id.h>
 #include <linux/i2c-algo-bit.h>
 #include <linux/mutex.h>
 #include <video/vga.h>
diff --git a/drivers/video/sh_mipi_dsi.c b/drivers/video/sh_mipi_dsi.c
new file mode 100644
index 000000000000..5699ce0c1780
--- /dev/null
+++ b/drivers/video/sh_mipi_dsi.c
@@ -0,0 +1,505 @@
+/*
+ * Renesas SH-mobile MIPI DSI support
+ *
+ * Copyright (C) 2010 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
+#include <video/mipi_display.h>
+#include <video/sh_mipi_dsi.h>
+#include <video/sh_mobile_lcdc.h>
+
+#define CMTSRTCTR       0x80d0
+#define CMTSRTREQ       0x8070
+
+#define DSIINTE         0x0060
+
+/* E.g., sh7372 has 2 MIPI-DSIs - one for each LCDC */
+#define MAX_SH_MIPI_DSI 2
+
+struct sh_mipi {
+        void __iomem    *base;
+        struct clk      *dsit_clk;
+        struct clk      *dsip_clk;
+};
+
+static struct sh_mipi *mipi_dsi[MAX_SH_MIPI_DSI];
+
+/* Protect the above array */
+static DEFINE_MUTEX(array_lock);
+
+static struct sh_mipi *sh_mipi_by_handle(int handle)
+{
+        if (handle >= ARRAY_SIZE(mipi_dsi) || handle < 0)
+                return NULL;
+
+        return mipi_dsi[handle];
+}
+
+static int sh_mipi_send_short(struct sh_mipi *mipi, u8 dsi_cmd,
+                              u8 cmd, u8 param)
+{
+        u32 data = (dsi_cmd << 24) | (cmd << 16) | (param << 8);
+        int cnt = 100;
+
+        /* transmit a short packet to LCD panel */
+        iowrite32(1 | data, mipi->base + 0x80d0); /* CMTSRTCTR */
+        iowrite32(1, mipi->base + 0x8070); /* CMTSRTREQ */
+
+        while ((ioread32(mipi->base + 0x8070) & 1) && --cnt)
+                udelay(1);
+
+        return cnt ? 0 : -ETIMEDOUT;
+}
+
+#define LCD_CHAN2MIPI(c) ((c) < LCDC_CHAN_MAINLCD || (c) > LCDC_CHAN_SUBLCD ? \
+                                -EINVAL : (c) - 1)
+
+static int sh_mipi_dcs(int handle, u8 cmd)
+{
+        struct sh_mipi *mipi = sh_mipi_by_handle(LCD_CHAN2MIPI(handle));
+        if (!mipi)
+                return -ENODEV;
+        return sh_mipi_send_short(mipi, MIPI_DSI_DCS_SHORT_WRITE, cmd, 0);
+}
+
+static int sh_mipi_dcs_param(int handle, u8 cmd, u8 param)
+{
+        struct sh_mipi *mipi = sh_mipi_by_handle(LCD_CHAN2MIPI(handle));
+        if (!mipi)
+                return -ENODEV;
+        return sh_mipi_send_short(mipi, MIPI_DSI_DCS_SHORT_WRITE_PARAM, cmd,
+                                  param);
+}
+
+static void sh_mipi_dsi_enable(struct sh_mipi *mipi, bool enable)
+{
+        /*
+         * enable LCDC data tx, transition to LPS after completion of each HS
+         * packet
+         */
+        iowrite32(0x00000002 | enable, mipi->base + 0x8000); /* DTCTR */
+}
+
+static void sh_mipi_shutdown(struct platform_device *pdev)
+{
+        struct sh_mipi *mipi = platform_get_drvdata(pdev);
+
+        sh_mipi_dsi_enable(mipi, false);
+}
+
+static void mipi_display_on(void *arg, struct fb_info *info)
+{
+        struct sh_mipi *mipi = arg;
+
+        sh_mipi_dsi_enable(mipi, true);
+}
+
+static void mipi_display_off(void *arg)
+{
+        struct sh_mipi *mipi = arg;
+
+        sh_mipi_dsi_enable(mipi, false);
+}
+
+static int __init sh_mipi_setup(struct sh_mipi *mipi,
+                                struct sh_mipi_dsi_info *pdata)
+{
+        void __iomem *base = mipi->base;
+        struct sh_mobile_lcdc_chan_cfg *ch = pdata->lcd_chan;
+        u32 pctype, datatype, pixfmt;
+        u32 linelength;
+        bool yuv;
+
+        /* Select data format */
+        switch (pdata->data_format) {
+        case MIPI_RGB888:
+                pctype = 0;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_24;
+                pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
+                linelength = ch->lcd_cfg.xres * 3;
+                yuv = false;
+                break;
+        case MIPI_RGB565:
+                pctype = 1;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_16;
+                pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
+                linelength = ch->lcd_cfg.xres * 2;
+                yuv = false;
+                break;
+        case MIPI_RGB666_LP:
+                pctype = 2;
+                datatype = MIPI_DSI_PIXEL_STREAM_3BYTE_18;
+                pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
+                linelength = ch->lcd_cfg.xres * 3;
+                yuv = false;
+                break;
+        case MIPI_RGB666:
+                pctype = 3;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_18;
+                pixfmt = MIPI_DCS_PIXEL_FMT_18BIT;
+                linelength = (ch->lcd_cfg.xres * 18 + 7) / 8;
+                yuv = false;
+                break;
+        case MIPI_BGR888:
+                pctype = 8;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_24;
+                pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
+                linelength = ch->lcd_cfg.xres * 3;
+                yuv = false;
+                break;
+        case MIPI_BGR565:
+                pctype = 9;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_16;
+                pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
+                linelength = ch->lcd_cfg.xres * 2;
+                yuv = false;
+                break;
+        case MIPI_BGR666_LP:
+                pctype = 0xa;
+                datatype = MIPI_DSI_PIXEL_STREAM_3BYTE_18;
+                pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
+                linelength = ch->lcd_cfg.xres * 3;
+                yuv = false;
+                break;
+        case MIPI_BGR666:
+                pctype = 0xb;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_18;
+                pixfmt = MIPI_DCS_PIXEL_FMT_18BIT;
+                linelength = (ch->lcd_cfg.xres * 18 + 7) / 8;
+                yuv = false;
+                break;
+        case MIPI_YUYV:
+                pctype = 4;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR16;
+                pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
+                linelength = ch->lcd_cfg.xres * 2;
+                yuv = true;
+                break;
+        case MIPI_UYVY:
+                pctype = 5;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR16;
+                pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
+                linelength = ch->lcd_cfg.xres * 2;
+                yuv = true;
+                break;
+        case MIPI_YUV420_L:
+                pctype = 6;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR12;
+                pixfmt = MIPI_DCS_PIXEL_FMT_12BIT;
+                linelength = (ch->lcd_cfg.xres * 12 + 7) / 8;
+                yuv = true;
+                break;
+        case MIPI_YUV420:
+                pctype = 7;
+                datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR12;
+                pixfmt = MIPI_DCS_PIXEL_FMT_12BIT;
+                /* Length of U/V line */
+                linelength = (ch->lcd_cfg.xres + 1) / 2;
+                yuv = true;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        if ((yuv && ch->interface_type != YUV422) ||
+            (!yuv && ch->interface_type != RGB24))
+                return -EINVAL;
+
+        /* reset DSI link */
+        iowrite32(0x00000001, base); /* SYSCTRL */
+        /* Hold reset for 100 cycles of the slowest of bus, HS byte and LP clock */
+        udelay(50);
+        iowrite32(0x00000000, base); /* SYSCTRL */
+
+        /* setup DSI link */
+
+        /*
+         * Default = ULPS enable |
+         *      Contention detection enabled |
+         *      EoT packet transmission enable |
+         *      CRC check enable |
+         *      ECC check enable
+         * additionally enable first two lanes
+         */
+        iowrite32(0x00003703, base + 0x04); /* SYSCONF */
+        /*
+         * T_wakeup = 0x7000
+         * T_hs-trail = 3
+         * T_hs-prepare = 3
+         * T_clk-trail = 3
+         * T_clk-prepare = 2
+         */
+        iowrite32(0x70003332, base + 0x08); /* TIMSET */
+        /* no responses requested */
+        iowrite32(0x00000000, base + 0x18); /* RESREQSET0 */
+        /* request response to packets of type 0x28 */
+        iowrite32(0x00000100, base + 0x1c); /* RESREQSET1 */
+        /* High-speed transmission timeout, default 0xffffffff */
+        iowrite32(0x0fffffff, base + 0x20); /* HSTTOVSET */
+        /* LP reception timeout, default 0xffffffff */
+        iowrite32(0x0fffffff, base + 0x24); /* LPRTOVSET */
+        /* Turn-around timeout, default 0xffffffff */
+        iowrite32(0x0fffffff, base + 0x28); /* TATOVSET */
+        /* Peripheral reset timeout, default 0xffffffff */
+        iowrite32(0x0fffffff, base + 0x2c); /* PRTOVSET */
+        /* Enable timeout counters */
+        iowrite32(0x00000f00, base + 0x30); /* DSICTRL */
+        /* Interrupts not used, disable all */
+        iowrite32(0, base + DSIINTE);
+        /* DSI-Tx bias on */
+        iowrite32(0x00000001, base + 0x70); /* PHYCTRL */
+        udelay(200);
+        /* Deassert resets, power on, set multiplier */
+        iowrite32(0x03070b01, base + 0x70); /* PHYCTRL */
+
+        /* setup l-bridge */
+
+        /*
+         * Enable transmission of all packets,
+         * transmit LPS after each HS packet completion
+         */
+        iowrite32(0x00000006, base + 0x8000); /* DTCTR */
+        /* VSYNC width = 2 (<< 17) */
+        iowrite32(0x00040000 | (pctype << 12) | datatype, base + 0x8020); /* VMCTR1 */
+        /*
+         * Non-burst mode with sync pulses: VSE and HSE are output,
+         * HSA period allowed, no commands in LP
+         */
+        iowrite32(0x00e00000, base + 0x8024); /* VMCTR2 */
+        /*
+         * 0x660 = 1632 bytes per line (RGB24, 544 pixels: see
+         * sh_mobile_lcdc_info.ch[0].lcd_cfg.xres), HSALEN = 1 - default
+         * (unused, since VMCTR2[HSABM] = 0)
+         */
+        iowrite32(1 | (linelength << 16), base + 0x8028); /* VMLEN1 */
+
+        msleep(5);
+
+        /* setup LCD panel */
+
+        /* cf. drivers/video/omap/lcd_mipid.c */
+        sh_mipi_dcs(ch->chan, MIPI_DCS_EXIT_SLEEP_MODE);
+        msleep(120);
+        /*
+         * [7] - Page Address Mode
+         * [6] - Column Address Mode
+         * [5] - Page / Column Address Mode
+         * [4] - Display Device Line Refresh Order
+         * [3] - RGB/BGR Order
+         * [2] - Display Data Latch Data Order
+         * [1] - Flip Horizontal
+         * [0] - Flip Vertical
+         */
+        sh_mipi_dcs_param(ch->chan, MIPI_DCS_SET_ADDRESS_MODE, 0x00);
+        /* cf. set_data_lines() */
+        sh_mipi_dcs_param(ch->chan, MIPI_DCS_SET_PIXEL_FORMAT,
+                          pixfmt << 4);
+        sh_mipi_dcs(ch->chan, MIPI_DCS_SET_DISPLAY_ON);
+
+        return 0;
+}
+
+static int __init sh_mipi_probe(struct platform_device *pdev)
+{
+        struct sh_mipi *mipi;
+        struct sh_mipi_dsi_info *pdata = pdev->dev.platform_data;
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        unsigned long rate, f_current;
+        int idx = pdev->id, ret;
+        char dsip_clk[] = "dsi.p_clk";
+
+        if (!res || idx >= ARRAY_SIZE(mipi_dsi) || !pdata)
+                return -ENODEV;
+
+        mutex_lock(&array_lock);
+        if (idx < 0)
+                for (idx = 0; idx < ARRAY_SIZE(mipi_dsi) && mipi_dsi[idx]; idx++)
+                        ;
+
+        if (idx == ARRAY_SIZE(mipi_dsi)) {
+                ret = -EBUSY;
+                goto efindslot;
+        }
+
+        mipi = kzalloc(sizeof(*mipi), GFP_KERNEL);
+        if (!mipi) {
+                ret = -ENOMEM;
+                goto ealloc;
+        }
+
+        if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
+                dev_err(&pdev->dev, "MIPI register region already claimed\n");
+                ret = -EBUSY;
+                goto ereqreg;
+        }
+
+        mipi->base = ioremap(res->start, resource_size(res));
+        if (!mipi->base) {
+                ret = -ENOMEM;
+                goto emap;
+        }
+
+        mipi->dsit_clk = clk_get(&pdev->dev, "dsit_clk");
+        if (IS_ERR(mipi->dsit_clk)) {
+                ret = PTR_ERR(mipi->dsit_clk);
+                goto eclktget;
+        }
+
+        f_current = clk_get_rate(mipi->dsit_clk);
+        /* 80MHz required by the datasheet */
+        rate = clk_round_rate(mipi->dsit_clk, 80000000);
+        if (rate > 0 && rate != f_current)
+                ret = clk_set_rate(mipi->dsit_clk, rate);
+        else
+                ret = rate;
+        if (ret < 0)
+                goto esettrate;
+
+        dev_dbg(&pdev->dev, "DSI-T clk %lu -> %lu\n", f_current, rate);
+
+        sprintf(dsip_clk, "dsi%1.1dp_clk", idx);
+        mipi->dsip_clk = clk_get(&pdev->dev, dsip_clk);
+        if (IS_ERR(mipi->dsip_clk)) {
+                ret = PTR_ERR(mipi->dsip_clk);
+                goto eclkpget;
+        }
+
+        f_current = clk_get_rate(mipi->dsip_clk);
+        /* Between 10 and 50MHz */
+        rate = clk_round_rate(mipi->dsip_clk, 24000000);
+        if (rate > 0 && rate != f_current)
+                ret = clk_set_rate(mipi->dsip_clk, rate);
+        else
+                ret = rate;
+        if (ret < 0)
+                goto esetprate;
+
+        dev_dbg(&pdev->dev, "DSI-P clk %lu -> %lu\n", f_current, rate);
+
+        msleep(10);
+
+        ret = clk_enable(mipi->dsit_clk);
+        if (ret < 0)
+                goto eclkton;
+
+        ret = clk_enable(mipi->dsip_clk);
+        if (ret < 0)
+                goto eclkpon;
+
+        mipi_dsi[idx] = mipi;
+
+        ret = sh_mipi_setup(mipi, pdata);
+        if (ret < 0)
+                goto emipisetup;
+
+        mutex_unlock(&array_lock);
+        platform_set_drvdata(pdev, mipi);
+
+        /* Set up LCDC callbacks */
+        pdata->lcd_chan->board_cfg.board_data = mipi;
+        pdata->lcd_chan->board_cfg.display_on = mipi_display_on;
+        pdata->lcd_chan->board_cfg.display_off = mipi_display_off;
+
+        return 0;
+
+emipisetup:
+        mipi_dsi[idx] = NULL;
+        clk_disable(mipi->dsip_clk);
+eclkpon:
+        clk_disable(mipi->dsit_clk);
+eclkton:
+esetprate:
+        clk_put(mipi->dsip_clk);
+eclkpget:
+esettrate:
+        clk_put(mipi->dsit_clk);
+eclktget:
+        iounmap(mipi->base);
+emap:
+        release_mem_region(res->start, resource_size(res));
+ereqreg:
+        kfree(mipi);
+ealloc:
+efindslot:
+        mutex_unlock(&array_lock);
+
+        return ret;
+}
+
+static int __exit sh_mipi_remove(struct platform_device *pdev)
+{
+        struct sh_mipi_dsi_info *pdata = pdev->dev.platform_data;
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        struct sh_mipi *mipi = platform_get_drvdata(pdev);
+        int i, ret;
+
+        mutex_lock(&array_lock);
+
+        for (i = 0; i < ARRAY_SIZE(mipi_dsi) && mipi_dsi[i] != mipi; i++)
+                ;
+
+        if (i == ARRAY_SIZE(mipi_dsi)) {
+                ret = -EINVAL;
+        } else {
+                ret = 0;
+                mipi_dsi[i] = NULL;
+        }
+
+        mutex_unlock(&array_lock);
+
+        if (ret < 0)
+                return ret;
+
+        pdata->lcd_chan->board_cfg.display_on = NULL;
+        pdata->lcd_chan->board_cfg.display_off = NULL;
+        pdata->lcd_chan->board_cfg.board_data = NULL;
+
+        clk_disable(mipi->dsip_clk);
+        clk_disable(mipi->dsit_clk);
+        clk_put(mipi->dsit_clk);
+        clk_put(mipi->dsip_clk);
+        iounmap(mipi->base);
+        if (res)
+                release_mem_region(res->start, resource_size(res));
+        platform_set_drvdata(pdev, NULL);
+        kfree(mipi);
+
+        return 0;
+}
+
+static struct platform_driver sh_mipi_driver = {
+        .remove         = __exit_p(sh_mipi_remove),
+        .shutdown       = sh_mipi_shutdown,
+        .driver = {
+                .name   = "sh-mipi-dsi",
+        },
+};
+
+static int __init sh_mipi_init(void)
+{
+        return platform_driver_probe(&sh_mipi_driver, sh_mipi_probe);
+}
+module_init(sh_mipi_init);
+
+static void __exit sh_mipi_exit(void)
+{
+        platform_driver_unregister(&sh_mipi_driver);
+}
+module_exit(sh_mipi_exit);
+
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_DESCRIPTION("SuperH / ARM-shmobile MIPI DSI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sh_mobile_hdmi.c b/drivers/video/sh_mobile_hdmi.c
new file mode 100644
index 000000000000..ef989d94511c
--- /dev/null
+++ b/drivers/video/sh_mobile_hdmi.c
@@ -0,0 +1,1110 @@
+/*
+ * SH-Mobile High-Definition Multimedia Interface (HDMI) driver
+ * for SLISHDMI13T and SLIPHDMIT IP cores
+ *
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.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/clk.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+#include <sound/soc-dapm.h>
+#include <sound/initval.h>
+
+#include <video/sh_mobile_hdmi.h>
+#include <video/sh_mobile_lcdc.h>
+
+#define HDMI_SYSTEM_CTRL                        0x00 /* System control */
+#define HDMI_L_R_DATA_SWAP_CTRL_RPKT            0x01 /* L/R data swap control,
+                                                        bits 19..16 of 20-bit N for Audio Clock Regeneration packet */
+#define HDMI_20_BIT_N_FOR_AUDIO_RPKT_15_8       0x02 /* bits 15..8 of 20-bit N for Audio Clock Regeneration packet */
+#define HDMI_20_BIT_N_FOR_AUDIO_RPKT_7_0        0x03 /* bits 7..0 of 20-bit N for Audio Clock Regeneration packet */
+#define HDMI_SPDIF_AUDIO_SAMP_FREQ_CTS          0x04 /* SPDIF audio sampling frequency,
+                                                        bits 19..16 of Internal CTS */
+#define HDMI_INTERNAL_CTS_15_8                  0x05 /* bits 15..8 of Internal CTS */
+#define HDMI_INTERNAL_CTS_7_0                   0x06 /* bits 7..0 of Internal CTS */
+#define HDMI_EXTERNAL_CTS_19_16                 0x07 /* External CTS */
+#define HDMI_EXTERNAL_CTS_15_8                  0x08 /* External CTS */
+#define HDMI_EXTERNAL_CTS_7_0                   0x09 /* External CTS */
+#define HDMI_AUDIO_SETTING_1                    0x0A /* Audio setting.1 */
+#define HDMI_AUDIO_SETTING_2                    0x0B /* Audio setting.2 */
+#define HDMI_I2S_AUDIO_SET                      0x0C /* I2S audio setting */
+#define HDMI_DSD_AUDIO_SET                      0x0D /* DSD audio setting */
+#define HDMI_DEBUG_MONITOR_1                    0x0E /* Debug monitor.1 */
+#define HDMI_DEBUG_MONITOR_2                    0x0F /* Debug monitor.2 */
+#define HDMI_I2S_INPUT_PIN_SWAP                 0x10 /* I2S input pin swap */
+#define HDMI_AUDIO_STATUS_BITS_SETTING_1        0x11 /* Audio status bits setting.1 */
+#define HDMI_AUDIO_STATUS_BITS_SETTING_2        0x12 /* Audio status bits setting.2 */
+#define HDMI_CATEGORY_CODE                      0x13 /* Category code */
+#define HDMI_SOURCE_NUM_AUDIO_WORD_LEN          0x14 /* Source number/Audio word length */
+#define HDMI_AUDIO_VIDEO_SETTING_1              0x15 /* Audio/Video setting.1 */
+#define HDMI_VIDEO_SETTING_1                    0x16 /* Video setting.1 */
+#define HDMI_DEEP_COLOR_MODES                   0x17 /* Deep Color Modes */
+
+/* 12 16- and 10-bit Color space conversion parameters: 0x18..0x2f */
+#define HDMI_COLOR_SPACE_CONVERSION_PARAMETERS  0x18
+
+#define HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS      0x30 /* External video parameter settings */
+#define HDMI_EXTERNAL_H_TOTAL_7_0               0x31 /* External horizontal total (LSB) */
+#define HDMI_EXTERNAL_H_TOTAL_11_8              0x32 /* External horizontal total (MSB) */
+#define HDMI_EXTERNAL_H_BLANK_7_0               0x33 /* External horizontal blank (LSB) */
+#define HDMI_EXTERNAL_H_BLANK_9_8               0x34 /* External horizontal blank (MSB) */
+#define HDMI_EXTERNAL_H_DELAY_7_0               0x35 /* External horizontal delay (LSB) */
+#define HDMI_EXTERNAL_H_DELAY_9_8               0x36 /* External horizontal delay (MSB) */
+#define HDMI_EXTERNAL_H_DURATION_7_0            0x37 /* External horizontal duration (LSB) */
+#define HDMI_EXTERNAL_H_DURATION_9_8            0x38 /* External horizontal duration (MSB) */
+#define HDMI_EXTERNAL_V_TOTAL_7_0               0x39 /* External vertical total (LSB) */
+#define HDMI_EXTERNAL_V_TOTAL_9_8               0x3A /* External vertical total (MSB) */
+#define HDMI_AUDIO_VIDEO_SETTING_2              0x3B /* Audio/Video setting.2 */
+#define HDMI_EXTERNAL_V_BLANK                   0x3D /* External vertical blank */
+#define HDMI_EXTERNAL_V_DELAY                   0x3E /* External vertical delay */
+#define HDMI_EXTERNAL_V_DURATION                0x3F /* External vertical duration */
+#define HDMI_CTRL_PKT_MANUAL_SEND_CONTROL       0x40 /* Control packet manual send control */
+#define HDMI_CTRL_PKT_AUTO_SEND                 0x41 /* Control packet auto send with VSYNC control */
+#define HDMI_AUTO_CHECKSUM_OPTION               0x42 /* Auto checksum option */
+#define HDMI_VIDEO_SETTING_2                    0x45 /* Video setting.2 */
+#define HDMI_OUTPUT_OPTION                      0x46 /* Output option */
+#define HDMI_SLIPHDMIT_PARAM_OPTION             0x51 /* SLIPHDMIT parameter option */
+#define HDMI_HSYNC_PMENT_AT_EMB_7_0             0x52 /* HSYNC placement at embedded sync (LSB) */
+#define HDMI_HSYNC_PMENT_AT_EMB_15_8            0x53 /* HSYNC placement at embedded sync (MSB) */
+#define HDMI_VSYNC_PMENT_AT_EMB_7_0             0x54 /* VSYNC placement at embedded sync (LSB) */
+#define HDMI_VSYNC_PMENT_AT_EMB_14_8            0x55 /* VSYNC placement at embedded sync (MSB) */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_1         0x56 /* SLIPHDMIT parameter settings.1 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_2         0x57 /* SLIPHDMIT parameter settings.2 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_3         0x58 /* SLIPHDMIT parameter settings.3 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_5         0x59 /* SLIPHDMIT parameter settings.5 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_6         0x5A /* SLIPHDMIT parameter settings.6 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_7         0x5B /* SLIPHDMIT parameter settings.7 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_8         0x5C /* SLIPHDMIT parameter settings.8 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_9         0x5D /* SLIPHDMIT parameter settings.9 */
+#define HDMI_SLIPHDMIT_PARAM_SETTINGS_10        0x5E /* SLIPHDMIT parameter settings.10 */
+#define HDMI_CTRL_PKT_BUF_INDEX                 0x5F /* Control packet buffer index */
+#define HDMI_CTRL_PKT_BUF_ACCESS_HB0            0x60 /* Control packet data buffer access window - HB0 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_HB1            0x61 /* Control packet data buffer access window - HB1 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_HB2            0x62 /* Control packet data buffer access window - HB2 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB0            0x63 /* Control packet data buffer access window - PB0 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB1            0x64 /* Control packet data buffer access window - PB1 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB2            0x65 /* Control packet data buffer access window - PB2 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB3            0x66 /* Control packet data buffer access window - PB3 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB4            0x67 /* Control packet data buffer access window - PB4 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB5            0x68 /* Control packet data buffer access window - PB5 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB6            0x69 /* Control packet data buffer access window - PB6 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB7            0x6A /* Control packet data buffer access window - PB7 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB8            0x6B /* Control packet data buffer access window - PB8 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB9            0x6C /* Control packet data buffer access window - PB9 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB10           0x6D /* Control packet data buffer access window - PB10 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB11           0x6E /* Control packet data buffer access window - PB11 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB12           0x6F /* Control packet data buffer access window - PB12 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB13           0x70 /* Control packet data buffer access window - PB13 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB14           0x71 /* Control packet data buffer access window - PB14 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB15           0x72 /* Control packet data buffer access window - PB15 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB16           0x73 /* Control packet data buffer access window - PB16 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB17           0x74 /* Control packet data buffer access window - PB17 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB18           0x75 /* Control packet data buffer access window - PB18 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB19           0x76 /* Control packet data buffer access window - PB19 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB20           0x77 /* Control packet data buffer access window - PB20 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB21           0x78 /* Control packet data buffer access window - PB21 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB22           0x79 /* Control packet data buffer access window - PB22 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB23           0x7A /* Control packet data buffer access window - PB23 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB24           0x7B /* Control packet data buffer access window - PB24 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB25           0x7C /* Control packet data buffer access window - PB25 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB26           0x7D /* Control packet data buffer access window - PB26 */
+#define HDMI_CTRL_PKT_BUF_ACCESS_PB27           0x7E /* Control packet data buffer access window - PB27 */
+#define HDMI_EDID_KSV_FIFO_ACCESS_WINDOW        0x80 /* EDID/KSV FIFO access window */
+#define HDMI_DDC_BUS_ACCESS_FREQ_CTRL_7_0       0x81 /* DDC bus access frequency control (LSB) */
+#define HDMI_DDC_BUS_ACCESS_FREQ_CTRL_15_8      0x82 /* DDC bus access frequency control (MSB) */
+#define HDMI_INTERRUPT_MASK_1                   0x92 /* Interrupt mask.1 */
+#define HDMI_INTERRUPT_MASK_2                   0x93 /* Interrupt mask.2 */
+#define HDMI_INTERRUPT_STATUS_1                 0x94 /* Interrupt status.1 */
+#define HDMI_INTERRUPT_STATUS_2                 0x95 /* Interrupt status.2 */
+#define HDMI_INTERRUPT_MASK_3                   0x96 /* Interrupt mask.3 */
+#define HDMI_INTERRUPT_MASK_4                   0x97 /* Interrupt mask.4 */
+#define HDMI_INTERRUPT_STATUS_3                 0x98 /* Interrupt status.3 */
+#define HDMI_INTERRUPT_STATUS_4                 0x99 /* Interrupt status.4 */
+#define HDMI_SOFTWARE_HDCP_CONTROL_1            0x9A /* Software HDCP control.1 */
+#define HDMI_FRAME_COUNTER                      0x9C /* Frame counter */
+#define HDMI_FRAME_COUNTER_FOR_RI_CHECK         0x9D /* Frame counter for Ri check */
+#define HDMI_HDCP_CONTROL                       0xAF /* HDCP control */
+#define HDMI_RI_FRAME_COUNT_REGISTER            0xB2 /* Ri frame count register */
+#define HDMI_DDC_BUS_CONTROL                    0xB7 /* DDC bus control */
+#define HDMI_HDCP_STATUS                        0xB8 /* HDCP status */
+#define HDMI_SHA0                               0xB9 /* sha0 */
+#define HDMI_SHA1                               0xBA /* sha1 */
+#define HDMI_SHA2                               0xBB /* sha2 */
+#define HDMI_SHA3                               0xBC /* sha3 */
+#define HDMI_SHA4                               0xBD /* sha4 */
+#define HDMI_BCAPS_READ                         0xBE /* BCAPS read / debug */
+#define HDMI_AKSV_BKSV_7_0_MONITOR              0xBF /* AKSV/BKSV[7:0] monitor */
+#define HDMI_AKSV_BKSV_15_8_MONITOR             0xC0 /* AKSV/BKSV[15:8] monitor */
+#define HDMI_AKSV_BKSV_23_16_MONITOR            0xC1 /* AKSV/BKSV[23:16] monitor */
+#define HDMI_AKSV_BKSV_31_24_MONITOR            0xC2 /* AKSV/BKSV[31:24] monitor */
+#define HDMI_AKSV_BKSV_39_32_MONITOR            0xC3 /* AKSV/BKSV[39:32] monitor */
+#define HDMI_EDID_SEGMENT_POINTER               0xC4 /* EDID segment pointer */
+#define HDMI_EDID_WORD_ADDRESS                  0xC5 /* EDID word address */
+#define HDMI_EDID_DATA_FIFO_ADDRESS             0xC6 /* EDID data FIFO address */
+#define HDMI_NUM_OF_HDMI_DEVICES                0xC7 /* Number of HDMI devices */
+#define HDMI_HDCP_ERROR_CODE                    0xC8 /* HDCP error code */
+#define HDMI_100MS_TIMER_SET                    0xC9 /* 100ms timer setting */
+#define HDMI_5SEC_TIMER_SET                     0xCA /* 5sec timer setting */
+#define HDMI_RI_READ_COUNT                      0xCB /* Ri read count */
+#define HDMI_AN_SEED                            0xCC /* An seed */
+#define HDMI_MAX_NUM_OF_RCIVRS_ALLOWED          0xCD /* Maximum number of receivers allowed */
+#define HDMI_HDCP_MEMORY_ACCESS_CONTROL_1       0xCE /* HDCP memory access control.1 */
+#define HDMI_HDCP_MEMORY_ACCESS_CONTROL_2       0xCF /* HDCP memory access control.2 */
+#define HDMI_HDCP_CONTROL_2                     0xD0 /* HDCP Control 2 */
+#define HDMI_HDCP_KEY_MEMORY_CONTROL            0xD2 /* HDCP Key Memory Control */
+#define HDMI_COLOR_SPACE_CONV_CONFIG_1          0xD3 /* Color space conversion configuration.1 */
+#define HDMI_VIDEO_SETTING_3                    0xD4 /* Video setting.3 */
+#define HDMI_RI_7_0                             0xD5 /* Ri[7:0] */
+#define HDMI_RI_15_8                            0xD6 /* Ri[15:8] */
+#define HDMI_PJ                                 0xD7 /* Pj */
+#define HDMI_SHA_RD                             0xD8 /* sha_rd */
+#define HDMI_RI_7_0_SAVED                       0xD9 /* Ri[7:0] saved */
+#define HDMI_RI_15_8_SAVED                      0xDA /* Ri[15:8] saved */
+#define HDMI_PJ_SAVED                           0xDB /* Pj saved */
+#define HDMI_NUM_OF_DEVICES                     0xDC /* Number of devices */
+#define HDMI_HOT_PLUG_MSENS_STATUS              0xDF /* Hot plug/MSENS status */
+#define HDMI_BCAPS_WRITE                        0xE0 /* bcaps */
+#define HDMI_BSTAT_7_0                          0xE1 /* bstat[7:0] */
+#define HDMI_BSTAT_15_8                         0xE2 /* bstat[15:8] */
+#define HDMI_BKSV_7_0                           0xE3 /* bksv[7:0] */
+#define HDMI_BKSV_15_8                          0xE4 /* bksv[15:8] */
+#define HDMI_BKSV_23_16                         0xE5 /* bksv[23:16] */
+#define HDMI_BKSV_31_24                         0xE6 /* bksv[31:24] */
+#define HDMI_BKSV_39_32                         0xE7 /* bksv[39:32] */
+#define HDMI_AN_7_0                             0xE8 /* An[7:0] */
+#define HDMI_AN_15_8                            0xE9 /* An [15:8] */
+#define HDMI_AN_23_16                           0xEA /* An [23:16] */
+#define HDMI_AN_31_24                           0xEB /* An [31:24] */
+#define HDMI_AN_39_32                           0xEC /* An [39:32] */
+#define HDMI_AN_47_40                           0xED /* An [47:40] */
+#define HDMI_AN_55_48                           0xEE /* An [55:48] */
+#define HDMI_AN_63_56                           0xEF /* An [63:56] */
+#define HDMI_PRODUCT_ID                         0xF0 /* Product ID */
+#define HDMI_REVISION_ID                        0xF1 /* Revision ID */
+#define HDMI_TEST_MODE                          0xFE /* Test mode */
+
+enum hotplug_state {
+        HDMI_HOTPLUG_DISCONNECTED,
+        HDMI_HOTPLUG_CONNECTED,
+        HDMI_HOTPLUG_EDID_DONE,
+};
+
+struct sh_hdmi {
+        void __iomem *base;
+        enum hotplug_state hp_state;
+        struct clk *hdmi_clk;
+        struct device *dev;
+        struct fb_info *info;
+        struct delayed_work edid_work;
+        struct fb_var_screeninfo var;
+};
+
+static void hdmi_write(struct sh_hdmi *hdmi, u8 data, u8 reg)
+{
+        iowrite8(data, hdmi->base + reg);
+}
+
+static u8 hdmi_read(struct sh_hdmi *hdmi, u8 reg)
+{
+        return ioread8(hdmi->base + reg);
+}
+
+/*
+ *      HDMI sound
+ */
+static unsigned int sh_hdmi_snd_read(struct snd_soc_codec *codec,
+                                     unsigned int reg)
+{
+        struct sh_hdmi *hdmi = snd_soc_codec_get_drvdata(codec);
+
+        return hdmi_read(hdmi, reg);
+}
+
+static int sh_hdmi_snd_write(struct snd_soc_codec *codec,
+                             unsigned int reg,
+                             unsigned int value)
+{
+        struct sh_hdmi *hdmi = snd_soc_codec_get_drvdata(codec);
+
+        hdmi_write(hdmi, value, reg);
+        return 0;
+}
+
+static struct snd_soc_dai_driver sh_hdmi_dai = {
+        .name = "sh_mobile_hdmi-hifi",
+        .playback = {
+                .stream_name = "Playback",
+                .channels_min = 2,
+                .channels_max = 8,
+                .rates = SNDRV_PCM_RATE_32000 | SNDRV_PCM_RATE_44100  |
+                         SNDRV_PCM_RATE_48000 | SNDRV_PCM_RATE_88200  |
+                         SNDRV_PCM_RATE_96000 | SNDRV_PCM_RATE_176400 |
+                         SNDRV_PCM_RATE_192000,
+                .formats = SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_S24_LE,
+        },
+};
+
+static int sh_hdmi_snd_probe(struct snd_soc_codec *codec)
+{
+        dev_info(codec->dev, "SH Mobile HDMI Audio Codec");
+
+        return 0;
+}
+
+static struct snd_soc_codec_driver soc_codec_dev_sh_hdmi = {
+        .probe          = sh_hdmi_snd_probe,
+        .read           = sh_hdmi_snd_read,
+        .write          = sh_hdmi_snd_write,
+};
+
+/*
+ *      HDMI video
+ */
+
+/* External video parameter settings */
+static void hdmi_external_video_param(struct sh_hdmi *hdmi)
+{
+        struct fb_var_screeninfo *var = &hdmi->var;
+        u16 htotal, hblank, hdelay, vtotal, vblank, vdelay, voffset;
+        u8 sync = 0;
+
+        htotal = var->xres + var->right_margin + var->left_margin + var->hsync_len;
+
+        hdelay = var->hsync_len + var->left_margin;
+        hblank = var->right_margin + hdelay;
+
+        /*
+         * Vertical timing looks a bit different in Figure 18,
+         * but let's try the same first by setting offset = 0
+         */
+        vtotal = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
+
+        vdelay = var->vsync_len + var->upper_margin;
+        vblank = var->lower_margin + vdelay;
+        voffset = min(var->upper_margin / 2, 6U);
+
+        /*
+         * [3]: VSYNC polarity: Positive
+         * [2]: HSYNC polarity: Positive
+         * [1]: Interlace/Progressive: Progressive
+         * [0]: External video settings enable: used.
+         */
+        if (var->sync & FB_SYNC_HOR_HIGH_ACT)
+                sync |= 4;
+        if (var->sync & FB_SYNC_VERT_HIGH_ACT)
+                sync |= 8;
+
+        pr_debug("H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
+                 htotal, hblank, hdelay, var->hsync_len,
+                 vtotal, vblank, vdelay, var->vsync_len, sync);
+
+        hdmi_write(hdmi, sync | (voffset << 4), HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
+
+        hdmi_write(hdmi, htotal, HDMI_EXTERNAL_H_TOTAL_7_0);
+        hdmi_write(hdmi, htotal >> 8, HDMI_EXTERNAL_H_TOTAL_11_8);
+
+        hdmi_write(hdmi, hblank, HDMI_EXTERNAL_H_BLANK_7_0);
+        hdmi_write(hdmi, hblank >> 8, HDMI_EXTERNAL_H_BLANK_9_8);
+
+        hdmi_write(hdmi, hdelay, HDMI_EXTERNAL_H_DELAY_7_0);
+        hdmi_write(hdmi, hdelay >> 8, HDMI_EXTERNAL_H_DELAY_9_8);
+
+        hdmi_write(hdmi, var->hsync_len, HDMI_EXTERNAL_H_DURATION_7_0);
+        hdmi_write(hdmi, var->hsync_len >> 8, HDMI_EXTERNAL_H_DURATION_9_8);
+
+        hdmi_write(hdmi, vtotal, HDMI_EXTERNAL_V_TOTAL_7_0);
+        hdmi_write(hdmi, vtotal >> 8, HDMI_EXTERNAL_V_TOTAL_9_8);
+
+        hdmi_write(hdmi, vblank, HDMI_EXTERNAL_V_BLANK);
+
+        hdmi_write(hdmi, vdelay, HDMI_EXTERNAL_V_DELAY);
+
+        hdmi_write(hdmi, var->vsync_len, HDMI_EXTERNAL_V_DURATION);
+
+        /* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for manual mode */
+}
+
+/**
+ * sh_hdmi_video_config()
+ */
+static void sh_hdmi_video_config(struct sh_hdmi *hdmi)
+{
+        /*
+         * [7:4]: Audio sampling frequency: 48kHz
+         * [3:1]: Input video format: RGB and YCbCr 4:4:4 (Y on Green)
+         * [0]: Internal/External DE select: internal
+         */
+        hdmi_write(hdmi, 0x20, HDMI_AUDIO_VIDEO_SETTING_1);
+
+        /*
+         * [7:6]: Video output format: RGB 4:4:4
+         * [5:4]: Input video data width: 8 bit
+         * [3:1]: EAV/SAV location: channel 1
+         * [0]: Video input color space: RGB
+         */
+        hdmi_write(hdmi, 0x34, HDMI_VIDEO_SETTING_1);
+
+        /*
+         * [7:6]: Together with bit [6] of HDMI_AUDIO_VIDEO_SETTING_2, which is
+         * left at 0 by default, this configures 24bpp and sets the Color Depth
+         * (CD) field in the General Control Packet
+         */
+        hdmi_write(hdmi, 0x20, HDMI_DEEP_COLOR_MODES);
+}
+
+/**
+ * sh_hdmi_audio_config()
+ */
+static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
+{
+        u8 data;
+        struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+
+        /*
+         * [7:4] L/R data swap control
+         * [3:0] appropriate N[19:16]
+         */
+        hdmi_write(hdmi, 0x00, HDMI_L_R_DATA_SWAP_CTRL_RPKT);
+        /* appropriate N[15:8] */
+        hdmi_write(hdmi, 0x18, HDMI_20_BIT_N_FOR_AUDIO_RPKT_15_8);
+        /* appropriate N[7:0] */
+        hdmi_write(hdmi, 0x00, HDMI_20_BIT_N_FOR_AUDIO_RPKT_7_0);
+
+        /* [7:4] 48 kHz SPDIF not used */
+        hdmi_write(hdmi, 0x20, HDMI_SPDIF_AUDIO_SAMP_FREQ_CTS);
+
+        /*
+         * [6:5] set required down sampling rate if required
+         * [4:3] set required audio source
+         */
+        switch (pdata->flags & HDMI_SND_SRC_MASK) {
+        default:
+                /* fall through */
+        case HDMI_SND_SRC_I2S:
+                data = 0x0 << 3;
+                break;
+        case HDMI_SND_SRC_SPDIF:
+                data = 0x1 << 3;
+                break;
+        case HDMI_SND_SRC_DSD:
+                data = 0x2 << 3;
+                break;
+        case HDMI_SND_SRC_HBR:
+                data = 0x3 << 3;
+                break;
+        }
+        hdmi_write(hdmi, data, HDMI_AUDIO_SETTING_1);
+
+        /* [3:0] set sending channel number for channel status */
+        hdmi_write(hdmi, 0x40, HDMI_AUDIO_SETTING_2);
+
+        /*
+         * [5:2] set valid I2S source input pin
+         * [1:0] set input I2S source mode
+         */
+        hdmi_write(hdmi, 0x04, HDMI_I2S_AUDIO_SET);
+
+        /* [7:4] set valid DSD source input pin */
+        hdmi_write(hdmi, 0x00, HDMI_DSD_AUDIO_SET);
+
+        /* [7:0] set appropriate I2S input pin swap settings if required */
+        hdmi_write(hdmi, 0x00, HDMI_I2S_INPUT_PIN_SWAP);
+
+        /*
+         * [7] set validity bit for channel status
+         * [3:0] set original sample frequency for channel status
+         */
+        hdmi_write(hdmi, 0x00, HDMI_AUDIO_STATUS_BITS_SETTING_1);
+
+        /*
+         * [7] set value for channel status
+         * [6] set value for channel status
+         * [5] set copyright bit for channel status
+         * [4:2] set additional information for channel status
+         * [1:0] set clock accuracy for channel status
+         */
+        hdmi_write(hdmi, 0x00, HDMI_AUDIO_STATUS_BITS_SETTING_2);
+
+        /* [7:0] set category code for channel status */
+        hdmi_write(hdmi, 0x00, HDMI_CATEGORY_CODE);
+
+        /*
+         * [7:4] set source number for channel status
+         * [3:0] set word length for channel status
+         */
+        hdmi_write(hdmi, 0x00, HDMI_SOURCE_NUM_AUDIO_WORD_LEN);
+
+        /* [7:4] set sample frequency for channel status */
+        hdmi_write(hdmi, 0x20, HDMI_AUDIO_VIDEO_SETTING_1);
+}
+
+/**
+ * sh_hdmi_phy_config()
+ */
+static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
+{
+        /* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
+        hdmi_write(hdmi, 0x19, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+        hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+        hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
+        /* PLLA_CONFIG[7:0]: VCO gain, VCO offset, LPF resistance[0] */
+        hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+        hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+        hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+        hdmi_write(hdmi, 0x0E, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+        hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+        hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+}
+
+/**
+ * sh_hdmi_avi_infoframe_setup() - Auxiliary Video Information InfoFrame CONTROL PACKET
+ */
+static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
+{
+        /* AVI InfoFrame */
+        hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_INDEX);
+
+        /* Packet Type = 0x82 */
+        hdmi_write(hdmi, 0x82, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
+
+        /* Version = 0x02 */
+        hdmi_write(hdmi, 0x02, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
+
+        /* Length = 13 (0x0D) */
+        hdmi_write(hdmi, 0x0D, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
+
+        /* N. A. Checksum */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0);
+
+        /*
+         * Y = RGB
+         * A0 = No Data
+         * B = Bar Data not valid
+         * S = No Data
+         */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
+
+        /*
+         * C = No Data
+         * M = 16:9 Picture Aspect Ratio
+         * R = Same as picture aspect ratio
+         */
+        hdmi_write(hdmi, 0x28, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
+
+        /*
+         * ITC = No Data
+         * EC = xvYCC601
+         * Q = Default (depends on video format)
+         * SC = No Known non_uniform Scaling
+         */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB3);
+
+        /*
+         * VIC = 1280 x 720p: ignored if external config is used
+         * Send 2 for 720 x 480p, 16 for 1080p
+         */
+        hdmi_write(hdmi, 4, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
+
+        /* PR = No Repetition */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
+
+        /* Line Number of End of Top Bar (lower 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB6);
+
+        /* Line Number of End of Top Bar (upper 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB7);
+
+        /* Line Number of Start of Bottom Bar (lower 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB8);
+
+        /* Line Number of Start of Bottom Bar (upper 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB9);
+
+        /* Pixel Number of End of Left Bar (lower 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
+
+        /* Pixel Number of End of Left Bar (upper 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB11);
+
+        /* Pixel Number of Start of Right Bar (lower 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB12);
+
+        /* Pixel Number of Start of Right Bar (upper 8 bits) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB13);
+}
+
+/**
+ * sh_hdmi_audio_infoframe_setup() - Audio InfoFrame of CONTROL PACKET
+ */
+static void sh_hdmi_audio_infoframe_setup(struct sh_hdmi *hdmi)
+{
+        /* Audio InfoFrame */
+        hdmi_write(hdmi, 0x08, HDMI_CTRL_PKT_BUF_INDEX);
+
+        /* Packet Type = 0x84 */
+        hdmi_write(hdmi, 0x84, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
+
+        /* Version Number = 0x01 */
+        hdmi_write(hdmi, 0x01, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
+
+        /* 0 Length = 10 (0x0A) */
+        hdmi_write(hdmi, 0x0A, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
+
+        /* n. a. Checksum */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0);
+
+        /* Audio Channel Count = Refer to Stream Header */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
+
+        /* Refer to Stream Header */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
+
+        /* Format depends on coding type (i.e. CT0...CT3) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB3);
+
+        /* Speaker Channel Allocation = Front Right + Front Left */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
+
+        /* Level Shift Value = 0 dB, Down - mix is permitted or no information */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
+
+        /* Reserved (0) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB6);
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB7);
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB8);
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB9);
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
+}
+
+/**
+ * sh_hdmi_gamut_metadata_setup() - Gamut Metadata Packet of CONTROL PACKET
+ */
+static void sh_hdmi_gamut_metadata_setup(struct sh_hdmi *hdmi)
+{
+        int i;
+
+        /* Gamut Metadata Packet */
+        hdmi_write(hdmi, 0x04, HDMI_CTRL_PKT_BUF_INDEX);
+
+        /* Packet Type = 0x0A */
+        hdmi_write(hdmi, 0x0A, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
+        /* Gamut Packet is not used, so default value */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
+        /* Gamut Packet is not used, so default value */
+        hdmi_write(hdmi, 0x10, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
+
+        /* GBD bytes 0 through 27 */
+        for (i = 0; i <= 27; i++)
+                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0_63H - PB27_7EH */
+                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
+}
+
+/**
+ * sh_hdmi_acp_setup() - Audio Content Protection Packet (ACP)
+ */
+static void sh_hdmi_acp_setup(struct sh_hdmi *hdmi)
+{
+        int i;
+
+        /* Audio Content Protection Packet (ACP) */
+        hdmi_write(hdmi, 0x01, HDMI_CTRL_PKT_BUF_INDEX);
+
+        /* Packet Type = 0x04 */
+        hdmi_write(hdmi, 0x04, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
+        /* ACP_Type */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
+        /* Reserved (0) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
+
+        /* GBD bytes 0 through 27 */
+        for (i = 0; i <= 27; i++)
+                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
+                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
+}
+
+/**
+ * sh_hdmi_isrc1_setup() - ISRC1 Packet
+ */
+static void sh_hdmi_isrc1_setup(struct sh_hdmi *hdmi)
+{
+        int i;
+
+        /* ISRC1 Packet */
+        hdmi_write(hdmi, 0x02, HDMI_CTRL_PKT_BUF_INDEX);
+
+        /* Packet Type = 0x05 */
+        hdmi_write(hdmi, 0x05, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
+        /* ISRC_Cont, ISRC_Valid, Reserved (0), ISRC_Status */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
+        /* Reserved (0) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
+
+        /* PB0 UPC_EAN_ISRC_0-15 */
+        /* Bytes PB16-PB27 shall be set to a value of 0. */
+        for (i = 0; i <= 27; i++)
+                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
+                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
+}
+
+/**
+ * sh_hdmi_isrc2_setup() - ISRC2 Packet
+ */
+static void sh_hdmi_isrc2_setup(struct sh_hdmi *hdmi)
+{
+        int i;
+
+        /* ISRC2 Packet */
+        hdmi_write(hdmi, 0x03, HDMI_CTRL_PKT_BUF_INDEX);
+
+        /* HB0 Packet Type = 0x06 */
+        hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
+        /* Reserved (0) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
+        /* Reserved (0) */
+        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
+
+        /* PB0 UPC_EAN_ISRC_16-31 */
+        /* Bytes PB16-PB27 shall be set to a value of 0. */
+        for (i = 0; i <= 27; i++)
+                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
+                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
+}
+
+/**
+ * sh_hdmi_configure() - Initialise HDMI for output
+ */
+static void sh_hdmi_configure(struct sh_hdmi *hdmi)
+{
+        /* Configure video format */
+        sh_hdmi_video_config(hdmi);
+
+        /* Configure audio format */
+        sh_hdmi_audio_config(hdmi);
+
+        /* Configure PHY */
+        sh_hdmi_phy_config(hdmi);
+
+        /* Auxiliary Video Information (AVI) InfoFrame */
+        sh_hdmi_avi_infoframe_setup(hdmi);
+
+        /* Audio InfoFrame */
+        sh_hdmi_audio_infoframe_setup(hdmi);
+
+        /* Gamut Metadata packet */
+        sh_hdmi_gamut_metadata_setup(hdmi);
+
+        /* Audio Content Protection (ACP) Packet */
+        sh_hdmi_acp_setup(hdmi);
+
+        /* ISRC1 Packet */
+        sh_hdmi_isrc1_setup(hdmi);
+
+        /* ISRC2 Packet */
+        sh_hdmi_isrc2_setup(hdmi);
+
+        /*
+         * Control packet auto send with VSYNC control: auto send
+         * General control, Gamut metadata, ISRC, and ACP packets
+         */
+        hdmi_write(hdmi, 0x8E, HDMI_CTRL_PKT_AUTO_SEND);
+
+        /* FIXME */
+        msleep(10);
+
+        /* PS mode b->d, reset PLLA and PLLB */
+        hdmi_write(hdmi, 0x4C, HDMI_SYSTEM_CTRL);
+
+        udelay(10);
+
+        hdmi_write(hdmi, 0x40, HDMI_SYSTEM_CTRL);
+}
+
+static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
+{
+        struct fb_var_screeninfo *var = &hdmi->var;
+        struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+        struct fb_videomode *lcd_cfg = &pdata->lcd_chan->lcd_cfg;
+        unsigned long height = var->height, width = var->width;
+        int i;
+        u8 edid[128];
+
+        /* Read EDID */
+        pr_debug("Read back EDID code:");
+        for (i = 0; i < 128; i++) {
+                edid[i] = hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
+#ifdef DEBUG
+                if ((i % 16) == 0) {
+                        printk(KERN_CONT "\n");
+                        printk(KERN_DEBUG "%02X | %02X", i, edid[i]);
+                } else {
+                        printk(KERN_CONT " %02X", edid[i]);
+                }
+#endif
+        }
+#ifdef DEBUG
+        printk(KERN_CONT "\n");
+#endif
+        fb_parse_edid(edid, var);
+        pr_debug("%u-%u-%u-%u x %u-%u-%u-%u @ %lu kHz monitor detected\n",
+                 var->left_margin, var->xres, var->right_margin, var->hsync_len,
+                 var->upper_margin, var->yres, var->lower_margin, var->vsync_len,
+                 PICOS2KHZ(var->pixclock));
+
+        /* FIXME: Use user-provided configuration instead of EDID */
+        var->width              = width;
+        var->xres               = lcd_cfg->xres;
+        var->xres_virtual       = lcd_cfg->xres;
+        var->left_margin        = lcd_cfg->left_margin;
+        var->right_margin       = lcd_cfg->right_margin;
+        var->hsync_len          = lcd_cfg->hsync_len;
+        var->height             = height;
+        var->yres               = lcd_cfg->yres;
+        var->yres_virtual       = lcd_cfg->yres * 2;
+        var->upper_margin       = lcd_cfg->upper_margin;
+        var->lower_margin       = lcd_cfg->lower_margin;
+        var->vsync_len          = lcd_cfg->vsync_len;
+        var->sync               = lcd_cfg->sync;
+        var->pixclock           = lcd_cfg->pixclock;
+
+        hdmi_external_video_param(hdmi);
+}
+
+static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
+{
+        struct sh_hdmi *hdmi = dev_id;
+        u8 status1, status2, mask1, mask2;
+
+        /* mode_b and PLLA and PLLB reset */
+        hdmi_write(hdmi, 0x2C, HDMI_SYSTEM_CTRL);
+
+        /* How long shall reset be held? */
+        udelay(10);
+
+        /* mode_b and PLLA and PLLB reset release */
+        hdmi_write(hdmi, 0x20, HDMI_SYSTEM_CTRL);
+
+        status1 = hdmi_read(hdmi, HDMI_INTERRUPT_STATUS_1);
+        status2 = hdmi_read(hdmi, HDMI_INTERRUPT_STATUS_2);
+
+        mask1 = hdmi_read(hdmi, HDMI_INTERRUPT_MASK_1);
+        mask2 = hdmi_read(hdmi, HDMI_INTERRUPT_MASK_2);
+
+        /* Correct would be to ack only set bits, but the datasheet requires 0xff */
+        hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_1);
+        hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_2);
+
+        if (printk_ratelimit())
+                pr_debug("IRQ #%d: Status #1: 0x%x & 0x%x, #2: 0x%x & 0x%x\n",
+                         irq, status1, mask1, status2, mask2);
+
+        if (!((status1 & mask1) | (status2 & mask2))) {
+                return IRQ_NONE;
+        } else if (status1 & 0xc0) {
+                u8 msens;
+
+                /* Datasheet specifies 10ms... */
+                udelay(500);
+
+                msens = hdmi_read(hdmi, HDMI_HOT_PLUG_MSENS_STATUS);
+                pr_debug("MSENS 0x%x\n", msens);
+                /* Check, if hot plug & MSENS pin status are both high */
+                if ((msens & 0xC0) == 0xC0) {
+                        /* Display plug in */
+                        hdmi->hp_state = HDMI_HOTPLUG_CONNECTED;
+
+                        /* Set EDID word address  */
+                        hdmi_write(hdmi, 0x00, HDMI_EDID_WORD_ADDRESS);
+                        /* Set EDID segment pointer */
+                        hdmi_write(hdmi, 0x00, HDMI_EDID_SEGMENT_POINTER);
+                        /* Enable EDID interrupt */
+                        hdmi_write(hdmi, 0xC6, HDMI_INTERRUPT_MASK_1);
+                } else if (!(status1 & 0x80)) {
+                        /* Display unplug, beware multiple interrupts */
+                        if (hdmi->hp_state != HDMI_HOTPLUG_DISCONNECTED)
+                                schedule_delayed_work(&hdmi->edid_work, 0);
+
+                        hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
+                        /* display_off will switch back to mode_a */
+                }
+        } else if (status1 & 2) {
+                /* EDID error interrupt: retry */
+                /* Set EDID word address  */
+                hdmi_write(hdmi, 0x00, HDMI_EDID_WORD_ADDRESS);
+                /* Set EDID segment pointer */
+                hdmi_write(hdmi, 0x00, HDMI_EDID_SEGMENT_POINTER);
+        } else if (status1 & 4) {
+                /* Disable EDID interrupt */
+                hdmi_write(hdmi, 0xC0, HDMI_INTERRUPT_MASK_1);
+                hdmi->hp_state = HDMI_HOTPLUG_EDID_DONE;
+                schedule_delayed_work(&hdmi->edid_work, msecs_to_jiffies(10));
+        }
+
+        return IRQ_HANDLED;
+}
+
+static void hdmi_display_on(void *arg, struct fb_info *info)
+{
+        struct sh_hdmi *hdmi = arg;
+        struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+
+        if (info->var.xres != 1280 || info->var.yres != 720) {
+                dev_warn(info->device, "Unsupported framebuffer geometry %ux%u\n",
+                         info->var.xres, info->var.yres);
+                return;
+        }
+
+        pr_debug("%s(%p): state %x\n", __func__, pdata->lcd_dev, info->state);
+        /*
+         * FIXME: not a good place to store fb_info. And we cannot nullify it
+         * even on monitor disconnect. What should the lifecycle be?
+         */
+        hdmi->info = info;
+        switch (hdmi->hp_state) {
+        case HDMI_HOTPLUG_EDID_DONE:
+                /* PS mode d->e. All functions are active */
+                hdmi_write(hdmi, 0x80, HDMI_SYSTEM_CTRL);
+                pr_debug("HDMI running\n");
+                break;
+        case HDMI_HOTPLUG_DISCONNECTED:
+                info->state = FBINFO_STATE_SUSPENDED;
+        default:
+                hdmi->var = info->var;
+        }
+}
+
+static void hdmi_display_off(void *arg)
+{
+        struct sh_hdmi *hdmi = arg;
+        struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+
+        pr_debug("%s(%p)\n", __func__, pdata->lcd_dev);
+        /* PS mode e->a */
+        hdmi_write(hdmi, 0x10, HDMI_SYSTEM_CTRL);
+}
+
+/* Hotplug interrupt occurred, read EDID */
+static void edid_work_fn(struct work_struct *work)
+{
+        struct sh_hdmi *hdmi = container_of(work, struct sh_hdmi, edid_work.work);
+        struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+
+        pr_debug("%s(%p): begin, hotplug status %d\n", __func__,
+                 pdata->lcd_dev, hdmi->hp_state);
+
+        if (!pdata->lcd_dev)
+                return;
+
+        if (hdmi->hp_state == HDMI_HOTPLUG_EDID_DONE) {
+                pm_runtime_get_sync(hdmi->dev);
+                /* A device has been plugged in */
+                sh_hdmi_read_edid(hdmi);
+                msleep(10);
+                sh_hdmi_configure(hdmi);
+                /* Switched to another (d) power-save mode */
+                msleep(10);
+
+                if (!hdmi->info)
+                        return;
+
+                acquire_console_sem();
+
+                /* HDMI plug in */
+                hdmi->info->var = hdmi->var;
+                if (hdmi->info->state != FBINFO_STATE_RUNNING)
+                        fb_set_suspend(hdmi->info, 0);
+                else
+                        hdmi_display_on(hdmi, hdmi->info);
+
+                release_console_sem();
+        } else {
+                if (!hdmi->info)
+                        return;
+
+                acquire_console_sem();
+
+                /* HDMI disconnect */
+                fb_set_suspend(hdmi->info, 1);
+
+                release_console_sem();
+                pm_runtime_put(hdmi->dev);
+        }
+
+        pr_debug("%s(%p): end\n", __func__, pdata->lcd_dev);
+}
+
+static int __init sh_hdmi_probe(struct platform_device *pdev)
+{
+        struct sh_mobile_hdmi_info *pdata = pdev->dev.platform_data;
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        int irq = platform_get_irq(pdev, 0), ret;
+        struct sh_hdmi *hdmi;
+        long rate;
+
+        if (!res || !pdata || irq < 0)
+                return -ENODEV;
+
+        hdmi = kzalloc(sizeof(*hdmi), GFP_KERNEL);
+        if (!hdmi) {
+                dev_err(&pdev->dev, "Cannot allocate device data\n");
+                return -ENOMEM;
+        }
+
+        ret =  snd_soc_register_codec(&pdev->dev,
+                        &soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
+        if (ret < 0)
+                goto esndreg;
+
+        hdmi->dev = &pdev->dev;
+
+        hdmi->hdmi_clk = clk_get(&pdev->dev, "ick");
+        if (IS_ERR(hdmi->hdmi_clk)) {
+                ret = PTR_ERR(hdmi->hdmi_clk);
+                dev_err(&pdev->dev, "Unable to get clock: %d\n", ret);
+                goto egetclk;
+        }
+
+        rate = PICOS2KHZ(pdata->lcd_chan->lcd_cfg.pixclock) * 1000;
+
+        rate = clk_round_rate(hdmi->hdmi_clk, rate);
+        if (rate < 0) {
+                ret = rate;
+                dev_err(&pdev->dev, "Cannot get suitable rate: %ld\n", rate);
+                goto erate;
+        }
+
+        ret = clk_set_rate(hdmi->hdmi_clk, rate);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "Cannot set rate %ld: %d\n", rate, ret);
+                goto erate;
+        }
+
+        pr_debug("HDMI set frequency %lu\n", rate);
+
+        ret = clk_enable(hdmi->hdmi_clk);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "Cannot enable clock: %d\n", ret);
+                goto eclkenable;
+        }
+
+        dev_info(&pdev->dev, "Enabled HDMI clock at %luHz\n", rate);
+
+        if (!request_mem_region(res->start, resource_size(res), dev_name(&pdev->dev))) {
+                dev_err(&pdev->dev, "HDMI register region already claimed\n");
+                ret = -EBUSY;
+                goto ereqreg;
+        }
+
+        hdmi->base = ioremap(res->start, resource_size(res));
+        if (!hdmi->base) {
+                dev_err(&pdev->dev, "HDMI register region already claimed\n");
+                ret = -ENOMEM;
+                goto emap;
+        }
+
+        platform_set_drvdata(pdev, hdmi);
+
+#if 1
+        /* Product and revision IDs are 0 in sh-mobile version */
+        dev_info(&pdev->dev, "Detected HDMI controller 0x%x:0x%x\n",
+                 hdmi_read(hdmi, HDMI_PRODUCT_ID), hdmi_read(hdmi, HDMI_REVISION_ID));
+#endif
+
+        /* Set up LCDC callbacks */
+        pdata->lcd_chan->board_cfg.board_data = hdmi;
+        pdata->lcd_chan->board_cfg.display_on = hdmi_display_on;
+        pdata->lcd_chan->board_cfg.display_off = hdmi_display_off;
+
+        INIT_DELAYED_WORK(&hdmi->edid_work, edid_work_fn);
+
+        pm_runtime_enable(&pdev->dev);
+        pm_runtime_resume(&pdev->dev);
+
+        ret = request_irq(irq, sh_hdmi_hotplug, 0,
+                          dev_name(&pdev->dev), hdmi);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "Unable to request irq: %d\n", ret);
+                goto ereqirq;
+        }
+
+        return 0;
+
+ereqirq:
+        pm_runtime_disable(&pdev->dev);
+        iounmap(hdmi->base);
+emap:
+        release_mem_region(res->start, resource_size(res));
+ereqreg:
+        clk_disable(hdmi->hdmi_clk);
+eclkenable:
+erate:
+        clk_put(hdmi->hdmi_clk);
+egetclk:
+        snd_soc_unregister_codec(&pdev->dev);
+esndreg:
+        kfree(hdmi);
+
+        return ret;
+}
+
+static int __exit sh_hdmi_remove(struct platform_device *pdev)
+{
+        struct sh_mobile_hdmi_info *pdata = pdev->dev.platform_data;
+        struct sh_hdmi *hdmi = platform_get_drvdata(pdev);
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        int irq = platform_get_irq(pdev, 0);
+
+        snd_soc_unregister_codec(&pdev->dev);
+
+        pdata->lcd_chan->board_cfg.display_on = NULL;
+        pdata->lcd_chan->board_cfg.display_off = NULL;
+        pdata->lcd_chan->board_cfg.board_data = NULL;
+
+        free_irq(irq, hdmi);
+        pm_runtime_disable(&pdev->dev);
+        cancel_delayed_work_sync(&hdmi->edid_work);
+        clk_disable(hdmi->hdmi_clk);
+        clk_put(hdmi->hdmi_clk);
+        iounmap(hdmi->base);
+        release_mem_region(res->start, resource_size(res));
+        kfree(hdmi);
+
+        return 0;
+}
+
+static struct platform_driver sh_hdmi_driver = {
+        .remove         = __exit_p(sh_hdmi_remove),
+        .driver = {
+                .name   = "sh-mobile-hdmi",
+        },
+};
+
+static int __init sh_hdmi_init(void)
+{
+        return platform_driver_probe(&sh_hdmi_driver, sh_hdmi_probe);
+}
+module_init(sh_hdmi_init);
+
+static void __exit sh_hdmi_exit(void)
+{
+        platform_driver_unregister(&sh_hdmi_driver);
+}
+module_exit(sh_hdmi_exit);
+
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_DESCRIPTION("SuperH / ARM-shmobile HDMI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index 12c451a711e9..7a1419279c8f 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -56,6 +56,7 @@ static int lcdc_shared_regs[] = {
 /* per-channel registers */
 enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
        LDSM2R, LDSA1R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR,
+       LDHAJR,
        NR_CH_REGS };
 
 static unsigned long lcdc_offs_mainlcd[NR_CH_REGS] = {
@@ -74,6 +75,7 @@ static unsigned long lcdc_offs_mainlcd[NR_CH_REGS] = {
         [LDVLNR] = 0x450,
         [LDVSYNR] = 0x454,
         [LDPMR] = 0x460,
+        [LDHAJR] = 0x4a0,
 };
 
 static unsigned long lcdc_offs_sublcd[NR_CH_REGS] = {
@@ -137,6 +139,7 @@ struct sh_mobile_lcdc_priv {
         struct clk *dot_clk;
         unsigned long lddckr;
         struct sh_mobile_lcdc_chan ch[2];
+        struct notifier_block notifier;
         unsigned long saved_shared_regs[NR_SHARED_REGS];
         int started;
 };
@@ -404,6 +407,56 @@ static void sh_mobile_lcdc_start_stop(struct sh_mobile_lcdc_priv *priv,
                 lcdc_write(priv, _LDDCKSTPR, 1); /* stop dotclock */
 }
 
+static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
+{
+        struct fb_var_screeninfo *var = &ch->info->var;
+        unsigned long h_total, hsync_pos;
+        u32 tmp;
+
+        tmp = ch->ldmt1r_value;
+        tmp |= (var->sync & FB_SYNC_VERT_HIGH_ACT) ? 0 : 1 << 28;
+        tmp |= (var->sync & FB_SYNC_HOR_HIGH_ACT) ? 0 : 1 << 27;
+        tmp |= (ch->cfg.flags & LCDC_FLAGS_DWPOL) ? 1 << 26 : 0;
+        tmp |= (ch->cfg.flags & LCDC_FLAGS_DIPOL) ? 1 << 25 : 0;
+        tmp |= (ch->cfg.flags & LCDC_FLAGS_DAPOL) ? 1 << 24 : 0;
+        tmp |= (ch->cfg.flags & LCDC_FLAGS_HSCNT) ? 1 << 17 : 0;
+        tmp |= (ch->cfg.flags & LCDC_FLAGS_DWCNT) ? 1 << 16 : 0;
+        lcdc_write_chan(ch, LDMT1R, tmp);
+
+        /* setup SYS bus */
+        lcdc_write_chan(ch, LDMT2R, ch->cfg.sys_bus_cfg.ldmt2r);
+        lcdc_write_chan(ch, LDMT3R, ch->cfg.sys_bus_cfg.ldmt3r);
+
+        /* horizontal configuration */
+        h_total = var->xres + var->hsync_len +
+                var->left_margin + var->right_margin;
+        tmp = h_total / 8; /* HTCN */
+        tmp |= (var->xres / 8) << 16; /* HDCN */
+        lcdc_write_chan(ch, LDHCNR, tmp);
+
+        hsync_pos = var->xres + var->right_margin;
+        tmp = hsync_pos / 8; /* HSYNP */
+        tmp |= (var->hsync_len / 8) << 16; /* HSYNW */
+        lcdc_write_chan(ch, LDHSYNR, tmp);
+
+        /* vertical configuration */
+        tmp = var->yres + var->vsync_len +
+                var->upper_margin + var->lower_margin; /* VTLN */
+        tmp |= var->yres << 16; /* VDLN */
+        lcdc_write_chan(ch, LDVLNR, tmp);
+
+        tmp = var->yres + var->lower_margin; /* VSYNP */
+        tmp |= var->vsync_len << 16; /* VSYNW */
+        lcdc_write_chan(ch, LDVSYNR, tmp);
+
+        /* Adjust horizontal synchronisation for HDMI */
+        tmp = ((var->xres & 7) << 24) |
+                ((h_total & 7) << 16) |
+                ((var->hsync_len & 7) << 8) |
+                hsync_pos;
+        lcdc_write_chan(ch, LDHAJR, tmp);
+}
+
 static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 {
         struct sh_mobile_lcdc_chan *ch;
@@ -470,49 +523,11 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
                 if (!ch->enabled)
                         continue;
 
-                tmp = ch->ldmt1r_value;
-                tmp |= (lcd_cfg->sync & FB_SYNC_VERT_HIGH_ACT) ? 0 : 1 << 28;
-                tmp |= (lcd_cfg->sync & FB_SYNC_HOR_HIGH_ACT) ? 0 : 1 << 27;
-                tmp |= (ch->cfg.flags & LCDC_FLAGS_DWPOL) ? 1 << 26 : 0;
-                tmp |= (ch->cfg.flags & LCDC_FLAGS_DIPOL) ? 1 << 25 : 0;
-                tmp |= (ch->cfg.flags & LCDC_FLAGS_DAPOL) ? 1 << 24 : 0;
-                tmp |= (ch->cfg.flags & LCDC_FLAGS_HSCNT) ? 1 << 17 : 0;
-                tmp |= (ch->cfg.flags & LCDC_FLAGS_DWCNT) ? 1 << 16 : 0;
-                lcdc_write_chan(ch, LDMT1R, tmp);
-
-                /* setup SYS bus */
-                lcdc_write_chan(ch, LDMT2R, ch->cfg.sys_bus_cfg.ldmt2r);
-                lcdc_write_chan(ch, LDMT3R, ch->cfg.sys_bus_cfg.ldmt3r);
-
-                /* horizontal configuration */
-                tmp = lcd_cfg->xres + lcd_cfg->hsync_len;
-                tmp += lcd_cfg->left_margin;
-                tmp += lcd_cfg->right_margin;
-                tmp /= 8; /* HTCN */
-                tmp |= (lcd_cfg->xres / 8) << 16; /* HDCN */
-                lcdc_write_chan(ch, LDHCNR, tmp);
-
-                tmp = lcd_cfg->xres;
-                tmp += lcd_cfg->right_margin;
-                tmp /= 8; /* HSYNP */
-                tmp |= (lcd_cfg->hsync_len / 8) << 16; /* HSYNW */
-                lcdc_write_chan(ch, LDHSYNR, tmp);
+                sh_mobile_lcdc_geometry(ch);
 
                 /* power supply */
                 lcdc_write_chan(ch, LDPMR, 0);
 
-                /* vertical configuration */
-                tmp = lcd_cfg->yres + lcd_cfg->vsync_len;
-                tmp += lcd_cfg->upper_margin;
-                tmp += lcd_cfg->lower_margin; /* VTLN */
-                tmp |= lcd_cfg->yres << 16; /* VDLN */
-                lcdc_write_chan(ch, LDVLNR, tmp);
-
-                tmp = lcd_cfg->yres;
-                tmp += lcd_cfg->lower_margin; /* VSYNP */
-                tmp |= lcd_cfg->vsync_len << 16; /* VSYNW */
-                lcdc_write_chan(ch, LDVSYNR, tmp);
-
                 board_cfg = &ch->cfg.board_cfg;
                 if (board_cfg->setup_sys)
                         ret = board_cfg->setup_sys(board_cfg->board_data, ch,
@@ -577,7 +592,7 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
                 board_cfg = &ch->cfg.board_cfg;
                 if (board_cfg->display_on)
-                        board_cfg->display_on(board_cfg->board_data);
+                        board_cfg->display_on(board_cfg->board_data, ch->info);
         }
 
         return 0;
@@ -943,6 +958,62 @@ static const struct dev_pm_ops sh_mobile_lcdc_dev_pm_ops = {
         .runtime_resume = sh_mobile_lcdc_runtime_resume,
 };
 
+static int sh_mobile_lcdc_notify(struct notifier_block *nb,
+                                 unsigned long action, void *data)
+{
+        struct fb_event *event = data;
+        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;
+        struct fb_var_screeninfo *var;
+
+        if (&ch->lcdc->notifier != nb)
+                return 0;
+
+        dev_dbg(info->dev, "%s(): action = %lu, data = %p\n",
+                __func__, action, event->data);
+
+        switch(action) {
+        case FB_EVENT_SUSPEND:
+                if (board_cfg->display_off)
+                        board_cfg->display_off(board_cfg->board_data);
+                pm_runtime_put(info->device);
+                break;
+        case FB_EVENT_RESUME:
+                var = &info->var;
+
+                /* HDMI must be enabled before LCDC configuration */
+                if (board_cfg->display_on)
+                        board_cfg->display_on(board_cfg->board_data, ch->info);
+
+                /* Check if the new display is not in our modelist */
+                if (ch->info->modelist.next &&
+                    !fb_match_mode(var, &ch->info->modelist)) {
+                        struct fb_videomode mode;
+                        int ret;
+
+                        /* Can we handle this display? */
+                        if (var->xres > ch->cfg.lcd_cfg.xres ||
+                            var->yres > ch->cfg.lcd_cfg.yres)
+                                return -ENOMEM;
+
+                        /* Add to the modelist */
+                        fb_var_to_videomode(&mode, var);
+                        ret = fb_add_videomode(&mode, &ch->info->modelist);
+                        if (ret < 0)
+                                return ret;
+                }
+
+                pm_runtime_get_sync(info->device);
+
+                sh_mobile_lcdc_geometry(ch);
+
+                break;
+        }
+
+        return 0;
+}
+
 static int sh_mobile_lcdc_remove(struct platform_device *pdev);
 
 static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
@@ -1020,15 +1091,19 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                 goto err1;
         }
 
+        priv->base = ioremap_nocache(res->start, resource_size(res));
+        if (!priv->base)
+                goto err1;
+
         error = sh_mobile_lcdc_setup_clocks(pdev, pdata->clock_source, priv);
         if (error) {
                 dev_err(&pdev->dev, "unable to setup clocks\n");
                 goto err1;
         }
 
-        priv->base = ioremap_nocache(res->start, (res->end - res->start) + 1);
-
         for (i = 0; i < j; i++) {
+                struct fb_var_screeninfo *var;
+                struct fb_videomode *lcd_cfg;
                 cfg = &priv->ch[i].cfg;
 
                 priv->ch[i].info = framebuffer_alloc(0, &pdev->dev);
@@ -1039,22 +1114,33 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                 }
 
                 info = priv->ch[i].info;
+                var = &info->var;
+                lcd_cfg = &cfg->lcd_cfg;
                 info->fbops = &sh_mobile_lcdc_ops;
-                info->var.xres = info->var.xres_virtual = cfg->lcd_cfg.xres;
-                info->var.yres = cfg->lcd_cfg.yres;
+                var->xres = var->xres_virtual = lcd_cfg->xres;
+                var->yres = lcd_cfg->yres;
                 /* Default Y virtual resolution is 2x panel size */
-                info->var.yres_virtual = info->var.yres * 2;
-                info->var.width = cfg->lcd_size_cfg.width;
-                info->var.height = cfg->lcd_size_cfg.height;
-                info->var.activate = FB_ACTIVATE_NOW;
-                error = sh_mobile_lcdc_set_bpp(&info->var, cfg->bpp);
+                var->yres_virtual = var->yres * 2;
+                var->width = cfg->lcd_size_cfg.width;
+                var->height = cfg->lcd_size_cfg.height;
+                var->activate = FB_ACTIVATE_NOW;
+                var->left_margin = lcd_cfg->left_margin;
+                var->right_margin = lcd_cfg->right_margin;
+                var->upper_margin = lcd_cfg->upper_margin;
+                var->lower_margin = lcd_cfg->lower_margin;
+                var->hsync_len = lcd_cfg->hsync_len;
+                var->vsync_len = lcd_cfg->vsync_len;
+                var->sync = lcd_cfg->sync;
+                var->pixclock = lcd_cfg->pixclock;
+
+                error = sh_mobile_lcdc_set_bpp(var, cfg->bpp);
                 if (error)
                         break;
 
                 info->fix = sh_mobile_lcdc_fix;
-                info->fix.line_length = cfg->lcd_cfg.xres * (cfg->bpp / 8);
+                info->fix.line_length = lcd_cfg->xres * (cfg->bpp / 8);
                 info->fix.smem_len = info->fix.line_length *
-                        info->var.yres_virtual;
+                        var->yres_virtual;
 
                 buf = dma_alloc_coherent(&pdev->dev, info->fix.smem_len,
                                          &priv->ch[i].dma_handle, GFP_KERNEL);
@@ -1119,10 +1205,14 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                          ch->cfg.bpp);
 
                 /* deferred io mode: disable clock to save power */
-                if (info->fbdefio)
+                if (info->fbdefio || info->state == FBINFO_STATE_SUSPENDED)
                         sh_mobile_lcdc_clk_off(priv);
         }
 
+        /* Failure ignored */
+        priv->notifier.notifier_call = sh_mobile_lcdc_notify;
+        fb_register_client(&priv->notifier);
+
         return 0;
 err1:
         sh_mobile_lcdc_remove(pdev);
@@ -1136,6 +1226,8 @@ static int sh_mobile_lcdc_remove(struct platform_device *pdev)
         struct fb_info *info;
         int i;
 
+        fb_unregister_client(&priv->notifier);
+
         for (i = 0; i < ARRAY_SIZE(priv->ch); i++)
                 if (priv->ch[i].info && priv->ch[i].info->dev)
                         unregister_framebuffer(priv->ch[i].info);
@@ -1151,8 +1243,10 @@ static int sh_mobile_lcdc_remove(struct platform_device *pdev)
                 if (priv->ch[i].sglist)
                         vfree(priv->ch[i].sglist);
 
-                dma_free_coherent(&pdev->dev, info->fix.smem_len,
-                                  info->screen_base, priv->ch[i].dma_handle);
+                if (info->screen_base)
+                        dma_free_coherent(&pdev->dev, info->fix.smem_len,
+                                          info->screen_base,
+                                          priv->ch[i].dma_handle);
                 fb_dealloc_cmap(&info->cmap);
                 framebuffer_release(info);
         }
diff --git a/drivers/video/sis/sis_main.c b/drivers/video/sis/sis_main.c
index 559bf1727a2b..b52f8e4ef1fd 100644
--- a/drivers/video/sis/sis_main.c
+++ b/drivers/video/sis/sis_main.c
@@ -1701,6 +1701,9 @@ static int	sisfb_ioctl(struct fb_info *info, unsigned int cmd,
                 break;
 
            case FBIOGET_VBLANK:
+
+                memset(&sisvbblank, 0, sizeof(struct fb_vblank));
+
                 sisvbblank.count = 0;
                 sisvbblank.flags = sisfb_setupvbblankflags(ivideo, &sisvbblank.vcount, &sisvbblank.hcount);
 
diff --git a/drivers/video/sunxvr1000.c b/drivers/video/sunxvr1000.c
index 489b44e8db81..5dbe06af2226 100644
--- a/drivers/video/sunxvr1000.c
+++ b/drivers/video/sunxvr1000.c
@@ -111,7 +111,7 @@ static int __devinit gfb_set_fbinfo(struct gfb_info *gp)
         return 0;
 }
 
-static int __devinit gfb_probe(struct of_device *op,
+static int __devinit gfb_probe(struct platform_device *op,
                                const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
@@ -172,7 +172,7 @@ err_out:
         return err;
 }
 
-static int __devexit gfb_remove(struct of_device *op)
+static int __devexit gfb_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct gfb_info *gp = info->par;
@@ -213,12 +213,12 @@ static int __init gfb_init(void)
         if (fb_get_options("gfb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&gfb_driver, &of_bus_type);
+        return of_register_platform_driver(&gfb_driver);
 }
 
 static void __exit gfb_exit(void)
 {
-        of_unregister_driver(&gfb_driver);
+        of_unregister_platform_driver(&gfb_driver);
 }
 
 module_init(gfb_init);
diff --git a/drivers/video/tcx.c b/drivers/video/tcx.c
index cc039b33d2d8..77ad27955cf0 100644
--- a/drivers/video/tcx.c
+++ b/drivers/video/tcx.c
@@ -342,7 +342,7 @@ tcx_init_fix(struct fb_info *info, int linebytes)
         info->fix.accel = FB_ACCEL_SUN_TCX;
 }
 
-static void tcx_unmap_regs(struct of_device *op, struct fb_info *info,
+static void tcx_unmap_regs(struct platform_device *op, struct fb_info *info,
                            struct tcx_par *par)
 {
         if (par->tec)
@@ -362,7 +362,7 @@ static void tcx_unmap_regs(struct of_device *op, struct fb_info *info,
                            info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit tcx_probe(struct of_device *op,
+static int __devinit tcx_probe(struct platform_device *op,
                                const struct of_device_id *match)
 {
         struct device_node *dp = op->dev.of_node;
@@ -486,7 +486,7 @@ out_err:
         return err;
 }
 
-static int __devexit tcx_remove(struct of_device *op)
+static int __devexit tcx_remove(struct platform_device *op)
 {
         struct fb_info *info = dev_get_drvdata(&op->dev);
         struct tcx_par *par = info->par;
@@ -526,12 +526,12 @@ static int __init tcx_init(void)
         if (fb_get_options("tcxfb", NULL))
                 return -ENODEV;
 
-        return of_register_driver(&tcx_driver, &of_bus_type);
+        return of_register_platform_driver(&tcx_driver);
 }
 
 static void __exit tcx_exit(void)
 {
-        of_unregister_driver(&tcx_driver);
+        of_unregister_platform_driver(&tcx_driver);
 }
 
 module_init(tcx_init);
diff --git a/drivers/video/tdfxfb.c b/drivers/video/tdfxfb.c
index 980548390048..3ee5e63cfa4f 100644
--- a/drivers/video/tdfxfb.c
+++ b/drivers/video/tdfxfb.c
@@ -1571,8 +1571,8 @@ out_err_iobase:
         if (default_par->mtrr_handle >= 0)
                 mtrr_del(default_par->mtrr_handle, info->fix.smem_start,
                          info->fix.smem_len);
-        release_mem_region(pci_resource_start(pdev, 2),
-                           pci_resource_len(pdev, 2));
+        release_region(pci_resource_start(pdev, 2),
+                       pci_resource_len(pdev, 2));
 out_err_screenbase:
         if (info->screen_base)
                 iounmap(info->screen_base);
diff --git a/drivers/video/tgafb.c b/drivers/video/tgafb.c
index 1b3b1c718e80..aba7686b1a32 100644
--- a/drivers/video/tgafb.c
+++ b/drivers/video/tgafb.c
@@ -305,7 +305,7 @@ tgafb_set_par(struct fb_info *info)
         TGA_WRITE_REG(par, htimings, TGA_HORIZ_REG);
         TGA_WRITE_REG(par, vtimings, TGA_VERT_REG);
 
-        /* Initalise RAMDAC. */
+        /* Initialise RAMDAC. */
         if (tga_type == TGA_TYPE_8PLANE && tga_bus_pci) {
 
                 /* Init BT485 RAMDAC registers.  */
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index 7b8839ebf3c4..52ec0959d462 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -1977,8 +1977,7 @@ static void __devexit uvesafb_exit(void)
 
 module_exit(uvesafb_exit);
 
-#define param_get_scroll NULL
-static int param_set_scroll(const char *val, struct kernel_param *kp)
+static int param_set_scroll(const char *val, const struct kernel_param *kp)
 {
         ypan = 0;
 
@@ -1993,7 +1992,9 @@ static int param_set_scroll(const char *val, struct kernel_param *kp)
 
         return 0;
 }
-
+static struct kernel_param_ops param_ops_scroll = {
+        .set = param_set_scroll,
+};
 #define param_check_scroll(name, p) __param_check(name, p, void)
 
 module_param_named(scroll, ypan, scroll, 0);
diff --git a/drivers/video/vesafb.c b/drivers/video/vesafb.c
index 090aa1a9be6e..6a069d047914 100644
--- a/drivers/video/vesafb.c
+++ b/drivers/video/vesafb.c
@@ -253,7 +253,7 @@ static int __init vesafb_probe(struct platform_device *dev)
         size_vmode = vesafb_defined.yres * vesafb_fix.line_length;
 
         /*   size_total -- all video memory we have. Used for mtrr
-         *                 entries, ressource allocation and bounds
+         *                 entries, resource allocation and bounds
          *                 checking. */
         size_total = screen_info.lfb_size * 65536;
         if (vram_total)
diff --git a/drivers/video/vt8623fb.c b/drivers/video/vt8623fb.c
index d31dc96f838a..85d76ec4c63e 100644
--- a/drivers/video/vt8623fb.c
+++ b/drivers/video/vt8623fb.c
@@ -726,7 +726,9 @@ static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_devi
 
         /* Prepare startup mode */
 
+        kparam_block_sysfs_write(mode_option);
         rc = fb_find_mode(&(info->var), info, mode_option, NULL, 0, NULL, 8);
+        kparam_unblock_sysfs_write(mode_option);
         if (! ((rc == 1) || (rc == 2))) {
                 rc = -EINVAL;
                 dev_err(info->device, "mode %s not found\n", mode_option);
diff --git a/drivers/video/w100fb.c b/drivers/video/w100fb.c
index e66b8b19ce5d..d8b12c32e3ef 100644
--- a/drivers/video/w100fb.c
+++ b/drivers/video/w100fb.c
@@ -858,9 +858,9 @@ unsigned long w100fb_gpio_read(int port)
 void w100fb_gpio_write(int port, unsigned long value)
 {
         if (port==W100_GPIO_PORT_A)
-                value = writel(value, remapped_regs + mmGPIO_DATA);
+                writel(value, remapped_regs + mmGPIO_DATA);
         else
-                value = writel(value, remapped_regs + mmGPIO_DATA2);
+                writel(value, remapped_regs + mmGPIO_DATA2);
 }
 EXPORT_SYMBOL(w100fb_gpio_read);
 EXPORT_SYMBOL(w100fb_gpio_write);
diff --git a/drivers/video/xen-fbfront.c b/drivers/video/xen-fbfront.c
index fa97d3e7c21a..7c7f42a12796 100644
--- a/drivers/video/xen-fbfront.c
+++ b/drivers/video/xen-fbfront.c
@@ -684,7 +684,7 @@ static struct xenbus_driver xenfb_driver = {
 
 static int __init xenfb_init(void)
 {
-        if (!xen_domain())
+        if (!xen_pv_domain())
                 return -ENODEV;
 
         /* Nothing to do if running in dom0. */
diff --git a/drivers/video/xilinxfb.c b/drivers/video/xilinxfb.c
index 574dc54e12d4..68bd23476c64 100644
--- a/drivers/video/xilinxfb.c
+++ b/drivers/video/xilinxfb.c
@@ -32,10 +32,14 @@
 #include <linux/dma-mapping.h>
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
+#include <linux/of_address.h>
 #include <linux/io.h>
 #include <linux/xilinxfb.h>
 #include <linux/slab.h>
+
+#ifdef CONFIG_PPC_DCR
 #include <asm/dcr.h>
+#endif
 
 #define DRIVER_NAME             "xilinxfb"
 
@@ -123,10 +127,10 @@ struct xilinxfb_drvdata {
                                                 registers */
         void __iomem    *regs;          /* virt. address of the control
                                                 registers */
-
+#ifdef CONFIG_PPC_DCR
         dcr_host_t      dcr_host;
         unsigned int    dcr_len;
-
+#endif
         void            *fb_virt;       /* virt. address of the frame buffer */
         dma_addr_t      fb_phys;        /* phys. address of the frame buffer */
         int             fb_alloced;     /* Flag, was the fb memory alloced? */
@@ -152,9 +156,10 @@ static void xilinx_fb_out_be32(struct xilinxfb_drvdata *drvdata, u32 offset,
 {
         if (drvdata->flags & PLB_ACCESS_FLAG)
                 out_be32(drvdata->regs + (offset << 2), val);
+#ifdef CONFIG_PPC_DCR
         else
                 dcr_write(drvdata->dcr_host, offset, val);
-
+#endif
 }
 
 static int
@@ -383,8 +388,11 @@ static int xilinxfb_release(struct device *dev)
         if (drvdata->flags & PLB_ACCESS_FLAG) {
                 iounmap(drvdata->regs);
                 release_mem_region(drvdata->regs_phys, 8);
-        } else
+        }
+#ifdef CONFIG_PPC_DCR
+        else
                 dcr_unmap(drvdata->dcr_host, drvdata->dcr_len);
+#endif
 
         kfree(drvdata);
         dev_set_drvdata(dev, NULL);
@@ -397,14 +405,14 @@ static int xilinxfb_release(struct device *dev)
  */
 
 static int __devinit
-xilinxfb_of_probe(struct of_device *op, const struct of_device_id *match)
+xilinxfb_of_probe(struct platform_device *op, const struct of_device_id *match)
 {
         const u32 *prop;
         u32 *p;
         u32 tft_access;
         struct xilinxfb_platform_data pdata;
         struct resource res;
-        int size, rc, start;
+        int size, rc;
         struct xilinxfb_drvdata *drvdata;
 
         /* Copy with the default pdata (not a ptr reference!) */
@@ -437,7 +445,10 @@ xilinxfb_of_probe(struct of_device *op, const struct of_device_id *match)
                         dev_err(&op->dev, "invalid address\n");
                         goto err;
                 }
-        } else {
+        }
+#ifdef CONFIG_PPC_DCR
+        else {
+                int start;
                 res.start = 0;
                 start = dcr_resource_start(op->dev.of_node, 0);
                 drvdata->dcr_len = dcr_resource_len(op->dev.of_node, 0);
@@ -447,6 +458,7 @@ xilinxfb_of_probe(struct of_device *op, const struct of_device_id *match)
                         goto err;
                 }
         }
+#endif
 
         prop = of_get_property(op->dev.of_node, "phys-size", &size);
         if ((prop) && (size >= sizeof(u32)*2)) {
@@ -477,7 +489,7 @@ xilinxfb_of_probe(struct of_device *op, const struct of_device_id *match)
         return -ENODEV;
 }
 
-static int __devexit xilinxfb_of_remove(struct of_device *op)
+static int __devexit xilinxfb_of_remove(struct platform_device *op)
 {
         return xilinxfb_release(&op->dev);
 }
@@ -485,6 +497,8 @@ static int __devexit xilinxfb_of_remove(struct of_device *op)
 /* Match table for of_platform binding */
 static struct of_device_id xilinxfb_of_match[] __devinitdata = {
         { .compatible = "xlnx,xps-tft-1.00.a", },
+        { .compatible = "xlnx,xps-tft-2.00.a", },
+        { .compatible = "xlnx,xps-tft-2.01.a", },
         { .compatible = "xlnx,plb-tft-cntlr-ref-1.00.a", },
         { .compatible = "xlnx,plb-dvi-cntlr-ref-1.00.c", },
         {},