Merge branch 'mpi-master' into wip-k-fmlpwip-k-fmlp

Conflicts:
	litmus/sched_cedf.c

271 files changed, 29819 insertions, 11405 deletions

diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 8b31fdfefc98..549b960667c8 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -8,7 +8,11 @@ menu "Graphics support"
 config HAVE_FB_ATMEL
         bool
 
-config HAVE_FB_IMX
+config SH_MIPI_DSI
+        tristate
+        depends on (SUPERH || ARCH_SHMOBILE) && HAVE_CLK
+
+config SH_LCD_MIPI_DSI
         bool
 
 source "drivers/char/agp/Kconfig"
@@ -17,6 +21,8 @@ source "drivers/gpu/vga/Kconfig"
 
 source "drivers/gpu/drm/Kconfig"
 
+source "drivers/gpu/stub/Kconfig"
+
 config VGASTATE
        tristate
        default n
@@ -49,7 +55,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
@@ -184,6 +190,14 @@ config FB_SYS_FOPS
        depends on FB
        default n
 
+config FB_WMT_GE_ROPS
+        tristate
+        depends on FB
+        default n
+        ---help---
+          Include functions for accelerated rectangle filling and area
+          copying using WonderMedia Graphics Engine operations.
+
 config FB_DEFERRED_IO
         bool
         depends on FB
@@ -305,69 +319,6 @@ config FB_ARMCLCD
           here and read <file:Documentation/kbuild/modules.txt>.  The module
           will be called amba-clcd.
 
-choice
-
-        depends on FB_ARMCLCD && (ARCH_LH7A40X || ARCH_LH7952X)
-        prompt "LCD Panel"
-        default FB_ARMCLCD_SHARP_LQ035Q7DB02
-
-config FB_ARMCLCD_SHARP_LQ035Q7DB02_HRTFT
-        bool "LogicPD LCD 3.5\" QVGA w/HRTFT IC"
-        help
-          This is an implementation of the Sharp LQ035Q7DB02, a 3.5"
-          color QVGA, HRTFT panel.  The LogicPD device includes
-          an integrated HRTFT controller IC.
-          The native resolution is 240x320.
-
-config FB_ARMCLCD_SHARP_LQ057Q3DC02
-        bool "LogicPD LCD 5.7\" QVGA"
-        help
-          This is an implementation of the Sharp LQ057Q3DC02, a 5.7"
-          color QVGA, TFT panel.  The LogicPD device includes an
-          The native resolution is 320x240.
-
-config FB_ARMCLCD_SHARP_LQ64D343
-        bool "LogicPD LCD 6.4\" VGA"
-        help
-          This is an implementation of the Sharp LQ64D343, a 6.4"
-          color VGA, TFT panel.  The LogicPD device includes an
-          The native resolution is 640x480.
-
-config FB_ARMCLCD_SHARP_LQ10D368
-        bool "LogicPD LCD 10.4\" VGA"
-        help
-          This is an implementation of the Sharp LQ10D368, a 10.4"
-          color VGA, TFT panel.  The LogicPD device includes an
-          The native resolution is 640x480.
-
-
-config FB_ARMCLCD_SHARP_LQ121S1DG41
-        bool "LogicPD LCD 12.1\" SVGA"
-        help
-          This is an implementation of the Sharp LQ121S1DG41, a 12.1"
-          color SVGA, TFT panel.  The LogicPD device includes an
-          The native resolution is 800x600.
-
-          This panel requires a clock rate may be an integer fraction
-          of the base LCDCLK frequency.  The driver will select the
-          highest frequency available that is lower than the maximum
-          allowed.  The panel may flicker if the clock rate is
-          slower than the recommended minimum.
-
-config FB_ARMCLCD_AUO_A070VW01_WIDE
-        bool "AU Optronics A070VW01 LCD 7.0\" WIDE"
-        help
-          This is an implementation of the AU Optronics, a 7.0"
-          WIDE Color.  The native resolution is 234x480.
-
-config FB_ARMCLCD_HITACHI
-        bool "Hitachi Wide Screen 800x480"
-        help
-          This is an implementation of the Hitachi 800x480.
-
-endchoice
-
-
 config FB_ACORN
         bool "Acorn VIDC support"
         depends on (FB = y) && ARM && ARCH_ACORN
@@ -404,8 +355,8 @@ config FB_SA1100
           Y here.
 
 config FB_IMX
-        tristate "Motorola i.MX LCD support"
-        depends on FB && (HAVE_FB_IMX || ARCH_MX1 || ARCH_MX2)
+        tristate "Freescale i.MX LCD support"
+        depends on FB && IMX_HAVE_PLATFORM_IMX_FB
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
@@ -422,6 +373,24 @@ config FB_CYBER2000
           Say Y if you have a NetWinder or a graphics card containing this
           device, otherwise say N.
 
+config FB_CYBER2000_DDC
+        bool "DDC for CyberPro support"
+        depends on FB_CYBER2000
+        select FB_DDC
+        default y
+        help
+          Say Y here if you want DDC support for your CyberPro graphics
+          card. This is only I2C bus support, driver does not use EDID.
+
+config FB_CYBER2000_I2C
+        bool "CyberPro 2000/2010/5000 I2C support"
+        depends on FB_CYBER2000 && I2C && ARCH_NETWINDER
+        select I2C_ALGOBIT
+        help
+          Enable support for the I2C video decoder interface on the
+          Integraphics CyberPro 20x0 and 5000 VGA chips.  This is used
+          on the Netwinder machines for the SAA7111 video capture.
+
 config FB_APOLLO
         bool
         depends on (FB = y) && APOLLO
@@ -633,6 +602,72 @@ config FB_BFIN_LQ035Q1
           To compile this driver as a module, choose M here: the
           module will be called bfin-lq035q1-fb.
 
+config FB_BF537_LQ035
+        tristate "SHARP LQ035 TFT LCD (BF537 STAMP)"
+        depends on FB && (BF534 || BF536 || BF537) && I2C_BLACKFIN_TWI
+        select FB_CFB_FILLRECT
+        select FB_CFB_COPYAREA
+        select FB_CFB_IMAGEBLIT
+        select BFIN_GPTIMERS
+        help
+          This is the framebuffer device for a SHARP LQ035Q7DB03 TFT LCD
+          attached to a BF537.
+
+          To compile this driver as a module, choose M here: the
+          module will be called bf537-lq035.
+
+config FB_BFIN_7393
+        tristate "Blackfin ADV7393 Video encoder"
+        depends on FB && BLACKFIN
+        select I2C
+        select FB_CFB_FILLRECT
+        select FB_CFB_COPYAREA
+        select FB_CFB_IMAGEBLIT
+        help
+          This is the framebuffer device for a ADV7393 video encoder
+          attached to a Blackfin on the PPI port.
+          If your Blackfin board has a ADV7393 select Y.
+
+          To compile this driver as a module, choose M here: the
+          module will be called bfin_adv7393fb.
+
+choice
+        prompt  "Video mode support"
+        depends on FB_BFIN_7393
+        default NTSC
+
+config NTSC
+        bool 'NTSC 720x480'
+
+config PAL
+        bool 'PAL 720x576'
+
+config NTSC_640x480
+        bool 'NTSC 640x480 (Experimental)'
+
+config PAL_640x480
+        bool 'PAL 640x480 (Experimental)'
+
+config NTSC_YCBCR
+        bool 'NTSC 720x480 YCbCR input'
+
+config PAL_YCBCR
+        bool 'PAL 720x576 YCbCR input'
+
+endchoice
+
+choice
+        prompt  "Size of ADV7393 frame buffer memory Single/Double Size"
+        depends on (FB_BFIN_7393)
+        default ADV7393_1XMEM
+
+config ADV7393_1XMEM
+        bool 'Single'
+
+config ADV7393_2XMEM
+        bool 'Double'
+endchoice
+
 config FB_STI
         tristate "HP STI frame buffer device support"
         depends on FB && PARISC
@@ -748,24 +783,14 @@ config FB_N411
 config FB_HGA
         tristate "Hercules mono graphics support"
         depends on FB && X86
-        select FB_CFB_FILLRECT
-        select FB_CFB_COPYAREA
-        select FB_CFB_IMAGEBLIT
         help
           Say Y here if you have a Hercules mono graphics card.
 
           To compile this driver as a module, choose M here: the
           module will be called hgafb.
 
-          As this card technology is 15 years old, most people will answer N
-          here.
-
-config FB_HGA_ACCEL
-        bool "Hercules mono Acceleration functions (EXPERIMENTAL)"
-        depends on FB_HGA && EXPERIMENTAL
-        ---help---
-        This will compile the Hercules mono graphics with
-        acceleration functions.
+          As this card technology is at least 25 years old,
+          most people will answer N here.
 
 config FB_SGIVW
         tristate "SGI Visual Workstation framebuffer support"
@@ -955,7 +980,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 +991,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"
@@ -1154,7 +1179,7 @@ config FB_CARILLO_RANCH
 
 config FB_INTEL
         tristate "Intel 830M/845G/852GM/855GM/865G/915G/945G/945GM/965G/965GM support (EXPERIMENTAL)"
-        depends on EXPERIMENTAL && FB && PCI && X86 && AGP_INTEL && EMBEDDED
+        depends on EXPERIMENTAL && FB && PCI && X86 && AGP_INTEL && EXPERT
         select FB_MODE_HELPERS
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
@@ -1207,7 +1232,7 @@ config FB_MATROX
           module will be called matroxfb.
 
           You can pass several parameters to the driver at boot time or at
-          module load time. The parameters look like "video=matrox:XXX", and
+          module load time. The parameters look like "video=matroxfb:XXX", and
           are described in <file:Documentation/fb/matroxfb.txt>.
 
 config FB_MATROX_MILLENIUM
@@ -1323,7 +1348,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 +1420,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)"
@@ -1435,6 +1460,14 @@ config FB_S3
         ---help---
           Driver for graphics boards with S3 Trio / S3 Virge chip.
 
+config FB_S3_DDC
+        bool "DDC for S3 support"
+        depends on FB_S3
+        select FB_DDC
+        default y
+        help
+          Say Y here if you want DDC support for your S3 graphics card.
+
 config FB_SAVAGE
         tristate "S3 Savage support"
         depends on FB && PCI && EXPERIMENTAL
@@ -1534,6 +1567,17 @@ config FB_VIA_DIRECT_PROCFS
           correct output device configuration.
           Its use is strongly discouraged.
 
+config FB_VIA_X_COMPATIBILITY
+        bool "X server compatibility"
+        depends on FB_VIA
+        default n
+        help
+          This option reduces the functionality (power saving, ...) of the
+          framebuffer to avoid negative impact on the OpenChrome X server.
+          If you use any X server other than fbdev you should enable this
+          otherwise it should be safe to disable it and allow using all
+          features.
+
 endif
 
 config FB_NEOMAGIC
@@ -1720,6 +1764,24 @@ config FB_AU1200
           various panels and CRTs by passing in kernel cmd line option
           au1200fb:panel=<name>.
 
+config FB_VT8500
+        bool "VT8500 LCD Driver"
+        depends on (FB = y) && ARM && ARCH_VT8500 && VTWM_VERSION_VT8500
+        select FB_WMT_GE_ROPS
+        select FB_SYS_IMAGEBLIT
+        help
+          This is the framebuffer driver for VIA VT8500 integrated LCD
+          controller.
+
+config FB_WM8505
+        bool "WM8505 frame buffer support"
+        depends on (FB = y) && ARM && ARCH_VT8500 && VTWM_VERSION_WM8505
+        select FB_WMT_GE_ROPS
+        select FB_SYS_IMAGEBLIT
+        help
+          This is the framebuffer driver for WonderMedia WM8505
+          integrated LCD controller.
+
 source "drivers/video/geode/Kconfig"
 
 config FB_HIT
@@ -1848,6 +1910,16 @@ config FB_PXA_PARAMETERS
 
           <file:Documentation/fb/pxafb.txt> describes the available parameters.
 
+config PXA3XX_GCU
+        tristate "PXA3xx 2D graphics accelerator driver"
+        depends on FB_PXA
+        help
+          Kernelspace driver for the 2D graphics controller unit (GCU)
+          found on PXA3xx processors. There is a counterpart driver in the
+          DirectFB suite, see http://www.directfb.org/
+
+          If you compile this as a module, it will be called pxa3xx_gcu.
+
 config FB_MBX
         tristate "2700G LCD framebuffer support"
         depends on FB && ARCH_PXA
@@ -1896,13 +1968,6 @@ 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
@@ -1911,6 +1976,7 @@ config FB_SH_MOBILE_LCDC
         select FB_SYS_IMAGEBLIT
         select FB_SYS_FOPS
         select FB_DEFERRED_IO
+        select FB_BACKLIGHT
         select SH_MIPI_DSI if SH_LCD_MIPI_DSI
         ---help---
           Frame buffer driver for the on-chip SH-Mobile LCD controller.
@@ -1919,9 +1985,24 @@ 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_SH_MOBILE_MERAM
+        tristate "SuperH Mobile MERAM read ahead support for LCDC"
+        depends on FB_SH_MOBILE_LCDC
+        default y
+        ---help---
+          Enable MERAM support for the SH-Mobile LCD controller.
+
+          This will allow for caching of the framebuffer to provide more
+          reliable access under heavy main memory bus traffic situations.
+          Up to 4 memory channels can be configured, allowing 4 RGB or
+          2 YCbCr framebuffers to be configured.
+
 config FB_TMIO
         tristate "Toshiba Mobile IO FrameBuffer support"
         depends on FB && MFD_CORE
@@ -2029,6 +2110,20 @@ config FB_SM501
 
           If unsure, say N.
 
+config FB_UDL
+        tristate "Displaylink USB Framebuffer support"
+        depends on FB && USB
+        select FB_MODE_HELPERS
+        select FB_SYS_FILLRECT
+        select FB_SYS_COPYAREA
+        select FB_SYS_IMAGEBLIT
+        select FB_SYS_FOPS
+        select FB_DEFERRED_IO
+        ---help---
+          This is a kernel framebuffer driver for DisplayLink USB devices.
+          Supports fbdev clients like xf86-video-fbdev, kdrive, fbi, and
+          mplayer -vo fbdev. Supports all USB 2.0 era DisplayLink devices.
+          To compile as a module, choose M here: the module name is udlfb.
 
 config FB_PNX4008_DUM
         tristate "Display Update Module support on Philips PNX4008 board"
@@ -2171,29 +2266,43 @@ config FB_METRONOME
 config FB_MB862XX
         tristate "Fujitsu MB862xx GDC support"
         depends on FB
+        depends on PCI || (OF && PPC)
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
         ---help---
           Frame buffer driver for Fujitsu Carmine/Coral-P(A)/Lime controllers.
 
+choice
+        prompt "GDC variant"
+        depends on FB_MB862XX
+
 config FB_MB862XX_PCI_GDC
         bool "Carmine/Coral-P(A) GDC"
-        depends on PCI && FB_MB862XX
+        depends on PCI
         ---help---
           This enables framebuffer support for Fujitsu Carmine/Coral-P(A)
           PCI graphics controller devices.
 
 config FB_MB862XX_LIME
         bool "Lime GDC"
-        depends on FB_MB862XX
-        depends on OF && !FB_MB862XX_PCI_GDC
-        depends on PPC
+        depends on OF && PPC
         select FB_FOREIGN_ENDIAN
         select FB_LITTLE_ENDIAN
         ---help---
           Framebuffer support for Fujitsu Lime GDC on host CPU bus.
 
+endchoice
+
+config FB_MB862XX_I2C
+        bool "Support I2C bus on MB862XX GDC"
+        depends on FB_MB862XX && I2C
+        default y
+        help
+          Selecting this option adds Coral-P(A)/Lime GDC I2C bus adapter
+          driver to support accessing I2C devices on controller's I2C bus.
+          These are usually some video decoder chips.
+
 config FB_EP93XX
         tristate "EP93XX frame buffer support"
         depends on FB && ARCH_EP93XX
@@ -2254,6 +2363,26 @@ config FB_JZ4740
         help
           Framebuffer support for the JZ4740 SoC.
 
+config FB_MXS
+        tristate "MXS LCD framebuffer support"
+        depends on FB && ARCH_MXS
+        select FB_CFB_FILLRECT
+        select FB_CFB_COPYAREA
+        select FB_CFB_IMAGEBLIT
+        help
+          Framebuffer support for the MXS SoC.
+
+config FB_PUV3_UNIGFX
+        tristate "PKUnity v3 Unigfx framebuffer support"
+        depends on FB && UNICORE32 && ARCH_PUV3
+        select FB_SYS_FILLRECT
+        select FB_SYS_COPYAREA
+        select FB_SYS_IMAGEBLIT
+        select FB_SYS_FOPS
+        help
+          Choose this option if you want to use the Unigfx device as a
+          framebuffer device. Without the support of PCI & AGP.
+
 source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
 
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 485e8ed1318c..8b83129e209c 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -26,6 +26,7 @@ obj-$(CONFIG_FB_SVGALIB)       += svgalib.o
 obj-$(CONFIG_FB_MACMODES)      += macmodes.o
 obj-$(CONFIG_FB_DDC)           += fb_ddc.o
 obj-$(CONFIG_FB_DEFERRED_IO)   += fb_defio.o
+obj-$(CONFIG_FB_WMT_GE_ROPS)   += wmt_ge_rops.o
 
 # Hardware specific drivers go first
 obj-$(CONFIG_FB_AMIGA)            += amifb.o c2p_planar.o
@@ -100,10 +101,13 @@ obj-$(CONFIG_FB_CIRRUS)		  += cirrusfb.o
 obj-$(CONFIG_FB_ASILIANT)         += asiliantfb.o
 obj-$(CONFIG_FB_PXA)              += pxafb.o
 obj-$(CONFIG_FB_PXA168)           += pxa168fb.o
+obj-$(CONFIG_PXA3XX_GCU)          += pxa3xx-gcu.o
 obj-$(CONFIG_FB_W100)             += w100fb.o
 obj-$(CONFIG_FB_TMIO)             += tmiofb.o
 obj-$(CONFIG_FB_AU1100)           += au1100fb.o
 obj-$(CONFIG_FB_AU1200)           += au1200fb.o
+obj-$(CONFIG_FB_VT8500)           += vt8500lcdfb.o
+obj-$(CONFIG_FB_WM8505)           += wm8505fb.o
 obj-$(CONFIG_FB_PMAG_AA)          += pmag-aa-fb.o
 obj-$(CONFIG_FB_PMAG_BA)          += pmag-ba-fb.o
 obj-$(CONFIG_FB_PMAGB_B)          += pmagb-b-fb.o
@@ -122,9 +126,11 @@ obj-$(CONFIG_FB_PNX4008_DUM_RGB)  += pnx4008/
 obj-$(CONFIG_FB_IBM_GXT4500)      += gxt4500.o
 obj-$(CONFIG_FB_PS3)              += ps3fb.o
 obj-$(CONFIG_FB_SM501)            += sm501fb.o
+obj-$(CONFIG_FB_UDL)              += udlfb.o
 obj-$(CONFIG_FB_XILINX)           += xilinxfb.o
 obj-$(CONFIG_SH_MIPI_DSI)         += sh_mipi_dsi.o
 obj-$(CONFIG_FB_SH_MOBILE_HDMI)   += sh_mobile_hdmi.o
+obj-$(CONFIG_FB_SH_MOBILE_MERAM)  += sh_mobile_meram.o
 obj-$(CONFIG_FB_SH_MOBILE_LCDC)   += sh_mobile_lcdcfb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
 obj-y                             += omap2/
@@ -134,6 +140,7 @@ obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
 obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
 obj-$(CONFIG_FB_JZ4740)           += jz4740_fb.o
+obj-$(CONFIG_FB_PUV3_UNIGFX)      += fb-puv3.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
@@ -141,11 +148,14 @@ obj-$(CONFIG_FB_VESA)             += vesafb.o
 obj-$(CONFIG_FB_EFI)              += efifb.o
 obj-$(CONFIG_FB_VGA16)            += vga16fb.o
 obj-$(CONFIG_FB_OF)               += offb.o
+obj-$(CONFIG_FB_BF537_LQ035)      += bf537-lq035.o
 obj-$(CONFIG_FB_BF54X_LQ043)      += bf54x-lq043fb.o
 obj-$(CONFIG_FB_BFIN_LQ035Q1)     += bfin-lq035q1-fb.o
 obj-$(CONFIG_FB_BFIN_T350MCQB)    += bfin-t350mcqb-fb.o
+obj-$(CONFIG_FB_BFIN_7393)        += bfin_adv7393fb.o
 obj-$(CONFIG_FB_MX3)              += mx3fb.o
 obj-$(CONFIG_FB_DA8XX)            += da8xx-fb.o
+obj-$(CONFIG_FB_MXS)              += mxsfb.o
 
 # the test framebuffer is last
 obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
diff --git a/drivers/video/acornfb.c b/drivers/video/acornfb.c
index 82acb8dc4aa1..6183a57eb69d 100644
--- a/drivers/video/acornfb.c
+++ b/drivers/video/acornfb.c
@@ -66,7 +66,7 @@
  * have.  Allow 1% either way on the nominal for TVs.
  */
 #define NR_MONTYPES     6
-static struct fb_monspecs monspecs[NR_MONTYPES] __initdata = {
+static struct fb_monspecs monspecs[NR_MONTYPES] __devinitdata = {
         {       /* TV           */
                 .hfmin  = 15469,
                 .hfmax  = 15781,
@@ -873,7 +873,7 @@ static struct fb_ops acornfb_ops = {
 /*
  * Everything after here is initialisation!!!
  */
-static struct fb_videomode modedb[] __initdata = {
+static struct fb_videomode modedb[] __devinitdata = {
         {       /* 320x256 @ 50Hz */
                 NULL, 50,  320,  256, 125000,  92,  62,  35, 19,  38, 2,
                 FB_SYNC_COMP_HIGH_ACT,
@@ -925,8 +925,7 @@ static struct fb_videomode modedb[] __initdata = {
         }
 };
 
-static struct fb_videomode __initdata
-acornfb_default_mode = {
+static struct fb_videomode acornfb_default_mode __devinitdata = {
         .name =         NULL,
         .refresh =      60,
         .xres =         640,
@@ -942,7 +941,7 @@ acornfb_default_mode = {
         .vmode =        FB_VMODE_NONINTERLACED
 };
 
-static void __init acornfb_init_fbinfo(void)
+static void __devinit acornfb_init_fbinfo(void)
 {
         static int first = 1;
 
@@ -1018,8 +1017,7 @@ static void __init acornfb_init_fbinfo(void)
  *      size can optionally be followed by 'M' or 'K' for
  *      MB or KB respectively.
  */
-static void __init
-acornfb_parse_mon(char *opt)
+static void __devinit acornfb_parse_mon(char *opt)
 {
         char *p = opt;
 
@@ -1066,8 +1064,7 @@ bad:
         current_par.montype = -1;
 }
 
-static void __init
-acornfb_parse_montype(char *opt)
+static void __devinit acornfb_parse_montype(char *opt)
 {
         current_par.montype = -2;
 
@@ -1108,8 +1105,7 @@ acornfb_parse_montype(char *opt)
         }
 }
 
-static void __init
-acornfb_parse_dram(char *opt)
+static void __devinit acornfb_parse_dram(char *opt)
 {
         unsigned int size;
 
@@ -1134,15 +1130,14 @@ acornfb_parse_dram(char *opt)
 static struct options {
         char *name;
         void (*parse)(char *opt);
-} opt_table[] __initdata = {
+} opt_table[] __devinitdata = {
         { "mon",     acornfb_parse_mon     },
         { "montype", acornfb_parse_montype },
         { "dram",    acornfb_parse_dram    },
         { NULL, NULL }
 };
 
-int __init
-acornfb_setup(char *options)
+static int __devinit acornfb_setup(char *options)
 {
         struct options *optp;
         char *opt;
@@ -1179,8 +1174,7 @@ acornfb_setup(char *options)
  * Detect type of monitor connected
  *  For now, we just assume SVGA
  */
-static int __init
-acornfb_detect_monitortype(void)
+static int __devinit acornfb_detect_monitortype(void)
 {
         return 4;
 }
diff --git a/drivers/video/amba-clcd.c b/drivers/video/amba-clcd.c
index 1c2c68356ea7..cf03ad067147 100644
--- a/drivers/video/amba-clcd.c
+++ b/drivers/video/amba-clcd.c
@@ -120,8 +120,23 @@ static void clcdfb_enable(struct clcd_fb *fb, u32 cntl)
 static int
 clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 {
+        u32 caps;
         int ret = 0;
 
+        if (fb->panel->caps && fb->board->caps)
+                caps = fb->panel->caps & fb->board->caps;
+        else {
+                /* Old way of specifying what can be used */
+                caps = fb->panel->cntl & CNTL_BGR ?
+                        CLCD_CAP_BGR : CLCD_CAP_RGB;
+                /* But mask out 444 modes as they weren't supported */
+                caps &= ~CLCD_CAP_444;
+        }
+
+        /* Only TFT panels can do RGB888/BGR888 */
+        if (!(fb->panel->cntl & CNTL_LCDTFT))
+                caps &= ~CLCD_CAP_888;
+
         memset(&var->transp, 0, sizeof(var->transp));
 
         var->red.msb_right = 0;
@@ -133,6 +148,13 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
         case 2:
         case 4:
         case 8:
+                /* If we can't do 5551, reject */
+                caps &= CLCD_CAP_5551;
+                if (!caps) {
+                        ret = -EINVAL;
+                        break;
+                }
+
                 var->red.length         = var->bits_per_pixel;
                 var->red.offset         = 0;
                 var->green.length       = var->bits_per_pixel;
@@ -140,23 +162,61 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
                 var->blue.length        = var->bits_per_pixel;
                 var->blue.offset        = 0;
                 break;
+
         case 16:
-                var->red.length = 5;
-                var->blue.length = 5;
+                /* If we can't do 444, 5551 or 565, reject */
+                if (!(caps & (CLCD_CAP_444 | CLCD_CAP_5551 | CLCD_CAP_565))) {
+                        ret = -EINVAL;
+                        break;
+                }
+
                 /*
-                 * Green length can be 5 or 6 depending whether
-                 * we're operating in RGB555 or RGB565 mode.
+                 * Green length can be 4, 5 or 6 depending whether
+                 * we're operating in 444, 5551 or 565 mode.
                  */
-                if (var->green.length != 5 && var->green.length != 6)
-                        var->green.length = 6;
+                if (var->green.length == 4 && caps & CLCD_CAP_444)
+                        caps &= CLCD_CAP_444;
+                if (var->green.length == 5 && caps & CLCD_CAP_5551)
+                        caps &= CLCD_CAP_5551;
+                else if (var->green.length == 6 && caps & CLCD_CAP_565)
+                        caps &= CLCD_CAP_565;
+                else {
+                        /*
+                         * PL110 officially only supports RGB555,
+                         * but may be wired up to allow RGB565.
+                         */
+                        if (caps & CLCD_CAP_565) {
+                                var->green.length = 6;
+                                caps &= CLCD_CAP_565;
+                        } else if (caps & CLCD_CAP_5551) {
+                                var->green.length = 5;
+                                caps &= CLCD_CAP_5551;
+                        } else {
+                                var->green.length = 4;
+                                caps &= CLCD_CAP_444;
+                        }
+                }
+
+                if (var->green.length >= 5) {
+                        var->red.length = 5;
+                        var->blue.length = 5;
+                } else {
+                        var->red.length = 4;
+                        var->blue.length = 4;
+                }
                 break;
         case 32:
-                if (fb->panel->cntl & CNTL_LCDTFT) {
-                        var->red.length         = 8;
-                        var->green.length       = 8;
-                        var->blue.length        = 8;
+                /* If we can't do 888, reject */
+                caps &= CLCD_CAP_888;
+                if (!caps) {
+                        ret = -EINVAL;
                         break;
                 }
+
+                var->red.length = 8;
+                var->green.length = 8;
+                var->blue.length = 8;
+                break;
         default:
                 ret = -EINVAL;
                 break;
@@ -168,7 +228,20 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
          * the bitfield length defined above.
          */
         if (ret == 0 && var->bits_per_pixel >= 16) {
-                if (fb->panel->cntl & CNTL_BGR) {
+                bool bgr, rgb;
+
+                bgr = caps & CLCD_CAP_BGR && var->blue.offset == 0;
+                rgb = caps & CLCD_CAP_RGB && var->red.offset == 0;
+
+                if (!bgr && !rgb)
+                        /*
+                         * The requested format was not possible, try just
+                         * our capabilities.  One of BGR or RGB must be
+                         * supported.
+                         */
+                        bgr = caps & CLCD_CAP_BGR;
+
+                if (bgr) {
                         var->blue.offset = 0;
                         var->green.offset = var->blue.offset + var->blue.length;
                         var->red.offset = var->green.offset + var->green.length;
@@ -374,6 +447,8 @@ static int clcdfb_register(struct clcd_fb *fb)
                 goto out;
         }
 
+        fb->fb.device           = &fb->dev->dev;
+
         fb->fb.fix.mmio_start   = fb->dev->res.start;
         fb->fb.fix.mmio_len     = resource_size(&fb->dev->res);
 
@@ -443,8 +518,8 @@ static int clcdfb_register(struct clcd_fb *fb)
 
         fb_set_var(&fb->fb, &fb->fb.var);
 
-        printk(KERN_INFO "CLCD: %s hardware, %s display\n",
-               fb->board->name, fb->panel->mode.name);
+        dev_info(&fb->dev->dev, "%s hardware, %s display\n",
+                 fb->board->name, fb->panel->mode.name);
 
         ret = register_framebuffer(&fb->fb);
         if (ret == 0)
@@ -461,7 +536,7 @@ static int clcdfb_register(struct clcd_fb *fb)
         return ret;
 }
 
-static int clcdfb_probe(struct amba_device *dev, struct amba_id *id)
+static int clcdfb_probe(struct amba_device *dev, const struct amba_id *id)
 {
         struct clcd_board *board = dev->dev.platform_data;
         struct clcd_fb *fb;
@@ -486,6 +561,10 @@ static int clcdfb_probe(struct amba_device *dev, struct amba_id *id)
         fb->dev = dev;
         fb->board = board;
 
+        dev_info(&fb->dev->dev, "PL%03x rev%u at 0x%08llx\n",
+                amba_part(dev), amba_rev(dev),
+                (unsigned long long)dev->res.start);
+
         ret = fb->board->setup(fb);
         if (ret)
                 goto free_fb;
diff --git a/drivers/video/amifb.c b/drivers/video/amifb.c
index e5d6b56d4447..5ea6596dd824 100644
--- a/drivers/video/amifb.c
+++ b/drivers/video/amifb.c
@@ -2224,22 +2224,23 @@ static int amifb_ioctl(struct fb_info *info,
          * Allocate, Clear and Align a Block of Chip Memory
          */
 
-static u_long unaligned_chipptr = 0;
+static void *aligned_chipptr;
 
 static inline u_long __init chipalloc(u_long size)
 {
-        size += PAGE_SIZE-1;
-        if (!(unaligned_chipptr = (u_long)amiga_chip_alloc(size,
-                                                           "amifb [RAM]")))
-                panic("No Chip RAM for frame buffer");
-        memset((void *)unaligned_chipptr, 0, size);
-        return PAGE_ALIGN(unaligned_chipptr);
+        aligned_chipptr = amiga_chip_alloc(size, "amifb [RAM]");
+        if (!aligned_chipptr) {
+                pr_err("amifb: No Chip RAM for frame buffer");
+                return 0;
+        }
+        memset(aligned_chipptr, 0, size);
+        return (u_long)aligned_chipptr;
 }
 
 static inline void chipfree(void)
 {
-        if (unaligned_chipptr)
-                amiga_chip_free((void *)unaligned_chipptr);
+        if (aligned_chipptr)
+                amiga_chip_free(aligned_chipptr);
 }
 
 
@@ -2295,7 +2296,7 @@ default_chipset:
                             defmode = amiga_vblank == 50 ? DEFMODE_PAL
                                                          : DEFMODE_NTSC;
                         if (amiga_chip_avail()-CHIPRAM_SAFETY_LIMIT >
-                            VIDEOMEMSIZE_ECS_1M)
+                            VIDEOMEMSIZE_ECS_2M)
                                 fb_info.fix.smem_len = VIDEOMEMSIZE_ECS_2M;
                         else
                                 fb_info.fix.smem_len = VIDEOMEMSIZE_ECS_1M;
@@ -2312,7 +2313,7 @@ default_chipset:
                         maxfmode = TAG_FMODE_4;
                         defmode = DEFMODE_AGA;
                         if (amiga_chip_avail()-CHIPRAM_SAFETY_LIMIT >
-                            VIDEOMEMSIZE_AGA_1M)
+                            VIDEOMEMSIZE_AGA_2M)
                                 fb_info.fix.smem_len = VIDEOMEMSIZE_AGA_2M;
                         else
                                 fb_info.fix.smem_len = VIDEOMEMSIZE_AGA_1M;
@@ -2385,6 +2386,10 @@ default_chipset:
                             DUMMYSPRITEMEMSIZE+
                             COPINITSIZE+
                             4*COPLISTSIZE);
+        if (!chipptr) {
+                err = -ENOMEM;
+                goto amifb_error;
+        }
 
         assignchunk(videomemory, u_long, chipptr, fb_info.fix.smem_len);
         assignchunk(spritememory, u_long, chipptr, SPRITEMEMSIZE);
diff --git a/drivers/video/arcfb.c b/drivers/video/arcfb.c
index f3d7440f0072..c22e8d39a2cb 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
@@ -516,11 +515,10 @@ static int __devinit arcfb_probe(struct platform_device *dev)
 
         /* We need a flat backing store for the Arc's
            less-flat actual paged framebuffer */
-        if (!(videomemory = vmalloc(videomemorysize)))
+        videomemory = vzalloc(videomemorysize);
+        if (!videomemory)
                 return retval;
 
-        memset(videomemory, 0, videomemorysize);
-
         info = framebuffer_alloc(sizeof(struct arcfb_par), &dev->dev);
         if (!info)
                 goto err;
diff --git a/drivers/video/arkfb.c b/drivers/video/arkfb.c
index d583bea608fd..8686429cbdf0 100644
--- a/drivers/video/arkfb.c
+++ b/drivers/video/arkfb.c
@@ -23,7 +23,7 @@
 #include <linux/svga.h>
 #include <linux/init.h>
 #include <linux/pci.h>
-#include <linux/console.h> /* Why should fb driver call console functions? because acquire_console_sem() */
+#include <linux/console.h> /* Why should fb driver call console functions? because console_lock() */
 #include <video/vga.h>
 
 #ifdef CONFIG_MTRR
@@ -158,12 +158,19 @@ static void arkfb_settile(struct fb_info *info, struct fb_tilemap *map)
         }
 }
 
+static void arkfb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+        struct arkfb_info *par = info->par;
+
+        svga_tilecursor(par->state.vgabase, info, cursor);
+}
+
 static struct fb_tile_ops arkfb_tile_ops = {
         .fb_settile     = arkfb_settile,
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = arkfb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -466,32 +473,40 @@ static unsigned short dac_regs[4] = {0x3c8, 0x3c9, 0x3c6, 0x3c7};
 
 static void ark_dac_read_regs(void *data, u8 *code, int count)
 {
-        u8 regval = vga_rseq(NULL, 0x1C);
+        struct fb_info *info = data;
+        struct arkfb_info *par;
+        u8 regval;
 
+        par = info->par;
+        regval = vga_rseq(par->state.vgabase, 0x1C);
         while (count != 0)
         {
-                vga_wseq(NULL, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
-                code[1] = vga_r(NULL, dac_regs[code[0] & 3]);
+                vga_wseq(par->state.vgabase, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
+                code[1] = vga_r(par->state.vgabase, dac_regs[code[0] & 3]);
                 count--;
                 code += 2;
         }
 
-        vga_wseq(NULL, 0x1C, regval);
+        vga_wseq(par->state.vgabase, 0x1C, regval);
 }
 
 static void ark_dac_write_regs(void *data, u8 *code, int count)
 {
-        u8 regval = vga_rseq(NULL, 0x1C);
+        struct fb_info *info = data;
+        struct arkfb_info *par;
+        u8 regval;
 
+        par = info->par;
+        regval = vga_rseq(par->state.vgabase, 0x1C);
         while (count != 0)
         {
-                vga_wseq(NULL, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
-                vga_w(NULL, dac_regs[code[0] & 3], code[1]);
+                vga_wseq(par->state.vgabase, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
+                vga_w(par->state.vgabase, dac_regs[code[0] & 3], code[1]);
                 count--;
                 code += 2;
         }
 
-        vga_wseq(NULL, 0x1C, regval);
+        vga_wseq(par->state.vgabase, 0x1C, regval);
 }
 
 
@@ -507,8 +522,8 @@ static void ark_set_pixclock(struct fb_info *info, u32 pixclock)
         }
 
         /* Set VGA misc register  */
-        regval = vga_r(NULL, VGA_MIS_R);
-        vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+        regval = vga_r(par->state.vgabase, VGA_MIS_R);
+        vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 }
 
 
@@ -520,7 +535,10 @@ static int arkfb_open(struct fb_info *info, int user)
 
         mutex_lock(&(par->open_lock));
         if (par->ref_count == 0) {
+                void __iomem *vgabase = par->state.vgabase;
+
                 memset(&(par->state), 0, sizeof(struct vgastate));
+                par->state.vgabase = vgabase;
                 par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
                 par->state.num_crtc = 0x60;
                 par->state.num_seq = 0x30;
@@ -646,50 +664,50 @@ static int arkfb_set_par(struct fb_info *info)
         info->var.activate = FB_ACTIVATE_NOW;
 
         /* Unlock registers */
-        svga_wcrt_mask(0x11, 0x00, 0x80);
+        svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
 
         /* Blank screen and turn off sync */
-        svga_wseq_mask(0x01, 0x20, 0x20);
-        svga_wcrt_mask(0x17, 0x00, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
         /* Set default values */
-        svga_set_default_gfx_regs();
-        svga_set_default_atc_regs();
-        svga_set_default_seq_regs();
-        svga_set_default_crt_regs();
-        svga_wcrt_multi(ark_line_compare_regs, 0xFFFFFFFF);
-        svga_wcrt_multi(ark_start_address_regs, 0);
+        svga_set_default_gfx_regs(par->state.vgabase);
+        svga_set_default_atc_regs(par->state.vgabase);
+        svga_set_default_seq_regs(par->state.vgabase);
+        svga_set_default_crt_regs(par->state.vgabase);
+        svga_wcrt_multi(par->state.vgabase, ark_line_compare_regs, 0xFFFFFFFF);
+        svga_wcrt_multi(par->state.vgabase, ark_start_address_regs, 0);
 
         /* ARK specific initialization */
-        svga_wseq_mask(0x10, 0x1F, 0x1F); /* enable linear framebuffer and full memory access */
-        svga_wseq_mask(0x12, 0x03, 0x03); /* 4 MB linear framebuffer size */
+        svga_wseq_mask(par->state.vgabase, 0x10, 0x1F, 0x1F); /* enable linear framebuffer and full memory access */
+        svga_wseq_mask(par->state.vgabase, 0x12, 0x03, 0x03); /* 4 MB linear framebuffer size */
 
-        vga_wseq(NULL, 0x13, info->fix.smem_start >> 16);
-        vga_wseq(NULL, 0x14, info->fix.smem_start >> 24);
-        vga_wseq(NULL, 0x15, 0);
-        vga_wseq(NULL, 0x16, 0);
+        vga_wseq(par->state.vgabase, 0x13, info->fix.smem_start >> 16);
+        vga_wseq(par->state.vgabase, 0x14, info->fix.smem_start >> 24);
+        vga_wseq(par->state.vgabase, 0x15, 0);
+        vga_wseq(par->state.vgabase, 0x16, 0);
 
         /* Set the FIFO threshold register */
         /* It is fascinating way to store 5-bit value in 8-bit register */
         regval = 0x10 | ((threshold & 0x0E) >> 1) | (threshold & 0x01) << 7 | (threshold & 0x10) << 1;
-        vga_wseq(NULL, 0x18, regval);
+        vga_wseq(par->state.vgabase, 0x18, regval);
 
         /* Set the offset register */
         pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
-        svga_wcrt_multi(ark_offset_regs, offset_value);
+        svga_wcrt_multi(par->state.vgabase, ark_offset_regs, offset_value);
 
         /* fix for hi-res textmode */
-        svga_wcrt_mask(0x40, 0x08, 0x08);
+        svga_wcrt_mask(par->state.vgabase, 0x40, 0x08, 0x08);
 
         if (info->var.vmode & FB_VMODE_DOUBLE)
-                svga_wcrt_mask(0x09, 0x80, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
         else
-                svga_wcrt_mask(0x09, 0x00, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
         if (info->var.vmode & FB_VMODE_INTERLACED)
-                svga_wcrt_mask(0x44, 0x04, 0x04);
+                svga_wcrt_mask(par->state.vgabase, 0x44, 0x04, 0x04);
         else
-                svga_wcrt_mask(0x44, 0x00, 0x04);
+                svga_wcrt_mask(par->state.vgabase, 0x44, 0x00, 0x04);
 
         hmul = 1;
         hdiv = 1;
@@ -699,40 +717,40 @@ static int arkfb_set_par(struct fb_info *info)
         switch (mode) {
         case 0:
                 pr_debug("fb%d: text mode\n", info->node);
-                svga_set_textmode_vga_regs();
+                svga_set_textmode_vga_regs(par->state.vgabase);
 
-                vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-                svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                 dac_set_mode(par->dac, DAC_PSEUDO8_8);
 
                 break;
         case 1:
                 pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-                vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
+                vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
 
-                vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-                svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                 dac_set_mode(par->dac, DAC_PSEUDO8_8);
                 break;
         case 2:
                 pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-                svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                 dac_set_mode(par->dac, DAC_PSEUDO8_8);
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x16); /* 8bpp accel mode */
+                vga_wseq(par->state.vgabase, 0x11, 0x16); /* 8bpp accel mode */
 
                 if (info->var.pixclock > 20000) {
                         pr_debug("fb%d: not using multiplex\n", info->node);
-                        svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                        svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                         dac_set_mode(par->dac, DAC_PSEUDO8_8);
                 } else {
                         pr_debug("fb%d: using multiplex\n", info->node);
-                        svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                        svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                         dac_set_mode(par->dac, DAC_PSEUDO8_16);
                         hdiv = 2;
                 }
@@ -740,22 +758,22 @@ static int arkfb_set_par(struct fb_info *info)
         case 4:
                 pr_debug("fb%d: 5/5/5 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x1A); /* 16bpp accel mode */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x1A); /* 16bpp accel mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB1555_16);
                 break;
         case 5:
                 pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x1A); /* 16bpp accel mode */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x1A); /* 16bpp accel mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB0565_16);
                 break;
         case 6:
                 pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x16); /* 8bpp accel mode ??? */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x16); /* 8bpp accel mode ??? */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB0888_16);
                 hmul = 3;
                 hdiv = 2;
@@ -763,8 +781,8 @@ static int arkfb_set_par(struct fb_info *info)
         case 7:
                 pr_debug("fb%d: 8/8/8/8 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x1E); /* 32bpp accel mode */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x1E); /* 32bpp accel mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB8888_16);
                 hmul = 2;
                 break;
@@ -774,7 +792,7 @@ static int arkfb_set_par(struct fb_info *info)
         }
 
         ark_set_pixclock(info, (hdiv * info->var.pixclock) / hmul);
-        svga_set_timings(&ark_timing_regs, &(info->var), hmul, hdiv,
+        svga_set_timings(par->state.vgabase, &ark_timing_regs, &(info->var), hmul, hdiv,
                          (info->var.vmode & FB_VMODE_DOUBLE)     ? 2 : 1,
                          (info->var.vmode & FB_VMODE_INTERLACED) ? 2 : 1,
                           hmul, info->node);
@@ -782,12 +800,12 @@ static int arkfb_set_par(struct fb_info *info)
         /* Set interlaced mode start/end register */
         value = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
         value = ((value * hmul / hdiv) / 8) - 5;
-        vga_wcrt(NULL, 0x42, (value + 1) / 2);
+        vga_wcrt(par->state.vgabase, 0x42, (value + 1) / 2);
 
         memset_io(info->screen_base, 0x00, screen_size);
         /* Device and screen back on */
-        svga_wcrt_mask(0x17, 0x80, 0x80);
-        svga_wseq_mask(0x01, 0x00, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
         return 0;
 }
@@ -857,23 +875,25 @@ static int arkfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int arkfb_blank(int blank_mode, struct fb_info *info)
 {
+        struct arkfb_info *par = info->par;
+
         switch (blank_mode) {
         case FB_BLANK_UNBLANK:
                 pr_debug("fb%d: unblank\n", info->node);
-                svga_wseq_mask(0x01, 0x00, 0x20);
-                svga_wcrt_mask(0x17, 0x80, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
                 break;
         case FB_BLANK_NORMAL:
                 pr_debug("fb%d: blank\n", info->node);
-                svga_wseq_mask(0x01, 0x20, 0x20);
-                svga_wcrt_mask(0x17, 0x80, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
                 break;
         case FB_BLANK_POWERDOWN:
         case FB_BLANK_HSYNC_SUSPEND:
         case FB_BLANK_VSYNC_SUSPEND:
                 pr_debug("fb%d: sync down\n", info->node);
-                svga_wseq_mask(0x01, 0x20, 0x20);
-                svga_wcrt_mask(0x17, 0x00, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
                 break;
         }
         return 0;
@@ -884,6 +904,7 @@ static int arkfb_blank(int blank_mode, struct fb_info *info)
 
 static int arkfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+        struct arkfb_info *par = info->par;
         unsigned int offset;
 
         /* Calculate the offset */
@@ -897,7 +918,7 @@ static int arkfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info
         }
 
         /* Set the offset */
-        svga_wcrt_multi(ark_start_address_regs, offset);
+        svga_wcrt_multi(par->state.vgabase, ark_start_address_regs, offset);
 
         return 0;
 }
@@ -930,6 +951,8 @@ static struct fb_ops arkfb_ops = {
 /* PCI probe */
 static int __devinit ark_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+        struct pci_bus_region bus_reg;
+        struct resource vga_res;
         struct fb_info *info;
         struct arkfb_info *par;
         int rc;
@@ -985,8 +1008,17 @@ static int __devinit ark_pci_probe(struct pci_dev *dev, const struct pci_device_
                 goto err_iomap;
         }
 
+        bus_reg.start = 0;
+        bus_reg.end = 64 * 1024;
+
+        vga_res.flags = IORESOURCE_IO;
+
+        pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+        par->state.vgabase = (void __iomem *) vga_res.start;
+
         /* FIXME get memsize */
-        regval = vga_rseq(NULL, 0x10);
+        regval = vga_rseq(par->state.vgabase, 0x10);
         info->screen_size = (1 << (regval >> 6)) << 20;
         info->fix.smem_len = info->screen_size;
 
@@ -1091,12 +1123,12 @@ static int ark_pci_suspend (struct pci_dev* dev, pm_message_t state)
 
         dev_info(info->device, "suspend\n");
 
-        acquire_console_sem();
+        console_lock();
         mutex_lock(&(par->open_lock));
 
         if ((state.event == PM_EVENT_FREEZE) || (par->ref_count == 0)) {
                 mutex_unlock(&(par->open_lock));
-                release_console_sem();
+                console_unlock();
                 return 0;
         }
 
@@ -1107,7 +1139,7 @@ static int ark_pci_suspend (struct pci_dev* dev, pm_message_t state)
         pci_set_power_state(dev, pci_choose_state(dev, state));
 
         mutex_unlock(&(par->open_lock));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1122,7 +1154,7 @@ static int ark_pci_resume (struct pci_dev* dev)
 
         dev_info(info->device, "resume\n");
 
-        acquire_console_sem();
+        console_lock();
         mutex_lock(&(par->open_lock));
 
         if (par->ref_count == 0)
@@ -1141,7 +1173,7 @@ static int ark_pci_resume (struct pci_dev* dev)
 
 fail:
         mutex_unlock(&(par->open_lock));
-        release_console_sem();
+        console_unlock();
         return 0;
 }
 #else
diff --git a/drivers/video/atafb.c b/drivers/video/atafb.c
index f3aada20fa02..64e41f5448c4 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;
 }
@@ -3119,7 +3117,7 @@ int __init atafb_init(void)
                         atafb_ops.fb_setcolreg = &falcon_setcolreg;
                         error = request_irq(IRQ_AUTO_4, falcon_vbl_switcher,
                                             IRQ_TYPE_PRIO,
-                                            "framebuffer/modeswitch",
+                                            "framebuffer:modeswitch",
                                             falcon_vbl_switcher);
                         if (error)
                                 return error;
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 8dce25126330..4484c721f0f9 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -68,7 +68,7 @@ static void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
 }
 #endif
 
-static const u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
+static u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
                 | ATMEL_LCDC_POL_POSITIVE
                 | ATMEL_LCDC_ENA_PWMENABLE;
 
@@ -111,7 +111,7 @@ static int atmel_bl_get_brightness(struct backlight_device *bl)
         return lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_VAL);
 }
 
-static struct backlight_ops atmel_lcdc_bl_ops = {
+static const struct backlight_ops atmel_lcdc_bl_ops = {
         .update_status = atmel_bl_update_status,
         .get_brightness = atmel_bl_get_brightness,
 };
@@ -127,6 +127,7 @@ static void init_backlight(struct atmel_lcdfb_info *sinfo)
                 return;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = 0xff;
         bl = backlight_device_register("backlight", &sinfo->pdev->dev, sinfo,
                                        &atmel_lcdc_bl_ops, &props);
@@ -163,6 +164,10 @@ static void exit_backlight(struct atmel_lcdfb_info *sinfo)
 
 static void init_contrast(struct atmel_lcdfb_info *sinfo)
 {
+        /* contrast pwm can be 'inverted' */
+        if (sinfo->lcdcon_pol_negative)
+                        contrast_ctr &= ~(ATMEL_LCDC_POL_POSITIVE);
+
         /* have some default contrast/backlight settings */
         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
@@ -632,7 +637,7 @@ static inline unsigned int chan_to_field(unsigned int chan, const struct fb_bitf
  *      magnitude which needs to be scaled in this function for the hardware.
  *      Things to take into consideration are how many color registers, if
  *      any, are supported with the current color visual. With truecolor mode
- *      no color palettes are supported. Here a psuedo palette is created
+ *      no color palettes are supported. Here a pseudo palette is created
  *      which we store the value in pseudo_palette in struct fb_info. For
  *      pseudocolor mode we have a limited color palette. To deal with this
  *      we can program what color is displayed for a particular pixel value.
@@ -710,11 +715,35 @@ static int atmel_lcdfb_pan_display(struct fb_var_screeninfo *var,
         return 0;
 }
 
+static int atmel_lcdfb_blank(int blank_mode, struct fb_info *info)
+{
+        struct atmel_lcdfb_info *sinfo = info->par;
+
+        switch (blank_mode) {
+        case FB_BLANK_UNBLANK:
+        case FB_BLANK_NORMAL:
+                atmel_lcdfb_start(sinfo);
+                break;
+        case FB_BLANK_VSYNC_SUSPEND:
+        case FB_BLANK_HSYNC_SUSPEND:
+                break;
+        case FB_BLANK_POWERDOWN:
+                atmel_lcdfb_stop(sinfo);
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        /* let fbcon do a soft blank for us */
+        return ((blank_mode == FB_BLANK_NORMAL) ? 1 : 0);
+}
+
 static struct fb_ops atmel_lcdfb_ops = {
         .owner          = THIS_MODULE,
         .fb_check_var   = atmel_lcdfb_check_var,
         .fb_set_par     = atmel_lcdfb_set_par,
         .fb_setcolreg   = atmel_lcdfb_setcolreg,
+        .fb_blank       = atmel_lcdfb_blank,
         .fb_pan_display = atmel_lcdfb_pan_display,
         .fb_fillrect    = cfb_fillrect,
         .fb_copyarea    = cfb_copyarea,
@@ -816,6 +845,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 sinfo->guard_time = pdata_sinfo->guard_time;
                 sinfo->smem_len = pdata_sinfo->smem_len;
                 sinfo->lcdcon_is_backlight = pdata_sinfo->lcdcon_is_backlight;
+                sinfo->lcdcon_pol_negative = pdata_sinfo->lcdcon_pol_negative;
                 sinfo->lcd_wiring_mode = pdata_sinfo->lcd_wiring_mode;
         } else {
                 dev_err(dev, "cannot get default configuration\n");
diff --git a/drivers/video/aty/aty128fb.c b/drivers/video/aty/aty128fb.c
index 34a0851bcbfa..b0b2ac335347 100644
--- a/drivers/video/aty/aty128fb.c
+++ b/drivers/video/aty/aty128fb.c
@@ -1786,7 +1786,7 @@ static int aty128_bl_get_brightness(struct backlight_device *bd)
         return bd->props.brightness;
 }
 
-static struct backlight_ops aty128_bl_data = {
+static const struct backlight_ops aty128_bl_data = {
         .get_brightness = aty128_bl_get_brightness,
         .update_status  = aty128_bl_update_status,
 };
@@ -1818,6 +1818,7 @@ static void aty128_bl_init(struct aty128fb_par *par)
         snprintf(name, sizeof(name), "aty128bl%d", info->node);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
         bd = backlight_device_register(name, info->dev, par, &aty128_bl_data,
                                        &props);
@@ -1860,11 +1861,11 @@ static void aty128_early_resume(void *data)
 {
         struct aty128fb_par *par = data;
 
-        if (try_acquire_console_sem())
+        if (!console_trylock())
                 return;
         pci_restore_state(par->pdev);
         aty128_do_resume(par->pdev);
-        release_console_sem();
+        console_unlock();
 }
 #endif /* CONFIG_PPC_PMAC */
 
@@ -2438,7 +2439,7 @@ static int aty128_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 
         printk(KERN_DEBUG "aty128fb: suspending...\n");
         
-        acquire_console_sem();
+        console_lock();
 
         fb_set_suspend(info, 1);
 
@@ -2470,7 +2471,7 @@ static int aty128_pci_suspend(struct pci_dev *pdev, pm_message_t state)
         if (state.event != PM_EVENT_ON)
                 aty128_set_suspend(par, 1);
 
-        release_console_sem();
+        console_unlock();
 
         pdev->dev.power.power_state = state;
 
@@ -2527,9 +2528,9 @@ static int aty128_pci_resume(struct pci_dev *pdev)
 {
         int rc;
 
-        acquire_console_sem();
+        console_lock();
         rc = aty128_do_resume(pdev);
-        release_console_sem();
+        console_unlock();
 
         return rc;
 }
diff --git a/drivers/video/aty/atyfb_base.c b/drivers/video/aty/atyfb_base.c
index f8d69ad36830..d7aaec5667bf 100644
--- a/drivers/video/aty/atyfb_base.c
+++ b/drivers/video/aty/atyfb_base.c
@@ -248,10 +248,6 @@ static int atyfb_sync(struct fb_info *info);
 
 static int aty_init(struct fb_info *info);
 
-#ifdef CONFIG_ATARI
-static int store_video_par(char *videopar, unsigned char m64_num);
-#endif
-
 static void aty_get_crtc(const struct atyfb_par *par, struct crtc *crtc);
 
 static void aty_set_crtc(const struct atyfb_par *par, const struct crtc *crtc);
@@ -2069,7 +2065,7 @@ static int atyfb_pci_suspend(struct pci_dev *pdev, pm_message_t state)
         if (state.event == pdev->dev.power.power_state.event)
                 return 0;
 
-        acquire_console_sem();
+        console_lock();
 
         fb_set_suspend(info, 1);
 
@@ -2097,14 +2093,14 @@ static int atyfb_pci_suspend(struct pci_dev *pdev, pm_message_t state)
                 par->lock_blank = 0;
                 atyfb_blank(FB_BLANK_UNBLANK, info);
                 fb_set_suspend(info, 0);
-                release_console_sem();
+                console_unlock();
                 return -EIO;
         }
 #else
         pci_set_power_state(pdev, pci_choose_state(pdev, state));
 #endif
 
-        release_console_sem();
+        console_unlock();
 
         pdev->dev.power.power_state = state;
 
@@ -2133,7 +2129,7 @@ static int atyfb_pci_resume(struct pci_dev *pdev)
         if (pdev->dev.power.power_state.event == PM_EVENT_ON)
                 return 0;
 
-        acquire_console_sem();
+        console_lock();
 
         /*
          * PCI state will have been restored by the core, so
@@ -2161,7 +2157,7 @@ static int atyfb_pci_resume(struct pci_dev *pdev)
         par->lock_blank = 0;
         atyfb_blank(FB_BLANK_UNBLANK, info);
 
-        release_console_sem();
+        console_unlock();
 
         pdev->dev.power.power_state = PMSG_ON;
 
@@ -2221,7 +2217,7 @@ static int aty_bl_get_brightness(struct backlight_device *bd)
         return bd->props.brightness;
 }
 
-static struct backlight_ops aty_bl_data = {
+static const struct backlight_ops aty_bl_data = {
         .get_brightness = aty_bl_get_brightness,
         .update_status  = aty_bl_update_status,
 };
@@ -2241,6 +2237,7 @@ static void aty_bl_init(struct atyfb_par *par)
         snprintf(name, sizeof(name), "atybl%d", info->node);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
         bd = backlight_device_register(name, info->dev, par, &aty_bl_data,
                                        &props);
@@ -2267,11 +2264,13 @@ error:
         return;
 }
 
+#ifdef CONFIG_PCI
 static void aty_bl_exit(struct backlight_device *bd)
 {
         backlight_device_unregister(bd);
         printk("aty: Backlight unloaded\n");
 }
+#endif /* CONFIG_PCI */
 
 #endif /* CONFIG_FB_ATY_BACKLIGHT */
 
@@ -2788,7 +2787,7 @@ aty_init_exit:
         return ret;
 }
 
-#ifdef CONFIG_ATARI
+#if defined(CONFIG_ATARI) && !defined(MODULE)
 static int __devinit store_video_par(char *video_str, unsigned char m64_num)
 {
         char *p;
@@ -2817,7 +2816,7 @@ static int __devinit store_video_par(char *video_str, unsigned char m64_num)
         phys_vmembase[m64_num] = 0;
         return -1;
 }
-#endif /* CONFIG_ATARI */
+#endif /* CONFIG_ATARI && !MODULE */
 
 /*
  * Blank the display.
@@ -2969,9 +2968,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;
         u32 mem, chip_id;
+        int i, j, ret;
 
         /*
          * Map memory-mapped registers.
@@ -3087,23 +3085,8 @@ static int __devinit atyfb_setup_sparc(struct pci_dev *pdev,
                 aty_st_le32(MEM_CNTL, mem, par);
         }
 
-        /*
-         * If this is the console device, we will set default video
-         * settings to what the PROM left us with.
-         */
-        node = prom_getchild(prom_root_node);
-        node = prom_searchsiblings(node, "aliases");
-        if (node) {
-                len = prom_getproperty(node, "screen", prop, sizeof(prop));
-                if (len > 0) {
-                        prop[len] = '\0';
-                        node = prom_finddevice(prop);
-                } else
-                        node = 0;
-        }
-
         dp = pci_device_to_OF_node(pdev);
-        if (node == dp->phandle) {
+        if (dp == of_console_device) {
                 struct fb_var_screeninfo *var = &default_var;
                 unsigned int N, P, Q, M, T, R;
                 u32 v_total, h_total;
@@ -3111,9 +3094,9 @@ static int __devinit atyfb_setup_sparc(struct pci_dev *pdev,
                 u8 pll_regs[16];
                 u8 clock_cntl;
 
-                crtc.vxres = prom_getintdefault(node, "width", 1024);
-                crtc.vyres = prom_getintdefault(node, "height", 768);
-                var->bits_per_pixel = prom_getintdefault(node, "depth", 8);
+                crtc.vxres = of_getintprop_default(dp, "width", 1024);
+                crtc.vyres = of_getintprop_default(dp, "height", 768);
+                var->bits_per_pixel = of_getintprop_default(dp, "depth", 8);
                 var->xoffset = var->yoffset = 0;
                 crtc.h_tot_disp = aty_ld_le32(CRTC_H_TOTAL_DISP, par);
                 crtc.h_sync_strt_wid = aty_ld_le32(CRTC_H_SYNC_STRT_WID, par);
@@ -3139,12 +3122,12 @@ static int __devinit atyfb_setup_sparc(struct pci_dev *pdev,
                 M = pll_regs[2];
 
                 /*
-                 * PLL Feedback Divider N (Dependant on CLOCK_CNTL):
+                 * PLL Feedback Divider N (Dependent on CLOCK_CNTL):
                  */
                 N = pll_regs[7 + (clock_cntl & 3)];
 
                 /*
-                 * PLL Post Divider P (Dependant on CLOCK_CNTL):
+                 * PLL Post Divider P (Dependent on CLOCK_CNTL):
                  */
                 P = 1 << (pll_regs[6] >> ((clock_cntl & 3) << 1));
 
diff --git a/drivers/video/aty/mach64_cursor.c b/drivers/video/aty/mach64_cursor.c
index 2ba8b3c421a1..46f72ed53510 100644
--- a/drivers/video/aty/mach64_cursor.c
+++ b/drivers/video/aty/mach64_cursor.c
@@ -51,7 +51,7 @@
  * to a larger number and saturate CUR_HORZ_POSN to zero.
  *
  * if Y becomes negative, CUR_VERT_OFFSET must be adjusted to a larger number,
- * CUR_OFFSET must be adjusted to a point to the appropraite line in the cursor
+ * CUR_OFFSET must be adjusted to a point to the appropriate line in the cursor
  * definitation and CUR_VERT_POSN must be saturated to zero.
  */
 
diff --git a/drivers/video/aty/radeon_backlight.c b/drivers/video/aty/radeon_backlight.c
index 256966e9667d..db572df7e1ef 100644
--- a/drivers/video/aty/radeon_backlight.c
+++ b/drivers/video/aty/radeon_backlight.c
@@ -128,7 +128,7 @@ static int radeon_bl_get_brightness(struct backlight_device *bd)
         return bd->props.brightness;
 }
 
-static struct backlight_ops radeon_bl_data = {
+static const struct backlight_ops radeon_bl_data = {
         .get_brightness = radeon_bl_get_brightness,
         .update_status  = radeon_bl_update_status,
 };
@@ -158,6 +158,7 @@ void radeonfb_bl_init(struct radeonfb_info *rinfo)
         snprintf(name, sizeof(name), "radeonbl%d", rinfo->info->node);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
         bd = backlight_device_register(name, rinfo->info->dev, pdata,
                                        &radeon_bl_data, &props);
diff --git a/drivers/video/aty/radeon_base.c b/drivers/video/aty/radeon_base.c
index 3c1e13ed1cba..32f8cf6200a7 100644
--- a/drivers/video/aty/radeon_base.c
+++ b/drivers/video/aty/radeon_base.c
@@ -1248,7 +1248,7 @@ static void radeon_write_pll_regs(struct radeonfb_info *rinfo, struct radeon_reg
 
         /* Workaround from XFree */
         if (rinfo->is_mobility) {
-                /* A temporal workaround for the occational blanking on certain laptop
+                /* A temporal workaround for the occasional blanking on certain laptop
                  * panels. This appears to related to the PLL divider registers
                  * (fail to lock?). It occurs even when all dividers are the same
                  * with their old settings. In this case we really don't need to
diff --git a/drivers/video/aty/radeon_i2c.c b/drivers/video/aty/radeon_i2c.c
index 359fc64e761a..ab1d0fd76316 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>
@@ -101,6 +100,9 @@ void radeon_create_i2c_busses(struct radeonfb_info *rinfo)
 {
         rinfo->i2c[0].rinfo     = rinfo;
         rinfo->i2c[0].ddc_reg   = GPIO_MONID;
+#ifndef CONFIG_PPC
+        rinfo->i2c[0].adapter.class = I2C_CLASS_HWMON;
+#endif
         radeon_setup_i2c_bus(&rinfo->i2c[0], "monid");
 
         rinfo->i2c[1].rinfo     = rinfo;
diff --git a/drivers/video/aty/radeon_pm.c b/drivers/video/aty/radeon_pm.c
index c4e17642d9c5..92bda5848516 100644
--- a/drivers/video/aty/radeon_pm.c
+++ b/drivers/video/aty/radeon_pm.c
@@ -2626,7 +2626,7 @@ int radeonfb_pci_suspend(struct pci_dev *pdev, pm_message_t mesg)
                 goto done;
         }
 
-        acquire_console_sem();
+        console_lock();
 
         fb_set_suspend(info, 1);
 
@@ -2690,7 +2690,7 @@ int radeonfb_pci_suspend(struct pci_dev *pdev, pm_message_t mesg)
         if (rinfo->pm_mode & radeon_pm_d2)
                 radeon_set_suspend(rinfo, 1);
 
-        release_console_sem();
+        console_unlock();
 
  done:
         pdev->dev.power.power_state = mesg;
@@ -2715,10 +2715,10 @@ int radeonfb_pci_resume(struct pci_dev *pdev)
                 return 0;
 
         if (rinfo->no_schedule) {
-                if (try_acquire_console_sem())
+                if (!console_trylock())
                         return 0;
         } else
-                acquire_console_sem();
+                console_lock();
 
         printk(KERN_DEBUG "radeonfb (%s): resuming from state: %d...\n",
                pci_name(pdev), pdev->dev.power.power_state.event);
@@ -2783,7 +2783,7 @@ int radeonfb_pci_resume(struct pci_dev *pdev)
         pdev->dev.power.power_state = PMSG_ON;
 
  bail:
-        release_console_sem();
+        console_unlock();
 
         return rc;
 }
diff --git a/drivers/video/au1200fb.c b/drivers/video/au1200fb.c
index e77e8e4280fb..5dff32ac8044 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;
@@ -1572,7 +1572,7 @@ static int au1200fb_init_fbinfo(struct au1200fb_device *fbdev)
         /* Copy monitor specs from panel data */
         /* fixme: we're setting up LCD controller windows, so these dont give a
         damn as to what the monitor specs are (the panel itself does, but that
-        isnt done here...so maybe need a generic catchall monitor setting??? */
+        isn't done here...so maybe need a generic catchall monitor setting??? */
         memcpy(&fbi->monspecs, &panel->monspecs, sizeof(struct fb_monspecs));
 
         /* We first try the user mode passed in argument. If that failed,
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index 38ffc3fbcbe4..c04b94da81f7 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -12,16 +12,16 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/fb.h>
 #include <linux/i2c.h>
 #include <linux/backlight.h>
 #include <linux/mfd/88pm860x.h>
-#include <linux/slab.h>
 
 #define MAX_BRIGHTNESS          (0xFF)
 #define MIN_BRIGHTNESS          (0)
 
-#define CURRENT_MASK            (0x1F << 1)
+#define CURRENT_BITMASK         (0x1F << 1)
 
 struct pm860x_backlight_data {
         struct pm860x_chip *chip;
@@ -85,7 +85,7 @@ static int pm860x_backlight_set(struct backlight_device *bl, int brightness)
         if ((data->current_brightness == 0) && brightness) {
                 if (data->iset) {
                         ret = pm860x_set_bits(data->i2c, wled_idc(data->port),
-                                              CURRENT_MASK, data->iset);
+                                              CURRENT_BITMASK, data->iset);
                         if (ret < 0)
                                 goto out;
                 }
@@ -155,35 +155,15 @@ out:
         return -EINVAL;
 }
 
-static struct backlight_ops pm860x_backlight_ops = {
+static const struct backlight_ops pm860x_backlight_ops = {
         .options        = BL_CORE_SUSPENDRESUME,
         .update_status  = pm860x_backlight_update_status,
         .get_brightness = pm860x_backlight_get_brightness,
 };
 
-static int __check_device(struct pm860x_backlight_pdata *pdata, char *name)
-{
-        struct pm860x_backlight_pdata *p = pdata;
-        int ret = -EINVAL;
-
-        while (p && p->id) {
-                if ((p->id != PM8606_ID_BACKLIGHT) || (p->flags < 0))
-                        break;
-
-                if (!strncmp(name, pm860x_backlight_name[p->flags],
-                        MFD_NAME_SIZE)) {
-                        ret = (int)p->flags;
-                        break;
-                }
-                p++;
-        }
-        return ret;
-}
-
 static int pm860x_backlight_probe(struct platform_device *pdev)
 {
         struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
-        struct pm860x_platform_data *pm860x_pdata;
         struct pm860x_backlight_pdata *pdata = NULL;
         struct pm860x_backlight_data *data;
         struct backlight_device *bl;
@@ -199,10 +179,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
                 return -EINVAL;
         }
 
-        if (pdev->dev.parent->platform_data) {
-                pm860x_pdata = pdev->dev.parent->platform_data;
-                pdata = pm860x_pdata->backlight;
-        }
+        pdata = pdev->dev.platform_data;
         if (pdata == NULL) {
                 dev_err(&pdev->dev, "platform data isn't assigned to "
                         "backlight\n");
@@ -219,7 +196,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
         data->current_brightness = MAX_BRIGHTNESS;
         data->pwm = pdata->pwm;
         data->iset = pdata->iset;
-        data->port = __check_device(pdata, name);
+        data->port = pdata->flags;
         if (data->port < 0) {
                 dev_err(&pdev->dev, "wrong platform data is assigned");
                 kfree(data);
@@ -227,6 +204,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
         }
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = MAX_BRIGHTNESS;
         bl = backlight_device_register(name, &pdev->dev, data,
                                         &pm860x_backlight_ops, &props);
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index e54a337227ea..2d93c8d61ad5 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -109,6 +109,14 @@ config LCD_S6E63M0
           If you have an S6E63M0 LCD Panel, say Y to enable its
           LCD control driver.
 
+config LCD_LD9040
+        tristate "LD9040 AMOLED LCD Driver"
+        depends on SPI && BACKLIGHT_CLASS_DEVICE
+        default n
+        help
+          If you have an LD9040 Panel, say Y to enable its
+          control driver.
+
 endif # LCD_CLASS_DEVICE
 
 #
@@ -236,12 +244,12 @@ config BACKLIGHT_MAX8925
           If you have a LCD backlight connected to the WLED output of MAX8925
           WLED output, say Y here to enable this driver.
 
-config BACKLIGHT_MBP_NVIDIA
-       tristate "MacBook Pro Nvidia Backlight Driver"
-       depends on X86
+config BACKLIGHT_APPLE
+       tristate "Apple Backlight Driver"
+       depends on X86 && ACPI
        help
-         If you have an Apple Macbook Pro with Nvidia graphics hardware say Y
-         to enable a driver for its backlight
+         If you have an Intel-based Apple say Y to enable a driver for its
+         backlight.
 
 config BACKLIGHT_TOSA
         tristate "Sharp SL-6000 Backlight Driver"
@@ -294,6 +302,18 @@ config BACKLIGHT_ADP8860
           To compile this driver as a module, choose M here: the module will
           be called adp8860_bl.
 
+config BACKLIGHT_ADP8870
+        tristate "Backlight Driver for ADP8870 using WLED"
+        depends on BACKLIGHT_CLASS_DEVICE && I2C
+        select NEW_LEDS
+        select LEDS_CLASS
+        help
+          If you have a LCD backlight connected to the ADP8870,
+          say Y here to enable this driver.
+
+          To compile this driver as a module, choose M here: the module will
+          be called adp8870_bl.
+
 config BACKLIGHT_88PM860X
         tristate "Backlight Driver for 88PM8606 using WLED"
         depends on MFD_88PM860X
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index 44c0f81ad85d..ee72adb8786e 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_LCD_VGG2432A4)	   += vgg2432a4.o
 obj-$(CONFIG_LCD_TDO24M)           += tdo24m.o
 obj-$(CONFIG_LCD_TOSA)             += tosa_lcd.o
 obj-$(CONFIG_LCD_S6E63M0)       += s6e63m0.o
+obj-$(CONFIG_LCD_LD9040)        += ld9040.o
 
 obj-$(CONFIG_BACKLIGHT_CLASS_DEVICE) += backlight.o
 obj-$(CONFIG_BACKLIGHT_ATMEL_PWM)    += atmel-pwm-bl.o
@@ -26,13 +27,14 @@ obj-$(CONFIG_BACKLIGHT_CARILLO_RANCH) += cr_bllcd.o
 obj-$(CONFIG_BACKLIGHT_PWM)     += pwm_bl.o
 obj-$(CONFIG_BACKLIGHT_DA903X)  += da903x_bl.o
 obj-$(CONFIG_BACKLIGHT_MAX8925) += max8925_bl.o
-obj-$(CONFIG_BACKLIGHT_MBP_NVIDIA) += mbp_nvidia_bl.o
+obj-$(CONFIG_BACKLIGHT_APPLE)   += apple_bl.o
 obj-$(CONFIG_BACKLIGHT_TOSA)    += tosa_bl.o
 obj-$(CONFIG_BACKLIGHT_SAHARA)  += kb3886_bl.o
 obj-$(CONFIG_BACKLIGHT_WM831X)  += wm831x_bl.o
 obj-$(CONFIG_BACKLIGHT_ADX)    += adx_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP5520) += adp5520_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP8860) += adp8860_bl.o
+obj-$(CONFIG_BACKLIGHT_ADP8870) += adp8870_bl.o
 obj-$(CONFIG_BACKLIGHT_88PM860X) += 88pm860x_bl.o
 obj-$(CONFIG_BACKLIGHT_PCF50633)        += pcf50633-backlight.o
 
diff --git a/drivers/video/backlight/adp5520_bl.c b/drivers/video/backlight/adp5520_bl.c
index 9f436e014f85..d1aee730d7d8 100644
--- a/drivers/video/backlight/adp5520_bl.c
+++ b/drivers/video/backlight/adp5520_bl.c
@@ -211,8 +211,12 @@ static ssize_t adp5520_bl_daylight_max_store(struct device *dev,
                         const char *buf, size_t count)
 {
         struct adp5520_bl *data = dev_get_drvdata(dev);
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+        if (ret < 0)
+                return ret;
 
-        strict_strtoul(buf, 10, &data->cached_daylight_max);
         return adp5520_store(dev, buf, count, ADP5520_DAYLIGHT_MAX);
 }
 static DEVICE_ATTR(daylight_max, 0664, adp5520_bl_daylight_max_show,
@@ -303,6 +307,7 @@ static int __devinit adp5520_bl_probe(struct platform_device *pdev)
         mutex_init(&data->lock);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = ADP5020_MAX_BRIGHTNESS;
         bl = backlight_device_register(pdev->name, data->master, data,
                                        &adp5520_bl_ops, &props);
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
index 3ec24609151e..d2a96a421ffd 100644
--- a/drivers/video/backlight/adp8860_bl.c
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -502,8 +502,10 @@ static ssize_t adp8860_bl_l1_daylight_max_store(struct device *dev,
                 struct device_attribute *attr, const char *buf, size_t count)
 {
         struct adp8860_bl *data = dev_get_drvdata(dev);
+        int ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+        if (ret)
+                return ret;
 
-        strict_strtoul(buf, 10, &data->cached_daylight_max);
         return adp8860_store(dev, buf, count, ADP8860_BLMX1);
 }
 static DEVICE_ATTR(l1_daylight_max, 0664, adp8860_bl_l1_daylight_max_show,
@@ -614,7 +616,7 @@ static ssize_t adp8860_bl_ambient_light_zone_store(struct device *dev,
         if (val == 0) {
                 /* Enable automatic ambient light sensing */
                 adp8860_set_bits(data->client, ADP8860_MDCR, CMP_AUTOEN);
-        } else if ((val > 0) && (val < 6)) {
+        } else if ((val > 0) && (val <= 3)) {
                 /* Disable automatic ambient light sensing */
                 adp8860_clr_bits(data->client, ADP8860_MDCR, CMP_AUTOEN);
 
@@ -622,7 +624,7 @@ static ssize_t adp8860_bl_ambient_light_zone_store(struct device *dev,
                 mutex_lock(&data->lock);
                 adp8860_read(data->client, ADP8860_CFGR, &reg_val);
                 reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
-                reg_val |= val << CFGR_BLV_SHIFT;
+                reg_val |= (val - 1) << CFGR_BLV_SHIFT;
                 adp8860_write(data->client, ADP8860_CFGR, reg_val);
                 mutex_unlock(&data->lock);
         }
@@ -707,6 +709,7 @@ static int __devinit adp8860_probe(struct i2c_client *client,
         i2c_set_clientdata(client, data);
 
         memset(&props, 0, sizeof(props));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = ADP8860_MAX_BRIGHTNESS;
 
         mutex_init(&data->lock);
diff --git a/drivers/video/backlight/adp8870_bl.c b/drivers/video/backlight/adp8870_bl.c
new file mode 100644
index 000000000000..05a8832bb3eb
--- /dev/null
+++ b/drivers/video/backlight/adp8870_bl.c
@@ -0,0 +1,1012 @@
+/*
+ * Backlight driver for Analog Devices ADP8870 Backlight Devices
+ *
+ * Copyright 2009-2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/leds.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+
+#include <linux/i2c/adp8870.h>
+#define ADP8870_EXT_FEATURES
+#define ADP8870_USE_LEDS
+
+
+#define ADP8870_MFDVID  0x00  /* Manufacturer and device ID */
+#define ADP8870_MDCR    0x01  /* Device mode and status */
+#define ADP8870_INT_STAT 0x02  /* Interrupts status */
+#define ADP8870_INT_EN  0x03  /* Interrupts enable */
+#define ADP8870_CFGR    0x04  /* Configuration register */
+#define ADP8870_BLSEL   0x05  /* Sink enable backlight or independent */
+#define ADP8870_PWMLED  0x06  /* PWM Enable Selection Register */
+#define ADP8870_BLOFF   0x07  /* Backlight off timeout */
+#define ADP8870_BLDIM   0x08  /* Backlight dim timeout */
+#define ADP8870_BLFR    0x09  /* Backlight fade in and out rates */
+#define ADP8870_BLMX1   0x0A  /* Backlight (Brightness Level 1-daylight) maximum current */
+#define ADP8870_BLDM1   0x0B  /* Backlight (Brightness Level 1-daylight) dim current */
+#define ADP8870_BLMX2   0x0C  /* Backlight (Brightness Level 2-bright) maximum current */
+#define ADP8870_BLDM2   0x0D  /* Backlight (Brightness Level 2-bright) dim current */
+#define ADP8870_BLMX3   0x0E  /* Backlight (Brightness Level 3-office) maximum current */
+#define ADP8870_BLDM3   0x0F  /* Backlight (Brightness Level 3-office) dim current */
+#define ADP8870_BLMX4   0x10  /* Backlight (Brightness Level 4-indoor) maximum current */
+#define ADP8870_BLDM4   0x11  /* Backlight (Brightness Level 4-indoor) dim current */
+#define ADP8870_BLMX5   0x12  /* Backlight (Brightness Level 5-dark) maximum current */
+#define ADP8870_BLDM5   0x13  /* Backlight (Brightness Level 5-dark) dim current */
+#define ADP8870_ISCLAW  0x1A  /* Independent sink current fade law register */
+#define ADP8870_ISCC    0x1B  /* Independent sink current control register */
+#define ADP8870_ISCT1   0x1C  /* Independent Sink Current Timer Register LED[7:5] */
+#define ADP8870_ISCT2   0x1D  /* Independent Sink Current Timer Register LED[4:1] */
+#define ADP8870_ISCF    0x1E  /* Independent sink current fade register */
+#define ADP8870_ISC1    0x1F  /* Independent Sink Current LED1 */
+#define ADP8870_ISC2    0x20  /* Independent Sink Current LED2 */
+#define ADP8870_ISC3    0x21  /* Independent Sink Current LED3 */
+#define ADP8870_ISC4    0x22  /* Independent Sink Current LED4 */
+#define ADP8870_ISC5    0x23  /* Independent Sink Current LED5 */
+#define ADP8870_ISC6    0x24  /* Independent Sink Current LED6 */
+#define ADP8870_ISC7    0x25  /* Independent Sink Current LED7 (Brightness Level 1-daylight) */
+#define ADP8870_ISC7_L2 0x26  /* Independent Sink Current LED7 (Brightness Level 2-bright) */
+#define ADP8870_ISC7_L3 0x27  /* Independent Sink Current LED7 (Brightness Level 3-office) */
+#define ADP8870_ISC7_L4 0x28  /* Independent Sink Current LED7 (Brightness Level 4-indoor) */
+#define ADP8870_ISC7_L5 0x29  /* Independent Sink Current LED7 (Brightness Level 5-dark) */
+#define ADP8870_CMP_CTL 0x2D  /* ALS Comparator Control Register */
+#define ADP8870_ALS1_EN 0x2E  /* Main ALS comparator level enable */
+#define ADP8870_ALS2_EN 0x2F  /* Second ALS comparator level enable */
+#define ADP8870_ALS1_STAT 0x30  /* Main ALS Comparator Status Register */
+#define ADP8870_ALS2_STAT 0x31  /* Second ALS Comparator Status Register */
+#define ADP8870_L2TRP   0x32  /* L2 comparator reference */
+#define ADP8870_L2HYS   0x33  /* L2 hysteresis */
+#define ADP8870_L3TRP   0x34  /* L3 comparator reference */
+#define ADP8870_L3HYS   0x35  /* L3 hysteresis */
+#define ADP8870_L4TRP   0x36  /* L4 comparator reference */
+#define ADP8870_L4HYS   0x37  /* L4 hysteresis */
+#define ADP8870_L5TRP   0x38  /* L5 comparator reference */
+#define ADP8870_L5HYS   0x39  /* L5 hysteresis */
+#define ADP8870_PH1LEVL 0x40  /* First phototransistor ambient light level-low byte register */
+#define ADP8870_PH1LEVH 0x41  /* First phototransistor ambient light level-high byte register */
+#define ADP8870_PH2LEVL 0x42  /* Second phototransistor ambient light level-low byte register */
+#define ADP8870_PH2LEVH 0x43  /* Second phototransistor ambient light level-high byte register */
+
+#define ADP8870_MANUFID         0x3  /* Analog Devices AD8870 Manufacturer and device ID */
+#define ADP8870_DEVID(x)        ((x) & 0xF)
+#define ADP8870_MANID(x)        ((x) >> 4)
+
+/* MDCR Device mode and status */
+#define D7ALSEN                 (1 << 7)
+#define INT_CFG                 (1 << 6)
+#define NSTBY                   (1 << 5)
+#define DIM_EN                  (1 << 4)
+#define GDWN_DIS                (1 << 3)
+#define SIS_EN                  (1 << 2)
+#define CMP_AUTOEN              (1 << 1)
+#define BLEN                    (1 << 0)
+
+/* ADP8870_ALS1_EN Main ALS comparator level enable */
+#define L5_EN                   (1 << 3)
+#define L4_EN                   (1 << 2)
+#define L3_EN                   (1 << 1)
+#define L2_EN                   (1 << 0)
+
+#define CFGR_BLV_SHIFT          3
+#define CFGR_BLV_MASK           0x7
+#define ADP8870_FLAG_LED_MASK   0xFF
+
+#define FADE_VAL(in, out)       ((0xF & (in)) | ((0xF & (out)) << 4))
+#define BL_CFGR_VAL(law, blv)   ((((blv) & CFGR_BLV_MASK) << CFGR_BLV_SHIFT) | ((0x3 & (law)) << 1))
+#define ALS_CMPR_CFG_VAL(filt)  ((0x7 & (filt)) << 1)
+
+struct adp8870_bl {
+        struct i2c_client *client;
+        struct backlight_device *bl;
+        struct adp8870_led *led;
+        struct adp8870_backlight_platform_data *pdata;
+        struct mutex lock;
+        unsigned long cached_daylight_max;
+        int id;
+        int revid;
+        int current_brightness;
+};
+
+struct adp8870_led {
+        struct led_classdev     cdev;
+        struct work_struct      work;
+        struct i2c_client       *client;
+        enum led_brightness     new_brightness;
+        int                     id;
+        int                     flags;
+};
+
+static int adp8870_read(struct i2c_client *client, int reg, uint8_t *val)
+{
+        int ret;
+
+        ret = i2c_smbus_read_byte_data(client, reg);
+        if (ret < 0) {
+                dev_err(&client->dev, "failed reading at 0x%02x\n", reg);
+                return ret;
+        }
+
+        *val = ret;
+        return 0;
+}
+
+
+static int adp8870_write(struct i2c_client *client, u8 reg, u8 val)
+{
+        int ret = i2c_smbus_write_byte_data(client, reg, val);
+        if (ret)
+                dev_err(&client->dev, "failed to write\n");
+
+        return ret;
+}
+
+static int adp8870_set_bits(struct i2c_client *client, int reg, uint8_t bit_mask)
+{
+        struct adp8870_bl *data = i2c_get_clientdata(client);
+        uint8_t reg_val;
+        int ret;
+
+        mutex_lock(&data->lock);
+
+        ret = adp8870_read(client, reg, &reg_val);
+
+        if (!ret && ((reg_val & bit_mask) == 0)) {
+                reg_val |= bit_mask;
+                ret = adp8870_write(client, reg, reg_val);
+        }
+
+        mutex_unlock(&data->lock);
+        return ret;
+}
+
+static int adp8870_clr_bits(struct i2c_client *client, int reg, uint8_t bit_mask)
+{
+        struct adp8870_bl *data = i2c_get_clientdata(client);
+        uint8_t reg_val;
+        int ret;
+
+        mutex_lock(&data->lock);
+
+        ret = adp8870_read(client, reg, &reg_val);
+
+        if (!ret && (reg_val & bit_mask)) {
+                reg_val &= ~bit_mask;
+                ret = adp8870_write(client, reg, reg_val);
+        }
+
+        mutex_unlock(&data->lock);
+        return ret;
+}
+
+/*
+ * Independent sink / LED
+ */
+#if defined(ADP8870_USE_LEDS)
+static void adp8870_led_work(struct work_struct *work)
+{
+        struct adp8870_led *led = container_of(work, struct adp8870_led, work);
+        adp8870_write(led->client, ADP8870_ISC1 + led->id - 1,
+                         led->new_brightness >> 1);
+}
+
+static void adp8870_led_set(struct led_classdev *led_cdev,
+                           enum led_brightness value)
+{
+        struct adp8870_led *led;
+
+        led = container_of(led_cdev, struct adp8870_led, cdev);
+        led->new_brightness = value;
+        /*
+         * Use workqueue for IO since I2C operations can sleep.
+         */
+        schedule_work(&led->work);
+}
+
+static int adp8870_led_setup(struct adp8870_led *led)
+{
+        struct i2c_client *client = led->client;
+        int ret = 0;
+
+        ret = adp8870_write(client, ADP8870_ISC1 + led->id - 1, 0);
+        if (ret)
+                return ret;
+
+        ret = adp8870_set_bits(client, ADP8870_ISCC, 1 << (led->id - 1));
+        if (ret)
+                return ret;
+
+        if (led->id > 4)
+                ret = adp8870_set_bits(client, ADP8870_ISCT1,
+                                (led->flags & 0x3) << ((led->id - 5) * 2));
+        else
+                ret = adp8870_set_bits(client, ADP8870_ISCT2,
+                                (led->flags & 0x3) << ((led->id - 1) * 2));
+
+        return ret;
+}
+
+static int __devinit adp8870_led_probe(struct i2c_client *client)
+{
+        struct adp8870_backlight_platform_data *pdata =
+                client->dev.platform_data;
+        struct adp8870_bl *data = i2c_get_clientdata(client);
+        struct adp8870_led *led, *led_dat;
+        struct led_info *cur_led;
+        int ret, i;
+
+
+        led = kcalloc(pdata->num_leds, sizeof(*led), GFP_KERNEL);
+        if (led == NULL) {
+                dev_err(&client->dev, "failed to alloc memory\n");
+                return -ENOMEM;
+        }
+
+        ret = adp8870_write(client, ADP8870_ISCLAW, pdata->led_fade_law);
+        if (ret)
+                goto err_free;
+
+        ret = adp8870_write(client, ADP8870_ISCT1,
+                        (pdata->led_on_time & 0x3) << 6);
+        if (ret)
+                goto err_free;
+
+        ret = adp8870_write(client, ADP8870_ISCF,
+                        FADE_VAL(pdata->led_fade_in, pdata->led_fade_out));
+        if (ret)
+                goto err_free;
+
+        for (i = 0; i < pdata->num_leds; ++i) {
+                cur_led = &pdata->leds[i];
+                led_dat = &led[i];
+
+                led_dat->id = cur_led->flags & ADP8870_FLAG_LED_MASK;
+
+                if (led_dat->id > 7 || led_dat->id < 1) {
+                        dev_err(&client->dev, "Invalid LED ID %d\n",
+                                led_dat->id);
+                        goto err;
+                }
+
+                if (pdata->bl_led_assign & (1 << (led_dat->id - 1))) {
+                        dev_err(&client->dev, "LED %d used by Backlight\n",
+                                led_dat->id);
+                        goto err;
+                }
+
+                led_dat->cdev.name = cur_led->name;
+                led_dat->cdev.default_trigger = cur_led->default_trigger;
+                led_dat->cdev.brightness_set = adp8870_led_set;
+                led_dat->cdev.brightness = LED_OFF;
+                led_dat->flags = cur_led->flags >> FLAG_OFFT_SHIFT;
+                led_dat->client = client;
+                led_dat->new_brightness = LED_OFF;
+                INIT_WORK(&led_dat->work, adp8870_led_work);
+
+                ret = led_classdev_register(&client->dev, &led_dat->cdev);
+                if (ret) {
+                        dev_err(&client->dev, "failed to register LED %d\n",
+                                led_dat->id);
+                        goto err;
+                }
+
+                ret = adp8870_led_setup(led_dat);
+                if (ret) {
+                        dev_err(&client->dev, "failed to write\n");
+                        i++;
+                        goto err;
+                }
+        }
+
+        data->led = led;
+
+        return 0;
+
+ err:
+        for (i = i - 1; i >= 0; --i) {
+                led_classdev_unregister(&led[i].cdev);
+                cancel_work_sync(&led[i].work);
+        }
+
+ err_free:
+        kfree(led);
+
+        return ret;
+}
+
+static int __devexit adp8870_led_remove(struct i2c_client *client)
+{
+        struct adp8870_backlight_platform_data *pdata =
+                client->dev.platform_data;
+        struct adp8870_bl *data = i2c_get_clientdata(client);
+        int i;
+
+        for (i = 0; i < pdata->num_leds; i++) {
+                led_classdev_unregister(&data->led[i].cdev);
+                cancel_work_sync(&data->led[i].work);
+        }
+
+        kfree(data->led);
+        return 0;
+}
+#else
+static int __devinit adp8870_led_probe(struct i2c_client *client)
+{
+        return 0;
+}
+
+static int __devexit adp8870_led_remove(struct i2c_client *client)
+{
+        return 0;
+}
+#endif
+
+static int adp8870_bl_set(struct backlight_device *bl, int brightness)
+{
+        struct adp8870_bl *data = bl_get_data(bl);
+        struct i2c_client *client = data->client;
+        int ret = 0;
+
+        if (data->pdata->en_ambl_sens) {
+                if ((brightness > 0) && (brightness < ADP8870_MAX_BRIGHTNESS)) {
+                        /* Disable Ambient Light auto adjust */
+                        ret = adp8870_clr_bits(client, ADP8870_MDCR,
+                                        CMP_AUTOEN);
+                        if (ret)
+                                return ret;
+                        ret = adp8870_write(client, ADP8870_BLMX1, brightness);
+                        if (ret)
+                                return ret;
+                } else {
+                        /*
+                         * MAX_BRIGHTNESS -> Enable Ambient Light auto adjust
+                         * restore daylight l1 sysfs brightness
+                         */
+                        ret = adp8870_write(client, ADP8870_BLMX1,
+                                         data->cached_daylight_max);
+                        if (ret)
+                                return ret;
+
+                        ret = adp8870_set_bits(client, ADP8870_MDCR,
+                                         CMP_AUTOEN);
+                        if (ret)
+                                return ret;
+                }
+        } else {
+                ret = adp8870_write(client, ADP8870_BLMX1, brightness);
+                if (ret)
+                        return ret;
+        }
+
+        if (data->current_brightness && brightness == 0)
+                ret = adp8870_set_bits(client,
+                                ADP8870_MDCR, DIM_EN);
+        else if (data->current_brightness == 0 && brightness)
+                ret = adp8870_clr_bits(client,
+                                ADP8870_MDCR, DIM_EN);
+
+        if (!ret)
+                data->current_brightness = brightness;
+
+        return ret;
+}
+
+static int adp8870_bl_update_status(struct backlight_device *bl)
+{
+        int brightness = bl->props.brightness;
+        if (bl->props.power != FB_BLANK_UNBLANK)
+                brightness = 0;
+
+        if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+                brightness = 0;
+
+        return adp8870_bl_set(bl, brightness);
+}
+
+static int adp8870_bl_get_brightness(struct backlight_device *bl)
+{
+        struct adp8870_bl *data = bl_get_data(bl);
+
+        return data->current_brightness;
+}
+
+static const struct backlight_ops adp8870_bl_ops = {
+        .update_status  = adp8870_bl_update_status,
+        .get_brightness = adp8870_bl_get_brightness,
+};
+
+static int adp8870_bl_setup(struct backlight_device *bl)
+{
+        struct adp8870_bl *data = bl_get_data(bl);
+        struct i2c_client *client = data->client;
+        struct adp8870_backlight_platform_data *pdata = data->pdata;
+        int ret = 0;
+
+        ret = adp8870_write(client, ADP8870_BLSEL, ~pdata->bl_led_assign);
+        if (ret)
+                return ret;
+
+        ret = adp8870_write(client, ADP8870_PWMLED, pdata->pwm_assign);
+        if (ret)
+                return ret;
+
+        ret = adp8870_write(client, ADP8870_BLMX1, pdata->l1_daylight_max);
+        if (ret)
+                return ret;
+
+        ret = adp8870_write(client, ADP8870_BLDM1, pdata->l1_daylight_dim);
+        if (ret)
+                return ret;
+
+        if (pdata->en_ambl_sens) {
+                data->cached_daylight_max = pdata->l1_daylight_max;
+                ret = adp8870_write(client, ADP8870_BLMX2,
+                                                pdata->l2_bright_max);
+                if (ret)
+                        return ret;
+                ret = adp8870_write(client, ADP8870_BLDM2,
+                                                pdata->l2_bright_dim);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_BLMX3,
+                                                pdata->l3_office_max);
+                if (ret)
+                        return ret;
+                ret = adp8870_write(client, ADP8870_BLDM3,
+                                                pdata->l3_office_dim);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_BLMX4,
+                                                pdata->l4_indoor_max);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_BLDM4,
+                                                pdata->l4_indor_dim);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_BLMX5,
+                                                pdata->l5_dark_max);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_BLDM5,
+                                                pdata->l5_dark_dim);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L2TRP, pdata->l2_trip);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L2HYS, pdata->l2_hyst);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L3TRP, pdata->l3_trip);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L3HYS, pdata->l3_hyst);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L4TRP, pdata->l4_trip);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L4HYS, pdata->l4_hyst);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L5TRP, pdata->l5_trip);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_L5HYS, pdata->l5_hyst);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_ALS1_EN, L5_EN | L4_EN |
+                                                L3_EN | L2_EN);
+                if (ret)
+                        return ret;
+
+                ret = adp8870_write(client, ADP8870_CMP_CTL,
+                        ALS_CMPR_CFG_VAL(pdata->abml_filt));
+                if (ret)
+                        return ret;
+        }
+
+        ret = adp8870_write(client, ADP8870_CFGR,
+                        BL_CFGR_VAL(pdata->bl_fade_law, 0));
+        if (ret)
+                return ret;
+
+        ret = adp8870_write(client, ADP8870_BLFR, FADE_VAL(pdata->bl_fade_in,
+                        pdata->bl_fade_out));
+        if (ret)
+                return ret;
+        /*
+         * ADP8870 Rev0 requires GDWN_DIS bit set
+         */
+
+        ret = adp8870_set_bits(client, ADP8870_MDCR, BLEN | DIM_EN | NSTBY |
+                        (data->revid == 0 ? GDWN_DIS : 0));
+
+        return ret;
+}
+
+static ssize_t adp8870_show(struct device *dev, char *buf, int reg)
+{
+        struct adp8870_bl *data = dev_get_drvdata(dev);
+        int error;
+        uint8_t reg_val;
+
+        mutex_lock(&data->lock);
+        error = adp8870_read(data->client, reg, &reg_val);
+        mutex_unlock(&data->lock);
+
+        if (error < 0)
+                return error;
+
+        return sprintf(buf, "%u\n", reg_val);
+}
+
+static ssize_t adp8870_store(struct device *dev, const char *buf,
+                         size_t count, int reg)
+{
+        struct adp8870_bl *data = dev_get_drvdata(dev);
+        unsigned long val;
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &val);
+        if (ret)
+                return ret;
+
+        mutex_lock(&data->lock);
+        adp8870_write(data->client, reg, val);
+        mutex_unlock(&data->lock);
+
+        return count;
+}
+
+static ssize_t adp8870_bl_l5_dark_max_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLMX5);
+}
+
+static ssize_t adp8870_bl_l5_dark_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLMX5);
+}
+static DEVICE_ATTR(l5_dark_max, 0664, adp8870_bl_l5_dark_max_show,
+                        adp8870_bl_l5_dark_max_store);
+
+
+static ssize_t adp8870_bl_l4_indoor_max_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLMX4);
+}
+
+static ssize_t adp8870_bl_l4_indoor_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLMX4);
+}
+static DEVICE_ATTR(l4_indoor_max, 0664, adp8870_bl_l4_indoor_max_show,
+                        adp8870_bl_l4_indoor_max_store);
+
+
+static ssize_t adp8870_bl_l3_office_max_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLMX3);
+}
+
+static ssize_t adp8870_bl_l3_office_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLMX3);
+}
+
+static DEVICE_ATTR(l3_office_max, 0664, adp8870_bl_l3_office_max_show,
+                        adp8870_bl_l3_office_max_store);
+
+static ssize_t adp8870_bl_l2_bright_max_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLMX2);
+}
+
+static ssize_t adp8870_bl_l2_bright_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLMX2);
+}
+static DEVICE_ATTR(l2_bright_max, 0664, adp8870_bl_l2_bright_max_show,
+                        adp8870_bl_l2_bright_max_store);
+
+static ssize_t adp8870_bl_l1_daylight_max_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLMX1);
+}
+
+static ssize_t adp8870_bl_l1_daylight_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct adp8870_bl *data = dev_get_drvdata(dev);
+        int ret = strict_strtoul(buf, 10, &data->cached_daylight_max);
+        if (ret)
+                return ret;
+
+        return adp8870_store(dev, buf, count, ADP8870_BLMX1);
+}
+static DEVICE_ATTR(l1_daylight_max, 0664, adp8870_bl_l1_daylight_max_show,
+                        adp8870_bl_l1_daylight_max_store);
+
+static ssize_t adp8870_bl_l5_dark_dim_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLDM5);
+}
+
+static ssize_t adp8870_bl_l5_dark_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLDM5);
+}
+static DEVICE_ATTR(l5_dark_dim, 0664, adp8870_bl_l5_dark_dim_show,
+                        adp8870_bl_l5_dark_dim_store);
+
+static ssize_t adp8870_bl_l4_indoor_dim_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLDM4);
+}
+
+static ssize_t adp8870_bl_l4_indoor_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLDM4);
+}
+static DEVICE_ATTR(l4_indoor_dim, 0664, adp8870_bl_l4_indoor_dim_show,
+                        adp8870_bl_l4_indoor_dim_store);
+
+
+static ssize_t adp8870_bl_l3_office_dim_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLDM3);
+}
+
+static ssize_t adp8870_bl_l3_office_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLDM3);
+}
+static DEVICE_ATTR(l3_office_dim, 0664, adp8870_bl_l3_office_dim_show,
+                        adp8870_bl_l3_office_dim_store);
+
+static ssize_t adp8870_bl_l2_bright_dim_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLDM2);
+}
+
+static ssize_t adp8870_bl_l2_bright_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLDM2);
+}
+static DEVICE_ATTR(l2_bright_dim, 0664, adp8870_bl_l2_bright_dim_show,
+                        adp8870_bl_l2_bright_dim_store);
+
+static ssize_t adp8870_bl_l1_daylight_dim_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        return adp8870_show(dev, buf, ADP8870_BLDM1);
+}
+
+static ssize_t adp8870_bl_l1_daylight_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8870_store(dev, buf, count, ADP8870_BLDM1);
+}
+static DEVICE_ATTR(l1_daylight_dim, 0664, adp8870_bl_l1_daylight_dim_show,
+                        adp8870_bl_l1_daylight_dim_store);
+
+#ifdef ADP8870_EXT_FEATURES
+static ssize_t adp8870_bl_ambient_light_level_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        struct adp8870_bl *data = dev_get_drvdata(dev);
+        int error;
+        uint8_t reg_val;
+        uint16_t ret_val;
+
+        mutex_lock(&data->lock);
+        error = adp8870_read(data->client, ADP8870_PH1LEVL, &reg_val);
+        if (error < 0) {
+                mutex_unlock(&data->lock);
+                return error;
+        }
+        ret_val = reg_val;
+        error = adp8870_read(data->client, ADP8870_PH1LEVH, &reg_val);
+        mutex_unlock(&data->lock);
+
+        if (error < 0)
+                return error;
+
+        /* Return 13-bit conversion value for the first light sensor */
+        ret_val += (reg_val & 0x1F) << 8;
+
+        return sprintf(buf, "%u\n", ret_val);
+}
+static DEVICE_ATTR(ambient_light_level, 0444,
+                adp8870_bl_ambient_light_level_show, NULL);
+
+static ssize_t adp8870_bl_ambient_light_zone_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        struct adp8870_bl *data = dev_get_drvdata(dev);
+        int error;
+        uint8_t reg_val;
+
+        mutex_lock(&data->lock);
+        error = adp8870_read(data->client, ADP8870_CFGR, &reg_val);
+        mutex_unlock(&data->lock);
+
+        if (error < 0)
+                return error;
+
+        return sprintf(buf, "%u\n",
+                ((reg_val >> CFGR_BLV_SHIFT) & CFGR_BLV_MASK) + 1);
+}
+
+static ssize_t adp8870_bl_ambient_light_zone_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        struct adp8870_bl *data = dev_get_drvdata(dev);
+        unsigned long val;
+        uint8_t reg_val;
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &val);
+        if (ret)
+                return ret;
+
+        if (val == 0) {
+                /* Enable automatic ambient light sensing */
+                adp8870_set_bits(data->client, ADP8870_MDCR, CMP_AUTOEN);
+        } else if ((val > 0) && (val < 6)) {
+                /* Disable automatic ambient light sensing */
+                adp8870_clr_bits(data->client, ADP8870_MDCR, CMP_AUTOEN);
+
+                /* Set user supplied ambient light zone */
+                mutex_lock(&data->lock);
+                adp8870_read(data->client, ADP8870_CFGR, &reg_val);
+                reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
+                reg_val |= (val - 1) << CFGR_BLV_SHIFT;
+                adp8870_write(data->client, ADP8870_CFGR, reg_val);
+                mutex_unlock(&data->lock);
+        }
+
+        return count;
+}
+static DEVICE_ATTR(ambient_light_zone, 0664,
+                adp8870_bl_ambient_light_zone_show,
+                adp8870_bl_ambient_light_zone_store);
+#endif
+
+static struct attribute *adp8870_bl_attributes[] = {
+        &dev_attr_l5_dark_max.attr,
+        &dev_attr_l5_dark_dim.attr,
+        &dev_attr_l4_indoor_max.attr,
+        &dev_attr_l4_indoor_dim.attr,
+        &dev_attr_l3_office_max.attr,
+        &dev_attr_l3_office_dim.attr,
+        &dev_attr_l2_bright_max.attr,
+        &dev_attr_l2_bright_dim.attr,
+        &dev_attr_l1_daylight_max.attr,
+        &dev_attr_l1_daylight_dim.attr,
+#ifdef ADP8870_EXT_FEATURES
+        &dev_attr_ambient_light_level.attr,
+        &dev_attr_ambient_light_zone.attr,
+#endif
+        NULL
+};
+
+static const struct attribute_group adp8870_bl_attr_group = {
+        .attrs = adp8870_bl_attributes,
+};
+
+static int __devinit adp8870_probe(struct i2c_client *client,
+                                        const struct i2c_device_id *id)
+{
+        struct backlight_properties props;
+        struct backlight_device *bl;
+        struct adp8870_bl *data;
+        struct adp8870_backlight_platform_data *pdata =
+                client->dev.platform_data;
+        uint8_t reg_val;
+        int ret;
+
+        if (!i2c_check_functionality(client->adapter,
+                                        I2C_FUNC_SMBUS_BYTE_DATA)) {
+                dev_err(&client->dev, "SMBUS Byte Data not Supported\n");
+                return -EIO;
+        }
+
+        if (!pdata) {
+                dev_err(&client->dev, "no platform data?\n");
+                return -EINVAL;
+        }
+
+        ret = adp8870_read(client, ADP8870_MFDVID, &reg_val);
+        if (ret < 0)
+                return -EIO;
+
+        if (ADP8870_MANID(reg_val) != ADP8870_MANUFID) {
+                dev_err(&client->dev, "failed to probe\n");
+                return -ENODEV;
+        }
+
+        data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (data == NULL)
+                return -ENOMEM;
+
+        data->revid = ADP8870_DEVID(reg_val);
+        data->client = client;
+        data->pdata = pdata;
+        data->id = id->driver_data;
+        data->current_brightness = 0;
+        i2c_set_clientdata(client, data);
+
+        mutex_init(&data->lock);
+
+        memset(&props, 0, sizeof(props));
+        props.type = BACKLIGHT_RAW;
+        props.max_brightness = props.brightness = ADP8870_MAX_BRIGHTNESS;
+        bl = backlight_device_register(dev_driver_string(&client->dev),
+                        &client->dev, data, &adp8870_bl_ops, &props);
+        if (IS_ERR(bl)) {
+                dev_err(&client->dev, "failed to register backlight\n");
+                ret = PTR_ERR(bl);
+                goto out2;
+        }
+
+        data->bl = bl;
+
+        if (pdata->en_ambl_sens)
+                ret = sysfs_create_group(&bl->dev.kobj,
+                        &adp8870_bl_attr_group);
+
+        if (ret) {
+                dev_err(&client->dev, "failed to register sysfs\n");
+                goto out1;
+        }
+
+        ret = adp8870_bl_setup(bl);
+        if (ret) {
+                ret = -EIO;
+                goto out;
+        }
+
+        backlight_update_status(bl);
+
+        dev_info(&client->dev, "Rev.%d Backlight\n", data->revid);
+
+        if (pdata->num_leds)
+                adp8870_led_probe(client);
+
+        return 0;
+
+out:
+        if (data->pdata->en_ambl_sens)
+                sysfs_remove_group(&data->bl->dev.kobj,
+                        &adp8870_bl_attr_group);
+out1:
+        backlight_device_unregister(bl);
+out2:
+        i2c_set_clientdata(client, NULL);
+        kfree(data);
+
+        return ret;
+}
+
+static int __devexit adp8870_remove(struct i2c_client *client)
+{
+        struct adp8870_bl *data = i2c_get_clientdata(client);
+
+        adp8870_clr_bits(client, ADP8870_MDCR, NSTBY);
+
+        if (data->led)
+                adp8870_led_remove(client);
+
+        if (data->pdata->en_ambl_sens)
+                sysfs_remove_group(&data->bl->dev.kobj,
+                        &adp8870_bl_attr_group);
+
+        backlight_device_unregister(data->bl);
+        i2c_set_clientdata(client, NULL);
+        kfree(data);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int adp8870_i2c_suspend(struct i2c_client *client, pm_message_t message)
+{
+        adp8870_clr_bits(client, ADP8870_MDCR, NSTBY);
+
+        return 0;
+}
+
+static int adp8870_i2c_resume(struct i2c_client *client)
+{
+        adp8870_set_bits(client, ADP8870_MDCR, NSTBY);
+
+        return 0;
+}
+#else
+#define adp8870_i2c_suspend NULL
+#define adp8870_i2c_resume NULL
+#endif
+
+static const struct i2c_device_id adp8870_id[] = {
+        { "adp8870", 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, adp8870_id);
+
+static struct i2c_driver adp8870_driver = {
+        .driver = {
+                .name = KBUILD_MODNAME,
+        },
+        .probe    = adp8870_probe,
+        .remove   = __devexit_p(adp8870_remove),
+        .suspend = adp8870_i2c_suspend,
+        .resume  = adp8870_i2c_resume,
+        .id_table = adp8870_id,
+};
+
+static int __init adp8870_init(void)
+{
+        return i2c_add_driver(&adp8870_driver);
+}
+module_init(adp8870_init);
+
+static void __exit adp8870_exit(void)
+{
+        i2c_del_driver(&adp8870_driver);
+}
+module_exit(adp8870_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("ADP8870 Backlight driver");
+MODULE_ALIAS("platform:adp8870-backlight");
diff --git a/drivers/video/backlight/adx_bl.c b/drivers/video/backlight/adx_bl.c
index fe9af129c5dd..c861c41af442 100644
--- a/drivers/video/backlight/adx_bl.c
+++ b/drivers/video/backlight/adx_bl.c
@@ -104,6 +104,7 @@ static int __devinit adx_backlight_probe(struct platform_device *pdev)
         }
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = 0xff;
         bldev = backlight_device_register(dev_name(&pdev->dev), &pdev->dev,
                                           bl, &adx_backlight_ops, &props);
diff --git a/drivers/video/backlight/apple_bl.c b/drivers/video/backlight/apple_bl.c
new file mode 100644
index 000000000000..be98d152b7fd
--- /dev/null
+++ b/drivers/video/backlight/apple_bl.c
@@ -0,0 +1,241 @@
+/*
+ *  Backlight Driver for Intel-based Apples
+ *
+ *  Copyright (c) Red Hat <mjg@redhat.com>
+ *  Based on code from Pommed:
+ *  Copyright (C) 2006 Nicolas Boichat <nicolas @boichat.ch>
+ *  Copyright (C) 2006 Felipe Alfaro Solana <felipe_alfaro @linuxmail.org>
+ *  Copyright (C) 2007 Julien BLACHE <jb@jblache.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This driver triggers SMIs which cause the firmware to change the
+ *  backlight brightness. This is icky in many ways, but it's impractical to
+ *  get at the firmware code in order to figure out what it's actually doing.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/backlight.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/pci.h>
+#include <linux/acpi.h>
+
+static struct backlight_device *apple_backlight_device;
+
+struct hw_data {
+        /* I/O resource to allocate. */
+        unsigned long iostart;
+        unsigned long iolen;
+        /* Backlight operations structure. */
+        const struct backlight_ops backlight_ops;
+        void (*set_brightness)(int);
+};
+
+static const struct hw_data *hw_data;
+
+#define DRIVER "apple_backlight: "
+
+/* Module parameters. */
+static int debug;
+module_param_named(debug, debug, int, 0644);
+MODULE_PARM_DESC(debug, "Set to one to enable debugging messages.");
+
+/*
+ * Implementation for machines with Intel chipset.
+ */
+static void intel_chipset_set_brightness(int intensity)
+{
+        outb(0x04 | (intensity << 4), 0xb3);
+        outb(0xbf, 0xb2);
+}
+
+static int intel_chipset_send_intensity(struct backlight_device *bd)
+{
+        int intensity = bd->props.brightness;
+
+        if (debug)
+                printk(KERN_DEBUG DRIVER "setting brightness to %d\n",
+                       intensity);
+
+        intel_chipset_set_brightness(intensity);
+        return 0;
+}
+
+static int intel_chipset_get_intensity(struct backlight_device *bd)
+{
+        int intensity;
+
+        outb(0x03, 0xb3);
+        outb(0xbf, 0xb2);
+        intensity = inb(0xb3) >> 4;
+
+        if (debug)
+                printk(KERN_DEBUG DRIVER "read brightness of %d\n",
+                       intensity);
+
+        return intensity;
+}
+
+static const struct hw_data intel_chipset_data = {
+        .iostart = 0xb2,
+        .iolen = 2,
+        .backlight_ops  = {
+                .options        = BL_CORE_SUSPENDRESUME,
+                .get_brightness = intel_chipset_get_intensity,
+                .update_status  = intel_chipset_send_intensity,
+        },
+        .set_brightness = intel_chipset_set_brightness,
+};
+
+/*
+ * Implementation for machines with Nvidia chipset.
+ */
+static void nvidia_chipset_set_brightness(int intensity)
+{
+        outb(0x04 | (intensity << 4), 0x52f);
+        outb(0xbf, 0x52e);
+}
+
+static int nvidia_chipset_send_intensity(struct backlight_device *bd)
+{
+        int intensity = bd->props.brightness;
+
+        if (debug)
+                printk(KERN_DEBUG DRIVER "setting brightness to %d\n",
+                       intensity);
+
+        nvidia_chipset_set_brightness(intensity);
+        return 0;
+}
+
+static int nvidia_chipset_get_intensity(struct backlight_device *bd)
+{
+        int intensity;
+
+        outb(0x03, 0x52f);
+        outb(0xbf, 0x52e);
+        intensity = inb(0x52f) >> 4;
+
+        if (debug)
+                printk(KERN_DEBUG DRIVER "read brightness of %d\n",
+                       intensity);
+
+        return intensity;
+}
+
+static const struct hw_data nvidia_chipset_data = {
+        .iostart = 0x52e,
+        .iolen = 2,
+        .backlight_ops          = {
+                .options        = BL_CORE_SUSPENDRESUME,
+                .get_brightness = nvidia_chipset_get_intensity,
+                .update_status  = nvidia_chipset_send_intensity
+        },
+        .set_brightness = nvidia_chipset_set_brightness,
+};
+
+static int __devinit apple_bl_add(struct acpi_device *dev)
+{
+        struct backlight_properties props;
+        struct pci_dev *host;
+        int intensity;
+
+        host = pci_get_bus_and_slot(0, 0);
+
+        if (!host) {
+                printk(KERN_ERR DRIVER "unable to find PCI host\n");
+                return -ENODEV;
+        }
+
+        if (host->vendor == PCI_VENDOR_ID_INTEL)
+                hw_data = &intel_chipset_data;
+        else if (host->vendor == PCI_VENDOR_ID_NVIDIA)
+                hw_data = &nvidia_chipset_data;
+
+        pci_dev_put(host);
+
+        if (!hw_data) {
+                printk(KERN_ERR DRIVER "unknown hardware\n");
+                return -ENODEV;
+        }
+
+        /* Check that the hardware responds - this may not work under EFI */
+
+        intensity = hw_data->backlight_ops.get_brightness(NULL);
+
+        if (!intensity) {
+                hw_data->set_brightness(1);
+                if (!hw_data->backlight_ops.get_brightness(NULL))
+                        return -ENODEV;
+
+                hw_data->set_brightness(0);
+        }
+
+        if (!request_region(hw_data->iostart, hw_data->iolen,
+                            "Apple backlight"))
+                return -ENXIO;
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_PLATFORM;
+        props.max_brightness = 15;
+        apple_backlight_device = backlight_device_register("apple_backlight",
+                                  NULL, NULL, &hw_data->backlight_ops, &props);
+
+        if (IS_ERR(apple_backlight_device)) {
+                release_region(hw_data->iostart, hw_data->iolen);
+                return PTR_ERR(apple_backlight_device);
+        }
+
+        apple_backlight_device->props.brightness =
+                hw_data->backlight_ops.get_brightness(apple_backlight_device);
+        backlight_update_status(apple_backlight_device);
+
+        return 0;
+}
+
+static int __devexit apple_bl_remove(struct acpi_device *dev, int type)
+{
+        backlight_device_unregister(apple_backlight_device);
+
+        release_region(hw_data->iostart, hw_data->iolen);
+        hw_data = NULL;
+        return 0;
+}
+
+static const struct acpi_device_id apple_bl_ids[] = {
+        {"APP0002", 0},
+        {"", 0},
+};
+
+static struct acpi_driver apple_bl_driver = {
+        .name = "Apple backlight",
+        .ids = apple_bl_ids,
+        .ops = {
+                .add = apple_bl_add,
+                .remove = apple_bl_remove,
+        },
+};
+
+static int __init apple_bl_init(void)
+{
+        return acpi_bus_register_driver(&apple_bl_driver);
+}
+
+static void __exit apple_bl_exit(void)
+{
+        acpi_bus_unregister_driver(&apple_bl_driver);
+}
+
+module_init(apple_bl_init);
+module_exit(apple_bl_exit);
+
+MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
+MODULE_DESCRIPTION("Apple Backlight Driver");
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(acpi, apple_bl_ids);
+MODULE_ALIAS("mbp_nvidia_bl");
diff --git a/drivers/video/backlight/atmel-pwm-bl.c b/drivers/video/backlight/atmel-pwm-bl.c
index e6a66dab088c..0443a4f71858 100644
--- a/drivers/video/backlight/atmel-pwm-bl.c
+++ b/drivers/video/backlight/atmel-pwm-bl.c
@@ -168,6 +168,7 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
         }
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = pdata->pwm_duty_max - pdata->pwm_duty_min;
         bldev = backlight_device_register("atmel-pwm-bl", &pdev->dev, pwmbl,
                                           &atmel_pwm_bl_ops, &props);
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index e207810bba3c..80d292fb92d8 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -19,6 +19,12 @@
 #include <asm/backlight.h>
 #endif
 
+static const char const *backlight_types[] = {
+        [BACKLIGHT_RAW] = "raw",
+        [BACKLIGHT_PLATFORM] = "platform",
+        [BACKLIGHT_FIRMWARE] = "firmware",
+};
+
 #if defined(CONFIG_FB) || (defined(CONFIG_FB_MODULE) && \
                            defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE))
 /* This callback gets called when something important happens inside a
@@ -169,6 +175,14 @@ static ssize_t backlight_store_brightness(struct device *dev,
         return rc;
 }
 
+static ssize_t backlight_show_type(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        struct backlight_device *bd = to_backlight_device(dev);
+
+        return sprintf(buf, "%s\n", backlight_types[bd->props.type]);
+}
+
 static ssize_t backlight_show_max_brightness(struct device *dev,
                 struct device_attribute *attr, char *buf)
 {
@@ -197,12 +211,12 @@ static int backlight_suspend(struct device *dev, pm_message_t state)
 {
         struct backlight_device *bd = to_backlight_device(dev);
 
-        if (bd->ops->options & BL_CORE_SUSPENDRESUME) {
-                mutex_lock(&bd->ops_lock);
+        mutex_lock(&bd->ops_lock);
+        if (bd->ops && bd->ops->options & BL_CORE_SUSPENDRESUME) {
                 bd->props.state |= BL_CORE_SUSPENDED;
                 backlight_update_status(bd);
-                mutex_unlock(&bd->ops_lock);
         }
+        mutex_unlock(&bd->ops_lock);
 
         return 0;
 }
@@ -211,12 +225,12 @@ static int backlight_resume(struct device *dev)
 {
         struct backlight_device *bd = to_backlight_device(dev);
 
-        if (bd->ops->options & BL_CORE_SUSPENDRESUME) {
-                mutex_lock(&bd->ops_lock);
+        mutex_lock(&bd->ops_lock);
+        if (bd->ops && bd->ops->options & BL_CORE_SUSPENDRESUME) {
                 bd->props.state &= ~BL_CORE_SUSPENDED;
                 backlight_update_status(bd);
-                mutex_unlock(&bd->ops_lock);
         }
+        mutex_unlock(&bd->ops_lock);
 
         return 0;
 }
@@ -234,6 +248,7 @@ static struct device_attribute bl_device_attributes[] = {
         __ATTR(actual_brightness, 0444, backlight_show_actual_brightness,
                      NULL),
         __ATTR(max_brightness, 0444, backlight_show_max_brightness, NULL),
+        __ATTR(type, 0444, backlight_show_type, NULL),
         __ATTR_NULL,
 };
 
@@ -292,9 +307,16 @@ struct backlight_device *backlight_device_register(const char *name,
         dev_set_drvdata(&new_bd->dev, devdata);
 
         /* Set default properties */
-        if (props)
+        if (props) {
                 memcpy(&new_bd->props, props,
                        sizeof(struct backlight_properties));
+                if (props->type <= 0 || props->type >= BACKLIGHT_TYPE_MAX) {
+                        WARN(1, "%s: invalid backlight type", name);
+                        new_bd->props.type = BACKLIGHT_RAW;
+                }
+        } else {
+                new_bd->props.type = BACKLIGHT_RAW;
+        }
 
         rc = device_register(&new_bd->dev);
         if (rc) {
diff --git a/drivers/video/backlight/corgi_lcd.c b/drivers/video/backlight/corgi_lcd.c
index 1e71c35083bb..c6533bad26f8 100644
--- a/drivers/video/backlight/corgi_lcd.c
+++ b/drivers/video/backlight/corgi_lcd.c
@@ -109,7 +109,7 @@ static unsigned long corgibl_flags;
 #define CORGIBL_BATTLOW       0x02
 
 /*
- * This is only a psuedo I2C interface. We can't use the standard kernel
+ * This is only a pseudo I2C interface. We can't use the standard kernel
  * routines as the interface is write only. We just assume the data is acked...
  */
 static void lcdtg_ssp_i2c_send(struct corgi_lcd *lcd, uint8_t data)
@@ -562,6 +562,7 @@ static int __devinit corgi_lcd_probe(struct spi_device *spi)
         lcd->mode = (pdata) ? pdata->init_mode : CORGI_LCD_MODE_VGA;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = pdata->max_intensity;
         lcd->bl_dev = backlight_device_register("corgi_bl", &spi->dev, lcd,
                                                 &corgi_bl_ops, &props);
diff --git a/drivers/video/backlight/cr_bllcd.c b/drivers/video/backlight/cr_bllcd.c
index a4f4546f0be0..6c8c54041fae 100644
--- a/drivers/video/backlight/cr_bllcd.c
+++ b/drivers/video/backlight/cr_bllcd.c
@@ -193,6 +193,7 @@ static int cr_backlight_probe(struct platform_device *pdev)
         }
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         bdp = backlight_device_register("cr-backlight", &pdev->dev, NULL,
                                         &cr_backlight_ops, &props);
         if (IS_ERR(bdp)) {
@@ -242,6 +243,7 @@ static int cr_backlight_remove(struct platform_device *pdev)
         backlight_device_unregister(crp->cr_backlight_device);
         lcd_device_unregister(crp->cr_lcd_device);
         pci_dev_put(lpc_dev);
+        kfree(crp);
 
         return 0;
 }
diff --git a/drivers/video/backlight/da903x_bl.c b/drivers/video/backlight/da903x_bl.c
index 87659ed79bd7..62043f12a5a4 100644
--- a/drivers/video/backlight/da903x_bl.c
+++ b/drivers/video/backlight/da903x_bl.c
@@ -136,6 +136,7 @@ static int da903x_backlight_probe(struct platform_device *pdev)
                 da903x_write(data->da903x_dev, DA9034_WLED_CONTROL2,
                                 DA9034_WLED_ISET(pdata->output_current));
 
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = max_brightness;
         bl = backlight_device_register(pdev->name, data->da903x_dev, data,
                                        &da903x_backlight_ops, &props);
diff --git a/drivers/video/backlight/ep93xx_bl.c b/drivers/video/backlight/ep93xx_bl.c
index b0cc49184803..9f1e389d51d2 100644
--- a/drivers/video/backlight/ep93xx_bl.c
+++ b/drivers/video/backlight/ep93xx_bl.c
@@ -87,6 +87,7 @@ static int __init ep93xxbl_probe(struct platform_device *dev)
         ep93xxbl->mmio = EP93XX_RASTER_BRIGHTNESS;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = EP93XX_MAX_BRIGHT;
         bl = backlight_device_register(dev->name, &dev->dev, ep93xxbl,
                                        &ep93xxbl_ops, &props);
diff --git a/drivers/video/backlight/generic_bl.c b/drivers/video/backlight/generic_bl.c
index 312ca619735d..8c6befd65a33 100644
--- a/drivers/video/backlight/generic_bl.c
+++ b/drivers/video/backlight/generic_bl.c
@@ -91,6 +91,7 @@ static int genericbl_probe(struct platform_device *pdev)
                 name = machinfo->name;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = machinfo->max_intensity;
         bd = backlight_device_register(name, &pdev->dev, NULL, &genericbl_ops,
                                        &props);
diff --git a/drivers/video/backlight/hp680_bl.c b/drivers/video/backlight/hp680_bl.c
index 267d23f8d645..38aa00272141 100644
--- a/drivers/video/backlight/hp680_bl.c
+++ b/drivers/video/backlight/hp680_bl.c
@@ -109,6 +109,7 @@ static int __devinit hp680bl_probe(struct platform_device *pdev)
         struct backlight_device *bd;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = HP680_MAX_INTENSITY;
         bd = backlight_device_register("hp680-bl", &pdev->dev, NULL,
                                        &hp680bl_ops, &props);
diff --git a/drivers/video/backlight/jornada720_bl.c b/drivers/video/backlight/jornada720_bl.c
index 2f177b3a4885..de65d80159be 100644
--- a/drivers/video/backlight/jornada720_bl.c
+++ b/drivers/video/backlight/jornada720_bl.c
@@ -106,6 +106,7 @@ static int jornada_bl_probe(struct platform_device *pdev)
         struct backlight_device *bd;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = BL_MAX_BRIGHT;
         bd = backlight_device_register(S1D_DEVICENAME, &pdev->dev, NULL,
                                        &jornada_bl_ops, &props);
@@ -146,12 +147,12 @@ static struct platform_driver jornada_bl_driver = {
         },
 };
 
-int __init jornada_bl_init(void)
+static int __init jornada_bl_init(void)
 {
         return platform_driver_register(&jornada_bl_driver);
 }
 
-void __exit jornada_bl_exit(void)
+static void __exit jornada_bl_exit(void)
 {
         platform_driver_unregister(&jornada_bl_driver);
 }
diff --git a/drivers/video/backlight/jornada720_lcd.c b/drivers/video/backlight/jornada720_lcd.c
index cbbb167fd268..d2ff658b4144 100644
--- a/drivers/video/backlight/jornada720_lcd.c
+++ b/drivers/video/backlight/jornada720_lcd.c
@@ -135,12 +135,12 @@ static struct platform_driver jornada_lcd_driver = {
         },
 };
 
-int __init jornada_lcd_init(void)
+static int __init jornada_lcd_init(void)
 {
         return platform_driver_register(&jornada_lcd_driver);
 }
 
-void __exit jornada_lcd_exit(void)
+static void __exit jornada_lcd_exit(void)
 {
         platform_driver_unregister(&jornada_lcd_driver);
 }
diff --git a/drivers/video/backlight/kb3886_bl.c b/drivers/video/backlight/kb3886_bl.c
index f439a8632287..72dd5556a35b 100644
--- a/drivers/video/backlight/kb3886_bl.c
+++ b/drivers/video/backlight/kb3886_bl.c
@@ -149,6 +149,7 @@ static int kb3886bl_probe(struct platform_device *pdev)
                 machinfo->limit_mask = -1;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = machinfo->max_intensity;
         kb3886_backlight_device = backlight_device_register("kb3886-bl",
                                                             &pdev->dev, NULL,
diff --git a/drivers/video/backlight/l4f00242t03.c b/drivers/video/backlight/l4f00242t03.c
index 9093ef0fa869..98ad3e5f7c85 100644
--- a/drivers/video/backlight/l4f00242t03.c
+++ b/drivers/video/backlight/l4f00242t03.c
@@ -25,7 +25,7 @@
 struct l4f00242t03_priv {
         struct spi_device       *spi;
         struct lcd_device       *ld;
-        int lcd_on:1;
+        int lcd_state;
         struct regulator *io_reg;
         struct regulator *core_reg;
 };
@@ -62,11 +62,36 @@ static void l4f00242t03_lcd_init(struct spi_device *spi)
                 regulator_enable(priv->core_reg);
         }
 
+        l4f00242t03_reset(pdata->reset_gpio);
+
         gpio_set_value(pdata->data_enable_gpio, 1);
         msleep(60);
         spi_write(spi, (const u8 *)cmd, ARRAY_SIZE(cmd) * sizeof(u16));
 }
 
+static void l4f00242t03_lcd_powerdown(struct spi_device *spi)
+{
+        struct l4f00242t03_pdata *pdata = spi->dev.platform_data;
+        struct l4f00242t03_priv *priv = dev_get_drvdata(&spi->dev);
+
+        dev_dbg(&spi->dev, "Powering down LCD\n");
+
+        gpio_set_value(pdata->data_enable_gpio, 0);
+
+        if (priv->io_reg)
+                regulator_disable(priv->io_reg);
+
+        if (priv->core_reg)
+                regulator_disable(priv->core_reg);
+}
+
+static int l4f00242t03_lcd_power_get(struct lcd_device *ld)
+{
+        struct l4f00242t03_priv *priv = lcd_get_data(ld);
+
+        return priv->lcd_state;
+}
+
 static int l4f00242t03_lcd_power_set(struct lcd_device *ld, int power)
 {
         struct l4f00242t03_priv *priv = lcd_get_data(ld);
@@ -78,36 +103,55 @@ static int l4f00242t03_lcd_power_set(struct lcd_device *ld, int power)
         const u16 slpin = 0x10;
         const u16 disoff = 0x28;
 
-        if (power) {
-                if (priv->lcd_on)
-                        return 0;
-
-                dev_dbg(&spi->dev, "turning on LCD\n");
-
-                spi_write(spi, (const u8 *)&slpout, sizeof(u16));
-                msleep(60);
-                spi_write(spi, (const u8 *)&dison, sizeof(u16));
-
-                priv->lcd_on = 1;
+        if (power <= FB_BLANK_NORMAL) {
+                if (priv->lcd_state <= FB_BLANK_NORMAL) {
+                        /* Do nothing, the LCD is running */
+                } else if (priv->lcd_state < FB_BLANK_POWERDOWN) {
+                        dev_dbg(&spi->dev, "Resuming LCD\n");
+
+                        spi_write(spi, (const u8 *)&slpout, sizeof(u16));
+                        msleep(60);
+                        spi_write(spi, (const u8 *)&dison, sizeof(u16));
+                } else {
+                        /* priv->lcd_state == FB_BLANK_POWERDOWN */
+                        l4f00242t03_lcd_init(spi);
+                        priv->lcd_state = FB_BLANK_VSYNC_SUSPEND;
+                        l4f00242t03_lcd_power_set(priv->ld, power);
+                }
+        } else if (power < FB_BLANK_POWERDOWN) {
+                if (priv->lcd_state <= FB_BLANK_NORMAL) {
+                        /* Send the display in standby */
+                        dev_dbg(&spi->dev, "Standby the LCD\n");
+
+                        spi_write(spi, (const u8 *)&disoff, sizeof(u16));
+                        msleep(60);
+                        spi_write(spi, (const u8 *)&slpin, sizeof(u16));
+                } else if (priv->lcd_state < FB_BLANK_POWERDOWN) {
+                        /* Do nothing, the LCD is already in standby */
+                } else {
+                        /* priv->lcd_state == FB_BLANK_POWERDOWN */
+                        l4f00242t03_lcd_init(spi);
+                        priv->lcd_state = FB_BLANK_UNBLANK;
+                        l4f00242t03_lcd_power_set(ld, power);
+                }
         } else {
-                if (!priv->lcd_on)
-                        return 0;
-
-                dev_dbg(&spi->dev, "turning off LCD\n");
-
-                spi_write(spi, (const u8 *)&disoff, sizeof(u16));
-                msleep(60);
-                spi_write(spi, (const u8 *)&slpin, sizeof(u16));
-
-                priv->lcd_on = 0;
+                /* power == FB_BLANK_POWERDOWN */
+                if (priv->lcd_state != FB_BLANK_POWERDOWN) {
+                        /* Clear the screen before shutting down */
+                        spi_write(spi, (const u8 *)&disoff, sizeof(u16));
+                        msleep(60);
+                        l4f00242t03_lcd_powerdown(spi);
+                }
         }
 
+        priv->lcd_state = power;
+
         return 0;
 }
 
 static struct lcd_ops l4f_ops = {
         .set_power      = l4f00242t03_lcd_power_set,
-        .get_power      = NULL,
+        .get_power      = l4f00242t03_lcd_power_get,
 };
 
 static int __devinit l4f00242t03_probe(struct spi_device *spi)
@@ -185,9 +229,9 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
         }
 
         /* Init the LCD */
-        l4f00242t03_reset(pdata->reset_gpio);
         l4f00242t03_lcd_init(spi);
-        l4f00242t03_lcd_power_set(priv->ld, 1);
+        priv->lcd_state = FB_BLANK_VSYNC_SUSPEND;
+        l4f00242t03_lcd_power_set(priv->ld, FB_BLANK_UNBLANK);
 
         dev_info(&spi->dev, "Epson l4f00242t03 lcd probed.\n");
 
@@ -214,9 +258,11 @@ static int __devexit l4f00242t03_remove(struct spi_device *spi)
         struct l4f00242t03_priv *priv = dev_get_drvdata(&spi->dev);
         struct l4f00242t03_pdata *pdata = priv->spi->dev.platform_data;
 
-        l4f00242t03_lcd_power_set(priv->ld, 0);
+        l4f00242t03_lcd_power_set(priv->ld, FB_BLANK_POWERDOWN);
         lcd_device_unregister(priv->ld);
 
+        dev_set_drvdata(&spi->dev, NULL);
+
         gpio_free(pdata->data_enable_gpio);
         gpio_free(pdata->reset_gpio);
 
@@ -230,6 +276,15 @@ static int __devexit l4f00242t03_remove(struct spi_device *spi)
         return 0;
 }
 
+static void l4f00242t03_shutdown(struct spi_device *spi)
+{
+        struct l4f00242t03_priv *priv = dev_get_drvdata(&spi->dev);
+
+        if (priv)
+                l4f00242t03_lcd_power_set(priv->ld, FB_BLANK_POWERDOWN);
+
+}
+
 static struct spi_driver l4f00242t03_driver = {
         .driver = {
                 .name   = "l4f00242t03",
@@ -237,6 +292,7 @@ static struct spi_driver l4f00242t03_driver = {
         },
         .probe          = l4f00242t03_probe,
         .remove         = __devexit_p(l4f00242t03_remove),
+        .shutdown       = l4f00242t03_shutdown,
 };
 
 static __init int l4f00242t03_init(void)
diff --git a/drivers/video/backlight/ld9040.c b/drivers/video/backlight/ld9040.c
new file mode 100644
index 000000000000..7281b2506a67
--- /dev/null
+++ b/drivers/video/backlight/ld9040.c
@@ -0,0 +1,819 @@
+/*
+ * ld9040 AMOLED LCD panel driver.
+ *
+ * Copyright (c) 2011 Samsung Electronics
+ * Author: Donghwa Lee  <dh09.lee@samsung.com>
+ * Derived from drivers/video/backlight/s6e63m0.c
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ */
+
+#include <linux/wait.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/lcd.h>
+#include <linux/backlight.h>
+
+#include "ld9040_gamma.h"
+
+#define SLEEPMSEC               0x1000
+#define ENDDEF                  0x2000
+#define DEFMASK                 0xFF00
+#define COMMAND_ONLY            0xFE
+#define DATA_ONLY               0xFF
+
+#define MIN_BRIGHTNESS          0
+#define MAX_BRIGHTNESS          24
+#define power_is_on(pwr)        ((pwr) <= FB_BLANK_NORMAL)
+
+struct ld9040 {
+        struct device                   *dev;
+        struct spi_device               *spi;
+        unsigned int                    power;
+        unsigned int                    current_brightness;
+
+        struct lcd_device               *ld;
+        struct backlight_device         *bd;
+        struct lcd_platform_data        *lcd_pd;
+};
+
+static const unsigned short seq_swreset[] = {
+        0x01, COMMAND_ONLY,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_user_setting[] = {
+        0xF0, 0x5A,
+
+        DATA_ONLY, 0x5A,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_elvss_on[] = {
+        0xB1, 0x0D,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x16,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_gtcon[] = {
+        0xF7, 0x09,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_panel_condition[] = {
+        0xF8, 0x05,
+
+        DATA_ONLY, 0x65,
+        DATA_ONLY, 0x96,
+        DATA_ONLY, 0x71,
+        DATA_ONLY, 0x7D,
+        DATA_ONLY, 0x19,
+        DATA_ONLY, 0x3B,
+        DATA_ONLY, 0x0D,
+        DATA_ONLY, 0x19,
+        DATA_ONLY, 0x7E,
+        DATA_ONLY, 0x0D,
+        DATA_ONLY, 0xE2,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x7E,
+        DATA_ONLY, 0x7D,
+        DATA_ONLY, 0x07,
+        DATA_ONLY, 0x07,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x02,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_gamma_set1[] = {
+        0xF9, 0x00,
+
+        DATA_ONLY, 0xA7,
+        DATA_ONLY, 0xB4,
+        DATA_ONLY, 0xAE,
+        DATA_ONLY, 0xBF,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x91,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0xB2,
+        DATA_ONLY, 0xB4,
+        DATA_ONLY, 0xAA,
+        DATA_ONLY, 0xBB,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0xAC,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0xB3,
+        DATA_ONLY, 0xB1,
+        DATA_ONLY, 0xAA,
+        DATA_ONLY, 0xBC,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0xB3,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_gamma_ctrl[] = {
+        0xFB, 0x02,
+
+        DATA_ONLY, 0x5A,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_gamma_start[] = {
+        0xF9, COMMAND_ONLY,
+
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_apon[] = {
+        0xF3, 0x00,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x0A,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_display_ctrl[] = {
+        0xF2, 0x02,
+
+        DATA_ONLY, 0x08,
+        DATA_ONLY, 0x08,
+        DATA_ONLY, 0x10,
+        DATA_ONLY, 0x10,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_manual_pwr[] = {
+        0xB0, 0x04,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_pwr_ctrl[] = {
+        0xF4, 0x0A,
+
+        DATA_ONLY, 0x87,
+        DATA_ONLY, 0x25,
+        DATA_ONLY, 0x6A,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x02,
+        DATA_ONLY, 0x88,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_sleep_out[] = {
+        0x11, COMMAND_ONLY,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_sleep_in[] = {
+        0x10, COMMAND_ONLY,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_display_on[] = {
+        0x29, COMMAND_ONLY,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_display_off[] = {
+        0x28, COMMAND_ONLY,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vci1_1st_en[] = {
+        0xF3, 0x10,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vl1_en[] = {
+        0xF3, 0x11,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vl2_en[] = {
+        0xF3, 0x13,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vci1_2nd_en[] = {
+        0xF3, 0x33,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vl3_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vreg1_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0x01,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vgh_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0x11,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vgl_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0x31,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x02,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vmos_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xB1,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vint_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xF1,
+        /* DATA_ONLY, 0x71,     VMOS/VBL/VBH not used */
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        /* DATA_ONLY, 0x02,     VMOS/VBL/VBH not used */
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vbh_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xF9,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_vbl_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xFD,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_gam_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xFF,
+        /* DATA_ONLY, 0x73,     VMOS/VBL/VBH not used */
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        /* DATA_ONLY, 0x02,     VMOS/VBL/VBH not used */
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_sd_amp_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xFF,
+        /* DATA_ONLY, 0x73,     VMOS/VBL/VBH not used */
+        DATA_ONLY, 0x80,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        /* DATA_ONLY, 0x02,     VMOS/VBL/VBH not used */
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_gls_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xFF,
+        /* DATA_ONLY, 0x73,     VMOS/VBL/VBH not used */
+        DATA_ONLY, 0x81,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        /* DATA_ONLY, 0x02,     VMOS/VBL/VBH not used */
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_els_en[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xFF,
+        /* DATA_ONLY, 0x73,     VMOS/VBL/VBH not used */
+        DATA_ONLY, 0x83,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        /* DATA_ONLY, 0x02,     VMOS/VBL/VBH not used */
+        ENDDEF, 0x00
+};
+
+static const unsigned short seq_el_on[] = {
+        0xF3, 0x37,
+
+        DATA_ONLY, 0xFF,
+        /* DATA_ONLY, 0x73,     VMOS/VBL/VBH not used */
+        DATA_ONLY, 0x87,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        /* DATA_ONLY, 0x02,     VMOS/VBL/VBH not used */
+        ENDDEF, 0x00
+};
+
+static int ld9040_spi_write_byte(struct ld9040 *lcd, int addr, int data)
+{
+        u16 buf[1];
+        struct spi_message msg;
+
+        struct spi_transfer xfer = {
+                .len            = 2,
+                .tx_buf         = buf,
+        };
+
+        buf[0] = (addr << 8) | data;
+
+        spi_message_init(&msg);
+        spi_message_add_tail(&xfer, &msg);
+
+        return spi_sync(lcd->spi, &msg);
+}
+
+static int ld9040_spi_write(struct ld9040 *lcd, unsigned char address,
+        unsigned char command)
+{
+        int ret = 0;
+
+        if (address != DATA_ONLY)
+                ret = ld9040_spi_write_byte(lcd, 0x0, address);
+        if (command != COMMAND_ONLY)
+                ret = ld9040_spi_write_byte(lcd, 0x1, command);
+
+        return ret;
+}
+
+static int ld9040_panel_send_sequence(struct ld9040 *lcd,
+        const unsigned short *wbuf)
+{
+        int ret = 0, i = 0;
+
+        while ((wbuf[i] & DEFMASK) != ENDDEF) {
+                if ((wbuf[i] & DEFMASK) != SLEEPMSEC) {
+                        ret = ld9040_spi_write(lcd, wbuf[i], wbuf[i+1]);
+                        if (ret)
+                                break;
+                } else
+                        udelay(wbuf[i+1]*1000);
+                i += 2;
+        }
+
+        return ret;
+}
+
+static int _ld9040_gamma_ctl(struct ld9040 *lcd, const unsigned int *gamma)
+{
+        unsigned int i = 0;
+        int ret = 0;
+
+        /* start gamma table updating. */
+        ret = ld9040_panel_send_sequence(lcd, seq_gamma_start);
+        if (ret) {
+                dev_err(lcd->dev, "failed to disable gamma table updating.\n");
+                goto gamma_err;
+        }
+
+        for (i = 0 ; i < GAMMA_TABLE_COUNT; i++) {
+                ret = ld9040_spi_write(lcd, DATA_ONLY, gamma[i]);
+                if (ret) {
+                        dev_err(lcd->dev, "failed to set gamma table.\n");
+                        goto gamma_err;
+                }
+        }
+
+        /* update gamma table. */
+        ret = ld9040_panel_send_sequence(lcd, seq_gamma_ctrl);
+        if (ret)
+                dev_err(lcd->dev, "failed to update gamma table.\n");
+
+gamma_err:
+        return ret;
+}
+
+static int ld9040_gamma_ctl(struct ld9040 *lcd, int gamma)
+{
+        int ret = 0;
+
+        ret = _ld9040_gamma_ctl(lcd, gamma_table.gamma_22_table[gamma]);
+
+        return ret;
+}
+
+
+static int ld9040_ldi_init(struct ld9040 *lcd)
+{
+        int ret, i;
+        static const unsigned short *init_seq[] = {
+                seq_user_setting,
+                seq_panel_condition,
+                seq_display_ctrl,
+                seq_manual_pwr,
+                seq_elvss_on,
+                seq_gtcon,
+                seq_gamma_set1,
+                seq_gamma_ctrl,
+                seq_sleep_out,
+        };
+
+        for (i = 0; i < ARRAY_SIZE(init_seq); i++) {
+                ret = ld9040_panel_send_sequence(lcd, init_seq[i]);
+                /* workaround: minimum delay time for transferring CMD */
+                udelay(300);
+                if (ret)
+                        break;
+        }
+
+        return ret;
+}
+
+static int ld9040_ldi_enable(struct ld9040 *lcd)
+{
+        int ret = 0;
+
+        ret = ld9040_panel_send_sequence(lcd, seq_display_on);
+
+        return ret;
+}
+
+static int ld9040_ldi_disable(struct ld9040 *lcd)
+{
+        int ret;
+
+        ret = ld9040_panel_send_sequence(lcd, seq_display_off);
+        ret = ld9040_panel_send_sequence(lcd, seq_sleep_in);
+
+        return ret;
+}
+
+static int ld9040_power_on(struct ld9040 *lcd)
+{
+        int ret = 0;
+        struct lcd_platform_data *pd = NULL;
+        pd = lcd->lcd_pd;
+        if (!pd) {
+                dev_err(lcd->dev, "platform data is NULL.\n");
+                return -EFAULT;
+        }
+
+        if (!pd->power_on) {
+                dev_err(lcd->dev, "power_on is NULL.\n");
+                return -EFAULT;
+        } else {
+                pd->power_on(lcd->ld, 1);
+                mdelay(pd->power_on_delay);
+        }
+
+        if (!pd->reset) {
+                dev_err(lcd->dev, "reset is NULL.\n");
+                return -EFAULT;
+        } else {
+                pd->reset(lcd->ld);
+                mdelay(pd->reset_delay);
+        }
+
+        ret = ld9040_ldi_init(lcd);
+        if (ret) {
+                dev_err(lcd->dev, "failed to initialize ldi.\n");
+                return ret;
+        }
+
+        ret = ld9040_ldi_enable(lcd);
+        if (ret) {
+                dev_err(lcd->dev, "failed to enable ldi.\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+static int ld9040_power_off(struct ld9040 *lcd)
+{
+        int ret = 0;
+        struct lcd_platform_data *pd = NULL;
+
+        pd = lcd->lcd_pd;
+        if (!pd) {
+                dev_err(lcd->dev, "platform data is NULL.\n");
+                return -EFAULT;
+        }
+
+        ret = ld9040_ldi_disable(lcd);
+        if (ret) {
+                dev_err(lcd->dev, "lcd setting failed.\n");
+                return -EIO;
+        }
+
+        mdelay(pd->power_off_delay);
+
+        if (!pd->power_on) {
+                dev_err(lcd->dev, "power_on is NULL.\n");
+                return -EFAULT;
+        } else
+                pd->power_on(lcd->ld, 0);
+
+        return 0;
+}
+
+static int ld9040_power(struct ld9040 *lcd, int power)
+{
+        int ret = 0;
+
+        if (power_is_on(power) && !power_is_on(lcd->power))
+                ret = ld9040_power_on(lcd);
+        else if (!power_is_on(power) && power_is_on(lcd->power))
+                ret = ld9040_power_off(lcd);
+
+        if (!ret)
+                lcd->power = power;
+
+        return ret;
+}
+
+static int ld9040_set_power(struct lcd_device *ld, int power)
+{
+        struct ld9040 *lcd = lcd_get_data(ld);
+
+        if (power != FB_BLANK_UNBLANK && power != FB_BLANK_POWERDOWN &&
+                power != FB_BLANK_NORMAL) {
+                dev_err(lcd->dev, "power value should be 0, 1 or 4.\n");
+                return -EINVAL;
+        }
+
+        return ld9040_power(lcd, power);
+}
+
+static int ld9040_get_power(struct lcd_device *ld)
+{
+        struct ld9040 *lcd = lcd_get_data(ld);
+
+        return lcd->power;
+}
+
+static int ld9040_get_brightness(struct backlight_device *bd)
+{
+        return bd->props.brightness;
+}
+
+static int ld9040_set_brightness(struct backlight_device *bd)
+{
+        int ret = 0, brightness = bd->props.brightness;
+        struct ld9040 *lcd = bl_get_data(bd);
+
+        if (brightness < MIN_BRIGHTNESS ||
+                brightness > bd->props.max_brightness) {
+                dev_err(&bd->dev, "lcd brightness should be %d to %d.\n",
+                        MIN_BRIGHTNESS, MAX_BRIGHTNESS);
+                return -EINVAL;
+        }
+
+        ret = ld9040_gamma_ctl(lcd, bd->props.brightness);
+        if (ret) {
+                dev_err(&bd->dev, "lcd brightness setting failed.\n");
+                return -EIO;
+        }
+
+        return ret;
+}
+
+static struct lcd_ops ld9040_lcd_ops = {
+        .set_power = ld9040_set_power,
+        .get_power = ld9040_get_power,
+};
+
+static const struct backlight_ops ld9040_backlight_ops  = {
+        .get_brightness = ld9040_get_brightness,
+        .update_status = ld9040_set_brightness,
+};
+
+
+static int ld9040_probe(struct spi_device *spi)
+{
+        int ret = 0;
+        struct ld9040 *lcd = NULL;
+        struct lcd_device *ld = NULL;
+        struct backlight_device *bd = NULL;
+
+        lcd = kzalloc(sizeof(struct ld9040), GFP_KERNEL);
+        if (!lcd)
+                return -ENOMEM;
+
+        /* ld9040 lcd panel uses 3-wire 9bits SPI Mode. */
+        spi->bits_per_word = 9;
+
+        ret = spi_setup(spi);
+        if (ret < 0) {
+                dev_err(&spi->dev, "spi setup failed.\n");
+                goto out_free_lcd;
+        }
+
+        lcd->spi = spi;
+        lcd->dev = &spi->dev;
+
+        lcd->lcd_pd = spi->dev.platform_data;
+        if (!lcd->lcd_pd) {
+                dev_err(&spi->dev, "platform data is NULL.\n");
+                goto out_free_lcd;
+        }
+
+        ld = lcd_device_register("ld9040", &spi->dev, lcd, &ld9040_lcd_ops);
+        if (IS_ERR(ld)) {
+                ret = PTR_ERR(ld);
+                goto out_free_lcd;
+        }
+
+        lcd->ld = ld;
+
+        bd = backlight_device_register("ld9040-bl", &spi->dev,
+                lcd, &ld9040_backlight_ops, NULL);
+        if (IS_ERR(ld)) {
+                ret = PTR_ERR(ld);
+                goto out_free_lcd;
+        }
+
+        bd->props.max_brightness = MAX_BRIGHTNESS;
+        bd->props.brightness = MAX_BRIGHTNESS;
+        lcd->bd = bd;
+
+        /*
+         * if lcd panel was on from bootloader like u-boot then
+         * do not lcd on.
+         */
+        if (!lcd->lcd_pd->lcd_enabled) {
+                /*
+                 * if lcd panel was off from bootloader then
+                 * current lcd status is powerdown and then
+                 * it enables lcd panel.
+                 */
+                lcd->power = FB_BLANK_POWERDOWN;
+
+                ld9040_power(lcd, FB_BLANK_UNBLANK);
+        } else
+                lcd->power = FB_BLANK_UNBLANK;
+
+        dev_set_drvdata(&spi->dev, lcd);
+
+        dev_info(&spi->dev, "ld9040 panel driver has been probed.\n");
+        return 0;
+
+out_free_lcd:
+        kfree(lcd);
+        return ret;
+}
+
+static int __devexit ld9040_remove(struct spi_device *spi)
+{
+        struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+        ld9040_power(lcd, FB_BLANK_POWERDOWN);
+        lcd_device_unregister(lcd->ld);
+        kfree(lcd);
+
+        return 0;
+}
+
+#if defined(CONFIG_PM)
+static int ld9040_suspend(struct spi_device *spi, pm_message_t mesg)
+{
+        int ret = 0;
+        struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+        dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+
+        /*
+         * when lcd panel is suspend, lcd panel becomes off
+         * regardless of status.
+         */
+        ret = ld9040_power(lcd, FB_BLANK_POWERDOWN);
+
+        return ret;
+}
+
+static int ld9040_resume(struct spi_device *spi)
+{
+        int ret = 0;
+        struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+        lcd->power = FB_BLANK_POWERDOWN;
+
+        ret = ld9040_power(lcd, FB_BLANK_UNBLANK);
+
+        return ret;
+}
+#else
+#define ld9040_suspend          NULL
+#define ld9040_resume           NULL
+#endif
+
+/* Power down all displays on reboot, poweroff or halt. */
+static void ld9040_shutdown(struct spi_device *spi)
+{
+        struct ld9040 *lcd = dev_get_drvdata(&spi->dev);
+
+        ld9040_power(lcd, FB_BLANK_POWERDOWN);
+}
+
+static struct spi_driver ld9040_driver = {
+        .driver = {
+                .name   = "ld9040",
+                .bus    = &spi_bus_type,
+                .owner  = THIS_MODULE,
+        },
+        .probe          = ld9040_probe,
+        .remove         = __devexit_p(ld9040_remove),
+        .shutdown       = ld9040_shutdown,
+        .suspend        = ld9040_suspend,
+        .resume         = ld9040_resume,
+};
+
+static int __init ld9040_init(void)
+{
+        return spi_register_driver(&ld9040_driver);
+}
+
+static void __exit ld9040_exit(void)
+{
+        spi_unregister_driver(&ld9040_driver);
+}
+
+module_init(ld9040_init);
+module_exit(ld9040_exit);
+
+MODULE_AUTHOR("Donghwa Lee <dh09.lee@samsung.com>");
+MODULE_DESCRIPTION("ld9040 LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/ld9040_gamma.h b/drivers/video/backlight/ld9040_gamma.h
new file mode 100644
index 000000000000..038d9c86ec03
--- /dev/null
+++ b/drivers/video/backlight/ld9040_gamma.h
@@ -0,0 +1,200 @@
+/*
+ * Gamma level definitions.
+ *
+ * Copyright (c) 2011 Samsung Electronics
+ * InKi Dae <inki.dae@samsung.com>
+ * Donghwa Lee <dh09.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef _LD9040_BRIGHTNESS_H
+#define _LD9040_BRIGHTNESS_H
+
+#define MAX_GAMMA_LEVEL         25
+#define GAMMA_TABLE_COUNT       21
+
+/* gamma value: 2.2 */
+static const unsigned int ld9040_22_300[] = {
+        0x00, 0xa7, 0xb4, 0xae, 0xbf, 0x00, 0x91,
+        0x00, 0xb2, 0xb4, 0xaa, 0xbb, 0x00, 0xac,
+        0x00, 0xb3, 0xb1, 0xaa, 0xbc, 0x00, 0xb3
+};
+
+static const unsigned int ld9040_22_290[] = {
+        0x00, 0xa9, 0xb7, 0xae, 0xbd, 0x00, 0x89,
+        0x00, 0xb7, 0xb6, 0xa8, 0xba, 0x00, 0xa4,
+        0x00, 0xb1, 0xb4, 0xaa, 0xbb, 0x00, 0xaa
+};
+
+static const unsigned int ld9040_22_280[] = {
+        0x00, 0xa9, 0xb6, 0xad, 0xbf, 0x00, 0x86,
+        0x00, 0xb8, 0xb5, 0xa8, 0xbc, 0x00, 0xa0,
+        0x00, 0xb3, 0xb3, 0xa9, 0xbc, 0x00, 0xa7
+};
+
+static const unsigned int ld9040_22_270[] = {
+        0x00, 0xa8, 0xb8, 0xae, 0xbe, 0x00, 0x84,
+        0x00, 0xb9, 0xb7, 0xa8, 0xbc, 0x00, 0x9d,
+        0x00, 0xb2, 0xb5, 0xaa, 0xbc, 0x00, 0xa4
+
+};
+static const unsigned int ld9040_22_260[] = {
+        0x00, 0xa4, 0xb8, 0xb0, 0xbf, 0x00, 0x80,
+        0x00, 0xb8, 0xb6, 0xaa, 0xbc, 0x00, 0x9a,
+        0x00, 0xb0, 0xb5, 0xab, 0xbd, 0x00, 0xa0
+};
+
+static const unsigned int ld9040_22_250[] = {
+        0x00, 0xa4, 0xb9, 0xaf, 0xc1, 0x00, 0x7d,
+        0x00, 0xb9, 0xb6, 0xaa, 0xbb, 0x00, 0x97,
+        0x00, 0xb1, 0xb5, 0xaa, 0xbf, 0x00, 0x9d
+};
+
+static const unsigned int ld9040_22_240[] = {
+        0x00, 0xa2, 0xb9, 0xaf, 0xc2, 0x00, 0x7a,
+        0x00, 0xb9, 0xb7, 0xaa, 0xbd, 0x00, 0x94,
+        0x00, 0xb0, 0xb5, 0xab, 0xbf, 0x00, 0x9a
+};
+
+static const unsigned int ld9040_22_230[] = {
+        0x00, 0xa0, 0xb9, 0xaf, 0xc3, 0x00, 0x77,
+        0x00, 0xb9, 0xb7, 0xab, 0xbe, 0x00, 0x90,
+        0x00, 0xb0, 0xb6, 0xab, 0xbf, 0x00, 0x97
+};
+
+static const unsigned int ld9040_22_220[] = {
+        0x00, 0x9e, 0xba, 0xb0, 0xc2, 0x00, 0x75,
+        0x00, 0xb9, 0xb8, 0xab, 0xbe, 0x00, 0x8e,
+        0x00, 0xb0, 0xb6, 0xac, 0xbf, 0x00, 0x94
+};
+
+static const unsigned int ld9040_22_210[] = {
+        0x00, 0x9c, 0xb9, 0xb0, 0xc4, 0x00, 0x72,
+        0x00, 0xb8, 0xb8, 0xac, 0xbf, 0x00, 0x8a,
+        0x00, 0xb0, 0xb6, 0xac, 0xc0, 0x00, 0x91
+};
+
+static const unsigned int ld9040_22_200[] = {
+        0x00, 0x9a, 0xba, 0xb1, 0xc4, 0x00, 0x6f,
+        0x00, 0xb8, 0xb8, 0xad, 0xc0, 0x00, 0x86,
+        0x00, 0xb0, 0xb7, 0xad, 0xc0, 0x00, 0x8d
+};
+
+static const unsigned int ld9040_22_190[] = {
+        0x00, 0x97, 0xba, 0xb2, 0xc5, 0x00, 0x6c,
+        0x00, 0xb8, 0xb8, 0xae, 0xc1, 0x00, 0x82,
+        0x00, 0xb0, 0xb6, 0xae, 0xc2, 0x00, 0x89
+};
+
+static const unsigned int ld9040_22_180[] = {
+        0x00, 0x93, 0xba, 0xb3, 0xc5, 0x00, 0x69,
+        0x00, 0xb8, 0xb9, 0xae, 0xc1, 0x00, 0x7f,
+        0x00, 0xb0, 0xb6, 0xae, 0xc3, 0x00, 0x85
+};
+
+static const unsigned int ld9040_22_170[] = {
+        0x00, 0x8b, 0xb9, 0xb3, 0xc7, 0x00, 0x65,
+        0x00, 0xb7, 0xb8, 0xaf, 0xc3, 0x00, 0x7a,
+        0x00, 0x80, 0xb6, 0xae, 0xc4, 0x00, 0x81
+};
+
+static const unsigned int ld9040_22_160[] = {
+        0x00, 0x89, 0xba, 0xb3, 0xc8, 0x00, 0x62,
+        0x00, 0xb6, 0xba, 0xaf, 0xc3, 0x00, 0x76,
+        0x00, 0xaf, 0xb7, 0xae, 0xc4, 0x00, 0x7e
+};
+
+static const unsigned int ld9040_22_150[] = {
+        0x00, 0x82, 0xba, 0xb4, 0xc7, 0x00, 0x5f,
+        0x00, 0xb5, 0xba, 0xb0, 0xc3, 0x00, 0x72,
+        0x00, 0xae, 0xb8, 0xb0, 0xc3, 0x00, 0x7a
+};
+
+static const unsigned int ld9040_22_140[] = {
+        0x00, 0x7b, 0xbb, 0xb4, 0xc8, 0x00, 0x5b,
+        0x00, 0xb5, 0xba, 0xb1, 0xc4, 0x00, 0x6e,
+        0x00, 0xae, 0xb9, 0xb0, 0xc5, 0x00, 0x75
+};
+
+static const unsigned int ld9040_22_130[] = {
+        0x00, 0x71, 0xbb, 0xb5, 0xc8, 0x00, 0x57,
+        0x00, 0xb5, 0xbb, 0xb0, 0xc5, 0x00, 0x6a,
+        0x00, 0xae, 0xb9, 0xb1, 0xc6, 0x00, 0x70
+};
+
+static const unsigned int ld9040_22_120[] = {
+        0x00, 0x47, 0xba, 0xb6, 0xca, 0x00, 0x53,
+        0x00, 0xb5, 0xbb, 0xb3, 0xc6, 0x00, 0x65,
+        0x00, 0xae, 0xb8, 0xb3, 0xc7, 0x00, 0x6c
+};
+
+static const unsigned int ld9040_22_110[] = {
+        0x00, 0x13, 0xbb, 0xb7, 0xca, 0x00, 0x4f,
+        0x00, 0xb4, 0xbb, 0xb3, 0xc7, 0x00, 0x60,
+        0x00, 0xad, 0xb8, 0xb4, 0xc7, 0x00, 0x67
+};
+
+static const unsigned int ld9040_22_100[] = {
+        0x00, 0x13, 0xba, 0xb8, 0xcb, 0x00, 0x4b,
+        0x00, 0xb3, 0xbc, 0xb4, 0xc7, 0x00, 0x5c,
+        0x00, 0xac, 0xb8, 0xb4, 0xc8, 0x00, 0x62
+};
+
+static const unsigned int ld9040_22_90[] = {
+        0x00, 0x13, 0xb9, 0xb8, 0xcd, 0x00, 0x46,
+        0x00, 0xb1, 0xbc, 0xb5, 0xc8, 0x00, 0x56,
+        0x00, 0xaa, 0xb8, 0xb4, 0xc9, 0x00, 0x5d
+};
+
+static const unsigned int ld9040_22_80[] = {
+        0x00, 0x13, 0xba, 0xb9, 0xcd, 0x00, 0x41,
+        0x00, 0xb0, 0xbe, 0xb5, 0xc9, 0x00, 0x51,
+        0x00, 0xa9, 0xb9, 0xb5, 0xca, 0x00, 0x57
+};
+
+static const unsigned int ld9040_22_70[] = {
+        0x00, 0x13, 0xb9, 0xb9, 0xd0, 0x00, 0x3c,
+        0x00, 0xaf, 0xbf, 0xb6, 0xcb, 0x00, 0x4b,
+        0x00, 0xa8, 0xb9, 0xb5, 0xcc, 0x00, 0x52
+};
+
+static const unsigned int ld9040_22_50[] = {
+        0x00, 0x13, 0xb2, 0xba, 0xd2, 0x00, 0x30,
+        0x00, 0xaf, 0xc0, 0xb8, 0xcd, 0x00, 0x3d,
+        0x00, 0xa8, 0xb8, 0xb7, 0xcd, 0x00, 0x44
+};
+
+struct ld9040_gamma {
+        unsigned int *gamma_22_table[MAX_GAMMA_LEVEL];
+} gamma_table = {
+        .gamma_22_table[0] = (unsigned int *)&ld9040_22_50,
+        .gamma_22_table[1] = (unsigned int *)&ld9040_22_70,
+        .gamma_22_table[2] = (unsigned int *)&ld9040_22_80,
+        .gamma_22_table[3] = (unsigned int *)&ld9040_22_90,
+        .gamma_22_table[4] = (unsigned int *)&ld9040_22_100,
+        .gamma_22_table[5] = (unsigned int *)&ld9040_22_110,
+        .gamma_22_table[6] = (unsigned int *)&ld9040_22_120,
+        .gamma_22_table[7] = (unsigned int *)&ld9040_22_130,
+        .gamma_22_table[8] = (unsigned int *)&ld9040_22_140,
+        .gamma_22_table[9] = (unsigned int *)&ld9040_22_150,
+        .gamma_22_table[10] = (unsigned int *)&ld9040_22_160,
+        .gamma_22_table[11] = (unsigned int *)&ld9040_22_170,
+        .gamma_22_table[12] = (unsigned int *)&ld9040_22_180,
+        .gamma_22_table[13] = (unsigned int *)&ld9040_22_190,
+        .gamma_22_table[14] = (unsigned int *)&ld9040_22_200,
+        .gamma_22_table[15] = (unsigned int *)&ld9040_22_210,
+        .gamma_22_table[16] = (unsigned int *)&ld9040_22_220,
+        .gamma_22_table[17] = (unsigned int *)&ld9040_22_230,
+        .gamma_22_table[18] = (unsigned int *)&ld9040_22_240,
+        .gamma_22_table[19] = (unsigned int *)&ld9040_22_250,
+        .gamma_22_table[20] = (unsigned int *)&ld9040_22_260,
+        .gamma_22_table[21] = (unsigned int *)&ld9040_22_270,
+        .gamma_22_table[22] = (unsigned int *)&ld9040_22_280,
+        .gamma_22_table[23] = (unsigned int *)&ld9040_22_290,
+        .gamma_22_table[24] = (unsigned int *)&ld9040_22_300,
+};
+
+#endif
diff --git a/drivers/video/backlight/lms283gf05.c b/drivers/video/backlight/lms283gf05.c
index abc43a0eb97d..5d3cf33953ac 100644
--- a/drivers/video/backlight/lms283gf05.c
+++ b/drivers/video/backlight/lms283gf05.c
@@ -129,7 +129,7 @@ static int lms283gf05_power_set(struct lcd_device *ld, int power)
         struct spi_device *spi = st->spi;
         struct lms283gf05_pdata *pdata = spi->dev.platform_data;
 
-        if (power) {
+        if (power <= FB_BLANK_NORMAL) {
                 if (pdata)
                         lms283gf05_reset(pdata->reset_gpio,
                                         pdata->reset_inverted);
diff --git a/drivers/video/backlight/locomolcd.c b/drivers/video/backlight/locomolcd.c
index d2f59015d517..be20b5cbe26c 100644
--- a/drivers/video/backlight/locomolcd.c
+++ b/drivers/video/backlight/locomolcd.c
@@ -6,7 +6,7 @@
  * GPL v2
  *
  * This driver assumes single CPU. That's okay, because collie is
- * slightly old hardware, and noone is going to retrofit second CPU to
+ * slightly old hardware, and no one is going to retrofit second CPU to
  * old PDA.
  */
 
@@ -184,6 +184,7 @@ static int locomolcd_probe(struct locomo_dev *ldev)
         local_irq_restore(flags);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = 4;
         locomolcd_bl_device = backlight_device_register("locomo-bl",
                                                         &ldev->dev, NULL,
diff --git a/drivers/video/backlight/ltv350qv.c b/drivers/video/backlight/ltv350qv.c
index 8010aaeb5adb..cca43c06d3c8 100644
--- a/drivers/video/backlight/ltv350qv.c
+++ b/drivers/video/backlight/ltv350qv.c
@@ -239,11 +239,15 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
         lcd->spi = spi;
         lcd->power = FB_BLANK_POWERDOWN;
         lcd->buffer = kzalloc(8, GFP_KERNEL);
+        if (!lcd->buffer) {
+                ret = -ENOMEM;
+                goto out_free_lcd;
+        }
 
         ld = lcd_device_register("ltv350qv", &spi->dev, lcd, &ltv_ops);
         if (IS_ERR(ld)) {
                 ret = PTR_ERR(ld);
-                goto out_free_lcd;
+                goto out_free_buffer;
         }
         lcd->ld = ld;
 
@@ -257,6 +261,8 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 
 out_unregister:
         lcd_device_unregister(ld);
+out_free_buffer:
+        kfree(lcd->buffer);
 out_free_lcd:
         kfree(lcd);
         return ret;
@@ -268,6 +274,7 @@ static int __devexit ltv350qv_remove(struct spi_device *spi)
 
         ltv350qv_power(lcd, FB_BLANK_POWERDOWN);
         lcd_device_unregister(lcd->ld);
+        kfree(lcd->buffer);
         kfree(lcd);
 
         return 0;
@@ -326,7 +333,7 @@ static void __exit ltv350qv_exit(void)
 module_init(ltv350qv_init);
 module_exit(ltv350qv_exit);
 
-MODULE_AUTHOR("Haavard Skinnemoen <hskinnemoen@atmel.com>");
+MODULE_AUTHOR("Haavard Skinnemoen (Atmel)");
 MODULE_DESCRIPTION("Samsung LTV350QV LCD Driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("spi:ltv350qv");
diff --git a/drivers/video/backlight/max8925_bl.c b/drivers/video/backlight/max8925_bl.c
index b2b2c7ba1f63..07e8e273ced0 100644
--- a/drivers/video/backlight/max8925_bl.c
+++ b/drivers/video/backlight/max8925_bl.c
@@ -92,7 +92,7 @@ static int max8925_backlight_get_brightness(struct backlight_device *bl)
         return ret;
 }
 
-static struct backlight_ops max8925_backlight_ops = {
+static const struct backlight_ops max8925_backlight_ops = {
         .options        = BL_CORE_SUSPENDRESUME,
         .update_status  = max8925_backlight_update_status,
         .get_brightness = max8925_backlight_get_brightness,
@@ -136,6 +136,7 @@ static int __devinit max8925_backlight_probe(struct platform_device *pdev)
         data->current_brightness = 0;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = MAX_BRIGHTNESS;
         bl = backlight_device_register(name, &pdev->dev, data,
                                         &max8925_backlight_ops, &props);
diff --git a/drivers/video/backlight/mbp_nvidia_bl.c b/drivers/video/backlight/mbp_nvidia_bl.c
deleted file mode 100644
index 9fb533f6373e..000000000000
--- a/drivers/video/backlight/mbp_nvidia_bl.c
+++ /dev/null
@@ -1,382 +0,0 @@
-/*
- *  Backlight Driver for Nvidia 8600 in Macbook Pro
- *
- *  Copyright (c) Red Hat <mjg@redhat.com>
- *  Based on code from Pommed:
- *  Copyright (C) 2006 Nicolas Boichat <nicolas @boichat.ch>
- *  Copyright (C) 2006 Felipe Alfaro Solana <felipe_alfaro @linuxmail.org>
- *  Copyright (C) 2007 Julien BLACHE <jb@jblache.org>
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License version 2 as
- *  published by the Free Software Foundation.
- *
- *  This driver triggers SMIs which cause the firmware to change the
- *  backlight brightness. This is icky in many ways, but it's impractical to
- *  get at the firmware code in order to figure out what it's actually doing.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/backlight.h>
-#include <linux/err.h>
-#include <linux/dmi.h>
-#include <linux/io.h>
-
-static struct backlight_device *mbp_backlight_device;
-
-/* Structure to be passed to the DMI_MATCH function. */
-struct dmi_match_data {
-        /* I/O resource to allocate. */
-        unsigned long iostart;
-        unsigned long iolen;
-        /* Backlight operations structure. */
-        const struct backlight_ops backlight_ops;
-};
-
-/* Module parameters. */
-static int debug;
-module_param_named(debug, debug, int, 0644);
-MODULE_PARM_DESC(debug, "Set to one to enable debugging messages.");
-
-/*
- * Implementation for MacBooks with Intel chipset.
- */
-static int intel_chipset_send_intensity(struct backlight_device *bd)
-{
-        int intensity = bd->props.brightness;
-
-        if (debug)
-                printk(KERN_DEBUG "mbp_nvidia_bl: setting brightness to %d\n",
-                       intensity);
-
-        outb(0x04 | (intensity << 4), 0xb3);
-        outb(0xbf, 0xb2);
-        return 0;
-}
-
-static int intel_chipset_get_intensity(struct backlight_device *bd)
-{
-        int intensity;
-
-        outb(0x03, 0xb3);
-        outb(0xbf, 0xb2);
-        intensity = inb(0xb3) >> 4;
-
-        if (debug)
-                printk(KERN_DEBUG "mbp_nvidia_bl: read brightness of %d\n",
-                       intensity);
-
-        return intensity;
-}
-
-static const struct dmi_match_data intel_chipset_data = {
-        .iostart = 0xb2,
-        .iolen = 2,
-        .backlight_ops  = {
-                .options        = BL_CORE_SUSPENDRESUME,
-                .get_brightness = intel_chipset_get_intensity,
-                .update_status  = intel_chipset_send_intensity,
-        }
-};
-
-/*
- * Implementation for MacBooks with Nvidia chipset.
- */
-static int nvidia_chipset_send_intensity(struct backlight_device *bd)
-{
-        int intensity = bd->props.brightness;
-
-        if (debug)
-                printk(KERN_DEBUG "mbp_nvidia_bl: setting brightness to %d\n",
-                       intensity);
-
-        outb(0x04 | (intensity << 4), 0x52f);
-        outb(0xbf, 0x52e);
-        return 0;
-}
-
-static int nvidia_chipset_get_intensity(struct backlight_device *bd)
-{
-        int intensity;
-
-        outb(0x03, 0x52f);
-        outb(0xbf, 0x52e);
-        intensity = inb(0x52f) >> 4;
-
-        if (debug)
-                printk(KERN_DEBUG "mbp_nvidia_bl: read brightness of %d\n",
-                       intensity);
-
-        return intensity;
-}
-
-static const struct dmi_match_data nvidia_chipset_data = {
-        .iostart = 0x52e,
-        .iolen = 2,
-        .backlight_ops          = {
-                .options        = BL_CORE_SUSPENDRESUME,
-                .get_brightness = nvidia_chipset_get_intensity,
-                .update_status  = nvidia_chipset_send_intensity
-        }
-};
-
-/*
- * DMI matching.
- */
-static /* const */ struct dmi_match_data *driver_data;
-
-static int mbp_dmi_match(const struct dmi_system_id *id)
-{
-        driver_data = id->driver_data;
-
-        printk(KERN_INFO "mbp_nvidia_bl: %s detected\n", id->ident);
-        return 1;
-}
-
-static const struct dmi_system_id __initdata mbp_device_table[] = {
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 1,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Computer, Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook1,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 2,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook2,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 3,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook3,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 4,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook4,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 4,2",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook4,2"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 1,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro1,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 1,2",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro1,2"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 2,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro2,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 2,2",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro2,2"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 3,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro3,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 3,2",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro3,2"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 4,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro4,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookAir 1,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir1,1"),
-                },
-                .driver_data    = (void *)&intel_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 5,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook5,1"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 5,2",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook5,2"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBook 6,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook6,1"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookAir 2,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir2,1"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 5,1",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,1"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 5,2",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,2"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 5,3",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,3"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 5,4",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,4"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        {
-                .callback       = mbp_dmi_match,
-                .ident          = "MacBookPro 5,5",
-                .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
-                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,5"),
-                },
-                .driver_data    = (void *)&nvidia_chipset_data,
-        },
-        { }
-};
-
-static int __init mbp_init(void)
-{
-        struct backlight_properties props;
-        if (!dmi_check_system(mbp_device_table))
-                return -ENODEV;
-
-        if (!request_region(driver_data->iostart, driver_data->iolen, 
-                                                "Macbook Pro backlight"))
-                return -ENXIO;
-
-        memset(&props, 0, sizeof(struct backlight_properties));
-        props.max_brightness = 15;
-        mbp_backlight_device = backlight_device_register("mbp_backlight", NULL,
-                                                         NULL,
-                                                         &driver_data->backlight_ops,
-                                                         &props);
-        if (IS_ERR(mbp_backlight_device)) {
-                release_region(driver_data->iostart, driver_data->iolen);
-                return PTR_ERR(mbp_backlight_device);
-        }
-
-        mbp_backlight_device->props.brightness =
-                driver_data->backlight_ops.get_brightness(mbp_backlight_device);
-        backlight_update_status(mbp_backlight_device);
-
-        return 0;
-}
-
-static void __exit mbp_exit(void)
-{
-        backlight_device_unregister(mbp_backlight_device);
-
-        release_region(driver_data->iostart, driver_data->iolen);
-}
-
-module_init(mbp_init);
-module_exit(mbp_exit);
-
-MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
-MODULE_DESCRIPTION("Nvidia-based Macbook Pro Backlight Driver");
-MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(dmi, mbp_device_table);
diff --git a/drivers/video/backlight/omap1_bl.c b/drivers/video/backlight/omap1_bl.c
index d3bc56296c8d..08d26a72394c 100644
--- a/drivers/video/backlight/omap1_bl.c
+++ b/drivers/video/backlight/omap1_bl.c
@@ -146,6 +146,7 @@ static int omapbl_probe(struct platform_device *pdev)
                 return -ENOMEM;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = OMAPBL_MAX_INTENSITY;
         dev = backlight_device_register("omap-bl", &pdev->dev, bl, &omapbl_ops,
                                         &props);
diff --git a/drivers/video/backlight/pcf50633-backlight.c b/drivers/video/backlight/pcf50633-backlight.c
index 3c424f7efdcc..ef5628d60563 100644
--- a/drivers/video/backlight/pcf50633-backlight.c
+++ b/drivers/video/backlight/pcf50633-backlight.c
@@ -112,6 +112,7 @@ static int __devinit pcf50633_bl_probe(struct platform_device *pdev)
         if (!pcf_bl)
                 return -ENOMEM;
 
+        bl_props.type = BACKLIGHT_RAW;
         bl_props.max_brightness = 0x3f;
         bl_props.power = FB_BLANK_UNBLANK;
 
diff --git a/drivers/video/backlight/progear_bl.c b/drivers/video/backlight/progear_bl.c
index 809278c90738..6af183d6465e 100644
--- a/drivers/video/backlight/progear_bl.c
+++ b/drivers/video/backlight/progear_bl.c
@@ -84,6 +84,7 @@ static int progearbl_probe(struct platform_device *pdev)
         pci_write_config_byte(sb_dev, SB_MPS1, temp | 0x20);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = HW_LEVEL_MAX - HW_LEVEL_MIN;
         progear_backlight_device = backlight_device_register("progear-bl",
                                                              &pdev->dev, NULL,
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 550443518891..b8f38ec6eb18 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -25,8 +25,10 @@ struct pwm_bl_data {
         struct pwm_device       *pwm;
         struct device           *dev;
         unsigned int            period;
+        unsigned int            lth_brightness;
         int                     (*notify)(struct device *,
                                           int brightness);
+        int                     (*check_fb)(struct device *, struct fb_info *);
 };
 
 static int pwm_backlight_update_status(struct backlight_device *bl)
@@ -48,7 +50,9 @@ static int pwm_backlight_update_status(struct backlight_device *bl)
                 pwm_config(pb->pwm, 0, pb->period);
                 pwm_disable(pb->pwm);
         } else {
-                pwm_config(pb->pwm, brightness * pb->period / max, pb->period);
+                brightness = pb->lth_brightness +
+                        (brightness * (pb->period - pb->lth_brightness) / max);
+                pwm_config(pb->pwm, brightness, pb->period);
                 pwm_enable(pb->pwm);
         }
         return 0;
@@ -59,9 +63,18 @@ static int pwm_backlight_get_brightness(struct backlight_device *bl)
         return bl->props.brightness;
 }
 
+static int pwm_backlight_check_fb(struct backlight_device *bl,
+                                  struct fb_info *info)
+{
+        struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
+
+        return !pb->check_fb || pb->check_fb(pb->dev, info);
+}
+
 static const struct backlight_ops pwm_backlight_ops = {
         .update_status  = pwm_backlight_update_status,
         .get_brightness = pwm_backlight_get_brightness,
+        .check_fb       = pwm_backlight_check_fb,
 };
 
 static int pwm_backlight_probe(struct platform_device *pdev)
@@ -92,6 +105,9 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 
         pb->period = data->pwm_period_ns;
         pb->notify = data->notify;
+        pb->check_fb = data->check_fb;
+        pb->lth_brightness = data->lth_brightness *
+                (data->pwm_period_ns / data->max_brightness);
         pb->dev = &pdev->dev;
 
         pb->pwm = pwm_request(data->pwm_id, "backlight");
@@ -103,6 +119,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
                 dev_dbg(&pdev->dev, "got pwm for backlight\n");
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = data->max_brightness;
         bl = backlight_device_register(dev_name(&pdev->dev), &pdev->dev, pb,
                                        &pwm_backlight_ops, &props);
diff --git a/drivers/video/backlight/s6e63m0.c b/drivers/video/backlight/s6e63m0.c
index a3128c9cb7ad..322040f686c2 100644
--- a/drivers/video/backlight/s6e63m0.c
+++ b/drivers/video/backlight/s6e63m0.c
@@ -729,10 +729,10 @@ static ssize_t s6e63m0_sysfs_show_gamma_table(struct device *dev,
 
         return strlen(buf);
 }
-static DEVICE_ATTR(gamma_table, 0644,
+static DEVICE_ATTR(gamma_table, 0444,
                 s6e63m0_sysfs_show_gamma_table, NULL);
 
-static int __init s6e63m0_probe(struct spi_device *spi)
+static int __devinit s6e63m0_probe(struct spi_device *spi)
 {
         int ret = 0;
         struct s6e63m0 *lcd = NULL;
@@ -778,6 +778,7 @@ static int __init s6e63m0_probe(struct spi_device *spi)
 
         bd->props.max_brightness = MAX_BRIGHTNESS;
         bd->props.brightness = MAX_BRIGHTNESS;
+        bd->props.type = BACKLIGHT_RAW;
         lcd->bd = bd;
 
         /*
@@ -829,6 +830,9 @@ static int __devexit s6e63m0_remove(struct spi_device *spi)
         struct s6e63m0 *lcd = dev_get_drvdata(&spi->dev);
 
         s6e63m0_power(lcd, FB_BLANK_POWERDOWN);
+        device_remove_file(&spi->dev, &dev_attr_gamma_table);
+        device_remove_file(&spi->dev, &dev_attr_gamma_mode);
+        backlight_device_unregister(lcd->bd);
         lcd_device_unregister(lcd->ld);
         kfree(lcd);
 
diff --git a/drivers/video/backlight/tosa_bl.c b/drivers/video/backlight/tosa_bl.c
index 2a04b382ec48..425a7365470b 100644
--- a/drivers/video/backlight/tosa_bl.c
+++ b/drivers/video/backlight/tosa_bl.c
@@ -102,6 +102,7 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
         data->i2c = client;
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = 512 - 1;
         data->bl = backlight_device_register("tosa-bl", &client->dev, data,
                                              &bl_ops, &props);
diff --git a/drivers/video/backlight/wm831x_bl.c b/drivers/video/backlight/wm831x_bl.c
index 08fd87f3aecc..d4c6eb248ff9 100644
--- a/drivers/video/backlight/wm831x_bl.c
+++ b/drivers/video/backlight/wm831x_bl.c
@@ -193,6 +193,7 @@ static int wm831x_backlight_probe(struct platform_device *pdev)
         data->current_brightness = 0;
         data->isink_reg = isink_reg;
 
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = max_isel;
         bl = backlight_device_register("wm831x", &pdev->dev, data,
                                        &wm831x_backlight_ops, &props);
diff --git a/drivers/video/bf537-lq035.c b/drivers/video/bf537-lq035.c
new file mode 100644
index 000000000000..bea53c1a4950
--- /dev/null
+++ b/drivers/video/bf537-lq035.c
@@ -0,0 +1,923 @@
+/*
+ * Analog Devices Blackfin(BF537 STAMP) + SHARP TFT LCD.
+ * http://docs.blackfin.uclinux.org/doku.php?id=hw:cards:tft-lcd
+ *
+ * Copyright 2006-2010 Analog Devices Inc.
+ * Licensed under the GPL-2.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/timer.h>
+#include <linux/device.h>
+#include <linux/backlight.h>
+#include <linux/lcd.h>
+#include <linux/i2c.h>
+#include <linux/spinlock.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include <asm/blackfin.h>
+#include <asm/irq.h>
+#include <asm/dpmc.h>
+#include <asm/dma.h>
+#include <asm/portmux.h>
+
+#define NO_BL 1
+
+#define MAX_BRIGHENESS  95
+#define MIN_BRIGHENESS  5
+#define NBR_PALETTE     256
+
+static const unsigned short ppi_pins[] = {
+        P_PPI0_CLK, P_PPI0_D0, P_PPI0_D1, P_PPI0_D2, P_PPI0_D3,
+        P_PPI0_D4, P_PPI0_D5, P_PPI0_D6, P_PPI0_D7,
+        P_PPI0_D8, P_PPI0_D9, P_PPI0_D10, P_PPI0_D11,
+        P_PPI0_D12, P_PPI0_D13, P_PPI0_D14, P_PPI0_D15, 0
+};
+
+static unsigned char *fb_buffer;          /* RGB Buffer */
+static unsigned long *dma_desc_table;
+static int t_conf_done, lq035_open_cnt;
+static DEFINE_SPINLOCK(bfin_lq035_lock);
+
+static int landscape;
+module_param(landscape, int, 0);
+MODULE_PARM_DESC(landscape,
+        "LANDSCAPE use 320x240 instead of Native 240x320 Resolution");
+
+static int bgr;
+module_param(bgr, int, 0);
+MODULE_PARM_DESC(bgr,
+        "BGR use 16-bit BGR-565 instead of RGB-565");
+
+static int nocursor = 1;
+module_param(nocursor, int, 0644);
+MODULE_PARM_DESC(nocursor, "cursor enable/disable");
+
+static unsigned long current_brightness;  /* backlight */
+
+/* AD5280 vcomm */
+static unsigned char vcomm_value = 150;
+static struct i2c_client *ad5280_client;
+
+static void set_vcomm(void)
+{
+        int nr;
+
+        if (!ad5280_client)
+                return;
+
+        nr = i2c_smbus_write_byte_data(ad5280_client, 0x00, vcomm_value);
+        if (nr)
+                pr_err("i2c_smbus_write_byte_data fail: %d\n", nr);
+}
+
+static int __devinit ad5280_probe(struct i2c_client *client,
+                                  const struct i2c_device_id *id)
+{
+        int ret;
+        if (!i2c_check_functionality(client->adapter,
+                                     I2C_FUNC_SMBUS_BYTE_DATA)) {
+                dev_err(&client->dev, "SMBUS Byte Data not Supported\n");
+                return -EIO;
+        }
+
+        ret = i2c_smbus_write_byte_data(client, 0x00, vcomm_value);
+        if (ret) {
+                dev_err(&client->dev, "write fail: %d\n", ret);
+                return ret;
+        }
+
+        ad5280_client = client;
+
+        return 0;
+}
+
+static int __devexit ad5280_remove(struct i2c_client *client)
+{
+        ad5280_client = NULL;
+        return 0;
+}
+
+static const struct i2c_device_id ad5280_id[] = {
+        {"bf537-lq035-ad5280", 0},
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, ad5280_id);
+
+static struct i2c_driver ad5280_driver = {
+        .driver = {
+                .name = "bf537-lq035-ad5280",
+        },
+        .probe = ad5280_probe,
+        .remove = __devexit_p(ad5280_remove),
+        .id_table = ad5280_id,
+};
+
+#ifdef CONFIG_PNAV10
+#define MOD GPIO_PH13
+
+#define bfin_write_TIMER_LP_CONFIG      bfin_write_TIMER0_CONFIG
+#define bfin_write_TIMER_LP_WIDTH       bfin_write_TIMER0_WIDTH
+#define bfin_write_TIMER_LP_PERIOD      bfin_write_TIMER0_PERIOD
+#define bfin_read_TIMER_LP_COUNTER      bfin_read_TIMER0_COUNTER
+#define TIMDIS_LP                       TIMDIS0
+#define TIMEN_LP                        TIMEN0
+
+#define bfin_write_TIMER_SPS_CONFIG     bfin_write_TIMER1_CONFIG
+#define bfin_write_TIMER_SPS_WIDTH      bfin_write_TIMER1_WIDTH
+#define bfin_write_TIMER_SPS_PERIOD     bfin_write_TIMER1_PERIOD
+#define TIMDIS_SPS                      TIMDIS1
+#define TIMEN_SPS                       TIMEN1
+
+#define bfin_write_TIMER_SP_CONFIG      bfin_write_TIMER5_CONFIG
+#define bfin_write_TIMER_SP_WIDTH       bfin_write_TIMER5_WIDTH
+#define bfin_write_TIMER_SP_PERIOD      bfin_write_TIMER5_PERIOD
+#define TIMDIS_SP                       TIMDIS5
+#define TIMEN_SP                        TIMEN5
+
+#define bfin_write_TIMER_PS_CLS_CONFIG  bfin_write_TIMER2_CONFIG
+#define bfin_write_TIMER_PS_CLS_WIDTH   bfin_write_TIMER2_WIDTH
+#define bfin_write_TIMER_PS_CLS_PERIOD  bfin_write_TIMER2_PERIOD
+#define TIMDIS_PS_CLS                   TIMDIS2
+#define TIMEN_PS_CLS                    TIMEN2
+
+#define bfin_write_TIMER_REV_CONFIG     bfin_write_TIMER3_CONFIG
+#define bfin_write_TIMER_REV_WIDTH      bfin_write_TIMER3_WIDTH
+#define bfin_write_TIMER_REV_PERIOD     bfin_write_TIMER3_PERIOD
+#define TIMDIS_REV                      TIMDIS3
+#define TIMEN_REV                       TIMEN3
+#define bfin_read_TIMER_REV_COUNTER     bfin_read_TIMER3_COUNTER
+
+#define FREQ_PPI_CLK         (5*1024*1024)  /* PPI_CLK 5MHz */
+
+#define TIMERS {P_TMR0, P_TMR1, P_TMR2, P_TMR3, P_TMR5, 0}
+
+#else
+
+#define UD      GPIO_PF13       /* Up / Down */
+#define MOD     GPIO_PF10
+#define LBR     GPIO_PF14       /* Left Right */
+
+#define bfin_write_TIMER_LP_CONFIG      bfin_write_TIMER6_CONFIG
+#define bfin_write_TIMER_LP_WIDTH       bfin_write_TIMER6_WIDTH
+#define bfin_write_TIMER_LP_PERIOD      bfin_write_TIMER6_PERIOD
+#define bfin_read_TIMER_LP_COUNTER      bfin_read_TIMER6_COUNTER
+#define TIMDIS_LP                       TIMDIS6
+#define TIMEN_LP                        TIMEN6
+
+#define bfin_write_TIMER_SPS_CONFIG     bfin_write_TIMER1_CONFIG
+#define bfin_write_TIMER_SPS_WIDTH      bfin_write_TIMER1_WIDTH
+#define bfin_write_TIMER_SPS_PERIOD     bfin_write_TIMER1_PERIOD
+#define TIMDIS_SPS                      TIMDIS1
+#define TIMEN_SPS                       TIMEN1
+
+#define bfin_write_TIMER_SP_CONFIG      bfin_write_TIMER0_CONFIG
+#define bfin_write_TIMER_SP_WIDTH       bfin_write_TIMER0_WIDTH
+#define bfin_write_TIMER_SP_PERIOD      bfin_write_TIMER0_PERIOD
+#define TIMDIS_SP                       TIMDIS0
+#define TIMEN_SP                        TIMEN0
+
+#define bfin_write_TIMER_PS_CLS_CONFIG  bfin_write_TIMER7_CONFIG
+#define bfin_write_TIMER_PS_CLS_WIDTH   bfin_write_TIMER7_WIDTH
+#define bfin_write_TIMER_PS_CLS_PERIOD  bfin_write_TIMER7_PERIOD
+#define TIMDIS_PS_CLS                   TIMDIS7
+#define TIMEN_PS_CLS                    TIMEN7
+
+#define bfin_write_TIMER_REV_CONFIG     bfin_write_TIMER5_CONFIG
+#define bfin_write_TIMER_REV_WIDTH      bfin_write_TIMER5_WIDTH
+#define bfin_write_TIMER_REV_PERIOD     bfin_write_TIMER5_PERIOD
+#define TIMDIS_REV                      TIMDIS5
+#define TIMEN_REV                       TIMEN5
+#define bfin_read_TIMER_REV_COUNTER     bfin_read_TIMER5_COUNTER
+
+#define FREQ_PPI_CLK         (6*1000*1000)  /* PPI_CLK 6MHz */
+#define TIMERS {P_TMR0, P_TMR1, P_TMR5, P_TMR6, P_TMR7, 0}
+
+#endif
+
+#define LCD_X_RES                       240 /* Horizontal Resolution */
+#define LCD_Y_RES                       320 /* Vertical Resolution */
+
+#define LCD_BBP                         16  /* Bit Per Pixel */
+
+/* the LCD and the DMA start counting differently;
+ * since one starts at 0 and the other starts at 1,
+ * we have a difference of 1 between START_LINES
+ * and U_LINES.
+ */
+#define START_LINES       8   /* lines for field flyback or field blanking signal */
+#define U_LINES           9   /* number of undisplayed blanking lines */
+
+#define FRAMES_PER_SEC    (60)
+
+#define DCLKS_PER_FRAME   (FREQ_PPI_CLK/FRAMES_PER_SEC)
+#define DCLKS_PER_LINE    (DCLKS_PER_FRAME/(LCD_Y_RES+U_LINES))
+
+#define PPI_CONFIG_VALUE  (PORT_DIR|XFR_TYPE|DLEN_16|POLS)
+#define PPI_DELAY_VALUE   (0)
+#define TIMER_CONFIG      (PWM_OUT|PERIOD_CNT|TIN_SEL|CLK_SEL)
+
+#define ACTIVE_VIDEO_MEM_OFFSET (LCD_X_RES*START_LINES*(LCD_BBP/8))
+#define ACTIVE_VIDEO_MEM_SIZE   (LCD_Y_RES*LCD_X_RES*(LCD_BBP/8))
+#define TOTAL_VIDEO_MEM_SIZE    ((LCD_Y_RES+U_LINES)*LCD_X_RES*(LCD_BBP/8))
+#define TOTAL_DMA_DESC_SIZE     (2 * sizeof(u32) * (LCD_Y_RES + U_LINES))
+
+static void start_timers(void) /* CHECK with HW */
+{
+        unsigned long flags;
+
+        local_irq_save(flags);
+
+        bfin_write_TIMER_ENABLE(TIMEN_REV);
+        SSYNC();
+
+        while (bfin_read_TIMER_REV_COUNTER() <= 11)
+                continue;
+        bfin_write_TIMER_ENABLE(TIMEN_LP);
+        SSYNC();
+
+        while (bfin_read_TIMER_LP_COUNTER() < 3)
+                continue;
+        bfin_write_TIMER_ENABLE(TIMEN_SP|TIMEN_SPS|TIMEN_PS_CLS);
+        SSYNC();
+        t_conf_done = 1;
+        local_irq_restore(flags);
+}
+
+static void config_timers(void)
+{
+        /* Stop timers */
+        bfin_write_TIMER_DISABLE(TIMDIS_SP|TIMDIS_SPS|TIMDIS_REV|
+                                 TIMDIS_LP|TIMDIS_PS_CLS);
+        SSYNC();
+
+        /* LP, timer 6 */
+        bfin_write_TIMER_LP_CONFIG(TIMER_CONFIG|PULSE_HI);
+        bfin_write_TIMER_LP_WIDTH(1);
+
+        bfin_write_TIMER_LP_PERIOD(DCLKS_PER_LINE);
+        SSYNC();
+
+        /* SPS, timer 1 */
+        bfin_write_TIMER_SPS_CONFIG(TIMER_CONFIG|PULSE_HI);
+        bfin_write_TIMER_SPS_WIDTH(DCLKS_PER_LINE*2);
+        bfin_write_TIMER_SPS_PERIOD((DCLKS_PER_LINE * (LCD_Y_RES+U_LINES)));
+        SSYNC();
+
+        /* SP, timer 0 */
+        bfin_write_TIMER_SP_CONFIG(TIMER_CONFIG|PULSE_HI);
+        bfin_write_TIMER_SP_WIDTH(1);
+        bfin_write_TIMER_SP_PERIOD(DCLKS_PER_LINE);
+        SSYNC();
+
+        /* PS & CLS, timer 7 */
+        bfin_write_TIMER_PS_CLS_CONFIG(TIMER_CONFIG);
+        bfin_write_TIMER_PS_CLS_WIDTH(LCD_X_RES + START_LINES);
+        bfin_write_TIMER_PS_CLS_PERIOD(DCLKS_PER_LINE);
+
+        SSYNC();
+
+#ifdef NO_BL
+        /* REV, timer 5 */
+        bfin_write_TIMER_REV_CONFIG(TIMER_CONFIG|PULSE_HI);
+
+        bfin_write_TIMER_REV_WIDTH(DCLKS_PER_LINE);
+        bfin_write_TIMER_REV_PERIOD(DCLKS_PER_LINE*2);
+
+        SSYNC();
+#endif
+}
+
+static void config_ppi(void)
+{
+        bfin_write_PPI_DELAY(PPI_DELAY_VALUE);
+        bfin_write_PPI_COUNT(LCD_X_RES-1);
+        /* 0x10 -> PORT_CFG -> 2 or 3 frame syncs */
+        bfin_write_PPI_CONTROL((PPI_CONFIG_VALUE|0x10) & (~POLS));
+}
+
+static int config_dma(void)
+{
+        u32 i;
+
+        if (landscape) {
+
+                for (i = 0; i < U_LINES; ++i) {
+                        /* blanking lines point to first line of fb_buffer */
+                        dma_desc_table[2*i] = (unsigned long)&dma_desc_table[2*i+2];
+                        dma_desc_table[2*i+1] = (unsigned long)fb_buffer;
+                }
+
+                for (i = U_LINES; i < U_LINES + LCD_Y_RES; ++i) {
+                        /* visible lines */
+                        dma_desc_table[2*i] = (unsigned long)&dma_desc_table[2*i+2];
+                        dma_desc_table[2*i+1] = (unsigned long)fb_buffer +
+                                                (LCD_Y_RES+U_LINES-1-i)*2;
+                }
+
+                /* last descriptor points to first */
+                dma_desc_table[2*(LCD_Y_RES+U_LINES-1)] = (unsigned long)&dma_desc_table[0];
+
+                set_dma_x_count(CH_PPI, LCD_X_RES);
+                set_dma_x_modify(CH_PPI, LCD_Y_RES * (LCD_BBP / 8));
+                set_dma_y_count(CH_PPI, 0);
+                set_dma_y_modify(CH_PPI, 0);
+                set_dma_next_desc_addr(CH_PPI, (void *)dma_desc_table[0]);
+                set_dma_config(CH_PPI, DMAFLOW_LARGE | NDSIZE_4 | WDSIZE_16);
+
+        } else {
+
+                set_dma_config(CH_PPI, set_bfin_dma_config(DIR_READ,
+                                DMA_FLOW_AUTO,
+                                INTR_DISABLE,
+                                DIMENSION_2D,
+                                DATA_SIZE_16,
+                                DMA_NOSYNC_KEEP_DMA_BUF));
+                set_dma_x_count(CH_PPI, LCD_X_RES);
+                set_dma_x_modify(CH_PPI, LCD_BBP / 8);
+                set_dma_y_count(CH_PPI, LCD_Y_RES+U_LINES);
+                set_dma_y_modify(CH_PPI, LCD_BBP / 8);
+                set_dma_start_addr(CH_PPI, (unsigned long) fb_buffer);
+        }
+
+        return 0;
+}
+
+static int __devinit request_ports(void)
+{
+        u16 tmr_req[] = TIMERS;
+
+        /*
+                UD:      PF13
+                MOD:     PF10
+                LBR:     PF14
+                PPI_CLK: PF15
+        */
+
+        if (peripheral_request_list(ppi_pins, KBUILD_MODNAME)) {
+                pr_err("requesting PPI peripheral failed\n");
+                return -EBUSY;
+        }
+
+        if (peripheral_request_list(tmr_req, KBUILD_MODNAME)) {
+                peripheral_free_list(ppi_pins);
+                pr_err("requesting timer peripheral failed\n");
+                return -EBUSY;
+        }
+
+#if (defined(UD) && defined(LBR))
+        if (gpio_request(UD, KBUILD_MODNAME)) {
+                pr_err("requesting GPIO %d failed\n", UD);
+                return -EBUSY;
+        }
+
+        if (gpio_request(LBR, KBUILD_MODNAME)) {
+                pr_err("requesting GPIO %d failed\n", LBR);
+                gpio_free(UD);
+                return -EBUSY;
+        }
+
+        gpio_direction_output(UD, 0);
+        gpio_direction_output(LBR, 1);
+
+#endif
+
+        if (gpio_request(MOD, KBUILD_MODNAME)) {
+                pr_err("requesting GPIO %d failed\n", MOD);
+#if (defined(UD) && defined(LBR))
+                gpio_free(LBR);
+                gpio_free(UD);
+#endif
+                return -EBUSY;
+        }
+
+        gpio_direction_output(MOD, 1);
+
+        SSYNC();
+        return 0;
+}
+
+static void free_ports(void)
+{
+        u16 tmr_req[] = TIMERS;
+
+        peripheral_free_list(ppi_pins);
+        peripheral_free_list(tmr_req);
+
+#if defined(UD) && defined(LBR)
+        gpio_free(LBR);
+        gpio_free(UD);
+#endif
+        gpio_free(MOD);
+}
+
+static struct fb_info bfin_lq035_fb;
+
+static struct fb_var_screeninfo bfin_lq035_fb_defined = {
+        .bits_per_pixel         = LCD_BBP,
+        .activate               = FB_ACTIVATE_TEST,
+        .xres                   = LCD_X_RES,    /*default portrait mode RGB*/
+        .yres                   = LCD_Y_RES,
+        .xres_virtual           = LCD_X_RES,
+        .yres_virtual           = LCD_Y_RES,
+        .height                 = -1,
+        .width                  = -1,
+        .left_margin            = 0,
+        .right_margin           = 0,
+        .upper_margin           = 0,
+        .lower_margin           = 0,
+        .red                    = {11, 5, 0},
+        .green                  = {5, 6, 0},
+        .blue                   = {0, 5, 0},
+        .transp         = {0, 0, 0},
+};
+
+static struct fb_fix_screeninfo bfin_lq035_fb_fix __devinitdata = {
+        .id             = KBUILD_MODNAME,
+        .smem_len       = ACTIVE_VIDEO_MEM_SIZE,
+        .type           = FB_TYPE_PACKED_PIXELS,
+        .visual         = FB_VISUAL_TRUECOLOR,
+        .xpanstep       = 0,
+        .ypanstep       = 0,
+        .line_length    = LCD_X_RES*(LCD_BBP/8),
+        .accel          = FB_ACCEL_NONE,
+};
+
+
+static int bfin_lq035_fb_open(struct fb_info *info, int user)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&bfin_lq035_lock, flags);
+        lq035_open_cnt++;
+        spin_unlock_irqrestore(&bfin_lq035_lock, flags);
+
+        if (lq035_open_cnt <= 1) {
+                bfin_write_PPI_CONTROL(0);
+                SSYNC();
+
+                set_vcomm();
+                config_dma();
+                config_ppi();
+
+                /* start dma */
+                enable_dma(CH_PPI);
+                SSYNC();
+                bfin_write_PPI_CONTROL(bfin_read_PPI_CONTROL() | PORT_EN);
+                SSYNC();
+
+                if (!t_conf_done) {
+                        config_timers();
+                        start_timers();
+                }
+                /* gpio_set_value(MOD,1); */
+        }
+
+        return 0;
+}
+
+static int bfin_lq035_fb_release(struct fb_info *info, int user)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&bfin_lq035_lock, flags);
+        lq035_open_cnt--;
+        spin_unlock_irqrestore(&bfin_lq035_lock, flags);
+
+
+        if (lq035_open_cnt <= 0) {
+
+                bfin_write_PPI_CONTROL(0);
+                SSYNC();
+
+                disable_dma(CH_PPI);
+        }
+
+        return 0;
+}
+
+
+static int bfin_lq035_fb_check_var(struct fb_var_screeninfo *var,
+                                   struct fb_info *info)
+{
+        switch (var->bits_per_pixel) {
+        case 16:/* DIRECTCOLOUR, 64k */
+                var->red.offset = info->var.red.offset;
+                var->green.offset = info->var.green.offset;
+                var->blue.offset = info->var.blue.offset;
+                var->red.length = info->var.red.length;
+                var->green.length = info->var.green.length;
+                var->blue.length = info->var.blue.length;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                var->transp.msb_right = 0;
+                var->red.msb_right = 0;
+                var->green.msb_right = 0;
+                var->blue.msb_right = 0;
+                break;
+        default:
+                pr_debug("%s: depth not supported: %u BPP\n", __func__,
+                         var->bits_per_pixel);
+                return -EINVAL;
+        }
+
+        if (info->var.xres != var->xres ||
+            info->var.yres != var->yres ||
+            info->var.xres_virtual != var->xres_virtual ||
+            info->var.yres_virtual != var->yres_virtual) {
+                pr_debug("%s: Resolution not supported: X%u x Y%u\n",
+                         __func__, var->xres, var->yres);
+                return -EINVAL;
+        }
+
+        /*
+         *  Memory limit
+         */
+
+        if ((info->fix.line_length * var->yres_virtual) > info->fix.smem_len) {
+                pr_debug("%s: Memory Limit requested yres_virtual = %u\n",
+                         __func__, var->yres_virtual);
+                return -ENOMEM;
+        }
+
+        return 0;
+}
+
+/* fb_rotate
+ * Rotate the display of this angle. This doesn't seems to be used by the core,
+ * but as our hardware supports it, so why not implementing it...
+ */
+static void bfin_lq035_fb_rotate(struct fb_info *fbi, int angle)
+{
+        pr_debug("%s: %p %d", __func__, fbi, angle);
+#if (defined(UD) && defined(LBR))
+        switch (angle) {
+
+        case 180:
+                gpio_set_value(LBR, 0);
+                gpio_set_value(UD, 1);
+                break;
+        default:
+                gpio_set_value(LBR, 1);
+                gpio_set_value(UD, 0);
+                break;
+        }
+#endif
+}
+
+static int bfin_lq035_fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
+{
+        if (nocursor)
+                return 0;
+        else
+                return -EINVAL; /* just to force soft_cursor() call */
+}
+
+static int bfin_lq035_fb_setcolreg(u_int regno, u_int red, u_int green,
+                                   u_int blue, u_int transp,
+                                   struct fb_info *info)
+{
+        if (regno >= NBR_PALETTE)
+                return -EINVAL;
+
+        if (info->var.grayscale)
+                /* grayscale = 0.30*R + 0.59*G + 0.11*B */
+                red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
+
+        if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+
+                u32 value;
+                /* Place color in the pseudopalette */
+                if (regno > 16)
+                        return -EINVAL;
+
+                red   >>= (16 - info->var.red.length);
+                green >>= (16 - info->var.green.length);
+                blue  >>= (16 - info->var.blue.length);
+
+                value = (red   << info->var.red.offset) |
+                        (green << info->var.green.offset)|
+                        (blue  << info->var.blue.offset);
+                value &= 0xFFFF;
+
+                ((u32 *) (info->pseudo_palette))[regno] = value;
+
+        }
+
+        return 0;
+}
+
+static struct fb_ops bfin_lq035_fb_ops = {
+        .owner                  = THIS_MODULE,
+        .fb_open                = bfin_lq035_fb_open,
+        .fb_release             = bfin_lq035_fb_release,
+        .fb_check_var           = bfin_lq035_fb_check_var,
+        .fb_rotate              = bfin_lq035_fb_rotate,
+        .fb_fillrect            = cfb_fillrect,
+        .fb_copyarea            = cfb_copyarea,
+        .fb_imageblit           = cfb_imageblit,
+        .fb_cursor              = bfin_lq035_fb_cursor,
+        .fb_setcolreg           = bfin_lq035_fb_setcolreg,
+};
+
+static int bl_get_brightness(struct backlight_device *bd)
+{
+        return current_brightness;
+}
+
+static const struct backlight_ops bfin_lq035fb_bl_ops = {
+        .get_brightness = bl_get_brightness,
+};
+
+static struct backlight_device *bl_dev;
+
+static int bfin_lcd_get_power(struct lcd_device *dev)
+{
+        return 0;
+}
+
+static int bfin_lcd_set_power(struct lcd_device *dev, int power)
+{
+        return 0;
+}
+
+static int bfin_lcd_get_contrast(struct lcd_device *dev)
+{
+        return (int)vcomm_value;
+}
+
+static int bfin_lcd_set_contrast(struct lcd_device *dev, int contrast)
+{
+        if (contrast > 255)
+                contrast = 255;
+        if (contrast < 0)
+                contrast = 0;
+
+        vcomm_value = (unsigned char)contrast;
+        set_vcomm();
+        return 0;
+}
+
+static int bfin_lcd_check_fb(struct lcd_device *lcd, struct fb_info *fi)
+{
+        if (!fi || (fi == &bfin_lq035_fb))
+                return 1;
+        return 0;
+}
+
+static struct lcd_ops bfin_lcd_ops = {
+        .get_power      = bfin_lcd_get_power,
+        .set_power      = bfin_lcd_set_power,
+        .get_contrast   = bfin_lcd_get_contrast,
+        .set_contrast   = bfin_lcd_set_contrast,
+        .check_fb       = bfin_lcd_check_fb,
+};
+
+static struct lcd_device *lcd_dev;
+
+static int __devinit bfin_lq035_probe(struct platform_device *pdev)
+{
+        struct backlight_properties props;
+        dma_addr_t dma_handle;
+        int ret;
+
+        if (request_dma(CH_PPI, KBUILD_MODNAME)) {
+                pr_err("couldn't request PPI DMA\n");
+                return -EFAULT;
+        }
+
+        if (request_ports()) {
+                pr_err("couldn't request gpio port\n");
+                ret = -EFAULT;
+                goto out_ports;
+        }
+
+        fb_buffer = dma_alloc_coherent(NULL, TOTAL_VIDEO_MEM_SIZE,
+                                       &dma_handle, GFP_KERNEL);
+        if (fb_buffer == NULL) {
+                pr_err("couldn't allocate dma buffer\n");
+                ret = -ENOMEM;
+                goto out_dma_coherent;
+        }
+
+        if (L1_DATA_A_LENGTH)
+                dma_desc_table = l1_data_sram_zalloc(TOTAL_DMA_DESC_SIZE);
+        else
+                dma_desc_table = dma_alloc_coherent(NULL, TOTAL_DMA_DESC_SIZE,
+                                                    &dma_handle, 0);
+
+        if (dma_desc_table == NULL) {
+                pr_err("couldn't allocate dma descriptor\n");
+                ret = -ENOMEM;
+                goto out_table;
+        }
+
+        bfin_lq035_fb.screen_base = (void *)fb_buffer;
+        bfin_lq035_fb_fix.smem_start = (int)fb_buffer;
+        if (landscape) {
+                bfin_lq035_fb_defined.xres = LCD_Y_RES;
+                bfin_lq035_fb_defined.yres = LCD_X_RES;
+                bfin_lq035_fb_defined.xres_virtual = LCD_Y_RES;
+                bfin_lq035_fb_defined.yres_virtual = LCD_X_RES;
+
+                bfin_lq035_fb_fix.line_length = LCD_Y_RES*(LCD_BBP/8);
+        } else {
+                bfin_lq035_fb.screen_base += ACTIVE_VIDEO_MEM_OFFSET;
+                bfin_lq035_fb_fix.smem_start += ACTIVE_VIDEO_MEM_OFFSET;
+        }
+
+        bfin_lq035_fb_defined.green.msb_right = 0;
+        bfin_lq035_fb_defined.red.msb_right   = 0;
+        bfin_lq035_fb_defined.blue.msb_right  = 0;
+        bfin_lq035_fb_defined.green.offset    = 5;
+        bfin_lq035_fb_defined.green.length    = 6;
+        bfin_lq035_fb_defined.red.length      = 5;
+        bfin_lq035_fb_defined.blue.length     = 5;
+
+        if (bgr) {
+                bfin_lq035_fb_defined.red.offset  = 0;
+                bfin_lq035_fb_defined.blue.offset = 11;
+        } else {
+                bfin_lq035_fb_defined.red.offset  = 11;
+                bfin_lq035_fb_defined.blue.offset = 0;
+        }
+
+        bfin_lq035_fb.fbops = &bfin_lq035_fb_ops;
+        bfin_lq035_fb.var = bfin_lq035_fb_defined;
+
+        bfin_lq035_fb.fix = bfin_lq035_fb_fix;
+        bfin_lq035_fb.flags = FBINFO_DEFAULT;
+
+
+        bfin_lq035_fb.pseudo_palette = kzalloc(sizeof(u32) * 16, GFP_KERNEL);
+        if (bfin_lq035_fb.pseudo_palette == NULL) {
+                pr_err("failed to allocate pseudo_palette\n");
+                ret = -ENOMEM;
+                goto out_palette;
+        }
+
+        if (fb_alloc_cmap(&bfin_lq035_fb.cmap, NBR_PALETTE, 0) < 0) {
+                pr_err("failed to allocate colormap (%d entries)\n",
+                        NBR_PALETTE);
+                ret = -EFAULT;
+                goto out_cmap;
+        }
+
+        if (register_framebuffer(&bfin_lq035_fb) < 0) {
+                pr_err("unable to register framebuffer\n");
+                ret = -EINVAL;
+                goto out_reg;
+        }
+
+        i2c_add_driver(&ad5280_driver);
+
+        memset(&props, 0, sizeof(props));
+        props.type = BACKLIGHT_RAW;
+        props.max_brightness = MAX_BRIGHENESS;
+        bl_dev = backlight_device_register("bf537-bl", NULL, NULL,
+                                           &bfin_lq035fb_bl_ops, &props);
+
+        lcd_dev = lcd_device_register(KBUILD_MODNAME, &pdev->dev, NULL,
+                                      &bfin_lcd_ops);
+        if (IS_ERR(lcd_dev)) {
+                pr_err("unable to register lcd\n");
+                ret = PTR_ERR(lcd_dev);
+                goto out_lcd;
+        }
+        lcd_dev->props.max_contrast = 255,
+
+        pr_info("initialized");
+
+        return 0;
+out_lcd:
+        unregister_framebuffer(&bfin_lq035_fb);
+out_reg:
+        fb_dealloc_cmap(&bfin_lq035_fb.cmap);
+out_cmap:
+        kfree(bfin_lq035_fb.pseudo_palette);
+out_palette:
+out_table:
+        dma_free_coherent(NULL, TOTAL_VIDEO_MEM_SIZE, fb_buffer, 0);
+        fb_buffer = NULL;
+out_dma_coherent:
+        free_ports();
+out_ports:
+        free_dma(CH_PPI);
+        return ret;
+}
+
+static int __devexit bfin_lq035_remove(struct platform_device *pdev)
+{
+        if (fb_buffer != NULL)
+                dma_free_coherent(NULL, TOTAL_VIDEO_MEM_SIZE, fb_buffer, 0);
+
+        if (L1_DATA_A_LENGTH)
+                l1_data_sram_free(dma_desc_table);
+        else
+                dma_free_coherent(NULL, TOTAL_DMA_DESC_SIZE, NULL, 0);
+
+        bfin_write_TIMER_DISABLE(TIMEN_SP|TIMEN_SPS|TIMEN_PS_CLS|
+                                 TIMEN_LP|TIMEN_REV);
+        t_conf_done = 0;
+
+        free_dma(CH_PPI);
+
+
+        kfree(bfin_lq035_fb.pseudo_palette);
+        fb_dealloc_cmap(&bfin_lq035_fb.cmap);
+
+
+        lcd_device_unregister(lcd_dev);
+        backlight_device_unregister(bl_dev);
+
+        unregister_framebuffer(&bfin_lq035_fb);
+        i2c_del_driver(&ad5280_driver);
+
+        free_ports();
+
+        pr_info("unregistered LCD driver\n");
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int bfin_lq035_suspend(struct platform_device *pdev, pm_message_t state)
+{
+        if (lq035_open_cnt > 0) {
+                bfin_write_PPI_CONTROL(0);
+                SSYNC();
+                disable_dma(CH_PPI);
+        }
+
+        return 0;
+}
+
+static int bfin_lq035_resume(struct platform_device *pdev)
+{
+        if (lq035_open_cnt > 0) {
+                bfin_write_PPI_CONTROL(0);
+                SSYNC();
+
+                config_dma();
+                config_ppi();
+
+                enable_dma(CH_PPI);
+                bfin_write_PPI_CONTROL(bfin_read_PPI_CONTROL() | PORT_EN);
+                SSYNC();
+
+                config_timers();
+                start_timers();
+        } else {
+                t_conf_done = 0;
+        }
+
+        return 0;
+}
+#else
+# define bfin_lq035_suspend     NULL
+# define bfin_lq035_resume      NULL
+#endif
+
+static struct platform_driver bfin_lq035_driver = {
+        .probe = bfin_lq035_probe,
+        .remove = __devexit_p(bfin_lq035_remove),
+        .suspend = bfin_lq035_suspend,
+        .resume = bfin_lq035_resume,
+        .driver = {
+                .name = KBUILD_MODNAME,
+                .owner = THIS_MODULE,
+        },
+};
+
+static int __init bfin_lq035_driver_init(void)
+{
+        request_module("i2c-bfin-twi");
+        return platform_driver_register(&bfin_lq035_driver);
+}
+module_init(bfin_lq035_driver_init);
+
+static void __exit bfin_lq035_driver_cleanup(void)
+{
+        platform_driver_unregister(&bfin_lq035_driver);
+}
+module_exit(bfin_lq035_driver_cleanup);
+
+MODULE_DESCRIPTION("SHARP LQ035Q7DB03 TFT LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index b020ba7f1cf2..2464b910b590 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;
@@ -649,6 +649,7 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
         }
 #ifndef NO_BL_SUPPORT
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = 255;
         bl_dev = backlight_device_register("bf54x-bl", NULL, NULL,
                                            &bfin_lq043fb_bl_ops, &props);
diff --git a/drivers/video/bfin-lq035q1-fb.c b/drivers/video/bfin-lq035q1-fb.c
index c8e1f04941bd..23b6c4b62c78 100644
--- a/drivers/video/bfin-lq035q1-fb.c
+++ b/drivers/video/bfin-lq035q1-fb.c
@@ -154,8 +154,10 @@ static int __devinit lq035q1_spidev_probe(struct spi_device *spi)
 
         ret = lq035q1_control(spi, LQ035_SHUT_CTL, LQ035_ON);
         ret |= lq035q1_control(spi, LQ035_DRIVER_OUTPUT_CTL, ctl->mode);
-        if (ret)
+        if (ret) {
+                kfree(ctl);
                 return ret;
+        }
 
         spi_set_drvdata(spi, ctl);
 
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 7a50272eaab9..d8de29f0dd8d 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
@@ -545,6 +545,7 @@ static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
         }
 #ifndef NO_BL_SUPPORT
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = 255;
         bl_dev = backlight_device_register("bf52x-bl", NULL, NULL,
                                            &bfin_lq043fb_bl_ops, &props);
diff --git a/drivers/video/bfin_adv7393fb.c b/drivers/video/bfin_adv7393fb.c
new file mode 100644
index 000000000000..8486f541156b
--- /dev/null
+++ b/drivers/video/bfin_adv7393fb.c
@@ -0,0 +1,832 @@
+/*
+ * Frame buffer driver for ADV7393/2 video encoder
+ *
+ * Copyright 2006-2009 Analog Devices Inc.
+ * Licensed under the GPL-2 or late.
+ */
+
+/*
+ * TODO: Remove Globals
+ * TODO: Code Cleanup
+ */
+
+#define pr_fmt(fmt) DRIVER_NAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <asm/blackfin.h>
+#include <asm/irq.h>
+#include <asm/dma.h>
+#include <linux/uaccess.h>
+#include <linux/gpio.h>
+#include <asm/portmux.h>
+
+#include <linux/dma-mapping.h>
+#include <linux/proc_fs.h>
+#include <linux/platform_device.h>
+
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+
+#include "bfin_adv7393fb.h"
+
+static int mode = VMODE;
+static int mem = VMEM;
+static int nocursor = 1;
+
+static const unsigned short ppi_pins[] = {
+        P_PPI0_CLK, P_PPI0_FS1, P_PPI0_FS2,
+        P_PPI0_D0, P_PPI0_D1, P_PPI0_D2, P_PPI0_D3,
+        P_PPI0_D4, P_PPI0_D5, P_PPI0_D6, P_PPI0_D7,
+        P_PPI0_D8, P_PPI0_D9, P_PPI0_D10, P_PPI0_D11,
+        P_PPI0_D12, P_PPI0_D13, P_PPI0_D14, P_PPI0_D15,
+        0
+};
+
+/*
+ * card parameters
+ */
+
+static struct bfin_adv7393_fb_par {
+        /* structure holding blackfin / adv7393 paramters when
+           screen is blanked */
+        struct {
+                u8 Mode;        /* ntsc/pal/? */
+        } vga_state;
+        atomic_t ref_count;
+} bfin_par;
+
+/* --------------------------------------------------------------------- */
+
+static struct fb_var_screeninfo bfin_adv7393_fb_defined = {
+        .xres = 720,
+        .yres = 480,
+        .xres_virtual = 720,
+        .yres_virtual = 480,
+        .bits_per_pixel = 16,
+        .activate = FB_ACTIVATE_TEST,
+        .height = -1,
+        .width = -1,
+        .left_margin = 0,
+        .right_margin = 0,
+        .upper_margin = 0,
+        .lower_margin = 0,
+        .vmode = FB_VMODE_INTERLACED,
+        .red = {11, 5, 0},
+        .green = {5, 6, 0},
+        .blue = {0, 5, 0},
+        .transp = {0, 0, 0},
+};
+
+static struct fb_fix_screeninfo bfin_adv7393_fb_fix __devinitdata = {
+        .id = "BFIN ADV7393",
+        .smem_len = 720 * 480 * 2,
+        .type = FB_TYPE_PACKED_PIXELS,
+        .visual = FB_VISUAL_TRUECOLOR,
+        .xpanstep = 0,
+        .ypanstep = 0,
+        .line_length = 720 * 2,
+        .accel = FB_ACCEL_NONE
+};
+
+static struct fb_ops bfin_adv7393_fb_ops = {
+        .owner = THIS_MODULE,
+        .fb_open = bfin_adv7393_fb_open,
+        .fb_release = bfin_adv7393_fb_release,
+        .fb_check_var = bfin_adv7393_fb_check_var,
+        .fb_pan_display = bfin_adv7393_fb_pan_display,
+        .fb_blank = bfin_adv7393_fb_blank,
+        .fb_fillrect = cfb_fillrect,
+        .fb_copyarea = cfb_copyarea,
+        .fb_imageblit = cfb_imageblit,
+        .fb_cursor = bfin_adv7393_fb_cursor,
+        .fb_setcolreg = bfin_adv7393_fb_setcolreg,
+};
+
+static int dma_desc_list(struct adv7393fb_device *fbdev, u16 arg)
+{
+        if (arg == BUILD) {     /* Build */
+                fbdev->vb1 = l1_data_sram_zalloc(sizeof(struct dmasg));
+                if (fbdev->vb1 == NULL)
+                        goto error;
+
+                fbdev->av1 = l1_data_sram_zalloc(sizeof(struct dmasg));
+                if (fbdev->av1 == NULL)
+                        goto error;
+
+                fbdev->vb2 = l1_data_sram_zalloc(sizeof(struct dmasg));
+                if (fbdev->vb2 == NULL)
+                        goto error;
+
+                fbdev->av2 = l1_data_sram_zalloc(sizeof(struct dmasg));
+                if (fbdev->av2 == NULL)
+                        goto error;
+
+                /* Build linked DMA descriptor list */
+                fbdev->vb1->next_desc_addr = fbdev->av1;
+                fbdev->av1->next_desc_addr = fbdev->vb2;
+                fbdev->vb2->next_desc_addr = fbdev->av2;
+                fbdev->av2->next_desc_addr = fbdev->vb1;
+
+                /* Save list head */
+                fbdev->descriptor_list_head = fbdev->av2;
+
+                /* Vertical Blanking Field 1 */
+                fbdev->vb1->start_addr = VB_DUMMY_MEMORY_SOURCE;
+                fbdev->vb1->cfg = DMA_CFG_VAL;
+
+                fbdev->vb1->x_count =
+                    fbdev->modes[mode].xres + fbdev->modes[mode].boeft_blank;
+
+                fbdev->vb1->x_modify = 0;
+                fbdev->vb1->y_count = fbdev->modes[mode].vb1_lines;
+                fbdev->vb1->y_modify = 0;
+
+                /* Active Video Field 1 */
+
+                fbdev->av1->start_addr = (unsigned long)fbdev->fb_mem;
+                fbdev->av1->cfg = DMA_CFG_VAL;
+                fbdev->av1->x_count =
+                    fbdev->modes[mode].xres + fbdev->modes[mode].boeft_blank;
+                fbdev->av1->x_modify = fbdev->modes[mode].bpp / 8;
+                fbdev->av1->y_count = fbdev->modes[mode].a_lines;
+                fbdev->av1->y_modify =
+                    (fbdev->modes[mode].xres - fbdev->modes[mode].boeft_blank +
+                     1) * (fbdev->modes[mode].bpp / 8);
+
+                /* Vertical Blanking Field 2 */
+
+                fbdev->vb2->start_addr = VB_DUMMY_MEMORY_SOURCE;
+                fbdev->vb2->cfg = DMA_CFG_VAL;
+                fbdev->vb2->x_count =
+                    fbdev->modes[mode].xres + fbdev->modes[mode].boeft_blank;
+
+                fbdev->vb2->x_modify = 0;
+                fbdev->vb2->y_count = fbdev->modes[mode].vb2_lines;
+                fbdev->vb2->y_modify = 0;
+
+                /* Active Video Field 2 */
+
+                fbdev->av2->start_addr =
+                    (unsigned long)fbdev->fb_mem + fbdev->line_len;
+
+                fbdev->av2->cfg = DMA_CFG_VAL;
+
+                fbdev->av2->x_count =
+                    fbdev->modes[mode].xres + fbdev->modes[mode].boeft_blank;
+
+                fbdev->av2->x_modify = (fbdev->modes[mode].bpp / 8);
+                fbdev->av2->y_count = fbdev->modes[mode].a_lines;
+
+                fbdev->av2->y_modify =
+                    (fbdev->modes[mode].xres - fbdev->modes[mode].boeft_blank +
+                     1) * (fbdev->modes[mode].bpp / 8);
+
+                return 1;
+        }
+
+error:
+        l1_data_sram_free(fbdev->vb1);
+        l1_data_sram_free(fbdev->av1);
+        l1_data_sram_free(fbdev->vb2);
+        l1_data_sram_free(fbdev->av2);
+
+        return 0;
+}
+
+static int bfin_config_dma(struct adv7393fb_device *fbdev)
+{
+        BUG_ON(!(fbdev->fb_mem));
+
+        set_dma_x_count(CH_PPI, fbdev->descriptor_list_head->x_count);
+        set_dma_x_modify(CH_PPI, fbdev->descriptor_list_head->x_modify);
+        set_dma_y_count(CH_PPI, fbdev->descriptor_list_head->y_count);
+        set_dma_y_modify(CH_PPI, fbdev->descriptor_list_head->y_modify);
+        set_dma_start_addr(CH_PPI, fbdev->descriptor_list_head->start_addr);
+        set_dma_next_desc_addr(CH_PPI,
+                               fbdev->descriptor_list_head->next_desc_addr);
+        set_dma_config(CH_PPI, fbdev->descriptor_list_head->cfg);
+
+        return 1;
+}
+
+static void bfin_disable_dma(void)
+{
+        bfin_write_DMA0_CONFIG(bfin_read_DMA0_CONFIG() & ~DMAEN);
+}
+
+static void bfin_config_ppi(struct adv7393fb_device *fbdev)
+{
+        if (ANOMALY_05000183) {
+                bfin_write_TIMER2_CONFIG(WDTH_CAP);
+                bfin_write_TIMER_ENABLE(TIMEN2);
+        }
+
+        bfin_write_PPI_CONTROL(0x381E);
+        bfin_write_PPI_FRAME(fbdev->modes[mode].tot_lines);
+        bfin_write_PPI_COUNT(fbdev->modes[mode].xres +
+                             fbdev->modes[mode].boeft_blank - 1);
+        bfin_write_PPI_DELAY(fbdev->modes[mode].aoeft_blank - 1);
+}
+
+static void bfin_enable_ppi(void)
+{
+        bfin_write_PPI_CONTROL(bfin_read_PPI_CONTROL() | PORT_EN);
+}
+
+static void bfin_disable_ppi(void)
+{
+        bfin_write_PPI_CONTROL(bfin_read_PPI_CONTROL() & ~PORT_EN);
+}
+
+static inline int adv7393_write(struct i2c_client *client, u8 reg, u8 value)
+{
+        return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int adv7393_read(struct i2c_client *client, u8 reg)
+{
+        return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int
+adv7393_write_block(struct i2c_client *client,
+                    const u8 *data, unsigned int len)
+{
+        int ret = -1;
+        u8 reg;
+
+        while (len >= 2) {
+                reg = *data++;
+                ret = adv7393_write(client, reg, *data++);
+                if (ret < 0)
+                        break;
+                len -= 2;
+        }
+
+        return ret;
+}
+
+static int adv7393_mode(struct i2c_client *client, u16 mode)
+{
+        switch (mode) {
+        case POWER_ON:          /* ADV7393 Sleep mode OFF */
+                adv7393_write(client, 0x00, 0x1E);
+                break;
+        case POWER_DOWN:        /* ADV7393 Sleep mode ON */
+                adv7393_write(client, 0x00, 0x1F);
+                break;
+        case BLANK_OFF:         /* Pixel Data Valid */
+                adv7393_write(client, 0x82, 0xCB);
+                break;
+        case BLANK_ON:          /* Pixel Data Invalid */
+                adv7393_write(client, 0x82, 0x8B);
+                break;
+        default:
+                return -EINVAL;
+                break;
+        }
+        return 0;
+}
+
+static irqreturn_t ppi_irq_error(int irq, void *dev_id)
+{
+
+        struct adv7393fb_device *fbdev = (struct adv7393fb_device *)dev_id;
+
+        u16 status = bfin_read_PPI_STATUS();
+
+        pr_debug("%s: PPI Status = 0x%X\n", __func__, status);
+
+        if (status) {
+                bfin_disable_dma();     /* TODO: Check Sequence */
+                bfin_disable_ppi();
+                bfin_clear_PPI_STATUS();
+                bfin_config_dma(fbdev);
+                bfin_enable_ppi();
+        }
+
+        return IRQ_HANDLED;
+
+}
+
+static int proc_output(char *buf)
+{
+        char *p = buf;
+
+        p += sprintf(p,
+                "Usage:\n"
+                "echo 0x[REG][Value] > adv7393\n"
+                "example: echo 0x1234 >adv7393\n"
+                "writes 0x34 into Register 0x12\n");
+
+        return p - buf;
+}
+
+static int
+adv7393_read_proc(char *page, char **start, off_t off,
+                  int count, int *eof, void *data)
+{
+        int len;
+
+        len = proc_output(page);
+        if (len <= off + count)
+                *eof = 1;
+        *start = page + off;
+        len -= off;
+        if (len > count)
+                len = count;
+        if (len < 0)
+                len = 0;
+        return len;
+}
+
+static int
+adv7393_write_proc(struct file *file, const char __user * buffer,
+                   unsigned long count, void *data)
+{
+        struct adv7393fb_device *fbdev = data;
+        char line[8];
+        unsigned int val;
+        int ret;
+
+        ret = copy_from_user(line, buffer, count);
+        if (ret)
+                return -EFAULT;
+
+        val = simple_strtoul(line, NULL, 0);
+        adv7393_write(fbdev->client, val >> 8, val & 0xff);
+
+        return count;
+}
+
+static int __devinit bfin_adv7393_fb_probe(struct i2c_client *client,
+                                           const struct i2c_device_id *id)
+{
+        int ret = 0;
+        struct proc_dir_entry *entry;
+        int num_modes = ARRAY_SIZE(known_modes);
+
+        struct adv7393fb_device *fbdev = NULL;
+
+        if (mem > 2) {
+                dev_err(&client->dev, "mem out of allowed range [1;2]\n");
+                return -EINVAL;
+        }
+
+        if (mode > num_modes) {
+                dev_err(&client->dev, "mode %d: not supported", mode);
+                return -EFAULT;
+        }
+
+        fbdev = kzalloc(sizeof(*fbdev), GFP_KERNEL);
+        if (!fbdev) {
+                dev_err(&client->dev, "failed to allocate device private record");
+                return -ENOMEM;
+        }
+
+        i2c_set_clientdata(client, fbdev);
+
+        fbdev->modes = known_modes;
+        fbdev->client = client;
+
+        fbdev->fb_len =
+            mem * fbdev->modes[mode].xres * fbdev->modes[mode].xres *
+            (fbdev->modes[mode].bpp / 8);
+
+        fbdev->line_len =
+            fbdev->modes[mode].xres * (fbdev->modes[mode].bpp / 8);
+
+        /* Workaround "PPI Does Not Start Properly In Specific Mode" */
+        if (ANOMALY_05000400) {
+                if (gpio_request(P_IDENT(P_PPI0_FS3), "PPI0_FS3")) {
+                        dev_err(&client->dev, "PPI0_FS3 GPIO request failed\n");
+                        ret = -EBUSY;
+                        goto out_8;
+                }
+                gpio_direction_output(P_IDENT(P_PPI0_FS3), 0);
+        }
+
+        if (peripheral_request_list(ppi_pins, DRIVER_NAME)) {
+                dev_err(&client->dev, "requesting PPI peripheral failed\n");
+                ret = -EFAULT;
+                goto out_8;
+        }
+
+        fbdev->fb_mem =
+            dma_alloc_coherent(NULL, fbdev->fb_len, &fbdev->dma_handle,
+                               GFP_KERNEL);
+
+        if (NULL == fbdev->fb_mem) {
+                dev_err(&client->dev, "couldn't allocate dma buffer (%d bytes)\n",
+                       (u32) fbdev->fb_len);
+                ret = -ENOMEM;
+                goto out_7;
+        }
+
+        fbdev->info.screen_base = (void *)fbdev->fb_mem;
+        bfin_adv7393_fb_fix.smem_start = (int)fbdev->fb_mem;
+
+        bfin_adv7393_fb_fix.smem_len = fbdev->fb_len;
+        bfin_adv7393_fb_fix.line_length = fbdev->line_len;
+
+        if (mem > 1)
+                bfin_adv7393_fb_fix.ypanstep = 1;
+
+        bfin_adv7393_fb_defined.red.length = 5;
+        bfin_adv7393_fb_defined.green.length = 6;
+        bfin_adv7393_fb_defined.blue.length = 5;
+
+        bfin_adv7393_fb_defined.xres = fbdev->modes[mode].xres;
+        bfin_adv7393_fb_defined.yres = fbdev->modes[mode].yres;
+        bfin_adv7393_fb_defined.xres_virtual = fbdev->modes[mode].xres;
+        bfin_adv7393_fb_defined.yres_virtual = mem * fbdev->modes[mode].yres;
+        bfin_adv7393_fb_defined.bits_per_pixel = fbdev->modes[mode].bpp;
+
+        fbdev->info.fbops = &bfin_adv7393_fb_ops;
+        fbdev->info.var = bfin_adv7393_fb_defined;
+        fbdev->info.fix = bfin_adv7393_fb_fix;
+        fbdev->info.par = &bfin_par;
+        fbdev->info.flags = FBINFO_DEFAULT;
+
+        fbdev->info.pseudo_palette = kzalloc(sizeof(u32) * 16, GFP_KERNEL);
+        if (!fbdev->info.pseudo_palette) {
+                dev_err(&client->dev, "failed to allocate pseudo_palette\n");
+                ret = -ENOMEM;
+                goto out_6;
+        }
+
+        if (fb_alloc_cmap(&fbdev->info.cmap, BFIN_LCD_NBR_PALETTE_ENTRIES, 0) < 0) {
+                dev_err(&client->dev, "failed to allocate colormap (%d entries)\n",
+                           BFIN_LCD_NBR_PALETTE_ENTRIES);
+                ret = -EFAULT;
+                goto out_5;
+        }
+
+        if (request_dma(CH_PPI, "BF5xx_PPI_DMA") < 0) {
+                dev_err(&client->dev, "unable to request PPI DMA\n");
+                ret = -EFAULT;
+                goto out_4;
+        }
+
+        if (request_irq(IRQ_PPI_ERROR, ppi_irq_error, IRQF_DISABLED,
+                        "PPI ERROR", fbdev) < 0) {
+                dev_err(&client->dev, "unable to request PPI ERROR IRQ\n");
+                ret = -EFAULT;
+                goto out_3;
+        }
+
+        fbdev->open = 0;
+
+        ret = adv7393_write_block(client, fbdev->modes[mode].adv7393_i2c_initd,
+                                fbdev->modes[mode].adv7393_i2c_initd_len);
+
+        if (ret) {
+                dev_err(&client->dev, "i2c attach: init error\n");
+                goto out_1;
+        }
+
+
+        if (register_framebuffer(&fbdev->info) < 0) {
+                dev_err(&client->dev, "unable to register framebuffer\n");
+                ret = -EFAULT;
+                goto out_1;
+        }
+
+        dev_info(&client->dev, "fb%d: %s frame buffer device\n",
+               fbdev->info.node, fbdev->info.fix.id);
+        dev_info(&client->dev, "fb memory address : 0x%p\n", fbdev->fb_mem);
+
+        entry = create_proc_entry("driver/adv7393", 0, NULL);
+        if (!entry) {
+                dev_err(&client->dev, "unable to create /proc entry\n");
+                ret = -EFAULT;
+                goto out_0;
+        }
+
+        entry->read_proc = adv7393_read_proc;
+        entry->write_proc = adv7393_write_proc;
+        entry->data = fbdev;
+
+        return 0;
+
+ out_0:
+        unregister_framebuffer(&fbdev->info);
+ out_1:
+        free_irq(IRQ_PPI_ERROR, fbdev);
+ out_3:
+        free_dma(CH_PPI);
+ out_4:
+        dma_free_coherent(NULL, fbdev->fb_len, fbdev->fb_mem,
+                          fbdev->dma_handle);
+ out_5:
+        fb_dealloc_cmap(&fbdev->info.cmap);
+ out_6:
+        kfree(fbdev->info.pseudo_palette);
+ out_7:
+        peripheral_free_list(ppi_pins);
+ out_8:
+        kfree(fbdev);
+
+        return ret;
+}
+
+static int bfin_adv7393_fb_open(struct fb_info *info, int user)
+{
+        struct adv7393fb_device *fbdev = to_adv7393fb_device(info);
+
+        fbdev->info.screen_base = (void *)fbdev->fb_mem;
+        if (!fbdev->info.screen_base) {
+                dev_err(&fbdev->client->dev, "unable to map device\n");
+                return -ENOMEM;
+        }
+
+        fbdev->open = 1;
+        dma_desc_list(fbdev, BUILD);
+        adv7393_mode(fbdev->client, BLANK_OFF);
+        bfin_config_ppi(fbdev);
+        bfin_config_dma(fbdev);
+        bfin_enable_ppi();
+
+        return 0;
+}
+
+static int bfin_adv7393_fb_release(struct fb_info *info, int user)
+{
+        struct adv7393fb_device *fbdev = to_adv7393fb_device(info);
+
+        adv7393_mode(fbdev->client, BLANK_ON);
+        bfin_disable_dma();
+        bfin_disable_ppi();
+        dma_desc_list(fbdev, DESTRUCT);
+        fbdev->open = 0;
+        return 0;
+}
+
+static int
+bfin_adv7393_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+
+        switch (var->bits_per_pixel) {
+        case 16:/* DIRECTCOLOUR, 64k */
+                var->red.offset = info->var.red.offset;
+                var->green.offset = info->var.green.offset;
+                var->blue.offset = info->var.blue.offset;
+                var->red.length = info->var.red.length;
+                var->green.length = info->var.green.length;
+                var->blue.length = info->var.blue.length;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                var->transp.msb_right = 0;
+                var->red.msb_right = 0;
+                var->green.msb_right = 0;
+                var->blue.msb_right = 0;
+                break;
+        default:
+                pr_debug("%s: depth not supported: %u BPP\n", __func__,
+                         var->bits_per_pixel);
+                return -EINVAL;
+        }
+
+        if (info->var.xres != var->xres ||
+            info->var.yres != var->yres ||
+            info->var.xres_virtual != var->xres_virtual ||
+            info->var.yres_virtual != var->yres_virtual) {
+                pr_debug("%s: Resolution not supported: X%u x Y%u\n",
+                         __func__, var->xres, var->yres);
+                return -EINVAL;
+        }
+
+        /*
+         *  Memory limit
+         */
+
+        if ((info->fix.line_length * var->yres_virtual) > info->fix.smem_len) {
+                pr_debug("%s: Memory Limit requested yres_virtual = %u\n",
+                         __func__, var->yres_virtual);
+                return -ENOMEM;
+        }
+
+        return 0;
+}
+
+static int
+bfin_adv7393_fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+        int dy;
+        u32 dmaaddr;
+        struct adv7393fb_device *fbdev = to_adv7393fb_device(info);
+
+        if (!var || !info)
+                return -EINVAL;
+
+        if (var->xoffset - info->var.xoffset) {
+                /* No support for X panning for now! */
+                return -EINVAL;
+        }
+        dy = var->yoffset - info->var.yoffset;
+
+        if (dy) {
+                pr_debug("%s: Panning screen of %d lines\n", __func__, dy);
+
+                dmaaddr = fbdev->av1->start_addr;
+                dmaaddr += (info->fix.line_length * dy);
+                /* TODO: Wait for current frame to finished */
+
+                fbdev->av1->start_addr = (unsigned long)dmaaddr;
+                fbdev->av2->start_addr = (unsigned long)dmaaddr + fbdev->line_len;
+        }
+
+        return 0;
+
+}
+
+/* 0 unblank, 1 blank, 2 no vsync, 3 no hsync, 4 off */
+static int bfin_adv7393_fb_blank(int blank, struct fb_info *info)
+{
+        struct adv7393fb_device *fbdev = to_adv7393fb_device(info);
+
+        switch (blank) {
+
+        case VESA_NO_BLANKING:
+                /* Turn on panel */
+                adv7393_mode(fbdev->client, BLANK_OFF);
+                break;
+
+        case VESA_VSYNC_SUSPEND:
+        case VESA_HSYNC_SUSPEND:
+        case VESA_POWERDOWN:
+                /* Turn off panel */
+                adv7393_mode(fbdev->client, BLANK_ON);
+                break;
+
+        default:
+                return -EINVAL;
+                break;
+        }
+        return 0;
+}
+
+int bfin_adv7393_fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
+{
+        if (nocursor)
+                return 0;
+        else
+                return -EINVAL; /* just to force soft_cursor() call */
+}
+
+static int bfin_adv7393_fb_setcolreg(u_int regno, u_int red, u_int green,
+                                     u_int blue, u_int transp,
+                                     struct fb_info *info)
+{
+        if (regno >= BFIN_LCD_NBR_PALETTE_ENTRIES)
+                return -EINVAL;
+
+        if (info->var.grayscale)
+                /* grayscale = 0.30*R + 0.59*G + 0.11*B */
+                red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
+
+        if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+                u32 value;
+                /* Place color in the pseudopalette */
+                if (regno > 16)
+                        return -EINVAL;
+
+                red   >>= (16 - info->var.red.length);
+                green >>= (16 - info->var.green.length);
+                blue  >>= (16 - info->var.blue.length);
+
+                value = (red   << info->var.red.offset) |
+                        (green << info->var.green.offset)|
+                        (blue  << info->var.blue.offset);
+                value &= 0xFFFF;
+
+                ((u32 *) (info->pseudo_palette))[regno] = value;
+        }
+
+        return 0;
+}
+
+static int __devexit bfin_adv7393_fb_remove(struct i2c_client *client)
+{
+        struct adv7393fb_device *fbdev = i2c_get_clientdata(client);
+
+        adv7393_mode(client, POWER_DOWN);
+
+        if (fbdev->fb_mem)
+                dma_free_coherent(NULL, fbdev->fb_len, fbdev->fb_mem, fbdev->dma_handle);
+        free_dma(CH_PPI);
+        free_irq(IRQ_PPI_ERROR, fbdev);
+        unregister_framebuffer(&fbdev->info);
+        remove_proc_entry("driver/adv7393", NULL);
+        fb_dealloc_cmap(&fbdev->info.cmap);
+        kfree(fbdev->info.pseudo_palette);
+
+        if (ANOMALY_05000400)
+                gpio_free(P_IDENT(P_PPI0_FS3)); /* FS3 */
+        peripheral_free_list(ppi_pins);
+        kfree(fbdev);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int bfin_adv7393_fb_suspend(struct device *dev)
+{
+        struct adv7393fb_device *fbdev = dev_get_drvdata(dev);
+
+        if (fbdev->open) {
+                bfin_disable_dma();
+                bfin_disable_ppi();
+                dma_desc_list(fbdev, DESTRUCT);
+        }
+        adv7393_mode(fbdev->client, POWER_DOWN);
+
+        return 0;
+}
+
+static int bfin_adv7393_fb_resume(struct device *dev)
+{
+        struct adv7393fb_device *fbdev = dev_get_drvdata(dev);
+
+        adv7393_mode(fbdev->client, POWER_ON);
+
+        if (fbdev->open) {
+                dma_desc_list(fbdev, BUILD);
+                bfin_config_ppi(fbdev);
+                bfin_config_dma(fbdev);
+                bfin_enable_ppi();
+        }
+
+        return 0;
+}
+
+static const struct dev_pm_ops bfin_adv7393_dev_pm_ops = {
+        .suspend = bfin_adv7393_fb_suspend,
+        .resume  = bfin_adv7393_fb_resume,
+};
+#endif
+
+static const struct i2c_device_id bfin_adv7393_id[] = {
+        {DRIVER_NAME, 0},
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, bfin_adv7393_id);
+
+static struct i2c_driver bfin_adv7393_fb_driver = {
+        .driver = {
+                .name = DRIVER_NAME,
+#ifdef CONFIG_PM
+                .pm   = &bfin_adv7393_dev_pm_ops,
+#endif
+        },
+        .probe = bfin_adv7393_fb_probe,
+        .remove = __devexit_p(bfin_adv7393_fb_remove),
+        .id_table = bfin_adv7393_id,
+};
+
+static int __init bfin_adv7393_fb_driver_init(void)
+{
+#if  defined(CONFIG_I2C_BLACKFIN_TWI) || defined(CONFIG_I2C_BLACKFIN_TWI_MODULE)
+        request_module("i2c-bfin-twi");
+#else
+        request_module("i2c-gpio");
+#endif
+
+        return i2c_add_driver(&bfin_adv7393_fb_driver);
+}
+module_init(bfin_adv7393_fb_driver_init);
+
+static void __exit bfin_adv7393_fb_driver_cleanup(void)
+{
+        i2c_del_driver(&bfin_adv7393_fb_driver);
+}
+module_exit(bfin_adv7393_fb_driver_cleanup);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("Frame buffer driver for ADV7393/2 Video Encoder");
+
+module_param(mode, int, 0);
+MODULE_PARM_DESC(mode,
+        "Video Mode (0=NTSC,1=PAL,2=NTSC 640x480,3=PAL 640x480,4=NTSC YCbCr input,5=PAL YCbCr input)");
+
+module_param(mem, int, 0);
+MODULE_PARM_DESC(mem,
+        "Size of frame buffer memory 1=Single 2=Double Size (allows y-panning / frame stacking)");
+
+module_param(nocursor, int, 0644);
+MODULE_PARM_DESC(nocursor, "cursor enable/disable");
diff --git a/drivers/video/bfin_adv7393fb.h b/drivers/video/bfin_adv7393fb.h
new file mode 100644
index 000000000000..cd591b5152a5
--- /dev/null
+++ b/drivers/video/bfin_adv7393fb.h
@@ -0,0 +1,321 @@
+/*
+ * Frame buffer driver for ADV7393/2 video encoder
+ *
+ * Copyright 2006-2009 Analog Devices Inc.
+ * Licensed under the GPL-2 or late.
+ */
+
+#ifndef __BFIN_ADV7393FB_H__
+#define __BFIN_ADV7393FB_H__
+
+#define BFIN_LCD_NBR_PALETTE_ENTRIES    256
+
+#ifdef CONFIG_NTSC
+# define VMODE 0
+#endif
+#ifdef CONFIG_PAL
+# define VMODE 1
+#endif
+#ifdef CONFIG_NTSC_640x480
+# define VMODE 2
+#endif
+#ifdef CONFIG_PAL_640x480
+# define VMODE 3
+#endif
+#ifdef CONFIG_NTSC_YCBCR
+# define VMODE 4
+#endif
+#ifdef CONFIG_PAL_YCBCR
+# define VMODE 5
+#endif
+
+#ifndef VMODE
+# define VMODE 1
+#endif
+
+#ifdef CONFIG_ADV7393_2XMEM
+# define VMEM 2
+#else
+# define VMEM 1
+#endif
+
+#if defined(CONFIG_BF537) || defined(CONFIG_BF536) || defined(CONFIG_BF534)
+# define DMA_CFG_VAL    0x7935  /* Set Sync Bit */
+# define VB_DUMMY_MEMORY_SOURCE L1_DATA_B_START
+#else
+# define DMA_CFG_VAL    0x7915
+# define VB_DUMMY_MEMORY_SOURCE BOOT_ROM_START
+#endif
+
+enum {
+        DESTRUCT,
+        BUILD,
+};
+
+enum {
+        POWER_ON,
+        POWER_DOWN,
+        BLANK_ON,
+        BLANK_OFF,
+};
+
+#define DRIVER_NAME "bfin-adv7393"
+
+struct adv7393fb_modes {
+        const s8 name[25];      /* Full name */
+        u16 xres;               /* Active Horizonzal Pixels  */
+        u16 yres;               /* Active Vertical Pixels  */
+        u16 bpp;
+        u16 vmode;
+        u16 a_lines;            /* Active Lines per Field */
+        u16 vb1_lines;          /* Vertical Blanking Field 1 Lines */
+        u16 vb2_lines;          /* Vertical Blanking Field 2 Lines */
+        u16 tot_lines;          /* Total Lines per Frame */
+        u16 boeft_blank;        /* Before Odd/Even Field Transition No. of Blank Pixels */
+        u16 aoeft_blank;        /* After Odd/Even Field Transition No. of Blank Pixels */
+        const s8 *adv7393_i2c_initd;
+        u16 adv7393_i2c_initd_len;
+};
+
+static const u8 init_NTSC_TESTPATTERN[] = {
+        0x00, 0x1E,     /* Power up all DACs and PLL */
+        0x01, 0x00,     /* SD-Only Mode */
+        0x80, 0x10,     /* SSAF Luma Filter Enabled, NTSC Mode */
+        0x82, 0xCB,     /* Step control on, pixel data valid, pedestal on, PrPb SSAF on, CVBS/YC output */
+        0x84, 0x40,     /* SD Color Bar Test Pattern Enabled, DAC 2 = Luma, DAC 3 = Chroma */
+};
+
+static const u8 init_NTSC[] = {
+        0x00, 0x1E,     /* Power up all DACs and PLL */
+        0xC3, 0x26,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xC5, 0x12,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xC2, 0x4A,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xC6, 0x5E,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xBD, 0x19,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xBF, 0x42,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0x8C, 0x1F,     /* NTSC Subcarrier Frequency */
+        0x8D, 0x7C,     /* NTSC Subcarrier Frequency */
+        0x8E, 0xF0,     /* NTSC Subcarrier Frequency */
+        0x8F, 0x21,     /* NTSC Subcarrier Frequency */
+        0x01, 0x00,     /* SD-Only Mode */
+        0x80, 0x30,     /* SSAF Luma Filter Enabled, NTSC Mode */
+        0x82, 0x8B,     /* Step control on, pixel data invalid, pedestal on, PrPb SSAF on, CVBS/YC output */
+        0x87, 0x80,     /* SD Color Bar Test Pattern Enabled, DAC 2 = Luma, DAC 3 = Chroma */
+        0x86, 0x82,
+        0x8B, 0x11,
+        0x88, 0x20,
+        0x8A, 0x0d,
+};
+
+static const u8 init_PAL[] = {
+        0x00, 0x1E,     /* Power up all DACs and PLL */
+        0xC3, 0x26,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xC5, 0x12,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xC2, 0x4A,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xC6, 0x5E,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xBD, 0x19,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0xBF, 0x42,     /* Program RGB->YCrCb Color Space conversion matrix */
+        0x8C, 0xCB,     /* PAL Subcarrier Frequency */
+        0x8D, 0x8A,     /* PAL Subcarrier Frequency */
+        0x8E, 0x09,     /* PAL Subcarrier Frequency */
+        0x8F, 0x2A,     /* PAL Subcarrier Frequency */
+        0x01, 0x00,     /* SD-Only Mode */
+        0x80, 0x11,     /* SSAF Luma Filter Enabled, PAL Mode */
+        0x82, 0x8B,     /* Step control on, pixel data invalid, pedestal on, PrPb SSAF on, CVBS/YC output */
+        0x87, 0x80,     /* SD Color Bar Test Pattern Enabled, DAC 2 = Luma, DAC 3 = Chroma */
+        0x86, 0x82,
+        0x8B, 0x11,
+        0x88, 0x20,
+        0x8A, 0x0d,
+};
+
+static const u8 init_NTSC_YCbCr[] = {
+        0x00, 0x1E,     /* Power up all DACs and PLL */
+        0x8C, 0x1F,     /* NTSC Subcarrier Frequency */
+        0x8D, 0x7C,     /* NTSC Subcarrier Frequency */
+        0x8E, 0xF0,     /* NTSC Subcarrier Frequency */
+        0x8F, 0x21,     /* NTSC Subcarrier Frequency */
+        0x01, 0x00,     /* SD-Only Mode */
+        0x80, 0x30,     /* SSAF Luma Filter Enabled, NTSC Mode */
+        0x82, 0x8B,     /* Step control on, pixel data invalid, pedestal on, PrPb SSAF on, CVBS/YC output */
+        0x87, 0x00,     /* DAC 2 = Luma, DAC 3 = Chroma */
+        0x86, 0x82,
+        0x8B, 0x11,
+        0x88, 0x08,
+        0x8A, 0x0d,
+};
+
+static const u8 init_PAL_YCbCr[] = {
+        0x00, 0x1E,     /* Power up all DACs and PLL */
+        0x8C, 0xCB,     /* PAL Subcarrier Frequency */
+        0x8D, 0x8A,     /* PAL Subcarrier Frequency */
+        0x8E, 0x09,     /* PAL Subcarrier Frequency */
+        0x8F, 0x2A,     /* PAL Subcarrier Frequency */
+        0x01, 0x00,     /* SD-Only Mode */
+        0x80, 0x11,     /* SSAF Luma Filter Enabled, PAL Mode */
+        0x82, 0x8B,     /* Step control on, pixel data invalid, pedestal on, PrPb SSAF on, CVBS/YC output */
+        0x87, 0x00,     /* DAC 2 = Luma, DAC 3 = Chroma */
+        0x86, 0x82,
+        0x8B, 0x11,
+        0x88, 0x08,
+        0x8A, 0x0d,
+};
+
+static struct adv7393fb_modes known_modes[] = {
+        /* NTSC 720x480 CRT */
+        {
+                .name = "NTSC 720x480",
+                .xres = 720,
+                .yres = 480,
+                .bpp = 16,
+                .vmode = FB_VMODE_INTERLACED,
+                .a_lines = 240,
+                .vb1_lines = 22,
+                .vb2_lines = 23,
+                .tot_lines = 525,
+                .boeft_blank = 16,
+                .aoeft_blank = 122,
+                .adv7393_i2c_initd = init_NTSC,
+                .adv7393_i2c_initd_len = sizeof(init_NTSC)
+        },
+        /* PAL 720x480 CRT */
+        {
+                .name = "PAL 720x576",
+                .xres = 720,
+                .yres = 576,
+                .bpp = 16,
+                .vmode = FB_VMODE_INTERLACED,
+                .a_lines = 288,
+                .vb1_lines = 24,
+                .vb2_lines = 25,
+                .tot_lines = 625,
+                .boeft_blank = 12,
+                .aoeft_blank = 132,
+                .adv7393_i2c_initd = init_PAL,
+                .adv7393_i2c_initd_len = sizeof(init_PAL)
+        },
+        /* NTSC 640x480 CRT Experimental */
+        {
+                .name = "NTSC 640x480",
+                .xres = 640,
+                .yres = 480,
+                .bpp = 16,
+                .vmode = FB_VMODE_INTERLACED,
+                .a_lines = 240,
+                .vb1_lines = 22,
+                .vb2_lines = 23,
+                .tot_lines = 525,
+                .boeft_blank = 16 + 40,
+                .aoeft_blank = 122 + 40,
+                .adv7393_i2c_initd = init_NTSC,
+                .adv7393_i2c_initd_len = sizeof(init_NTSC)
+        },
+        /* PAL 640x480 CRT Experimental */
+        {
+                .name = "PAL 640x480",
+                .xres = 640,
+                .yres = 480,
+                .bpp = 16,
+                .vmode = FB_VMODE_INTERLACED,
+                .a_lines = 288 - 20,
+                .vb1_lines = 24 + 20,
+                .vb2_lines = 25 + 20,
+                .tot_lines = 625,
+                .boeft_blank = 12 + 40,
+                .aoeft_blank = 132 + 40,
+                .adv7393_i2c_initd = init_PAL,
+                .adv7393_i2c_initd_len = sizeof(init_PAL)
+        },
+        /* NTSC 720x480 YCbCR */
+        {
+                .name = "NTSC 720x480 YCbCR",
+                .xres = 720,
+                .yres = 480,
+                .bpp = 16,
+                .vmode = FB_VMODE_INTERLACED,
+                .a_lines = 240,
+                .vb1_lines = 22,
+                .vb2_lines = 23,
+                .tot_lines = 525,
+                .boeft_blank = 16,
+                .aoeft_blank = 122,
+                .adv7393_i2c_initd = init_NTSC_YCbCr,
+                .adv7393_i2c_initd_len = sizeof(init_NTSC_YCbCr)
+        },
+        /* PAL 720x480 CRT */
+        {
+                .name = "PAL 720x576 YCbCR",
+                .xres = 720,
+                .yres = 576,
+                .bpp = 16,
+                .vmode = FB_VMODE_INTERLACED,
+                .a_lines = 288,
+                .vb1_lines = 24,
+                .vb2_lines = 25,
+                .tot_lines = 625,
+                .boeft_blank = 12,
+                .aoeft_blank = 132,
+                .adv7393_i2c_initd = init_PAL_YCbCr,
+                .adv7393_i2c_initd_len = sizeof(init_PAL_YCbCr)
+        }
+};
+
+struct adv7393fb_regs {
+
+};
+
+struct adv7393fb_device {
+        struct fb_info info;    /* FB driver info record */
+
+        struct i2c_client *client;
+
+        struct dmasg *descriptor_list_head;
+        struct dmasg *vb1;
+        struct dmasg *av1;
+        struct dmasg *vb2;
+        struct dmasg *av2;
+
+        dma_addr_t dma_handle;
+
+        struct fb_info bfin_adv7393_fb;
+
+        struct adv7393fb_modes *modes;
+
+        struct adv7393fb_regs *regs;    /* Registers memory map */
+        size_t regs_len;
+        size_t fb_len;
+        size_t line_len;
+        u16 open;
+        u16 *fb_mem;            /* RGB Buffer */
+
+};
+
+#define to_adv7393fb_device(_info) \
+          (_info ? container_of(_info, struct adv7393fb_device, info) : NULL);
+
+static int bfin_adv7393_fb_open(struct fb_info *info, int user);
+static int bfin_adv7393_fb_release(struct fb_info *info, int user);
+static int bfin_adv7393_fb_check_var(struct fb_var_screeninfo *var,
+                                     struct fb_info *info);
+
+static int bfin_adv7393_fb_pan_display(struct fb_var_screeninfo *var,
+                                       struct fb_info *info);
+
+static int bfin_adv7393_fb_blank(int blank, struct fb_info *info);
+
+static void bfin_config_ppi(struct adv7393fb_device *fbdev);
+static int bfin_config_dma(struct adv7393fb_device *fbdev);
+static void bfin_disable_dma(void);
+static void bfin_enable_ppi(void);
+static void bfin_disable_ppi(void);
+
+static inline int adv7393_write(struct i2c_client *client, u8 reg, u8 value);
+static inline int adv7393_read(struct i2c_client *client, u8 reg);
+static int adv7393_write_block(struct i2c_client *client, const u8 *data,
+                               unsigned int len);
+
+int bfin_adv7393_fb_cursor(struct fb_info *info, struct fb_cursor *cursor);
+static int bfin_adv7393_fb_setcolreg(u_int, u_int, u_int, u_int,
+                                     u_int, struct fb_info *info);
+
+#endif
diff --git a/drivers/video/broadsheetfb.c b/drivers/video/broadsheetfb.c
index ebda6876d3a9..377dde3d5bfc 100644
--- a/drivers/video/broadsheetfb.c
+++ b/drivers/video/broadsheetfb.c
@@ -1101,12 +1101,10 @@ static int __devinit broadsheetfb_probe(struct platform_device *dev)
 
         videomemorysize = roundup((dpyw*dpyh), PAGE_SIZE);
 
-        videomemory = vmalloc(videomemorysize);
+        videomemory = vzalloc(videomemorysize);
         if (!videomemory)
                 goto err_fb_rel;
 
-        memset(videomemory, 0, videomemorysize);
-
         info->screen_base = (char *)videomemory;
         info->fbops = &broadsheetfb_ops;
 
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 4dc13467281d..7ba74cd4be61 100644
--- a/drivers/video/bw2.c
+++ b/drivers/video/bw2.c
@@ -273,7 +273,7 @@ static int __devinit bw2_do_default_mode(struct bw2_par *par,
         return 0;
 }
 
-static int __devinit bw2_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit bw2_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -375,7 +375,7 @@ static const struct of_device_id bw2_match[] = {
 };
 MODULE_DEVICE_TABLE(of, bw2_match);
 
-static struct of_platform_driver bw2_driver = {
+static struct platform_driver bw2_driver = {
         .driver = {
                 .name = "bw2",
                 .owner = THIS_MODULE,
@@ -390,12 +390,12 @@ static int __init bw2_init(void)
         if (fb_get_options("bw2fb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&bw2_driver);
+        return platform_driver_register(&bw2_driver);
 }
 
 static void __exit bw2_exit(void)
 {
-        of_unregister_platform_driver(&bw2_driver);
+        platform_driver_unregister(&bw2_driver);
 }
 
 module_init(bw2_init);
diff --git a/drivers/video/carminefb.c b/drivers/video/carminefb.c
index 6b19136aa181..caaa27d4a46a 100644
--- a/drivers/video/carminefb.c
+++ b/drivers/video/carminefb.c
@@ -654,7 +654,7 @@ static int __devinit carminefb_probe(struct pci_dev *dev,
                 printk(KERN_ERR "carminefb: Memory bar is only %d bytes, %d "
                                 "are required.", carminefb_fix.smem_len,
                                 CARMINE_TOTAL_DIPLAY_MEM);
-                goto err_free_reg_mmio;
+                goto err_unmap_vregs;
         }
 
         if (!request_mem_region(carminefb_fix.smem_start,
@@ -667,8 +667,6 @@ static int __devinit carminefb_probe(struct pci_dev *dev,
                         carminefb_fix.smem_len);
         if (!hw->screen_mem) {
                 printk(KERN_ERR "carmine: Can't ioremap smem area.\n");
-                release_mem_region(carminefb_fix.smem_start,
-                                carminefb_fix.smem_len);
                 goto err_reg_smem;
         }
 
@@ -710,7 +708,7 @@ err_deinit_hw:
 err_unmap_screen:
         iounmap(hw->screen_mem);
 err_reg_smem:
-        release_mem_region(carminefb_fix.mmio_start, carminefb_fix.mmio_len);
+        release_mem_region(carminefb_fix.smem_start, carminefb_fix.smem_len);
 err_unmap_vregs:
         iounmap(hw->v_regs);
 err_free_reg_mmio:
diff --git a/drivers/video/cg14.c b/drivers/video/cg14.c
index 24249535ac86..f18895006627 100644
--- a/drivers/video/cg14.c
+++ b/drivers/video/cg14.c
@@ -463,7 +463,7 @@ static void cg14_unmap_regs(struct platform_device *op, struct fb_info *info,
                            info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit cg14_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit cg14_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -565,6 +565,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
         cg14_unmap_regs(op, info, par);
+        framebuffer_release(info);
 
 out_err:
         return err;
@@ -595,7 +596,7 @@ static const struct of_device_id cg14_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg14_match);
 
-static struct of_platform_driver cg14_driver = {
+static struct platform_driver cg14_driver = {
         .driver = {
                 .name = "cg14",
                 .owner = THIS_MODULE,
@@ -610,12 +611,12 @@ static int __init cg14_init(void)
         if (fb_get_options("cg14fb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&cg14_driver);
+        return platform_driver_register(&cg14_driver);
 }
 
 static void __exit cg14_exit(void)
 {
-        of_unregister_platform_driver(&cg14_driver);
+        platform_driver_unregister(&cg14_driver);
 }
 
 module_init(cg14_init);
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index 09c0c3c42482..f927a7b1a8d4 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -346,8 +346,7 @@ static int __devinit cg3_do_default_mode(struct cg3_par *par)
         return 0;
 }
 
-static int __devinit cg3_probe(struct platform_device *op,
-                               const struct of_device_id *match)
+static int __devinit cg3_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -462,7 +461,7 @@ static const struct of_device_id cg3_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg3_match);
 
-static struct of_platform_driver cg3_driver = {
+static struct platform_driver cg3_driver = {
         .driver = {
                 .name = "cg3",
                 .owner = THIS_MODULE,
@@ -477,12 +476,12 @@ static int __init cg3_init(void)
         if (fb_get_options("cg3fb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&cg3_driver);
+        return platform_driver_register(&cg3_driver);
 }
 
 static void __exit cg3_exit(void)
 {
-        of_unregister_platform_driver(&cg3_driver);
+        platform_driver_unregister(&cg3_driver);
 }
 
 module_init(cg3_init);
diff --git a/drivers/video/cg6.c b/drivers/video/cg6.c
index 2b5a97058b08..179e96cdb323 100644
--- a/drivers/video/cg6.c
+++ b/drivers/video/cg6.c
@@ -737,8 +737,7 @@ static void cg6_unmap_regs(struct platform_device *op, struct fb_info *info,
                            info->fix.smem_len);
 }
 
-static int __devinit cg6_probe(struct platform_device *op,
-                                const struct of_device_id *match)
+static int __devinit cg6_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -822,6 +821,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
         cg6_unmap_regs(op, info, par);
+        framebuffer_release(info);
 
 out_err:
         return err;
@@ -855,7 +855,7 @@ static const struct of_device_id cg6_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg6_match);
 
-static struct of_platform_driver cg6_driver = {
+static struct platform_driver cg6_driver = {
         .driver = {
                 .name = "cg6",
                 .owner = THIS_MODULE,
@@ -870,12 +870,12 @@ static int __init cg6_init(void)
         if (fb_get_options("cg6fb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&cg6_driver);
+        return platform_driver_register(&cg6_driver);
 }
 
 static void __exit cg6_exit(void)
 {
-        of_unregister_platform_driver(&cg6_driver);
+        platform_driver_unregister(&cg6_driver);
 }
 
 module_init(cg6_init);
diff --git a/drivers/video/chipsfb.c b/drivers/video/chipsfb.c
index d637e1f53172..cff742abdc5d 100644
--- a/drivers/video/chipsfb.c
+++ b/drivers/video/chipsfb.c
@@ -460,10 +460,10 @@ static int chipsfb_pci_suspend(struct pci_dev *pdev, pm_message_t state)
         if (!(state.event & PM_EVENT_SLEEP))
                 goto done;
 
-        acquire_console_sem();
+        console_lock();
         chipsfb_blank(1, p);
         fb_set_suspend(p, 1);
-        release_console_sem();
+        console_unlock();
  done:
         pdev->dev.power.power_state = state;
         return 0;
@@ -473,10 +473,10 @@ static int chipsfb_pci_resume(struct pci_dev *pdev)
 {
         struct fb_info *p = pci_get_drvdata(pdev);
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(p, 0);
         chipsfb_blank(0, p);
-        release_console_sem();
+        console_unlock();
 
         pdev->dev.power.power_state = PMSG_ON;
         return 0;
diff --git a/drivers/video/console/Kconfig b/drivers/video/console/Kconfig
index 5a35f22372b9..2209e354f531 100644
--- a/drivers/video/console/Kconfig
+++ b/drivers/video/console/Kconfig
@@ -5,7 +5,7 @@
 menu "Console display driver support"
 
 config VGA_CONSOLE
-        bool "VGA text console" if EMBEDDED || !X86
+        bool "VGA text console" if EXPERT || !X86
         depends on !4xx && !8xx && !SPARC && !M68K && !PARISC && !FRV && !SUPERH && !BLACKFIN && !AVR32 && !MN10300 && (!ARM || ARCH_FOOTBRIDGE || ARCH_INTEGRATOR || ARCH_NETWINDER)
         default y
         help
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index 7ccc967831f0..8745637e4b7e 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -370,29 +370,27 @@ static void fb_flashcursor(struct work_struct *work)
 {
         struct fb_info *info = container_of(work, struct fb_info, queue);
         struct fbcon_ops *ops = info->fbcon_par;
-        struct display *p;
         struct vc_data *vc = NULL;
         int c;
         int mode;
 
-        acquire_console_sem();
+        console_lock();
         if (ops && ops->currcon != -1)
                 vc = vc_cons[ops->currcon].d;
 
         if (!vc || !CON_IS_VISIBLE(vc) ||
             registered_fb[con2fb_map[vc->vc_num]] != info ||
             vc->vc_deccm != 1) {
-                release_console_sem();
+                console_unlock();
                 return;
         }
 
-        p = &fb_display[vc->vc_num];
         c = scr_readw((u16 *) vc->vc_pos);
         mode = (!ops->cursor_flash || ops->cursor_state.enable) ?
                 CM_ERASE : CM_DRAW;
         ops->cursor(vc, info, mode, softback_lines, get_color(vc, info, c, 1),
                     get_color(vc, info, c, 0));
-        release_console_sem();
+        console_unlock();
 }
 
 static void cursor_timer_handler(unsigned long dev_addr)
@@ -823,10 +821,10 @@ static int set_con2fb_map(int unit, int newidx, int user)
         if (oldidx == newidx)
                 return 0;
 
-        if (!info || fbcon_has_exited)
+        if (!info)
                 return -EINVAL;
 
-        if (!err && !search_for_mapped_con()) {
+        if (!search_for_mapped_con() || !con_is_bound(&fb_con)) {
                 info_idx = newidx;
                 return fbcon_takeover(0);
         }
@@ -836,7 +834,7 @@ static int set_con2fb_map(int unit, int newidx, int user)
 
         found = search_fb_in_map(newidx);
 
-        acquire_console_sem();
+        console_lock();
         con2fb_map[unit] = newidx;
         if (!err && !found)
                 err = con2fb_acquire_newinfo(vc, info, unit, oldidx);
@@ -863,7 +861,7 @@ static int set_con2fb_map(int unit, int newidx, int user)
         if (!search_fb_in_map(info_idx))
                 info_idx = newidx;
 
-        release_console_sem();
+        console_unlock();
         return err;
 }
 
@@ -3321,7 +3319,7 @@ static ssize_t store_rotate(struct device *device,
         if (fbcon_has_exited)
                 return count;
 
-        acquire_console_sem();
+        console_lock();
         idx = con2fb_map[fg_console];
 
         if (idx == -1 || registered_fb[idx] == NULL)
@@ -3331,7 +3329,7 @@ static ssize_t store_rotate(struct device *device,
         rotate = simple_strtoul(buf, last, 0);
         fbcon_rotate(info, rotate);
 err:
-        release_console_sem();
+        console_unlock();
         return count;
 }
 
@@ -3346,7 +3344,7 @@ static ssize_t store_rotate_all(struct device *device,
         if (fbcon_has_exited)
                 return count;
 
-        acquire_console_sem();
+        console_lock();
         idx = con2fb_map[fg_console];
 
         if (idx == -1 || registered_fb[idx] == NULL)
@@ -3356,7 +3354,7 @@ static ssize_t store_rotate_all(struct device *device,
         rotate = simple_strtoul(buf, last, 0);
         fbcon_rotate_all(info, rotate);
 err:
-        release_console_sem();
+        console_unlock();
         return count;
 }
 
@@ -3369,7 +3367,7 @@ static ssize_t show_rotate(struct device *device,
         if (fbcon_has_exited)
                 return 0;
 
-        acquire_console_sem();
+        console_lock();
         idx = con2fb_map[fg_console];
 
         if (idx == -1 || registered_fb[idx] == NULL)
@@ -3378,7 +3376,7 @@ static ssize_t show_rotate(struct device *device,
         info = registered_fb[idx];
         rotate = fbcon_get_rotate(info);
 err:
-        release_console_sem();
+        console_unlock();
         return snprintf(buf, PAGE_SIZE, "%d\n", rotate);
 }
 
@@ -3392,7 +3390,7 @@ static ssize_t show_cursor_blink(struct device *device,
         if (fbcon_has_exited)
                 return 0;
 
-        acquire_console_sem();
+        console_lock();
         idx = con2fb_map[fg_console];
 
         if (idx == -1 || registered_fb[idx] == NULL)
@@ -3406,7 +3404,7 @@ static ssize_t show_cursor_blink(struct device *device,
 
         blink = (ops->flags & FBCON_FLAGS_CURSOR_TIMER) ? 1 : 0;
 err:
-        release_console_sem();
+        console_unlock();
         return snprintf(buf, PAGE_SIZE, "%d\n", blink);
 }
 
@@ -3421,7 +3419,7 @@ static ssize_t store_cursor_blink(struct device *device,
         if (fbcon_has_exited)
                 return count;
 
-        acquire_console_sem();
+        console_lock();
         idx = con2fb_map[fg_console];
 
         if (idx == -1 || registered_fb[idx] == NULL)
@@ -3443,7 +3441,7 @@ static ssize_t store_cursor_blink(struct device *device,
         }
 
 err:
-        release_console_sem();
+        console_unlock();
         return count;
 }
 
@@ -3482,7 +3480,7 @@ static void fbcon_start(void)
         if (num_registered_fb) {
                 int i;
 
-                acquire_console_sem();
+                console_lock();
 
                 for (i = 0; i < FB_MAX; i++) {
                         if (registered_fb[i] != NULL) {
@@ -3491,7 +3489,7 @@ static void fbcon_start(void)
                         }
                 }
 
-                release_console_sem();
+                console_unlock();
                 fbcon_takeover(0);
         }
 }
@@ -3552,7 +3550,7 @@ static int __init fb_console_init(void)
 {
         int i;
 
-        acquire_console_sem();
+        console_lock();
         fb_register_client(&fbcon_event_notifier);
         fbcon_device = device_create(fb_class, NULL, MKDEV(0, 0), NULL,
                                      "fbcon");
@@ -3568,7 +3566,7 @@ static int __init fb_console_init(void)
         for (i = 0; i < MAX_NR_CONSOLES; i++)
                 con2fb_map[i] = -1;
 
-        release_console_sem();
+        console_unlock();
         fbcon_start();
         return 0;
 }
@@ -3591,12 +3589,12 @@ static void __exit fbcon_deinit_device(void)
 
 static void __exit fb_console_exit(void)
 {
-        acquire_console_sem();
+        console_lock();
         fb_unregister_client(&fbcon_event_notifier);
         fbcon_deinit_device();
         device_destroy(fb_class, MKDEV(0, 0));
         fbcon_exit();
-        release_console_sem();
+        console_unlock();
         unregister_con_driver(&fb_con);
 }       
 
diff --git a/drivers/video/console/font_mini_4x6.c b/drivers/video/console/font_mini_4x6.c
index a19a7f33133e..fa6e698e63c4 100644
--- a/drivers/video/console/font_mini_4x6.c
+++ b/drivers/video/console/font_mini_4x6.c
@@ -1,5 +1,5 @@
 
-/* Hand composed "Miniscule" 4x6 font, with binary data generated using
+/* Hand composed "Minuscule" 4x6 font, with binary data generated using
  * Perl stub.
  *
  * Use 'perl -x mini_4x6.c < mini_4x6.c > new_version.c' to regenerate
diff --git a/drivers/video/console/newport_con.c b/drivers/video/console/newport_con.c
index 3772433c49d1..93317b5b8740 100644
--- a/drivers/video/console/newport_con.c
+++ b/drivers/video/console/newport_con.c
@@ -6,7 +6,7 @@
  * 
  * This driver is based on sgicons.c and cons_newport.
  * 
- * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
+ * Copyright (C) 1996 David S. Miller (davem@davemloft.net)
  * Copyright (C) 1997 Miguel de Icaza (miguel@nuclecu.unam.mx)
  */
 #include <linux/init.h>
diff --git a/drivers/video/console/tileblit.c b/drivers/video/console/tileblit.c
index 0056a41e5c35..15e8e1a89c45 100644
--- a/drivers/video/console/tileblit.c
+++ b/drivers/video/console/tileblit.c
@@ -83,7 +83,7 @@ static void tile_cursor(struct vc_data *vc, struct fb_info *info, int mode,
                         int softback_lines, int fg, int bg)
 {
         struct fb_tilecursor cursor;
-        int use_sw = (vc->vc_cursor_type & 0x01);
+        int use_sw = (vc->vc_cursor_type & 0x10);
 
         cursor.sx = vc->vc_x;
         cursor.sy = vc->vc_y;
diff --git a/drivers/video/console/vgacon.c b/drivers/video/console/vgacon.c
index 54e32c513610..915fd74da7a2 100644
--- a/drivers/video/console/vgacon.c
+++ b/drivers/video/console/vgacon.c
@@ -47,7 +47,6 @@
 #include <linux/ioport.h>
 #include <linux/init.h>
 #include <linux/screen_info.h>
-#include <linux/smp_lock.h>
 #include <video/vga.h>
 #include <asm/io.h>
 
@@ -203,11 +202,7 @@ static void vgacon_scrollback_init(int pitch)
         }
 }
 
-/*
- * Called only duing init so call of alloc_bootmen is ok.
- * Marked __init_refok to silence modpost.
- */
-static void __init_refok vgacon_scrollback_startup(void)
+static void vgacon_scrollback_startup(void)
 {
         vgacon_scrollback = kcalloc(CONFIG_VGACON_SOFT_SCROLLBACK_SIZE, 1024, GFP_NOWAIT);
         vgacon_scrollback_init(vga_video_num_columns * 2);
@@ -376,7 +371,8 @@ static const char *vgacon_startup(void)
         u16 saved1, saved2;
         volatile u16 *p;
 
-        if (screen_info.orig_video_isVGA == VIDEO_TYPE_VLFB) {
+        if (screen_info.orig_video_isVGA == VIDEO_TYPE_VLFB ||
+            screen_info.orig_video_isVGA == VIDEO_TYPE_EFI) {
               no_vga:
 #ifdef CONFIG_DUMMY_CONSOLE
                 conswitchp = &dummy_con;
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index 0c1afd13ddd3..850380795b05 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -47,6 +47,8 @@
 #include <linux/pci.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
 
 #include <asm/pgtable.h>
 #include <asm/system.h>
@@ -61,10 +63,10 @@ struct cfb_info {
         struct fb_info          fb;
         struct display_switch   *dispsw;
         struct display          *display;
-        struct pci_dev          *dev;
         unsigned char           __iomem *region;
         unsigned char           __iomem *regs;
         u_int                   id;
+        u_int                   irq;
         int                     func_use_count;
         u_long                  ref_ps;
 
@@ -88,6 +90,19 @@ struct cfb_info {
         u_char                  ramdac_powerdown;
 
         u32                     pseudo_palette[16];
+
+        spinlock_t              reg_b0_lock;
+
+#ifdef CONFIG_FB_CYBER2000_DDC
+        bool                    ddc_registered;
+        struct i2c_adapter      ddc_adapter;
+        struct i2c_algo_bit_data        ddc_algo;
+#endif
+
+#ifdef CONFIG_FB_CYBER2000_I2C
+        struct i2c_adapter      i2c_adapter;
+        struct i2c_algo_bit_data i2c_algo;
+#endif
 };
 
 static char *default_font = "Acorn8x8";
@@ -494,6 +509,7 @@ static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
         cyber2000_attrw(0x14, 0x00, cfb);
 
         /* PLL registers */
+        spin_lock(&cfb->reg_b0_lock);
         cyber2000_grphw(EXT_DCLK_MULT, hw->clock_mult, cfb);
         cyber2000_grphw(EXT_DCLK_DIV, hw->clock_div, cfb);
         cyber2000_grphw(EXT_MCLK_MULT, cfb->mclk_mult, cfb);
@@ -501,6 +517,7 @@ static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
         cyber2000_grphw(0x90, 0x01, cfb);
         cyber2000_grphw(0xb9, 0x80, cfb);
         cyber2000_grphw(0xb9, 0x00, cfb);
+        spin_unlock(&cfb->reg_b0_lock);
 
         cfb->ramdac_ctrl = hw->ramdac;
         cyber2000fb_write_ramdac_ctrl(cfb);
@@ -681,9 +698,9 @@ cyber2000fb_decode_clock(struct par_info *hw, struct cfb_info *cfb,
          *    pll_ps_calc = best_div1 / (ref_ps * best_mult)
          */
         best_diff = 0x7fffffff;
-        best_mult = 32;
-        best_div1 = 255;
-        for (t_div1 = 32; t_div1 > 1; t_div1 -= 1) {
+        best_mult = 2;
+        best_div1 = 32;
+        for (t_div1 = 2; t_div1 < 32; t_div1 += 1) {
                 u_int rr, t_mult, t_pll_ps;
                 int diff;
 
@@ -1105,24 +1122,22 @@ void cyber2000fb_disable_extregs(struct cfb_info *cfb)
 }
 EXPORT_SYMBOL(cyber2000fb_disable_extregs);
 
-void cyber2000fb_get_fb_var(struct cfb_info *cfb, struct fb_var_screeninfo *var)
-{
-        memcpy(var, &cfb->fb.var, sizeof(struct fb_var_screeninfo));
-}
-EXPORT_SYMBOL(cyber2000fb_get_fb_var);
-
 /*
  * Attach a capture/tv driver to the core CyberX0X0 driver.
  */
 int cyber2000fb_attach(struct cyberpro_info *info, int idx)
 {
         if (int_cfb_info != NULL) {
-                info->dev             = int_cfb_info->dev;
+                info->dev             = int_cfb_info->fb.device;
+#ifdef CONFIG_FB_CYBER2000_I2C
+                info->i2c             = &int_cfb_info->i2c_adapter;
+#else
+                info->i2c             = NULL;
+#endif
                 info->regs            = int_cfb_info->regs;
+                info->irq             = int_cfb_info->irq;
                 info->fb              = int_cfb_info->fb.screen_base;
                 info->fb_size         = int_cfb_info->fb.fix.smem_len;
-                info->enable_extregs  = cyber2000fb_enable_extregs;
-                info->disable_extregs = cyber2000fb_disable_extregs;
                 info->info            = int_cfb_info;
 
                 strlcpy(info->dev_name, int_cfb_info->fb.fix.id,
@@ -1141,6 +1156,183 @@ void cyber2000fb_detach(int idx)
 }
 EXPORT_SYMBOL(cyber2000fb_detach);
 
+#ifdef CONFIG_FB_CYBER2000_DDC
+
+#define DDC_REG         0xb0
+#define DDC_SCL_OUT     (1 << 0)
+#define DDC_SDA_OUT     (1 << 4)
+#define DDC_SCL_IN      (1 << 2)
+#define DDC_SDA_IN      (1 << 6)
+
+static void cyber2000fb_enable_ddc(struct cfb_info *cfb)
+{
+        spin_lock(&cfb->reg_b0_lock);
+        cyber2000fb_writew(0x1bf, 0x3ce, cfb);
+}
+
+static void cyber2000fb_disable_ddc(struct cfb_info *cfb)
+{
+        cyber2000fb_writew(0x0bf, 0x3ce, cfb);
+        spin_unlock(&cfb->reg_b0_lock);
+}
+
+
+static void cyber2000fb_ddc_setscl(void *data, int val)
+{
+        struct cfb_info *cfb = data;
+        unsigned char reg;
+
+        cyber2000fb_enable_ddc(cfb);
+        reg = cyber2000_grphr(DDC_REG, cfb);
+        if (!val)       /* bit is inverted */
+                reg |= DDC_SCL_OUT;
+        else
+                reg &= ~DDC_SCL_OUT;
+        cyber2000_grphw(DDC_REG, reg, cfb);
+        cyber2000fb_disable_ddc(cfb);
+}
+
+static void cyber2000fb_ddc_setsda(void *data, int val)
+{
+        struct cfb_info *cfb = data;
+        unsigned char reg;
+
+        cyber2000fb_enable_ddc(cfb);
+        reg = cyber2000_grphr(DDC_REG, cfb);
+        if (!val)       /* bit is inverted */
+                reg |= DDC_SDA_OUT;
+        else
+                reg &= ~DDC_SDA_OUT;
+        cyber2000_grphw(DDC_REG, reg, cfb);
+        cyber2000fb_disable_ddc(cfb);
+}
+
+static int cyber2000fb_ddc_getscl(void *data)
+{
+        struct cfb_info *cfb = data;
+        int retval;
+
+        cyber2000fb_enable_ddc(cfb);
+        retval = !!(cyber2000_grphr(DDC_REG, cfb) & DDC_SCL_IN);
+        cyber2000fb_disable_ddc(cfb);
+
+        return retval;
+}
+
+static int cyber2000fb_ddc_getsda(void *data)
+{
+        struct cfb_info *cfb = data;
+        int retval;
+
+        cyber2000fb_enable_ddc(cfb);
+        retval = !!(cyber2000_grphr(DDC_REG, cfb) & DDC_SDA_IN);
+        cyber2000fb_disable_ddc(cfb);
+
+        return retval;
+}
+
+static int __devinit cyber2000fb_setup_ddc_bus(struct cfb_info *cfb)
+{
+        strlcpy(cfb->ddc_adapter.name, cfb->fb.fix.id,
+                sizeof(cfb->ddc_adapter.name));
+        cfb->ddc_adapter.owner          = THIS_MODULE;
+        cfb->ddc_adapter.class          = I2C_CLASS_DDC;
+        cfb->ddc_adapter.algo_data      = &cfb->ddc_algo;
+        cfb->ddc_adapter.dev.parent     = cfb->fb.device;
+        cfb->ddc_algo.setsda            = cyber2000fb_ddc_setsda;
+        cfb->ddc_algo.setscl            = cyber2000fb_ddc_setscl;
+        cfb->ddc_algo.getsda            = cyber2000fb_ddc_getsda;
+        cfb->ddc_algo.getscl            = cyber2000fb_ddc_getscl;
+        cfb->ddc_algo.udelay            = 10;
+        cfb->ddc_algo.timeout           = 20;
+        cfb->ddc_algo.data              = cfb;
+
+        i2c_set_adapdata(&cfb->ddc_adapter, cfb);
+
+        return i2c_bit_add_bus(&cfb->ddc_adapter);
+}
+#endif /* CONFIG_FB_CYBER2000_DDC */
+
+#ifdef CONFIG_FB_CYBER2000_I2C
+static void cyber2000fb_i2c_setsda(void *data, int state)
+{
+        struct cfb_info *cfb = data;
+        unsigned int latch2;
+
+        spin_lock(&cfb->reg_b0_lock);
+        latch2 = cyber2000_grphr(EXT_LATCH2, cfb);
+        latch2 &= EXT_LATCH2_I2C_CLKEN;
+        if (state)
+                latch2 |= EXT_LATCH2_I2C_DATEN;
+        cyber2000_grphw(EXT_LATCH2, latch2, cfb);
+        spin_unlock(&cfb->reg_b0_lock);
+}
+
+static void cyber2000fb_i2c_setscl(void *data, int state)
+{
+        struct cfb_info *cfb = data;
+        unsigned int latch2;
+
+        spin_lock(&cfb->reg_b0_lock);
+        latch2 = cyber2000_grphr(EXT_LATCH2, cfb);
+        latch2 &= EXT_LATCH2_I2C_DATEN;
+        if (state)
+                latch2 |= EXT_LATCH2_I2C_CLKEN;
+        cyber2000_grphw(EXT_LATCH2, latch2, cfb);
+        spin_unlock(&cfb->reg_b0_lock);
+}
+
+static int cyber2000fb_i2c_getsda(void *data)
+{
+        struct cfb_info *cfb = data;
+        int ret;
+
+        spin_lock(&cfb->reg_b0_lock);
+        ret = !!(cyber2000_grphr(EXT_LATCH2, cfb) & EXT_LATCH2_I2C_DAT);
+        spin_unlock(&cfb->reg_b0_lock);
+
+        return ret;
+}
+
+static int cyber2000fb_i2c_getscl(void *data)
+{
+        struct cfb_info *cfb = data;
+        int ret;
+
+        spin_lock(&cfb->reg_b0_lock);
+        ret = !!(cyber2000_grphr(EXT_LATCH2, cfb) & EXT_LATCH2_I2C_CLK);
+        spin_unlock(&cfb->reg_b0_lock);
+
+        return ret;
+}
+
+static int __devinit cyber2000fb_i2c_register(struct cfb_info *cfb)
+{
+        strlcpy(cfb->i2c_adapter.name, cfb->fb.fix.id,
+                sizeof(cfb->i2c_adapter.name));
+        cfb->i2c_adapter.owner = THIS_MODULE;
+        cfb->i2c_adapter.algo_data = &cfb->i2c_algo;
+        cfb->i2c_adapter.dev.parent = cfb->fb.device;
+        cfb->i2c_algo.setsda = cyber2000fb_i2c_setsda;
+        cfb->i2c_algo.setscl = cyber2000fb_i2c_setscl;
+        cfb->i2c_algo.getsda = cyber2000fb_i2c_getsda;
+        cfb->i2c_algo.getscl = cyber2000fb_i2c_getscl;
+        cfb->i2c_algo.udelay = 5;
+        cfb->i2c_algo.timeout = msecs_to_jiffies(100);
+        cfb->i2c_algo.data = cfb;
+
+        return i2c_bit_add_bus(&cfb->i2c_adapter);
+}
+
+static void cyber2000fb_i2c_unregister(struct cfb_info *cfb)
+{
+        i2c_del_adapter(&cfb->i2c_adapter);
+}
+#else
+#define cyber2000fb_i2c_register(cfb)   (0)
+#define cyber2000fb_i2c_unregister(cfb) do { } while (0)
+#endif
+
 /*
  * These parameters give
  * 640x480, hsync 31.5kHz, vsync 60Hz
@@ -1275,6 +1467,8 @@ static struct cfb_info __devinit *cyberpro_alloc_fb_info(unsigned int id,
         cfb->fb.flags           = FBINFO_DEFAULT | FBINFO_HWACCEL_YPAN;
         cfb->fb.pseudo_palette  = cfb->pseudo_palette;
 
+        spin_lock_init(&cfb->reg_b0_lock);
+
         fb_alloc_cmap(&cfb->fb.cmap, NR_PALETTE, 0);
 
         return cfb;
@@ -1369,6 +1563,11 @@ static int __devinit cyberpro_common_probe(struct cfb_info *cfb)
         cfb->fb.fix.mmio_len   = MMIO_SIZE;
         cfb->fb.screen_base    = cfb->region;
 
+#ifdef CONFIG_FB_CYBER2000_DDC
+        if (cyber2000fb_setup_ddc_bus(cfb) == 0)
+                cfb->ddc_registered = true;
+#endif
+
         err = -EINVAL;
         if (!fb_find_mode(&cfb->fb.var, &cfb->fb, NULL, NULL, 0,
                           &cyber2000fb_default_mode, 8)) {
@@ -1401,14 +1600,32 @@ static int __devinit cyberpro_common_probe(struct cfb_info *cfb)
                 cfb->fb.var.xres, cfb->fb.var.yres,
                 h_sync / 1000, h_sync % 1000, v_sync);
 
-        if (cfb->dev)
-                cfb->fb.device = &cfb->dev->dev;
+        err = cyber2000fb_i2c_register(cfb);
+        if (err)
+                goto failed;
+
         err = register_framebuffer(&cfb->fb);
+        if (err)
+                cyber2000fb_i2c_unregister(cfb);
 
 failed:
+#ifdef CONFIG_FB_CYBER2000_DDC
+        if (err && cfb->ddc_registered)
+                i2c_del_adapter(&cfb->ddc_adapter);
+#endif
         return err;
 }
 
+static void __devexit cyberpro_common_remove(struct cfb_info *cfb)
+{
+        unregister_framebuffer(&cfb->fb);
+#ifdef CONFIG_FB_CYBER2000_DDC
+        if (cfb->ddc_registered)
+                i2c_del_adapter(&cfb->ddc_adapter);
+#endif
+        cyber2000fb_i2c_unregister(cfb);
+}
+
 static void cyberpro_common_resume(struct cfb_info *cfb)
 {
         cyberpro_init_hw(cfb);
@@ -1442,12 +1659,13 @@ static int __devinit cyberpro_vl_probe(void)
         if (!cfb)
                 goto failed_release;
 
-        cfb->dev = NULL;
+        cfb->irq = -1;
         cfb->region = ioremap(FB_START, FB_SIZE);
         if (!cfb->region)
                 goto failed_ioremap;
 
         cfb->regs = cfb->region + MMIO_OFFSET;
+        cfb->fb.device = NULL;
         cfb->fb.fix.mmio_start = FB_START + MMIO_OFFSET;
         cfb->fb.fix.smem_start = FB_START;
 
@@ -1585,12 +1803,13 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
         if (err)
                 goto failed_regions;
 
-        cfb->dev = dev;
+        cfb->irq = dev->irq;
         cfb->region = pci_ioremap_bar(dev, 0);
         if (!cfb->region)
                 goto failed_ioremap;
 
         cfb->regs = cfb->region + MMIO_OFFSET;
+        cfb->fb.device = &dev->dev;
         cfb->fb.fix.mmio_start = pci_resource_start(dev, 0) + MMIO_OFFSET;
         cfb->fb.fix.smem_start = pci_resource_start(dev, 0);
 
@@ -1648,15 +1867,7 @@ static void __devexit cyberpro_pci_remove(struct pci_dev *dev)
         struct cfb_info *cfb = pci_get_drvdata(dev);
 
         if (cfb) {
-                /*
-                 * If unregister_framebuffer fails, then
-                 * we will be leaving hooks that could cause
-                 * oopsen laying around.
-                 */
-                if (unregister_framebuffer(&cfb->fb))
-                        printk(KERN_WARNING "%s: danger Will Robinson, "
-                                "danger danger!  Oopsen imminent!\n",
-                                cfb->fb.fix.id);
+                cyberpro_common_remove(cfb);
                 iounmap(cfb->region);
                 cyberpro_free_fb_info(cfb);
 
diff --git a/drivers/video/cyber2000fb.h b/drivers/video/cyber2000fb.h
index de4fc43e51c1..bad69102e774 100644
--- a/drivers/video/cyber2000fb.h
+++ b/drivers/video/cyber2000fb.h
@@ -464,12 +464,14 @@ static void debug_printf(char *fmt, ...)
 struct cfb_info;
 
 struct cyberpro_info {
-        struct pci_dev  *dev;
+        struct device   *dev;
+        struct i2c_adapter *i2c;
         unsigned char   __iomem *regs;
         char            __iomem *fb;
         char            dev_name[32];
         unsigned int    fb_size;
         unsigned int    chip_id;
+        unsigned int    irq;
 
         /*
          * The following is a pointer to be passed into the
@@ -478,15 +480,6 @@ struct cyberpro_info {
          * is within this structure.
          */
         struct cfb_info *info;
-
-        /*
-         * Use these to enable the BM or TV registers.  In an SMP
-         * environment, these two function pointers should only be
-         * called from the module_init() or module_exit()
-         * functions.
-         */
-        void (*enable_extregs)(struct cfb_info *);
-        void (*disable_extregs)(struct cfb_info *);
 };
 
 #define ID_IGA_1682             0
@@ -494,8 +487,6 @@ struct cyberpro_info {
 #define ID_CYBERPRO_2010        2
 #define ID_CYBERPRO_5000        3
 
-struct fb_var_screeninfo;
-
 /*
  * Note! Writing to the Cyber20x0 registers from an interrupt
  * routine is definitely a bad idea atm.
@@ -504,4 +495,3 @@ int cyber2000fb_attach(struct cyberpro_info *info, int idx);
 void cyber2000fb_detach(int idx);
 void cyber2000fb_enable_extregs(struct cfb_info *cfb);
 void cyber2000fb_disable_extregs(struct cfb_info *cfb);
-void cyber2000fb_get_fb_var(struct cfb_info *cfb, struct fb_var_screeninfo *var);
diff --git a/drivers/video/da8xx-fb.c b/drivers/video/da8xx-fb.c
index cad7d45c8bac..fcdac872522d 100644
--- a/drivers/video/da8xx-fb.c
+++ b/drivers/video/da8xx-fb.c
@@ -763,7 +763,7 @@ static int fb_wait_for_vsync(struct fb_info *info)
 
         /*
          * Set flag to 0 and wait for isr to set to 1. It would seem there is a
-         * race condition here where the ISR could have occured just before or
+         * race condition here where the ISR could have occurred just before or
          * just after this set. But since we are just coarsely waiting for
          * a frame to complete then that's OK. i.e. if the frame completed
          * just before this code executed then we have to wait another full
@@ -899,7 +899,7 @@ static struct fb_ops da8xx_fb_ops = {
         .fb_blank = cfb_blank,
 };
 
-static int __init fb_probe(struct platform_device *device)
+static int __devinit fb_probe(struct platform_device *device)
 {
         struct da8xx_lcdc_platform_data *fb_pdata =
                                                 device->dev.platform_data;
@@ -1029,10 +1029,6 @@ static int __init fb_probe(struct platform_device *device)
                 goto err_release_pl_mem;
         }
 
-        ret = request_irq(par->irq, lcdc_irq_handler, 0, DRIVER_NAME, par);
-        if (ret)
-                goto err_release_pl_mem;
-
         /* Initialize par */
         da8xx_fb_info->var.bits_per_pixel = lcd_cfg->bpp;
 
@@ -1060,7 +1056,7 @@ static int __init fb_probe(struct platform_device *device)
 
         ret = fb_alloc_cmap(&da8xx_fb_info->cmap, PALETTE_SIZE, 0);
         if (ret)
-                goto err_free_irq;
+                goto err_release_pl_mem;
         da8xx_fb_info->cmap.len = par->palette_sz;
 
         /* initialize var_screeninfo */
@@ -1088,19 +1084,22 @@ static int __init fb_probe(struct platform_device *device)
                 goto err_cpu_freq;
         }
 #endif
+
+        ret = request_irq(par->irq, lcdc_irq_handler, 0, DRIVER_NAME, par);
+        if (ret)
+                goto irq_freq;
         return 0;
 
+irq_freq:
 #ifdef CONFIG_CPU_FREQ
+        lcd_da8xx_cpufreq_deregister(par);
+#endif
 err_cpu_freq:
         unregister_framebuffer(da8xx_fb_info);
-#endif
 
 err_dealloc_cmap:
         fb_dealloc_cmap(&da8xx_fb_info->cmap);
 
-err_free_irq:
-        free_irq(par->irq, par);
-
 err_release_pl_mem:
         dma_free_coherent(NULL, PALETTE_SIZE, par->v_palette_base,
                           par->p_palette_base);
@@ -1132,14 +1131,14 @@ static int fb_suspend(struct platform_device *dev, pm_message_t state)
         struct fb_info *info = platform_get_drvdata(dev);
         struct da8xx_fb_par *par = info->par;
 
-        acquire_console_sem();
+        console_lock();
         if (par->panel_power_ctrl)
                 par->panel_power_ctrl(0);
 
         fb_set_suspend(info, 1);
         lcd_disable_raster();
         clk_disable(par->lcdc_clk);
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1148,14 +1147,14 @@ static int fb_resume(struct platform_device *dev)
         struct fb_info *info = platform_get_drvdata(dev);
         struct da8xx_fb_par *par = info->par;
 
-        acquire_console_sem();
+        console_lock();
         if (par->panel_power_ctrl)
                 par->panel_power_ctrl(1);
 
         clk_enable(par->lcdc_clk);
         lcd_enable_raster();
         fb_set_suspend(info, 0);
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1166,7 +1165,7 @@ static int fb_resume(struct platform_device *dev)
 
 static struct platform_driver da8xx_fb_driver = {
         .probe = fb_probe,
-        .remove = fb_remove,
+        .remove = __devexit_p(fb_remove),
         .suspend = fb_suspend,
         .resume = fb_resume,
         .driver = {
diff --git a/drivers/video/display/display-sysfs.c b/drivers/video/display/display-sysfs.c
index f6a09ab0dac6..0c647d7af0ee 100644
--- a/drivers/video/display/display-sysfs.c
+++ b/drivers/video/display/display-sysfs.c
@@ -182,7 +182,7 @@ void display_device_unregister(struct display_device *ddev)
         mutex_lock(&ddev->lock);
         device_unregister(ddev->dev);
         mutex_unlock(&ddev->lock);
-        // Mark device index as avaliable
+        // Mark device index as available
         mutex_lock(&allocated_dsp_lock);
         idr_remove(&allocated_dsp, ddev->idx);
         mutex_unlock(&allocated_dsp_lock);
diff --git a/drivers/video/edid.h b/drivers/video/edid.h
index bd89fb3be8c2..d03a232d90b2 100644
--- a/drivers/video/edid.h
+++ b/drivers/video/edid.h
@@ -101,8 +101,8 @@
 #define V_SYNC_WIDTH       COMBINE_HI_4LO( V_SYNC_WIDTH_HI, V_SYNC_WIDTH_LO )
 #define V_SYNC_OFFSET      COMBINE_HI_4LO( V_SYNC_OFFSET_HI, V_SYNC_OFFSET_LO )
 
-#define H_SYNC_WIDTH       COMBINE_HI_4LO( H_SYNC_WIDTH_HI, H_SYNC_WIDTH_LO )
-#define H_SYNC_OFFSET      COMBINE_HI_4LO( H_SYNC_OFFSET_HI, H_SYNC_OFFSET_LO )
+#define H_SYNC_WIDTH       COMBINE_HI_8LO( H_SYNC_WIDTH_HI, H_SYNC_WIDTH_LO )
+#define H_SYNC_OFFSET      COMBINE_HI_8LO( H_SYNC_OFFSET_HI, H_SYNC_OFFSET_LO )
 
 #define H_SIZE_LO          (unsigned)block[ 12 ]
 #define V_SIZE_LO          (unsigned)block[ 13 ]
diff --git a/drivers/video/efifb.c b/drivers/video/efifb.c
index 70477c2e4b61..784139aed079 100644
--- a/drivers/video/efifb.c
+++ b/drivers/video/efifb.c
@@ -16,6 +16,8 @@
 #include <linux/pci.h>
 #include <video/vga.h>
 
+static bool request_mem_succeeded = false;
+
 static struct fb_var_screeninfo efifb_defined __devinitdata = {
         .activate               = FB_ACTIVATE_NOW,
         .height                 = -1,
@@ -53,6 +55,7 @@ enum {
         M_MB_7_1,       /* MacBook, 7th rev. */
         M_MB_SR,        /* MacBook, 2nd gen, (Santa Rosa) */
         M_MBA,          /* MacBook Air */
+        M_MBA_3,        /* Macbook Air, 3rd rev */
         M_MBP,          /* MacBook Pro */
         M_MBP_2,        /* MacBook Pro 2nd gen */
         M_MBP_2_2,      /* MacBook Pro 2,2nd gen */
@@ -64,43 +67,54 @@ enum {
         M_MBP_6_1,      /* MacBook Pro, 6,1th gen */
         M_MBP_6_2,      /* MacBook Pro, 6,2th gen */
         M_MBP_7_1,      /* MacBook Pro, 7,1th gen */
+        M_MBP_8_2,      /* MacBook Pro, 8,2nd gen */
         M_UNKNOWN       /* placeholder */
 };
 
+#define OVERRIDE_NONE   0x0
+#define OVERRIDE_BASE   0x1
+#define OVERRIDE_STRIDE 0x2
+#define OVERRIDE_HEIGHT 0x4
+#define OVERRIDE_WIDTH  0x8
+
 static struct efifb_dmi_info {
         char *optname;
         unsigned long base;
         int stride;
         int width;
         int height;
+        int flags;
 } dmi_list[] __initdata = {
-        [M_I17] = { "i17", 0x80010000, 1472 * 4, 1440, 900 },
-        [M_I20] = { "i20", 0x80010000, 1728 * 4, 1680, 1050 }, /* guess */
-        [M_I20_SR] = { "imac7", 0x40010000, 1728 * 4, 1680, 1050 },
-        [M_I24] = { "i24", 0x80010000, 2048 * 4, 1920, 1200 }, /* guess */
-        [M_I24_8_1] = { "imac8", 0xc0060000, 2048 * 4, 1920, 1200 },
-        [M_I24_10_1] = { "imac10", 0xc0010000, 2048 * 4, 1920, 1080 },
-        [M_I27_11_1] = { "imac11", 0xc0010000, 2560 * 4, 2560, 1440 },
-        [M_MINI]= { "mini", 0x80000000, 2048 * 4, 1024, 768 },
-        [M_MINI_3_1] = { "mini31", 0x40010000, 1024 * 4, 1024, 768 },
-        [M_MINI_4_1] = { "mini41", 0xc0010000, 2048 * 4, 1920, 1200 },
-        [M_MB] = { "macbook", 0x80000000, 2048 * 4, 1280, 800 },
-        [M_MB_5_1] = { "macbook51", 0x80010000, 2048 * 4, 1280, 800 },
-        [M_MB_6_1] = { "macbook61", 0x80010000, 2048 * 4, 1280, 800 },
-        [M_MB_7_1] = { "macbook71", 0x80010000, 2048 * 4, 1280, 800 },
-        [M_MBA] = { "mba", 0x80000000, 2048 * 4, 1280, 800 },
-        [M_MBP] = { "mbp", 0x80010000, 1472 * 4, 1440, 900 },
-        [M_MBP_2] = { "mbp2", 0, 0, 0, 0 }, /* placeholder */
-        [M_MBP_2_2] = { "mbp22", 0x80010000, 1472 * 4, 1440, 900 },
-        [M_MBP_SR] = { "mbp3", 0x80030000, 2048 * 4, 1440, 900 },
-        [M_MBP_4] = { "mbp4", 0xc0060000, 2048 * 4, 1920, 1200 },
-        [M_MBP_5_1] = { "mbp51", 0xc0010000, 2048 * 4, 1440, 900 },
-        [M_MBP_5_2] = { "mbp52", 0xc0010000, 2048 * 4, 1920, 1200 },
-        [M_MBP_5_3] = { "mbp53", 0xd0010000, 2048 * 4, 1440, 900 },
-        [M_MBP_6_1] = { "mbp61", 0x90030000, 2048 * 4, 1920, 1200 },
-        [M_MBP_6_2] = { "mbp62", 0x90030000, 2048 * 4, 1680, 1050 },
-        [M_MBP_7_1] = { "mbp71", 0xc0010000, 2048 * 4, 1280, 800 },
-        [M_UNKNOWN] = { NULL, 0, 0, 0, 0 }
+        [M_I17] = { "i17", 0x80010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_I20] = { "i20", 0x80010000, 1728 * 4, 1680, 1050, OVERRIDE_NONE }, /* guess */
+        [M_I20_SR] = { "imac7", 0x40010000, 1728 * 4, 1680, 1050, OVERRIDE_NONE },
+        [M_I24] = { "i24", 0x80010000, 2048 * 4, 1920, 1200, OVERRIDE_NONE }, /* guess */
+        [M_I24_8_1] = { "imac8", 0xc0060000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+        [M_I24_10_1] = { "imac10", 0xc0010000, 2048 * 4, 1920, 1080, OVERRIDE_NONE },
+        [M_I27_11_1] = { "imac11", 0xc0010000, 2560 * 4, 2560, 1440, OVERRIDE_NONE },
+        [M_MINI]= { "mini", 0x80000000, 2048 * 4, 1024, 768, OVERRIDE_NONE },
+        [M_MINI_3_1] = { "mini31", 0x40010000, 1024 * 4, 1024, 768, OVERRIDE_NONE },
+        [M_MINI_4_1] = { "mini41", 0xc0010000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+        [M_MB] = { "macbook", 0x80000000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+        [M_MB_5_1] = { "macbook51", 0x80010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+        [M_MB_6_1] = { "macbook61", 0x80010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+        [M_MB_7_1] = { "macbook71", 0x80010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+        [M_MBA] = { "mba", 0x80000000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+        /* 11" Macbook Air 3,1 passes the wrong stride */
+        [M_MBA_3] = { "mba3", 0, 2048 * 4, 0, 0, OVERRIDE_STRIDE },
+        [M_MBP] = { "mbp", 0x80010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_MBP_2] = { "mbp2", 0, 0, 0, 0, OVERRIDE_NONE }, /* placeholder */
+        [M_MBP_2_2] = { "mbp22", 0x80010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_MBP_SR] = { "mbp3", 0x80030000, 2048 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_MBP_4] = { "mbp4", 0xc0060000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+        [M_MBP_5_1] = { "mbp51", 0xc0010000, 2048 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_MBP_5_2] = { "mbp52", 0xc0010000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+        [M_MBP_5_3] = { "mbp53", 0xd0010000, 2048 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_MBP_6_1] = { "mbp61", 0x90030000, 2048 * 4, 1920, 1200, OVERRIDE_NONE },
+        [M_MBP_6_2] = { "mbp62", 0x90030000, 2048 * 4, 1680, 1050, OVERRIDE_NONE },
+        [M_MBP_7_1] = { "mbp71", 0xc0010000, 2048 * 4, 1280, 800, OVERRIDE_NONE },
+        [M_MBP_8_2] = { "mbp82", 0x90010000, 1472 * 4, 1440, 900, OVERRIDE_NONE },
+        [M_UNKNOWN] = { NULL, 0, 0, 0, 0, OVERRIDE_NONE }
 };
 
 static int set_system(const struct dmi_system_id *id);
@@ -138,6 +152,7 @@ static const struct dmi_system_id dmi_system_table[] __initconst = {
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook6,1", M_MB_6_1),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBook7,1", M_MB_7_1),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookAir1,1", M_MBA),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookAir3,1", M_MBA_3),
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro1,1", M_MBP),
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro2,1", M_MBP_2),
         EFIFB_DMI_SYSTEM_ID("Apple Computer, Inc.", "MacBookPro2,2", M_MBP_2_2),
@@ -151,19 +166,26 @@ static const struct dmi_system_id dmi_system_table[] __initconst = {
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro6,1", M_MBP_6_1),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro6,2", M_MBP_6_2),
         EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro7,1", M_MBP_7_1),
+        EFIFB_DMI_SYSTEM_ID("Apple Inc.", "MacBookPro8,2", M_MBP_8_2),
         {},
 };
 
+#define choose_value(dmivalue, fwvalue, field, flags) ({        \
+                typeof(fwvalue) _ret_ = fwvalue;                \
+                if ((flags) & (field))                          \
+                        _ret_ = dmivalue;                       \
+                else if ((fwvalue) == 0)                        \
+                        _ret_ = dmivalue;                       \
+                _ret_;                                          \
+        })
+
 static int set_system(const struct dmi_system_id *id)
 {
         struct efifb_dmi_info *info = id->driver_data;
-        if (info->base == 0)
-                return 0;
 
-        printk(KERN_INFO "efifb: dmi detected %s - framebuffer at %p "
-                         "(%dx%d, stride %d)\n", id->ident,
-                         (void *)info->base, info->width, info->height,
-                         info->stride);
+        if (info->base == 0 && info->height == 0 && info->width == 0
+                        && info->stride == 0)
+                return 0;
 
         /* Trust the bootloader over the DMI tables */
         if (screen_info.lfb_base == 0) {
@@ -171,40 +193,47 @@ static int set_system(const struct dmi_system_id *id)
                 struct pci_dev *dev = NULL;
                 int found_bar = 0;
 #endif
-                screen_info.lfb_base = info->base;
+                if (info->base) {
+                        screen_info.lfb_base = choose_value(info->base,
+                                screen_info.lfb_base, OVERRIDE_BASE,
+                                info->flags);
 
 #if defined(CONFIG_PCI)
-                /* make sure that the address in the table is actually on a
-                 * VGA device's PCI BAR */
-
-                for_each_pci_dev(dev) {
-                        int i;
-                        if ((dev->class >> 8) != PCI_CLASS_DISPLAY_VGA)
-                                continue;
-                        for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
-                                resource_size_t start, end;
-
-                                start = pci_resource_start(dev, i);
-                                if (start == 0)
-                                        break;
-                                end = pci_resource_end(dev, i);
-                                if (screen_info.lfb_base >= start &&
-                                                screen_info.lfb_base < end) {
-                                        found_bar = 1;
+                        /* make sure that the address in the table is actually
+                         * on a VGA device's PCI BAR */
+
+                        for_each_pci_dev(dev) {
+                                int i;
+                                if ((dev->class >> 8) != PCI_CLASS_DISPLAY_VGA)
+                                        continue;
+                                for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
+                                        resource_size_t start, end;
+
+                                        start = pci_resource_start(dev, i);
+                                        if (start == 0)
+                                                break;
+                                        end = pci_resource_end(dev, i);
+                                        if (screen_info.lfb_base >= start &&
+                                            screen_info.lfb_base < end) {
+                                                found_bar = 1;
+                                        }
                                 }
                         }
-                }
-                if (!found_bar)
-                        screen_info.lfb_base = 0;
+                        if (!found_bar)
+                                screen_info.lfb_base = 0;
 #endif
+                }
         }
         if (screen_info.lfb_base) {
-                if (screen_info.lfb_linelength == 0)
-                        screen_info.lfb_linelength = info->stride;
-                if (screen_info.lfb_width == 0)
-                        screen_info.lfb_width = info->width;
-                if (screen_info.lfb_height == 0)
-                        screen_info.lfb_height = info->height;
+                screen_info.lfb_linelength = choose_value(info->stride,
+                        screen_info.lfb_linelength, OVERRIDE_STRIDE,
+                        info->flags);
+                screen_info.lfb_width = choose_value(info->width,
+                        screen_info.lfb_width, OVERRIDE_WIDTH,
+                        info->flags);
+                screen_info.lfb_height = choose_value(info->height,
+                        screen_info.lfb_height, OVERRIDE_HEIGHT,
+                        info->flags);
                 if (screen_info.orig_video_isVGA == 0)
                         screen_info.orig_video_isVGA = VIDEO_TYPE_EFI;
         } else {
@@ -214,6 +243,13 @@ static int set_system(const struct dmi_system_id *id)
                 screen_info.orig_video_isVGA = 0;
                 return 0;
         }
+
+        printk(KERN_INFO "efifb: dmi detected %s - framebuffer at 0x%08x "
+                         "(%dx%d, stride %d)\n", id->ident,
+                         screen_info.lfb_base, screen_info.lfb_width,
+                         screen_info.lfb_height, screen_info.lfb_linelength);
+
+
         return 1;
 }
 
@@ -247,7 +283,9 @@ static void efifb_destroy(struct fb_info *info)
 {
         if (info->screen_base)
                 iounmap(info->screen_base);
-        release_mem_region(info->apertures->ranges[0].base, info->apertures->ranges[0].size);
+        if (request_mem_succeeded)
+                release_mem_region(info->apertures->ranges[0].base,
+                                   info->apertures->ranges[0].size);
         framebuffer_release(info);
 }
 
@@ -292,14 +330,13 @@ static int __init efifb_setup(char *options)
         return 0;
 }
 
-static int __devinit efifb_probe(struct platform_device *dev)
+static int __init efifb_probe(struct platform_device *dev)
 {
         struct fb_info *info;
         int err;
         unsigned int size_vmode;
         unsigned int size_remap;
         unsigned int size_total;
-        int request_succeeded = 0;
 
         if (!screen_info.lfb_depth)
                 screen_info.lfb_depth = 32;
@@ -353,7 +390,7 @@ static int __devinit efifb_probe(struct platform_device *dev)
         efifb_fix.smem_len = size_remap;
 
         if (request_mem_region(efifb_fix.smem_start, size_remap, "efifb")) {
-                request_succeeded = 1;
+                request_mem_succeeded = true;
         } else {
                 /* We cannot make this fatal. Sometimes this comes from magic
                    spaces our resource handlers simply don't know about */
@@ -379,7 +416,7 @@ static int __devinit efifb_probe(struct platform_device *dev)
         info->apertures->ranges[0].base = efifb_fix.smem_start;
         info->apertures->ranges[0].size = size_remap;
 
-        info->screen_base = ioremap(efifb_fix.smem_start, efifb_fix.smem_len);
+        info->screen_base = ioremap_wc(efifb_fix.smem_start, efifb_fix.smem_len);
         if (!info->screen_base) {
                 printk(KERN_ERR "efifb: abort, cannot ioremap video memory "
                                 "0x%x @ 0x%lx\n",
@@ -457,13 +494,12 @@ err_unmap:
 err_release_fb:
         framebuffer_release(info);
 err_release_mem:
-        if (request_succeeded)
+        if (request_mem_succeeded)
                 release_mem_region(efifb_fix.smem_start, size_total);
         return err;
 }
 
 static struct platform_driver efifb_driver = {
-        .probe  = efifb_probe,
         .driver = {
                 .name   = "efifb",
         },
@@ -494,13 +530,21 @@ static int __init efifb_init(void)
         if (!screen_info.lfb_linelength)
                 return -ENODEV;
 
-        ret = platform_driver_register(&efifb_driver);
+        ret = platform_device_register(&efifb_device);
+        if (ret)
+                return ret;
 
-        if (!ret) {
-                ret = platform_device_register(&efifb_device);
-                if (ret)
-                        platform_driver_unregister(&efifb_driver);
+        /*
+         * This is not just an optimization.  We will interfere
+         * with a real driver if we get reprobed, so don't allow
+         * it.
+         */
+        ret = platform_driver_probe(&efifb_driver, efifb_probe);
+        if (ret) {
+                platform_device_unregister(&efifb_device);
+                return ret;
         }
+
         return ret;
 }
 module_init(efifb_init);
diff --git a/drivers/video/ep93xx-fb.c b/drivers/video/ep93xx-fb.c
index 0c99de0562ca..cbdb1bd77c21 100644
--- a/drivers/video/ep93xx-fb.c
+++ b/drivers/video/ep93xx-fb.c
@@ -456,7 +456,7 @@ static int __init ep93xxfb_alloc_videomem(struct fb_info *info)
          * There is a bug in the ep93xx framebuffer which causes problems
          * if bit 27 of the physical address is set.
          * See: http://marc.info/?l=linux-arm-kernel&m=110061245502000&w=2
-         * There does not seem to be any offical errata for this, but I
+         * There does not seem to be any official errata for this, but I
          * have confirmed the problem exists on my hardware (ep9315) at
          * least.
          */
@@ -483,7 +483,7 @@ static void ep93xxfb_dealloc_videomem(struct fb_info *info)
                                   info->screen_base, info->fix.smem_start);
 }
 
-static int __init ep93xxfb_probe(struct platform_device *pdev)
+static int __devinit ep93xxfb_probe(struct platform_device *pdev)
 {
         struct ep93xxfb_mach_info *mach_info = pdev->dev.platform_data;
         struct fb_info *info;
@@ -598,7 +598,7 @@ failed:
         return err;
 }
 
-static int ep93xxfb_remove(struct platform_device *pdev)
+static int __devexit ep93xxfb_remove(struct platform_device *pdev)
 {
         struct fb_info *info = platform_get_drvdata(pdev);
         struct ep93xx_fbi *fbi = info->par;
@@ -622,7 +622,7 @@ static int ep93xxfb_remove(struct platform_device *pdev)
 
 static struct platform_driver ep93xxfb_driver = {
         .probe          = ep93xxfb_probe,
-        .remove         = ep93xxfb_remove,
+        .remove         = __devexit_p(ep93xxfb_remove),
         .driver = {
                 .name   = "ep93xx-fb",
                 .owner  = THIS_MODULE,
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/fb-puv3.c b/drivers/video/fb-puv3.c
new file mode 100644
index 000000000000..27f2c57e06e9
--- /dev/null
+++ b/drivers/video/fb-puv3.c
@@ -0,0 +1,852 @@
+/*
+ * Frame Buffer Driver for PKUnity-v3 Unigfx
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *      Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *      Copyright (C) 2001-2010 Guan Xuetao
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/console.h>
+
+#include <asm/sizes.h>
+#include <mach/hardware.h>
+
+/* Platform_data reserved for unifb registers. */
+#define UNIFB_REGS_NUM          10
+/* RAM reserved for the frame buffer. */
+#define UNIFB_MEMSIZE           (SZ_4M)         /* 4 MB for 1024*768*32b */
+
+/*
+ * cause UNIGFX don not have EDID
+ * all the modes are organized as follow
+ */
+static const struct fb_videomode unifb_modes[] = {
+        /* 0 640x480-60 VESA */
+        { "640x480@60",  60,  640, 480,  25175000,  48, 16, 34, 10,  96, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 1 640x480-75 VESA */
+        { "640x480@75",  75,  640, 480,  31500000, 120, 16, 18,  1,  64, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 2 800x600-60 VESA */
+        { "800x600@60",  60,  800, 600,  40000000,  88, 40, 26,  1, 128, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 3 800x600-75 VESA */
+        { "800x600@75",  75,  800, 600,  49500000, 160, 16, 23,  1,  80, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 4 1024x768-60 VESA */
+        { "1024x768@60", 60, 1024, 768,  65000000, 160, 24, 34,  3, 136, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 5 1024x768-75 VESA */
+        { "1024x768@75", 75, 1024, 768,  78750000, 176, 16, 30,  1,  96, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 6 1280x960-60 VESA */
+        { "1280x960@60", 60, 1280, 960, 108000000, 312, 96, 38,  1, 112, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 7 1440x900-60 VESA */
+        { "1440x900@60", 60, 1440, 900, 106500000, 232, 80, 30,  3, 152, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 8 FIXME 9 1024x600-60 VESA UNTESTED */
+        { "1024x600@60", 60, 1024, 600,  50650000, 160, 24, 26,  1, 136, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 9 FIXME 10 1024x600-75 VESA UNTESTED */
+        { "1024x600@75", 75, 1024, 600,  61500000, 176, 16, 23,  1,  96, 1,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+        /* 10 FIXME 11 1366x768-60 VESA UNTESTED */
+        { "1366x768@60", 60, 1366, 768,  85500000, 256, 58, 18,  1,  112, 3,
+          0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+};
+
+static struct fb_var_screeninfo unifb_default = {
+        .xres =         640,
+        .yres =         480,
+        .xres_virtual = 640,
+        .yres_virtual = 480,
+        .bits_per_pixel = 16,
+        .red =          { 11, 5, 0 },
+        .green =        { 5,  6, 0 },
+        .blue =         { 0,  5, 0 },
+        .activate =     FB_ACTIVATE_NOW,
+        .height =       -1,
+        .width =        -1,
+        .pixclock =     25175000,
+        .left_margin =  48,
+        .right_margin = 16,
+        .upper_margin = 33,
+        .lower_margin = 10,
+        .hsync_len =    96,
+        .vsync_len =    2,
+        .vmode =        FB_VMODE_NONINTERLACED,
+};
+
+static struct fb_fix_screeninfo unifb_fix = {
+        .id =           "UNIGFX FB",
+        .type =         FB_TYPE_PACKED_PIXELS,
+        .visual =       FB_VISUAL_TRUECOLOR,
+        .xpanstep =     1,
+        .ypanstep =     1,
+        .ywrapstep =    1,
+        .accel =        FB_ACCEL_NONE,
+};
+
+static void unifb_sync(struct fb_info *info)
+{
+        /* TODO: may, this can be replaced by interrupt */
+        int cnt;
+
+        for (cnt = 0; cnt < 0x10000000; cnt++) {
+                if (readl(UGE_COMMAND) & 0x1000000)
+                        return;
+        }
+
+        if (cnt > 0x8000000)
+                dev_warn(info->device, "Warning: UniGFX GE time out ...\n");
+}
+
+static void unifb_prim_fillrect(struct fb_info *info,
+                                const struct fb_fillrect *region)
+{
+        int awidth = region->width;
+        int aheight = region->height;
+        int m_iBpp = info->var.bits_per_pixel;
+        int screen_width = info->var.xres;
+        int src_sel = 1;        /* from fg_color */
+        int pat_sel = 1;
+        int src_x0 = 0;
+        int dst_x0 = region->dx;
+        int src_y0 = 0;
+        int dst_y0 = region->dy;
+        int rop_alpha_sel = 0;
+        int rop_alpha_code = 0xCC;
+        int x_dir = 1;
+        int y_dir = 1;
+        int alpha_r = 0;
+        int alpha_sel = 0;
+        int dst_pitch = screen_width * (m_iBpp / 8);
+        int dst_offset = dst_y0 * dst_pitch + dst_x0 * (m_iBpp / 8);
+        int src_pitch = screen_width * (m_iBpp / 8);
+        int src_offset = src_y0 * src_pitch + src_x0 * (m_iBpp / 8);
+        unsigned int command = 0;
+        int clip_region = 0;
+        int clip_en = 0;
+        int tp_en = 0;
+        int fg_color = 0;
+        int bottom = info->var.yres - 1;
+        int right = info->var.xres - 1;
+        int top = 0;
+
+        bottom = (bottom << 16) | right;
+        command = (rop_alpha_sel << 26) | (pat_sel << 18) | (src_sel << 16)
+                | (x_dir << 20) | (y_dir << 21) | (command << 24)
+                | (clip_region << 23) | (clip_en << 22) | (tp_en << 27);
+        src_pitch = (dst_pitch << 16) | src_pitch;
+        awidth = awidth | (aheight << 16);
+        alpha_r = ((rop_alpha_code & 0xff) << 8) | (alpha_r & 0xff)
+                | (alpha_sel << 16);
+        src_x0 = (src_x0 & 0x1fff) | ((src_y0 & 0x1fff) << 16);
+        dst_x0 = (dst_x0 & 0x1fff) | ((dst_y0 & 0x1fff) << 16);
+        fg_color = region->color;
+
+        unifb_sync(info);
+
+        writel(((u32 *)(info->pseudo_palette))[fg_color], UGE_FCOLOR);
+        writel(0, UGE_BCOLOR);
+        writel(src_pitch, UGE_PITCH);
+        writel(src_offset, UGE_SRCSTART);
+        writel(dst_offset, UGE_DSTSTART);
+        writel(awidth, UGE_WIDHEIGHT);
+        writel(top, UGE_CLIP0);
+        writel(bottom, UGE_CLIP1);
+        writel(alpha_r, UGE_ROPALPHA);
+        writel(src_x0, UGE_SRCXY);
+        writel(dst_x0, UGE_DSTXY);
+        writel(command, UGE_COMMAND);
+}
+
+static void unifb_fillrect(struct fb_info *info,
+                const struct fb_fillrect *region)
+{
+        struct fb_fillrect modded;
+        int vxres, vyres;
+
+        if (info->flags & FBINFO_HWACCEL_DISABLED) {
+                sys_fillrect(info, region);
+                return;
+        }
+
+        vxres = info->var.xres_virtual;
+        vyres = info->var.yres_virtual;
+
+        memcpy(&modded, region, sizeof(struct fb_fillrect));
+
+        if (!modded.width || !modded.height ||
+            modded.dx >= vxres || modded.dy >= vyres)
+                return;
+
+        if (modded.dx + modded.width > vxres)
+                modded.width = vxres - modded.dx;
+        if (modded.dy + modded.height > vyres)
+                modded.height = vyres - modded.dy;
+
+        unifb_prim_fillrect(info, &modded);
+}
+
+static void unifb_prim_copyarea(struct fb_info *info,
+                                const struct fb_copyarea *area)
+{
+        int awidth = area->width;
+        int aheight = area->height;
+        int m_iBpp = info->var.bits_per_pixel;
+        int screen_width = info->var.xres;
+        int src_sel = 2;        /* from mem */
+        int pat_sel = 0;
+        int src_x0 = area->sx;
+        int dst_x0 = area->dx;
+        int src_y0 = area->sy;
+        int dst_y0 = area->dy;
+
+        int rop_alpha_sel = 0;
+        int rop_alpha_code = 0xCC;
+        int x_dir = 1;
+        int y_dir = 1;
+
+        int alpha_r = 0;
+        int alpha_sel = 0;
+        int dst_pitch = screen_width * (m_iBpp / 8);
+        int dst_offset = dst_y0 * dst_pitch + dst_x0 * (m_iBpp / 8);
+        int src_pitch = screen_width * (m_iBpp / 8);
+        int src_offset = src_y0 * src_pitch + src_x0 * (m_iBpp / 8);
+        unsigned int command = 0;
+        int clip_region = 0;
+        int clip_en = 1;
+        int tp_en = 0;
+        int top = 0;
+        int bottom = info->var.yres;
+        int right = info->var.xres;
+        int fg_color = 0;
+        int bg_color = 0;
+
+        if (src_x0 < 0)
+                src_x0 = 0;
+        if (src_y0 < 0)
+                src_y0 = 0;
+
+        if (src_y0 - dst_y0 > 0) {
+                y_dir = 1;
+        } else {
+                y_dir = 0;
+                src_offset = (src_y0 + aheight) * src_pitch +
+                                src_x0 * (m_iBpp / 8);
+                dst_offset = (dst_y0 + aheight) * dst_pitch +
+                                dst_x0 * (m_iBpp / 8);
+                src_y0 += aheight;
+                dst_y0 += aheight;
+        }
+
+        command = (rop_alpha_sel << 26) | (pat_sel << 18) | (src_sel << 16) |
+                (x_dir << 20) | (y_dir << 21) | (command << 24) |
+                (clip_region << 23) | (clip_en << 22) | (tp_en << 27);
+        src_pitch = (dst_pitch << 16) | src_pitch;
+        awidth = awidth | (aheight << 16);
+        alpha_r = ((rop_alpha_code & 0xff) << 8) | (alpha_r & 0xff) |
+                (alpha_sel << 16);
+        src_x0 = (src_x0 & 0x1fff) | ((src_y0 & 0x1fff) << 16);
+        dst_x0 = (dst_x0 & 0x1fff) | ((dst_y0 & 0x1fff) << 16);
+        bottom = (bottom << 16) | right;
+
+        unifb_sync(info);
+
+        writel(src_pitch, UGE_PITCH);
+        writel(src_offset, UGE_SRCSTART);
+        writel(dst_offset, UGE_DSTSTART);
+        writel(awidth, UGE_WIDHEIGHT);
+        writel(top, UGE_CLIP0);
+        writel(bottom, UGE_CLIP1);
+        writel(bg_color, UGE_BCOLOR);
+        writel(fg_color, UGE_FCOLOR);
+        writel(alpha_r, UGE_ROPALPHA);
+        writel(src_x0, UGE_SRCXY);
+        writel(dst_x0, UGE_DSTXY);
+        writel(command, UGE_COMMAND);
+}
+
+static void unifb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
+{
+        struct fb_copyarea modded;
+        u32 vxres, vyres;
+        modded.sx = area->sx;
+        modded.sy = area->sy;
+        modded.dx = area->dx;
+        modded.dy = area->dy;
+        modded.width = area->width;
+        modded.height = area->height;
+
+        if (info->flags & FBINFO_HWACCEL_DISABLED) {
+                sys_copyarea(info, area);
+                return;
+        }
+
+        vxres = info->var.xres_virtual;
+        vyres = info->var.yres_virtual;
+
+        if (!modded.width || !modded.height ||
+            modded.sx >= vxres || modded.sy >= vyres ||
+            modded.dx >= vxres || modded.dy >= vyres)
+                return;
+
+        if (modded.sx + modded.width > vxres)
+                modded.width = vxres - modded.sx;
+        if (modded.dx + modded.width > vxres)
+                modded.width = vxres - modded.dx;
+        if (modded.sy + modded.height > vyres)
+                modded.height = vyres - modded.sy;
+        if (modded.dy + modded.height > vyres)
+                modded.height = vyres - modded.dy;
+
+        unifb_prim_copyarea(info, &modded);
+}
+
+static void unifb_imageblit(struct fb_info *info, const struct fb_image *image)
+{
+        sys_imageblit(info, image);
+}
+
+static u_long get_line_length(int xres_virtual, int bpp)
+{
+        u_long length;
+
+        length = xres_virtual * bpp;
+        length = (length + 31) & ~31;
+        length >>= 3;
+        return length;
+}
+
+/*
+ *  Setting the video mode has been split into two parts.
+ *  First part, xxxfb_check_var, must not write anything
+ *  to hardware, it should only verify and adjust var.
+ *  This means it doesn't alter par but it does use hardware
+ *  data from it to check this var.
+ */
+static int unifb_check_var(struct fb_var_screeninfo *var,
+                         struct fb_info *info)
+{
+        u_long line_length;
+
+        /*
+         *  FB_VMODE_CONUPDATE and FB_VMODE_SMOOTH_XPAN are equal!
+         *  as FB_VMODE_SMOOTH_XPAN is only used internally
+         */
+
+        if (var->vmode & FB_VMODE_CONUPDATE) {
+                var->vmode |= FB_VMODE_YWRAP;
+                var->xoffset = info->var.xoffset;
+                var->yoffset = info->var.yoffset;
+        }
+
+        /*
+         *  Some very basic checks
+         */
+        if (!var->xres)
+                var->xres = 1;
+        if (!var->yres)
+                var->yres = 1;
+        if (var->xres > var->xres_virtual)
+                var->xres_virtual = var->xres;
+        if (var->yres > var->yres_virtual)
+                var->yres_virtual = var->yres;
+        if (var->bits_per_pixel <= 1)
+                var->bits_per_pixel = 1;
+        else if (var->bits_per_pixel <= 8)
+                var->bits_per_pixel = 8;
+        else if (var->bits_per_pixel <= 16)
+                var->bits_per_pixel = 16;
+        else if (var->bits_per_pixel <= 24)
+                var->bits_per_pixel = 24;
+        else if (var->bits_per_pixel <= 32)
+                var->bits_per_pixel = 32;
+        else
+                return -EINVAL;
+
+        if (var->xres_virtual < var->xoffset + var->xres)
+                var->xres_virtual = var->xoffset + var->xres;
+        if (var->yres_virtual < var->yoffset + var->yres)
+                var->yres_virtual = var->yoffset + var->yres;
+
+        /*
+         *  Memory limit
+         */
+        line_length =
+            get_line_length(var->xres_virtual, var->bits_per_pixel);
+        if (line_length * var->yres_virtual > UNIFB_MEMSIZE)
+                return -ENOMEM;
+
+        /*
+         * Now that we checked it we alter var. The reason being is that the
+         * video mode passed in might not work but slight changes to it might
+         * make it work. This way we let the user know what is acceptable.
+         */
+        switch (var->bits_per_pixel) {
+        case 1:
+        case 8:
+                var->red.offset = 0;
+                var->red.length = 8;
+                var->green.offset = 0;
+                var->green.length = 8;
+                var->blue.offset = 0;
+                var->blue.length = 8;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                break;
+        case 16:                /* RGBA 5551 */
+                if (var->transp.length) {
+                        var->red.offset = 0;
+                        var->red.length = 5;
+                        var->green.offset = 5;
+                        var->green.length = 5;
+                        var->blue.offset = 10;
+                        var->blue.length = 5;
+                        var->transp.offset = 15;
+                        var->transp.length = 1;
+                } else {        /* RGB 565 */
+                        var->red.offset = 11;
+                        var->red.length = 5;
+                        var->green.offset = 5;
+                        var->green.length = 6;
+                        var->blue.offset = 0;
+                        var->blue.length = 5;
+                        var->transp.offset = 0;
+                        var->transp.length = 0;
+                }
+                break;
+        case 24:                /* RGB 888 */
+                var->red.offset = 0;
+                var->red.length = 8;
+                var->green.offset = 8;
+                var->green.length = 8;
+                var->blue.offset = 16;
+                var->blue.length = 8;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                break;
+        case 32:                /* RGBA 8888 */
+                var->red.offset = 16;
+                var->red.length = 8;
+                var->green.offset = 8;
+                var->green.length = 8;
+                var->blue.offset = 0;
+                var->blue.length = 8;
+                var->transp.offset = 24;
+                var->transp.length = 8;
+                break;
+        }
+        var->red.msb_right = 0;
+        var->green.msb_right = 0;
+        var->blue.msb_right = 0;
+        var->transp.msb_right = 0;
+
+        return 0;
+}
+
+/*
+ * This routine actually sets the video mode. It's in here where we
+ * the hardware state info->par and fix which can be affected by the
+ * change in par. For this driver it doesn't do much.
+ */
+static int unifb_set_par(struct fb_info *info)
+{
+        int hTotal, vTotal, hSyncStart, hSyncEnd, vSyncStart, vSyncEnd;
+        int format;
+
+#ifdef CONFIG_PUV3_PM
+        struct clk *clk_vga;
+        u32 pixclk = 0;
+        int i;
+
+        for (i = 0; i <= 10; i++) {
+                if    (info->var.xres         == unifb_modes[i].xres
+                    && info->var.yres         == unifb_modes[i].yres
+                    && info->var.upper_margin == unifb_modes[i].upper_margin
+                    && info->var.lower_margin == unifb_modes[i].lower_margin
+                    && info->var.left_margin  == unifb_modes[i].left_margin
+                    && info->var.right_margin == unifb_modes[i].right_margin
+                    && info->var.hsync_len    == unifb_modes[i].hsync_len
+                    && info->var.vsync_len    == unifb_modes[i].vsync_len) {
+                        pixclk = unifb_modes[i].pixclock;
+                        break;
+                }
+        }
+
+        /* set clock rate */
+        clk_vga = clk_get(info->device, "VGA_CLK");
+        if (clk_vga == ERR_PTR(-ENOENT))
+                return -ENOENT;
+
+        if (pixclk != 0) {
+                if (clk_set_rate(clk_vga, pixclk)) { /* set clock failed */
+                        info->fix = unifb_fix;
+                        info->var = unifb_default;
+                        if (clk_set_rate(clk_vga, unifb_default.pixclock))
+                                return -EINVAL;
+                }
+        }
+#endif
+
+        info->fix.line_length = get_line_length(info->var.xres_virtual,
+                                                info->var.bits_per_pixel);
+
+        hSyncStart = info->var.xres + info->var.right_margin;
+        hSyncEnd = hSyncStart + info->var.hsync_len;
+        hTotal = hSyncEnd + info->var.left_margin;
+
+        vSyncStart = info->var.yres + info->var.lower_margin;
+        vSyncEnd = vSyncStart + info->var.vsync_len;
+        vTotal = vSyncEnd + info->var.upper_margin;
+
+        switch (info->var.bits_per_pixel) {
+        case 8:
+                format = UDE_CFG_DST8;
+                break;
+        case 16:
+                format = UDE_CFG_DST16;
+                break;
+        case 24:
+                format = UDE_CFG_DST24;
+                break;
+        case 32:
+                format = UDE_CFG_DST32;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        writel(info->fix.smem_start, UDE_FSA);
+        writel(info->var.yres, UDE_LS);
+        writel(get_line_length(info->var.xres,
+                        info->var.bits_per_pixel) >> 3, UDE_PS);
+                        /* >> 3 for hardware required. */
+        writel((hTotal << 16) | (info->var.xres), UDE_HAT);
+        writel(((hTotal - 1) << 16) | (info->var.xres - 1), UDE_HBT);
+        writel(((hSyncEnd - 1) << 16) | (hSyncStart - 1), UDE_HST);
+        writel((vTotal << 16) | (info->var.yres), UDE_VAT);
+        writel(((vTotal - 1) << 16) | (info->var.yres - 1), UDE_VBT);
+        writel(((vSyncEnd - 1) << 16) | (vSyncStart - 1), UDE_VST);
+        writel(UDE_CFG_GDEN_ENABLE | UDE_CFG_TIMEUP_ENABLE
+                        | format | 0xC0000001, UDE_CFG);
+
+        return 0;
+}
+
+/*
+ *  Set a single color register. The values supplied are already
+ *  rounded down to the hardware's capabilities (according to the
+ *  entries in the var structure). Return != 0 for invalid regno.
+ */
+static int unifb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+                         u_int transp, struct fb_info *info)
+{
+        if (regno >= 256)       /* no. of hw registers */
+                return 1;
+
+        /* grayscale works only partially under directcolor */
+        if (info->var.grayscale) {
+                /* grayscale = 0.30*R + 0.59*G + 0.11*B */
+                red = green = blue =
+                    (red * 77 + green * 151 + blue * 28) >> 8;
+        }
+
+#define CNVT_TOHW(val, width) ((((val)<<(width))+0x7FFF-(val))>>16)
+        switch (info->fix.visual) {
+        case FB_VISUAL_TRUECOLOR:
+        case FB_VISUAL_PSEUDOCOLOR:
+                red = CNVT_TOHW(red, info->var.red.length);
+                green = CNVT_TOHW(green, info->var.green.length);
+                blue = CNVT_TOHW(blue, info->var.blue.length);
+                transp = CNVT_TOHW(transp, info->var.transp.length);
+                break;
+        case FB_VISUAL_DIRECTCOLOR:
+                red = CNVT_TOHW(red, 8);        /* expect 8 bit DAC */
+                green = CNVT_TOHW(green, 8);
+                blue = CNVT_TOHW(blue, 8);
+                /* hey, there is bug in transp handling... */
+                transp = CNVT_TOHW(transp, 8);
+                break;
+        }
+#undef CNVT_TOHW
+        /* Truecolor has hardware independent palette */
+        if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+                u32 v;
+
+                if (regno >= 16)
+                        return 1;
+
+                v = (red << info->var.red.offset) |
+                    (green << info->var.green.offset) |
+                    (blue << info->var.blue.offset) |
+                    (transp << info->var.transp.offset);
+                switch (info->var.bits_per_pixel) {
+                case 8:
+                        break;
+                case 16:
+                case 24:
+                case 32:
+                        ((u32 *) (info->pseudo_palette))[regno] = v;
+                        break;
+                default:
+                        return 1;
+                }
+                return 0;
+        }
+        return 0;
+}
+
+/*
+ *  Pan or Wrap the Display
+ *
+ *  This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
+ */
+static int unifb_pan_display(struct fb_var_screeninfo *var,
+                           struct fb_info *info)
+{
+        if (var->vmode & FB_VMODE_YWRAP) {
+                if (var->yoffset < 0
+                    || var->yoffset >= info->var.yres_virtual
+                    || var->xoffset)
+                        return -EINVAL;
+        } else {
+                if (var->xoffset + var->xres > info->var.xres_virtual ||
+                    var->yoffset + var->yres > info->var.yres_virtual)
+                        return -EINVAL;
+        }
+        info->var.xoffset = var->xoffset;
+        info->var.yoffset = var->yoffset;
+        if (var->vmode & FB_VMODE_YWRAP)
+                info->var.vmode |= FB_VMODE_YWRAP;
+        else
+                info->var.vmode &= ~FB_VMODE_YWRAP;
+        return 0;
+}
+
+int unifb_mmap(struct fb_info *info,
+                    struct vm_area_struct *vma)
+{
+        unsigned long size = vma->vm_end - vma->vm_start;
+        unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+        unsigned long pos = info->fix.smem_start + offset;
+
+        if (offset + size > info->fix.smem_len)
+                return -EINVAL;
+
+        vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+        if (io_remap_pfn_range(vma, vma->vm_start, pos >> PAGE_SHIFT, size,
+                                vma->vm_page_prot))
+                return -EAGAIN;
+
+        vma->vm_flags |= VM_RESERVED;   /* avoid to swap out this VMA */
+        return 0;
+
+}
+
+static struct fb_ops unifb_ops = {
+        .fb_read        = fb_sys_read,
+        .fb_write       = fb_sys_write,
+        .fb_check_var   = unifb_check_var,
+        .fb_set_par     = unifb_set_par,
+        .fb_setcolreg   = unifb_setcolreg,
+        .fb_pan_display = unifb_pan_display,
+        .fb_fillrect    = unifb_fillrect,
+        .fb_copyarea    = unifb_copyarea,
+        .fb_imageblit   = unifb_imageblit,
+        .fb_mmap        = unifb_mmap,
+};
+
+/*
+ *  Initialisation
+ */
+static int unifb_probe(struct platform_device *dev)
+{
+        struct fb_info *info;
+        u32 unifb_regs[UNIFB_REGS_NUM];
+        int retval = -ENOMEM;
+        struct resource *iomem;
+        void *videomemory;
+
+        videomemory = (void *)__get_free_pages(GFP_KERNEL | __GFP_COMP,
+                                get_order(UNIFB_MEMSIZE));
+        if (!videomemory)
+                goto err;
+
+        memset(videomemory, 0, UNIFB_MEMSIZE);
+
+        unifb_fix.smem_start = virt_to_phys(videomemory);
+        unifb_fix.smem_len = UNIFB_MEMSIZE;
+
+        iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
+        unifb_fix.mmio_start = iomem->start;
+
+        info = framebuffer_alloc(sizeof(u32)*256, &dev->dev);
+        if (!info)
+                goto err;
+
+        info->screen_base = (char __iomem *)videomemory;
+        info->fbops = &unifb_ops;
+
+        retval = fb_find_mode(&info->var, info, NULL,
+                              unifb_modes, 10, &unifb_modes[0], 16);
+
+        if (!retval || (retval == 4))
+                info->var = unifb_default;
+
+        info->fix = unifb_fix;
+        info->pseudo_palette = info->par;
+        info->par = NULL;
+        info->flags = FBINFO_FLAG_DEFAULT;
+#ifdef FB_ACCEL_PUV3_UNIGFX
+        info->fix.accel = FB_ACCEL_PUV3_UNIGFX;
+#endif
+
+        retval = fb_alloc_cmap(&info->cmap, 256, 0);
+        if (retval < 0)
+                goto err1;
+
+        retval = register_framebuffer(info);
+        if (retval < 0)
+                goto err2;
+        platform_set_drvdata(dev, info);
+        platform_device_add_data(dev, unifb_regs, sizeof(u32) * UNIFB_REGS_NUM);
+
+        printk(KERN_INFO
+               "fb%d: Virtual frame buffer device, using %dM of video memory\n",
+               info->node, UNIFB_MEMSIZE >> 20);
+        return 0;
+err2:
+        fb_dealloc_cmap(&info->cmap);
+err1:
+        framebuffer_release(info);
+err:
+        return retval;
+}
+
+static int unifb_remove(struct platform_device *dev)
+{
+        struct fb_info *info = platform_get_drvdata(dev);
+
+        if (info) {
+                unregister_framebuffer(info);
+                fb_dealloc_cmap(&info->cmap);
+                framebuffer_release(info);
+        }
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int unifb_resume(struct platform_device *dev)
+{
+        int rc = 0;
+        u32 *unifb_regs = dev->dev.platform_data;
+
+        if (dev->dev.power.power_state.event == PM_EVENT_ON)
+                return 0;
+
+        console_lock();
+
+        if (dev->dev.power.power_state.event == PM_EVENT_SUSPEND) {
+                writel(unifb_regs[0], UDE_FSA);
+                writel(unifb_regs[1], UDE_LS);
+                writel(unifb_regs[2], UDE_PS);
+                writel(unifb_regs[3], UDE_HAT);
+                writel(unifb_regs[4], UDE_HBT);
+                writel(unifb_regs[5], UDE_HST);
+                writel(unifb_regs[6], UDE_VAT);
+                writel(unifb_regs[7], UDE_VBT);
+                writel(unifb_regs[8], UDE_VST);
+                writel(unifb_regs[9], UDE_CFG);
+        }
+        dev->dev.power.power_state = PMSG_ON;
+
+        console_unlock();
+
+        return rc;
+}
+
+static int unifb_suspend(struct platform_device *dev, pm_message_t mesg)
+{
+        u32 *unifb_regs = dev->dev.platform_data;
+
+        unifb_regs[0] = readl(UDE_FSA);
+        unifb_regs[1] = readl(UDE_LS);
+        unifb_regs[2] = readl(UDE_PS);
+        unifb_regs[3] = readl(UDE_HAT);
+        unifb_regs[4] = readl(UDE_HBT);
+        unifb_regs[5] = readl(UDE_HST);
+        unifb_regs[6] = readl(UDE_VAT);
+        unifb_regs[7] = readl(UDE_VBT);
+        unifb_regs[8] = readl(UDE_VST);
+        unifb_regs[9] = readl(UDE_CFG);
+
+        if (mesg.event == dev->dev.power.power_state.event)
+                return 0;
+
+        switch (mesg.event) {
+        case PM_EVENT_FREEZE:           /* about to take snapshot */
+        case PM_EVENT_PRETHAW:          /* before restoring snapshot */
+                goto done;
+        }
+
+        console_lock();
+
+        /* do nothing... */
+
+        console_unlock();
+
+done:
+        dev->dev.power.power_state = mesg;
+
+        return 0;
+}
+#else
+#define unifb_resume    NULL
+#define unifb_suspend   NULL
+#endif
+
+static struct platform_driver unifb_driver = {
+        .probe   = unifb_probe,
+        .remove  = unifb_remove,
+        .resume  = unifb_resume,
+        .suspend = unifb_suspend,
+        .driver  = {
+                .name   = "PKUnity-v3-UNIGFX",
+        },
+};
+
+static int __init unifb_init(void)
+{
+#ifndef MODULE
+        if (fb_get_options("unifb", NULL))
+                return -ENODEV;
+#endif
+
+        return platform_driver_register(&unifb_driver);
+}
+
+module_init(unifb_init);
+
+static void __exit unifb_exit(void)
+{
+        platform_driver_unregister(&unifb_driver);
+}
+
+module_exit(unifb_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/fb_defio.c b/drivers/video/fb_defio.c
index 6b93ef93cb12..804000183c5e 100644
--- a/drivers/video/fb_defio.c
+++ b/drivers/video/fb_defio.c
@@ -75,7 +75,7 @@ int fb_deferred_io_fsync(struct file *file, int datasync)
                 return 0;
 
         /* Kill off the delayed work */
-        cancel_rearming_delayed_work(&info->deferred_work);
+        cancel_delayed_work_sync(&info->deferred_work);
 
         /* Run it immediately */
         return schedule_delayed_work(&info->deferred_work, 0);
diff --git a/drivers/video/fbcmap.c b/drivers/video/fbcmap.c
index f53b9f1d6aba..5c3960da755a 100644
--- a/drivers/video/fbcmap.c
+++ b/drivers/video/fbcmap.c
@@ -80,6 +80,7 @@ static const struct fb_cmap default_16_colors = {
  *      @cmap: frame buffer colormap structure
  *      @len: length of @cmap
  *      @transp: boolean, 1 if there is transparency, 0 otherwise
+ *      @flags: flags for kmalloc memory allocation
  *
  *      Allocates memory for a colormap @cmap.  @len is the
  *      number of entries in the palette.
@@ -88,34 +89,48 @@ static const struct fb_cmap default_16_colors = {
  *
  */
 
-int fb_alloc_cmap(struct fb_cmap *cmap, int len, int transp)
+int fb_alloc_cmap_gfp(struct fb_cmap *cmap, int len, int transp, gfp_t flags)
 {
-    int size = len*sizeof(u16);
-
-    if (cmap->len != len) {
-        fb_dealloc_cmap(cmap);
-        if (!len)
-            return 0;
-        if (!(cmap->red = kmalloc(size, GFP_ATOMIC)))
-            goto fail;
-        if (!(cmap->green = kmalloc(size, GFP_ATOMIC)))
-            goto fail;
-        if (!(cmap->blue = kmalloc(size, GFP_ATOMIC)))
-            goto fail;
-        if (transp) {
-            if (!(cmap->transp = kmalloc(size, GFP_ATOMIC)))
+        int size = len * sizeof(u16);
+        int ret = -ENOMEM;
+
+        if (cmap->len != len) {
+                fb_dealloc_cmap(cmap);
+                if (!len)
+                        return 0;
+
+                cmap->red = kmalloc(size, flags);
+                if (!cmap->red)
+                        goto fail;
+                cmap->green = kmalloc(size, flags);
+                if (!cmap->green)
+                        goto fail;
+                cmap->blue = kmalloc(size, flags);
+                if (!cmap->blue)
+                        goto fail;
+                if (transp) {
+                        cmap->transp = kmalloc(size, flags);
+                        if (!cmap->transp)
+                                goto fail;
+                } else {
+                        cmap->transp = NULL;
+                }
+        }
+        cmap->start = 0;
+        cmap->len = len;
+        ret = fb_copy_cmap(fb_default_cmap(len), cmap);
+        if (ret)
                 goto fail;
-        } else
-            cmap->transp = NULL;
-    }
-    cmap->start = 0;
-    cmap->len = len;
-    fb_copy_cmap(fb_default_cmap(len), cmap);
-    return 0;
+        return 0;
 
 fail:
-    fb_dealloc_cmap(cmap);
-    return -ENOMEM;
+        fb_dealloc_cmap(cmap);
+        return ret;
+}
+
+int fb_alloc_cmap(struct fb_cmap *cmap, int len, int transp)
+{
+        return fb_alloc_cmap_gfp(cmap, len, transp, GFP_ATOMIC);
 }
 
 /**
@@ -250,8 +265,12 @@ int fb_set_user_cmap(struct fb_cmap_user *cmap, struct fb_info *info)
         int rc, size = cmap->len * sizeof(u16);
         struct fb_cmap umap;
 
+        if (size < 0 || size < cmap->len)
+                return -E2BIG;
+
         memset(&umap, 0, sizeof(struct fb_cmap));
-        rc = fb_alloc_cmap(&umap, cmap->len, cmap->transp != NULL);
+        rc = fb_alloc_cmap_gfp(&umap, cmap->len, cmap->transp != NULL,
+                                GFP_KERNEL);
         if (rc)
                 return rc;
         if (copy_from_user(umap.red, cmap->red, size) ||
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 b06647517c0e..5aac00eb1830 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -42,9 +42,34 @@
 
 #define FBPIXMAPSIZE    (1024 * 8)
 
+static DEFINE_MUTEX(registration_lock);
 struct fb_info *registered_fb[FB_MAX] __read_mostly;
 int num_registered_fb __read_mostly;
 
+static struct fb_info *get_fb_info(unsigned int idx)
+{
+        struct fb_info *fb_info;
+
+        if (idx >= FB_MAX)
+                return ERR_PTR(-ENODEV);
+
+        mutex_lock(&registration_lock);
+        fb_info = registered_fb[idx];
+        if (fb_info)
+                atomic_inc(&fb_info->count);
+        mutex_unlock(&registration_lock);
+
+        return fb_info;
+}
+
+static void put_fb_info(struct fb_info *fb_info)
+{
+        if (!atomic_dec_and_test(&fb_info->count))
+                return;
+        if (fb_info->fbops->fb_destroy)
+                fb_info->fbops->fb_destroy(fb_info);
+}
+
 int lock_fb_info(struct fb_info *info)
 {
         mutex_lock(&info->lock);
@@ -647,6 +672,7 @@ int fb_show_logo(struct fb_info *info, int rotate) { return 0; }
 
 static void *fb_seq_start(struct seq_file *m, loff_t *pos)
 {
+        mutex_lock(&registration_lock);
         return (*pos < FB_MAX) ? pos : NULL;
 }
 
@@ -658,6 +684,7 @@ static void *fb_seq_next(struct seq_file *m, void *v, loff_t *pos)
 
 static void fb_seq_stop(struct seq_file *m, void *v)
 {
+        mutex_unlock(&registration_lock);
 }
 
 static int fb_seq_show(struct seq_file *m, void *v)
@@ -690,16 +717,33 @@ static const struct file_operations fb_proc_fops = {
         .release        = seq_release,
 };
 
-static ssize_t
-fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
+/*
+ * We hold a reference to the fb_info in file->private_data,
+ * but if the current registered fb has changed, we don't
+ * actually want to use it.
+ *
+ * So look up the fb_info using the inode minor number,
+ * and just verify it against the reference we have.
+ */
+static struct fb_info *file_fb_info(struct file *file)
 {
-        unsigned long p = *ppos;
         struct inode *inode = file->f_path.dentry->d_inode;
         int fbidx = iminor(inode);
         struct fb_info *info = registered_fb[fbidx];
-        u32 *buffer, *dst;
-        u32 __iomem *src;
-        int c, i, cnt = 0, err = 0;
+
+        if (info != file->private_data)
+                info = NULL;
+        return info;
+}
+
+static ssize_t
+fb_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
+{
+        unsigned long p = *ppos;
+        struct fb_info *info = file_fb_info(file);
+        u8 *buffer, *dst;
+        u8 __iomem *src;
+        int c, cnt = 0, err = 0;
         unsigned long total_size;
 
         if (!info || ! info->screen_base)
@@ -730,7 +774,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 +782,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;
@@ -769,12 +805,10 @@ static ssize_t
 fb_write(struct file *file, const char __user *buf, size_t count, loff_t *ppos)
 {
         unsigned long p = *ppos;
-        struct inode *inode = file->f_path.dentry->d_inode;
-        int fbidx = iminor(inode);
-        struct fb_info *info = registered_fb[fbidx];
-        u32 *buffer, *src;
-        u32 __iomem *dst;
-        int c, i, cnt = 0, err = 0;
+        struct fb_info *info = file_fb_info(file);
+        u8 *buffer, *src;
+        u8 __iomem *dst;
+        int c, cnt = 0, err = 0;
         unsigned long total_size;
 
         if (!info || !info->screen_base)
@@ -811,7 +845,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 +859,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 +901,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,
@@ -1054,11 +1078,11 @@ static long do_fb_ioctl(struct fb_info *info, unsigned int cmd,
                         return -EFAULT;
                 if (!lock_fb_info(info))
                         return -ENODEV;
-                acquire_console_sem();
+                console_lock();
                 info->flags |= FBINFO_MISC_USEREVENT;
                 ret = fb_set_var(info, &var);
                 info->flags &= ~FBINFO_MISC_USEREVENT;
-                release_console_sem();
+                console_unlock();
                 unlock_fb_info(info);
                 if (!ret && copy_to_user(argp, &var, sizeof(var)))
                         ret = -EFAULT;
@@ -1090,9 +1114,9 @@ static long do_fb_ioctl(struct fb_info *info, unsigned int cmd,
                         return -EFAULT;
                 if (!lock_fb_info(info))
                         return -ENODEV;
-                acquire_console_sem();
+                console_lock();
                 ret = fb_pan_display(info, &var);
-                release_console_sem();
+                console_unlock();
                 unlock_fb_info(info);
                 if (ret == 0 && copy_to_user(argp, &var, sizeof(var)))
                         return -EFAULT;
@@ -1137,11 +1161,11 @@ static long do_fb_ioctl(struct fb_info *info, unsigned int cmd,
         case FBIOBLANK:
                 if (!lock_fb_info(info))
                         return -ENODEV;
-                acquire_console_sem();
+                console_lock();
                 info->flags |= FBINFO_MISC_USEREVENT;
                 ret = fb_blank(info, arg);
                 info->flags &= ~FBINFO_MISC_USEREVENT;
-                release_console_sem();
+                console_unlock();
                 unlock_fb_info(info);
                 break;
         default:
@@ -1159,10 +1183,10 @@ static long do_fb_ioctl(struct fb_info *info, unsigned int cmd,
 
 static long fb_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
-        struct inode *inode = file->f_path.dentry->d_inode;
-        int fbidx = iminor(inode);
-        struct fb_info *info = registered_fb[fbidx];
+        struct fb_info *info = file_fb_info(file);
 
+        if (!info)
+                return -ENODEV;
         return do_fb_ioctl(info, cmd, arg);
 }
 
@@ -1283,12 +1307,13 @@ static int fb_get_fscreeninfo(struct fb_info *info, unsigned int cmd,
 static long fb_compat_ioctl(struct file *file, unsigned int cmd,
                             unsigned long arg)
 {
-        struct inode *inode = file->f_path.dentry->d_inode;
-        int fbidx = iminor(inode);
-        struct fb_info *info = registered_fb[fbidx];
-        struct fb_ops *fb = info->fbops;
+        struct fb_info *info = file_fb_info(file);
+        struct fb_ops *fb;
         long ret = -ENOIOCTLCMD;
 
+        if (!info)
+                return -ENODEV;
+        fb = info->fbops;
         switch(cmd) {
         case FBIOGET_VSCREENINFO:
         case FBIOPUT_VSCREENINFO:
@@ -1321,16 +1346,18 @@ static long fb_compat_ioctl(struct file *file, unsigned int cmd,
 static int
 fb_mmap(struct file *file, struct vm_area_struct * vma)
 {
-        int fbidx = iminor(file->f_path.dentry->d_inode);
-        struct fb_info *info = registered_fb[fbidx];
-        struct fb_ops *fb = info->fbops;
+        struct fb_info *info = file_fb_info(file);
+        struct fb_ops *fb;
         unsigned long off;
         unsigned long start;
         u32 len;
 
+        if (!info)
+                return -ENODEV;
         if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
                 return -EINVAL;
         off = vma->vm_pgoff << PAGE_SHIFT;
+        fb = info->fbops;
         if (!fb)
                 return -ENODEV;
         mutex_lock(&info->mm_lock);
@@ -1379,14 +1406,16 @@ __releases(&info->lock)
         struct fb_info *info;
         int res = 0;
 
-        if (fbidx >= FB_MAX)
-                return -ENODEV;
-        info = registered_fb[fbidx];
-        if (!info)
+        info = get_fb_info(fbidx);
+        if (!info) {
                 request_module("fb%d", fbidx);
-        info = registered_fb[fbidx];
-        if (!info)
-                return -ENODEV;
+                info = get_fb_info(fbidx);
+                if (!info)
+                        return -ENODEV;
+        }
+        if (IS_ERR(info))
+                return PTR_ERR(info);
+
         mutex_lock(&info->lock);
         if (!try_module_get(info->fbops->owner)) {
                 res = -ENODEV;
@@ -1404,6 +1433,8 @@ __releases(&info->lock)
 #endif
 out:
         mutex_unlock(&info->lock);
+        if (res)
+                put_fb_info(info);
         return res;
 }
 
@@ -1419,6 +1450,7 @@ __releases(&info->lock)
                 info->fbops->fb_release(info,1);
         module_put(info->fbops->owner);
         mutex_unlock(&info->lock);
+        put_fb_info(info);
         return 0;
 }
 
@@ -1439,6 +1471,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;
@@ -1475,7 +1508,7 @@ static bool apertures_overlap(struct aperture *gen, struct aperture *hw)
         if (gen->base == hw->base)
                 return true;
         /* is the generic aperture base inside the hw base->hw base+size */
-        if (gen->base > hw->base && gen->base <= hw->base + hw->size)
+        if (gen->base > hw->base && gen->base < hw->base + hw->size)
                 return true;
         return false;
 }
@@ -1504,8 +1537,10 @@ static bool fb_do_apertures_overlap(struct apertures_struct *gena,
         return false;
 }
 
+static int do_unregister_framebuffer(struct fb_info *fb_info);
+
 #define VGA_FB_PHYS 0xA0000
-void remove_conflicting_framebuffers(struct apertures_struct *a,
+static void do_remove_conflicting_framebuffers(struct apertures_struct *a,
                                      const char *name, bool primary)
 {
         int i;
@@ -1524,46 +1559,35 @@ void remove_conflicting_framebuffers(struct apertures_struct *a,
                         (primary && gen_aper && gen_aper->count &&
                          gen_aper->ranges[0].base == VGA_FB_PHYS)) {
 
-                        printk(KERN_ERR "fb: conflicting fb hw usage "
+                        printk(KERN_INFO "fb: conflicting fb hw usage "
                                "%s vs %s - removing generic driver\n",
                                name, registered_fb[i]->fix.id);
-                        unregister_framebuffer(registered_fb[i]);
+                        do_unregister_framebuffer(registered_fb[i]);
                 }
         }
 }
-EXPORT_SYMBOL(remove_conflicting_framebuffers);
-
-/**
- *      register_framebuffer - registers a frame buffer device
- *      @fb_info: frame buffer info structure
- *
- *      Registers a frame buffer device @fb_info.
- *
- *      Returns negative errno on error, or zero for success.
- *
- */
 
-int
-register_framebuffer(struct fb_info *fb_info)
+static int do_register_framebuffer(struct fb_info *fb_info)
 {
         int i;
         struct fb_event event;
         struct fb_videomode mode;
 
-        if (num_registered_fb == FB_MAX)
-                return -ENXIO;
-
         if (fb_check_foreignness(fb_info))
                 return -ENOSYS;
 
-        remove_conflicting_framebuffers(fb_info->apertures, fb_info->fix.id,
+        do_remove_conflicting_framebuffers(fb_info->apertures, fb_info->fix.id,
                                          fb_is_primary_device(fb_info));
 
+        if (num_registered_fb == FB_MAX)
+                return -ENXIO;
+
         num_registered_fb++;
         for (i = 0 ; i < FB_MAX; i++)
                 if (!registered_fb[i])
                         break;
         fb_info->node = i;
+        atomic_set(&fb_info->count, 1);
         mutex_init(&fb_info->lock);
         mutex_init(&fb_info->mm_lock);
 
@@ -1609,36 +1633,14 @@ register_framebuffer(struct fb_info *fb_info)
         return 0;
 }
 
-
-/**
- *      unregister_framebuffer - releases a frame buffer device
- *      @fb_info: frame buffer info structure
- *
- *      Unregisters a frame buffer device @fb_info.
- *
- *      Returns negative errno on error, or zero for success.
- *
- *      This function will also notify the framebuffer console
- *      to release the driver.
- *
- *      This is meant to be called within a driver's module_exit()
- *      function. If this is called outside module_exit(), ensure
- *      that the driver implements fb_open() and fb_release() to
- *      check that no processes are using the device.
- */
-
-int
-unregister_framebuffer(struct fb_info *fb_info)
+static int do_unregister_framebuffer(struct fb_info *fb_info)
 {
         struct fb_event event;
         int i, ret = 0;
 
         i = fb_info->node;
-        if (!registered_fb[i]) {
-                ret = -EINVAL;
-                goto done;
-        }
-
+        if (i < 0 || i >= FB_MAX || registered_fb[i] != fb_info)
+                return -EINVAL;
 
         if (!lock_fb_info(fb_info))
                 return -ENODEV;
@@ -1646,16 +1648,14 @@ unregister_framebuffer(struct fb_info *fb_info)
         ret = fb_notifier_call_chain(FB_EVENT_FB_UNBIND, &event);
         unlock_fb_info(fb_info);
 
-        if (ret) {
-                ret = -EINVAL;
-                goto done;
-        }
+        if (ret)
+                return -EINVAL;
 
         if (fb_info->pixmap.addr &&
             (fb_info->pixmap.flags & FB_PIXMAP_DEFAULT))
                 kfree(fb_info->pixmap.addr);
         fb_destroy_modelist(&fb_info->modelist);
-        registered_fb[i]=NULL;
+        registered_fb[i] = NULL;
         num_registered_fb--;
         fb_cleanup_device(fb_info);
         device_destroy(fb_class, MKDEV(FB_MAJOR, i));
@@ -1663,9 +1663,65 @@ unregister_framebuffer(struct fb_info *fb_info)
         fb_notifier_call_chain(FB_EVENT_FB_UNREGISTERED, &event);
 
         /* this may free fb info */
-        if (fb_info->fbops->fb_destroy)
-                fb_info->fbops->fb_destroy(fb_info);
-done:
+        put_fb_info(fb_info);
+        return 0;
+}
+
+void remove_conflicting_framebuffers(struct apertures_struct *a,
+                                     const char *name, bool primary)
+{
+        mutex_lock(&registration_lock);
+        do_remove_conflicting_framebuffers(a, name, primary);
+        mutex_unlock(&registration_lock);
+}
+EXPORT_SYMBOL(remove_conflicting_framebuffers);
+
+/**
+ *      register_framebuffer - registers a frame buffer device
+ *      @fb_info: frame buffer info structure
+ *
+ *      Registers a frame buffer device @fb_info.
+ *
+ *      Returns negative errno on error, or zero for success.
+ *
+ */
+int
+register_framebuffer(struct fb_info *fb_info)
+{
+        int ret;
+
+        mutex_lock(&registration_lock);
+        ret = do_register_framebuffer(fb_info);
+        mutex_unlock(&registration_lock);
+
+        return ret;
+}
+
+/**
+ *      unregister_framebuffer - releases a frame buffer device
+ *      @fb_info: frame buffer info structure
+ *
+ *      Unregisters a frame buffer device @fb_info.
+ *
+ *      Returns negative errno on error, or zero for success.
+ *
+ *      This function will also notify the framebuffer console
+ *      to release the driver.
+ *
+ *      This is meant to be called within a driver's module_exit()
+ *      function. If this is called outside module_exit(), ensure
+ *      that the driver implements fb_open() and fb_release() to
+ *      check that no processes are using the device.
+ */
+int
+unregister_framebuffer(struct fb_info *fb_info)
+{
+        int ret;
+
+        mutex_lock(&registration_lock);
+        ret = do_unregister_framebuffer(fb_info);
+        mutex_unlock(&registration_lock);
+
         return ret;
 }
 
diff --git a/drivers/video/fbmon.c b/drivers/video/fbmon.c
index 563a98b88e9b..4f57485f8c54 100644
--- a/drivers/video/fbmon.c
+++ b/drivers/video/fbmon.c
@@ -973,6 +973,90 @@ void fb_edid_to_monspecs(unsigned char *edid, struct fb_monspecs *specs)
         DPRINTK("========================================\n");
 }
 
+/**
+ * fb_edid_add_monspecs() - add monitor video modes from E-EDID data
+ * @edid:       128 byte array with an E-EDID block
+ * @spacs:      monitor specs to be extended
+ */
+void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
+{
+        unsigned char *block;
+        struct fb_videomode *m;
+        int num = 0, i;
+        u8 svd[64], edt[(128 - 4) / DETAILED_TIMING_DESCRIPTION_SIZE];
+        u8 pos = 4, svd_n = 0;
+
+        if (!edid)
+                return;
+
+        if (!edid_checksum(edid))
+                return;
+
+        if (edid[0] != 0x2 ||
+            edid[2] < 4 || edid[2] > 128 - DETAILED_TIMING_DESCRIPTION_SIZE)
+                return;
+
+        DPRINTK("  Short Video Descriptors\n");
+
+        while (pos < edid[2]) {
+                u8 len = edid[pos] & 0x1f, type = (edid[pos] >> 5) & 7;
+                pr_debug("Data block %u of %u bytes\n", type, len);
+                if (type == 2)
+                        for (i = pos; i < pos + len; i++) {
+                                u8 idx = edid[pos + i] & 0x7f;
+                                svd[svd_n++] = idx;
+                                pr_debug("N%sative mode #%d\n",
+                                         edid[pos + i] & 0x80 ? "" : "on-n", idx);
+                        }
+                pos += len + 1;
+        }
+
+        block = edid + edid[2];
+
+        DPRINTK("  Extended Detailed Timings\n");
+
+        for (i = 0; i < (128 - edid[2]) / DETAILED_TIMING_DESCRIPTION_SIZE;
+             i++, block += DETAILED_TIMING_DESCRIPTION_SIZE)
+                if (PIXEL_CLOCK)
+                        edt[num++] = block - edid;
+
+        /* Yikes, EDID data is totally useless */
+        if (!(num + svd_n))
+                return;
+
+        m = kzalloc((specs->modedb_len + num + svd_n) *
+                       sizeof(struct fb_videomode), GFP_KERNEL);
+
+        if (!m)
+                return;
+
+        memcpy(m, specs->modedb, specs->modedb_len * sizeof(struct fb_videomode));
+
+        for (i = specs->modedb_len; i < specs->modedb_len + num; i++) {
+                get_detailed_timing(edid + edt[i - specs->modedb_len], &m[i]);
+                if (i == specs->modedb_len)
+                        m[i].flag |= FB_MODE_IS_FIRST;
+                pr_debug("Adding %ux%u@%u\n", m[i].xres, m[i].yres, m[i].refresh);
+        }
+
+        for (i = specs->modedb_len + num; i < specs->modedb_len + num + svd_n; i++) {
+                int idx = svd[i - specs->modedb_len - num];
+                if (!idx || idx > 63) {
+                        pr_warning("Reserved SVD code %d\n", idx);
+                } else if (idx > ARRAY_SIZE(cea_modes) || !cea_modes[idx].xres) {
+                        pr_warning("Unimplemented SVD code %d\n", idx);
+                } else {
+                        memcpy(&m[i], cea_modes + idx, sizeof(m[i]));
+                        pr_debug("Adding SVD #%d: %ux%u@%u\n", idx,
+                                 m[i].xres, m[i].yres, m[i].refresh);
+                }
+        }
+
+        kfree(specs->modedb);
+        specs->modedb = m;
+        specs->modedb_len = specs->modedb_len + num + svd_n;
+}
+
 /*
  * VESA Generalized Timing Formula (GTF)
  */
@@ -1289,6 +1373,9 @@ void fb_edid_to_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 {
         specs = NULL;
 }
+void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
+{
+}
 void fb_destroy_modedb(struct fb_videomode *modedb)
 {
 }
@@ -1396,6 +1483,7 @@ EXPORT_SYMBOL(fb_firmware_edid);
 
 EXPORT_SYMBOL(fb_parse_edid);
 EXPORT_SYMBOL(fb_edid_to_monspecs);
+EXPORT_SYMBOL(fb_edid_add_monspecs);
 EXPORT_SYMBOL(fb_get_mode);
 EXPORT_SYMBOL(fb_validate_mode);
 EXPORT_SYMBOL(fb_destroy_modedb);
diff --git a/drivers/video/fbsysfs.c b/drivers/video/fbsysfs.c
index 0a08f1341227..04251ce89184 100644
--- a/drivers/video/fbsysfs.c
+++ b/drivers/video/fbsysfs.c
@@ -33,7 +33,7 @@
  * for driver private data (info->par). info->par (if any) will be
  * aligned to sizeof(long).
  *
- * Returns the new structure, or NULL if an error occured.
+ * Returns the new structure, or NULL if an error occurred.
  *
  */
 struct fb_info *framebuffer_alloc(size_t size, struct device *dev)
@@ -90,11 +90,11 @@ static int activate(struct fb_info *fb_info, struct fb_var_screeninfo *var)
         int err;
 
         var->activate |= FB_ACTIVATE_FORCE;
-        acquire_console_sem();
+        console_lock();
         fb_info->flags |= FBINFO_MISC_USEREVENT;
         err = fb_set_var(fb_info, var);
         fb_info->flags &= ~FBINFO_MISC_USEREVENT;
-        release_console_sem();
+        console_unlock();
         if (err)
                 return err;
         return 0;
@@ -175,7 +175,7 @@ static ssize_t store_modes(struct device *device,
         if (i * sizeof(struct fb_videomode) != count)
                 return -EINVAL;
 
-        acquire_console_sem();
+        console_lock();
         list_splice(&fb_info->modelist, &old_list);
         fb_videomode_to_modelist((const struct fb_videomode *)buf, i,
                                  &fb_info->modelist);
@@ -185,7 +185,7 @@ static ssize_t store_modes(struct device *device,
         } else
                 fb_destroy_modelist(&old_list);
 
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -301,11 +301,11 @@ static ssize_t store_blank(struct device *device,
         char *last = NULL;
         int err;
 
-        acquire_console_sem();
+        console_lock();
         fb_info->flags |= FBINFO_MISC_USEREVENT;
         err = fb_blank(fb_info, simple_strtoul(buf, &last, 0));
         fb_info->flags &= ~FBINFO_MISC_USEREVENT;
-        release_console_sem();
+        console_unlock();
         if (err < 0)
                 return err;
         return count;
@@ -364,9 +364,9 @@ static ssize_t store_pan(struct device *device,
                 return -EINVAL;
         var.yoffset = simple_strtoul(last, &last, 0);
 
-        acquire_console_sem();
+        console_lock();
         err = fb_pan_display(fb_info, &var);
-        release_console_sem();
+        console_unlock();
 
         if (err < 0)
                 return err;
@@ -399,9 +399,9 @@ static ssize_t store_fbstate(struct device *device,
 
         state = simple_strtoul(buf, &last, 0);
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(fb_info, (int)state);
-        release_console_sem();
+        console_unlock();
 
         return count;
 }
diff --git a/drivers/video/ffb.c b/drivers/video/ffb.c
index 6739b2af3bc0..14102a3f70f5 100644
--- a/drivers/video/ffb.c
+++ b/drivers/video/ffb.c
@@ -893,8 +893,7 @@ static void ffb_init_fix(struct fb_info *info)
         info->fix.accel = FB_ACCEL_SUN_CREATOR;
 }
 
-static int __devinit ffb_probe(struct platform_device *op,
-                               const struct of_device_id *match)
+static int __devinit ffb_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct ffb_fbc __iomem *fbc;
@@ -1011,7 +1010,7 @@ out_dealloc_cmap:
         fb_dealloc_cmap(&info->cmap);
 
 out_unmap_dac:
-        of_iounmap(&op->resource[2], par->fbc, sizeof(struct ffb_fbc));
+        of_iounmap(&op->resource[1], par->dac, sizeof(struct ffb_dac));
 
 out_unmap_fbc:
         of_iounmap(&op->resource[2], par->fbc, sizeof(struct ffb_fbc));
@@ -1052,7 +1051,7 @@ static const struct of_device_id ffb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ffb_match);
 
-static struct of_platform_driver ffb_driver = {
+static struct platform_driver ffb_driver = {
         .driver = {
                 .name = "ffb",
                 .owner = THIS_MODULE,
@@ -1067,12 +1066,12 @@ static int __init ffb_init(void)
         if (fb_get_options("ffb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&ffb_driver);
+        return platform_driver_register(&ffb_driver);
 }
 
 static void __exit ffb_exit(void)
 {
-        of_unregister_platform_driver(&ffb_driver);
+        platform_driver_unregister(&ffb_driver);
 }
 
 module_init(ffb_init);
diff --git a/drivers/video/fm2fb.c b/drivers/video/fm2fb.c
index 1b0feb8e7244..d0533b7aad79 100644
--- a/drivers/video/fm2fb.c
+++ b/drivers/video/fm2fb.c
@@ -45,7 +45,7 @@
  *      buffer needs an amount of memory of 1.769.472 bytes which
  *      is near to 2 MByte (the allocated address space of Zorro2).
  *      The memory is channel interleaved. That means every channel
- *      owns four VRAMs. Unfortunatly most FrameMasters II are
+ *      owns four VRAMs. Unfortunately most FrameMasters II are
  *      not assembled with memory for the alpha channel. In this
  *      case it could be possible to add the frame buffer into the
  *      normal memory pool.
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 8bbbf08fa3ce..0acc7d65aeaa 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -555,8 +555,6 @@ static void adjust_aoi_size_position(struct fb_var_screeninfo *var,
 static int fsl_diu_check_var(struct fb_var_screeninfo *var,
                                 struct fb_info *info)
 {
-        unsigned long htotal, vtotal;
-
         pr_debug("check_var xres: %d\n", var->xres);
         pr_debug("check_var yres: %d\n", var->yres);
 
@@ -635,20 +633,6 @@ static int fsl_diu_check_var(struct fb_var_screeninfo *var,
 
                 break;
         }
-        /* If the pixclock is below the minimum spec'd value then set to
-         * refresh rate for 60Hz since this is supported by most monitors.
-         * Refer to Documentation/fb/ for calculations.
-         */
-        if ((var->pixclock < MIN_PIX_CLK) || (var->pixclock > MAX_PIX_CLK)) {
-                htotal = var->xres + var->right_margin + var->hsync_len +
-                    var->left_margin;
-                vtotal = var->yres + var->lower_margin + var->vsync_len +
-                    var->upper_margin;
-                var->pixclock = (vtotal * htotal * 6UL) / 100UL;
-                var->pixclock = KHZ2PICOS(var->pixclock);
-                pr_debug("pixclock set for 60Hz refresh = %u ps\n",
-                        var->pixclock);
-        }
 
         var->height = -1;
         var->width = -1;
@@ -882,7 +866,7 @@ static inline __u32 CNVT_TOHW(__u32 val, __u32 width)
  * which needs to be scaled in this function for the hardware. Things to take
  * into consideration are how many color registers, if any, are supported with
  * the current color visual. With truecolor mode no color palettes are
- * supported. Here a psuedo palette is created which we store the value in
+ * supported. Here a pseudo palette is created which we store the value in
  * pseudo_palette in struct fb_info. For pseudocolor mode we have a limited
  * color palette.
  */
@@ -1487,8 +1471,7 @@ static ssize_t show_monitor(struct device *device,
         return diu_ops.show_monitor_port(machine_data->monitor_port, buf);
 }
 
-static int __devinit fsl_diu_probe(struct platform_device *ofdev,
-        const struct of_device_id *match)
+static int __devinit fsl_diu_probe(struct platform_device *ofdev)
 {
         struct device_node *np = ofdev->dev.of_node;
         struct mfb_info *mfbi;
@@ -1735,7 +1718,7 @@ static struct of_device_id fsl_diu_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fsl_diu_match);
 
-static struct of_platform_driver fsl_diu_driver = {
+static struct platform_driver fsl_diu_driver = {
         .driver = {
                 .name = "fsl_diu",
                 .owner = THIS_MODULE,
@@ -1797,7 +1780,7 @@ static int __init fsl_diu_init(void)
         if (!coherence_data)
                 return -ENOMEM;
 #endif
-        ret = of_register_platform_driver(&fsl_diu_driver);
+        ret = platform_driver_register(&fsl_diu_driver);
         if (ret) {
                 printk(KERN_ERR
                         "fsl-diu: failed to register platform driver\n");
@@ -1811,7 +1794,7 @@ static int __init fsl_diu_init(void)
 
 static void __exit fsl_diu_exit(void)
 {
-        of_unregister_platform_driver(&fsl_diu_driver);
+        platform_driver_unregister(&fsl_diu_driver);
 #if defined(CONFIG_NOT_COHERENT_CACHE)
         vfree(coherence_data);
 #endif
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index ca3355e430bf..7e7b7a9ba274 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -721,7 +721,7 @@ static int gbefb_set_par(struct fb_info *info)
 
            Tiles have the advantage that they can be allocated individually in
            memory. However, this mapping is not linear at all, which is not
-           really convienient. In order to support linear addressing, the GBE
+           really convenient. In order to support linear addressing, the GBE
            DMA hardware is fooled into thinking the screen is only one tile
            large and but has a greater height, so that the DMA transfer covers
            the same region.
@@ -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/geode/gx1fb_core.c b/drivers/video/geode/gx1fb_core.c
index c6b554f72c6d..5a5d0928df33 100644
--- a/drivers/video/geode/gx1fb_core.c
+++ b/drivers/video/geode/gx1fb_core.c
@@ -29,7 +29,7 @@ static int  crt_option = 1;
 static char panel_option[32] = "";
 
 /* Modes relevant to the GX1 (taken from modedb.c) */
-static const struct fb_videomode __initdata gx1_modedb[] = {
+static const struct fb_videomode __devinitdata gx1_modedb[] = {
         /* 640x480-60 VESA */
         { NULL, 60, 640, 480, 39682,  48, 16, 33, 10, 96, 2,
           0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
@@ -195,7 +195,7 @@ static int gx1fb_blank(int blank_mode, struct fb_info *info)
         return par->vid_ops->blank_display(info, blank_mode);
 }
 
-static int __init gx1fb_map_video_memory(struct fb_info *info, struct pci_dev *dev)
+static int __devinit gx1fb_map_video_memory(struct fb_info *info, struct pci_dev *dev)
 {
         struct geodefb_par *par = info->par;
         unsigned gx_base;
@@ -268,7 +268,7 @@ static struct fb_ops gx1fb_ops = {
         .fb_imageblit   = cfb_imageblit,
 };
 
-static struct fb_info * __init gx1fb_init_fbinfo(struct device *dev)
+static struct fb_info * __devinit gx1fb_init_fbinfo(struct device *dev)
 {
         struct geodefb_par *par;
         struct fb_info *info;
@@ -318,7 +318,7 @@ static struct fb_info * __init gx1fb_init_fbinfo(struct device *dev)
         return info;
 }
 
-static int __init gx1fb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+static int __devinit gx1fb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
         struct geodefb_par *par;
         struct fb_info *info;
@@ -382,7 +382,7 @@ static int __init gx1fb_probe(struct pci_dev *pdev, const struct pci_device_id *
         return ret;
 }
 
-static void gx1fb_remove(struct pci_dev *pdev)
+static void __devexit gx1fb_remove(struct pci_dev *pdev)
 {
         struct fb_info *info = pci_get_drvdata(pdev);
         struct geodefb_par *par = info->par;
@@ -441,7 +441,7 @@ static struct pci_driver gx1fb_driver = {
         .name           = "gx1fb",
         .id_table       = gx1fb_id_table,
         .probe          = gx1fb_probe,
-        .remove         = gx1fb_remove,
+        .remove         = __devexit_p(gx1fb_remove),
 };
 
 static int __init gx1fb_init(void)
@@ -456,7 +456,7 @@ static int __init gx1fb_init(void)
         return pci_register_driver(&gx1fb_driver);
 }
 
-static void __exit gx1fb_cleanup(void)
+static void __devexit gx1fb_cleanup(void)
 {
         pci_unregister_driver(&gx1fb_driver);
 }
diff --git a/drivers/video/geode/gxfb_core.c b/drivers/video/geode/gxfb_core.c
index 70b1d9d51c96..b4f19db9bb54 100644
--- a/drivers/video/geode/gxfb_core.c
+++ b/drivers/video/geode/gxfb_core.c
@@ -344,10 +344,10 @@ static int gxfb_suspend(struct pci_dev *pdev, pm_message_t state)
         struct fb_info *info = pci_get_drvdata(pdev);
 
         if (state.event == PM_EVENT_SUSPEND) {
-                acquire_console_sem();
+                console_lock();
                 gx_powerdown(info);
                 fb_set_suspend(info, 1);
-                release_console_sem();
+                console_unlock();
         }
 
         /* there's no point in setting PCI states; we emulate PCI, so
@@ -361,7 +361,7 @@ static int gxfb_resume(struct pci_dev *pdev)
         struct fb_info *info = pci_get_drvdata(pdev);
         int ret;
 
-        acquire_console_sem();
+        console_lock();
         ret = gx_powerup(info);
         if (ret) {
                 printk(KERN_ERR "gxfb:  power up failed!\n");
@@ -369,7 +369,7 @@ static int gxfb_resume(struct pci_dev *pdev)
         }
 
         fb_set_suspend(info, 0);
-        release_console_sem();
+        console_unlock();
         return 0;
 }
 #endif
diff --git a/drivers/video/geode/lxfb.h b/drivers/video/geode/lxfb.h
index e4c4d89b7860..cfcd8090f313 100644
--- a/drivers/video/geode/lxfb.h
+++ b/drivers/video/geode/lxfb.h
@@ -22,6 +22,7 @@
 #define DC_HFILT_COUNT  0x100
 #define DC_VFILT_COUNT  0x100
 #define VP_COEFF_SIZE   0x1000
+#define VP_PAL_COUNT    0x100
 
 #define OUTPUT_CRT   0x01
 #define OUTPUT_PANEL 0x02
@@ -48,7 +49,8 @@ struct lxfb_par {
         uint64_t vp[VP_REG_COUNT];
         uint64_t fp[FP_REG_COUNT];
 
-        uint32_t pal[DC_PAL_COUNT];
+        uint32_t dc_pal[DC_PAL_COUNT];
+        uint32_t vp_pal[VP_PAL_COUNT];
         uint32_t hcoeff[DC_HFILT_COUNT * 2];
         uint32_t vcoeff[DC_VFILT_COUNT];
         uint32_t vp_coeff[VP_COEFF_SIZE / 4];
@@ -115,7 +117,7 @@ enum gp_registers {
 };
 
 #define GP_BLT_STATUS_CE                (1 << 4)        /* cmd buf empty */
-#define GP_BLT_STATUS_PB                (1 << 0)        /* primative busy */
+#define GP_BLT_STATUS_PB                (1 << 0)        /* primitive busy */
 
 
 /* Display Controller registers (table 6-47 from the data book) */
diff --git a/drivers/video/geode/lxfb_core.c b/drivers/video/geode/lxfb_core.c
index 39bdbedf43b4..416851ca8754 100644
--- a/drivers/video/geode/lxfb_core.c
+++ b/drivers/video/geode/lxfb_core.c
@@ -465,10 +465,10 @@ static int lxfb_suspend(struct pci_dev *pdev, pm_message_t state)
         struct fb_info *info = pci_get_drvdata(pdev);
 
         if (state.event == PM_EVENT_SUSPEND) {
-                acquire_console_sem();
+                console_lock();
                 lx_powerdown(info);
                 fb_set_suspend(info, 1);
-                release_console_sem();
+                console_unlock();
         }
 
         /* there's no point in setting PCI states; we emulate PCI, so
@@ -482,7 +482,7 @@ static int lxfb_resume(struct pci_dev *pdev)
         struct fb_info *info = pci_get_drvdata(pdev);
         int ret;
 
-        acquire_console_sem();
+        console_lock();
         ret = lx_powerup(info);
         if (ret) {
                 printk(KERN_ERR "lxfb:  power up failed!\n");
@@ -490,7 +490,7 @@ static int lxfb_resume(struct pci_dev *pdev)
         }
 
         fb_set_suspend(info, 0);
-        release_console_sem();
+        console_unlock();
         return 0;
 }
 #else
diff --git a/drivers/video/geode/lxfb_ops.c b/drivers/video/geode/lxfb_ops.c
index bc35a95e59d4..79e9abc72b83 100644
--- a/drivers/video/geode/lxfb_ops.c
+++ b/drivers/video/geode/lxfb_ops.c
@@ -276,10 +276,10 @@ static void lx_graphics_enable(struct fb_info *info)
                 write_fp(par, FP_PT1, 0);
                 temp = FP_PT2_SCRC;
 
-                if (info->var.sync & FB_SYNC_HOR_HIGH_ACT)
+                if (!(info->var.sync & FB_SYNC_HOR_HIGH_ACT))
                         temp |= FP_PT2_HSP;
 
-                if (info->var.sync & FB_SYNC_VERT_HIGH_ACT)
+                if (!(info->var.sync & FB_SYNC_VERT_HIGH_ACT))
                         temp |= FP_PT2_VSP;
 
                 write_fp(par, FP_PT2, temp);
@@ -610,10 +610,15 @@ static void lx_save_regs(struct lxfb_par *par)
         memcpy(par->vp, par->vp_regs, sizeof(par->vp));
         memcpy(par->fp, par->vp_regs + VP_FP_START, sizeof(par->fp));
 
-        /* save the palette */
+        /* save the display controller palette */
         write_dc(par, DC_PAL_ADDRESS, 0);
-        for (i = 0; i < ARRAY_SIZE(par->pal); i++)
-                par->pal[i] = read_dc(par, DC_PAL_DATA);
+        for (i = 0; i < ARRAY_SIZE(par->dc_pal); i++)
+                par->dc_pal[i] = read_dc(par, DC_PAL_DATA);
+
+        /* save the video processor palette */
+        write_vp(par, VP_PAR, 0);
+        for (i = 0; i < ARRAY_SIZE(par->vp_pal); i++)
+                par->vp_pal[i] = read_vp(par, VP_PDR);
 
         /* save the horizontal filter coefficients */
         filt = par->dc[DC_IRQ_FILT_CTL] | DC_IRQ_FILT_CTL_H_FILT_SEL;
@@ -706,8 +711,8 @@ static void lx_restore_display_ctlr(struct lxfb_par *par)
 
         /* restore the palette */
         write_dc(par, DC_PAL_ADDRESS, 0);
-        for (i = 0; i < ARRAY_SIZE(par->pal); i++)
-                write_dc(par, DC_PAL_DATA, par->pal[i]);
+        for (i = 0; i < ARRAY_SIZE(par->dc_pal); i++)
+                write_dc(par, DC_PAL_DATA, par->dc_pal[i]);
 
         /* restore the horizontal filter coefficients */
         filt = par->dc[DC_IRQ_FILT_CTL] | DC_IRQ_FILT_CTL_H_FILT_SEL;
@@ -751,6 +756,11 @@ static void lx_restore_video_proc(struct lxfb_par *par)
                 }
         }
 
+        /* restore video processor palette */
+        write_vp(par, VP_PAR, 0);
+        for (i = 0; i < ARRAY_SIZE(par->vp_pal); i++)
+                write_vp(par, VP_PDR, par->vp_pal[i]);
+
         /* restore video coeff ram */
         memcpy(par->vp_regs + VP_VCR, par->vp_coeff, sizeof(par->vp_coeff));
 }
diff --git a/drivers/video/hecubafb.c b/drivers/video/hecubafb.c
index c77bcc6ab463..614251a9af91 100644
--- a/drivers/video/hecubafb.c
+++ b/drivers/video/hecubafb.c
@@ -231,10 +231,9 @@ static int __devinit hecubafb_probe(struct platform_device *dev)
 
         videomemorysize = (DPY_W*DPY_H)/8;
 
-        if (!(videomemory = vmalloc(videomemorysize)))
-                return retval;
-
-        memset(videomemory, 0, videomemorysize);
+        videomemory = vzalloc(videomemorysize);
+        if (!videomemory)
+                goto err_videomem_alloc;
 
         info = framebuffer_alloc(sizeof(struct hecubafb_par), &dev->dev);
         if (!info)
@@ -276,6 +275,7 @@ err_fbreg:
         framebuffer_release(info);
 err_fballoc:
         vfree(videomemory);
+err_videomem_alloc:
         module_put(board->owner);
         return retval;
 }
@@ -299,7 +299,7 @@ static int __devexit hecubafb_remove(struct platform_device *dev)
 
 static struct platform_driver hecubafb_driver = {
         .probe  = hecubafb_probe,
-        .remove = hecubafb_remove,
+        .remove = __devexit_p(hecubafb_remove),
         .driver = {
                 .owner  = THIS_MODULE,
                 .name   = "hecubafb",
diff --git a/drivers/video/hgafb.c b/drivers/video/hgafb.c
index af8f0f2cc782..4052718eefaa 100644
--- a/drivers/video/hgafb.c
+++ b/drivers/video/hgafb.c
@@ -454,7 +454,6 @@ static int hgafb_blank(int blank_mode, struct fb_info *info)
 /*
  * Accel functions
  */
-#ifdef CONFIG_FB_HGA_ACCEL
 static void hgafb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 {
         u_int rows, y;
@@ -466,7 +465,7 @@ static void hgafb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
                 dest = rowaddr(info, y) + (rect->dx >> 3);
                 switch (rect->rop) {
                 case ROP_COPY:
-                        //fb_memset(dest, rect->color, (rect->width >> 3));
+                        memset_io(dest, rect->color, (rect->width >> 3));
                         break;
                 case ROP_XOR:
                         fb_writeb(~(fb_readb(dest)), dest);
@@ -488,7 +487,7 @@ static void hgafb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
                 for (rows = area->height; rows--; ) {
                         src = rowaddr(info, y1) + (area->sx >> 3);
                         dest = rowaddr(info, y2) + (area->dx >> 3);
-                        //fb_memmove(dest, src, (area->width >> 3));
+                        memmove(dest, src, (area->width >> 3));
                         y1++;
                         y2++;
                 }
@@ -499,7 +498,7 @@ static void hgafb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
                 for (rows = area->height; rows--;) {
                         src = rowaddr(info, y1) + (area->sx >> 3);
                         dest = rowaddr(info, y2) + (area->dx >> 3);
-                        //fb_memmove(dest, src, (area->width >> 3));
+                        memmove(dest, src, (area->width >> 3));
                         y1--;
                         y2--;
                 }
@@ -511,20 +510,17 @@ static void hgafb_imageblit(struct fb_info *info, const struct fb_image *image)
         u8 __iomem *dest;
         u8 *cdat = (u8 *) image->data;
         u_int rows, y = image->dy;
+        u_int x;
         u8 d;
 
         for (rows = image->height; rows--; y++) {
-                d = *cdat++;
-                dest = rowaddr(info, y) + (image->dx >> 3);
-                fb_writeb(d, dest);
+                for (x = 0; x < image->width; x+= 8) {
+                        d = *cdat++;
+                        dest = rowaddr(info, y) + ((image->dx + x)>> 3);
+                        fb_writeb(d, dest);
+                }
         }
 }
-#else /* !CONFIG_FB_HGA_ACCEL */
-#define hgafb_fillrect cfb_fillrect
-#define hgafb_copyarea cfb_copyarea
-#define hgafb_imageblit cfb_imageblit
-#endif /* CONFIG_FB_HGA_ACCEL */
-
 
 static struct fb_ops hgafb_ops = {
         .owner          = THIS_MODULE,
diff --git a/drivers/video/hpfb.c b/drivers/video/hpfb.c
index c8e280f1bb0b..ebf8495ff198 100644
--- a/drivers/video/hpfb.c
+++ b/drivers/video/hpfb.c
@@ -321,11 +321,11 @@ static int __devinit hpfb_dio_probe(struct dio_dev * d, const struct dio_device_
         unsigned long paddr, vaddr;
 
         paddr = d->resource.start;
-        if (!request_mem_region(d->resource.start, d->resource.end - d->resource.start, d->name))
+        if (!request_mem_region(d->resource.start, resource_size(&d->resource), d->name))
                 return -EBUSY;
 
         if (d->scode >= DIOII_SCBASE) {
-                vaddr = (unsigned long)ioremap(paddr, d->resource.end - d->resource.start);
+                vaddr = (unsigned long)ioremap(paddr, resource_size(&d->resource));
         } else {
                 vaddr = paddr + DIO_VIRADDRBASE;
         }
@@ -344,7 +344,7 @@ static void __devexit hpfb_remove_one(struct dio_dev *d)
         unregister_framebuffer(&fb_info);
         if (d->scode >= DIOII_SCBASE)
                 iounmap((void *)fb_regs);
-        release_mem_region(d->resource.start, d->resource.end - d->resource.start);
+        release_mem_region(d->resource.start, resource_size(&d->resource));
 }
 
 static struct dio_device_id hpfb_dio_tbl[] = {
diff --git a/drivers/video/i810/i810-i2c.c b/drivers/video/i810/i810-i2c.c
index cd2c728a809b..7db17d0d8a8c 100644
--- a/drivers/video/i810/i810-i2c.c
+++ b/drivers/video/i810/i810-i2c.c
@@ -45,8 +45,10 @@ static void i810i2c_setscl(void *data, int state)
         struct i810fb_par         *par = chan->par;
         u8                        __iomem *mmio = par->mmio_start_virtual;
 
-        i810_writel(mmio, chan->ddc_base, (state ? SCL_VAL_OUT : 0) | SCL_DIR |
-                    SCL_DIR_MASK | SCL_VAL_MASK);
+        if (state)
+                i810_writel(mmio, chan->ddc_base, SCL_DIR_MASK | SCL_VAL_MASK);
+        else
+                i810_writel(mmio, chan->ddc_base, SCL_DIR | SCL_DIR_MASK | SCL_VAL_MASK);
         i810_readl(mmio, chan->ddc_base);       /* flush posted write */
 }
 
@@ -56,8 +58,10 @@ static void i810i2c_setsda(void *data, int state)
         struct i810fb_par         *par = chan->par;
         u8                        __iomem *mmio = par->mmio_start_virtual;
 
-        i810_writel(mmio, chan->ddc_base, (state ? SDA_VAL_OUT : 0) | SDA_DIR |
-                    SDA_DIR_MASK | SDA_VAL_MASK);
+        if (state)
+                i810_writel(mmio, chan->ddc_base, SDA_DIR_MASK | SDA_VAL_MASK);
+        else
+                i810_writel(mmio, chan->ddc_base, SDA_DIR | SDA_DIR_MASK | SDA_VAL_MASK);
         i810_readl(mmio, chan->ddc_base);       /* flush posted write */
 }
 
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/i810/i810_accel.c b/drivers/video/i810/i810_accel.c
index f5bedee4310a..7672d2ea9b35 100644
--- a/drivers/video/i810/i810_accel.c
+++ b/drivers/video/i810/i810_accel.c
@@ -112,7 +112,7 @@ static inline int wait_for_engine_idle(struct fb_info *info)
  * @par: pointer to i810fb_par structure
  *
  * DESCRIPTION:
- * Checks/waits for sufficent space in ringbuffer of size
+ * Checks/waits for sufficient space in ringbuffer of size
  * space.  Returns the tail of the buffer
  */ 
 static inline u32 begin_iring(struct fb_info *info, u32 space)
diff --git a/drivers/video/i810/i810_main.c b/drivers/video/i810/i810_main.c
index 5743ea25e818..318f6fb895b2 100644
--- a/drivers/video/i810/i810_main.c
+++ b/drivers/video/i810/i810_main.c
@@ -1574,7 +1574,7 @@ static int i810fb_suspend(struct pci_dev *dev, pm_message_t mesg)
                 return 0;
         }
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(info, 1);
 
         if (info->fbops->fb_sync)
@@ -1587,7 +1587,7 @@ static int i810fb_suspend(struct pci_dev *dev, pm_message_t mesg)
         pci_save_state(dev);
         pci_disable_device(dev);
         pci_set_power_state(dev, pci_choose_state(dev, mesg));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1605,7 +1605,7 @@ static int i810fb_resume(struct pci_dev *dev)
                 return 0;
         }
 
-        acquire_console_sem();
+        console_lock();
         pci_set_power_state(dev, PCI_D0);
         pci_restore_state(dev);
 
@@ -1621,7 +1621,7 @@ static int i810fb_resume(struct pci_dev *dev)
         fb_set_suspend (info, 0);
         info->fbops->fb_blank(VESA_NO_BLANKING, info);
 fail:
-        release_console_sem();
+        console_unlock();
         return 0;
 }
 /***********************************************************************
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index 5c363d026f64..f135dbead07d 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -53,11 +53,8 @@
 #define LCDC_SIZE       0x04
 #define SIZE_XMAX(x)    ((((x) >> 4) & 0x3f) << 20)
 
-#ifdef CONFIG_ARCH_MX1
-#define SIZE_YMAX(y)    ((y) & 0x1ff)
-#else
-#define SIZE_YMAX(y)    ((y) & 0x3ff)
-#endif
+#define YMAX_MASK       (cpu_is_mx1() ? 0x1ff : 0x3ff)
+#define SIZE_YMAX(y)    ((y) & YMAX_MASK)
 
 #define LCDC_VPW        0x08
 #define VPW_VPW(x)      ((x) & 0x3ff)
@@ -68,12 +65,6 @@
 #define CPOS_OP         (1<<28)
 #define CPOS_CXP(x)     (((x) & 3ff) << 16)
 
-#ifdef CONFIG_ARCH_MX1
-#define CPOS_CYP(y)     ((y) & 0x1ff)
-#else
-#define CPOS_CYP(y)     ((y) & 0x3ff)
-#endif
-
 #define LCDC_LCWHB      0x10
 #define LCWHB_BK_EN     (1<<31)
 #define LCWHB_CW(w)     (((w) & 0x1f) << 24)
@@ -82,16 +73,6 @@
 
 #define LCDC_LCHCC      0x14
 
-#ifdef CONFIG_ARCH_MX1
-#define LCHCC_CUR_COL_R(r) (((r) & 0x1f) << 11)
-#define LCHCC_CUR_COL_G(g) (((g) & 0x3f) << 5)
-#define LCHCC_CUR_COL_B(b) ((b) & 0x1f)
-#else
-#define LCHCC_CUR_COL_R(r) (((r) & 0x3f) << 12)
-#define LCHCC_CUR_COL_G(g) (((g) & 0x3f) << 6)
-#define LCHCC_CUR_COL_B(b) ((b) & 0x3f)
-#endif
-
 #define LCDC_PCR        0x18
 
 #define LCDC_HCR        0x1C
@@ -118,11 +99,7 @@
 
 #define LCDC_RMCR       0x34
 
-#ifdef CONFIG_ARCH_MX1
-#define RMCR_LCDC_EN    (1<<1)
-#else
-#define RMCR_LCDC_EN    0
-#endif
+#define RMCR_LCDC_EN_MX1        (1<<1)
 
 #define RMCR_SELF_REF   (1<<0)
 
@@ -502,6 +479,7 @@ static void imxfb_init_backlight(struct imxfb_info *fbi)
 
         memset(&props, 0, sizeof(struct backlight_properties));
         props.max_brightness = 0xff;
+        props.type = BACKLIGHT_RAW;
         writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
 
         bl = backlight_device_register("imxfb-bl", &fbi->pdev->dev, fbi,
@@ -538,7 +516,11 @@ static void imxfb_enable_controller(struct imxfb_info *fbi)
         writel(readl(fbi->regs + LCDC_CPOS) & ~(CPOS_CC0 | CPOS_CC1),
                 fbi->regs + LCDC_CPOS);
 
-        writel(RMCR_LCDC_EN, fbi->regs + LCDC_RMCR);
+        /*
+         * RMCR_LCDC_EN_MX1 is present on i.MX1 only, but doesn't hurt
+         * on other SoCs
+         */
+        writel(RMCR_LCDC_EN_MX1, fbi->regs + LCDC_RMCR);
 
         clk_enable(fbi->clk);
 
@@ -623,7 +605,7 @@ static int imxfb_activate_var(struct fb_var_screeninfo *var, struct fb_info *inf
         if (var->right_margin > 255)
                 printk(KERN_ERR "%s: invalid right_margin %d\n",
                         info->fix.id, var->right_margin);
-        if (var->yres < 1 || var->yres > 511)
+        if (var->yres < 1 || var->yres > YMAX_MASK)
                 printk(KERN_ERR "%s: invalid yres %d\n",
                         info->fix.id, var->yres);
         if (var->vsync_len > 100)
@@ -874,10 +856,10 @@ failed_platform_init:
                 dma_free_writecombine(&pdev->dev,fbi->map_size,fbi->map_cpu,
                         fbi->map_dma);
 failed_map:
-        clk_put(fbi->clk);
-failed_getclock:
         iounmap(fbi->regs);
 failed_ioremap:
+        clk_put(fbi->clk);
+failed_getclock:
         release_mem_region(res->start, resource_size(res));
 failed_req:
         kfree(info->pseudo_palette);
@@ -977,6 +959,6 @@ static void __exit imxfb_cleanup(void)
 module_init(imxfb_init);
 module_exit(imxfb_cleanup);
 
-MODULE_DESCRIPTION("Motorola i.MX framebuffer driver");
+MODULE_DESCRIPTION("Freescale i.MX framebuffer driver");
 MODULE_AUTHOR("Sascha Hauer, Pengutronix");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/intelfb/Makefile b/drivers/video/intelfb/Makefile
index 6c782d3ae1be..f7d631ebee8e 100644
--- a/drivers/video/intelfb/Makefile
+++ b/drivers/video/intelfb/Makefile
@@ -4,7 +4,4 @@ intelfb-y := intelfbdrv.o intelfbhw.o
 intelfb-$(CONFIG_FB_INTEL_I2C) += intelfb_i2c.o
 intelfb-objs := $(intelfb-y)
 
-ifdef CONFIG_FB_INTEL_DEBUG
-#EXTRA_CFLAGS += -DDEBUG -DVERBOSE -DREGDUMP
-EXTRA_CFLAGS += -DDEBUG -DREGDUMP
-endif
+ccflags-$(CONFIG_FB_INTEL_DEBUG) := -DDEBUG -DREGDUMP
diff --git a/drivers/video/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
index 670ecaa0385a..de366937c933 100644
--- a/drivers/video/jz4740_fb.c
+++ b/drivers/video/jz4740_fb.c
@@ -778,9 +778,9 @@ static int jzfb_suspend(struct device *dev)
 {
         struct jzfb *jzfb = dev_get_drvdata(dev);
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(jzfb->fb, 1);
-        release_console_sem();
+        console_unlock();
 
         mutex_lock(&jzfb->lock);
         if (jzfb->is_enabled)
@@ -800,9 +800,9 @@ static int jzfb_resume(struct device *dev)
                 jzfb_enable(jzfb);
         mutex_unlock(&jzfb->lock);
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(jzfb->fb, 0);
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
diff --git a/drivers/video/kyro/STG4000OverlayDevice.c b/drivers/video/kyro/STG4000OverlayDevice.c
index a8c9713413e6..0aeeaa10708b 100644
--- a/drivers/video/kyro/STG4000OverlayDevice.c
+++ b/drivers/video/kyro/STG4000OverlayDevice.c
@@ -417,7 +417,7 @@ int SetOverlayViewPort(volatile STG4000REG __iomem *pSTGReg,
         /***************** Horizontal decimation/scaling ***************************/
 
         /*
-         * Now we handle the horizontal case, this is a simplified verison of
+         * Now we handle the horizontal case, this is a simplified version of
          * the vertical case in that we decimate by factors of 2.  as we are
          * working in words we should always be able to decimate by these
          * factors.  as we always have to have a buffer which is aligned to a
diff --git a/drivers/video/kyro/STG4000Reg.h b/drivers/video/kyro/STG4000Reg.h
index 244549e61368..5d6269882589 100644
--- a/drivers/video/kyro/STG4000Reg.h
+++ b/drivers/video/kyro/STG4000Reg.h
@@ -16,7 +16,7 @@
 
 /*
  * Macros that access memory mapped card registers in PCI space
- * Add an appropraite section for your OS or processor architecture.
+ * Add an appropriate section for your OS or processor architecture.
  */
 #if defined(__KERNEL__)
 #include <asm/page.h>
diff --git a/drivers/video/leo.c b/drivers/video/leo.c
index b599e5e36ced..9e946e2c1da9 100644
--- a/drivers/video/leo.c
+++ b/drivers/video/leo.c
@@ -547,8 +547,7 @@ static void leo_unmap_regs(struct platform_device *op, struct fb_info *info,
                 of_iounmap(&op->resource[0], info->screen_base, 0x800000);
 }
 
-static int __devinit leo_probe(struct platform_device *op,
-                               const struct of_device_id *match)
+static int __devinit leo_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -662,7 +661,7 @@ static const struct of_device_id leo_match[] = {
 };
 MODULE_DEVICE_TABLE(of, leo_match);
 
-static struct of_platform_driver leo_driver = {
+static struct platform_driver leo_driver = {
         .driver = {
                 .name = "leo",
                 .owner = THIS_MODULE,
@@ -677,12 +676,12 @@ static int __init leo_init(void)
         if (fb_get_options("leofb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&leo_driver);
+        return platform_driver_register(&leo_driver);
 }
 
 static void __exit leo_exit(void)
 {
-        of_unregister_platform_driver(&leo_driver);
+        platform_driver_unregister(&leo_driver);
 }
 
 module_init(leo_init);
diff --git a/drivers/video/matrox/matroxfb_DAC1064.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_DAC1064.h b/drivers/video/matrox/matroxfb_DAC1064.h
index c6ed7801efe2..1e6e45b57b78 100644
--- a/drivers/video/matrox/matroxfb_DAC1064.h
+++ b/drivers/video/matrox/matroxfb_DAC1064.h
@@ -46,7 +46,7 @@ void DAC1064_global_restore(struct matrox_fb_info *minfo);
 #define      M1064_XDVICLKCTRL_DVILOOPCTL       0x30
         /* CRTC2 pixel clock allowed to(0)/blocked from(1) driving CRTC2 */
 #define      M1064_XDVICLKCTRL_C2DVICLKEN       0x40
-        /* P1PLL loop filter bandwith selection */
+        /* P1PLL loop filter bandwidth selection */
 #define      M1064_XDVICLKCTRL_P1LOOPBWDTCTL    0x80
 #define M1064_XCURCOL0RED       0x08
 #define M1064_XCURCOL0GREEN     0x09
diff --git a/drivers/video/matrox/matroxfb_Ti3026.c b/drivers/video/matrox/matroxfb_Ti3026.c
index 835aaaae6b96..9a44cec394b5 100644
--- a/drivers/video/matrox/matroxfb_Ti3026.c
+++ b/drivers/video/matrox/matroxfb_Ti3026.c
@@ -387,7 +387,7 @@ static int Ti3026_init(struct matrox_fb_info *minfo, struct my_timming *m)
                         hw->DACreg[POS3026_XMISCCTRL] = TVP3026_XMISCCTRL_DAC_PUP | TVP3026_XMISCCTRL_DAC_8BIT | TVP3026_XMISCCTRL_PSEL_DIS | TVP3026_XMISCCTRL_PSEL_LOW;
                         break;
                 case 16:
-                        /* XLATCHCTRL should be _4_1 / _2_1... Why is not? (_2_1 is used everytime) */
+                        /* XLATCHCTRL should be _4_1 / _2_1... Why is not? (_2_1 is used every time) */
                         hw->DACreg[POS3026_XTRUECOLORCTRL] = (minfo->fbcon.var.green.length == 5) ? (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_ORGB_1555) : (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_RGB_565);
                         hw->DACreg[POS3026_XMUXCTRL] = muxctrl | TVP3026_XMUXCTRL_PIXEL_16BIT;
                         hw->DACreg[POS3026_XCLKCTRL] = TVP3026_XCLKCTRL_SRC_PLL | TVP3026_XCLKCTRL_DIV2;
@@ -399,7 +399,7 @@ static int Ti3026_init(struct matrox_fb_info *minfo, struct my_timming *m)
                         hw->DACreg[POS3026_XCLKCTRL] = TVP3026_XCLKCTRL_SRC_PLL | TVP3026_XCLKCTRL_DIV4;
                         break;
                 case 32:
-                        /* XLATCHCTRL should be _2_1 / _1_1... Why is not? (_2_1 is used everytime) */
+                        /* XLATCHCTRL should be _2_1 / _1_1... Why is not? (_2_1 is used every time) */
                         hw->DACreg[POS3026_XMUXCTRL] = muxctrl | TVP3026_XMUXCTRL_PIXEL_32BIT;
                         break;
                 default:
diff --git a/drivers/video/matrox/matroxfb_base.c b/drivers/video/matrox/matroxfb_base.c
index 052dd9f0b760..44bf8d4a216b 100644
--- a/drivers/video/matrox/matroxfb_base.c
+++ b/drivers/video/matrox/matroxfb_base.c
@@ -101,8 +101,6 @@
 
 #include <linux/version.h>
 
-#define __OLD_VIDIOC_
-
 #include "matroxfb_base.h"
 #include "matroxfb_misc.h"
 #include "matroxfb_accel.h"
@@ -623,7 +621,7 @@ static int matroxfb_decode_var(const struct matrox_fb_info *minfo,
                 var->yoffset = var->yres_virtual - var->yres;
 
         if (bpp == 16 && var->green.length == 5) {
-                bpp--; /* an artifical value - 15 */
+                bpp--; /* an artificial value - 15 */
         }
 
         for (rgbt = table; rgbt->bpp < bpp; rgbt++);
@@ -1152,7 +1150,6 @@ static int matroxfb_ioctl(struct fb_info *info,
                                         return -EFAULT;
                                 return err;
                         }
-                case VIDIOC_S_CTRL_OLD:
                 case VIDIOC_S_CTRL:
                         {
                                 struct v4l2_control ctrl;
@@ -1247,46 +1244,46 @@ static struct { struct fb_bitfield red, green, blue, transp; int bits_per_pixel;
 };
 
 /* initialized by setup, see explanation at end of file (search for MODULE_PARM_DESC) */
-static unsigned int mem;                /* "matrox:mem:xxxxxM" */
+static unsigned int mem;                /* "matroxfb:mem:xxxxxM" */
 static int option_precise_width = 1;    /* cannot be changed, option_precise_width==0 must imply noaccel */
-static int inv24;                       /* "matrox:inv24" */
-static int cross4MB = -1;               /* "matrox:cross4MB" */
-static int disabled;                    /* "matrox:disabled" */
-static int noaccel;                     /* "matrox:noaccel" */
-static int nopan;                       /* "matrox:nopan" */
-static int no_pci_retry;                /* "matrox:nopciretry" */
-static int novga;                       /* "matrox:novga" */
-static int nobios;                      /* "matrox:nobios" */
-static int noinit = 1;                  /* "matrox:init" */
-static int inverse;                     /* "matrox:inverse" */
-static int sgram;                       /* "matrox:sgram" */
+static int inv24;                       /* "matroxfb:inv24" */
+static int cross4MB = -1;               /* "matroxfb:cross4MB" */
+static int disabled;                    /* "matroxfb:disabled" */
+static int noaccel;                     /* "matroxfb:noaccel" */
+static int nopan;                       /* "matroxfb:nopan" */
+static int no_pci_retry;                /* "matroxfb:nopciretry" */
+static int novga;                       /* "matroxfb:novga" */
+static int nobios;                      /* "matroxfb:nobios" */
+static int noinit = 1;                  /* "matroxfb:init" */
+static int inverse;                     /* "matroxfb:inverse" */
+static int sgram;                       /* "matroxfb:sgram" */
 #ifdef CONFIG_MTRR
-static int mtrr = 1;                    /* "matrox:nomtrr" */
+static int mtrr = 1;                    /* "matroxfb:nomtrr" */
 #endif
-static int grayscale;                   /* "matrox:grayscale" */
-static int dev = -1;                    /* "matrox:dev:xxxxx" */
-static unsigned int vesa = ~0;          /* "matrox:vesa:xxxxx" */
-static int depth = -1;                  /* "matrox:depth:xxxxx" */
-static unsigned int xres;               /* "matrox:xres:xxxxx" */
-static unsigned int yres;               /* "matrox:yres:xxxxx" */
-static unsigned int upper = ~0;         /* "matrox:upper:xxxxx" */
-static unsigned int lower = ~0;         /* "matrox:lower:xxxxx" */
-static unsigned int vslen;              /* "matrox:vslen:xxxxx" */
-static unsigned int left = ~0;          /* "matrox:left:xxxxx" */
-static unsigned int right = ~0;         /* "matrox:right:xxxxx" */
-static unsigned int hslen;              /* "matrox:hslen:xxxxx" */
-static unsigned int pixclock;           /* "matrox:pixclock:xxxxx" */
-static int sync = -1;                   /* "matrox:sync:xxxxx" */
-static unsigned int fv;                 /* "matrox:fv:xxxxx" */
-static unsigned int fh;                 /* "matrox:fh:xxxxxk" */
-static unsigned int maxclk;             /* "matrox:maxclk:xxxxM" */
-static int dfp;                         /* "matrox:dfp */
-static int dfp_type = -1;               /* "matrox:dfp:xxx */
-static int memtype = -1;                /* "matrox:memtype:xxx" */
-static char outputs[8];                 /* "matrox:outputs:xxx" */
+static int grayscale;                   /* "matroxfb:grayscale" */
+static int dev = -1;                    /* "matroxfb:dev:xxxxx" */
+static unsigned int vesa = ~0;          /* "matroxfb:vesa:xxxxx" */
+static int depth = -1;                  /* "matroxfb:depth:xxxxx" */
+static unsigned int xres;               /* "matroxfb:xres:xxxxx" */
+static unsigned int yres;               /* "matroxfb:yres:xxxxx" */
+static unsigned int upper = ~0;         /* "matroxfb:upper:xxxxx" */
+static unsigned int lower = ~0;         /* "matroxfb:lower:xxxxx" */
+static unsigned int vslen;              /* "matroxfb:vslen:xxxxx" */
+static unsigned int left = ~0;          /* "matroxfb:left:xxxxx" */
+static unsigned int right = ~0;         /* "matroxfb:right:xxxxx" */
+static unsigned int hslen;              /* "matroxfb:hslen:xxxxx" */
+static unsigned int pixclock;           /* "matroxfb:pixclock:xxxxx" */
+static int sync = -1;                   /* "matroxfb:sync:xxxxx" */
+static unsigned int fv;                 /* "matroxfb:fv:xxxxx" */
+static unsigned int fh;                 /* "matroxfb:fh:xxxxxk" */
+static unsigned int maxclk;             /* "matroxfb:maxclk:xxxxM" */
+static int dfp;                         /* "matroxfb:dfp */
+static int dfp_type = -1;               /* "matroxfb:dfp:xxx */
+static int memtype = -1;                /* "matroxfb:memtype:xxx" */
+static char outputs[8];                 /* "matroxfb:outputs:xxx" */
 
 #ifndef MODULE
-static char videomode[64];              /* "matrox:mode:xxxxx" or "matrox:xxxxx" */
+static char videomode[64];              /* "matroxfb:mode:xxxxx" or "matroxfb:xxxxx" */
 #endif
 
 static int matroxfb_getmemory(struct matrox_fb_info *minfo,
@@ -1461,13 +1458,6 @@ static struct board {
                 MGA_G100,
                 &vbG100,
                 "MGA-G100 (AGP)"},
-        {PCI_VENDOR_ID_MATROX,  PCI_DEVICE_ID_MATROX_G200EV_PCI,        0xFF,
-                0,                      0,
-                DEVF_G200,
-                230000,
-                MGA_G200,
-                &vbG200,
-                "MGA-G200eV (PCI)"},
         {PCI_VENDOR_ID_MATROX,  PCI_DEVICE_ID_MATROX_G200_PCI,  0xFF,
                 0,                      0,
                 DEVF_G200,
@@ -2119,8 +2109,6 @@ static struct pci_device_id matroxfb_devices[] = {
                 PCI_ANY_ID,     PCI_ANY_ID,     0, 0, 0},
         {PCI_VENDOR_ID_MATROX,  PCI_DEVICE_ID_MATROX_G100_AGP,
                 PCI_ANY_ID,     PCI_ANY_ID,     0, 0, 0},
-        {PCI_VENDOR_ID_MATROX,  PCI_DEVICE_ID_MATROX_G200EV_PCI,
-                PCI_ANY_ID,     PCI_ANY_ID,     0, 0, 0},
         {PCI_VENDOR_ID_MATROX,  PCI_DEVICE_ID_MATROX_G200_PCI,
                 PCI_ANY_ID,     PCI_ANY_ID,     0, 0, 0},
         {PCI_VENDOR_ID_MATROX,  PCI_DEVICE_ID_MATROX_G200_AGP,
diff --git a/drivers/video/matrox/matroxfb_base.h b/drivers/video/matrox/matroxfb_base.h
index f96a471cb1a8..11ed57bb704e 100644
--- a/drivers/video/matrox/matroxfb_base.h
+++ b/drivers/video/matrox/matroxfb_base.h
@@ -12,7 +12,7 @@
 #undef MATROXFB_DEBUG
 
 /* heavy debugging: */
-/* -- logs putc[s], so everytime a char is displayed, it's logged */
+/* -- logs putc[s], so every time a char is displayed, it's logged */
 #undef MATROXFB_DEBUG_HEAVY
 
 /* This one _could_ cause infinite loops */
diff --git a/drivers/video/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/Makefile b/drivers/video/mb862xx/Makefile
index d7777714166b..5707ed0e31a7 100644
--- a/drivers/video/mb862xx/Makefile
+++ b/drivers/video/mb862xx/Makefile
@@ -2,4 +2,7 @@
 # Makefile for the MB862xx framebuffer driver
 #
 
-obj-$(CONFIG_FB_MB862XX)        := mb862xxfb.o mb862xxfb_accel.o
+obj-$(CONFIG_FB_MB862XX) += mb862xxfb.o
+
+mb862xxfb-y := mb862xxfbdrv.o mb862xxfb_accel.o
+mb862xxfb-$(CONFIG_FB_MB862XX_I2C) += mb862xx-i2c.o
diff --git a/drivers/video/mb862xx/mb862xx-i2c.c b/drivers/video/mb862xx/mb862xx-i2c.c
new file mode 100644
index 000000000000..b953099edd8e
--- /dev/null
+++ b/drivers/video/mb862xx/mb862xx-i2c.c
@@ -0,0 +1,178 @@
+/*
+ * Coral-P(A)/Lime I2C adapter driver
+ *
+ * (C) 2011 DENX Software Engineering, Anatolij Gustschin <agust@denx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/fb.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include "mb862xxfb.h"
+#include "mb862xx_reg.h"
+
+static int mb862xx_i2c_wait_event(struct i2c_adapter *adap)
+{
+        struct mb862xxfb_par *par = adap->algo_data;
+        u32 reg;
+
+        do {
+                udelay(1);
+                reg = inreg(i2c, GC_I2C_BCR);
+                if (reg & (I2C_INT | I2C_BER))
+                        break;
+        } while (1);
+
+        return (reg & I2C_BER) ? 0 : 1;
+}
+
+static int mb862xx_i2c_do_address(struct i2c_adapter *adap, int addr)
+{
+        struct mb862xxfb_par *par = adap->algo_data;
+
+        outreg(i2c, GC_I2C_DAR, addr);
+        outreg(i2c, GC_I2C_CCR, I2C_CLOCK_AND_ENABLE);
+        outreg(i2c, GC_I2C_BCR, par->i2c_rs ? I2C_REPEATED_START : I2C_START);
+        if (!mb862xx_i2c_wait_event(adap))
+                return -EIO;
+        par->i2c_rs = !(inreg(i2c, GC_I2C_BSR) & I2C_LRB);
+        return par->i2c_rs;
+}
+
+static int mb862xx_i2c_write_byte(struct i2c_adapter *adap, u8 byte)
+{
+        struct mb862xxfb_par *par = adap->algo_data;
+
+        outreg(i2c, GC_I2C_DAR, byte);
+        outreg(i2c, GC_I2C_BCR, I2C_START);
+        if (!mb862xx_i2c_wait_event(adap))
+                return -EIO;
+        return !(inreg(i2c, GC_I2C_BSR) & I2C_LRB);
+}
+
+static int mb862xx_i2c_read_byte(struct i2c_adapter *adap, u8 *byte, int last)
+{
+        struct mb862xxfb_par *par = adap->algo_data;
+
+        outreg(i2c, GC_I2C_BCR, I2C_START | (last ? 0 : I2C_ACK));
+        if (!mb862xx_i2c_wait_event(adap))
+                return 0;
+        *byte = inreg(i2c, GC_I2C_DAR);
+        return 1;
+}
+
+void mb862xx_i2c_stop(struct i2c_adapter *adap)
+{
+        struct mb862xxfb_par *par = adap->algo_data;
+
+        outreg(i2c, GC_I2C_BCR, I2C_STOP);
+        outreg(i2c, GC_I2C_CCR, I2C_DISABLE);
+        par->i2c_rs = 0;
+}
+
+static int mb862xx_i2c_read(struct i2c_adapter *adap, struct i2c_msg *m)
+{
+        int i, ret = 0;
+        int last = m->len - 1;
+
+        for (i = 0; i < m->len; i++) {
+                if (!mb862xx_i2c_read_byte(adap, &m->buf[i], i == last)) {
+                        ret = -EIO;
+                        break;
+                }
+        }
+        return ret;
+}
+
+static int mb862xx_i2c_write(struct i2c_adapter *adap, struct i2c_msg *m)
+{
+        int i, ret = 0;
+
+        for (i = 0; i < m->len; i++) {
+                if (!mb862xx_i2c_write_byte(adap, m->buf[i])) {
+                        ret = -EIO;
+                        break;
+                }
+        }
+        return ret;
+}
+
+static int mb862xx_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
+                        int num)
+{
+        struct mb862xxfb_par *par = adap->algo_data;
+        struct i2c_msg *m;
+        int addr;
+        int i = 0, err = 0;
+
+        dev_dbg(par->dev, "%s: %d msgs\n", __func__, num);
+
+        for (i = 0; i < num; i++) {
+                m = &msgs[i];
+                if (!m->len) {
+                        dev_dbg(par->dev, "%s: null msgs\n", __func__);
+                        continue;
+                }
+                addr = m->addr;
+                if (m->flags & I2C_M_RD)
+                        addr |= 1;
+
+                err = mb862xx_i2c_do_address(adap, addr);
+                if (err < 0)
+                        break;
+                if (m->flags & I2C_M_RD)
+                        err = mb862xx_i2c_read(adap, m);
+                else
+                        err = mb862xx_i2c_write(adap, m);
+        }
+
+        if (i)
+                mb862xx_i2c_stop(adap);
+
+        return (err < 0) ? err : i;
+}
+
+static u32 mb862xx_func(struct i2c_adapter *adap)
+{
+        return I2C_FUNC_SMBUS_BYTE_DATA;
+}
+
+static const struct i2c_algorithm mb862xx_algo = {
+        .master_xfer    = mb862xx_xfer,
+        .functionality  = mb862xx_func,
+};
+
+static struct i2c_adapter mb862xx_i2c_adapter = {
+        .name           = "MB862xx I2C adapter",
+        .algo           = &mb862xx_algo,
+        .owner          = THIS_MODULE,
+};
+
+int mb862xx_i2c_init(struct mb862xxfb_par *par)
+{
+        int ret;
+
+        mb862xx_i2c_adapter.algo_data = par;
+        par->adap = &mb862xx_i2c_adapter;
+
+        ret = i2c_add_adapter(par->adap);
+        if (ret < 0) {
+                dev_err(par->dev, "failed to add %s\n",
+                        mb862xx_i2c_adapter.name);
+        }
+        return ret;
+}
+
+void mb862xx_i2c_exit(struct mb862xxfb_par *par)
+{
+        if (par->adap) {
+                i2c_del_adapter(par->adap);
+                par->adap = NULL;
+        }
+}
diff --git a/drivers/video/mb862xx/mb862xx_reg.h b/drivers/video/mb862xx/mb862xx_reg.h
index 2ba65e118500..9df48b8edc94 100644
--- a/drivers/video/mb862xx/mb862xx_reg.h
+++ b/drivers/video/mb862xx/mb862xx_reg.h
@@ -5,11 +5,8 @@
 #ifndef _MB862XX_REG_H
 #define _MB862XX_REG_H
 
-#ifdef MB862XX_MMIO_BOTTOM
-#define MB862XX_MMIO_BASE       0x03fc0000
-#else
 #define MB862XX_MMIO_BASE       0x01fc0000
-#endif
+#define MB862XX_MMIO_HIGH_BASE  0x03fc0000
 #define MB862XX_I2C_BASE        0x0000c000
 #define MB862XX_DISP_BASE       0x00010000
 #define MB862XX_CAP_BASE        0x00018000
@@ -23,6 +20,7 @@
 #define GC_IMASK                0x00000024
 #define GC_SRST                 0x0000002c
 #define GC_CCF                  0x00000038
+#define GC_RSW                  0x0000005c
 #define GC_CID                  0x000000f0
 #define GC_REVISION             0x00000084
 
@@ -53,10 +51,16 @@
 #define GC_L0OA0                0x00000024
 #define GC_L0DA0                0x00000028
 #define GC_L0DY_L0DX            0x0000002c
+#define GC_L1M                  0x00000030
+#define GC_L1DA                 0x00000034
 #define GC_DCM1                 0x00000100
 #define GC_L0EM                 0x00000110
 #define GC_L0WY_L0WX            0x00000114
 #define GC_L0WH_L0WW            0x00000118
+#define GC_L1EM                 0x00000120
+#define GC_L1WY_L1WX            0x00000124
+#define GC_L1WH_L1WW            0x00000128
+#define GC_DLS                  0x00000180
 #define GC_DCM2                 0x00000104
 #define GC_DCM3                 0x00000108
 #define GC_CPM_CUTC             0x000000a0
@@ -68,6 +72,11 @@
 
 #define GC_CPM_CEN0             0x00100000
 #define GC_CPM_CEN1             0x00200000
+#define GC_DCM1_DEN             0x80000000
+#define GC_DCM1_L1E             0x00020000
+#define GC_L1M_16               0x80000000
+#define GC_L1M_YC               0x40000000
+#define GC_L1M_CS               0x20000000
 
 #define GC_DCM01_ESY            0x00000004
 #define GC_DCM01_SC             0x00003f00
@@ -79,9 +88,50 @@
 #define GC_L0M_L0C_16           0x80000000
 #define GC_L0EM_L0EC_24         0x40000000
 #define GC_L0M_L0W_UNIT         64
+#define GC_L1EM_DM              0x02000000
 
 #define GC_DISP_REFCLK_400      400
 
+/* I2C */
+#define GC_I2C_BSR              0x00000000      /* BSR */
+#define GC_I2C_BCR              0x00000004      /* BCR */
+#define GC_I2C_CCR              0x00000008      /* CCR */
+#define GC_I2C_ADR              0x0000000C      /* ADR */
+#define GC_I2C_DAR              0x00000010      /* DAR */
+
+#define I2C_DISABLE             0x00000000
+#define I2C_STOP                0x00000000
+#define I2C_START               0x00000010
+#define I2C_REPEATED_START      0x00000030
+#define I2C_CLOCK_AND_ENABLE    0x0000003f
+#define I2C_READY               0x01
+#define I2C_INT                 0x01
+#define I2C_INTE                0x02
+#define I2C_ACK                 0x08
+#define I2C_BER                 0x80
+#define I2C_BEIE                0x40
+#define I2C_TRX                 0x80
+#define I2C_LRB                 0x10
+
+/* Capture registers and bits */
+#define GC_CAP_VCM              0x00000000
+#define GC_CAP_CSC              0x00000004
+#define GC_CAP_VCS              0x00000008
+#define GC_CAP_CBM              0x00000010
+#define GC_CAP_CBOA             0x00000014
+#define GC_CAP_CBLA             0x00000018
+#define GC_CAP_IMG_START        0x0000001C
+#define GC_CAP_IMG_END          0x00000020
+#define GC_CAP_CMSS             0x00000048
+#define GC_CAP_CMDS             0x0000004C
+
+#define GC_VCM_VIE              0x80000000
+#define GC_VCM_CM               0x03000000
+#define GC_VCM_VS_PAL           0x00000002
+#define GC_CBM_OO               0x80000000
+#define GC_CBM_HRV              0x00000010
+#define GC_CBM_CBST             0x00000001
+
 /* Carmine specific */
 #define MB86297_DRAW_BASE               0x00020000
 #define MB86297_DISP0_BASE              0x00100000
diff --git a/drivers/video/mb862xx/mb862xxfb.h b/drivers/video/mb862xx/mb862xxfb.h
index d7e7cb76bbf2..8550630c1e01 100644
--- a/drivers/video/mb862xx/mb862xxfb.h
+++ b/drivers/video/mb862xx/mb862xxfb.h
@@ -1,6 +1,26 @@
 #ifndef __MB862XX_H__
 #define __MB862XX_H__
 
+struct mb862xx_l1_cfg {
+        unsigned short sx;
+        unsigned short sy;
+        unsigned short sw;
+        unsigned short sh;
+        unsigned short dx;
+        unsigned short dy;
+        unsigned short dw;
+        unsigned short dh;
+        int mirror;
+};
+
+#define MB862XX_BASE            'M'
+#define MB862XX_L1_GET_CFG      _IOR(MB862XX_BASE, 0, struct mb862xx_l1_cfg*)
+#define MB862XX_L1_SET_CFG      _IOW(MB862XX_BASE, 1, struct mb862xx_l1_cfg*)
+#define MB862XX_L1_ENABLE       _IOW(MB862XX_BASE, 2, int)
+#define MB862XX_L1_CAP_CTL      _IOW(MB862XX_BASE, 3, int)
+
+#ifdef __KERNEL__
+
 #define PCI_VENDOR_ID_FUJITSU_LIMITED   0x10cf
 #define PCI_DEVICE_ID_FUJITSU_CORALP    0x2019
 #define PCI_DEVICE_ID_FUJITSU_CORALPA   0x201e
@@ -38,6 +58,8 @@ struct mb862xxfb_par {
         void __iomem            *mmio_base;     /* remapped registers */
         size_t                  mapped_vram;    /* length of remapped vram */
         size_t                  mmio_len;       /* length of register region */
+        unsigned long           cap_buf;        /* capture buffers offset */
+        size_t                  cap_len;        /* length of capture buffers */
 
         void __iomem            *host;          /* relocatable reg. bases */
         void __iomem            *i2c;
@@ -57,11 +79,23 @@ struct mb862xxfb_par {
         unsigned int            refclk;         /* disp. reference clock */
         struct mb862xx_gc_mode  *gc_mode;       /* GDC mode init data */
         int                     pre_init;       /* don't init display if 1 */
+        struct i2c_adapter      *adap;          /* GDC I2C bus adapter */
+        int                     i2c_rs;
+
+        struct mb862xx_l1_cfg   l1_cfg;
+        int                     l1_stride;
 
         u32                     pseudo_palette[16];
 };
 
 extern void mb862xxfb_init_accel(struct fb_info *info, int xres);
+#ifdef CONFIG_FB_MB862XX_I2C
+extern int mb862xx_i2c_init(struct mb862xxfb_par *par);
+extern void mb862xx_i2c_exit(struct mb862xxfb_par *par);
+#else
+static inline int mb862xx_i2c_init(struct mb862xxfb_par *par) { return 0; }
+static inline void mb862xx_i2c_exit(struct mb862xxfb_par *par) { }
+#endif
 
 #if defined(CONFIG_FB_MB862XX_LIME) && defined(CONFIG_FB_MB862XX_PCI_GDC)
 #error  "Select Lime GDC or CoralP/Carmine support, but not both together"
@@ -82,4 +116,6 @@ extern void mb862xxfb_init_accel(struct fb_info *info, int xres);
 
 #define pack(a, b)      (((a) << 16) | (b))
 
+#endif /* __KERNEL__ */
+
 #endif
diff --git a/drivers/video/mb862xx/mb862xxfb.c b/drivers/video/mb862xx/mb862xxfbdrv.c
index b1c4374cf940..f70bd63b0187 100644
--- a/drivers/video/mb862xx/mb862xxfb.c
+++ b/drivers/video/mb862xx/mb862xxfbdrv.c
@@ -16,6 +16,7 @@
 
 #include <linux/fb.h>
 #include <linux/delay.h>
+#include <linux/uaccess.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/pci.h>
@@ -27,7 +28,7 @@
 
 #define NR_PALETTE              256
 #define MB862XX_MEM_SIZE        0x1000000
-#define CORALP_MEM_SIZE         0x4000000
+#define CORALP_MEM_SIZE         0x2000000
 #define CARMINE_MEM_SIZE        0x8000000
 #define DRV_NAME                "mb862xxfb"
 
@@ -309,6 +310,97 @@ static int mb862xxfb_blank(int mode, struct fb_info *fbi)
         return 0;
 }
 
+static int mb862xxfb_ioctl(struct fb_info *fbi, unsigned int cmd,
+                           unsigned long arg)
+{
+        struct mb862xxfb_par *par = fbi->par;
+        struct mb862xx_l1_cfg *l1_cfg = &par->l1_cfg;
+        void __user *argp = (void __user *)arg;
+        int *enable;
+        u32 l1em = 0;
+
+        switch (cmd) {
+        case MB862XX_L1_GET_CFG:
+                if (copy_to_user(argp, l1_cfg, sizeof(*l1_cfg)))
+                        return -EFAULT;
+                break;
+        case MB862XX_L1_SET_CFG:
+                if (copy_from_user(l1_cfg, argp, sizeof(*l1_cfg)))
+                        return -EFAULT;
+                if ((l1_cfg->sw >= l1_cfg->dw) && (l1_cfg->sh >= l1_cfg->dh)) {
+                        /* downscaling */
+                        outreg(cap, GC_CAP_CSC,
+                                pack((l1_cfg->sh << 11) / l1_cfg->dh,
+                                     (l1_cfg->sw << 11) / l1_cfg->dw));
+                        l1em = inreg(disp, GC_L1EM);
+                        l1em &= ~GC_L1EM_DM;
+                } else if ((l1_cfg->sw <= l1_cfg->dw) &&
+                           (l1_cfg->sh <= l1_cfg->dh)) {
+                        /* upscaling */
+                        outreg(cap, GC_CAP_CSC,
+                                pack((l1_cfg->sh << 11) / l1_cfg->dh,
+                                     (l1_cfg->sw << 11) / l1_cfg->dw));
+                        outreg(cap, GC_CAP_CMSS,
+                                pack(l1_cfg->sw >> 1, l1_cfg->sh));
+                        outreg(cap, GC_CAP_CMDS,
+                                pack(l1_cfg->dw >> 1, l1_cfg->dh));
+                        l1em = inreg(disp, GC_L1EM);
+                        l1em |= GC_L1EM_DM;
+                }
+
+                if (l1_cfg->mirror) {
+                        outreg(cap, GC_CAP_CBM,
+                                inreg(cap, GC_CAP_CBM) | GC_CBM_HRV);
+                        l1em |= l1_cfg->dw * 2 - 8;
+                } else {
+                        outreg(cap, GC_CAP_CBM,
+                                inreg(cap, GC_CAP_CBM) & ~GC_CBM_HRV);
+                        l1em &= 0xffff0000;
+                }
+                outreg(disp, GC_L1EM, l1em);
+                break;
+        case MB862XX_L1_ENABLE:
+                enable = (int *)arg;
+                if (*enable) {
+                        outreg(disp, GC_L1DA, par->cap_buf);
+                        outreg(cap, GC_CAP_IMG_START,
+                                pack(l1_cfg->sy >> 1, l1_cfg->sx));
+                        outreg(cap, GC_CAP_IMG_END,
+                                pack(l1_cfg->sh, l1_cfg->sw));
+                        outreg(disp, GC_L1M, GC_L1M_16 | GC_L1M_YC | GC_L1M_CS |
+                                             (par->l1_stride << 16));
+                        outreg(disp, GC_L1WY_L1WX,
+                                pack(l1_cfg->dy, l1_cfg->dx));
+                        outreg(disp, GC_L1WH_L1WW,
+                                pack(l1_cfg->dh - 1, l1_cfg->dw));
+                        outreg(disp, GC_DLS, 1);
+                        outreg(cap, GC_CAP_VCM,
+                                GC_VCM_VIE | GC_VCM_CM | GC_VCM_VS_PAL);
+                        outreg(disp, GC_DCM1, inreg(disp, GC_DCM1) |
+                                              GC_DCM1_DEN | GC_DCM1_L1E);
+                } else {
+                        outreg(cap, GC_CAP_VCM,
+                                inreg(cap, GC_CAP_VCM) & ~GC_VCM_VIE);
+                        outreg(disp, GC_DCM1,
+                                inreg(disp, GC_DCM1) & ~GC_DCM1_L1E);
+                }
+                break;
+        case MB862XX_L1_CAP_CTL:
+                enable = (int *)arg;
+                if (*enable) {
+                        outreg(cap, GC_CAP_VCM,
+                                inreg(cap, GC_CAP_VCM) | GC_VCM_VIE);
+                } else {
+                        outreg(cap, GC_CAP_VCM,
+                                inreg(cap, GC_CAP_VCM) & ~GC_VCM_VIE);
+                }
+                break;
+        default:
+                return -EINVAL;
+        }
+        return 0;
+}
+
 /* framebuffer ops */
 static struct fb_ops mb862xxfb_ops = {
         .owner          = THIS_MODULE,
@@ -320,6 +412,7 @@ static struct fb_ops mb862xxfb_ops = {
         .fb_fillrect    = cfb_fillrect,
         .fb_copyarea    = cfb_copyarea,
         .fb_imageblit   = cfb_imageblit,
+        .fb_ioctl       = mb862xxfb_ioctl,
 };
 
 /* initialize fb_info data */
@@ -328,6 +421,7 @@ static int mb862xxfb_init_fbinfo(struct fb_info *fbi)
         struct mb862xxfb_par *par = fbi->par;
         struct mb862xx_gc_mode *mode = par->gc_mode;
         unsigned long reg;
+        int stride;
 
         fbi->fbops = &mb862xxfb_ops;
         fbi->pseudo_palette = par->pseudo_palette;
@@ -336,7 +430,6 @@ static int mb862xxfb_init_fbinfo(struct fb_info *fbi)
 
         strcpy(fbi->fix.id, DRV_NAME);
         fbi->fix.smem_start = (unsigned long)par->fb_base_phys;
-        fbi->fix.smem_len = par->mapped_vram;
         fbi->fix.mmio_start = (unsigned long)par->mmio_base_phys;
         fbi->fix.mmio_len = par->mmio_len;
         fbi->fix.accel = FB_ACCEL_NONE;
@@ -420,6 +513,28 @@ static int mb862xxfb_init_fbinfo(struct fb_info *fbi)
                          FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
         fbi->fix.line_length = (fbi->var.xres_virtual *
                                 fbi->var.bits_per_pixel) / 8;
+        fbi->fix.smem_len = fbi->fix.line_length * fbi->var.yres_virtual;
+
+        /*
+         * reserve space for capture buffers and two cursors
+         * at the end of vram: 720x576 * 2 * 2.2 + 64x64 * 16.
+         */
+        par->cap_buf = par->mapped_vram - 0x1bd800 - 0x10000;
+        par->cap_len = 0x1bd800;
+        par->l1_cfg.sx = 0;
+        par->l1_cfg.sy = 0;
+        par->l1_cfg.sw = 720;
+        par->l1_cfg.sh = 576;
+        par->l1_cfg.dx = 0;
+        par->l1_cfg.dy = 0;
+        par->l1_cfg.dw = 720;
+        par->l1_cfg.dh = 576;
+        stride = par->l1_cfg.sw * (fbi->var.bits_per_pixel / 8);
+        par->l1_stride = stride / 64 + ((stride % 64) ? 1 : 0);
+        outreg(cap, GC_CAP_CBM, GC_CBM_OO | GC_CBM_CBST |
+                                (par->l1_stride << 16));
+        outreg(cap, GC_CAP_CBOA, par->cap_buf);
+        outreg(cap, GC_CAP_CBLA, par->cap_buf + par->cap_len);
         return 0;
 }
 
@@ -550,8 +665,7 @@ static int mb862xx_gdc_init(struct mb862xxfb_par *par)
         return 0;
 }
 
-static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev,
-                                               const struct of_device_id *id)
+static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev)
 {
         struct device_node *np = ofdev->dev.of_node;
         struct device *dev = &ofdev->dev;
@@ -717,7 +831,7 @@ static struct of_device_id __devinitdata of_platform_mb862xx_tbl[] = {
         { /* end */ }
 };
 
-static struct of_platform_driver of_platform_mb862xxfb_driver = {
+static struct platform_driver of_platform_mb862xxfb_driver = {
         .driver = {
                 .name = DRV_NAME,
                 .owner = THIS_MODULE,
@@ -743,22 +857,38 @@ static int coralp_init(struct mb862xxfb_par *par)
 
         par->refclk = GC_DISP_REFCLK_400;
 
+        if (par->mapped_vram >= 0x2000000) {
+                /* relocate gdc registers space */
+                writel(1, par->fb_base + MB862XX_MMIO_BASE + GC_RSW);
+                udelay(1); /* wait at least 20 bus cycles */
+        }
+
         ver = inreg(host, GC_CID);
         cn = (ver & GC_CID_CNAME_MSK) >> 8;
         ver = ver & GC_CID_VERSION_MSK;
         if (cn == 3) {
+                unsigned long reg;
+
                 dev_info(par->dev, "Fujitsu Coral-%s GDC Rev.%d found\n",\
                          (ver == 6) ? "P" : (ver == 8) ? "PA" : "?",
                          par->pdev->revision);
-                outreg(host, GC_CCF, GC_CCF_CGE_166 | GC_CCF_COT_133);
-                udelay(200);
-                outreg(host, GC_MMR, GC_MMR_CORALP_EVB_VAL);
-                udelay(10);
+                reg = inreg(disp, GC_DCM1);
+                if (reg & GC_DCM01_DEN && reg & GC_DCM01_L0E)
+                        par->pre_init = 1;
+
+                if (!par->pre_init) {
+                        outreg(host, GC_CCF, GC_CCF_CGE_166 | GC_CCF_COT_133);
+                        udelay(200);
+                        outreg(host, GC_MMR, GC_MMR_CORALP_EVB_VAL);
+                        udelay(10);
+                }
                 /* Clear interrupt status */
                 outreg(host, GC_IST, 0);
         } else {
                 return -ENODEV;
         }
+
+        mb862xx_i2c_init(par);
         return 0;
 }
 
@@ -900,7 +1030,13 @@ static int __devinit mb862xx_pci_probe(struct pci_dev *pdev,
         case PCI_DEVICE_ID_FUJITSU_CORALPA:
                 par->fb_base_phys = pci_resource_start(par->pdev, 0);
                 par->mapped_vram = CORALP_MEM_SIZE;
-                par->mmio_base_phys = par->fb_base_phys + MB862XX_MMIO_BASE;
+                if (par->mapped_vram >= 0x2000000) {
+                        par->mmio_base_phys = par->fb_base_phys +
+                                              MB862XX_MMIO_HIGH_BASE;
+                } else {
+                        par->mmio_base_phys = par->fb_base_phys +
+                                              MB862XX_MMIO_BASE;
+                }
                 par->mmio_len = MB862XX_MMIO_SIZE;
                 par->type = BT_CORALP;
                 break;
@@ -1010,6 +1146,8 @@ static void __devexit mb862xx_pci_remove(struct pci_dev *pdev)
                 outreg(host, GC_IMASK, 0);
         }
 
+        mb862xx_i2c_exit(par);
+
         device_remove_file(&pdev->dev, &dev_attr_dispregs);
 
         pci_set_drvdata(pdev, NULL);
@@ -1038,7 +1176,7 @@ static int __devinit mb862xxfb_init(void)
         int ret = -ENODEV;
 
 #if defined(CONFIG_FB_MB862XX_LIME)
-        ret = of_register_platform_driver(&of_platform_mb862xxfb_driver);
+        ret = platform_driver_register(&of_platform_mb862xxfb_driver);
 #endif
 #if defined(CONFIG_FB_MB862XX_PCI_GDC)
         ret = pci_register_driver(&mb862xxfb_pci_driver);
@@ -1049,7 +1187,7 @@ static int __devinit mb862xxfb_init(void)
 static void __exit mb862xxfb_exit(void)
 {
 #if defined(CONFIG_FB_MB862XX_LIME)
-        of_unregister_platform_driver(&of_platform_mb862xxfb_driver);
+        platform_driver_unregister(&of_platform_mb862xxfb_driver);
 #endif
 #if defined(CONFIG_FB_MB862XX_PCI_GDC)
         pci_unregister_driver(&mb862xxfb_pci_driver);
diff --git a/drivers/video/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..97d45e5115e2 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
@@ -628,12 +628,10 @@ static int __devinit metronomefb_probe(struct platform_device *dev)
         /* we need to add a spare page because our csum caching scheme walks
          * to the end of the page */
         videomemorysize = PAGE_SIZE + (fw * fh);
-        videomemory = vmalloc(videomemorysize);
+        videomemory = vzalloc(videomemorysize);
         if (!videomemory)
                 goto err_fb_rel;
 
-        memset(videomemory, 0, videomemorysize);
-
         info->screen_base = (char __force __iomem *)videomemory;
         info->fbops = &metronomefb_ops;
 
@@ -765,7 +763,7 @@ static int __devexit metronomefb_remove(struct platform_device *dev)
 
 static struct platform_driver metronomefb_driver = {
         .probe  = metronomefb_probe,
-        .remove = metronomefb_remove,
+        .remove = __devexit_p(metronomefb_remove),
         .driver = {
                 .owner  = THIS_MODULE,
                 .name   = "metronomefb",
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index 0a4dbdc1693a..cb175fe7abc0 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -32,252 +32,323 @@
 const char *fb_mode_option;
 EXPORT_SYMBOL_GPL(fb_mode_option);
 
-    /*
-     *  Standard video mode definitions (taken from XFree86)
-     */
+/*
+ *  Standard video mode definitions (taken from XFree86)
+ */
 
 static const struct fb_videomode modedb[] = {
-    {
+
         /* 640x400 @ 70 Hz, 31.5 kHz hsync */
-        NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 640x480 @ 60 Hz, 31.5 kHz hsync */
-        NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,     0,
+                FB_VMODE_NONINTERLACED },
+
         /* 800x600 @ 56 Hz, 35.15 kHz hsync */
-        NULL, 56, 800, 600, 27777, 128, 24, 22, 1, 72, 2,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 56, 800, 600, 27777, 128, 24, 22, 1, 72, 2,     0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1024x768 @ 87 Hz interlaced, 35.5 kHz hsync */
-        NULL, 87, 1024, 768, 22271, 56, 24, 33, 8, 160, 8,
-        0, FB_VMODE_INTERLACED
-    }, {
+        { NULL, 87, 1024, 768, 22271, 56, 24, 33, 8, 160, 8, 0,
+                FB_VMODE_INTERLACED },
+
         /* 640x400 @ 85 Hz, 37.86 kHz hsync */
-        NULL, 85, 640, 400, 31746, 96, 32, 41, 1, 64, 3,
-        FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 640, 400, 31746, 96, 32, 41, 1, 64, 3,
+                FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED },
+
         /* 640x480 @ 72 Hz, 36.5 kHz hsync */
-        NULL, 72, 640, 480, 31746, 144, 40, 30, 8, 40, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 72, 640, 480, 31746, 144, 40, 30, 8, 40, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 640x480 @ 75 Hz, 37.50 kHz hsync */
-        NULL, 75, 640, 480, 31746, 120, 16, 16, 1, 64, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 75, 640, 480, 31746, 120, 16, 16, 1, 64, 3,     0,
+                FB_VMODE_NONINTERLACED },
+
         /* 800x600 @ 60 Hz, 37.8 kHz hsync */
-        NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 640x480 @ 85 Hz, 43.27 kHz hsync */
-        NULL, 85, 640, 480, 27777, 80, 56, 25, 1, 56, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 640, 480, 27777, 80, 56, 25, 1, 56, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x864 @ 89 Hz interlaced, 44 kHz hsync */
-        NULL, 89, 1152, 864, 15384, 96, 16, 110, 1, 216, 10,
-        0, FB_VMODE_INTERLACED
-    }, {
+        { NULL, 89, 1152, 864, 15384, 96, 16, 110, 1, 216, 10, 0,
+                FB_VMODE_INTERLACED },
         /* 800x600 @ 72 Hz, 48.0 kHz hsync */
-        NULL, 72, 800, 600, 20000, 64, 56, 23, 37, 120, 6,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 72, 800, 600, 20000, 64, 56, 23, 37, 120, 6,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1024x768 @ 60 Hz, 48.4 kHz hsync */
-        NULL, 60, 1024, 768, 15384, 168, 8, 29, 3, 144, 6,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1024, 768, 15384, 168, 8, 29, 3, 144, 6, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 640x480 @ 100 Hz, 53.01 kHz hsync */
-        NULL, 100, 640, 480, 21834, 96, 32, 36, 8, 96, 6,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 100, 640, 480, 21834, 96, 32, 36, 8, 96, 6,     0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x864 @ 60 Hz, 53.5 kHz hsync */
-        NULL, 60, 1152, 864, 11123, 208, 64, 16, 4, 256, 8,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1152, 864, 11123, 208, 64, 16, 4, 256, 8, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 800x600 @ 85 Hz, 55.84 kHz hsync */
-        NULL, 85, 800, 600, 16460, 160, 64, 36, 16, 64, 5,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 800, 600, 16460, 160, 64, 36, 16, 64, 5, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1024x768 @ 70 Hz, 56.5 kHz hsync */
-        NULL, 70, 1024, 768, 13333, 144, 24, 29, 3, 136, 6,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 70, 1024, 768, 13333, 144, 24, 29, 3, 136, 6, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 87 Hz interlaced, 51 kHz hsync */
-        NULL, 87, 1280, 1024, 12500, 56, 16, 128, 1, 216, 12,
-        0, FB_VMODE_INTERLACED
-    }, {
+        { NULL, 87, 1280, 1024, 12500, 56, 16, 128, 1, 216, 12, 0,
+                FB_VMODE_INTERLACED },
+
         /* 800x600 @ 100 Hz, 64.02 kHz hsync */
-        NULL, 100, 800, 600, 14357, 160, 64, 30, 4, 64, 6,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 100, 800, 600, 14357, 160, 64, 30, 4, 64, 6, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1024x768 @ 76 Hz, 62.5 kHz hsync */
-        NULL, 76, 1024, 768, 11764, 208, 8, 36, 16, 120, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 76, 1024, 768, 11764, 208, 8, 36, 16, 120, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x864 @ 70 Hz, 62.4 kHz hsync */
-        NULL, 70, 1152, 864, 10869, 106, 56, 20, 1, 160, 10,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 70, 1152, 864, 10869, 106, 56, 20, 1, 160, 10, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 61 Hz, 64.2 kHz hsync */
-        NULL, 61, 1280, 1024, 9090, 200, 48, 26, 1, 184, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 61, 1280, 1024, 9090, 200, 48, 26, 1, 184, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1400x1050 @ 60Hz, 63.9 kHz hsync */
-        NULL, 60, 1400, 1050, 9259, 136, 40, 13, 1, 112, 3,
-        0, FB_VMODE_NONINTERLACED       
-    }, {
+        { NULL, 60, 1400, 1050, 9259, 136, 40, 13, 1, 112, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1400x1050 @ 75,107 Hz, 82,392 kHz +hsync +vsync*/
-        NULL, 75, 1400, 1050, 7190, 120, 56, 23, 10, 112, 13,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 75, 1400, 1050, 7190, 120, 56, 23, 10, 112, 13,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1400x1050 @ 60 Hz, ? kHz +hsync +vsync*/
-        NULL, 60, 1400, 1050, 9259, 128, 40, 12, 0, 112, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1400, 1050, 9259, 128, 40, 12, 0, 112, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1024x768 @ 85 Hz, 70.24 kHz hsync */
-        NULL, 85, 1024, 768, 10111, 192, 32, 34, 14, 160, 6,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 1024, 768, 10111, 192, 32, 34, 14, 160, 6, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x864 @ 78 Hz, 70.8 kHz hsync */
-        NULL, 78, 1152, 864, 9090, 228, 88, 32, 0, 84, 12,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 78, 1152, 864, 9090, 228, 88, 32, 0, 84, 12, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 70 Hz, 74.59 kHz hsync */
-        NULL, 70, 1280, 1024, 7905, 224, 32, 28, 8, 160, 8,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 70, 1280, 1024, 7905, 224, 32, 28, 8, 160, 8, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1600x1200 @ 60Hz, 75.00 kHz hsync */
-        NULL, 60, 1600, 1200, 6172, 304, 64, 46, 1, 192, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1600, 1200, 6172, 304, 64, 46, 1, 192, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x864 @ 84 Hz, 76.0 kHz hsync */
-        NULL, 84, 1152, 864, 7407, 184, 312, 32, 0, 128, 12,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 84, 1152, 864, 7407, 184, 312, 32, 0, 128, 12, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 74 Hz, 78.85 kHz hsync */
-        NULL, 74, 1280, 1024, 7407, 256, 32, 34, 3, 144, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 74, 1280, 1024, 7407, 256, 32, 34, 3, 144, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1024x768 @ 100Hz, 80.21 kHz hsync */
-        NULL, 100, 1024, 768, 8658, 192, 32, 21, 3, 192, 10,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 100, 1024, 768, 8658, 192, 32, 21, 3, 192, 10, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 76 Hz, 81.13 kHz hsync */
-        NULL, 76, 1280, 1024, 7407, 248, 32, 34, 3, 104, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 76, 1280, 1024, 7407, 248, 32, 34, 3, 104, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1600x1200 @ 70 Hz, 87.50 kHz hsync */
-        NULL, 70, 1600, 1200, 5291, 304, 64, 46, 1, 192, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 70, 1600, 1200, 5291, 304, 64, 46, 1, 192, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x864 @ 100 Hz, 89.62 kHz hsync */
-        NULL, 100, 1152, 864, 7264, 224, 32, 17, 2, 128, 19,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 100, 1152, 864, 7264, 224, 32, 17, 2, 128, 19, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 85 Hz, 91.15 kHz hsync */
-        NULL, 85, 1280, 1024, 6349, 224, 64, 44, 1, 160, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 1280, 1024, 6349, 224, 64, 44, 1, 160, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1600x1200 @ 75 Hz, 93.75 kHz hsync */
-        NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1680x1050 @ 60 Hz, 65.191 kHz hsync */
-        NULL, 60, 1680, 1050, 6848, 280, 104, 30, 3, 176, 6,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1680, 1050, 6848, 280, 104, 30, 3, 176, 6,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1600x1200 @ 85 Hz, 105.77 kHz hsync */
-        NULL, 85, 1600, 1200, 4545, 272, 16, 37, 4, 192, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 1600, 1200, 4545, 272, 16, 37, 4, 192, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x1024 @ 100 Hz, 107.16 kHz hsync */
-        NULL, 100, 1280, 1024, 5502, 256, 32, 26, 7, 128, 15,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 100, 1280, 1024, 5502, 256, 32, 26, 7, 128, 15, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1800x1440 @ 64Hz, 96.15 kHz hsync  */
-        NULL, 64, 1800, 1440, 4347, 304, 96, 46, 1, 192, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 64, 1800, 1440, 4347, 304, 96, 46, 1, 192, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1800x1440 @ 70Hz, 104.52 kHz hsync  */
-        NULL, 70, 1800, 1440, 4000, 304, 96, 46, 1, 192, 3,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 70, 1800, 1440, 4000, 304, 96, 46, 1, 192, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 512x384 @ 78 Hz, 31.50 kHz hsync */
-        NULL, 78, 512, 384, 49603, 48, 16, 16, 1, 64, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 78, 512, 384, 49603, 48, 16, 16, 1, 64, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 512x384 @ 85 Hz, 34.38 kHz hsync */
-        NULL, 85, 512, 384, 45454, 48, 16, 16, 1, 64, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 85, 512, 384, 45454, 48, 16, 16, 1, 64, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 320x200 @ 70 Hz, 31.5 kHz hsync, 8:5 aspect ratio */
-        NULL, 70, 320, 200, 79440, 16, 16, 20, 4, 48, 1,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 70, 320, 200, 79440, 16, 16, 20, 4, 48, 1, 0,
+                FB_VMODE_DOUBLE },
+
         /* 320x240 @ 60 Hz, 31.5 kHz hsync, 4:3 aspect ratio */
-        NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1, 0,
+                FB_VMODE_DOUBLE },
+
         /* 320x240 @ 72 Hz, 36.5 kHz hsync */
-        NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2, 0,
+                FB_VMODE_DOUBLE },
+
         /* 400x300 @ 56 Hz, 35.2 kHz hsync, 4:3 aspect ratio */
-        NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1, 0,
+                FB_VMODE_DOUBLE },
+
         /* 400x300 @ 60 Hz, 37.8 kHz hsync */
-        NULL, 60, 400, 300, 50000, 48, 16, 11, 1, 64, 2,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 60, 400, 300, 50000, 48, 16, 11, 1, 64, 2, 0,
+                FB_VMODE_DOUBLE },
+
         /* 400x300 @ 72 Hz, 48.0 kHz hsync */
-        NULL, 72, 400, 300, 40000, 32, 24, 11, 19, 64, 3,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 72, 400, 300, 40000, 32, 24, 11, 19, 64, 3,     0,
+                FB_VMODE_DOUBLE },
+
         /* 480x300 @ 56 Hz, 35.2 kHz hsync, 8:5 aspect ratio */
-        NULL, 56, 480, 300, 46176, 80, 16, 10, 1, 40, 1,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 56, 480, 300, 46176, 80, 16, 10, 1, 40, 1, 0,
+                FB_VMODE_DOUBLE },
+
         /* 480x300 @ 60 Hz, 37.8 kHz hsync */
-        NULL, 60, 480, 300, 41858, 56, 16, 11, 1, 80, 2,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 60, 480, 300, 41858, 56, 16, 11, 1, 80, 2, 0,
+                FB_VMODE_DOUBLE },
+
         /* 480x300 @ 63 Hz, 39.6 kHz hsync */
-        NULL, 63, 480, 300, 40000, 56, 16, 11, 1, 80, 2,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 63, 480, 300, 40000, 56, 16, 11, 1, 80, 2, 0,
+                FB_VMODE_DOUBLE },
+
         /* 480x300 @ 72 Hz, 48.0 kHz hsync */
-        NULL, 72, 480, 300, 33386, 40, 24, 11, 19, 80, 3,
-        0, FB_VMODE_DOUBLE
-    }, {
+        { NULL, 72, 480, 300, 33386, 40, 24, 11, 19, 80, 3, 0,
+                FB_VMODE_DOUBLE },
+
         /* 1920x1200 @ 60 Hz, 74.5 Khz hsync */
-        NULL, 60, 1920, 1200, 5177, 128, 336, 1, 38, 208, 3,
-        FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-        FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1920, 1200, 5177, 128, 336, 1, 38, 208, 3,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1152x768, 60 Hz, PowerBook G4 Titanium I and II */
-        NULL, 60, 1152, 768, 14047, 158, 26, 29, 3, 136, 6,
-        FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
+        { NULL, 60, 1152, 768, 14047, 158, 26, 29, 3, 136, 6,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED },
+
         /* 1366x768, 60 Hz, 47.403 kHz hsync, WXGA 16:9 aspect ratio */
-        NULL, 60, 1366, 768, 13806, 120, 10, 14, 3, 32, 5,
-        0, FB_VMODE_NONINTERLACED
-   }, {
+        { NULL, 60, 1366, 768, 13806, 120, 10, 14, 3, 32, 5, 0,
+                FB_VMODE_NONINTERLACED },
+
         /* 1280x800, 60 Hz, 47.403 kHz hsync, WXGA 16:10 aspect ratio */
-        NULL, 60, 1280, 800, 12048, 200, 64, 24, 1, 136, 3,
-        0, FB_VMODE_NONINTERLACED
-    }, {
-       /* 720x576i @ 50 Hz, 15.625 kHz hsync (PAL RGB) */
-       NULL, 50, 720, 576, 74074, 64, 16, 39, 5, 64, 5,
-       0, FB_VMODE_INTERLACED
-    }, {
-       /* 800x520i @ 50 Hz, 15.625 kHz hsync (PAL RGB) */
-       NULL, 50, 800, 520, 58823, 144, 64, 72, 28, 80, 5,
-       0, FB_VMODE_INTERLACED
-    },
+        { NULL, 60, 1280, 800, 12048, 200, 64, 24, 1, 136, 3, 0,
+                FB_VMODE_NONINTERLACED },
+
+        /* 720x576i @ 50 Hz, 15.625 kHz hsync (PAL RGB) */
+        { NULL, 50, 720, 576, 74074, 64, 16, 39, 5, 64, 5, 0,
+                FB_VMODE_INTERLACED },
+
+        /* 800x520i @ 50 Hz, 15.625 kHz hsync (PAL RGB) */
+        { NULL, 50, 800, 520, 58823, 144, 64, 72, 28, 80, 5, 0,
+                FB_VMODE_INTERLACED },
+
+        /* 864x480 @ 60 Hz, 35.15 kHz hsync */
+        { NULL, 60, 864, 480, 27777, 1, 1, 1, 1, 0, 0,
+                0, FB_VMODE_NONINTERLACED },
 };
 
 #ifdef CONFIG_FB_MODE_HELPERS
+const struct fb_videomode cea_modes[64] = {
+        /* #1: 640x480p@59.94/60Hz */
+        [1] = {
+                NULL, 60, 640, 480, 39722, 48, 16, 33, 10, 96, 2, 0,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+        /* #3: 720x480p@59.94/60Hz */
+        [3] = {
+                NULL, 60, 720, 480, 37037, 60, 16, 30, 9, 62, 6, 0,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+        /* #5: 1920x1080i@59.94/60Hz */
+        [5] = {
+                NULL, 60, 1920, 1080, 13763, 148, 88, 15, 2, 44, 5,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_INTERLACED, 0,
+        },
+        /* #7: 720(1440)x480iH@59.94/60Hz */
+        [7] = {
+                NULL, 60, 1440, 480, 18554/*37108*/, 114, 38, 15, 4, 124, 3, 0,
+                FB_VMODE_INTERLACED, 0,
+        },
+        /* #9: 720(1440)x240pH@59.94/60Hz */
+        [9] = {
+                NULL, 60, 1440, 240, 18554, 114, 38, 16, 4, 124, 3, 0,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+        /* #18: 720x576pH@50Hz */
+        [18] = {
+                NULL, 50, 720, 576, 37037, 68, 12, 39, 5, 64, 5, 0,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+        /* #19: 1280x720p@50Hz */
+        [19] = {
+                NULL, 50, 1280, 720, 13468, 220, 440, 20, 5, 40, 5,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+        /* #20: 1920x1080i@50Hz */
+        [20] = {
+                NULL, 50, 1920, 1080, 13480, 148, 528, 15, 5, 528, 5,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_INTERLACED, 0,
+        },
+        /* #32: 1920x1080p@23.98/24Hz */
+        [32] = {
+                NULL, 24, 1920, 1080, 13468, 148, 638, 36, 4, 44, 5,
+                FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+        /* #35: (2880)x480p4x@59.94/60Hz */
+        [35] = {
+                NULL, 60, 2880, 480, 9250, 240, 64, 30, 9, 248, 6, 0,
+                FB_VMODE_NONINTERLACED, 0,
+        },
+};
+
 const struct fb_videomode vesa_modes[] = {
         /* 0 640x350-85 VESA */
         { NULL, 85, 640, 350, 31746,  96, 32, 60, 32, 64, 3,
@@ -289,10 +360,10 @@ const struct fb_videomode vesa_modes[] = {
         { NULL, 85, 721, 400, 28169, 108, 36, 42, 01, 72, 3,
           FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
         /* 3 640x480-60 VESA */
-        { NULL, 60, 640, 480, 39682,  48, 16, 33, 10, 96, 2, 
+        { NULL, 60, 640, 480, 39682,  48, 16, 33, 10, 96, 2,
           0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
         /* 4 640x480-72 VESA */
-        { NULL, 72, 640, 480, 31746, 128, 24, 29, 9, 40, 2, 
+        { NULL, 72, 640, 480, 31746, 128, 24, 29, 9, 40, 2,
           0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
         /* 5 640x480-75 VESA */
         { NULL, 75, 640, 480, 31746, 120, 16, 16, 01, 64, 3,
@@ -375,7 +446,7 @@ const struct fb_videomode vesa_modes[] = {
           FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
           FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
         /* 26 1600x1200-75 VESA */
-        { NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3, 
+        { NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
           FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
           FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
         /* 27 1600x1200-85 VESA */
@@ -855,6 +926,7 @@ const struct fb_videomode *fb_find_nearest_mode(const struct fb_videomode *mode,
                         abs(cmode->yres - mode->yres);
                 if (diff > d) {
                         diff = d;
+                        diff_refresh = abs(cmode->refresh - mode->refresh);
                         best = cmode;
                 } else if (diff == d) {
                         d = abs(cmode->refresh - mode->refresh);
@@ -1056,3 +1128,4 @@ EXPORT_SYMBOL(fb_find_best_mode);
 EXPORT_SYMBOL(fb_find_nearest_mode);
 EXPORT_SYMBOL(fb_videomode_to_modelist);
 EXPORT_SYMBOL(fb_find_mode);
+EXPORT_SYMBOL(fb_find_mode_cvt);
diff --git a/drivers/video/msm/mddi.c b/drivers/video/msm/mddi.c
index 7c316c34dfca..178b0720bd79 100644
--- a/drivers/video/msm/mddi.c
+++ b/drivers/video/msm/mddi.c
@@ -318,7 +318,7 @@ static long mddi_wait_interrupt_timeout(struct mddi_info *mddi,
 static void mddi_wait_interrupt(struct mddi_info *mddi, uint32_t intmask)
 {
         if (mddi_wait_interrupt_timeout(mddi, intmask, HZ/10) == 0)
-                printk(KERN_INFO KERN_ERR "mddi_wait_interrupt %d, timeout "
+                printk(KERN_INFO "mddi_wait_interrupt %d, timeout "
                        "waiting for %x, INT = %x, STAT = %x gotint = %x\n",
                        current->pid, intmask, mddi_readl(INT), mddi_readl(STAT),
                        mddi->got_int);
@@ -465,8 +465,7 @@ static int __init mddi_get_client_caps(struct mddi_info *mddi)
 
                 if (mddi->flags & FLAG_HAVE_CAPS)
                         break;
-                printk(KERN_INFO KERN_ERR "mddi_init, timeout waiting for "
-                                "caps\n");
+                printk(KERN_INFO "mddi_init, timeout waiting for caps\n");
         }
         return mddi->flags & FLAG_HAVE_CAPS;
 }
@@ -680,7 +679,7 @@ static int __devinit mddi_probe(struct platform_device *pdev)
                 printk(KERN_ERR "mddi: no associated mem resource!\n");
                 return -ENOMEM;
         }
-        mddi->base = ioremap(resource->start, resource->end - resource->start);
+        mddi->base = ioremap(resource->start, resource_size(resource));
         if (!mddi->base) {
                 printk(KERN_ERR "mddi: failed to remap base!\n");
                 ret = -EINVAL;
diff --git a/drivers/video/msm/mdp.c b/drivers/video/msm/mdp.c
index 3c28db03ad39..c3636d55a3c5 100644
--- a/drivers/video/msm/mdp.c
+++ b/drivers/video/msm/mdp.c
@@ -90,7 +90,7 @@ static int locked_disable_mdp_irq(struct mdp_info *mdp, uint32_t mask)
         mdp_irq_mask &= ~(mask);
         /* if no one is waiting on the interrupt, disable it */
         if (!mdp_irq_mask) {
-                disable_irq(mdp->irq);
+                disable_irq_nosync(mdp->irq);
                 if (clk)
                         clk_disable(clk);
         }
@@ -482,6 +482,7 @@ int mdp_probe(struct platform_device *pdev)
         /* register mdp device */
         mdp->mdp_dev.dev.parent = &pdev->dev;
         mdp->mdp_dev.dev.class = mdp_class;
+        dev_set_name(&mdp->mdp_dev.dev, "mdp%d", pdev->id);
 
         /* if you can remove the platform device you'd have to implement
          * this:
diff --git a/drivers/video/msm/mdp_hw.h b/drivers/video/msm/mdp_hw.h
index 4e3deb4e592b..d80477415caa 100644
--- a/drivers/video/msm/mdp_hw.h
+++ b/drivers/video/msm/mdp_hw.h
@@ -449,6 +449,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 #define PPP_CFG_MDP_XRGB_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 #define PPP_CFG_MDP_RGBA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 #define PPP_CFG_MDP_BGRA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
+#define PPP_CFG_MDP_RGBX_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 
 #define PPP_CFG_MDP_Y_CBCR_H2V2(dir)   (PPP_##dir##_C2R_8BIT | \
                                         PPP_##dir##_C0G_8BIT | \
@@ -488,12 +489,14 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
         MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8)
 #define PPP_PACK_PATTERN_MDP_RGB_888 PPP_PACK_PATTERN_MDP_RGB_565
 #define PPP_PACK_PATTERN_MDP_XRGB_8888 \
-        MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
+        MDP_GET_PACK_PATTERN(CLR_B, CLR_G, CLR_R, CLR_ALPHA, 8)
 #define PPP_PACK_PATTERN_MDP_ARGB_8888 PPP_PACK_PATTERN_MDP_XRGB_8888
 #define PPP_PACK_PATTERN_MDP_RGBA_8888 \
         MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
 #define PPP_PACK_PATTERN_MDP_BGRA_8888 \
         MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
+#define PPP_PACK_PATTERN_MDP_RGBX_8888 \
+        MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
 #define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1 \
         MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8)
 #define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V2 PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1
@@ -509,6 +512,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 #define PPP_CHROMA_SAMP_MDP_ARGB_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_RGBA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_BGRA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
+#define PPP_CHROMA_SAMP_MDP_RGBX_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
 #define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V2(dir) PPP_OP_##dir##_CHROMA_420
 #define PPP_CHROMA_SAMP_MDP_Y_CRCB_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
@@ -523,6 +527,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
         [MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888,\
         [MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888,\
         [MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888,\
+        [MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888,\
         [MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1,\
         [MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2,\
         [MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1,\
@@ -536,6 +541,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
         [MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888(dir),\
         [MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888(dir),\
         [MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888(dir),\
+        [MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888(dir),\
         [MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1(dir),\
         [MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2(dir),\
         [MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1(dir),\
@@ -547,7 +553,8 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
                        (img == MDP_YCRYCB_H2V1))
 #define IS_RGB(img) ((img == MDP_RGB_565) | (img == MDP_RGB_888) | \
                      (img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
-                     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888))
+                     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888) | \
+                     (img == MDP_RGBX_8888))
 #define HAS_ALPHA(img) ((img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
                         (img == MDP_BGRA_8888))
 
diff --git a/drivers/video/msm/mdp_ppp.c b/drivers/video/msm/mdp_ppp.c
index 4ff001f4cbbd..2b6564e8bfea 100644
--- a/drivers/video/msm/mdp_ppp.c
+++ b/drivers/video/msm/mdp_ppp.c
@@ -69,6 +69,7 @@ static uint32_t bytes_per_pixel[] = {
         [MDP_ARGB_8888] = 4,
         [MDP_RGBA_8888] = 4,
         [MDP_BGRA_8888] = 4,
+        [MDP_RGBX_8888] = 4,
         [MDP_Y_CBCR_H2V1] = 1,
         [MDP_Y_CBCR_H2V2] = 1,
         [MDP_Y_CRCB_H2V1] = 1,
diff --git a/drivers/video/msm/msm_fb.c b/drivers/video/msm/msm_fb.c
index debe5933fd2e..ec351309e607 100644
--- a/drivers/video/msm/msm_fb.c
+++ b/drivers/video/msm/msm_fb.c
@@ -81,7 +81,6 @@ struct msmfb_info {
         spinlock_t update_lock;
         struct mutex panel_init_lock;
         wait_queue_head_t frame_wq;
-        struct workqueue_struct *resume_workqueue;
         struct work_struct resume_work;
         struct msmfb_callback dma_callback;
         struct msmfb_callback vsync_callback;
@@ -111,7 +110,7 @@ static void msmfb_handle_dma_interrupt(struct msmfb_callback *callback)
         if (msmfb->sleeping == UPDATING &&
             msmfb->frame_done == msmfb->update_frame) {
                 DLOG(SUSPEND_RESUME, "full update completed\n");
-                queue_work(msmfb->resume_workqueue, &msmfb->resume_work);
+                schedule_work(&msmfb->resume_work);
         }
         spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
         wake_up(&msmfb->frame_wq);
@@ -220,8 +219,8 @@ restart:
 
         sleeping = msmfb->sleeping;
         /* on a full update, if the last frame has not completed, wait for it */
-        if (pan_display && (msmfb->frame_requested != msmfb->frame_done ||
-                            sleeping == UPDATING)) {
+        if ((pan_display && msmfb->frame_requested != msmfb->frame_done) ||
+                            sleeping == UPDATING) {
                 int ret;
                 spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
                 ret = wait_event_interruptible_timeout(msmfb->frame_wq,
@@ -470,6 +469,18 @@ static void setup_fb_info(struct msmfb_info *msmfb)
         fb_info->var.yoffset = 0;
 
         if (msmfb->panel->caps & MSMFB_CAP_PARTIAL_UPDATES) {
+                /*
+                 * Set the param in the fixed screen, so userspace can't
+                 * change it. This will be used to check for the
+                 * capability.
+                 */
+                fb_info->fix.reserved[0] = 0x5444;
+                fb_info->fix.reserved[1] = 0x5055;
+
+                /*
+                 * This preloads the value so that if userspace doesn't
+                 * change it, it will be a full update
+                 */
                 fb_info->var.reserved[0] = 0x54445055;
                 fb_info->var.reserved[1] = 0;
                 fb_info->var.reserved[2] = (uint16_t)msmfb->xres |
@@ -559,12 +570,6 @@ static int msmfb_probe(struct platform_device *pdev)
         spin_lock_init(&msmfb->update_lock);
         mutex_init(&msmfb->panel_init_lock);
         init_waitqueue_head(&msmfb->frame_wq);
-        msmfb->resume_workqueue = create_workqueue("panel_on");
-        if (msmfb->resume_workqueue == NULL) {
-                printk(KERN_ERR "failed to create panel_on workqueue\n");
-                ret = -ENOMEM;
-                goto error_create_workqueue;
-        }
         INIT_WORK(&msmfb->resume_work, power_on_panel);
         msmfb->black = kzalloc(msmfb->fb->var.bits_per_pixel*msmfb->xres,
                                GFP_KERNEL);
@@ -589,8 +594,6 @@ static int msmfb_probe(struct platform_device *pdev)
         return 0;
 
 error_register_framebuffer:
-        destroy_workqueue(msmfb->resume_workqueue);
-error_create_workqueue:
         iounmap(fb->screen_base);
 error_setup_fbmem:
         framebuffer_release(msmfb->fb);
diff --git a/drivers/video/mx3fb.c b/drivers/video/mx3fb.c
index 7cfc170bce19..7e3a490e8d76 100644
--- a/drivers/video/mx3fb.c
+++ b/drivers/video/mx3fb.c
@@ -27,6 +27,7 @@
 #include <linux/clk.h>
 #include <linux/mutex.h>
 
+#include <mach/dma.h>
 #include <mach/hardware.h>
 #include <mach/ipu.h>
 #include <mach/mx3fb.h>
@@ -1176,9 +1177,9 @@ static int mx3fb_suspend(struct platform_device *pdev, pm_message_t state)
         struct mx3fb_data *mx3fb = platform_get_drvdata(pdev);
         struct mx3fb_info *mx3_fbi = mx3fb->fbi->par;
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(mx3fb->fbi, 1);
-        release_console_sem();
+        console_unlock();
 
         if (mx3_fbi->blank == FB_BLANK_UNBLANK) {
                 sdc_disable_channel(mx3_fbi);
@@ -1201,9 +1202,9 @@ static int mx3fb_resume(struct platform_device *pdev)
                 sdc_set_brightness(mx3fb, mx3fb->backlight_level);
         }
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(mx3fb->fbi, 0);
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1420,6 +1421,9 @@ static bool chan_filter(struct dma_chan *chan, void *arg)
         struct device *dev;
         struct mx3fb_platform_data *mx3fb_pdata;
 
+        if (!imx_dma_is_ipu(chan))
+                return false;
+
         if (!rq)
                 return false;
 
@@ -1470,8 +1474,7 @@ static int mx3fb_probe(struct platform_device *pdev)
                 goto eremap;
         }
 
-        pr_debug("Remapped %x to %x at %p\n", sdc_reg->start, sdc_reg->end,
-                 mx3fb->reg_base);
+        pr_debug("Remapped %pR at %p\n", sdc_reg, mx3fb->reg_base);
 
         /* IDMAC interface */
         dmaengine_get();
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
new file mode 100644
index 000000000000..0b2f2dd41416
--- /dev/null
+++ b/drivers/video/mxsfb.c
@@ -0,0 +1,919 @@
+/*
+ * Copyright (C) 2010 Juergen Beisert, Pengutronix
+ *
+ * This code is based on:
+ * Author: Vitaly Wool <vital@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#define DRIVER_NAME "mxsfb"
+
+/**
+ * @file
+ * @brief LCDIF driver for i.MX23 and i.MX28
+ *
+ * The LCDIF support four modes of operation
+ * - MPU interface (to drive smart displays) -> not supported yet
+ * - VSYNC interface (like MPU interface plus Vsync) -> not supported yet
+ * - Dotclock interface (to drive LC displays with RGB data and sync signals)
+ * - DVI (to drive ITU-R BT656)  -> not supported yet
+ *
+ * This driver depends on a correct setup of the pins used for this purpose
+ * (platform specific).
+ *
+ * For the developer: Don't forget to set the data bus width to the display
+ * in the imx_fb_videomode structure. You will else end up with ugly colours.
+ * If you fight against jitter you can vary the clock delay. This is a feature
+ * of the i.MX28 and you can vary it between 2 ns ... 8 ns in 2 ns steps. Give
+ * the required value in the imx_fb_videomode structure.
+ */
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <mach/mxsfb.h>
+
+#define REG_SET 4
+#define REG_CLR 8
+
+#define LCDC_CTRL                       0x00
+#define LCDC_CTRL1                      0x10
+#define LCDC_V4_CTRL2                   0x20
+#define LCDC_V3_TRANSFER_COUNT          0x20
+#define LCDC_V4_TRANSFER_COUNT          0x30
+#define LCDC_V4_CUR_BUF                 0x40
+#define LCDC_V4_NEXT_BUF                0x50
+#define LCDC_V3_CUR_BUF                 0x30
+#define LCDC_V3_NEXT_BUF                0x40
+#define LCDC_TIMING                     0x60
+#define LCDC_VDCTRL0                    0x70
+#define LCDC_VDCTRL1                    0x80
+#define LCDC_VDCTRL2                    0x90
+#define LCDC_VDCTRL3                    0xa0
+#define LCDC_VDCTRL4                    0xb0
+#define LCDC_DVICTRL0                   0xc0
+#define LCDC_DVICTRL1                   0xd0
+#define LCDC_DVICTRL2                   0xe0
+#define LCDC_DVICTRL3                   0xf0
+#define LCDC_DVICTRL4                   0x100
+#define LCDC_V4_DATA                    0x180
+#define LCDC_V3_DATA                    0x1b0
+#define LCDC_V4_DEBUG0                  0x1d0
+#define LCDC_V3_DEBUG0                  0x1f0
+
+#define CTRL_SFTRST                     (1 << 31)
+#define CTRL_CLKGATE                    (1 << 30)
+#define CTRL_BYPASS_COUNT               (1 << 19)
+#define CTRL_VSYNC_MODE                 (1 << 18)
+#define CTRL_DOTCLK_MODE                (1 << 17)
+#define CTRL_DATA_SELECT                (1 << 16)
+#define CTRL_SET_BUS_WIDTH(x)           (((x) & 0x3) << 10)
+#define CTRL_GET_BUS_WIDTH(x)           (((x) >> 10) & 0x3)
+#define CTRL_SET_WORD_LENGTH(x)         (((x) & 0x3) << 8)
+#define CTRL_GET_WORD_LENGTH(x)         (((x) >> 8) & 0x3)
+#define CTRL_MASTER                     (1 << 5)
+#define CTRL_DF16                       (1 << 3)
+#define CTRL_DF18                       (1 << 2)
+#define CTRL_DF24                       (1 << 1)
+#define CTRL_RUN                        (1 << 0)
+
+#define CTRL1_FIFO_CLEAR                (1 << 21)
+#define CTRL1_SET_BYTE_PACKAGING(x)     (((x) & 0xf) << 16)
+#define CTRL1_GET_BYTE_PACKAGING(x)     (((x) >> 16) & 0xf)
+
+#define TRANSFER_COUNT_SET_VCOUNT(x)    (((x) & 0xffff) << 16)
+#define TRANSFER_COUNT_GET_VCOUNT(x)    (((x) >> 16) & 0xffff)
+#define TRANSFER_COUNT_SET_HCOUNT(x)    ((x) & 0xffff)
+#define TRANSFER_COUNT_GET_HCOUNT(x)    ((x) & 0xffff)
+
+
+#define VDCTRL0_ENABLE_PRESENT          (1 << 28)
+#define VDCTRL0_VSYNC_ACT_HIGH          (1 << 27)
+#define VDCTRL0_HSYNC_ACT_HIGH          (1 << 26)
+#define VDCTRL0_DOTCLK_ACT_FAILING      (1 << 25)
+#define VDCTRL0_ENABLE_ACT_HIGH         (1 << 24)
+#define VDCTRL0_VSYNC_PERIOD_UNIT       (1 << 21)
+#define VDCTRL0_VSYNC_PULSE_WIDTH_UNIT  (1 << 20)
+#define VDCTRL0_HALF_LINE               (1 << 19)
+#define VDCTRL0_HALF_LINE_MODE          (1 << 18)
+#define VDCTRL0_SET_VSYNC_PULSE_WIDTH(x) ((x) & 0x3ffff)
+#define VDCTRL0_GET_VSYNC_PULSE_WIDTH(x) ((x) & 0x3ffff)
+
+#define VDCTRL2_SET_HSYNC_PERIOD(x)     ((x) & 0x3ffff)
+#define VDCTRL2_GET_HSYNC_PERIOD(x)     ((x) & 0x3ffff)
+
+#define VDCTRL3_MUX_SYNC_SIGNALS        (1 << 29)
+#define VDCTRL3_VSYNC_ONLY              (1 << 28)
+#define SET_HOR_WAIT_CNT(x)             (((x) & 0xfff) << 16)
+#define GET_HOR_WAIT_CNT(x)             (((x) >> 16) & 0xfff)
+#define SET_VERT_WAIT_CNT(x)            ((x) & 0xffff)
+#define GET_VERT_WAIT_CNT(x)            ((x) & 0xffff)
+
+#define VDCTRL4_SET_DOTCLK_DLY(x)       (((x) & 0x7) << 29) /* v4 only */
+#define VDCTRL4_GET_DOTCLK_DLY(x)       (((x) >> 29) & 0x7) /* v4 only */
+#define VDCTRL4_SYNC_SIGNALS_ON         (1 << 18)
+#define SET_DOTCLK_H_VALID_DATA_CNT(x)  ((x) & 0x3ffff)
+
+#define DEBUG0_HSYNC                    (1 < 26)
+#define DEBUG0_VSYNC                    (1 < 25)
+
+#define MIN_XRES                        120
+#define MIN_YRES                        120
+
+#define RED 0
+#define GREEN 1
+#define BLUE 2
+#define TRANSP 3
+
+enum mxsfb_devtype {
+        MXSFB_V3,
+        MXSFB_V4,
+};
+
+/* CPU dependent register offsets */
+struct mxsfb_devdata {
+        unsigned transfer_count;
+        unsigned cur_buf;
+        unsigned next_buf;
+        unsigned debug0;
+        unsigned hs_wdth_mask;
+        unsigned hs_wdth_shift;
+        unsigned ipversion;
+};
+
+struct mxsfb_info {
+        struct fb_info fb_info;
+        struct platform_device *pdev;
+        struct clk *clk;
+        void __iomem *base;     /* registers */
+        unsigned allocated_size;
+        int enabled;
+        unsigned ld_intf_width;
+        unsigned dotclk_delay;
+        const struct mxsfb_devdata *devdata;
+        int mapped;
+};
+
+#define mxsfb_is_v3(host) (host->devdata->ipversion == 3)
+#define mxsfb_is_v4(host) (host->devdata->ipversion == 4)
+
+static const struct mxsfb_devdata mxsfb_devdata[] = {
+        [MXSFB_V3] = {
+                .transfer_count = LCDC_V3_TRANSFER_COUNT,
+                .cur_buf = LCDC_V3_CUR_BUF,
+                .next_buf = LCDC_V3_NEXT_BUF,
+                .debug0 = LCDC_V3_DEBUG0,
+                .hs_wdth_mask = 0xff,
+                .hs_wdth_shift = 24,
+                .ipversion = 3,
+        },
+        [MXSFB_V4] = {
+                .transfer_count = LCDC_V4_TRANSFER_COUNT,
+                .cur_buf = LCDC_V4_CUR_BUF,
+                .next_buf = LCDC_V4_NEXT_BUF,
+                .debug0 = LCDC_V4_DEBUG0,
+                .hs_wdth_mask = 0x3fff,
+                .hs_wdth_shift = 18,
+                .ipversion = 4,
+        },
+};
+
+#define to_imxfb_host(x) (container_of(x, struct mxsfb_info, fb_info))
+
+/* mask and shift depends on architecture */
+static inline u32 set_hsync_pulse_width(struct mxsfb_info *host, unsigned val)
+{
+        return (val & host->devdata->hs_wdth_mask) <<
+                host->devdata->hs_wdth_shift;
+}
+
+static inline u32 get_hsync_pulse_width(struct mxsfb_info *host, unsigned val)
+{
+        return (val >> host->devdata->hs_wdth_shift) &
+                host->devdata->hs_wdth_mask;
+}
+
+static const struct fb_bitfield def_rgb565[] = {
+        [RED] = {
+                .offset = 11,
+                .length = 5,
+        },
+        [GREEN] = {
+                .offset = 5,
+                .length = 6,
+        },
+        [BLUE] = {
+                .offset = 0,
+                .length = 5,
+        },
+        [TRANSP] = {    /* no support for transparency */
+                .length = 0,
+        }
+};
+
+static const struct fb_bitfield def_rgb666[] = {
+        [RED] = {
+                .offset = 16,
+                .length = 6,
+        },
+        [GREEN] = {
+                .offset = 8,
+                .length = 6,
+        },
+        [BLUE] = {
+                .offset = 0,
+                .length = 6,
+        },
+        [TRANSP] = {    /* no support for transparency */
+                .length = 0,
+        }
+};
+
+static const struct fb_bitfield def_rgb888[] = {
+        [RED] = {
+                .offset = 16,
+                .length = 8,
+        },
+        [GREEN] = {
+                .offset = 8,
+                .length = 8,
+        },
+        [BLUE] = {
+                .offset = 0,
+                .length = 8,
+        },
+        [TRANSP] = {    /* no support for transparency */
+                .length = 0,
+        }
+};
+
+static inline unsigned chan_to_field(unsigned chan, struct fb_bitfield *bf)
+{
+        chan &= 0xffff;
+        chan >>= 16 - bf->length;
+        return chan << bf->offset;
+}
+
+static int mxsfb_check_var(struct fb_var_screeninfo *var,
+                struct fb_info *fb_info)
+{
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+        const struct fb_bitfield *rgb = NULL;
+
+        if (var->xres < MIN_XRES)
+                var->xres = MIN_XRES;
+        if (var->yres < MIN_YRES)
+                var->yres = MIN_YRES;
+
+        var->xres_virtual = var->xres;
+
+        var->yres_virtual = var->yres;
+
+        switch (var->bits_per_pixel) {
+        case 16:
+                /* always expect RGB 565 */
+                rgb = def_rgb565;
+                break;
+        case 32:
+                switch (host->ld_intf_width) {
+                case STMLCDIF_8BIT:
+                        pr_debug("Unsupported LCD bus width mapping\n");
+                        break;
+                case STMLCDIF_16BIT:
+                case STMLCDIF_18BIT:
+                        /* 24 bit to 18 bit mapping */
+                        rgb = def_rgb666;
+                        break;
+                case STMLCDIF_24BIT:
+                        /* real 24 bit */
+                        rgb = def_rgb888;
+                        break;
+                }
+                break;
+        default:
+                pr_debug("Unsupported colour depth: %u\n", var->bits_per_pixel);
+                return -EINVAL;
+        }
+
+        /*
+         * Copy the RGB parameters for this display
+         * from the machine specific parameters.
+         */
+        var->red    = rgb[RED];
+        var->green  = rgb[GREEN];
+        var->blue   = rgb[BLUE];
+        var->transp = rgb[TRANSP];
+
+        return 0;
+}
+
+static void mxsfb_enable_controller(struct fb_info *fb_info)
+{
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+        u32 reg;
+
+        dev_dbg(&host->pdev->dev, "%s\n", __func__);
+
+        clk_enable(host->clk);
+        clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
+
+        /* if it was disabled, re-enable the mode again */
+        writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_SET);
+
+        /* enable the SYNC signals first, then the DMA engine */
+        reg = readl(host->base + LCDC_VDCTRL4);
+        reg |= VDCTRL4_SYNC_SIGNALS_ON;
+        writel(reg, host->base + LCDC_VDCTRL4);
+
+        writel(CTRL_RUN, host->base + LCDC_CTRL + REG_SET);
+
+        host->enabled = 1;
+}
+
+static void mxsfb_disable_controller(struct fb_info *fb_info)
+{
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+        unsigned loop;
+        u32 reg;
+
+        dev_dbg(&host->pdev->dev, "%s\n", __func__);
+
+        /*
+         * Even if we disable the controller here, it will still continue
+         * until its FIFOs are running out of data
+         */
+        writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_CLR);
+
+        loop = 1000;
+        while (loop) {
+                reg = readl(host->base + LCDC_CTRL);
+                if (!(reg & CTRL_RUN))
+                        break;
+                loop--;
+        }
+
+        writel(VDCTRL4_SYNC_SIGNALS_ON, host->base + LCDC_VDCTRL4 + REG_CLR);
+
+        clk_disable(host->clk);
+
+        host->enabled = 0;
+}
+
+static int mxsfb_set_par(struct fb_info *fb_info)
+{
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+        u32 ctrl, vdctrl0, vdctrl4;
+        int line_size, fb_size;
+        int reenable = 0;
+
+        line_size =  fb_info->var.xres * (fb_info->var.bits_per_pixel >> 3);
+        fb_size = fb_info->var.yres_virtual * line_size;
+
+        if (fb_size > fb_info->fix.smem_len)
+                return -ENOMEM;
+
+        fb_info->fix.line_length = line_size;
+
+        /*
+         * It seems, you can't re-program the controller if it is still running.
+         * This may lead into shifted pictures (FIFO issue?).
+         * So, first stop the controller and drain its FIFOs
+         */
+        if (host->enabled) {
+                reenable = 1;
+                mxsfb_disable_controller(fb_info);
+        }
+
+        /* clear the FIFOs */
+        writel(CTRL1_FIFO_CLEAR, host->base + LCDC_CTRL1 + REG_SET);
+
+        ctrl = CTRL_BYPASS_COUNT | CTRL_MASTER |
+                CTRL_SET_BUS_WIDTH(host->ld_intf_width);
+
+        switch (fb_info->var.bits_per_pixel) {
+        case 16:
+                dev_dbg(&host->pdev->dev, "Setting up RGB565 mode\n");
+                ctrl |= CTRL_SET_WORD_LENGTH(0);
+                writel(CTRL1_SET_BYTE_PACKAGING(0xf), host->base + LCDC_CTRL1);
+                break;
+        case 32:
+                dev_dbg(&host->pdev->dev, "Setting up RGB888/666 mode\n");
+                ctrl |= CTRL_SET_WORD_LENGTH(3);
+                switch (host->ld_intf_width) {
+                case STMLCDIF_8BIT:
+                        dev_dbg(&host->pdev->dev,
+                                        "Unsupported LCD bus width mapping\n");
+                        return -EINVAL;
+                case STMLCDIF_16BIT:
+                case STMLCDIF_18BIT:
+                        /* 24 bit to 18 bit mapping */
+                        ctrl |= CTRL_DF24; /* ignore the upper 2 bits in
+                                            *  each colour component
+                                            */
+                        break;
+                case STMLCDIF_24BIT:
+                        /* real 24 bit */
+                        break;
+                }
+                /* do not use packed pixels = one pixel per word instead */
+                writel(CTRL1_SET_BYTE_PACKAGING(0x7), host->base + LCDC_CTRL1);
+                break;
+        default:
+                dev_dbg(&host->pdev->dev, "Unhandled color depth of %u\n",
+                                fb_info->var.bits_per_pixel);
+                return -EINVAL;
+        }
+
+        writel(ctrl, host->base + LCDC_CTRL);
+
+        writel(TRANSFER_COUNT_SET_VCOUNT(fb_info->var.yres) |
+                        TRANSFER_COUNT_SET_HCOUNT(fb_info->var.xres),
+                        host->base + host->devdata->transfer_count);
+
+        vdctrl0 = VDCTRL0_ENABLE_PRESENT |      /* always in DOTCLOCK mode */
+                VDCTRL0_VSYNC_PERIOD_UNIT |
+                VDCTRL0_VSYNC_PULSE_WIDTH_UNIT |
+                VDCTRL0_SET_VSYNC_PULSE_WIDTH(fb_info->var.vsync_len);
+        if (fb_info->var.sync & FB_SYNC_HOR_HIGH_ACT)
+                vdctrl0 |= VDCTRL0_HSYNC_ACT_HIGH;
+        if (fb_info->var.sync & FB_SYNC_VERT_HIGH_ACT)
+                vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
+        if (fb_info->var.sync & FB_SYNC_DATA_ENABLE_HIGH_ACT)
+                vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
+        if (fb_info->var.sync & FB_SYNC_DOTCLK_FAILING_ACT)
+                vdctrl0 |= VDCTRL0_DOTCLK_ACT_FAILING;
+
+        writel(vdctrl0, host->base + LCDC_VDCTRL0);
+
+        /* frame length in lines */
+        writel(fb_info->var.upper_margin + fb_info->var.vsync_len +
+                fb_info->var.lower_margin + fb_info->var.yres,
+                host->base + LCDC_VDCTRL1);
+
+        /* line length in units of clocks or pixels */
+        writel(set_hsync_pulse_width(host, fb_info->var.hsync_len) |
+                VDCTRL2_SET_HSYNC_PERIOD(fb_info->var.left_margin +
+                fb_info->var.hsync_len + fb_info->var.right_margin +
+                fb_info->var.xres),
+                host->base + LCDC_VDCTRL2);
+
+        writel(SET_HOR_WAIT_CNT(fb_info->var.left_margin +
+                fb_info->var.hsync_len) |
+                SET_VERT_WAIT_CNT(fb_info->var.upper_margin +
+                        fb_info->var.vsync_len),
+                host->base + LCDC_VDCTRL3);
+
+        vdctrl4 = SET_DOTCLK_H_VALID_DATA_CNT(fb_info->var.xres);
+        if (mxsfb_is_v4(host))
+                vdctrl4 |= VDCTRL4_SET_DOTCLK_DLY(host->dotclk_delay);
+        writel(vdctrl4, host->base + LCDC_VDCTRL4);
+
+        writel(fb_info->fix.smem_start +
+                        fb_info->fix.line_length * fb_info->var.yoffset,
+                        host->base + host->devdata->next_buf);
+
+        if (reenable)
+                mxsfb_enable_controller(fb_info);
+
+        return 0;
+}
+
+static int mxsfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+                u_int transp, struct fb_info *fb_info)
+{
+        unsigned int val;
+        int ret = -EINVAL;
+
+        /*
+         * If greyscale is true, then we convert the RGB value
+         * to greyscale no matter what visual we are using.
+         */
+        if (fb_info->var.grayscale)
+                red = green = blue = (19595 * red + 38470 * green +
+                                        7471 * blue) >> 16;
+
+        switch (fb_info->fix.visual) {
+        case FB_VISUAL_TRUECOLOR:
+                /*
+                 * 12 or 16-bit True Colour.  We encode the RGB value
+                 * according to the RGB bitfield information.
+                 */
+                if (regno < 16) {
+                        u32 *pal = fb_info->pseudo_palette;
+
+                        val  = chan_to_field(red, &fb_info->var.red);
+                        val |= chan_to_field(green, &fb_info->var.green);
+                        val |= chan_to_field(blue, &fb_info->var.blue);
+
+                        pal[regno] = val;
+                        ret = 0;
+                }
+                break;
+
+        case FB_VISUAL_STATIC_PSEUDOCOLOR:
+        case FB_VISUAL_PSEUDOCOLOR:
+                break;
+        }
+
+        return ret;
+}
+
+static int mxsfb_blank(int blank, struct fb_info *fb_info)
+{
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+
+        switch (blank) {
+        case FB_BLANK_POWERDOWN:
+        case FB_BLANK_VSYNC_SUSPEND:
+        case FB_BLANK_HSYNC_SUSPEND:
+        case FB_BLANK_NORMAL:
+                if (host->enabled)
+                        mxsfb_disable_controller(fb_info);
+                break;
+
+        case FB_BLANK_UNBLANK:
+                if (!host->enabled)
+                        mxsfb_enable_controller(fb_info);
+                break;
+        }
+        return 0;
+}
+
+static int mxsfb_pan_display(struct fb_var_screeninfo *var,
+                struct fb_info *fb_info)
+{
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+        unsigned offset;
+
+        if (var->xoffset != 0)
+                return -EINVAL;
+
+        offset = fb_info->fix.line_length * var->yoffset;
+
+        /* update on next VSYNC */
+        writel(fb_info->fix.smem_start + offset,
+                        host->base + host->devdata->next_buf);
+
+        return 0;
+}
+
+static struct fb_ops mxsfb_ops = {
+        .owner = THIS_MODULE,
+        .fb_check_var = mxsfb_check_var,
+        .fb_set_par = mxsfb_set_par,
+        .fb_setcolreg = mxsfb_setcolreg,
+        .fb_blank = mxsfb_blank,
+        .fb_pan_display = mxsfb_pan_display,
+        .fb_fillrect = cfb_fillrect,
+        .fb_copyarea = cfb_copyarea,
+        .fb_imageblit = cfb_imageblit,
+};
+
+static int __devinit mxsfb_restore_mode(struct mxsfb_info *host)
+{
+        struct fb_info *fb_info = &host->fb_info;
+        unsigned line_count;
+        unsigned period;
+        unsigned long pa, fbsize;
+        int bits_per_pixel, ofs;
+        u32 transfer_count, vdctrl0, vdctrl2, vdctrl3, vdctrl4, ctrl;
+        struct fb_videomode vmode;
+
+        /* Only restore the mode when the controller is running */
+        ctrl = readl(host->base + LCDC_CTRL);
+        if (!(ctrl & CTRL_RUN))
+                return -EINVAL;
+
+        vdctrl0 = readl(host->base + LCDC_VDCTRL0);
+        vdctrl2 = readl(host->base + LCDC_VDCTRL2);
+        vdctrl3 = readl(host->base + LCDC_VDCTRL3);
+        vdctrl4 = readl(host->base + LCDC_VDCTRL4);
+
+        transfer_count = readl(host->base + host->devdata->transfer_count);
+
+        vmode.xres = TRANSFER_COUNT_GET_HCOUNT(transfer_count);
+        vmode.yres = TRANSFER_COUNT_GET_VCOUNT(transfer_count);
+
+        switch (CTRL_GET_WORD_LENGTH(ctrl)) {
+        case 0:
+                bits_per_pixel = 16;
+                break;
+        case 3:
+                bits_per_pixel = 32;
+        case 1:
+        default:
+                return -EINVAL;
+        }
+
+        fb_info->var.bits_per_pixel = bits_per_pixel;
+
+        vmode.pixclock = KHZ2PICOS(clk_get_rate(host->clk) / 1000U);
+        vmode.hsync_len = get_hsync_pulse_width(host, vdctrl2);
+        vmode.left_margin = GET_HOR_WAIT_CNT(vdctrl3) - vmode.hsync_len;
+        vmode.right_margin = VDCTRL2_GET_HSYNC_PERIOD(vdctrl2) - vmode.hsync_len -
+                vmode.left_margin - vmode.xres;
+        vmode.vsync_len = VDCTRL0_GET_VSYNC_PULSE_WIDTH(vdctrl0);
+        period = readl(host->base + LCDC_VDCTRL1);
+        vmode.upper_margin = GET_VERT_WAIT_CNT(vdctrl3) - vmode.vsync_len;
+        vmode.lower_margin = period - vmode.vsync_len - vmode.upper_margin - vmode.yres;
+
+        vmode.vmode = FB_VMODE_NONINTERLACED;
+
+        vmode.sync = 0;
+        if (vdctrl0 & VDCTRL0_HSYNC_ACT_HIGH)
+                vmode.sync |= FB_SYNC_HOR_HIGH_ACT;
+        if (vdctrl0 & VDCTRL0_VSYNC_ACT_HIGH)
+                vmode.sync |= FB_SYNC_VERT_HIGH_ACT;
+
+        pr_debug("Reconstructed video mode:\n");
+        pr_debug("%dx%d, hsync: %u left: %u, right: %u, vsync: %u, upper: %u, lower: %u\n",
+                        vmode.xres, vmode.yres,
+                        vmode.hsync_len, vmode.left_margin, vmode.right_margin,
+                        vmode.vsync_len, vmode.upper_margin, vmode.lower_margin);
+        pr_debug("pixclk: %ldkHz\n", PICOS2KHZ(vmode.pixclock));
+
+        fb_add_videomode(&vmode, &fb_info->modelist);
+
+        host->ld_intf_width = CTRL_GET_BUS_WIDTH(ctrl);
+        host->dotclk_delay = VDCTRL4_GET_DOTCLK_DLY(vdctrl4);
+
+        fb_info->fix.line_length = vmode.xres * (bits_per_pixel >> 3);
+
+        pa = readl(host->base + host->devdata->cur_buf);
+        fbsize = fb_info->fix.line_length * vmode.yres;
+        if (pa < fb_info->fix.smem_start)
+                return -EINVAL;
+        if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len)
+                return -EINVAL;
+        ofs = pa - fb_info->fix.smem_start;
+        if (ofs) {
+                memmove(fb_info->screen_base, fb_info->screen_base + ofs, fbsize);
+                writel(fb_info->fix.smem_start, host->base + host->devdata->next_buf);
+        }
+
+        line_count = fb_info->fix.smem_len / fb_info->fix.line_length;
+        fb_info->fix.ypanstep = 1;
+
+        clk_enable(host->clk);
+        host->enabled = 1;
+
+        return 0;
+}
+
+static int __devinit mxsfb_init_fbinfo(struct mxsfb_info *host)
+{
+        struct fb_info *fb_info = &host->fb_info;
+        struct fb_var_screeninfo *var = &fb_info->var;
+        struct mxsfb_platform_data *pdata = host->pdev->dev.platform_data;
+        dma_addr_t fb_phys;
+        void *fb_virt;
+        unsigned fb_size = pdata->fb_size;
+
+        fb_info->fbops = &mxsfb_ops;
+        fb_info->flags = FBINFO_FLAG_DEFAULT | FBINFO_READS_FAST;
+        strlcpy(fb_info->fix.id, "mxs", sizeof(fb_info->fix.id));
+        fb_info->fix.type = FB_TYPE_PACKED_PIXELS;
+        fb_info->fix.ypanstep = 1;
+        fb_info->fix.visual = FB_VISUAL_TRUECOLOR,
+        fb_info->fix.accel = FB_ACCEL_NONE;
+
+        var->bits_per_pixel = pdata->default_bpp ? pdata->default_bpp : 16;
+        var->nonstd = 0;
+        var->activate = FB_ACTIVATE_NOW;
+        var->accel_flags = 0;
+        var->vmode = FB_VMODE_NONINTERLACED;
+
+        host->dotclk_delay = pdata->dotclk_delay;
+        host->ld_intf_width = pdata->ld_intf_width;
+
+        /* Memory allocation for framebuffer */
+        if (pdata->fb_phys) {
+                if (!fb_size)
+                        return -EINVAL;
+
+                fb_phys = pdata->fb_phys;
+
+                if (!request_mem_region(fb_phys, fb_size, host->pdev->name))
+                        return -ENOMEM;
+
+                fb_virt = ioremap(fb_phys, fb_size);
+                if (!fb_virt) {
+                        release_mem_region(fb_phys, fb_size);
+                        return -ENOMEM;
+                }
+                host->mapped = 1;
+        } else {
+                if (!fb_size)
+                        fb_size = SZ_2M; /* default */
+                fb_virt = alloc_pages_exact(fb_size, GFP_DMA);
+                if (!fb_virt)
+                        return -ENOMEM;
+
+                fb_phys = virt_to_phys(fb_virt);
+        }
+
+        fb_info->fix.smem_start = fb_phys;
+        fb_info->screen_base = fb_virt;
+        fb_info->screen_size = fb_info->fix.smem_len = fb_size;
+
+        if (mxsfb_restore_mode(host))
+                memset(fb_virt, 0, fb_size);
+
+        return 0;
+}
+
+static void __devexit mxsfb_free_videomem(struct mxsfb_info *host)
+{
+        struct fb_info *fb_info = &host->fb_info;
+
+        if (host->mapped) {
+                iounmap(fb_info->screen_base);
+                release_mem_region(fb_info->fix.smem_start,
+                                fb_info->screen_size);
+        } else {
+                free_pages_exact(fb_info->screen_base, fb_info->fix.smem_len);
+        }
+}
+
+static int __devinit mxsfb_probe(struct platform_device *pdev)
+{
+        struct mxsfb_platform_data *pdata = pdev->dev.platform_data;
+        struct resource *res;
+        struct mxsfb_info *host;
+        struct fb_info *fb_info;
+        struct fb_modelist *modelist;
+        int i, ret;
+
+        if (!pdata) {
+                dev_err(&pdev->dev, "No platformdata. Giving up\n");
+                return -ENODEV;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "Cannot get memory IO resource\n");
+                return -ENODEV;
+        }
+
+        if (!request_mem_region(res->start, resource_size(res), pdev->name))
+                return -EBUSY;
+
+        fb_info = framebuffer_alloc(sizeof(struct mxsfb_info), &pdev->dev);
+        if (!fb_info) {
+                dev_err(&pdev->dev, "Failed to allocate fbdev\n");
+                ret = -ENOMEM;
+                goto error_alloc_info;
+        }
+
+        host = to_imxfb_host(fb_info);
+
+        host->base = ioremap(res->start, resource_size(res));
+        if (!host->base) {
+                dev_err(&pdev->dev, "ioremap failed\n");
+                ret = -ENOMEM;
+                goto error_ioremap;
+        }
+
+        host->pdev = pdev;
+        platform_set_drvdata(pdev, host);
+
+        host->devdata = &mxsfb_devdata[pdev->id_entry->driver_data];
+
+        host->clk = clk_get(&host->pdev->dev, NULL);
+        if (IS_ERR(host->clk)) {
+                ret = PTR_ERR(host->clk);
+                goto error_getclock;
+        }
+
+        fb_info->pseudo_palette = kmalloc(sizeof(u32) * 16, GFP_KERNEL);
+        if (!fb_info->pseudo_palette) {
+                ret = -ENOMEM;
+                goto error_pseudo_pallette;
+        }
+
+        INIT_LIST_HEAD(&fb_info->modelist);
+
+        ret = mxsfb_init_fbinfo(host);
+        if (ret != 0)
+                goto error_init_fb;
+
+        for (i = 0; i < pdata->mode_count; i++)
+                fb_add_videomode(&pdata->mode_list[i], &fb_info->modelist);
+
+        modelist = list_first_entry(&fb_info->modelist,
+                        struct fb_modelist, list);
+        fb_videomode_to_var(&fb_info->var, &modelist->mode);
+
+        /* init the color fields */
+        mxsfb_check_var(&fb_info->var, fb_info);
+
+        platform_set_drvdata(pdev, fb_info);
+
+        ret = register_framebuffer(fb_info);
+        if (ret != 0) {
+                dev_err(&pdev->dev,"Failed to register framebuffer\n");
+                goto error_register;
+        }
+
+        if (!host->enabled) {
+                writel(0, host->base + LCDC_CTRL);
+                mxsfb_set_par(fb_info);
+                mxsfb_enable_controller(fb_info);
+        }
+
+        dev_info(&pdev->dev, "initialized\n");
+
+        return 0;
+
+error_register:
+        if (host->enabled)
+                clk_disable(host->clk);
+        fb_destroy_modelist(&fb_info->modelist);
+error_init_fb:
+        kfree(fb_info->pseudo_palette);
+error_pseudo_pallette:
+        clk_put(host->clk);
+error_getclock:
+        iounmap(host->base);
+error_ioremap:
+        framebuffer_release(fb_info);
+error_alloc_info:
+        release_mem_region(res->start, resource_size(res));
+
+        return ret;
+}
+
+static int __devexit mxsfb_remove(struct platform_device *pdev)
+{
+        struct fb_info *fb_info = platform_get_drvdata(pdev);
+        struct mxsfb_info *host = to_imxfb_host(fb_info);
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+        if (host->enabled)
+                mxsfb_disable_controller(fb_info);
+
+        unregister_framebuffer(fb_info);
+        kfree(fb_info->pseudo_palette);
+        mxsfb_free_videomem(host);
+        iounmap(host->base);
+        clk_put(host->clk);
+
+        framebuffer_release(fb_info);
+        release_mem_region(res->start, resource_size(res));
+
+        platform_set_drvdata(pdev, NULL);
+
+        return 0;
+}
+
+static struct platform_device_id mxsfb_devtype[] = {
+        {
+                .name = "imx23-fb",
+                .driver_data = MXSFB_V3,
+        }, {
+                .name = "imx28-fb",
+                .driver_data = MXSFB_V4,
+        }, {
+                /* sentinel */
+        }
+};
+MODULE_DEVICE_TABLE(platform, mxsfb_devtype);
+
+static struct platform_driver mxsfb_driver = {
+        .probe = mxsfb_probe,
+        .remove = __devexit_p(mxsfb_remove),
+        .id_table = mxsfb_devtype,
+        .driver = {
+                   .name = DRIVER_NAME,
+        },
+};
+
+static int __init mxsfb_init(void)
+{
+        return platform_driver_register(&mxsfb_driver);
+}
+
+static void __exit mxsfb_exit(void)
+{
+        platform_driver_unregister(&mxsfb_driver);
+}
+
+module_init(mxsfb_init);
+module_exit(mxsfb_exit);
+
+MODULE_DESCRIPTION("Freescale mxs framebuffer driver");
+MODULE_AUTHOR("Sascha Hauer, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/nuc900fb.c b/drivers/video/nuc900fb.c
index 81687ed26ba9..f838d9e277f0 100644
--- a/drivers/video/nuc900fb.c
+++ b/drivers/video/nuc900fb.c
@@ -15,6 +15,7 @@
  */
 #include <linux/module.h>
 #include <linux/kernel.h>
+#include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/string.h>
 #include <linux/mm.h>
@@ -597,9 +598,9 @@ static int __devinit nuc900fb_probe(struct platform_device *pdev)
         }
 
         fbi->clk = clk_get(&pdev->dev, NULL);
-        if (!fbi->clk || IS_ERR(fbi->clk)) {
+        if (IS_ERR(fbi->clk)) {
                 printk(KERN_ERR "nuc900-lcd:failed to get lcd clock source\n");
-                ret = -ENOENT;
+                ret = PTR_ERR(fbi->clk);
                 goto release_irq;
         }
 
@@ -695,6 +696,8 @@ static int nuc900fb_remove(struct platform_device *pdev)
         nuc900fb_stop_lcd(fbinfo);
         msleep(1);
 
+        unregister_framebuffer(fbinfo);
+        nuc900fb_cpufreq_deregister(fbi);
         nuc900fb_unmap_video_memory(fbinfo);
 
         iounmap(fbi->io);
@@ -722,7 +725,7 @@ static int nuc900fb_suspend(struct platform_device *dev, pm_message_t state)
         struct fb_info     *fbinfo = platform_get_drvdata(dev);
         struct nuc900fb_info *info = fbinfo->par;
 
-        nuc900fb_stop_lcd();
+        nuc900fb_stop_lcd(fbinfo);
         msleep(1);
         clk_disable(info->clk);
         return 0;
@@ -739,7 +742,7 @@ static int nuc900fb_resume(struct platform_device *dev)
         msleep(1);
 
         nuc900fb_init_registers(fbinfo);
-        nuc900fb_activate_var(bfinfo);
+        nuc900fb_activate_var(fbinfo);
 
         return 0;
 }
diff --git a/drivers/video/nuc900fb.h b/drivers/video/nuc900fb.h
index 6c23aa3d3b89..bc7c9300f276 100644
--- a/drivers/video/nuc900fb.h
+++ b/drivers/video/nuc900fb.h
@@ -8,7 +8,7 @@
  * the Free Software Foundation; either version 2 of the License, or
  * (at your option) any later version.
  *
- *   Auther:
+ *   Author:
  *        Wang Qiang(rurality.linux@gmail.com)  2009/12/16
  */
 
diff --git a/drivers/video/nvidia/nv_backlight.c b/drivers/video/nvidia/nv_backlight.c
index 2fb552a6f32c..8471008aa6ff 100644
--- a/drivers/video/nvidia/nv_backlight.c
+++ b/drivers/video/nvidia/nv_backlight.c
@@ -87,7 +87,7 @@ static int nvidia_bl_get_brightness(struct backlight_device *bd)
         return bd->props.brightness;
 }
 
-static struct backlight_ops nvidia_bl_ops = {
+static const struct backlight_ops nvidia_bl_ops = {
         .get_brightness = nvidia_bl_get_brightness,
         .update_status  = nvidia_bl_update_status,
 };
@@ -111,6 +111,7 @@ void nvidia_bl_init(struct nvidia_par *par)
         snprintf(name, sizeof(name), "nvidiabl%d", info->node);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
         bd = backlight_device_register(name, info->dev, par, &nvidia_bl_ops,
                                        &props);
diff --git a/drivers/video/nvidia/nvidia.c b/drivers/video/nvidia/nvidia.c
index efe10ff86d63..081dc4745274 100644
--- a/drivers/video/nvidia/nvidia.c
+++ b/drivers/video/nvidia/nvidia.c
@@ -1057,7 +1057,7 @@ static int nvidiafb_suspend(struct pci_dev *dev, pm_message_t mesg)
 
         if (mesg.event == PM_EVENT_PRETHAW)
                 mesg.event = PM_EVENT_FREEZE;
-        acquire_console_sem();
+        console_lock();
         par->pm_state = mesg.event;
 
         if (mesg.event & PM_EVENT_SLEEP) {
@@ -1070,7 +1070,7 @@ static int nvidiafb_suspend(struct pci_dev *dev, pm_message_t mesg)
         }
         dev->dev.power.power_state = mesg;
 
-        release_console_sem();
+        console_unlock();
         return 0;
 }
 
@@ -1079,7 +1079,7 @@ static int nvidiafb_resume(struct pci_dev *dev)
         struct fb_info *info = pci_get_drvdata(dev);
         struct nvidia_par *par = info->par;
 
-        acquire_console_sem();
+        console_lock();
         pci_set_power_state(dev, PCI_D0);
 
         if (par->pm_state != PM_EVENT_FREEZE) {
@@ -1097,7 +1097,7 @@ static int nvidiafb_resume(struct pci_dev *dev)
         nvidiafb_blank(FB_BLANK_UNBLANK, info);
 
 fail:
-        release_console_sem();
+        console_unlock();
         return 0;
 }
 #else
diff --git a/drivers/video/omap/Kconfig b/drivers/video/omap/Kconfig
index 455c6055325d..196fa2e7f438 100644
--- a/drivers/video/omap/Kconfig
+++ b/drivers/video/omap/Kconfig
@@ -1,17 +1,22 @@
 config FB_OMAP
         tristate "OMAP frame buffer support (EXPERIMENTAL)"
-        depends on FB && ARCH_OMAP && (OMAP2_DSS = "n")
-
+        depends on FB && (OMAP2_DSS = "n")
+        depends on ARCH_OMAP1 || ARCH_OMAP2 || ARCH_OMAP3
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
+        select TWL4030_CORE if MACH_OMAP_2430SDP
         help
           Frame buffer driver for OMAP based boards.
 
 config FB_OMAP_LCD_VGA
         bool "Use LCD in VGA mode"
                 depends on MACH_OMAP_3430SDP || MACH_OMAP_LDP
-
+                help
+                  Set LCD resolution as VGA (640 X 480).
+                  Default resolution without this option is QVGA(320 X 240).
+                  Please take a look at drivers/video/omap/lcd_ldp.c file
+                  for lcd driver code.
 choice
         depends on FB_OMAP && MACH_OVERO
         prompt "Screen resolution"
@@ -59,7 +64,7 @@ config FB_OMAP_MANUAL_UPDATE
         depends on FB_OMAP && FB_OMAP_LCDC_EXTERNAL
         help
           Say Y here, if your user-space applications are capable of
-          notifying the frame buffer driver when a change has occured in
+          notifying the frame buffer driver when a change has occurred in
           the frame buffer content and thus a reload of the image data to
           the external frame buffer is required. If unsure, say N.
 
diff --git a/drivers/video/omap/Makefile b/drivers/video/omap/Makefile
index 49226a1b909e..25db55696e14 100644
--- a/drivers/video/omap/Makefile
+++ b/drivers/video/omap/Makefile
@@ -30,7 +30,6 @@ objs-y$(CONFIG_MACH_OMAP_APOLLON) += lcd_apollon.o
 objs-y$(CONFIG_MACH_OMAP_2430SDP) += lcd_2430sdp.o
 objs-y$(CONFIG_MACH_OMAP_3430SDP) += lcd_2430sdp.o
 objs-y$(CONFIG_MACH_OMAP_LDP) += lcd_ldp.o
-objs-y$(CONFIG_MACH_OMAP2EVM) += lcd_omap2evm.o
 objs-y$(CONFIG_MACH_OMAP3EVM) += lcd_omap3evm.o
 objs-y$(CONFIG_MACH_OMAP3_BEAGLE) += lcd_omap3beagle.o
 objs-y$(CONFIG_FB_OMAP_LCD_MIPID) += lcd_mipid.o
diff --git a/drivers/video/omap/blizzard.c b/drivers/video/omap/blizzard.c
index 2ffb34af4c59..c0504a8a5079 100644
--- a/drivers/video/omap/blizzard.c
+++ b/drivers/video/omap/blizzard.c
@@ -397,8 +397,7 @@ static inline void free_req(struct blizzard_request *req)
 
         spin_lock_irqsave(&blizzard.req_lock, flags);
 
-        list_del(&req->entry);
-        list_add(&req->entry, &blizzard.free_req_list);
+        list_move(&req->entry, &blizzard.free_req_list);
         if (!(req->flags & REQ_FROM_IRQ_POOL))
                 up(&blizzard.req_sema);
 
@@ -1590,7 +1589,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/dispc.c b/drivers/video/omap/dispc.c
index 529483467abf..0ccd7adf47bb 100644
--- a/drivers/video/omap/dispc.c
+++ b/drivers/video/omap/dispc.c
@@ -922,14 +922,14 @@ static int get_dss_clocks(void)
                 return PTR_ERR(dispc.dss_ick);
         }
 
-        dispc.dss1_fck = clk_get(&dispc.fbdev->dssdev->dev, "dss1_fck");
+        dispc.dss1_fck = clk_get(&dispc.fbdev->dssdev->dev, "fck");
         if (IS_ERR(dispc.dss1_fck)) {
                 dev_err(dispc.fbdev->dev, "can't get dss1_fck\n");
                 clk_put(dispc.dss_ick);
                 return PTR_ERR(dispc.dss1_fck);
         }
 
-        dispc.dss_54m_fck = clk_get(&dispc.fbdev->dssdev->dev, "tv_fck");
+        dispc.dss_54m_fck = clk_get(&dispc.fbdev->dssdev->dev, "tv_clk");
         if (IS_ERR(dispc.dss_54m_fck)) {
                 dev_err(dispc.fbdev->dev, "can't get tv_fck\n");
                 clk_put(dispc.dss_ick);
diff --git a/drivers/video/omap/hwa742.c b/drivers/video/omap/hwa742.c
index 0016f77cd13f..084aa0ac562b 100644
--- a/drivers/video/omap/hwa742.c
+++ b/drivers/video/omap/hwa742.c
@@ -269,8 +269,7 @@ static inline void free_req(struct hwa742_request *req)
 
         spin_lock_irqsave(&hwa742.req_lock, flags);
 
-        list_del(&req->entry);
-        list_add(&req->entry, &hwa742.free_req_list);
+        list_move(&req->entry, &hwa742.free_req_list);
         if (!(req->flags & REQ_FROM_IRQ_POOL))
                 up(&hwa742.req_sema);
 
diff --git a/drivers/video/omap/lcd_mipid.c b/drivers/video/omap/lcd_mipid.c
index 64dcc7439c99..90e3bdd1b7ab 100644
--- a/drivers/video/omap/lcd_mipid.c
+++ b/drivers/video/omap/lcd_mipid.c
@@ -396,7 +396,7 @@ static void mipid_esd_start_check(struct mipid_device *md)
 static void mipid_esd_stop_check(struct mipid_device *md)
 {
         if (md->esd_check != NULL)
-                cancel_rearming_delayed_workqueue(md->esd_wq, &md->esd_work);
+                cancel_delayed_work_sync(&md->esd_work);
 }
 
 static void mipid_esd_work(struct work_struct *work)
diff --git a/drivers/video/omap/lcd_omap2evm.c b/drivers/video/omap/lcd_omap2evm.c
deleted file mode 100644
index 7e7a65c08452..000000000000
--- a/drivers/video/omap/lcd_omap2evm.c
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * LCD panel support for the MISTRAL OMAP2EVM board
- *
- * Author: Arun C <arunedarath@mistralsolutions.com>
- *
- * Derived from drivers/video/omap/lcd_omap3evm.c
- * Derived from drivers/video/omap/lcd-apollon.c
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation; either version 2 of the License, or (at your
- * option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/gpio.h>
-#include <linux/i2c/twl.h>
-
-#include <plat/mux.h>
-#include <asm/mach-types.h>
-
-#include "omapfb.h"
-
-#define LCD_PANEL_ENABLE_GPIO   154
-#define LCD_PANEL_LR            128
-#define LCD_PANEL_UD            129
-#define LCD_PANEL_INI           152
-#define LCD_PANEL_QVGA          148
-#define LCD_PANEL_RESB          153
-
-#define TWL_LED_LEDEN           0x00
-#define TWL_PWMA_PWMAON         0x00
-#define TWL_PWMA_PWMAOFF        0x01
-
-static unsigned int bklight_level;
-
-static int omap2evm_panel_init(struct lcd_panel *panel,
-                                struct omapfb_device *fbdev)
-{
-        gpio_request(LCD_PANEL_ENABLE_GPIO, "LCD enable");
-        gpio_request(LCD_PANEL_LR, "LCD lr");
-        gpio_request(LCD_PANEL_UD, "LCD ud");
-        gpio_request(LCD_PANEL_INI, "LCD ini");
-        gpio_request(LCD_PANEL_QVGA, "LCD qvga");
-        gpio_request(LCD_PANEL_RESB, "LCD resb");
-
-        gpio_direction_output(LCD_PANEL_ENABLE_GPIO, 1);
-        gpio_direction_output(LCD_PANEL_RESB, 1);
-        gpio_direction_output(LCD_PANEL_INI, 1);
-        gpio_direction_output(LCD_PANEL_QVGA, 0);
-        gpio_direction_output(LCD_PANEL_LR, 1);
-        gpio_direction_output(LCD_PANEL_UD, 1);
-
-        twl_i2c_write_u8(TWL4030_MODULE_LED, 0x11, TWL_LED_LEDEN);
-        twl_i2c_write_u8(TWL4030_MODULE_PWMA, 0x01, TWL_PWMA_PWMAON);
-        twl_i2c_write_u8(TWL4030_MODULE_PWMA, 0x02, TWL_PWMA_PWMAOFF);
-        bklight_level = 100;
-
-        return 0;
-}
-
-static void omap2evm_panel_cleanup(struct lcd_panel *panel)
-{
-        gpio_free(LCD_PANEL_RESB);
-        gpio_free(LCD_PANEL_QVGA);
-        gpio_free(LCD_PANEL_INI);
-        gpio_free(LCD_PANEL_UD);
-        gpio_free(LCD_PANEL_LR);
-        gpio_free(LCD_PANEL_ENABLE_GPIO);
-}
-
-static int omap2evm_panel_enable(struct lcd_panel *panel)
-{
-        gpio_set_value(LCD_PANEL_ENABLE_GPIO, 0);
-        return 0;
-}
-
-static void omap2evm_panel_disable(struct lcd_panel *panel)
-{
-        gpio_set_value(LCD_PANEL_ENABLE_GPIO, 1);
-}
-
-static unsigned long omap2evm_panel_get_caps(struct lcd_panel *panel)
-{
-        return 0;
-}
-
-static int omap2evm_bklight_setlevel(struct lcd_panel *panel,
-                                                unsigned int level)
-{
-        u8 c;
-        if ((level >= 0) && (level <= 100)) {
-                c = (125 * (100 - level)) / 100 + 2;
-                twl_i2c_write_u8(TWL4030_MODULE_PWMA, c, TWL_PWMA_PWMAOFF);
-                bklight_level = level;
-        }
-        return 0;
-}
-
-static unsigned int omap2evm_bklight_getlevel(struct lcd_panel *panel)
-{
-        return bklight_level;
-}
-
-static unsigned int omap2evm_bklight_getmaxlevel(struct lcd_panel *panel)
-{
-        return 100;
-}
-
-struct lcd_panel omap2evm_panel = {
-        .name           = "omap2evm",
-        .config         = OMAP_LCDC_PANEL_TFT | OMAP_LCDC_INV_VSYNC |
-                          OMAP_LCDC_INV_HSYNC,
-
-        .bpp            = 16,
-        .data_lines     = 18,
-        .x_res          = 480,
-        .y_res          = 640,
-        .hsw            = 3,
-        .hfp            = 0,
-        .hbp            = 28,
-        .vsw            = 2,
-        .vfp            = 1,
-        .vbp            = 0,
-
-        .pixel_clock    = 20000,
-
-        .init           = omap2evm_panel_init,
-        .cleanup        = omap2evm_panel_cleanup,
-        .enable         = omap2evm_panel_enable,
-        .disable        = omap2evm_panel_disable,
-        .get_caps       = omap2evm_panel_get_caps,
-        .set_bklight_level      = omap2evm_bklight_setlevel,
-        .get_bklight_level      = omap2evm_bklight_getlevel,
-        .get_bklight_max        = omap2evm_bklight_getmaxlevel,
-};
-
-static int omap2evm_panel_probe(struct platform_device *pdev)
-{
-        omapfb_register_panel(&omap2evm_panel);
-        return 0;
-}
-
-static int omap2evm_panel_remove(struct platform_device *pdev)
-{
-        return 0;
-}
-
-static int omap2evm_panel_suspend(struct platform_device *pdev,
-                                   pm_message_t mesg)
-{
-        return 0;
-}
-
-static int omap2evm_panel_resume(struct platform_device *pdev)
-{
-        return 0;
-}
-
-struct platform_driver omap2evm_panel_driver = {
-        .probe          = omap2evm_panel_probe,
-        .remove         = omap2evm_panel_remove,
-        .suspend        = omap2evm_panel_suspend,
-        .resume         = omap2evm_panel_resume,
-        .driver         = {
-                .name   = "omap2evm_lcd",
-                .owner  = THIS_MODULE,
-        },
-};
-
-static int __init omap2evm_panel_drv_init(void)
-{
-        return platform_driver_register(&omap2evm_panel_driver);
-}
-
-static void __exit omap2evm_panel_drv_exit(void)
-{
-        platform_driver_unregister(&omap2evm_panel_driver);
-}
-
-module_init(omap2evm_panel_drv_init);
-module_exit(omap2evm_panel_drv_exit);
diff --git a/drivers/video/omap/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/omap/omapfb_main.c b/drivers/video/omap/omapfb_main.c
index e264efd0278f..b3ddd743d8a6 100644
--- a/drivers/video/omap/omapfb_main.c
+++ b/drivers/video/omap/omapfb_main.c
@@ -90,7 +90,7 @@ static void omapdss_release(struct device *dev)
 
 /* dummy device for clocks */
 static struct platform_device omapdss_device = {
-        .name           = "omapdss",
+        .name           = "omapdss_dss",
         .id             = -1,
         .dev            = {
                 .release = omapdss_release,
diff --git a/drivers/video/omap/rfbi.c b/drivers/video/omap/rfbi.c
index eada9f12efc7..0c6981f1a4a3 100644
--- a/drivers/video/omap/rfbi.c
+++ b/drivers/video/omap/rfbi.c
@@ -90,7 +90,7 @@ static int rfbi_get_clocks(void)
                 return PTR_ERR(rfbi.dss_ick);
         }
 
-        rfbi.dss1_fck = clk_get(&rfbi.fbdev->dssdev->dev, "dss1_fck");
+        rfbi.dss1_fck = clk_get(&rfbi.fbdev->dssdev->dev, "fck");
         if (IS_ERR(rfbi.dss1_fck)) {
                 dev_err(rfbi.fbdev->dev, "can't get dss1_fck\n");
                 clk_put(rfbi.dss_ick);
diff --git a/drivers/video/omap2/Makefile b/drivers/video/omap2/Makefile
index d853d05dad31..5ddef129f798 100644
--- a/drivers/video/omap2/Makefile
+++ b/drivers/video/omap2/Makefile
@@ -1,6 +1,6 @@
 obj-$(CONFIG_OMAP2_VRAM) += vram.o
 obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
 
-obj-y += dss/
-obj-y += omapfb/
+obj-$(CONFIG_OMAP2_DSS) += dss/
+obj-$(CONFIG_FB_OMAP2) += omapfb/
 obj-y += displays/
diff --git a/drivers/video/omap2/displays/Kconfig b/drivers/video/omap2/displays/Kconfig
index 881c9f77c75a..609a28073178 100644
--- a/drivers/video/omap2/displays/Kconfig
+++ b/drivers/video/omap2/displays/Kconfig
@@ -1,24 +1,34 @@
 menu "OMAP2/3 Display Device Drivers"
         depends on OMAP2_DSS
 
-config PANEL_GENERIC
-        tristate "Generic Panel"
+config PANEL_GENERIC_DPI
+        tristate "Generic DPI Panel"
+        depends on OMAP2_DSS_DPI
         help
-          Generic panel driver.
-          Used for DVI output for Beagle and OMAP3 SDP.
+          Generic DPI panel driver.
+          Supports DVI output for Beagle and OMAP3 SDP.
+          Supports LCD Panel used in TI SDP3430 and EVM boards,
+          OMAP3517 EVM boards and CM-T35.
+
+config PANEL_LGPHILIPS_LB035Q02
+        tristate "LG.Philips LB035Q02 LCD Panel"
+        depends on OMAP2_DSS_DPI && SPI
+        help
+          LCD Panel used on the Gumstix Overo Palo35
 
 config PANEL_SHARP_LS037V7DW01
         tristate "Sharp LS037V7DW01 LCD Panel"
-        depends on OMAP2_DSS
+        depends on OMAP2_DSS_DPI
         select BACKLIGHT_CLASS_DEVICE
         help
           LCD Panel used in TI's SDP3430 and EVM boards
 
-config PANEL_SHARP_LQ043T1DG01
-        tristate "Sharp LQ043T1DG01 LCD Panel"
-        depends on OMAP2_DSS
-        help
-          LCD Panel used in TI's OMAP3517 EVM boards
+config PANEL_NEC_NL8048HL11_01B
+        tristate "NEC NL8048HL11-01B Panel"
+        depends on OMAP2_DSS_DPI
+        help
+                This NEC NL8048HL11-01B panel is TFT LCD
+                used in the Zoom2/3/3630 sdp boards.
 
 config PANEL_TAAL
         tristate "Taal DSI Panel"
@@ -26,21 +36,15 @@ config PANEL_TAAL
         help
           Taal DSI command mode panel from TPO.
 
-config PANEL_TOPPOLY_TDO35S
-        tristate "Toppoly TDO35S LCD Panel support"
-        depends on OMAP2_DSS
-        help
-          LCD Panel used in CM-T35
-
 config PANEL_TPO_TD043MTEA1
         tristate "TPO TD043MTEA1 LCD Panel"
-        depends on OMAP2_DSS && SPI
+        depends on OMAP2_DSS_DPI && SPI
         help
           LCD Panel used in OMAP3 Pandora
 
 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/Makefile b/drivers/video/omap2/displays/Makefile
index aa386095d7c4..0f601ab3abf4 100644
--- a/drivers/video/omap2/displays/Makefile
+++ b/drivers/video/omap2/displays/Makefile
@@ -1,8 +1,8 @@
-obj-$(CONFIG_PANEL_GENERIC) += panel-generic.o
+obj-$(CONFIG_PANEL_GENERIC_DPI) += panel-generic-dpi.o
+obj-$(CONFIG_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
 obj-$(CONFIG_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
-obj-$(CONFIG_PANEL_SHARP_LQ043T1DG01) += panel-sharp-lq043t1dg01.o
+obj-$(CONFIG_PANEL_NEC_NL8048HL11_01B) += panel-nec-nl8048hl11-01b.o
 
 obj-$(CONFIG_PANEL_TAAL) += panel-taal.o
-obj-$(CONFIG_PANEL_TOPPOLY_TDO35S) += panel-toppoly-tdo35s.o
 obj-$(CONFIG_PANEL_TPO_TD043MTEA1) += panel-tpo-td043mtea1.o
 obj-$(CONFIG_PANEL_ACX565AKM) += panel-acx565akm.o
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index 07fbb8a733bb..dbd59b8e5b36 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -30,7 +30,7 @@
 #include <linux/backlight.h>
 #include <linux/fb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #define MIPID_CMD_READ_DISP_ID          0x04
 #define MIPID_CMD_READ_RED              0x06
@@ -534,6 +534,7 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 
         props.fb_blank = FB_BLANK_UNBLANK;
         props.power = FB_BLANK_UNBLANK;
+        props.type = BACKLIGHT_RAW;
 
         bldev = backlight_device_register("acx565akm", &md->spi->dev,
                         md, &acx565akm_bl_ops, &props);
@@ -587,6 +588,9 @@ 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);
@@ -644,6 +648,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-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
new file mode 100644
index 000000000000..9c90f75653fb
--- /dev/null
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -0,0 +1,441 @@
+/*
+ * Generic DPI Panels support
+ *
+ * Copyright (C) 2010 Canonical Ltd.
+ * Author: Bryan Wu <bryan.wu@canonical.com>
+ *
+ * LCD panel driver for Sharp LQ043T1DG01
+ *
+ * Copyright (C) 2009 Texas Instruments Inc
+ * Author: Vaibhav Hiremath <hvaibhav@ti.com>
+ *
+ * LCD panel driver for Toppoly TDO35S
+ *
+ * Copyright (C) 2009 CompuLab, Ltd.
+ * Author: Mike Rapoport <mike@compulab.co.il>
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <video/omapdss.h>
+
+#include <video/omap-panel-generic-dpi.h>
+
+struct panel_config {
+        struct omap_video_timings timings;
+
+        int acbi;       /* ac-bias pin transitions per interrupt */
+        /* Unit: line clocks */
+        int acb;        /* ac-bias pin frequency */
+
+        enum omap_panel_config config;
+
+        int power_on_delay;
+        int power_off_delay;
+
+        /*
+         * Used to match device to panel configuration
+         * when use generic panel driver
+         */
+        const char *name;
+};
+
+/* Panel configurations */
+static struct panel_config generic_dpi_panels[] = {
+        /* Generic Panel */
+        {
+                {
+                        .x_res          = 640,
+                        .y_res          = 480,
+
+                        .pixel_clock    = 23500,
+
+                        .hfp            = 48,
+                        .hsw            = 32,
+                        .hbp            = 80,
+
+                        .vfp            = 3,
+                        .vsw            = 4,
+                        .vbp            = 7,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT,
+                .power_on_delay         = 0,
+                .power_off_delay        = 0,
+                .name                   = "generic",
+        },
+
+        /* Sharp LQ043T1DG01 */
+        {
+                {
+                        .x_res          = 480,
+                        .y_res          = 272,
+
+                        .pixel_clock    = 9000,
+
+                        .hsw            = 42,
+                        .hfp            = 3,
+                        .hbp            = 2,
+
+                        .vsw            = 11,
+                        .vfp            = 3,
+                        .vbp            = 2,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                        OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO,
+                .power_on_delay         = 50,
+                .power_off_delay        = 100,
+                .name                   = "sharp_lq",
+        },
+
+        /* Sharp LS037V7DW01 */
+        {
+                {
+                        .x_res          = 480,
+                        .y_res          = 640,
+
+                        .pixel_clock    = 19200,
+
+                        .hsw            = 2,
+                        .hfp            = 1,
+                        .hbp            = 28,
+
+                        .vsw            = 1,
+                        .vfp            = 1,
+                        .vbp            = 1,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x28,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                                OMAP_DSS_LCD_IHS,
+                .power_on_delay         = 50,
+                .power_off_delay        = 100,
+                .name                   = "sharp_ls",
+        },
+
+        /* Toppoly TDO35S */
+        {
+                {
+                        .x_res          = 480,
+                        .y_res          = 640,
+
+                        .pixel_clock    = 26000,
+
+                        .hfp            = 104,
+                        .hsw            = 8,
+                        .hbp            = 8,
+
+                        .vfp            = 4,
+                        .vsw            = 2,
+                        .vbp            = 2,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                        OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IPC |
+                                        OMAP_DSS_LCD_ONOFF,
+                .power_on_delay         = 0,
+                .power_off_delay        = 0,
+                .name                   = "toppoly_tdo35s",
+        },
+
+        /* Samsung LTE430WQ-F0C */
+        {
+                {
+                        .x_res          = 480,
+                        .y_res          = 272,
+
+                        .pixel_clock    = 9200,
+
+                        .hfp            = 8,
+                        .hsw            = 41,
+                        .hbp            = 45 - 41,
+
+                        .vfp            = 4,
+                        .vsw            = 10,
+                        .vbp            = 12 - 10,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                                OMAP_DSS_LCD_IHS,
+                .power_on_delay         = 0,
+                .power_off_delay        = 0,
+                .name                   = "samsung_lte430wq_f0c",
+        },
+
+        /* Seiko 70WVW1TZ3Z3 */
+        {
+                {
+                        .x_res          = 800,
+                        .y_res          = 480,
+
+                        .pixel_clock    = 33000,
+
+                        .hsw            = 128,
+                        .hfp            = 10,
+                        .hbp            = 10,
+
+                        .vsw            = 2,
+                        .vfp            = 4,
+                        .vbp            = 11,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                                OMAP_DSS_LCD_IHS,
+                .power_on_delay         = 0,
+                .power_off_delay        = 0,
+                .name                   = "seiko_70wvw1tz3",
+        },
+
+        /* Powertip PH480272T */
+        {
+                {
+                        .x_res          = 480,
+                        .y_res          = 272,
+
+                        .pixel_clock    = 9000,
+
+                        .hsw            = 40,
+                        .hfp            = 2,
+                        .hbp            = 2,
+
+                        .vsw            = 10,
+                        .vfp            = 2,
+                        .vbp            = 2,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                          OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO,
+                .power_on_delay         = 0,
+                .power_off_delay        = 0,
+                .name                   = "powertip_ph480272t",
+        },
+};
+
+struct panel_drv_data {
+
+        struct omap_dss_device *dssdev;
+
+        struct panel_config *panel_config;
+};
+
+static inline struct panel_generic_dpi_data
+*get_panel_data(const struct omap_dss_device *dssdev)
+{
+        return (struct panel_generic_dpi_data *) dssdev->data;
+}
+
+static int generic_dpi_panel_power_on(struct omap_dss_device *dssdev)
+{
+        int r;
+        struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+        struct panel_config *panel_config = drv_data->panel_config;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
+        r = omapdss_dpi_display_enable(dssdev);
+        if (r)
+                goto err0;
+
+        /* wait couple of vsyncs until enabling the LCD */
+        if (panel_config->power_on_delay)
+                msleep(panel_config->power_on_delay);
+
+        if (panel_data->platform_enable) {
+                r = panel_data->platform_enable(dssdev);
+                if (r)
+                        goto err1;
+        }
+
+        return 0;
+err1:
+        omapdss_dpi_display_disable(dssdev);
+err0:
+        return r;
+}
+
+static void generic_dpi_panel_power_off(struct omap_dss_device *dssdev)
+{
+        struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+        struct panel_config *panel_config = drv_data->panel_config;
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
+        if (panel_data->platform_disable)
+                panel_data->platform_disable(dssdev);
+
+        /* wait couple of vsyncs after disabling the LCD */
+        if (panel_config->power_off_delay)
+                msleep(panel_config->power_off_delay);
+
+        omapdss_dpi_display_disable(dssdev);
+}
+
+static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
+{
+        struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
+        struct panel_config *panel_config = NULL;
+        struct panel_drv_data *drv_data = NULL;
+        int i;
+
+        dev_dbg(&dssdev->dev, "probe\n");
+
+        if (!panel_data || !panel_data->name)
+                return -EINVAL;
+
+        for (i = 0; i < ARRAY_SIZE(generic_dpi_panels); i++) {
+                if (strcmp(panel_data->name, generic_dpi_panels[i].name) == 0) {
+                        panel_config = &generic_dpi_panels[i];
+                        break;
+                }
+        }
+
+        if (!panel_config)
+                return -EINVAL;
+
+        dssdev->panel.config = panel_config->config;
+        dssdev->panel.timings = panel_config->timings;
+        dssdev->panel.acb = panel_config->acb;
+        dssdev->panel.acbi = panel_config->acbi;
+
+        drv_data = kzalloc(sizeof(*drv_data), GFP_KERNEL);
+        if (!drv_data)
+                return -ENOMEM;
+
+        drv_data->dssdev = dssdev;
+        drv_data->panel_config = panel_config;
+
+        dev_set_drvdata(&dssdev->dev, drv_data);
+
+        return 0;
+}
+
+static void __exit generic_dpi_panel_remove(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+        dev_dbg(&dssdev->dev, "remove\n");
+
+        kfree(drv_data);
+
+        dev_set_drvdata(&dssdev->dev, NULL);
+}
+
+static int generic_dpi_panel_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        r = generic_dpi_panel_power_on(dssdev);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void generic_dpi_panel_disable(struct omap_dss_device *dssdev)
+{
+        generic_dpi_panel_power_off(dssdev);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static int generic_dpi_panel_suspend(struct omap_dss_device *dssdev)
+{
+        generic_dpi_panel_power_off(dssdev);
+
+        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+        return 0;
+}
+
+static int generic_dpi_panel_resume(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        r = generic_dpi_panel_power_on(dssdev);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void generic_dpi_panel_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        dpi_set_timings(dssdev, timings);
+}
+
+static void generic_dpi_panel_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        *timings = dssdev->panel.timings;
+}
+
+static int generic_dpi_panel_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        return dpi_check_timings(dssdev, timings);
+}
+
+static struct omap_dss_driver dpi_driver = {
+        .probe          = generic_dpi_panel_probe,
+        .remove         = __exit_p(generic_dpi_panel_remove),
+
+        .enable         = generic_dpi_panel_enable,
+        .disable        = generic_dpi_panel_disable,
+        .suspend        = generic_dpi_panel_suspend,
+        .resume         = generic_dpi_panel_resume,
+
+        .set_timings    = generic_dpi_panel_set_timings,
+        .get_timings    = generic_dpi_panel_get_timings,
+        .check_timings  = generic_dpi_panel_check_timings,
+
+        .driver         = {
+                .name   = "generic_dpi_panel",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init generic_dpi_panel_drv_init(void)
+{
+        return omap_dss_register_driver(&dpi_driver);
+}
+
+static void __exit generic_dpi_panel_drv_exit(void)
+{
+        omap_dss_unregister_driver(&dpi_driver);
+}
+
+module_init(generic_dpi_panel_drv_init);
+module_exit(generic_dpi_panel_drv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-generic.c b/drivers/video/omap2/displays/panel-generic.c
deleted file mode 100644
index 300eff5de1b4..000000000000
--- a/drivers/video/omap2/displays/panel-generic.c
+++ /dev/null
@@ -1,168 +0,0 @@
-/*
- * Generic panel support
- *
- * Copyright (C) 2008 Nokia Corporation
- * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <linux/module.h>
-#include <linux/delay.h>
-
-#include <plat/display.h>
-
-static struct omap_video_timings generic_panel_timings = {
-        /* 640 x 480 @ 60 Hz  Reduced blanking VESA CVT 0.31M3-R */
-        .x_res          = 640,
-        .y_res          = 480,
-        .pixel_clock    = 23500,
-        .hfp            = 48,
-        .hsw            = 32,
-        .hbp            = 80,
-        .vfp            = 3,
-        .vsw            = 4,
-        .vbp            = 7,
-};
-
-static int generic_panel_power_on(struct omap_dss_device *dssdev)
-{
-        int r;
-
-        r = omapdss_dpi_display_enable(dssdev);
-        if (r)
-                goto err0;
-
-        if (dssdev->platform_enable) {
-                r = dssdev->platform_enable(dssdev);
-                if (r)
-                        goto err1;
-        }
-
-        return 0;
-err1:
-        omapdss_dpi_display_disable(dssdev);
-err0:
-        return r;
-}
-
-static void generic_panel_power_off(struct omap_dss_device *dssdev)
-{
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
-        omapdss_dpi_display_disable(dssdev);
-}
-
-static int generic_panel_probe(struct omap_dss_device *dssdev)
-{
-        dssdev->panel.config = OMAP_DSS_LCD_TFT;
-        dssdev->panel.timings = generic_panel_timings;
-
-        return 0;
-}
-
-static void generic_panel_remove(struct omap_dss_device *dssdev)
-{
-}
-
-static int generic_panel_enable(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        r = generic_panel_power_on(dssdev);
-        if (r)
-                return r;
-
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-        return 0;
-}
-
-static void generic_panel_disable(struct omap_dss_device *dssdev)
-{
-        generic_panel_power_off(dssdev);
-
-        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-}
-
-static int generic_panel_suspend(struct omap_dss_device *dssdev)
-{
-        generic_panel_power_off(dssdev);
-        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
-        return 0;
-}
-
-static int generic_panel_resume(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        r = generic_panel_power_on(dssdev);
-        if (r)
-                return r;
-
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-        return 0;
-}
-
-static void generic_panel_set_timings(struct omap_dss_device *dssdev,
-                struct omap_video_timings *timings)
-{
-        dpi_set_timings(dssdev, timings);
-}
-
-static void generic_panel_get_timings(struct omap_dss_device *dssdev,
-                struct omap_video_timings *timings)
-{
-        *timings = dssdev->panel.timings;
-}
-
-static int generic_panel_check_timings(struct omap_dss_device *dssdev,
-                struct omap_video_timings *timings)
-{
-        return dpi_check_timings(dssdev, timings);
-}
-
-static struct omap_dss_driver generic_driver = {
-        .probe          = generic_panel_probe,
-        .remove         = generic_panel_remove,
-
-        .enable         = generic_panel_enable,
-        .disable        = generic_panel_disable,
-        .suspend        = generic_panel_suspend,
-        .resume         = generic_panel_resume,
-
-        .set_timings    = generic_panel_set_timings,
-        .get_timings    = generic_panel_get_timings,
-        .check_timings  = generic_panel_check_timings,
-
-        .driver         = {
-                .name   = "generic_panel",
-                .owner  = THIS_MODULE,
-        },
-};
-
-static int __init generic_panel_drv_init(void)
-{
-        return omap_dss_register_driver(&generic_driver);
-}
-
-static void __exit generic_panel_drv_exit(void)
-{
-        omap_dss_unregister_driver(&generic_driver);
-}
-
-module_init(generic_panel_drv_init);
-module_exit(generic_panel_drv_exit);
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
new file mode 100644
index 000000000000..e0eb35be303e
--- /dev/null
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -0,0 +1,279 @@
+/*
+ * LCD panel driver for LG.Philips LB035Q02
+ *
+ * Author: Steve Sakoman <steve@sakoman.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/mutex.h>
+
+#include <video/omapdss.h>
+
+struct lb035q02_data {
+        struct mutex lock;
+};
+
+static struct omap_video_timings lb035q02_timings = {
+        .x_res = 320,
+        .y_res = 240,
+
+        .pixel_clock    = 6500,
+
+        .hsw            = 2,
+        .hfp            = 20,
+        .hbp            = 68,
+
+        .vsw            = 2,
+        .vfp            = 4,
+        .vbp            = 18,
+};
+
+static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
+{
+        int r;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
+        r = omapdss_dpi_display_enable(dssdev);
+        if (r)
+                goto err0;
+
+        if (dssdev->platform_enable) {
+                r = dssdev->platform_enable(dssdev);
+                if (r)
+                        goto err1;
+        }
+
+        return 0;
+err1:
+        omapdss_dpi_display_disable(dssdev);
+err0:
+        return r;
+}
+
+static void lb035q02_panel_power_off(struct omap_dss_device *dssdev)
+{
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+
+        omapdss_dpi_display_disable(dssdev);
+}
+
+static int lb035q02_panel_probe(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld;
+        int r;
+
+        dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                OMAP_DSS_LCD_IHS;
+        dssdev->panel.timings = lb035q02_timings;
+
+        ld = kzalloc(sizeof(*ld), GFP_KERNEL);
+        if (!ld) {
+                r = -ENOMEM;
+                goto err;
+        }
+        mutex_init(&ld->lock);
+        dev_set_drvdata(&dssdev->dev, ld);
+        return 0;
+err:
+        return r;
+}
+
+static void lb035q02_panel_remove(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+        kfree(ld);
+}
+
+static int lb035q02_panel_enable(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&ld->lock);
+
+        r = lb035q02_panel_power_on(dssdev);
+        if (r)
+                goto err;
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        mutex_unlock(&ld->lock);
+        return 0;
+err:
+        mutex_unlock(&ld->lock);
+        return r;
+}
+
+static void lb035q02_panel_disable(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&ld->lock);
+
+        lb035q02_panel_power_off(dssdev);
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        mutex_unlock(&ld->lock);
+}
+
+static int lb035q02_panel_suspend(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&ld->lock);
+
+        lb035q02_panel_power_off(dssdev);
+        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+        mutex_unlock(&ld->lock);
+        return 0;
+}
+
+static int lb035q02_panel_resume(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&ld->lock);
+
+        r = lb035q02_panel_power_on(dssdev);
+        if (r)
+                goto err;
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        mutex_unlock(&ld->lock);
+        return 0;
+err:
+        mutex_unlock(&ld->lock);
+        return r;
+}
+
+static struct omap_dss_driver lb035q02_driver = {
+        .probe          = lb035q02_panel_probe,
+        .remove         = lb035q02_panel_remove,
+
+        .enable         = lb035q02_panel_enable,
+        .disable        = lb035q02_panel_disable,
+        .suspend        = lb035q02_panel_suspend,
+        .resume         = lb035q02_panel_resume,
+
+        .driver         = {
+                .name   = "lgphilips_lb035q02_panel",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int lb035q02_write_reg(struct spi_device *spi, u8 reg, u16 val)
+{
+        struct spi_message msg;
+        struct spi_transfer index_xfer = {
+                .len            = 3,
+                .cs_change      = 1,
+        };
+        struct spi_transfer value_xfer = {
+                .len            = 3,
+        };
+        u8      buffer[16];
+
+        spi_message_init(&msg);
+
+        /* register index */
+        buffer[0] = 0x70;
+        buffer[1] = 0x00;
+        buffer[2] = reg & 0x7f;
+        index_xfer.tx_buf = buffer;
+        spi_message_add_tail(&index_xfer, &msg);
+
+        /* register value */
+        buffer[4] = 0x72;
+        buffer[5] = val >> 8;
+        buffer[6] = val;
+        value_xfer.tx_buf = buffer + 4;
+        spi_message_add_tail(&value_xfer, &msg);
+
+        return spi_sync(spi, &msg);
+}
+
+static void init_lb035q02_panel(struct spi_device *spi)
+{
+        /* Init sequence from page 28 of the lb035q02 spec */
+        lb035q02_write_reg(spi, 0x01, 0x6300);
+        lb035q02_write_reg(spi, 0x02, 0x0200);
+        lb035q02_write_reg(spi, 0x03, 0x0177);
+        lb035q02_write_reg(spi, 0x04, 0x04c7);
+        lb035q02_write_reg(spi, 0x05, 0xffc0);
+        lb035q02_write_reg(spi, 0x06, 0xe806);
+        lb035q02_write_reg(spi, 0x0a, 0x4008);
+        lb035q02_write_reg(spi, 0x0b, 0x0000);
+        lb035q02_write_reg(spi, 0x0d, 0x0030);
+        lb035q02_write_reg(spi, 0x0e, 0x2800);
+        lb035q02_write_reg(spi, 0x0f, 0x0000);
+        lb035q02_write_reg(spi, 0x16, 0x9f80);
+        lb035q02_write_reg(spi, 0x17, 0x0a0f);
+        lb035q02_write_reg(spi, 0x1e, 0x00c1);
+        lb035q02_write_reg(spi, 0x30, 0x0300);
+        lb035q02_write_reg(spi, 0x31, 0x0007);
+        lb035q02_write_reg(spi, 0x32, 0x0000);
+        lb035q02_write_reg(spi, 0x33, 0x0000);
+        lb035q02_write_reg(spi, 0x34, 0x0707);
+        lb035q02_write_reg(spi, 0x35, 0x0004);
+        lb035q02_write_reg(spi, 0x36, 0x0302);
+        lb035q02_write_reg(spi, 0x37, 0x0202);
+        lb035q02_write_reg(spi, 0x3a, 0x0a0d);
+        lb035q02_write_reg(spi, 0x3b, 0x0806);
+}
+
+static int __devinit lb035q02_panel_spi_probe(struct spi_device *spi)
+{
+        init_lb035q02_panel(spi);
+        return omap_dss_register_driver(&lb035q02_driver);
+}
+
+static int __devexit lb035q02_panel_spi_remove(struct spi_device *spi)
+{
+        omap_dss_unregister_driver(&lb035q02_driver);
+        return 0;
+}
+
+static struct spi_driver lb035q02_spi_driver = {
+        .driver         = {
+                .name   = "lgphilips_lb035q02_panel-spi",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = lb035q02_panel_spi_probe,
+        .remove         = __devexit_p(lb035q02_panel_spi_remove),
+};
+
+static int __init lb035q02_panel_drv_init(void)
+{
+        return spi_register_driver(&lb035q02_spi_driver);
+}
+
+static void __exit lb035q02_panel_drv_exit(void)
+{
+        spi_unregister_driver(&lb035q02_spi_driver);
+}
+
+module_init(lb035q02_panel_drv_init);
+module_exit(lb035q02_panel_drv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
new file mode 100644
index 000000000000..2ba9d0ca187c
--- /dev/null
+++ b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
@@ -0,0 +1,325 @@
+/*
+ * Support for NEC-nl8048hl11-01b panel driver
+ *
+ * Copyright (C) 2010 Texas Instruments Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/backlight.h>
+#include <linux/fb.h>
+
+#include <video/omapdss.h>
+
+#define LCD_XRES                800
+#define LCD_YRES                480
+/*
+ * NEC PIX Clock Ratings
+ * MIN:21.8MHz TYP:23.8MHz MAX:25.7MHz
+ */
+#define LCD_PIXEL_CLOCK         23800
+
+struct nec_8048_data {
+        struct backlight_device *bl;
+};
+
+static const struct {
+        unsigned char addr;
+        unsigned char dat;
+} nec_8048_init_seq[] = {
+        { 3, 0x01 }, { 0, 0x00 }, { 1, 0x01 }, { 4, 0x00 }, { 5, 0x14 },
+        { 6, 0x24 }, { 16, 0xD7 }, { 17, 0x00 }, { 18, 0x00 }, { 19, 0x55 },
+        { 20, 0x01 }, { 21, 0x70 }, { 22, 0x1E }, { 23, 0x25 }, { 24, 0x25 },
+        { 25, 0x02 }, { 26, 0x02 }, { 27, 0xA0 }, { 32, 0x2F }, { 33, 0x0F },
+        { 34, 0x0F }, { 35, 0x0F }, { 36, 0x0F }, { 37, 0x0F }, { 38, 0x0F },
+        { 39, 0x00 }, { 40, 0x02 }, { 41, 0x02 }, { 42, 0x02 }, { 43, 0x0F },
+        { 44, 0x0F }, { 45, 0x0F }, { 46, 0x0F }, { 47, 0x0F }, { 48, 0x0F },
+        { 49, 0x0F }, { 50, 0x00 }, { 51, 0x02 }, { 52, 0x02 }, { 53, 0x02 },
+        { 80, 0x0C }, { 83, 0x42 }, { 84, 0x42 }, { 85, 0x41 }, { 86, 0x14 },
+        { 89, 0x88 }, { 90, 0x01 }, { 91, 0x00 }, { 92, 0x02 }, { 93, 0x0C },
+        { 94, 0x1C }, { 95, 0x27 }, { 98, 0x49 }, { 99, 0x27 }, { 102, 0x76 },
+        { 103, 0x27 }, { 112, 0x01 }, { 113, 0x0E }, { 114, 0x02 },
+        { 115, 0x0C }, { 118, 0x0C }, { 121, 0x30 }, { 130, 0x00 },
+        { 131, 0x00 }, { 132, 0xFC }, { 134, 0x00 }, { 136, 0x00 },
+        { 138, 0x00 }, { 139, 0x00 }, { 140, 0x00 }, { 141, 0xFC },
+        { 143, 0x00 }, { 145, 0x00 }, { 147, 0x00 }, { 148, 0x00 },
+        { 149, 0x00 }, { 150, 0xFC }, { 152, 0x00 }, { 154, 0x00 },
+        { 156, 0x00 }, { 157, 0x00 }, { 2, 0x00 },
+};
+
+/*
+ * NEC NL8048HL11-01B  Manual
+ * defines HFB, HSW, HBP, VFP, VSW, VBP as shown below
+ */
+
+static struct omap_video_timings nec_8048_panel_timings = {
+        /* 800 x 480 @ 60 Hz  Reduced blanking VESA CVT 0.31M3-R */
+        .x_res          = LCD_XRES,
+        .y_res          = LCD_YRES,
+        .pixel_clock    = LCD_PIXEL_CLOCK,
+        .hfp            = 6,
+        .hsw            = 1,
+        .hbp            = 4,
+        .vfp            = 3,
+        .vsw            = 1,
+        .vbp            = 4,
+};
+
+static int nec_8048_bl_update_status(struct backlight_device *bl)
+{
+        struct omap_dss_device *dssdev = dev_get_drvdata(&bl->dev);
+        int level;
+
+        if (!dssdev->set_backlight)
+                return -EINVAL;
+
+        if (bl->props.fb_blank == FB_BLANK_UNBLANK &&
+                        bl->props.power == FB_BLANK_UNBLANK)
+                level = bl->props.brightness;
+        else
+                level = 0;
+
+        return dssdev->set_backlight(dssdev, level);
+}
+
+static int nec_8048_bl_get_brightness(struct backlight_device *bl)
+{
+        if (bl->props.fb_blank == FB_BLANK_UNBLANK &&
+                        bl->props.power == FB_BLANK_UNBLANK)
+                return bl->props.brightness;
+
+        return 0;
+}
+
+static const struct backlight_ops nec_8048_bl_ops = {
+        .get_brightness = nec_8048_bl_get_brightness,
+        .update_status  = nec_8048_bl_update_status,
+};
+
+static int nec_8048_panel_probe(struct omap_dss_device *dssdev)
+{
+        struct backlight_device *bl;
+        struct nec_8048_data *necd;
+        struct backlight_properties props;
+        int r;
+
+        dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_RF |
+                                OMAP_DSS_LCD_ONOFF;
+        dssdev->panel.timings = nec_8048_panel_timings;
+
+        necd = kzalloc(sizeof(*necd), GFP_KERNEL);
+        if (!necd)
+                return -ENOMEM;
+
+        dev_set_drvdata(&dssdev->dev, necd);
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = 255;
+
+        bl = backlight_device_register("nec-8048", &dssdev->dev, dssdev,
+                        &nec_8048_bl_ops, &props);
+        if (IS_ERR(bl)) {
+                r = PTR_ERR(bl);
+                kfree(necd);
+                return r;
+        }
+        necd->bl = bl;
+
+        bl->props.fb_blank = FB_BLANK_UNBLANK;
+        bl->props.power = FB_BLANK_UNBLANK;
+        bl->props.max_brightness = dssdev->max_backlight_level;
+        bl->props.brightness = dssdev->max_backlight_level;
+
+        r = nec_8048_bl_update_status(bl);
+        if (r < 0)
+                dev_err(&dssdev->dev, "failed to set lcd brightness\n");
+
+        return 0;
+}
+
+static void nec_8048_panel_remove(struct omap_dss_device *dssdev)
+{
+        struct nec_8048_data *necd = dev_get_drvdata(&dssdev->dev);
+        struct backlight_device *bl = necd->bl;
+
+        bl->props.power = FB_BLANK_POWERDOWN;
+        nec_8048_bl_update_status(bl);
+        backlight_device_unregister(bl);
+
+        kfree(necd);
+}
+
+static int nec_8048_panel_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+        struct nec_8048_data *necd = dev_get_drvdata(&dssdev->dev);
+        struct backlight_device *bl = necd->bl;
+
+        if (dssdev->platform_enable) {
+                r = dssdev->platform_enable(dssdev);
+                if (r)
+                        return r;
+        }
+
+        r = nec_8048_bl_update_status(bl);
+        if (r < 0)
+                dev_err(&dssdev->dev, "failed to set lcd brightness\n");
+
+        r = omapdss_dpi_display_enable(dssdev);
+
+        return r;
+}
+
+static void nec_8048_panel_disable(struct omap_dss_device *dssdev)
+{
+        struct nec_8048_data *necd = dev_get_drvdata(&dssdev->dev);
+        struct backlight_device *bl = necd->bl;
+
+        omapdss_dpi_display_disable(dssdev);
+
+        bl->props.brightness = 0;
+        nec_8048_bl_update_status(bl);
+
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+}
+
+static int nec_8048_panel_suspend(struct omap_dss_device *dssdev)
+{
+        nec_8048_panel_disable(dssdev);
+        return 0;
+}
+
+static int nec_8048_panel_resume(struct omap_dss_device *dssdev)
+{
+        return nec_8048_panel_enable(dssdev);
+}
+
+static int nec_8048_recommended_bpp(struct omap_dss_device *dssdev)
+{
+        return 16;
+}
+
+static struct omap_dss_driver nec_8048_driver = {
+        .probe                  = nec_8048_panel_probe,
+        .remove                 = nec_8048_panel_remove,
+        .enable                 = nec_8048_panel_enable,
+        .disable                = nec_8048_panel_disable,
+        .suspend                = nec_8048_panel_suspend,
+        .resume                 = nec_8048_panel_resume,
+        .get_recommended_bpp    = nec_8048_recommended_bpp,
+
+        .driver         = {
+                .name           = "NEC_8048_panel",
+                .owner          = THIS_MODULE,
+        },
+};
+
+static int nec_8048_spi_send(struct spi_device *spi, unsigned char reg_addr,
+                        unsigned char reg_data)
+{
+        int ret = 0;
+        unsigned int cmd = 0, data = 0;
+
+        cmd = 0x0000 | reg_addr; /* register address write */
+        data = 0x0100 | reg_data ; /* register data write */
+        data = (cmd << 16) | data;
+
+        ret = spi_write(spi, (unsigned char *)&data, 4);
+        if (ret)
+                pr_err("error in spi_write %x\n", data);
+
+        return ret;
+}
+
+static int init_nec_8048_wvga_lcd(struct spi_device *spi)
+{
+        unsigned int i;
+        /* Initialization Sequence */
+        /* nec_8048_spi_send(spi, REG, VAL) */
+        for (i = 0; i < (ARRAY_SIZE(nec_8048_init_seq) - 1); i++)
+                nec_8048_spi_send(spi, nec_8048_init_seq[i].addr,
+                                nec_8048_init_seq[i].dat);
+        udelay(20);
+        nec_8048_spi_send(spi, nec_8048_init_seq[i].addr,
+                                nec_8048_init_seq[i].dat);
+        return 0;
+}
+
+static int nec_8048_spi_probe(struct spi_device *spi)
+{
+        spi->mode = SPI_MODE_0;
+        spi->bits_per_word = 32;
+        spi_setup(spi);
+
+        init_nec_8048_wvga_lcd(spi);
+
+        return omap_dss_register_driver(&nec_8048_driver);
+}
+
+static int nec_8048_spi_remove(struct spi_device *spi)
+{
+        omap_dss_unregister_driver(&nec_8048_driver);
+
+        return 0;
+}
+
+static int nec_8048_spi_suspend(struct spi_device *spi, pm_message_t mesg)
+{
+        nec_8048_spi_send(spi, 2, 0x01);
+        mdelay(40);
+
+        return 0;
+}
+
+static int nec_8048_spi_resume(struct spi_device *spi)
+{
+        /* reinitialize the panel */
+        spi_setup(spi);
+        nec_8048_spi_send(spi, 2, 0x00);
+        init_nec_8048_wvga_lcd(spi);
+
+        return 0;
+}
+
+static struct spi_driver nec_8048_spi_driver = {
+        .probe          = nec_8048_spi_probe,
+        .remove         = __devexit_p(nec_8048_spi_remove),
+        .suspend        = nec_8048_spi_suspend,
+        .resume         = nec_8048_spi_resume,
+        .driver         = {
+                .name   = "nec_8048_spi",
+                .bus    = &spi_bus_type,
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init nec_8048_lcd_init(void)
+{
+        return spi_register_driver(&nec_8048_spi_driver);
+}
+
+static void __exit nec_8048_lcd_exit(void)
+{
+        return spi_unregister_driver(&nec_8048_spi_driver);
+}
+
+module_init(nec_8048_lcd_init);
+module_exit(nec_8048_lcd_exit);
+MODULE_AUTHOR("Erik Gilling <konkers@android.com>");
+MODULE_DESCRIPTION("NEC-nl8048hl11-01b Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c b/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c
deleted file mode 100644
index 10267461991c..000000000000
--- a/drivers/video/omap2/displays/panel-sharp-lq043t1dg01.c
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- * LCD panel driver for Sharp LQ043T1DG01
- *
- * Copyright (C) 2009 Texas Instruments Inc
- * Author: Vaibhav Hiremath <hvaibhav@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/module.h>
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/err.h>
-
-#include <plat/display.h>
-
-static struct omap_video_timings sharp_lq_timings = {
-        .x_res = 480,
-        .y_res = 272,
-
-        .pixel_clock    = 9000,
-
-        .hsw            = 42,
-        .hfp            = 3,
-        .hbp            = 2,
-
-        .vsw            = 11,
-        .vfp            = 3,
-        .vbp            = 2,
-};
-
-static int sharp_lq_panel_power_on(struct omap_dss_device *dssdev)
-{
-        int r;
-
-        r = omapdss_dpi_display_enable(dssdev);
-        if (r)
-                goto err0;
-
-        /* wait couple of vsyncs until enabling the LCD */
-        msleep(50);
-
-        if (dssdev->platform_enable) {
-                r = dssdev->platform_enable(dssdev);
-                if (r)
-                        goto err1;
-        }
-
-        return 0;
-err1:
-        omapdss_dpi_display_disable(dssdev);
-err0:
-        return r;
-}
-
-static void sharp_lq_panel_power_off(struct omap_dss_device *dssdev)
-{
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
-        /* wait at least 5 vsyncs after disabling the LCD */
-        msleep(100);
-
-        omapdss_dpi_display_disable(dssdev);
-}
-
-static int sharp_lq_panel_probe(struct omap_dss_device *dssdev)
-{
-
-        dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-                OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO;
-        dssdev->panel.acb = 0x0;
-        dssdev->panel.timings = sharp_lq_timings;
-
-        return 0;
-}
-
-static void sharp_lq_panel_remove(struct omap_dss_device *dssdev)
-{
-}
-
-static int sharp_lq_panel_enable(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        r = sharp_lq_panel_power_on(dssdev);
-        if (r)
-                return r;
-
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-        return 0;
-}
-
-static void sharp_lq_panel_disable(struct omap_dss_device *dssdev)
-{
-        sharp_lq_panel_power_off(dssdev);
-
-        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-}
-
-static int sharp_lq_panel_suspend(struct omap_dss_device *dssdev)
-{
-        sharp_lq_panel_power_off(dssdev);
-        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
-        return 0;
-}
-
-static int sharp_lq_panel_resume(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        r = sharp_lq_panel_power_on(dssdev);
-        if (r)
-                return r;
-
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-        return 0;
-}
-
-static struct omap_dss_driver sharp_lq_driver = {
-        .probe          = sharp_lq_panel_probe,
-        .remove         = sharp_lq_panel_remove,
-
-        .enable         = sharp_lq_panel_enable,
-        .disable        = sharp_lq_panel_disable,
-        .suspend        = sharp_lq_panel_suspend,
-        .resume         = sharp_lq_panel_resume,
-
-        .driver         = {
-                .name   = "sharp_lq_panel",
-                .owner  = THIS_MODULE,
-        },
-};
-
-static int __init sharp_lq_panel_drv_init(void)
-{
-        return omap_dss_register_driver(&sharp_lq_driver);
-}
-
-static void __exit sharp_lq_panel_drv_exit(void)
-{
-        omap_dss_unregister_driver(&sharp_lq_driver);
-}
-
-module_init(sharp_lq_panel_drv_init);
-module_exit(sharp_lq_panel_drv_exit);
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index 7d9eb2b1f5af..ba38b3ad17d6 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -25,7 +25,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 struct sharp_data {
         struct backlight_device *bl;
@@ -99,6 +99,7 @@ static int sharp_ls_panel_probe(struct omap_dss_device *dssdev)
 
         memset(&props, 0, sizeof(struct backlight_properties));
         props.max_brightness = dssdev->max_backlight_level;
+        props.type = BACKLIGHT_RAW;
 
         bl = backlight_device_register("sharp-ls", &dssdev->dev, dssdev,
                         &sharp_ls_bl_ops, &props);
@@ -119,7 +120,7 @@ static int sharp_ls_panel_probe(struct omap_dss_device *dssdev)
         return 0;
 }
 
-static void sharp_ls_panel_remove(struct omap_dss_device *dssdev)
+static void __exit sharp_ls_panel_remove(struct omap_dss_device *dssdev)
 {
         struct sharp_data *sd = dev_get_drvdata(&dssdev->dev);
         struct backlight_device *bl = sd->bl;
@@ -135,6 +136,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 +161,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);
 
@@ -198,7 +205,7 @@ static int sharp_ls_panel_resume(struct omap_dss_device *dssdev)
 
 static struct omap_dss_driver sharp_ls_driver = {
         .probe          = sharp_ls_panel_probe,
-        .remove         = sharp_ls_panel_remove,
+        .remove         = __exit_p(sharp_ls_panel_remove),
 
         .enable         = sharp_ls_panel_enable,
         .disable        = sharp_ls_panel_disable,
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index e1c765d11419..fdd5d4ae437d 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -33,8 +33,8 @@
 #include <linux/regulator/consumer.h>
 #include <linux/mutex.h>
 
-#include <plat/display.h>
-#include <plat/nokia-dsi-panel.h>
+#include <video/omapdss.h>
+#include <video/omap-panel-nokia-dsi.h>
 
 /* DSI Virtual channel. Hardcoded for now. */
 #define TCH 0
@@ -63,12 +63,12 @@
 #define DCS_GET_ID2             0xdb
 #define DCS_GET_ID3             0xdc
 
-#define TAAL_ESD_CHECK_PERIOD   msecs_to_jiffies(5000)
-
 static irqreturn_t taal_te_isr(int irq, void *data);
 static void taal_te_timeout_work_callback(struct work_struct *work);
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable);
 
+static int taal_panel_reset(struct omap_dss_device *dssdev);
+
 struct panel_regulator {
         struct regulator *regulator;
         const char *name;
@@ -218,6 +218,8 @@ struct taal_data {
                 u16 w;
                 u16 h;
         } update_region;
+        int channel;
+
         struct delayed_work te_timeout_work;
 
         bool use_dsi_bl;
@@ -227,8 +229,14 @@ struct taal_data {
 
         bool intro_printed;
 
-        struct workqueue_struct *esd_wq;
+        struct workqueue_struct *workqueue;
+
         struct delayed_work esd_work;
+        unsigned esd_interval;
+
+        bool ulps_enabled;
+        unsigned ulps_timeout;
+        struct delayed_work ulps_work;
 
         struct panel_config *panel_config;
 };
@@ -240,6 +248,7 @@ static inline struct nokia_dsi_panel_data
 }
 
 static void taal_esd_work(struct work_struct *work);
+static void taal_ulps_work(struct work_struct *work);
 
 static void hw_guard_start(struct taal_data *td, int guard_msec)
 {
@@ -257,12 +266,12 @@ static void hw_guard_wait(struct taal_data *td)
         }
 }
 
-static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
+static int taal_dcs_read_1(struct taal_data *td, u8 dcs_cmd, u8 *data)
 {
         int r;
         u8 buf[1];
 
-        r = dsi_vc_dcs_read(TCH, dcs_cmd, buf, 1);
+        r = dsi_vc_dcs_read(td->dssdev, td->channel, dcs_cmd, buf, 1);
 
         if (r < 0)
                 return r;
@@ -272,17 +281,17 @@ static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
         return 0;
 }
 
-static int taal_dcs_write_0(u8 dcs_cmd)
+static int taal_dcs_write_0(struct taal_data *td, u8 dcs_cmd)
 {
-        return dsi_vc_dcs_write(TCH, &dcs_cmd, 1);
+        return dsi_vc_dcs_write(td->dssdev, td->channel, &dcs_cmd, 1);
 }
 
-static int taal_dcs_write_1(u8 dcs_cmd, u8 param)
+static int taal_dcs_write_1(struct taal_data *td, u8 dcs_cmd, u8 param)
 {
         u8 buf[2];
         buf[0] = dcs_cmd;
         buf[1] = param;
-        return dsi_vc_dcs_write(TCH, buf, 2);
+        return dsi_vc_dcs_write(td->dssdev, td->channel, buf, 2);
 }
 
 static int taal_sleep_in(struct taal_data *td)
@@ -294,7 +303,7 @@ static int taal_sleep_in(struct taal_data *td)
         hw_guard_wait(td);
 
         cmd = DCS_SLEEP_IN;
-        r = dsi_vc_dcs_write_nosync(TCH, &cmd, 1);
+        r = dsi_vc_dcs_write_nosync(td->dssdev, td->channel, &cmd, 1);
         if (r)
                 return r;
 
@@ -312,7 +321,7 @@ static int taal_sleep_out(struct taal_data *td)
 
         hw_guard_wait(td);
 
-        r = taal_dcs_write_0(DCS_SLEEP_OUT);
+        r = taal_dcs_write_0(td, DCS_SLEEP_OUT);
         if (r)
                 return r;
 
@@ -324,30 +333,30 @@ static int taal_sleep_out(struct taal_data *td)
         return 0;
 }
 
-static int taal_get_id(u8 *id1, u8 *id2, u8 *id3)
+static int taal_get_id(struct taal_data *td, u8 *id1, u8 *id2, u8 *id3)
 {
         int r;
 
-        r = taal_dcs_read_1(DCS_GET_ID1, id1);
+        r = taal_dcs_read_1(td, DCS_GET_ID1, id1);
         if (r)
                 return r;
-        r = taal_dcs_read_1(DCS_GET_ID2, id2);
+        r = taal_dcs_read_1(td, DCS_GET_ID2, id2);
         if (r)
                 return r;
-        r = taal_dcs_read_1(DCS_GET_ID3, id3);
+        r = taal_dcs_read_1(td, DCS_GET_ID3, id3);
         if (r)
                 return r;
 
         return 0;
 }
 
-static int taal_set_addr_mode(u8 rotate, bool mirror)
+static int taal_set_addr_mode(struct taal_data *td, u8 rotate, bool mirror)
 {
         int r;
         u8 mode;
         int b5, b6, b7;
 
-        r = taal_dcs_read_1(DCS_READ_MADCTL, &mode);
+        r = taal_dcs_read_1(td, DCS_READ_MADCTL, &mode);
         if (r)
                 return r;
 
@@ -381,10 +390,11 @@ static int taal_set_addr_mode(u8 rotate, bool mirror)
         mode &= ~((1<<7) | (1<<6) | (1<<5));
         mode |= (b7 << 7) | (b6 << 6) | (b5 << 5);
 
-        return taal_dcs_write_1(DCS_MEM_ACC_CTRL, mode);
+        return taal_dcs_write_1(td, DCS_MEM_ACC_CTRL, mode);
 }
 
-static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
+static int taal_set_update_window(struct taal_data *td,
+                u16 x, u16 y, u16 w, u16 h)
 {
         int r;
         u16 x1 = x;
@@ -399,7 +409,7 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
         buf[3] = (x2 >> 8) & 0xff;
         buf[4] = (x2 >> 0) & 0xff;
 
-        r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+        r = dsi_vc_dcs_write_nosync(td->dssdev, td->channel, buf, sizeof(buf));
         if (r)
                 return r;
 
@@ -409,15 +419,132 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
         buf[3] = (y2 >> 8) & 0xff;
         buf[4] = (y2 >> 0) & 0xff;
 
-        r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+        r = dsi_vc_dcs_write_nosync(td->dssdev, td->channel, buf, sizeof(buf));
         if (r)
                 return r;
 
-        dsi_vc_send_bta_sync(TCH);
+        dsi_vc_send_bta_sync(td->dssdev, td->channel);
+
+        return r;
+}
+
+static void taal_queue_esd_work(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+        if (td->esd_interval > 0)
+                queue_delayed_work(td->workqueue, &td->esd_work,
+                                msecs_to_jiffies(td->esd_interval));
+}
+
+static void taal_cancel_esd_work(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+        cancel_delayed_work(&td->esd_work);
+}
+
+static void taal_queue_ulps_work(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+        if (td->ulps_timeout > 0)
+                queue_delayed_work(td->workqueue, &td->ulps_work,
+                                msecs_to_jiffies(td->ulps_timeout));
+}
+
+static void taal_cancel_ulps_work(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+        cancel_delayed_work(&td->ulps_work);
+}
+
+static int taal_enter_ulps(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+        int r;
+
+        if (td->ulps_enabled)
+                return 0;
+
+        taal_cancel_ulps_work(dssdev);
+
+        r = _taal_enable_te(dssdev, false);
+        if (r)
+                goto err;
+
+        disable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+
+        omapdss_dsi_display_disable(dssdev, false, true);
+
+        td->ulps_enabled = true;
+
+        return 0;
+
+err:
+        dev_err(&dssdev->dev, "enter ULPS failed");
+        taal_panel_reset(dssdev);
+
+        td->ulps_enabled = false;
+
+        taal_queue_ulps_work(dssdev);
+
+        return r;
+}
+
+static int taal_exit_ulps(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+        int r;
+
+        if (!td->ulps_enabled)
+                return 0;
+
+        r = omapdss_dsi_display_enable(dssdev);
+        if (r)
+                goto err;
+
+        omapdss_dsi_vc_enable_hs(dssdev, td->channel, true);
+
+        r = _taal_enable_te(dssdev, true);
+        if (r)
+                goto err;
+
+        enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+
+        taal_queue_ulps_work(dssdev);
+
+        td->ulps_enabled = false;
+
+        return 0;
+
+err:
+        dev_err(&dssdev->dev, "exit ULPS failed");
+        r = taal_panel_reset(dssdev);
+
+        enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+        td->ulps_enabled = false;
+
+        taal_queue_ulps_work(dssdev);
 
         return r;
 }
 
+static int taal_wake_up(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+        if (td->ulps_enabled)
+                return taal_exit_ulps(dssdev);
+
+        taal_cancel_ulps_work(dssdev);
+        taal_queue_ulps_work(dssdev);
+        return 0;
+}
+
 static int taal_bl_update_status(struct backlight_device *dev)
 {
         struct omap_dss_device *dssdev = dev_get_drvdata(&dev->dev);
@@ -438,9 +565,13 @@ static int taal_bl_update_status(struct backlight_device *dev)
 
         if (td->use_dsi_bl) {
                 if (td->enabled) {
-                        dsi_bus_lock();
-                        r = taal_dcs_write_1(DCS_BRIGHTNESS, level);
-                        dsi_bus_unlock();
+                        dsi_bus_lock(dssdev);
+
+                        r = taal_wake_up(dssdev);
+                        if (!r)
+                                r = taal_dcs_write_1(td, DCS_BRIGHTNESS, level);
+
+                        dsi_bus_unlock(dssdev);
                 } else {
                         r = 0;
                 }
@@ -465,7 +596,7 @@ static int taal_bl_get_intensity(struct backlight_device *dev)
         return 0;
 }
 
-static struct backlight_ops taal_bl_ops = {
+static const struct backlight_ops taal_bl_ops = {
         .get_brightness = taal_bl_get_intensity,
         .update_status  = taal_bl_update_status,
 };
@@ -501,9 +632,13 @@ static ssize_t taal_num_errors_show(struct device *dev,
         mutex_lock(&td->lock);
 
         if (td->enabled) {
-                dsi_bus_lock();
-                r = taal_dcs_read_1(DCS_READ_NUM_ERRORS, &errors);
-                dsi_bus_unlock();
+                dsi_bus_lock(dssdev);
+
+                r = taal_wake_up(dssdev);
+                if (!r)
+                        r = taal_dcs_read_1(td, DCS_READ_NUM_ERRORS, &errors);
+
+                dsi_bus_unlock(dssdev);
         } else {
                 r = -ENODEV;
         }
@@ -527,9 +662,13 @@ static ssize_t taal_hw_revision_show(struct device *dev,
         mutex_lock(&td->lock);
 
         if (td->enabled) {
-                dsi_bus_lock();
-                r = taal_get_id(&id1, &id2, &id3);
-                dsi_bus_unlock();
+                dsi_bus_lock(dssdev);
+
+                r = taal_wake_up(dssdev);
+                if (!r)
+                        r = taal_get_id(td, &id1, &id2, &id3);
+
+                dsi_bus_unlock(dssdev);
         } else {
                 r = -ENODEV;
         }
@@ -576,6 +715,7 @@ static ssize_t store_cabc_mode(struct device *dev,
         struct omap_dss_device *dssdev = to_dss_device(dev);
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
         int i;
+        int r;
 
         for (i = 0; i < ARRAY_SIZE(cabc_modes); i++) {
                 if (sysfs_streq(cabc_modes[i], buf))
@@ -588,10 +728,19 @@ static ssize_t store_cabc_mode(struct device *dev,
         mutex_lock(&td->lock);
 
         if (td->enabled) {
-                dsi_bus_lock();
-                if (!td->cabc_broken)
-                        taal_dcs_write_1(DCS_WRITE_CABC, i);
-                dsi_bus_unlock();
+                dsi_bus_lock(dssdev);
+
+                if (!td->cabc_broken) {
+                        r = taal_wake_up(dssdev);
+                        if (r)
+                                goto err;
+
+                        r = taal_dcs_write_1(td, DCS_WRITE_CABC, i);
+                        if (r)
+                                goto err;
+                }
+
+                dsi_bus_unlock(dssdev);
         }
 
         td->cabc_mode = i;
@@ -599,6 +748,10 @@ static ssize_t store_cabc_mode(struct device *dev,
         mutex_unlock(&td->lock);
 
         return count;
+err:
+        dsi_bus_unlock(dssdev);
+        mutex_unlock(&td->lock);
+        return r;
 }
 
 static ssize_t show_cabc_available_modes(struct device *dev,
@@ -617,18 +770,161 @@ static ssize_t show_cabc_available_modes(struct device *dev,
         return len < PAGE_SIZE ? len : PAGE_SIZE - 1;
 }
 
+static ssize_t taal_store_esd_interval(struct device *dev,
+                struct device_attribute *attr,
+                const char *buf, size_t count)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+
+        unsigned long t;
+        int r;
+
+        r = strict_strtoul(buf, 10, &t);
+        if (r)
+                return r;
+
+        mutex_lock(&td->lock);
+        taal_cancel_esd_work(dssdev);
+        td->esd_interval = t;
+        if (td->enabled)
+                taal_queue_esd_work(dssdev);
+        mutex_unlock(&td->lock);
+
+        return count;
+}
+
+static ssize_t taal_show_esd_interval(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        unsigned t;
+
+        mutex_lock(&td->lock);
+        t = td->esd_interval;
+        mutex_unlock(&td->lock);
+
+        return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static ssize_t taal_store_ulps(struct device *dev,
+                struct device_attribute *attr,
+                const char *buf, size_t count)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        unsigned long t;
+        int r;
+
+        r = strict_strtoul(buf, 10, &t);
+        if (r)
+                return r;
+
+        mutex_lock(&td->lock);
+
+        if (td->enabled) {
+                dsi_bus_lock(dssdev);
+
+                if (t)
+                        r = taal_enter_ulps(dssdev);
+                else
+                        r = taal_wake_up(dssdev);
+
+                dsi_bus_unlock(dssdev);
+        }
+
+        mutex_unlock(&td->lock);
+
+        if (r)
+                return r;
+
+        return count;
+}
+
+static ssize_t taal_show_ulps(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        unsigned t;
+
+        mutex_lock(&td->lock);
+        t = td->ulps_enabled;
+        mutex_unlock(&td->lock);
+
+        return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static ssize_t taal_store_ulps_timeout(struct device *dev,
+                struct device_attribute *attr,
+                const char *buf, size_t count)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        unsigned long t;
+        int r;
+
+        r = strict_strtoul(buf, 10, &t);
+        if (r)
+                return r;
+
+        mutex_lock(&td->lock);
+        td->ulps_timeout = t;
+
+        if (td->enabled) {
+                /* taal_wake_up will restart the timer */
+                dsi_bus_lock(dssdev);
+                r = taal_wake_up(dssdev);
+                dsi_bus_unlock(dssdev);
+        }
+
+        mutex_unlock(&td->lock);
+
+        if (r)
+                return r;
+
+        return count;
+}
+
+static ssize_t taal_show_ulps_timeout(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct omap_dss_device *dssdev = to_dss_device(dev);
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        unsigned t;
+
+        mutex_lock(&td->lock);
+        t = td->ulps_timeout;
+        mutex_unlock(&td->lock);
+
+        return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
 static DEVICE_ATTR(num_dsi_errors, S_IRUGO, taal_num_errors_show, NULL);
 static DEVICE_ATTR(hw_revision, S_IRUGO, taal_hw_revision_show, NULL);
 static DEVICE_ATTR(cabc_mode, S_IRUGO | S_IWUSR,
                 show_cabc_mode, store_cabc_mode);
 static DEVICE_ATTR(cabc_available_modes, S_IRUGO,
                 show_cabc_available_modes, NULL);
+static DEVICE_ATTR(esd_interval, S_IRUGO | S_IWUSR,
+                taal_show_esd_interval, taal_store_esd_interval);
+static DEVICE_ATTR(ulps, S_IRUGO | S_IWUSR,
+                taal_show_ulps, taal_store_ulps);
+static DEVICE_ATTR(ulps_timeout, S_IRUGO | S_IWUSR,
+                taal_show_ulps_timeout, taal_store_ulps_timeout);
 
 static struct attribute *taal_attrs[] = {
         &dev_attr_num_dsi_errors.attr,
         &dev_attr_hw_revision.attr,
         &dev_attr_cabc_mode.attr,
         &dev_attr_cabc_available_modes.attr,
+        &dev_attr_esd_interval.attr,
+        &dev_attr_ulps.attr,
+        &dev_attr_ulps_timeout.attr,
         NULL,
 };
 
@@ -697,6 +993,9 @@ static int taal_probe(struct omap_dss_device *dssdev)
         }
         td->dssdev = dssdev;
         td->panel_config = panel_config;
+        td->esd_interval = panel_data->esd_interval;
+        td->ulps_enabled = false;
+        td->ulps_timeout = panel_data->ulps_timeout;
 
         mutex_init(&td->lock);
 
@@ -707,13 +1006,14 @@ static int taal_probe(struct omap_dss_device *dssdev)
         if (r)
                 goto err_reg;
 
-        td->esd_wq = create_singlethread_workqueue("taal_esd");
-        if (td->esd_wq == NULL) {
+        td->workqueue = create_singlethread_workqueue("taal_esd");
+        if (td->workqueue == NULL) {
                 dev_err(&dssdev->dev, "can't create ESD workqueue\n");
                 r = -ENOMEM;
                 goto err_wq;
         }
         INIT_DELAYED_WORK_DEFERRABLE(&td->esd_work, taal_esd_work);
+        INIT_DELAYED_WORK(&td->ulps_work, taal_ulps_work);
 
         dev_set_drvdata(&dssdev->dev, td);
 
@@ -729,8 +1029,10 @@ static int taal_probe(struct omap_dss_device *dssdev)
                 props.max_brightness = 255;
         else
                 props.max_brightness = 127;
-        bldev = backlight_device_register("taal", &dssdev->dev, dssdev,
-                                          &taal_bl_ops, &props);
+
+        props.type = BACKLIGHT_RAW;
+        bldev = backlight_device_register(dev_name(&dssdev->dev), &dssdev->dev,
+                                        dssdev, &taal_bl_ops, &props);
         if (IS_ERR(bldev)) {
                 r = PTR_ERR(bldev);
                 goto err_bl;
@@ -774,14 +1076,29 @@ static int taal_probe(struct omap_dss_device *dssdev)
                 dev_dbg(&dssdev->dev, "Using GPIO TE\n");
         }
 
+        r = omap_dsi_request_vc(dssdev, &td->channel);
+        if (r) {
+                dev_err(&dssdev->dev, "failed to get virtual channel\n");
+                goto err_req_vc;
+        }
+
+        r = omap_dsi_set_vc_id(dssdev, td->channel, TCH);
+        if (r) {
+                dev_err(&dssdev->dev, "failed to set VC_ID\n");
+                goto err_vc_id;
+        }
+
         r = sysfs_create_group(&dssdev->dev.kobj, &taal_attr_group);
         if (r) {
                 dev_err(&dssdev->dev, "failed to create sysfs files\n");
-                goto err_sysfs;
+                goto err_vc_id;
         }
 
         return 0;
-err_sysfs:
+
+err_vc_id:
+        omap_dsi_release_vc(dssdev, td->channel);
+err_req_vc:
         if (panel_data->use_ext_te)
                 free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
 err_irq:
@@ -790,7 +1107,7 @@ err_irq:
 err_gpio:
         backlight_device_unregister(bldev);
 err_bl:
-        destroy_workqueue(td->esd_wq);
+        destroy_workqueue(td->workqueue);
 err_wq:
         free_regulators(panel_config->regulators, panel_config->num_regulators);
 err_reg:
@@ -799,7 +1116,7 @@ err:
         return r;
 }
 
-static void taal_remove(struct omap_dss_device *dssdev)
+static void __exit taal_remove(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
         struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
@@ -808,6 +1125,7 @@ static void taal_remove(struct omap_dss_device *dssdev)
         dev_dbg(&dssdev->dev, "remove\n");
 
         sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
+        omap_dsi_release_vc(dssdev, td->channel);
 
         if (panel_data->use_ext_te) {
                 int gpio = panel_data->ext_te_gpio;
@@ -820,8 +1138,9 @@ static void taal_remove(struct omap_dss_device *dssdev)
         taal_bl_update_status(bldev);
         backlight_device_unregister(bldev);
 
-        cancel_delayed_work(&td->esd_work);
-        destroy_workqueue(td->esd_wq);
+        taal_cancel_ulps_work(dssdev);
+        taal_cancel_esd_work(dssdev);
+        destroy_workqueue(td->workqueue);
 
         /* reset, to be sure that the panel is in a valid state */
         taal_hw_reset(dssdev);
@@ -846,13 +1165,13 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 
         taal_hw_reset(dssdev);
 
-        omapdss_dsi_vc_enable_hs(TCH, false);
+        omapdss_dsi_vc_enable_hs(dssdev, td->channel, false);
 
         r = taal_sleep_out(td);
         if (r)
                 goto err;
 
-        r = taal_get_id(&id1, &id2, &id3);
+        r = taal_get_id(td, &id1, &id2, &id3);
         if (r)
                 goto err;
 
@@ -861,30 +1180,30 @@ static int taal_power_on(struct omap_dss_device *dssdev)
                 (id2 == 0x00 || id2 == 0xff || id2 == 0x81))
                 td->cabc_broken = true;
 
-        r = taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
+        r = taal_dcs_write_1(td, DCS_BRIGHTNESS, 0xff);
         if (r)
                 goto err;
 
-        r = taal_dcs_write_1(DCS_CTRL_DISPLAY,
+        r = taal_dcs_write_1(td, DCS_CTRL_DISPLAY,
                         (1<<2) | (1<<5));       /* BL | BCTRL */
         if (r)
                 goto err;
 
-        r = taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+        r = taal_dcs_write_1(td, DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
         if (r)
                 goto err;
 
-        r = taal_set_addr_mode(td->rotate, td->mirror);
+        r = taal_set_addr_mode(td, td->rotate, td->mirror);
         if (r)
                 goto err;
 
         if (!td->cabc_broken) {
-                r = taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+                r = taal_dcs_write_1(td, DCS_WRITE_CABC, td->cabc_mode);
                 if (r)
                         goto err;
         }
 
-        r = taal_dcs_write_0(DCS_DISPLAY_ON);
+        r = taal_dcs_write_0(td, DCS_DISPLAY_ON);
         if (r)
                 goto err;
 
@@ -903,7 +1222,7 @@ static int taal_power_on(struct omap_dss_device *dssdev)
                 td->intro_printed = true;
         }
 
-        omapdss_dsi_vc_enable_hs(TCH, true);
+        omapdss_dsi_vc_enable_hs(dssdev, td->channel, true);
 
         return 0;
 err:
@@ -911,7 +1230,7 @@ err:
 
         taal_hw_reset(dssdev);
 
-        omapdss_dsi_display_disable(dssdev);
+        omapdss_dsi_display_disable(dssdev, true, false);
 err0:
         return r;
 }
@@ -921,7 +1240,7 @@ static void taal_power_off(struct omap_dss_device *dssdev)
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
         int r;
 
-        r = taal_dcs_write_0(DCS_DISPLAY_OFF);
+        r = taal_dcs_write_0(td, DCS_DISPLAY_OFF);
         if (!r) {
                 r = taal_sleep_in(td);
                 /* HACK: wait a bit so that the message goes through */
@@ -934,15 +1253,23 @@ static void taal_power_off(struct omap_dss_device *dssdev)
                 taal_hw_reset(dssdev);
         }
 
-        omapdss_dsi_display_disable(dssdev);
+        omapdss_dsi_display_disable(dssdev, true, false);
 
         td->enabled = 0;
 }
 
+static int taal_panel_reset(struct omap_dss_device *dssdev)
+{
+        dev_err(&dssdev->dev, "performing LCD reset\n");
+
+        taal_power_off(dssdev);
+        taal_hw_reset(dssdev);
+        return taal_power_on(dssdev);
+}
+
 static int taal_enable(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         dev_dbg(&dssdev->dev, "enable\n");
@@ -954,18 +1281,16 @@ static int taal_enable(struct omap_dss_device *dssdev)
                 goto err;
         }
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
 
         r = taal_power_on(dssdev);
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 
         if (r)
                 goto err;
 
-        if (panel_data->use_esd_check)
-                queue_delayed_work(td->esd_wq, &td->esd_work,
-                                TAAL_ESD_CHECK_PERIOD);
+        taal_queue_esd_work(dssdev);
 
         dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
@@ -986,14 +1311,17 @@ static void taal_disable(struct omap_dss_device *dssdev)
 
         mutex_lock(&td->lock);
 
-        cancel_delayed_work(&td->esd_work);
+        taal_cancel_ulps_work(dssdev);
+        taal_cancel_esd_work(dssdev);
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
 
-        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+                taal_wake_up(dssdev);
                 taal_power_off(dssdev);
+        }
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 
         dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 
@@ -1014,13 +1342,16 @@ static int taal_suspend(struct omap_dss_device *dssdev)
                 goto err;
         }
 
-        cancel_delayed_work(&td->esd_work);
+        taal_cancel_ulps_work(dssdev);
+        taal_cancel_esd_work(dssdev);
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
 
-        taal_power_off(dssdev);
+        r = taal_wake_up(dssdev);
+        if (!r)
+                taal_power_off(dssdev);
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 
         dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
@@ -1035,7 +1366,6 @@ err:
 static int taal_resume(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         dev_dbg(&dssdev->dev, "resume\n");
@@ -1047,19 +1377,17 @@ static int taal_resume(struct omap_dss_device *dssdev)
                 goto err;
         }
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
 
         r = taal_power_on(dssdev);
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 
         if (r) {
                 dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
         } else {
                 dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-                if (panel_data->use_esd_check)
-                        queue_delayed_work(td->esd_wq, &td->esd_work,
-                                        TAAL_ESD_CHECK_PERIOD);
+                taal_queue_esd_work(dssdev);
         }
 
         mutex_unlock(&td->lock);
@@ -1074,7 +1402,7 @@ static void taal_framedone_cb(int err, void *data)
 {
         struct omap_dss_device *dssdev = data;
         dev_dbg(&dssdev->dev, "framedone, err %d\n", err);
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 }
 
 static irqreturn_t taal_te_isr(int irq, void *data)
@@ -1089,7 +1417,7 @@ static irqreturn_t taal_te_isr(int irq, void *data)
         if (old) {
                 cancel_delayed_work(&td->te_timeout_work);
 
-                r = omap_dsi_update(dssdev, TCH,
+                r = omap_dsi_update(dssdev, td->channel,
                                 td->update_region.x,
                                 td->update_region.y,
                                 td->update_region.w,
@@ -1102,7 +1430,7 @@ static irqreturn_t taal_te_isr(int irq, void *data)
         return IRQ_HANDLED;
 err:
         dev_err(&dssdev->dev, "start update failed\n");
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
         return IRQ_HANDLED;
 }
 
@@ -1115,7 +1443,7 @@ static void taal_te_timeout_work_callback(struct work_struct *work)
         dev_err(&dssdev->dev, "TE not received for 250ms!\n");
 
         atomic_set(&td->do_update, 0);
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 }
 
 static int taal_update(struct omap_dss_device *dssdev,
@@ -1128,7 +1456,11 @@ static int taal_update(struct omap_dss_device *dssdev,
         dev_dbg(&dssdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
 
         mutex_lock(&td->lock);
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
+
+        r = taal_wake_up(dssdev);
+        if (r)
+                goto err;
 
         if (!td->enabled) {
                 r = 0;
@@ -1139,7 +1471,7 @@ static int taal_update(struct omap_dss_device *dssdev,
         if (r)
                 goto err;
 
-        r = taal_set_update_window(x, y, w, h);
+        r = taal_set_update_window(td, x, y, w, h);
         if (r)
                 goto err;
 
@@ -1153,7 +1485,7 @@ static int taal_update(struct omap_dss_device *dssdev,
                                 msecs_to_jiffies(250));
                 atomic_set(&td->do_update, 1);
         } else {
-                r = omap_dsi_update(dssdev, TCH, x, y, w, h,
+                r = omap_dsi_update(dssdev, td->channel, x, y, w, h,
                                 taal_framedone_cb, dssdev);
                 if (r)
                         goto err;
@@ -1163,7 +1495,7 @@ static int taal_update(struct omap_dss_device *dssdev,
         mutex_unlock(&td->lock);
         return 0;
 err:
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
         mutex_unlock(&td->lock);
         return r;
 }
@@ -1175,8 +1507,8 @@ static int taal_sync(struct omap_dss_device *dssdev)
         dev_dbg(&dssdev->dev, "sync\n");
 
         mutex_lock(&td->lock);
-        dsi_bus_lock();
-        dsi_bus_unlock();
+        dsi_bus_lock(dssdev);
+        dsi_bus_unlock(dssdev);
         mutex_unlock(&td->lock);
 
         dev_dbg(&dssdev->dev, "sync done\n");
@@ -1191,9 +1523,9 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
         int r;
 
         if (enable)
-                r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+                r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
         else
-                r = taal_dcs_write_0(DCS_TEAR_OFF);
+                r = taal_dcs_write_0(td, DCS_TEAR_OFF);
 
         if (!panel_data->use_ext_te)
                 omapdss_dsi_enable_te(dssdev, enable);
@@ -1214,9 +1546,13 @@ static int taal_enable_te(struct omap_dss_device *dssdev, bool enable)
         if (td->te_enabled == enable)
                 goto end;
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
 
         if (td->enabled) {
+                r = taal_wake_up(dssdev);
+                if (r)
+                        goto err;
+
                 r = _taal_enable_te(dssdev, enable);
                 if (r)
                         goto err;
@@ -1224,13 +1560,13 @@ static int taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 
         td->te_enabled = enable;
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 end:
         mutex_unlock(&td->lock);
 
         return 0;
 err:
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
         mutex_unlock(&td->lock);
 
         return r;
@@ -1260,22 +1596,26 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
         if (td->rotate == rotate)
                 goto end;
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
 
         if (td->enabled) {
-                r = taal_set_addr_mode(rotate, td->mirror);
+                r = taal_wake_up(dssdev);
+                if (r)
+                        goto err;
+
+                r = taal_set_addr_mode(td, rotate, td->mirror);
                 if (r)
                         goto err;
         }
 
         td->rotate = rotate;
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 end:
         mutex_unlock(&td->lock);
         return 0;
 err:
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
         mutex_unlock(&td->lock);
         return r;
 }
@@ -1304,21 +1644,25 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
         if (td->mirror == enable)
                 goto end;
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
         if (td->enabled) {
-                r = taal_set_addr_mode(td->rotate, enable);
+                r = taal_wake_up(dssdev);
+                if (r)
+                        goto err;
+
+                r = taal_set_addr_mode(td, td->rotate, enable);
                 if (r)
                         goto err;
         }
 
         td->mirror = enable;
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 end:
         mutex_unlock(&td->lock);
         return 0;
 err:
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
         mutex_unlock(&td->lock);
         return r;
 }
@@ -1348,23 +1692,27 @@ static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
                 goto err1;
         }
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
+
+        r = taal_wake_up(dssdev);
+        if (r)
+                goto err2;
 
-        r = taal_dcs_read_1(DCS_GET_ID1, &id1);
+        r = taal_dcs_read_1(td, DCS_GET_ID1, &id1);
         if (r)
                 goto err2;
-        r = taal_dcs_read_1(DCS_GET_ID2, &id2);
+        r = taal_dcs_read_1(td, DCS_GET_ID2, &id2);
         if (r)
                 goto err2;
-        r = taal_dcs_read_1(DCS_GET_ID3, &id3);
+        r = taal_dcs_read_1(td, DCS_GET_ID3, &id3);
         if (r)
                 goto err2;
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
         mutex_unlock(&td->lock);
         return 0;
 err2:
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 err1:
         mutex_unlock(&td->lock);
         return r;
@@ -1394,7 +1742,11 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
                         dssdev->panel.timings.x_res *
                         dssdev->panel.timings.y_res * 3);
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
+
+        r = taal_wake_up(dssdev);
+        if (r)
+                goto err2;
 
         /* plen 1 or 2 goes into short packet. until checksum error is fixed,
          * use short packets. plen 32 works, but bigger packets seem to cause
@@ -1404,9 +1756,9 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
         else
                 plen = 2;
 
-        taal_set_update_window(x, y, w, h);
+        taal_set_update_window(td, x, y, w, h);
 
-        r = dsi_vc_set_max_rx_packet_size(TCH, plen);
+        r = dsi_vc_set_max_rx_packet_size(dssdev, td->channel, plen);
         if (r)
                 goto err2;
 
@@ -1414,7 +1766,7 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
                 u8 dcs_cmd = first ? 0x2e : 0x3e;
                 first = 0;
 
-                r = dsi_vc_dcs_read(TCH, dcs_cmd,
+                r = dsi_vc_dcs_read(dssdev, td->channel, dcs_cmd,
                                 buf + buf_used, size - buf_used);
 
                 if (r < 0) {
@@ -1440,14 +1792,35 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
         r = buf_used;
 
 err3:
-        dsi_vc_set_max_rx_packet_size(TCH, 1);
+        dsi_vc_set_max_rx_packet_size(dssdev, td->channel, 1);
 err2:
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 err1:
         mutex_unlock(&td->lock);
         return r;
 }
 
+static void taal_ulps_work(struct work_struct *work)
+{
+        struct taal_data *td = container_of(work, struct taal_data,
+                        ulps_work.work);
+        struct omap_dss_device *dssdev = td->dssdev;
+
+        mutex_lock(&td->lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE || !td->enabled) {
+                mutex_unlock(&td->lock);
+                return;
+        }
+
+        dsi_bus_lock(dssdev);
+
+        taal_enter_ulps(dssdev);
+
+        dsi_bus_unlock(dssdev);
+        mutex_unlock(&td->lock);
+}
+
 static void taal_esd_work(struct work_struct *work)
 {
         struct taal_data *td = container_of(work, struct taal_data,
@@ -1464,9 +1837,15 @@ static void taal_esd_work(struct work_struct *work)
                 return;
         }
 
-        dsi_bus_lock();
+        dsi_bus_lock(dssdev);
+
+        r = taal_wake_up(dssdev);
+        if (r) {
+                dev_err(&dssdev->dev, "failed to exit ULPS\n");
+                goto err;
+        }
 
-        r = taal_dcs_read_1(DCS_RDDSDR, &state1);
+        r = taal_dcs_read_1(td, DCS_RDDSDR, &state1);
         if (r) {
                 dev_err(&dssdev->dev, "failed to read Taal status\n");
                 goto err;
@@ -1479,7 +1858,7 @@ static void taal_esd_work(struct work_struct *work)
                 goto err;
         }
 
-        r = taal_dcs_read_1(DCS_RDDSDR, &state2);
+        r = taal_dcs_read_1(td, DCS_RDDSDR, &state2);
         if (r) {
                 dev_err(&dssdev->dev, "failed to read Taal status\n");
                 goto err;
@@ -1495,27 +1874,25 @@ static void taal_esd_work(struct work_struct *work)
         /* Self-diagnostics result is also shown on TE GPIO line. We need
          * to re-enable TE after self diagnostics */
         if (td->te_enabled && panel_data->use_ext_te) {
-                r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+                r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
                 if (r)
                         goto err;
         }
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 
-        queue_delayed_work(td->esd_wq, &td->esd_work, TAAL_ESD_CHECK_PERIOD);
+        taal_queue_esd_work(dssdev);
 
         mutex_unlock(&td->lock);
         return;
 err:
         dev_err(&dssdev->dev, "performing LCD reset\n");
 
-        taal_power_off(dssdev);
-        taal_hw_reset(dssdev);
-        taal_power_on(dssdev);
+        taal_panel_reset(dssdev);
 
-        dsi_bus_unlock();
+        dsi_bus_unlock(dssdev);
 
-        queue_delayed_work(td->esd_wq, &td->esd_work, TAAL_ESD_CHECK_PERIOD);
+        taal_queue_esd_work(dssdev);
 
         mutex_unlock(&td->lock);
 }
@@ -1536,7 +1913,7 @@ static enum omap_dss_update_mode taal_get_update_mode(
 
 static struct omap_dss_driver taal_driver = {
         .probe          = taal_probe,
-        .remove         = taal_remove,
+        .remove         = __exit_p(taal_remove),
 
         .enable         = taal_enable,
         .disable        = taal_disable,
diff --git a/drivers/video/omap2/displays/panel-toppoly-tdo35s.c b/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
deleted file mode 100644
index e320e67d06f3..000000000000
--- a/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * LCD panel driver for Toppoly TDO35S
- *
- * Copyright (C) 2009 CompuLab, Ltd.
- * Author: Mike Rapoport <mike@compulab.co.il>
- *
- * Based on generic panel support
- * Copyright (C) 2008 Nokia Corporation
- * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <linux/module.h>
-#include <linux/delay.h>
-
-#include <plat/display.h>
-
-static struct omap_video_timings toppoly_tdo_panel_timings = {
-        /* 640 x 480 @ 60 Hz  Reduced blanking VESA CVT 0.31M3-R */
-        .x_res          = 480,
-        .y_res          = 640,
-
-        .pixel_clock    = 26000,
-
-        .hfp            = 104,
-        .hsw            = 8,
-        .hbp            = 8,
-
-        .vfp            = 4,
-        .vsw            = 2,
-        .vbp            = 2,
-};
-
-static int toppoly_tdo_panel_power_on(struct omap_dss_device *dssdev)
-{
-        int r;
-
-        r = omapdss_dpi_display_enable(dssdev);
-        if (r)
-                goto err0;
-
-        if (dssdev->platform_enable) {
-                r = dssdev->platform_enable(dssdev);
-                if (r)
-                        goto err1;
-        }
-
-        return 0;
-err1:
-        omapdss_dpi_display_disable(dssdev);
-err0:
-        return r;
-}
-
-static void toppoly_tdo_panel_power_off(struct omap_dss_device *dssdev)
-{
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
-        omapdss_dpi_display_disable(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 |
-                               OMAP_DSS_LCD_IPC |
-                               OMAP_DSS_LCD_ONOFF;
-
-        dssdev->panel.timings = toppoly_tdo_panel_timings;
-
-        return 0;
-}
-
-static void toppoly_tdo_panel_remove(struct omap_dss_device *dssdev)
-{
-}
-
-static int toppoly_tdo_panel_enable(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        r = toppoly_tdo_panel_power_on(dssdev);
-        if (r)
-                return r;
-
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-        return 0;
-}
-
-static void toppoly_tdo_panel_disable(struct omap_dss_device *dssdev)
-{
-        toppoly_tdo_panel_power_off(dssdev);
-
-        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-}
-
-static int toppoly_tdo_panel_suspend(struct omap_dss_device *dssdev)
-{
-        toppoly_tdo_panel_power_off(dssdev);
-        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
-        return 0;
-}
-
-static int toppoly_tdo_panel_resume(struct omap_dss_device *dssdev)
-{
-        int r = 0;
-
-        r = toppoly_tdo_panel_power_on(dssdev);
-        if (r)
-                return r;
-
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
-        return 0;
-}
-
-static struct omap_dss_driver generic_driver = {
-        .probe          = toppoly_tdo_panel_probe,
-        .remove         = toppoly_tdo_panel_remove,
-
-        .enable         = toppoly_tdo_panel_enable,
-        .disable        = toppoly_tdo_panel_disable,
-        .suspend        = toppoly_tdo_panel_suspend,
-        .resume         = toppoly_tdo_panel_resume,
-
-        .driver         = {
-                .name   = "toppoly_tdo35s_panel",
-                .owner  = THIS_MODULE,
-        },
-};
-
-static int __init toppoly_tdo_panel_drv_init(void)
-{
-        return omap_dss_register_driver(&generic_driver);
-}
-
-static void __exit toppoly_tdo_panel_drv_exit(void)
-{
-        omap_dss_unregister_driver(&generic_driver);
-}
-
-module_init(toppoly_tdo_panel_drv_init);
-module_exit(toppoly_tdo_panel_drv_exit);
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index e866e76b13d0..2462b9ec6662 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -17,7 +17,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #define TPO_R02_MODE(x)         ((x) & 7)
 #define TPO_R02_MODE_800x480    7
@@ -144,13 +144,15 @@ static ssize_t tpo_td043_vmirror_store(struct device *dev,
         struct device_attribute *attr, const char *buf, size_t count)
 {
         struct tpo_td043_device *tpo_td043 = dev_get_drvdata(dev);
-        long val;
+        int val;
         int ret;
 
-        ret = strict_strtol(buf, 0, &val);
+        ret = kstrtoint(buf, 0, &val);
         if (ret < 0)
                 return ret;
 
+        val = !!val;
+
         ret = tpo_td043_write_mirror(tpo_td043->spi, tpo_td043->hmirror, val);
         if (ret < 0)
                 return ret;
@@ -175,7 +177,7 @@ static ssize_t tpo_td043_mode_store(struct device *dev,
         long val;
         int ret;
 
-        ret = strict_strtol(buf, 0, &val);
+        ret = kstrtol(buf, 0, &val);
         if (ret != 0 || val & ~7)
                 return -EINVAL;
 
@@ -269,6 +271,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 +313,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/Kconfig b/drivers/video/omap2/dss/Kconfig
index 43b64403eaa4..6b3e2da11419 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -1,8 +1,8 @@
 menuconfig OMAP2_DSS
-        tristate "OMAP2/3 Display Subsystem support (EXPERIMENTAL)"
-        depends on ARCH_OMAP2 || ARCH_OMAP3
+        tristate "OMAP2+ Display Subsystem support (EXPERIMENTAL)"
+        depends on ARCH_OMAP2PLUS
         help
-          OMAP2/3 Display Subsystem support.
+          OMAP2+ Display Subsystem support.
 
 if OMAP2_DSS
 
@@ -60,6 +60,14 @@ config OMAP2_DSS_VENC
         help
           OMAP Video Encoder support for S-Video and composite TV-out.
 
+config OMAP4_DSS_HDMI
+        bool "HDMI support"
+        depends on ARCH_OMAP4
+        default y
+        help
+          HDMI Interface. This adds the High Definition Multimedia Interface.
+          See http://www.hdmi.org/ for HDMI specification.
+
 config OMAP2_DSS_SDI
         bool "SDI support"
         depends on ARCH_OMAP3
@@ -72,7 +80,7 @@ config OMAP2_DSS_SDI
 
 config OMAP2_DSS_DSI
         bool "DSI support"
-        depends on ARCH_OMAP3
+        depends on ARCH_OMAP3 || ARCH_OMAP4
         default n
         help
           MIPI DSI (Display Serial Interface) support.
@@ -82,14 +90,6 @@ config OMAP2_DSS_DSI
 
           See http://www.mipi.org/ for DSI spesifications.
 
-config OMAP2_DSS_USE_DSI_PLL
-        bool "Use DSI PLL for PCLK (EXPERIMENTAL)"
-        default n
-        depends on OMAP2_DSS_DSI
-        help
-          Use DSI PLL to generate pixel clock.  Currently only for DPI output.
-          DSI PLL can be used to generate higher and more precise pixel clocks.
-
 config OMAP2_DSS_FAKE_VSYNC
         bool "Fake VSYNC irq from manual update displays"
         default n
@@ -117,4 +117,27 @@ config OMAP2_DSS_MIN_FCK_PER_PCK
           Max FCK is 173MHz, so this doesn't work if your PCK
           is very high.
 
+config OMAP2_DSS_SLEEP_BEFORE_RESET
+        bool "Sleep 50ms before DSS reset"
+        default y
+        help
+          For some unknown reason we may get SYNC_LOST errors from the display
+          subsystem at initialization time if we don't sleep before resetting
+          the DSS. See the source (dss.c) for more comments.
+
+          However, 50ms is quite long time to sleep, and with some
+          configurations the SYNC_LOST may never happen, so the sleep can
+          be disabled here.
+
+config OMAP2_DSS_SLEEP_AFTER_VENC_RESET
+        bool "Sleep 20ms after VENC reset"
+        default y
+        help
+          There is a 20ms sleep after VENC reset which seemed to fix the
+          reset. The reason for the bug is unclear, and it's also unclear
+          on what platforms this happens.
+
+          This option enables the sleep, and is enabled by default. You can
+          disable the sleep if it doesn't cause problems on your platform.
+
 endif
diff --git a/drivers/video/omap2/dss/Makefile b/drivers/video/omap2/dss/Makefile
index d71b5d9d71b1..10d9d3bb3e24 100644
--- a/drivers/video/omap2/dss/Makefile
+++ b/drivers/video/omap2/dss/Makefile
@@ -1,7 +1,9 @@
 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
 omapdss-$(CONFIG_OMAP2_DSS_SDI) += sdi.o
 omapdss-$(CONFIG_OMAP2_DSS_DSI) += dsi.o
+omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi.o \
+                                    hdmi_omap4_panel.o
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index b3a498f22d36..3da426719dd6 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -33,331 +33,29 @@
 #include <linux/device.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
-#include <plat/clock.h>
+#include <video/omapdss.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static struct {
         struct platform_device *pdev;
-        int             ctx_id;
-
-        struct clk      *dss_ick;
-        struct clk      *dss1_fck;
-        struct clk      *dss2_fck;
-        struct clk      *dss_54m_fck;
-        struct clk      *dss_96m_fck;
-        unsigned        num_clks_enabled;
 
         struct regulator *vdds_dsi_reg;
         struct regulator *vdds_sdi_reg;
-        struct regulator *vdda_dac_reg;
 } core;
 
-static void dss_clk_enable_all_no_ctx(void);
-static void dss_clk_disable_all_no_ctx(void);
-static void dss_clk_enable_no_ctx(enum dss_clock clks);
-static void dss_clk_disable_no_ctx(enum dss_clock clks);
-
 static char *def_disp_name;
 module_param_named(def_disp, def_disp_name, charp, 0);
-MODULE_PARM_DESC(def_disp_name, "default display name");
+MODULE_PARM_DESC(def_disp, "default display name");
 
 #ifdef DEBUG
 unsigned int dss_debug;
 module_param_named(debug, dss_debug, bool, 0644);
 #endif
 
-/* CONTEXT */
-static int dss_get_ctx_id(void)
-{
-        struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
-        int r;
-
-        if (!pdata->get_last_off_on_transaction_id)
-                return 0;
-        r = pdata->get_last_off_on_transaction_id(&core.pdev->dev);
-        if (r < 0) {
-                dev_err(&core.pdev->dev, "getting transaction ID failed, "
-                                "will force context restore\n");
-                r = -1;
-        }
-        return r;
-}
-
-int dss_need_ctx_restore(void)
-{
-        int id = dss_get_ctx_id();
-
-        if (id < 0 || id != core.ctx_id) {
-                DSSDBG("ctx id %d -> id %d\n",
-                                core.ctx_id, id);
-                core.ctx_id = id;
-                return 1;
-        } else {
-                return 0;
-        }
-}
-
-static void save_all_ctx(void)
-{
-        DSSDBG("save context\n");
-
-        dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-        dss_save_context();
-        dispc_save_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-        dsi_save_context();
-#endif
-
-        dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-}
-
-static void restore_all_ctx(void)
-{
-        DSSDBG("restore context\n");
-
-        dss_clk_enable_all_no_ctx();
-
-        dss_restore_context();
-        dispc_restore_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-        dsi_restore_context();
-#endif
-
-        dss_clk_disable_all_no_ctx();
-}
-
-#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-/* CLOCKS */
-static void core_dump_clocks(struct seq_file *s)
-{
-        int i;
-        struct clk *clocks[5] = {
-                core.dss_ick,
-                core.dss1_fck,
-                core.dss2_fck,
-                core.dss_54m_fck,
-                core.dss_96m_fck
-        };
-
-        seq_printf(s, "- CORE -\n");
-
-        seq_printf(s, "internal clk count\t\t%u\n", core.num_clks_enabled);
-
-        for (i = 0; i < 5; i++) {
-                if (!clocks[i])
-                        continue;
-                seq_printf(s, "%-15s\t%lu\t%d\n",
-                                clocks[i]->name,
-                                clk_get_rate(clocks[i]),
-                                clocks[i]->usecount);
-        }
-}
-#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
-
-static int dss_get_clock(struct clk **clock, const char *clk_name)
-{
-        struct clk *clk;
-
-        clk = clk_get(&core.pdev->dev, clk_name);
-
-        if (IS_ERR(clk)) {
-                DSSERR("can't get clock %s", clk_name);
-                return PTR_ERR(clk);
-        }
-
-        *clock = clk;
-
-        DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
-
-        return 0;
-}
-
-static int dss_get_clocks(void)
-{
-        int r;
-
-        core.dss_ick = NULL;
-        core.dss1_fck = NULL;
-        core.dss2_fck = NULL;
-        core.dss_54m_fck = NULL;
-        core.dss_96m_fck = NULL;
-
-        r = dss_get_clock(&core.dss_ick, "ick");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss1_fck, "dss1_fck");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss2_fck, "dss2_fck");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss_54m_fck, "tv_fck");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss_96m_fck, "video_fck");
-        if (r)
-                goto err;
-
-        return 0;
-
-err:
-        if (core.dss_ick)
-                clk_put(core.dss_ick);
-        if (core.dss1_fck)
-                clk_put(core.dss1_fck);
-        if (core.dss2_fck)
-                clk_put(core.dss2_fck);
-        if (core.dss_54m_fck)
-                clk_put(core.dss_54m_fck);
-        if (core.dss_96m_fck)
-                clk_put(core.dss_96m_fck);
-
-        return r;
-}
-
-static void dss_put_clocks(void)
-{
-        if (core.dss_96m_fck)
-                clk_put(core.dss_96m_fck);
-        clk_put(core.dss_54m_fck);
-        clk_put(core.dss1_fck);
-        clk_put(core.dss2_fck);
-        clk_put(core.dss_ick);
-}
-
-unsigned long dss_clk_get_rate(enum dss_clock clk)
-{
-        switch (clk) {
-        case DSS_CLK_ICK:
-                return clk_get_rate(core.dss_ick);
-        case DSS_CLK_FCK1:
-                return clk_get_rate(core.dss1_fck);
-        case DSS_CLK_FCK2:
-                return clk_get_rate(core.dss2_fck);
-        case DSS_CLK_54M:
-                return clk_get_rate(core.dss_54m_fck);
-        case DSS_CLK_96M:
-                return clk_get_rate(core.dss_96m_fck);
-        }
-
-        BUG();
-        return 0;
-}
-
-static unsigned count_clk_bits(enum dss_clock clks)
-{
-        unsigned num_clks = 0;
-
-        if (clks & DSS_CLK_ICK)
-                ++num_clks;
-        if (clks & DSS_CLK_FCK1)
-                ++num_clks;
-        if (clks & DSS_CLK_FCK2)
-                ++num_clks;
-        if (clks & DSS_CLK_54M)
-                ++num_clks;
-        if (clks & DSS_CLK_96M)
-                ++num_clks;
-
-        return num_clks;
-}
-
-static void dss_clk_enable_no_ctx(enum dss_clock clks)
-{
-        unsigned num_clks = count_clk_bits(clks);
-
-        if (clks & DSS_CLK_ICK)
-                clk_enable(core.dss_ick);
-        if (clks & DSS_CLK_FCK1)
-                clk_enable(core.dss1_fck);
-        if (clks & DSS_CLK_FCK2)
-                clk_enable(core.dss2_fck);
-        if (clks & DSS_CLK_54M)
-                clk_enable(core.dss_54m_fck);
-        if (clks & DSS_CLK_96M)
-                clk_enable(core.dss_96m_fck);
-
-        core.num_clks_enabled += num_clks;
-}
-
-void dss_clk_enable(enum dss_clock clks)
-{
-        bool check_ctx = core.num_clks_enabled == 0;
-
-        dss_clk_enable_no_ctx(clks);
-
-        if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
-                restore_all_ctx();
-}
-
-static void dss_clk_disable_no_ctx(enum dss_clock clks)
-{
-        unsigned num_clks = count_clk_bits(clks);
-
-        if (clks & DSS_CLK_ICK)
-                clk_disable(core.dss_ick);
-        if (clks & DSS_CLK_FCK1)
-                clk_disable(core.dss1_fck);
-        if (clks & DSS_CLK_FCK2)
-                clk_disable(core.dss2_fck);
-        if (clks & DSS_CLK_54M)
-                clk_disable(core.dss_54m_fck);
-        if (clks & DSS_CLK_96M)
-                clk_disable(core.dss_96m_fck);
-
-        core.num_clks_enabled -= num_clks;
-}
-
-void dss_clk_disable(enum dss_clock clks)
-{
-        if (cpu_is_omap34xx()) {
-                unsigned num_clks = count_clk_bits(clks);
-
-                BUG_ON(core.num_clks_enabled < num_clks);
-
-                if (core.num_clks_enabled == num_clks)
-                        save_all_ctx();
-        }
-
-        dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_enable_all_no_ctx(void)
-{
-        enum dss_clock clks;
-
-        clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-        if (cpu_is_omap34xx())
-                clks |= DSS_CLK_96M;
-        dss_clk_enable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all_no_ctx(void)
-{
-        enum dss_clock clks;
-
-        clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-        if (cpu_is_omap34xx())
-                clks |= DSS_CLK_96M;
-        dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all(void)
-{
-        enum dss_clock clks;
-
-        clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-        if (cpu_is_omap34xx())
-                clks |= DSS_CLK_96M;
-        dss_clk_disable(clks);
-}
+static int omap_dss_register_device(struct omap_dss_device *);
+static void omap_dss_unregister_device(struct omap_dss_device *);
 
 /* REGULATORS */
 
@@ -389,32 +87,7 @@ struct regulator *dss_get_vdds_sdi(void)
         return reg;
 }
 
-struct regulator *dss_get_vdda_dac(void)
-{
-        struct regulator *reg;
-
-        if (core.vdda_dac_reg != NULL)
-                return core.vdda_dac_reg;
-
-        reg = regulator_get(&core.pdev->dev, "vdda_dac");
-        if (!IS_ERR(reg))
-                core.vdda_dac_reg = reg;
-
-        return reg;
-}
-
-/* DEBUGFS */
 #if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-static void dss_debug_dump_clocks(struct seq_file *s)
-{
-        core_dump_clocks(s);
-        dss_dump_clocks(s);
-        dispc_dump_clocks(s);
-#ifdef CONFIG_OMAP2_DSS_DSI
-        dsi_dump_clocks(s);
-#endif
-}
-
 static int dss_debug_show(struct seq_file *s, void *unused)
 {
         void (*func)(struct seq_file *) = s->private;
@@ -454,8 +127,7 @@ static int dss_initialize_debugfs(void)
 #endif
 
 #if defined(CONFIG_OMAP2_DSS_DSI) && defined(CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS)
-        debugfs_create_file("dsi_irq", S_IRUGO, dss_debugfs_dir,
-                        &dsi_dump_irqs, &dss_debug_fops);
+        dsi_create_debugfs_files_irq(dss_debugfs_dir, &dss_debug_fops);
 #endif
 
         debugfs_create_file("dss", S_IRUGO, dss_debugfs_dir,
@@ -467,8 +139,7 @@ static int dss_initialize_debugfs(void)
                         &rfbi_dump_regs, &dss_debug_fops);
 #endif
 #ifdef CONFIG_OMAP2_DSS_DSI
-        debugfs_create_file("dsi", S_IRUGO, dss_debugfs_dir,
-                        &dsi_dump_regs, &dss_debug_fops);
+        dsi_create_debugfs_files_reg(dss_debugfs_dir, &dss_debug_fops);
 #endif
 #ifdef CONFIG_OMAP2_DSS_VENC
         debugfs_create_file("venc", S_IRUGO, dss_debugfs_dir,
@@ -496,72 +167,53 @@ static inline void dss_uninitialize_debugfs(void)
 static int omap_dss_probe(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int skip_init = 0;
         int r;
         int i;
 
         core.pdev = pdev;
 
+        dss_features_init();
+
         dss_init_overlay_managers(pdev);
         dss_init_overlays(pdev);
 
-        r = dss_get_clocks();
-        if (r)
-                goto err_clocks;
-
-        dss_clk_enable_all_no_ctx();
-
-        core.ctx_id = dss_get_ctx_id();
-        DSSDBG("initial ctx id %u\n", core.ctx_id);
-
-#ifdef CONFIG_FB_OMAP_BOOTLOADER_INIT
-        /* DISPC_CONTROL */
-        if (omap_readl(0x48050440) & 1) /* LCD enabled? */
-                skip_init = 1;
-#endif
-
-        r = dss_init(skip_init);
+        r = dss_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize DSS\n");
+                DSSERR("Failed to initialize DSS platform driver\n");
                 goto err_dss;
         }
 
-        r = rfbi_init();
-        if (r) {
-                DSSERR("Failed to initialize rfbi\n");
-                goto err_rfbi;
-        }
+        /* keep clocks enabled to prevent context saves/restores during init */
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
-        r = dpi_init(pdev);
+        r = rfbi_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize dpi\n");
-                goto err_dpi;
+                DSSERR("Failed to initialize rfbi platform driver\n");
+                goto err_rfbi;
         }
 
-        r = dispc_init();
+        r = dispc_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize dispc\n");
+                DSSERR("Failed to initialize dispc platform driver\n");
                 goto err_dispc;
         }
 
-        r = venc_init(pdev);
+        r = venc_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize venc\n");
+                DSSERR("Failed to initialize venc platform driver\n");
                 goto err_venc;
         }
 
-        if (cpu_is_omap34xx()) {
-                r = sdi_init(skip_init);
-                if (r) {
-                        DSSERR("Failed to initialize SDI\n");
-                        goto err_sdi;
-                }
+        r = dsi_init_platform_driver();
+        if (r) {
+                DSSERR("Failed to initialize DSI platform driver\n");
+                goto err_dsi;
+        }
 
-                r = dsi_init(pdev);
-                if (r) {
-                        DSSERR("Failed to initialize DSI\n");
-                        goto err_dsi;
-                }
+        r = hdmi_init_platform_driver();
+        if (r) {
+                DSSERR("Failed to initialize hdmi\n");
+                goto err_hdmi;
         }
 
         r = dss_initialize_debugfs();
@@ -586,32 +238,25 @@ static int omap_dss_probe(struct platform_device *pdev)
                         pdata->default_device = dssdev;
         }
 
-        dss_clk_disable_all();
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         return 0;
 
 err_register:
         dss_uninitialize_debugfs();
 err_debugfs:
-        if (cpu_is_omap34xx())
-                dsi_exit();
+        hdmi_uninit_platform_driver();
+err_hdmi:
+        dsi_uninit_platform_driver();
 err_dsi:
-        if (cpu_is_omap34xx())
-                sdi_exit();
-err_sdi:
-        venc_exit();
+        venc_uninit_platform_driver();
 err_venc:
-        dispc_exit();
+        dispc_uninit_platform_driver();
 err_dispc:
-        dpi_exit();
-err_dpi:
-        rfbi_exit();
+        rfbi_uninit_platform_driver();
 err_rfbi:
-        dss_exit();
+        dss_uninit_platform_driver();
 err_dss:
-        dss_clk_disable_all_no_ctx();
-        dss_put_clocks();
-err_clocks:
 
         return r;
 }
@@ -620,61 +265,15 @@ static int omap_dss_remove(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
         int i;
-        int c;
 
         dss_uninitialize_debugfs();
 
-        venc_exit();
-        dispc_exit();
-        dpi_exit();
-        rfbi_exit();
-        if (cpu_is_omap34xx()) {
-                dsi_exit();
-                sdi_exit();
-        }
-
-        dss_exit();
-
-        /* these should be removed at some point */
-        c = core.dss_ick->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss_ick usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss_ick);
-        }
-
-        c = core.dss1_fck->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss1_fck usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss1_fck);
-        }
-
-        c = core.dss2_fck->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss2_fck usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss2_fck);
-        }
-
-        c = core.dss_54m_fck->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss_54m_fck usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss_54m_fck);
-        }
-
-        if (core.dss_96m_fck) {
-                c = core.dss_96m_fck->usecount;
-                if (c > 0) {
-                        DSSERR("warning: dss_96m_fck usecount %d, disabling\n",
-                                        c);
-                        while (c-- > 0)
-                                clk_disable(core.dss_96m_fck);
-                }
-        }
-
-        dss_put_clocks();
+        venc_uninit_platform_driver();
+        dispc_uninit_platform_driver();
+        rfbi_uninit_platform_driver();
+        dsi_uninit_platform_driver();
+        hdmi_uninit_platform_driver();
+        dss_uninit_platform_driver();
 
         dss_uninit_overlays(pdev);
         dss_uninit_overlay_managers(pdev);
@@ -882,7 +481,7 @@ static void omap_dss_dev_release(struct device *dev)
         reset_device(dev, 0);
 }
 
-int omap_dss_register_device(struct omap_dss_device *dssdev)
+static int omap_dss_register_device(struct omap_dss_device *dssdev)
 {
         static int dev_num;
 
@@ -896,7 +495,7 @@ int omap_dss_register_device(struct omap_dss_device *dssdev)
         return device_register(&dssdev->dev);
 }
 
-void omap_dss_unregister_device(struct omap_dss_device *dssdev)
+static void omap_dss_unregister_device(struct omap_dss_device *dssdev)
 {
         device_unregister(&dssdev->dev);
 }
@@ -962,11 +561,6 @@ static void __exit omap_dss_exit(void)
                 core.vdds_sdi_reg = NULL;
         }
 
-        if (core.vdda_dac_reg != NULL) {
-                regulator_put(core.vdda_dac_reg);
-                core.vdda_dac_reg = NULL;
-        }
-
         platform_driver_unregister(&omap_dss_driver);
 
         omap_dss_bus_unregister();
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index 5ecdc0004094..7a9a2e7d9685 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -32,97 +32,19 @@
 #include <linux/delay.h>
 #include <linux/workqueue.h>
 #include <linux/hardirq.h>
+#include <linux/interrupt.h>
 
 #include <plat/sram.h>
 #include <plat/clock.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #include "dss.h"
+#include "dss_features.h"
+#include "dispc.h"
 
 /* DISPC */
-#define DISPC_BASE                      0x48050400
-
-#define DISPC_SZ_REGS                   SZ_1K
-
-struct dispc_reg { u16 idx; };
-
-#define DISPC_REG(idx)                  ((const struct dispc_reg) { idx })
-
-/* DISPC common */
-#define DISPC_REVISION                  DISPC_REG(0x0000)
-#define DISPC_SYSCONFIG                 DISPC_REG(0x0010)
-#define DISPC_SYSSTATUS                 DISPC_REG(0x0014)
-#define DISPC_IRQSTATUS                 DISPC_REG(0x0018)
-#define DISPC_IRQENABLE                 DISPC_REG(0x001C)
-#define DISPC_CONTROL                   DISPC_REG(0x0040)
-#define DISPC_CONFIG                    DISPC_REG(0x0044)
-#define DISPC_CAPABLE                   DISPC_REG(0x0048)
-#define DISPC_DEFAULT_COLOR0            DISPC_REG(0x004C)
-#define DISPC_DEFAULT_COLOR1            DISPC_REG(0x0050)
-#define DISPC_TRANS_COLOR0              DISPC_REG(0x0054)
-#define DISPC_TRANS_COLOR1              DISPC_REG(0x0058)
-#define DISPC_LINE_STATUS               DISPC_REG(0x005C)
-#define DISPC_LINE_NUMBER               DISPC_REG(0x0060)
-#define DISPC_TIMING_H                  DISPC_REG(0x0064)
-#define DISPC_TIMING_V                  DISPC_REG(0x0068)
-#define DISPC_POL_FREQ                  DISPC_REG(0x006C)
-#define DISPC_DIVISOR                   DISPC_REG(0x0070)
-#define DISPC_GLOBAL_ALPHA              DISPC_REG(0x0074)
-#define DISPC_SIZE_DIG                  DISPC_REG(0x0078)
-#define DISPC_SIZE_LCD                  DISPC_REG(0x007C)
-
-/* DISPC GFX plane */
-#define DISPC_GFX_BA0                   DISPC_REG(0x0080)
-#define DISPC_GFX_BA1                   DISPC_REG(0x0084)
-#define DISPC_GFX_POSITION              DISPC_REG(0x0088)
-#define DISPC_GFX_SIZE                  DISPC_REG(0x008C)
-#define DISPC_GFX_ATTRIBUTES            DISPC_REG(0x00A0)
-#define DISPC_GFX_FIFO_THRESHOLD        DISPC_REG(0x00A4)
-#define DISPC_GFX_FIFO_SIZE_STATUS      DISPC_REG(0x00A8)
-#define DISPC_GFX_ROW_INC               DISPC_REG(0x00AC)
-#define DISPC_GFX_PIXEL_INC             DISPC_REG(0x00B0)
-#define DISPC_GFX_WINDOW_SKIP           DISPC_REG(0x00B4)
-#define DISPC_GFX_TABLE_BA              DISPC_REG(0x00B8)
-
-#define DISPC_DATA_CYCLE1               DISPC_REG(0x01D4)
-#define DISPC_DATA_CYCLE2               DISPC_REG(0x01D8)
-#define DISPC_DATA_CYCLE3               DISPC_REG(0x01DC)
-
-#define DISPC_CPR_COEF_R                DISPC_REG(0x0220)
-#define DISPC_CPR_COEF_G                DISPC_REG(0x0224)
-#define DISPC_CPR_COEF_B                DISPC_REG(0x0228)
-
-#define DISPC_GFX_PRELOAD               DISPC_REG(0x022C)
-
-/* DISPC Video plane, n = 0 for VID1 and n = 1 for VID2 */
-#define DISPC_VID_REG(n, idx)           DISPC_REG(0x00BC + (n)*0x90 + idx)
-
-#define DISPC_VID_BA0(n)                DISPC_VID_REG(n, 0x0000)
-#define DISPC_VID_BA1(n)                DISPC_VID_REG(n, 0x0004)
-#define DISPC_VID_POSITION(n)           DISPC_VID_REG(n, 0x0008)
-#define DISPC_VID_SIZE(n)               DISPC_VID_REG(n, 0x000C)
-#define DISPC_VID_ATTRIBUTES(n)         DISPC_VID_REG(n, 0x0010)
-#define DISPC_VID_FIFO_THRESHOLD(n)     DISPC_VID_REG(n, 0x0014)
-#define DISPC_VID_FIFO_SIZE_STATUS(n)   DISPC_VID_REG(n, 0x0018)
-#define DISPC_VID_ROW_INC(n)            DISPC_VID_REG(n, 0x001C)
-#define DISPC_VID_PIXEL_INC(n)          DISPC_VID_REG(n, 0x0020)
-#define DISPC_VID_FIR(n)                DISPC_VID_REG(n, 0x0024)
-#define DISPC_VID_PICTURE_SIZE(n)       DISPC_VID_REG(n, 0x0028)
-#define DISPC_VID_ACCU0(n)              DISPC_VID_REG(n, 0x002C)
-#define DISPC_VID_ACCU1(n)              DISPC_VID_REG(n, 0x0030)
-
-/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
-#define DISPC_VID_FIR_COEF_H(n, i)      DISPC_REG(0x00F0 + (n)*0x90 + (i)*0x8)
-/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
-#define DISPC_VID_FIR_COEF_HV(n, i)     DISPC_REG(0x00F4 + (n)*0x90 + (i)*0x8)
-/* coef index i = {0, 1, 2, 3, 4} */
-#define DISPC_VID_CONV_COEF(n, i)       DISPC_REG(0x0130 + (n)*0x90 + (i)*0x4)
-/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
-#define DISPC_VID_FIR_COEF_V(n, i)      DISPC_REG(0x01E0 + (n)*0x20 + (i)*0x4)
-
-#define DISPC_VID_PRELOAD(n)            DISPC_REG(0x230 + (n)*0x04)
-
+#define DISPC_SZ_REGS                   SZ_4K
 
 #define DISPC_IRQ_MASK_ERROR            (DISPC_IRQ_GFX_FIFO_UNDERFLOW | \
                                          DISPC_IRQ_OCP_ERR | \
@@ -139,16 +61,28 @@ 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)
 
 #define REG_FLD_MOD(idx, val, start, end)                               \
         dispc_write_reg(idx, FLD_MOD(dispc_read_reg(idx), val, start, end))
 
-static const struct dispc_reg dispc_reg_att[] = { DISPC_GFX_ATTRIBUTES,
-        DISPC_VID_ATTRIBUTES(0),
-        DISPC_VID_ATTRIBUTES(1) };
-
 struct dispc_irq_stats {
         unsigned long last_reset;
         unsigned irq_count;
@@ -156,7 +90,9 @@ struct dispc_irq_stats {
 };
 
 static struct {
+        struct platform_device *pdev;
         void __iomem    *base;
+        int irq;
 
         u32     fifo_size[3];
 
@@ -174,25 +110,38 @@ static struct {
 #endif
 } dispc;
 
+enum omap_color_component {
+        /* used for all color formats for OMAP3 and earlier
+         * and for RGB and Y color component on OMAP4
+         */
+        DISPC_COLOR_COMPONENT_RGB_Y             = 1 << 0,
+        /* used for UV component for
+         * OMAP_DSS_COLOR_YUV2, OMAP_DSS_COLOR_UYVY, OMAP_DSS_COLOR_NV12
+         * color formats on OMAP4
+         */
+        DISPC_COLOR_COMPONENT_UV                = 1 << 1,
+};
+
 static void _omap_dispc_set_irqs(void);
 
-static inline void dispc_write_reg(const struct dispc_reg idx, u32 val)
+static inline void dispc_write_reg(const u16 idx, u32 val)
 {
-        __raw_writel(val, dispc.base + idx.idx);
+        __raw_writel(val, dispc.base + idx);
 }
 
-static inline u32 dispc_read_reg(const struct dispc_reg idx)
+static inline u32 dispc_read_reg(const u16 idx)
 {
-        return __raw_readl(dispc.base + idx.idx);
+        return __raw_readl(dispc.base + idx);
 }
 
 #define SR(reg) \
-        dispc.ctx[(DISPC_##reg).idx / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
+        dispc.ctx[DISPC_##reg / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
 #define RR(reg) \
-        dispc_write_reg(DISPC_##reg, dispc.ctx[(DISPC_##reg).idx / sizeof(u32)])
+        dispc_write_reg(DISPC_##reg, dispc.ctx[DISPC_##reg / sizeof(u32)])
 
 void dispc_save_context(void)
 {
+        int i;
         if (cpu_is_omap24xx())
                 return;
 
@@ -200,280 +149,320 @@ void dispc_save_context(void)
         SR(IRQENABLE);
         SR(CONTROL);
         SR(CONFIG);
-        SR(DEFAULT_COLOR0);
-        SR(DEFAULT_COLOR1);
-        SR(TRANS_COLOR0);
-        SR(TRANS_COLOR1);
+        SR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD));
+        SR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_DIGIT));
+        SR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD));
+        SR(TRANS_COLOR(OMAP_DSS_CHANNEL_DIGIT));
         SR(LINE_NUMBER);
-        SR(TIMING_H);
-        SR(TIMING_V);
-        SR(POL_FREQ);
-        SR(DIVISOR);
+        SR(TIMING_H(OMAP_DSS_CHANNEL_LCD));
+        SR(TIMING_V(OMAP_DSS_CHANNEL_LCD));
+        SR(POL_FREQ(OMAP_DSS_CHANNEL_LCD));
+        SR(DIVISORo(OMAP_DSS_CHANNEL_LCD));
         SR(GLOBAL_ALPHA);
-        SR(SIZE_DIG);
-        SR(SIZE_LCD);
-
-        SR(GFX_BA0);
-        SR(GFX_BA1);
-        SR(GFX_POSITION);
-        SR(GFX_SIZE);
-        SR(GFX_ATTRIBUTES);
-        SR(GFX_FIFO_THRESHOLD);
-        SR(GFX_ROW_INC);
-        SR(GFX_PIXEL_INC);
-        SR(GFX_WINDOW_SKIP);
-        SR(GFX_TABLE_BA);
-
-        SR(DATA_CYCLE1);
-        SR(DATA_CYCLE2);
-        SR(DATA_CYCLE3);
-
-        SR(CPR_COEF_R);
-        SR(CPR_COEF_G);
-        SR(CPR_COEF_B);
-
-        SR(GFX_PRELOAD);
+        SR(SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT));
+        SR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                SR(CONTROL2);
+                SR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD2));
+                SR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD2));
+                SR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD2));
+                SR(TIMING_H(OMAP_DSS_CHANNEL_LCD2));
+                SR(TIMING_V(OMAP_DSS_CHANNEL_LCD2));
+                SR(POL_FREQ(OMAP_DSS_CHANNEL_LCD2));
+                SR(DIVISORo(OMAP_DSS_CHANNEL_LCD2));
+                SR(CONFIG2);
+        }
+
+        SR(OVL_BA0(OMAP_DSS_GFX));
+        SR(OVL_BA1(OMAP_DSS_GFX));
+        SR(OVL_POSITION(OMAP_DSS_GFX));
+        SR(OVL_SIZE(OMAP_DSS_GFX));
+        SR(OVL_ATTRIBUTES(OMAP_DSS_GFX));
+        SR(OVL_FIFO_THRESHOLD(OMAP_DSS_GFX));
+        SR(OVL_ROW_INC(OMAP_DSS_GFX));
+        SR(OVL_PIXEL_INC(OMAP_DSS_GFX));
+        SR(OVL_WINDOW_SKIP(OMAP_DSS_GFX));
+        SR(OVL_TABLE_BA(OMAP_DSS_GFX));
+
+        SR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD));
+        SR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD));
+        SR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD));
+
+        SR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD));
+        SR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD));
+        SR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                SR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD2));
+                SR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD2));
+                SR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD2));
+
+                SR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD2));
+                SR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD2));
+                SR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD2));
+        }
+
+        SR(OVL_PRELOAD(OMAP_DSS_GFX));
 
         /* VID1 */
-        SR(VID_BA0(0));
-        SR(VID_BA1(0));
-        SR(VID_POSITION(0));
-        SR(VID_SIZE(0));
-        SR(VID_ATTRIBUTES(0));
-        SR(VID_FIFO_THRESHOLD(0));
-        SR(VID_ROW_INC(0));
-        SR(VID_PIXEL_INC(0));
-        SR(VID_FIR(0));
-        SR(VID_PICTURE_SIZE(0));
-        SR(VID_ACCU0(0));
-        SR(VID_ACCU1(0));
-
-        SR(VID_FIR_COEF_H(0, 0));
-        SR(VID_FIR_COEF_H(0, 1));
-        SR(VID_FIR_COEF_H(0, 2));
-        SR(VID_FIR_COEF_H(0, 3));
-        SR(VID_FIR_COEF_H(0, 4));
-        SR(VID_FIR_COEF_H(0, 5));
-        SR(VID_FIR_COEF_H(0, 6));
-        SR(VID_FIR_COEF_H(0, 7));
-
-        SR(VID_FIR_COEF_HV(0, 0));
-        SR(VID_FIR_COEF_HV(0, 1));
-        SR(VID_FIR_COEF_HV(0, 2));
-        SR(VID_FIR_COEF_HV(0, 3));
-        SR(VID_FIR_COEF_HV(0, 4));
-        SR(VID_FIR_COEF_HV(0, 5));
-        SR(VID_FIR_COEF_HV(0, 6));
-        SR(VID_FIR_COEF_HV(0, 7));
-
-        SR(VID_CONV_COEF(0, 0));
-        SR(VID_CONV_COEF(0, 1));
-        SR(VID_CONV_COEF(0, 2));
-        SR(VID_CONV_COEF(0, 3));
-        SR(VID_CONV_COEF(0, 4));
-
-        SR(VID_FIR_COEF_V(0, 0));
-        SR(VID_FIR_COEF_V(0, 1));
-        SR(VID_FIR_COEF_V(0, 2));
-        SR(VID_FIR_COEF_V(0, 3));
-        SR(VID_FIR_COEF_V(0, 4));
-        SR(VID_FIR_COEF_V(0, 5));
-        SR(VID_FIR_COEF_V(0, 6));
-        SR(VID_FIR_COEF_V(0, 7));
-
-        SR(VID_PRELOAD(0));
+        SR(OVL_BA0(OMAP_DSS_VIDEO1));
+        SR(OVL_BA1(OMAP_DSS_VIDEO1));
+        SR(OVL_POSITION(OMAP_DSS_VIDEO1));
+        SR(OVL_SIZE(OMAP_DSS_VIDEO1));
+        SR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO1));
+        SR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO1));
+        SR(OVL_ROW_INC(OMAP_DSS_VIDEO1));
+        SR(OVL_PIXEL_INC(OMAP_DSS_VIDEO1));
+        SR(OVL_FIR(OMAP_DSS_VIDEO1));
+        SR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO1));
+        SR(OVL_ACCU0(OMAP_DSS_VIDEO1));
+        SR(OVL_ACCU1(OMAP_DSS_VIDEO1));
+
+        for (i = 0; i < 8; i++)
+                SR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, i));
+
+        for (i = 0; i < 8; i++)
+                SR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, i));
+
+        for (i = 0; i < 5; i++)
+                SR(OVL_CONV_COEF(OMAP_DSS_VIDEO1, i));
+
+        for (i = 0; i < 8; i++)
+                SR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, i));
+
+        if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                SR(OVL_BA0_UV(OMAP_DSS_VIDEO1));
+                SR(OVL_BA1_UV(OMAP_DSS_VIDEO1));
+                SR(OVL_FIR2(OMAP_DSS_VIDEO1));
+                SR(OVL_ACCU2_0(OMAP_DSS_VIDEO1));
+                SR(OVL_ACCU2_1(OMAP_DSS_VIDEO1));
+
+                for (i = 0; i < 8; i++)
+                        SR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, i));
+
+                for (i = 0; i < 8; i++)
+                        SR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, i));
+
+                for (i = 0; i < 8; i++)
+                        SR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, i));
+        }
+        if (dss_has_feature(FEAT_ATTR2))
+                SR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO1));
+
+        SR(OVL_PRELOAD(OMAP_DSS_VIDEO1));
 
         /* VID2 */
-        SR(VID_BA0(1));
-        SR(VID_BA1(1));
-        SR(VID_POSITION(1));
-        SR(VID_SIZE(1));
-        SR(VID_ATTRIBUTES(1));
-        SR(VID_FIFO_THRESHOLD(1));
-        SR(VID_ROW_INC(1));
-        SR(VID_PIXEL_INC(1));
-        SR(VID_FIR(1));
-        SR(VID_PICTURE_SIZE(1));
-        SR(VID_ACCU0(1));
-        SR(VID_ACCU1(1));
-
-        SR(VID_FIR_COEF_H(1, 0));
-        SR(VID_FIR_COEF_H(1, 1));
-        SR(VID_FIR_COEF_H(1, 2));
-        SR(VID_FIR_COEF_H(1, 3));
-        SR(VID_FIR_COEF_H(1, 4));
-        SR(VID_FIR_COEF_H(1, 5));
-        SR(VID_FIR_COEF_H(1, 6));
-        SR(VID_FIR_COEF_H(1, 7));
-
-        SR(VID_FIR_COEF_HV(1, 0));
-        SR(VID_FIR_COEF_HV(1, 1));
-        SR(VID_FIR_COEF_HV(1, 2));
-        SR(VID_FIR_COEF_HV(1, 3));
-        SR(VID_FIR_COEF_HV(1, 4));
-        SR(VID_FIR_COEF_HV(1, 5));
-        SR(VID_FIR_COEF_HV(1, 6));
-        SR(VID_FIR_COEF_HV(1, 7));
-
-        SR(VID_CONV_COEF(1, 0));
-        SR(VID_CONV_COEF(1, 1));
-        SR(VID_CONV_COEF(1, 2));
-        SR(VID_CONV_COEF(1, 3));
-        SR(VID_CONV_COEF(1, 4));
-
-        SR(VID_FIR_COEF_V(1, 0));
-        SR(VID_FIR_COEF_V(1, 1));
-        SR(VID_FIR_COEF_V(1, 2));
-        SR(VID_FIR_COEF_V(1, 3));
-        SR(VID_FIR_COEF_V(1, 4));
-        SR(VID_FIR_COEF_V(1, 5));
-        SR(VID_FIR_COEF_V(1, 6));
-        SR(VID_FIR_COEF_V(1, 7));
-
-        SR(VID_PRELOAD(1));
+        SR(OVL_BA0(OMAP_DSS_VIDEO2));
+        SR(OVL_BA1(OMAP_DSS_VIDEO2));
+        SR(OVL_POSITION(OMAP_DSS_VIDEO2));
+        SR(OVL_SIZE(OMAP_DSS_VIDEO2));
+        SR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO2));
+        SR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO2));
+        SR(OVL_ROW_INC(OMAP_DSS_VIDEO2));
+        SR(OVL_PIXEL_INC(OMAP_DSS_VIDEO2));
+        SR(OVL_FIR(OMAP_DSS_VIDEO2));
+        SR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO2));
+        SR(OVL_ACCU0(OMAP_DSS_VIDEO2));
+        SR(OVL_ACCU1(OMAP_DSS_VIDEO2));
+
+        for (i = 0; i < 8; i++)
+                SR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, i));
+
+        for (i = 0; i < 8; i++)
+                SR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, i));
+
+        for (i = 0; i < 5; i++)
+                SR(OVL_CONV_COEF(OMAP_DSS_VIDEO2, i));
+
+        for (i = 0; i < 8; i++)
+                SR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, i));
+
+        if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                SR(OVL_BA0_UV(OMAP_DSS_VIDEO2));
+                SR(OVL_BA1_UV(OMAP_DSS_VIDEO2));
+                SR(OVL_FIR2(OMAP_DSS_VIDEO2));
+                SR(OVL_ACCU2_0(OMAP_DSS_VIDEO2));
+                SR(OVL_ACCU2_1(OMAP_DSS_VIDEO2));
+
+                for (i = 0; i < 8; i++)
+                        SR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, i));
+
+                for (i = 0; i < 8; i++)
+                        SR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, i));
+
+                for (i = 0; i < 8; i++)
+                        SR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, i));
+        }
+        if (dss_has_feature(FEAT_ATTR2))
+                SR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO2));
+
+        SR(OVL_PRELOAD(OMAP_DSS_VIDEO2));
+
+        if (dss_has_feature(FEAT_CORE_CLK_DIV))
+                SR(DIVISOR);
 }
 
 void dispc_restore_context(void)
 {
+        int i;
         RR(SYSCONFIG);
         /*RR(IRQENABLE);*/
         /*RR(CONTROL);*/
         RR(CONFIG);
-        RR(DEFAULT_COLOR0);
-        RR(DEFAULT_COLOR1);
-        RR(TRANS_COLOR0);
-        RR(TRANS_COLOR1);
+        RR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD));
+        RR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_DIGIT));
+        RR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD));
+        RR(TRANS_COLOR(OMAP_DSS_CHANNEL_DIGIT));
         RR(LINE_NUMBER);
-        RR(TIMING_H);
-        RR(TIMING_V);
-        RR(POL_FREQ);
-        RR(DIVISOR);
+        RR(TIMING_H(OMAP_DSS_CHANNEL_LCD));
+        RR(TIMING_V(OMAP_DSS_CHANNEL_LCD));
+        RR(POL_FREQ(OMAP_DSS_CHANNEL_LCD));
+        RR(DIVISORo(OMAP_DSS_CHANNEL_LCD));
         RR(GLOBAL_ALPHA);
-        RR(SIZE_DIG);
-        RR(SIZE_LCD);
-
-        RR(GFX_BA0);
-        RR(GFX_BA1);
-        RR(GFX_POSITION);
-        RR(GFX_SIZE);
-        RR(GFX_ATTRIBUTES);
-        RR(GFX_FIFO_THRESHOLD);
-        RR(GFX_ROW_INC);
-        RR(GFX_PIXEL_INC);
-        RR(GFX_WINDOW_SKIP);
-        RR(GFX_TABLE_BA);
-
-        RR(DATA_CYCLE1);
-        RR(DATA_CYCLE2);
-        RR(DATA_CYCLE3);
-
-        RR(CPR_COEF_R);
-        RR(CPR_COEF_G);
-        RR(CPR_COEF_B);
-
-        RR(GFX_PRELOAD);
+        RR(SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT));
+        RR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                RR(DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD2));
+                RR(TRANS_COLOR(OMAP_DSS_CHANNEL_LCD2));
+                RR(SIZE_MGR(OMAP_DSS_CHANNEL_LCD2));
+                RR(TIMING_H(OMAP_DSS_CHANNEL_LCD2));
+                RR(TIMING_V(OMAP_DSS_CHANNEL_LCD2));
+                RR(POL_FREQ(OMAP_DSS_CHANNEL_LCD2));
+                RR(DIVISORo(OMAP_DSS_CHANNEL_LCD2));
+                RR(CONFIG2);
+        }
+
+        RR(OVL_BA0(OMAP_DSS_GFX));
+        RR(OVL_BA1(OMAP_DSS_GFX));
+        RR(OVL_POSITION(OMAP_DSS_GFX));
+        RR(OVL_SIZE(OMAP_DSS_GFX));
+        RR(OVL_ATTRIBUTES(OMAP_DSS_GFX));
+        RR(OVL_FIFO_THRESHOLD(OMAP_DSS_GFX));
+        RR(OVL_ROW_INC(OMAP_DSS_GFX));
+        RR(OVL_PIXEL_INC(OMAP_DSS_GFX));
+        RR(OVL_WINDOW_SKIP(OMAP_DSS_GFX));
+        RR(OVL_TABLE_BA(OMAP_DSS_GFX));
+
+
+        RR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD));
+        RR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD));
+        RR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD));
+
+        RR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD));
+        RR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD));
+        RR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                RR(DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD2));
+                RR(DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD2));
+                RR(DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD2));
+
+                RR(CPR_COEF_B(OMAP_DSS_CHANNEL_LCD2));
+                RR(CPR_COEF_G(OMAP_DSS_CHANNEL_LCD2));
+                RR(CPR_COEF_R(OMAP_DSS_CHANNEL_LCD2));
+        }
+
+        RR(OVL_PRELOAD(OMAP_DSS_GFX));
 
         /* VID1 */
-        RR(VID_BA0(0));
-        RR(VID_BA1(0));
-        RR(VID_POSITION(0));
-        RR(VID_SIZE(0));
-        RR(VID_ATTRIBUTES(0));
-        RR(VID_FIFO_THRESHOLD(0));
-        RR(VID_ROW_INC(0));
-        RR(VID_PIXEL_INC(0));
-        RR(VID_FIR(0));
-        RR(VID_PICTURE_SIZE(0));
-        RR(VID_ACCU0(0));
-        RR(VID_ACCU1(0));
-
-        RR(VID_FIR_COEF_H(0, 0));
-        RR(VID_FIR_COEF_H(0, 1));
-        RR(VID_FIR_COEF_H(0, 2));
-        RR(VID_FIR_COEF_H(0, 3));
-        RR(VID_FIR_COEF_H(0, 4));
-        RR(VID_FIR_COEF_H(0, 5));
-        RR(VID_FIR_COEF_H(0, 6));
-        RR(VID_FIR_COEF_H(0, 7));
-
-        RR(VID_FIR_COEF_HV(0, 0));
-        RR(VID_FIR_COEF_HV(0, 1));
-        RR(VID_FIR_COEF_HV(0, 2));
-        RR(VID_FIR_COEF_HV(0, 3));
-        RR(VID_FIR_COEF_HV(0, 4));
-        RR(VID_FIR_COEF_HV(0, 5));
-        RR(VID_FIR_COEF_HV(0, 6));
-        RR(VID_FIR_COEF_HV(0, 7));
-
-        RR(VID_CONV_COEF(0, 0));
-        RR(VID_CONV_COEF(0, 1));
-        RR(VID_CONV_COEF(0, 2));
-        RR(VID_CONV_COEF(0, 3));
-        RR(VID_CONV_COEF(0, 4));
-
-        RR(VID_FIR_COEF_V(0, 0));
-        RR(VID_FIR_COEF_V(0, 1));
-        RR(VID_FIR_COEF_V(0, 2));
-        RR(VID_FIR_COEF_V(0, 3));
-        RR(VID_FIR_COEF_V(0, 4));
-        RR(VID_FIR_COEF_V(0, 5));
-        RR(VID_FIR_COEF_V(0, 6));
-        RR(VID_FIR_COEF_V(0, 7));
-
-        RR(VID_PRELOAD(0));
+        RR(OVL_BA0(OMAP_DSS_VIDEO1));
+        RR(OVL_BA1(OMAP_DSS_VIDEO1));
+        RR(OVL_POSITION(OMAP_DSS_VIDEO1));
+        RR(OVL_SIZE(OMAP_DSS_VIDEO1));
+        RR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO1));
+        RR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO1));
+        RR(OVL_ROW_INC(OMAP_DSS_VIDEO1));
+        RR(OVL_PIXEL_INC(OMAP_DSS_VIDEO1));
+        RR(OVL_FIR(OMAP_DSS_VIDEO1));
+        RR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO1));
+        RR(OVL_ACCU0(OMAP_DSS_VIDEO1));
+        RR(OVL_ACCU1(OMAP_DSS_VIDEO1));
+
+        for (i = 0; i < 8; i++)
+                RR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, i));
+
+        for (i = 0; i < 8; i++)
+                RR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, i));
+
+        for (i = 0; i < 5; i++)
+                RR(OVL_CONV_COEF(OMAP_DSS_VIDEO1, i));
+
+        for (i = 0; i < 8; i++)
+                RR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, i));
+
+        if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                RR(OVL_BA0_UV(OMAP_DSS_VIDEO1));
+                RR(OVL_BA1_UV(OMAP_DSS_VIDEO1));
+                RR(OVL_FIR2(OMAP_DSS_VIDEO1));
+                RR(OVL_ACCU2_0(OMAP_DSS_VIDEO1));
+                RR(OVL_ACCU2_1(OMAP_DSS_VIDEO1));
+
+                for (i = 0; i < 8; i++)
+                        RR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, i));
+
+                for (i = 0; i < 8; i++)
+                        RR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, i));
+
+                for (i = 0; i < 8; i++)
+                        RR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, i));
+        }
+        if (dss_has_feature(FEAT_ATTR2))
+                RR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO1));
+
+        RR(OVL_PRELOAD(OMAP_DSS_VIDEO1));
 
         /* VID2 */
-        RR(VID_BA0(1));
-        RR(VID_BA1(1));
-        RR(VID_POSITION(1));
-        RR(VID_SIZE(1));
-        RR(VID_ATTRIBUTES(1));
-        RR(VID_FIFO_THRESHOLD(1));
-        RR(VID_ROW_INC(1));
-        RR(VID_PIXEL_INC(1));
-        RR(VID_FIR(1));
-        RR(VID_PICTURE_SIZE(1));
-        RR(VID_ACCU0(1));
-        RR(VID_ACCU1(1));
-
-        RR(VID_FIR_COEF_H(1, 0));
-        RR(VID_FIR_COEF_H(1, 1));
-        RR(VID_FIR_COEF_H(1, 2));
-        RR(VID_FIR_COEF_H(1, 3));
-        RR(VID_FIR_COEF_H(1, 4));
-        RR(VID_FIR_COEF_H(1, 5));
-        RR(VID_FIR_COEF_H(1, 6));
-        RR(VID_FIR_COEF_H(1, 7));
-
-        RR(VID_FIR_COEF_HV(1, 0));
-        RR(VID_FIR_COEF_HV(1, 1));
-        RR(VID_FIR_COEF_HV(1, 2));
-        RR(VID_FIR_COEF_HV(1, 3));
-        RR(VID_FIR_COEF_HV(1, 4));
-        RR(VID_FIR_COEF_HV(1, 5));
-        RR(VID_FIR_COEF_HV(1, 6));
-        RR(VID_FIR_COEF_HV(1, 7));
-
-        RR(VID_CONV_COEF(1, 0));
-        RR(VID_CONV_COEF(1, 1));
-        RR(VID_CONV_COEF(1, 2));
-        RR(VID_CONV_COEF(1, 3));
-        RR(VID_CONV_COEF(1, 4));
-
-        RR(VID_FIR_COEF_V(1, 0));
-        RR(VID_FIR_COEF_V(1, 1));
-        RR(VID_FIR_COEF_V(1, 2));
-        RR(VID_FIR_COEF_V(1, 3));
-        RR(VID_FIR_COEF_V(1, 4));
-        RR(VID_FIR_COEF_V(1, 5));
-        RR(VID_FIR_COEF_V(1, 6));
-        RR(VID_FIR_COEF_V(1, 7));
-
-        RR(VID_PRELOAD(1));
+        RR(OVL_BA0(OMAP_DSS_VIDEO2));
+        RR(OVL_BA1(OMAP_DSS_VIDEO2));
+        RR(OVL_POSITION(OMAP_DSS_VIDEO2));
+        RR(OVL_SIZE(OMAP_DSS_VIDEO2));
+        RR(OVL_ATTRIBUTES(OMAP_DSS_VIDEO2));
+        RR(OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO2));
+        RR(OVL_ROW_INC(OMAP_DSS_VIDEO2));
+        RR(OVL_PIXEL_INC(OMAP_DSS_VIDEO2));
+        RR(OVL_FIR(OMAP_DSS_VIDEO2));
+        RR(OVL_PICTURE_SIZE(OMAP_DSS_VIDEO2));
+        RR(OVL_ACCU0(OMAP_DSS_VIDEO2));
+        RR(OVL_ACCU1(OMAP_DSS_VIDEO2));
+
+        for (i = 0; i < 8; i++)
+                RR(OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, i));
+
+        for (i = 0; i < 8; i++)
+                RR(OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, i));
+
+        for (i = 0; i < 5; i++)
+                RR(OVL_CONV_COEF(OMAP_DSS_VIDEO2, i));
+
+        for (i = 0; i < 8; i++)
+                RR(OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, i));
+
+        if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                RR(OVL_BA0_UV(OMAP_DSS_VIDEO2));
+                RR(OVL_BA1_UV(OMAP_DSS_VIDEO2));
+                RR(OVL_FIR2(OMAP_DSS_VIDEO2));
+                RR(OVL_ACCU2_0(OMAP_DSS_VIDEO2));
+                RR(OVL_ACCU2_1(OMAP_DSS_VIDEO2));
+
+                for (i = 0; i < 8; i++)
+                        RR(OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, i));
+
+                for (i = 0; i < 8; i++)
+                        RR(OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, i));
+
+                for (i = 0; i < 8; i++)
+                        RR(OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, i));
+        }
+        if (dss_has_feature(FEAT_ATTR2))
+                RR(OVL_ATTRIBUTES2(OMAP_DSS_VIDEO2));
+
+        RR(OVL_PRELOAD(OMAP_DSS_VIDEO2));
+
+        if (dss_has_feature(FEAT_CORE_CLK_DIV))
+                RR(DIVISOR);
 
         /* enable last, because LCD & DIGIT enable are here */
         RR(CONTROL);
-
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                RR(CONTROL2);
         /* clear spurious SYNC_LOST_DIGIT interrupts */
         dispc_write_reg(DISPC_IRQSTATUS, DISPC_IRQ_SYNC_LOST_DIGIT);
 
@@ -490,180 +479,188 @@ void dispc_restore_context(void)
 static inline void enable_clocks(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 bool dispc_go_busy(enum omap_channel channel)
 {
         int bit;
 
-        if (channel == OMAP_DSS_CHANNEL_LCD)
+        if (channel == OMAP_DSS_CHANNEL_LCD ||
+                        channel == OMAP_DSS_CHANNEL_LCD2)
                 bit = 5; /* GOLCD */
         else
                 bit = 6; /* GODIGIT */
 
-        return REG_GET(DISPC_CONTROL, bit, bit) == 1;
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                return REG_GET(DISPC_CONTROL2, bit, bit) == 1;
+        else
+                return REG_GET(DISPC_CONTROL, bit, bit) == 1;
 }
 
 void dispc_go(enum omap_channel channel)
 {
         int bit;
+        bool enable_bit, go_bit;
 
         enable_clocks(1);
 
-        if (channel == OMAP_DSS_CHANNEL_LCD)
+        if (channel == OMAP_DSS_CHANNEL_LCD ||
+                        channel == OMAP_DSS_CHANNEL_LCD2)
                 bit = 0; /* LCDENABLE */
         else
                 bit = 1; /* DIGITALENABLE */
 
         /* if the channel is not enabled, we don't need GO */
-        if (REG_GET(DISPC_CONTROL, bit, bit) == 0)
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                enable_bit = REG_GET(DISPC_CONTROL2, bit, bit) == 1;
+        else
+                enable_bit = REG_GET(DISPC_CONTROL, bit, bit) == 1;
+
+        if (!enable_bit)
                 goto end;
 
-        if (channel == OMAP_DSS_CHANNEL_LCD)
+        if (channel == OMAP_DSS_CHANNEL_LCD ||
+                        channel == OMAP_DSS_CHANNEL_LCD2)
                 bit = 5; /* GOLCD */
         else
                 bit = 6; /* GODIGIT */
 
-        if (REG_GET(DISPC_CONTROL, bit, bit) == 1) {
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                go_bit = REG_GET(DISPC_CONTROL2, bit, bit) == 1;
+        else
+                go_bit = REG_GET(DISPC_CONTROL, bit, bit) == 1;
+
+        if (go_bit) {
                 DSSERR("GO bit not down for channel %d\n", channel);
                 goto end;
         }
 
-        DSSDBG("GO %s\n", channel == OMAP_DSS_CHANNEL_LCD ? "LCD" : "DIGIT");
+        DSSDBG("GO %s\n", channel == OMAP_DSS_CHANNEL_LCD ? "LCD" :
+                (channel == OMAP_DSS_CHANNEL_LCD2 ? "LCD2" : "DIGIT"));
 
-        REG_FLD_MOD(DISPC_CONTROL, 1, bit, bit);
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                REG_FLD_MOD(DISPC_CONTROL2, 1, bit, bit);
+        else
+                REG_FLD_MOD(DISPC_CONTROL, 1, bit, bit);
 end:
         enable_clocks(0);
 }
 
 static void _dispc_write_firh_reg(enum omap_plane plane, int reg, u32 value)
 {
+        dispc_write_reg(DISPC_OVL_FIR_COEF_H(plane, reg), value);
+}
+
+static void _dispc_write_firhv_reg(enum omap_plane plane, int reg, u32 value)
+{
+        dispc_write_reg(DISPC_OVL_FIR_COEF_HV(plane, reg), value);
+}
+
+static void _dispc_write_firv_reg(enum omap_plane plane, int reg, u32 value)
+{
+        dispc_write_reg(DISPC_OVL_FIR_COEF_V(plane, reg), value);
+}
+
+static void _dispc_write_firh2_reg(enum omap_plane plane, int reg, u32 value)
+{
         BUG_ON(plane == OMAP_DSS_GFX);
 
-        dispc_write_reg(DISPC_VID_FIR_COEF_H(plane-1, reg), value);
+        dispc_write_reg(DISPC_OVL_FIR_COEF_H2(plane, reg), value);
 }
 
-static void _dispc_write_firhv_reg(enum omap_plane plane, int reg, u32 value)
+static void _dispc_write_firhv2_reg(enum omap_plane plane, int reg, u32 value)
 {
         BUG_ON(plane == OMAP_DSS_GFX);
 
-        dispc_write_reg(DISPC_VID_FIR_COEF_HV(plane-1, reg), value);
+        dispc_write_reg(DISPC_OVL_FIR_COEF_HV2(plane, reg), value);
 }
 
-static void _dispc_write_firv_reg(enum omap_plane plane, int reg, u32 value)
+static void _dispc_write_firv2_reg(enum omap_plane plane, int reg, u32 value)
 {
         BUG_ON(plane == OMAP_DSS_GFX);
 
-        dispc_write_reg(DISPC_VID_FIR_COEF_V(plane-1, reg), value);
+        dispc_write_reg(DISPC_OVL_FIR_COEF_V2(plane, reg), value);
 }
 
 static void _dispc_set_scale_coef(enum omap_plane plane, int hscaleup,
-                int vscaleup, int five_taps)
+                                  int vscaleup, int five_taps,
+                                  enum omap_color_component color_comp)
 {
         /* Coefficients for horizontal up-sampling */
-        static const 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)
@@ -671,47 +668,43 @@ 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);
+
+                if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+                        _dispc_write_firh_reg(plane, i, h);
+                        _dispc_write_firhv_reg(plane, i, hv);
+                } else {
+                        _dispc_write_firh2_reg(plane, i, h);
+                        _dispc_write_firhv2_reg(plane, i, hv);
                 }
 
-                _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);
+                        if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y)
+                                _dispc_write_firv_reg(plane, i, v);
+                        else
+                                _dispc_write_firv2_reg(plane, i, v);
+                }
         }
 }
 
@@ -730,80 +723,104 @@ static void _dispc_setup_color_conv_coef(void)
 
         ct = &ctbl_bt601_5;
 
-        dispc_write_reg(DISPC_VID_CONV_COEF(0, 0), CVAL(ct->rcr, ct->ry));
-        dispc_write_reg(DISPC_VID_CONV_COEF(0, 1), CVAL(ct->gy,  ct->rcb));
-        dispc_write_reg(DISPC_VID_CONV_COEF(0, 2), CVAL(ct->gcb, ct->gcr));
-        dispc_write_reg(DISPC_VID_CONV_COEF(0, 3), CVAL(ct->bcr, ct->by));
-        dispc_write_reg(DISPC_VID_CONV_COEF(0, 4), CVAL(0,       ct->bcb));
-
-        dispc_write_reg(DISPC_VID_CONV_COEF(1, 0), CVAL(ct->rcr, ct->ry));
-        dispc_write_reg(DISPC_VID_CONV_COEF(1, 1), CVAL(ct->gy,  ct->rcb));
-        dispc_write_reg(DISPC_VID_CONV_COEF(1, 2), CVAL(ct->gcb, ct->gcr));
-        dispc_write_reg(DISPC_VID_CONV_COEF(1, 3), CVAL(ct->bcr, ct->by));
-        dispc_write_reg(DISPC_VID_CONV_COEF(1, 4), CVAL(0,       ct->bcb));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 0),
+                CVAL(ct->rcr, ct->ry));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 1),
+                CVAL(ct->gy,  ct->rcb));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 2),
+                CVAL(ct->gcb, ct->gcr));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 3),
+                CVAL(ct->bcr, ct->by));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 4),
+                CVAL(0, ct->bcb));
+
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 0),
+                CVAL(ct->rcr, ct->ry));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 1),
+                CVAL(ct->gy, ct->rcb));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 2),
+                CVAL(ct->gcb, ct->gcr));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 3),
+                CVAL(ct->bcr, ct->by));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 4),
+                CVAL(0, ct->bcb));
 
 #undef CVAL
 
-        REG_FLD_MOD(DISPC_VID_ATTRIBUTES(0), ct->full_range, 11, 11);
-        REG_FLD_MOD(DISPC_VID_ATTRIBUTES(1), ct->full_range, 11, 11);
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO1),
+                ct->full_range, 11, 11);
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO2),
+                ct->full_range, 11, 11);
 }
 
 
 static void _dispc_set_plane_ba0(enum omap_plane plane, u32 paddr)
 {
-        const struct dispc_reg ba0_reg[] = { DISPC_GFX_BA0,
-                DISPC_VID_BA0(0),
-                DISPC_VID_BA0(1) };
-
-        dispc_write_reg(ba0_reg[plane], paddr);
+        dispc_write_reg(DISPC_OVL_BA0(plane), paddr);
 }
 
 static void _dispc_set_plane_ba1(enum omap_plane plane, u32 paddr)
 {
-        const struct dispc_reg ba1_reg[] = { DISPC_GFX_BA1,
-                                      DISPC_VID_BA1(0),
-                                      DISPC_VID_BA1(1) };
+        dispc_write_reg(DISPC_OVL_BA1(plane), paddr);
+}
 
-        dispc_write_reg(ba1_reg[plane], paddr);
+static void _dispc_set_plane_ba0_uv(enum omap_plane plane, u32 paddr)
+{
+        dispc_write_reg(DISPC_OVL_BA0_UV(plane), paddr);
 }
 
-static void _dispc_set_plane_pos(enum omap_plane plane, int x, int y)
+static void _dispc_set_plane_ba1_uv(enum omap_plane plane, u32 paddr)
 {
-        const struct dispc_reg pos_reg[] = { DISPC_GFX_POSITION,
-                                      DISPC_VID_POSITION(0),
-                                      DISPC_VID_POSITION(1) };
+        dispc_write_reg(DISPC_OVL_BA1_UV(plane), paddr);
+}
 
+static void _dispc_set_plane_pos(enum omap_plane plane, int x, int y)
+{
         u32 val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
-        dispc_write_reg(pos_reg[plane], val);
+
+        dispc_write_reg(DISPC_OVL_POSITION(plane), val);
 }
 
 static void _dispc_set_pic_size(enum omap_plane plane, int width, int height)
 {
-        const struct dispc_reg siz_reg[] = { DISPC_GFX_SIZE,
-                                      DISPC_VID_PICTURE_SIZE(0),
-                                      DISPC_VID_PICTURE_SIZE(1) };
         u32 val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
-        dispc_write_reg(siz_reg[plane], val);
+
+        if (plane == OMAP_DSS_GFX)
+                dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+        else
+                dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
 }
 
 static void _dispc_set_vid_size(enum omap_plane plane, int width, int height)
 {
         u32 val;
-        const struct dispc_reg vsi_reg[] = { DISPC_VID_SIZE(0),
-                                      DISPC_VID_SIZE(1) };
 
         BUG_ON(plane == OMAP_DSS_GFX);
 
         val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
-        dispc_write_reg(vsi_reg[plane-1], val);
+
+        dispc_write_reg(DISPC_OVL_SIZE(plane), val);
 }
 
-static void _dispc_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
+static void _dispc_set_pre_mult_alpha(enum omap_plane plane, bool enable)
 {
+        if (!dss_has_feature(FEAT_PRE_MULT_ALPHA))
+                return;
 
-        BUG_ON(plane == OMAP_DSS_VIDEO1);
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA_VID1) &&
+                plane == OMAP_DSS_VIDEO1)
+                return;
 
-        if (cpu_is_omap24xx())
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 28, 28);
+}
+
+static void _dispc_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
+{
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
+                return;
+
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA_VID1) &&
+                plane == OMAP_DSS_VIDEO1)
                 return;
 
         if (plane == OMAP_DSS_GFX)
@@ -814,61 +831,93 @@ static void _dispc_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
 
 static void _dispc_set_pix_inc(enum omap_plane plane, s32 inc)
 {
-        const struct dispc_reg ri_reg[] = { DISPC_GFX_PIXEL_INC,
-                                     DISPC_VID_PIXEL_INC(0),
-                                     DISPC_VID_PIXEL_INC(1) };
-
-        dispc_write_reg(ri_reg[plane], inc);
+        dispc_write_reg(DISPC_OVL_PIXEL_INC(plane), inc);
 }
 
 static void _dispc_set_row_inc(enum omap_plane plane, s32 inc)
 {
-        const struct dispc_reg ri_reg[] = { DISPC_GFX_ROW_INC,
-                                     DISPC_VID_ROW_INC(0),
-                                     DISPC_VID_ROW_INC(1) };
-
-        dispc_write_reg(ri_reg[plane], inc);
+        dispc_write_reg(DISPC_OVL_ROW_INC(plane), inc);
 }
 
 static void _dispc_set_color_mode(enum omap_plane plane,
                 enum omap_color_mode color_mode)
 {
         u32 m = 0;
-
-        switch (color_mode) {
-        case OMAP_DSS_COLOR_CLUT1:
-                m = 0x0; break;
-        case OMAP_DSS_COLOR_CLUT2:
-                m = 0x1; break;
-        case OMAP_DSS_COLOR_CLUT4:
-                m = 0x2; break;
-        case OMAP_DSS_COLOR_CLUT8:
-                m = 0x3; break;
-        case OMAP_DSS_COLOR_RGB12U:
-                m = 0x4; break;
-        case OMAP_DSS_COLOR_ARGB16:
-                m = 0x5; break;
-        case OMAP_DSS_COLOR_RGB16:
-                m = 0x6; break;
-        case OMAP_DSS_COLOR_RGB24U:
-                m = 0x8; break;
-        case OMAP_DSS_COLOR_RGB24P:
-                m = 0x9; break;
-        case OMAP_DSS_COLOR_YUV2:
-                m = 0xa; break;
-        case OMAP_DSS_COLOR_UYVY:
-                m = 0xb; break;
-        case OMAP_DSS_COLOR_ARGB32:
-                m = 0xc; break;
-        case OMAP_DSS_COLOR_RGBA32:
-                m = 0xd; break;
-        case OMAP_DSS_COLOR_RGBX32:
-                m = 0xe; break;
-        default:
-                BUG(); break;
+        if (plane != OMAP_DSS_GFX) {
+                switch (color_mode) {
+                case OMAP_DSS_COLOR_NV12:
+                        m = 0x0; break;
+                case OMAP_DSS_COLOR_RGB12U:
+                        m = 0x1; break;
+                case OMAP_DSS_COLOR_RGBA16:
+                        m = 0x2; break;
+                case OMAP_DSS_COLOR_RGBX16:
+                        m = 0x4; break;
+                case OMAP_DSS_COLOR_ARGB16:
+                        m = 0x5; break;
+                case OMAP_DSS_COLOR_RGB16:
+                        m = 0x6; break;
+                case OMAP_DSS_COLOR_ARGB16_1555:
+                        m = 0x7; break;
+                case OMAP_DSS_COLOR_RGB24U:
+                        m = 0x8; break;
+                case OMAP_DSS_COLOR_RGB24P:
+                        m = 0x9; break;
+                case OMAP_DSS_COLOR_YUV2:
+                        m = 0xa; break;
+                case OMAP_DSS_COLOR_UYVY:
+                        m = 0xb; break;
+                case OMAP_DSS_COLOR_ARGB32:
+                        m = 0xc; break;
+                case OMAP_DSS_COLOR_RGBA32:
+                        m = 0xd; break;
+                case OMAP_DSS_COLOR_RGBX32:
+                        m = 0xe; break;
+                case OMAP_DSS_COLOR_XRGB16_1555:
+                        m = 0xf; break;
+                default:
+                        BUG(); break;
+                }
+        } else {
+                switch (color_mode) {
+                case OMAP_DSS_COLOR_CLUT1:
+                        m = 0x0; break;
+                case OMAP_DSS_COLOR_CLUT2:
+                        m = 0x1; break;
+                case OMAP_DSS_COLOR_CLUT4:
+                        m = 0x2; break;
+                case OMAP_DSS_COLOR_CLUT8:
+                        m = 0x3; break;
+                case OMAP_DSS_COLOR_RGB12U:
+                        m = 0x4; break;
+                case OMAP_DSS_COLOR_ARGB16:
+                        m = 0x5; break;
+                case OMAP_DSS_COLOR_RGB16:
+                        m = 0x6; break;
+                case OMAP_DSS_COLOR_ARGB16_1555:
+                        m = 0x7; break;
+                case OMAP_DSS_COLOR_RGB24U:
+                        m = 0x8; break;
+                case OMAP_DSS_COLOR_RGB24P:
+                        m = 0x9; break;
+                case OMAP_DSS_COLOR_YUV2:
+                        m = 0xa; break;
+                case OMAP_DSS_COLOR_UYVY:
+                        m = 0xb; break;
+                case OMAP_DSS_COLOR_ARGB32:
+                        m = 0xc; break;
+                case OMAP_DSS_COLOR_RGBA32:
+                        m = 0xd; break;
+                case OMAP_DSS_COLOR_RGBX32:
+                        m = 0xe; break;
+                case OMAP_DSS_COLOR_XRGB16_1555:
+                        m = 0xf; break;
+                default:
+                        BUG(); break;
+                }
         }
 
-        REG_FLD_MOD(dispc_reg_att[plane], m, 4, 1);
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), m, 4, 1);
 }
 
 static void _dispc_set_channel_out(enum omap_plane plane,
@@ -876,6 +925,7 @@ static void _dispc_set_channel_out(enum omap_plane plane,
 {
         int shift;
         u32 val;
+        int chan = 0, chan2 = 0;
 
         switch (plane) {
         case OMAP_DSS_GFX:
@@ -890,9 +940,31 @@ static void _dispc_set_channel_out(enum omap_plane plane,
                 return;
         }
 
-        val = dispc_read_reg(dispc_reg_att[plane]);
-        val = FLD_MOD(val, channel, shift, shift);
-        dispc_write_reg(dispc_reg_att[plane], val);
+        val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                switch (channel) {
+                case OMAP_DSS_CHANNEL_LCD:
+                        chan = 0;
+                        chan2 = 0;
+                        break;
+                case OMAP_DSS_CHANNEL_DIGIT:
+                        chan = 1;
+                        chan2 = 0;
+                        break;
+                case OMAP_DSS_CHANNEL_LCD2:
+                        chan = 0;
+                        chan2 = 1;
+                        break;
+                default:
+                        BUG();
+                }
+
+                val = FLD_MOD(val, chan, shift, shift);
+                val = FLD_MOD(val, chan2, 31, 30);
+        } else {
+                val = FLD_MOD(val, channel, shift, shift);
+        }
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 }
 
 void dispc_set_burst_size(enum omap_plane plane,
@@ -916,22 +988,36 @@ void dispc_set_burst_size(enum omap_plane plane,
                 return;
         }
 
-        val = dispc_read_reg(dispc_reg_att[plane]);
+        val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
         val = FLD_MOD(val, burst_size, shift+1, shift);
-        dispc_write_reg(dispc_reg_att[plane], val);
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 
         enable_clocks(0);
 }
 
+void dispc_enable_gamma_table(bool enable)
+{
+        /*
+         * This is partially implemented to support only disabling of
+         * the gamma table.
+         */
+        if (enable) {
+                DSSWARN("Gamma table enabling for TV not yet supported");
+                return;
+        }
+
+        REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
+}
+
 static void _dispc_set_vid_color_conv(enum omap_plane plane, bool enable)
 {
         u32 val;
 
         BUG_ON(plane == OMAP_DSS_GFX);
 
-        val = dispc_read_reg(dispc_reg_att[plane]);
+        val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
         val = FLD_MOD(val, enable, 9, 9);
-        dispc_write_reg(dispc_reg_att[plane], val);
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 }
 
 void dispc_enable_replication(enum omap_plane plane, bool enable)
@@ -944,17 +1030,17 @@ void dispc_enable_replication(enum omap_plane plane, bool enable)
                 bit = 10;
 
         enable_clocks(1);
-        REG_FLD_MOD(dispc_reg_att[plane], enable, bit, bit);
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, bit, bit);
         enable_clocks(0);
 }
 
-void dispc_set_lcd_size(u16 width, u16 height)
+void dispc_set_lcd_size(enum omap_channel channel, u16 width, u16 height)
 {
         u32 val;
         BUG_ON((width > (1 << 11)) || (height > (1 << 11)));
         val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
         enable_clocks(1);
-        dispc_write_reg(DISPC_SIZE_LCD, val);
+        dispc_write_reg(DISPC_SIZE_MGR(channel), val);
         enable_clocks(0);
 }
 
@@ -964,28 +1050,23 @@ void dispc_set_digit_size(u16 width, u16 height)
         BUG_ON((width > (1 << 11)) || (height > (1 << 11)));
         val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
         enable_clocks(1);
-        dispc_write_reg(DISPC_SIZE_DIG, val);
+        dispc_write_reg(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT), val);
         enable_clocks(0);
 }
 
 static void dispc_read_plane_fifo_sizes(void)
 {
-        const struct dispc_reg fsz_reg[] = { DISPC_GFX_FIFO_SIZE_STATUS,
-                                      DISPC_VID_FIFO_SIZE_STATUS(0),
-                                      DISPC_VID_FIFO_SIZE_STATUS(1) };
         u32 size;
         int plane;
+        u8 start, end;
 
         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(DISPC_OVL_FIFO_SIZE_STATUS(plane)),
+                        start, end);
                 dispc.fifo_size[plane] = size;
         }
 
@@ -999,23 +1080,24 @@ u32 dispc_get_plane_fifo_size(enum omap_plane plane)
 
 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;
+
+        dss_feat_get_reg_field(FEAT_REG_FIFOHIGHTHRESHOLD, &hi_start, &hi_end);
+        dss_feat_get_reg_field(FEAT_REG_FIFOLOWTHRESHOLD, &lo_start, &lo_end);
+
         enable_clocks(1);
 
         DSSDBG("fifo(%d) low/high old %u/%u, new %u/%u\n",
                         plane,
-                        REG_GET(ftrs_reg[plane], 11, 0),
-                        REG_GET(ftrs_reg[plane], 27, 16),
+                        REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+                                lo_start, lo_end),
+                        REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+                                hi_start, hi_end),
                         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));
+        dispc_write_reg(DISPC_OVL_FIFO_THRESHOLD(plane),
+                        FLD_VAL(high, hi_start, hi_end) |
+                        FLD_VAL(low, lo_start, lo_end));
 
         enable_clocks(0);
 }
@@ -1030,91 +1112,129 @@ void dispc_enable_fifomerge(bool enable)
         enable_clocks(0);
 }
 
-static void _dispc_set_fir(enum omap_plane plane, int hinc, int vinc)
+static void _dispc_set_fir(enum omap_plane plane,
+                                int hinc, int vinc,
+                                enum omap_color_component color_comp)
 {
         u32 val;
-        const struct dispc_reg fir_reg[] = { DISPC_VID_FIR(0),
-                                      DISPC_VID_FIR(1) };
 
-        BUG_ON(plane == OMAP_DSS_GFX);
+        if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+                u8 hinc_start, hinc_end, vinc_start, vinc_end;
 
-        if (cpu_is_omap24xx())
-                val = FLD_VAL(vinc, 27, 16) | FLD_VAL(hinc, 11, 0);
-        else
+                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(DISPC_OVL_FIR(plane), val);
+        } else {
                 val = FLD_VAL(vinc, 28, 16) | FLD_VAL(hinc, 12, 0);
-        dispc_write_reg(fir_reg[plane-1], val);
+                dispc_write_reg(DISPC_OVL_FIR2(plane), val);
+        }
 }
 
 static void _dispc_set_vid_accu0(enum omap_plane plane, int haccu, int vaccu)
 {
         u32 val;
-        const struct dispc_reg ac0_reg[] = { DISPC_VID_ACCU0(0),
-                                      DISPC_VID_ACCU0(1) };
+        u8 hor_start, hor_end, vert_start, vert_end;
 
-        BUG_ON(plane == OMAP_DSS_GFX);
+        dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+        dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
+
+        val = FLD_VAL(vaccu, vert_start, vert_end) |
+                        FLD_VAL(haccu, hor_start, hor_end);
 
-        val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
-        dispc_write_reg(ac0_reg[plane-1], val);
+        dispc_write_reg(DISPC_OVL_ACCU0(plane), val);
 }
 
 static void _dispc_set_vid_accu1(enum omap_plane plane, int haccu, int vaccu)
 {
         u32 val;
-        const struct dispc_reg ac1_reg[] = { DISPC_VID_ACCU1(0),
-                                      DISPC_VID_ACCU1(1) };
+        u8 hor_start, hor_end, vert_start, vert_end;
 
-        BUG_ON(plane == OMAP_DSS_GFX);
+        dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+        dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
+
+        val = FLD_VAL(vaccu, vert_start, vert_end) |
+                        FLD_VAL(haccu, hor_start, hor_end);
 
-        val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
-        dispc_write_reg(ac1_reg[plane-1], val);
+        dispc_write_reg(DISPC_OVL_ACCU1(plane), val);
 }
 
+static void _dispc_set_vid_accu2_0(enum omap_plane plane, int haccu, int vaccu)
+{
+        u32 val;
 
-static void _dispc_set_scaling(enum omap_plane plane,
+        val = FLD_VAL(vaccu, 26, 16) | FLD_VAL(haccu, 10, 0);
+        dispc_write_reg(DISPC_OVL_ACCU2_0(plane), val);
+}
+
+static void _dispc_set_vid_accu2_1(enum omap_plane plane, int haccu, int vaccu)
+{
+        u32 val;
+
+        val = FLD_VAL(vaccu, 26, 16) | FLD_VAL(haccu, 10, 0);
+        dispc_write_reg(DISPC_OVL_ACCU2_1(plane), val);
+}
+
+static void _dispc_set_scale_param(enum omap_plane plane,
                 u16 orig_width, u16 orig_height,
                 u16 out_width, u16 out_height,
-                bool ilace, bool five_taps,
-                bool fieldmode)
+                bool five_taps, u8 rotation,
+                enum omap_color_component color_comp)
 {
-        int fir_hinc;
-        int fir_vinc;
+        int fir_hinc, fir_vinc;
         int hscaleup, vscaleup;
-        int accu0 = 0;
-        int accu1 = 0;
-        u32 l;
-
-        BUG_ON(plane == OMAP_DSS_GFX);
 
         hscaleup = orig_width <= out_width;
         vscaleup = orig_height <= out_height;
 
-        _dispc_set_scale_coef(plane, hscaleup, vscaleup, five_taps);
+        _dispc_set_scale_coef(plane, hscaleup, vscaleup, five_taps, color_comp);
 
-        if (!orig_width || orig_width == out_width)
-                fir_hinc = 0;
-        else
-                fir_hinc = 1024 * orig_width / out_width;
+        fir_hinc = 1024 * orig_width / out_width;
+        fir_vinc = 1024 * orig_height / out_height;
 
-        if (!orig_height || orig_height == out_height)
-                fir_vinc = 0;
-        else
-                fir_vinc = 1024 * orig_height / out_height;
+        _dispc_set_fir(plane, fir_hinc, fir_vinc, color_comp);
+}
 
-        _dispc_set_fir(plane, fir_hinc, fir_vinc);
+static void _dispc_set_scaling_common(enum omap_plane plane,
+                u16 orig_width, u16 orig_height,
+                u16 out_width, u16 out_height,
+                bool ilace, bool five_taps,
+                bool fieldmode, enum omap_color_mode color_mode,
+                u8 rotation)
+{
+        int accu0 = 0;
+        int accu1 = 0;
+        u32 l;
 
-        l = dispc_read_reg(dispc_reg_att[plane]);
-        l &= ~((0x0f << 5) | (0x3 << 21));
+        _dispc_set_scale_param(plane, orig_width, orig_height,
+                                out_width, out_height, five_taps,
+                                rotation, DISPC_COLOR_COMPONENT_RGB_Y);
+        l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 
-        l |= fir_hinc ? (1 << 5) : 0;
-        l |= fir_vinc ? (1 << 6) : 0;
+        /* RESIZEENABLE and VERTICALTAPS */
+        l &= ~((0x3 << 5) | (0x1 << 21));
+        l |= (orig_width != out_width) ? (1 << 5) : 0;
+        l |= (orig_height != out_height) ? (1 << 6) : 0;
+        l |= five_taps ? (1 << 21) : 0;
 
-        l |= hscaleup ? 0 : (1 << 7);
-        l |= vscaleup ? 0 : (1 << 8);
+        /* VRESIZECONF and HRESIZECONF */
+        if (dss_has_feature(FEAT_RESIZECONF)) {
+                l &= ~(0x3 << 7);
+                l |= (orig_width <= out_width) ? 0 : (1 << 7);
+                l |= (orig_height <= out_height) ? 0 : (1 << 8);
+        }
 
-        l |= five_taps ? (1 << 21) : 0;
-        l |= five_taps ? (1 << 22) : 0;
+        /* LINEBUFFERSPLIT */
+        if (dss_has_feature(FEAT_LINEBUFFERSPLIT)) {
+                l &= ~(0x1 << 22);
+                l |= five_taps ? (1 << 22) : 0;
+        }
 
-        dispc_write_reg(dispc_reg_att[plane], l);
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
 
         /*
          * field 0 = even field = bottom field
@@ -1122,7 +1242,7 @@ static void _dispc_set_scaling(enum omap_plane plane,
          */
         if (ilace && !fieldmode) {
                 accu1 = 0;
-                accu0 = (fir_vinc / 2) & 0x3ff;
+                accu0 = ((1024 * orig_height / out_height) / 2) & 0x3ff;
                 if (accu0 >= 1024/2) {
                         accu1 = 1024/2;
                         accu0 -= accu1;
@@ -1133,12 +1253,101 @@ static void _dispc_set_scaling(enum omap_plane plane,
         _dispc_set_vid_accu1(plane, 0, accu1);
 }
 
+static void _dispc_set_scaling_uv(enum omap_plane plane,
+                u16 orig_width, u16 orig_height,
+                u16 out_width, u16 out_height,
+                bool ilace, bool five_taps,
+                bool fieldmode, enum omap_color_mode color_mode,
+                u8 rotation)
+{
+        int scale_x = out_width != orig_width;
+        int scale_y = out_height != orig_height;
+
+        if (!dss_has_feature(FEAT_HANDLE_UV_SEPARATE))
+                return;
+        if ((color_mode != OMAP_DSS_COLOR_YUV2 &&
+                        color_mode != OMAP_DSS_COLOR_UYVY &&
+                        color_mode != OMAP_DSS_COLOR_NV12)) {
+                /* reset chroma resampling for RGB formats  */
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
+                return;
+        }
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_NV12:
+                /* UV is subsampled by 2 vertically*/
+                orig_height >>= 1;
+                /* UV is subsampled by 2 horz.*/
+                orig_width >>= 1;
+                break;
+        case OMAP_DSS_COLOR_YUV2:
+        case OMAP_DSS_COLOR_UYVY:
+                /*For YUV422 with 90/270 rotation,
+                 *we don't upsample chroma
+                 */
+                if (rotation == OMAP_DSS_ROT_0 ||
+                        rotation == OMAP_DSS_ROT_180)
+                        /* UV is subsampled by 2 hrz*/
+                        orig_width >>= 1;
+                /* must use FIR for YUV422 if rotated */
+                if (rotation != OMAP_DSS_ROT_0)
+                        scale_x = scale_y = true;
+                break;
+        default:
+                BUG();
+        }
+
+        if (out_width != orig_width)
+                scale_x = true;
+        if (out_height != orig_height)
+                scale_y = true;
+
+        _dispc_set_scale_param(plane, orig_width, orig_height,
+                        out_width, out_height, five_taps,
+                                rotation, DISPC_COLOR_COMPONENT_UV);
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
+                (scale_x || scale_y) ? 1 : 0, 8, 8);
+        /* set H scaling */
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_x ? 1 : 0, 5, 5);
+        /* set V scaling */
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_y ? 1 : 0, 6, 6);
+
+        _dispc_set_vid_accu2_0(plane, 0x80, 0);
+        _dispc_set_vid_accu2_1(plane, 0x80, 0);
+}
+
+static void _dispc_set_scaling(enum omap_plane plane,
+                u16 orig_width, u16 orig_height,
+                u16 out_width, u16 out_height,
+                bool ilace, bool five_taps,
+                bool fieldmode, enum omap_color_mode color_mode,
+                u8 rotation)
+{
+        BUG_ON(plane == OMAP_DSS_GFX);
+
+        _dispc_set_scaling_common(plane,
+                        orig_width, orig_height,
+                        out_width, out_height,
+                        ilace, five_taps,
+                        fieldmode, color_mode,
+                        rotation);
+
+        _dispc_set_scaling_uv(plane,
+                orig_width, orig_height,
+                out_width, out_height,
+                ilace, five_taps,
+                fieldmode, color_mode,
+                rotation);
+}
+
 static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
                 bool mirroring, enum omap_color_mode color_mode)
 {
+        bool row_repeat = false;
+        int vidrot = 0;
+
         if (color_mode == OMAP_DSS_COLOR_YUV2 ||
                         color_mode == OMAP_DSS_COLOR_UYVY) {
-                int vidrot = 0;
 
                 if (mirroring) {
                         switch (rotation) {
@@ -1172,16 +1381,16 @@ static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
                         }
                 }
 
-                REG_FLD_MOD(dispc_reg_att[plane], vidrot, 13, 12);
-
                 if (rotation == OMAP_DSS_ROT_90 || rotation == OMAP_DSS_ROT_270)
-                        REG_FLD_MOD(dispc_reg_att[plane], 0x1, 18, 18);
+                        row_repeat = true;
                 else
-                        REG_FLD_MOD(dispc_reg_att[plane], 0x0, 18, 18);
-        } else {
-                REG_FLD_MOD(dispc_reg_att[plane], 0, 13, 12);
-                REG_FLD_MOD(dispc_reg_att[plane], 0, 18, 18);
+                        row_repeat = false;
         }
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), vidrot, 13, 12);
+        if (dss_has_feature(FEAT_ROWREPEATENABLE))
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane),
+                        row_repeat ? 1 : 0, 18, 18);
 }
 
 static int color_mode_to_bpp(enum omap_color_mode color_mode)
@@ -1194,12 +1403,17 @@ static int color_mode_to_bpp(enum omap_color_mode color_mode)
         case OMAP_DSS_COLOR_CLUT4:
                 return 4;
         case OMAP_DSS_COLOR_CLUT8:
+        case OMAP_DSS_COLOR_NV12:
                 return 8;
         case OMAP_DSS_COLOR_RGB12U:
         case OMAP_DSS_COLOR_RGB16:
         case OMAP_DSS_COLOR_ARGB16:
         case OMAP_DSS_COLOR_YUV2:
         case OMAP_DSS_COLOR_UYVY:
+        case OMAP_DSS_COLOR_RGBA16:
+        case OMAP_DSS_COLOR_RGBX16:
+        case OMAP_DSS_COLOR_ARGB16_1555:
+        case OMAP_DSS_COLOR_XRGB16_1555:
                 return 16;
         case OMAP_DSS_COLOR_RGB24P:
                 return 24;
@@ -1449,12 +1663,13 @@ static void calc_dma_rotation_offset(u8 rotation, bool mirror,
         }
 }
 
-static unsigned long calc_fclk_five_taps(u16 width, u16 height,
-                u16 out_width, u16 out_height, enum omap_color_mode color_mode)
+static unsigned long calc_fclk_five_taps(enum omap_channel channel, u16 width,
+                u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode)
 {
         u32 fclk = 0;
         /* FIXME venc pclk? */
-        u64 tmp, pclk = dispc_pclk_rate();
+        u64 tmp, pclk = dispc_pclk_rate(channel);
 
         if (height > out_height) {
                 /* FIXME get real display PPL */
@@ -1486,8 +1701,8 @@ static unsigned long calc_fclk_five_taps(u16 width, u16 height,
         return fclk;
 }
 
-static unsigned long calc_fclk(u16 width, u16 height,
-                u16 out_width, u16 out_height)
+static unsigned long calc_fclk(enum omap_channel channel, u16 width,
+                u16 height, u16 out_width, u16 out_height)
 {
         unsigned int hf, vf;
 
@@ -1511,7 +1726,7 @@ static unsigned long calc_fclk(u16 width, u16 height,
                 vf = 1;
 
         /* FIXME venc pclk? */
-        return dispc_pclk_rate() * vf * hf;
+        return dispc_pclk_rate(channel) * vf * hf;
 }
 
 void dispc_set_channel_out(enum omap_plane plane, enum omap_channel channel_out)
@@ -1530,7 +1745,8 @@ static int _dispc_setup_plane(enum omap_plane plane,
                 bool ilace,
                 enum omap_dss_rotation_type rotation_type,
                 u8 rotation, int mirror,
-                u8 global_alpha)
+                u8 global_alpha, u8 pre_mult_alpha,
+                enum omap_channel channel, u32 puv_addr)
 {
         const int maxdownscale = cpu_is_omap34xx() ? 4 : 2;
         bool five_taps = 0;
@@ -1559,27 +1775,12 @@ static int _dispc_setup_plane(enum omap_plane plane,
                                 height, pos_y, out_height);
         }
 
+        if (!dss_feat_color_mode_supported(plane, color_mode))
+                return -EINVAL;
+
         if (plane == OMAP_DSS_GFX) {
                 if (width != out_width || height != out_height)
                         return -EINVAL;
-
-                switch (color_mode) {
-                case OMAP_DSS_COLOR_ARGB16:
-                case OMAP_DSS_COLOR_ARGB32:
-                case OMAP_DSS_COLOR_RGBA32:
-                case OMAP_DSS_COLOR_RGBX32:
-                        if (cpu_is_omap24xx())
-                                return -EINVAL;
-                        /* fall through */
-                case OMAP_DSS_COLOR_RGB12U:
-                case OMAP_DSS_COLOR_RGB16:
-                case OMAP_DSS_COLOR_RGB24P:
-                case OMAP_DSS_COLOR_RGB24U:
-                        break;
-
-                default:
-                        return -EINVAL;
-                }
         } else {
                 /* video plane */
 
@@ -1593,41 +1794,17 @@ static int _dispc_setup_plane(enum omap_plane plane,
                    out_height > height * 8)
                         return -EINVAL;
 
-                switch (color_mode) {
-                case OMAP_DSS_COLOR_RGBX32:
-                case OMAP_DSS_COLOR_RGB12U:
-                        if (cpu_is_omap24xx())
-                                return -EINVAL;
-                        /* fall through */
-                case OMAP_DSS_COLOR_RGB16:
-                case OMAP_DSS_COLOR_RGB24P:
-                case OMAP_DSS_COLOR_RGB24U:
-                        break;
-
-                case OMAP_DSS_COLOR_ARGB16:
-                case OMAP_DSS_COLOR_ARGB32:
-                case OMAP_DSS_COLOR_RGBA32:
-                        if (cpu_is_omap24xx())
-                                return -EINVAL;
-                        if (plane == OMAP_DSS_VIDEO1)
-                                return -EINVAL;
-                        break;
-
-                case OMAP_DSS_COLOR_YUV2:
-                case OMAP_DSS_COLOR_UYVY:
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY ||
+                        color_mode == OMAP_DSS_COLOR_NV12)
                         cconv = 1;
-                        break;
-
-                default:
-                        return -EINVAL;
-                }
 
                 /* Must use 5-tap filter? */
                 five_taps = height > out_height * 2;
 
                 if (!five_taps) {
-                        fclk = calc_fclk(width, height,
-                                        out_width, out_height);
+                        fclk = calc_fclk(channel, width, height, out_width,
+                                        out_height);
 
                         /* Try 5-tap filter if 3-tap fclk is too high */
                         if (cpu_is_omap34xx() && height > out_height &&
@@ -1641,7 +1818,7 @@ static int _dispc_setup_plane(enum omap_plane plane,
                 }
 
                 if (five_taps)
-                        fclk = calc_fclk_five_taps(width, height,
+                        fclk = calc_fclk_five_taps(channel, width, height,
                                         out_width, out_height, color_mode);
 
                 DSSDBG("required fclk rate = %lu Hz\n", fclk);
@@ -1693,6 +1870,12 @@ static int _dispc_setup_plane(enum omap_plane plane,
         _dispc_set_plane_ba0(plane, paddr + offset0);
         _dispc_set_plane_ba1(plane, paddr + offset1);
 
+        if (OMAP_DSS_COLOR_NV12 == color_mode) {
+                _dispc_set_plane_ba0_uv(plane, puv_addr + offset0);
+                _dispc_set_plane_ba1_uv(plane, puv_addr + offset1);
+        }
+
+
         _dispc_set_row_inc(plane, row_inc);
         _dispc_set_pix_inc(plane, pix_inc);
 
@@ -1706,22 +1889,23 @@ static int _dispc_setup_plane(enum omap_plane plane,
         if (plane != OMAP_DSS_GFX) {
                 _dispc_set_scaling(plane, width, height,
                                    out_width, out_height,
-                                   ilace, five_taps, fieldmode);
+                                   ilace, five_taps, fieldmode,
+                                   color_mode, rotation);
                 _dispc_set_vid_size(plane, out_width, out_height);
                 _dispc_set_vid_color_conv(plane, cconv);
         }
 
         _dispc_set_rotation_attrs(plane, rotation, mirror, color_mode);
 
-        if (plane != OMAP_DSS_VIDEO1)
-                _dispc_setup_global_alpha(plane, global_alpha);
+        _dispc_set_pre_mult_alpha(plane, pre_mult_alpha);
+        _dispc_setup_global_alpha(plane, global_alpha);
 
         return 0;
 }
 
 static void _dispc_enable_plane(enum omap_plane plane, bool enable)
 {
-        REG_FLD_MOD(dispc_reg_att[plane], enable ? 1 : 0, 0, 0);
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 0, 0);
 }
 
 static void dispc_disable_isr(void *data, u32 mask)
@@ -1730,36 +1914,44 @@ static void dispc_disable_isr(void *data, u32 mask)
         complete(compl);
 }
 
-static void _enable_lcd_out(bool enable)
+static void _enable_lcd_out(enum omap_channel channel, bool enable)
 {
-        REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 0, 0);
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                REG_FLD_MOD(DISPC_CONTROL2, enable ? 1 : 0, 0, 0);
+        else
+                REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 0, 0);
 }
 
-static void dispc_enable_lcd_out(bool enable)
+static void dispc_enable_lcd_out(enum omap_channel channel, bool enable)
 {
         struct completion frame_done_completion;
         bool is_on;
         int r;
+        u32 irq;
 
         enable_clocks(1);
 
         /* When we disable LCD output, we need to wait until frame is done.
          * Otherwise the DSS is still working, and turning off the clocks
          * prevents DSS from going to OFF mode */
-        is_on = REG_GET(DISPC_CONTROL, 0, 0);
+        is_on = channel == OMAP_DSS_CHANNEL_LCD2 ?
+                        REG_GET(DISPC_CONTROL2, 0, 0) :
+                        REG_GET(DISPC_CONTROL, 0, 0);
+
+        irq = channel == OMAP_DSS_CHANNEL_LCD2 ? DISPC_IRQ_FRAMEDONE2 :
+                        DISPC_IRQ_FRAMEDONE;
 
         if (!enable && is_on) {
                 init_completion(&frame_done_completion);
 
                 r = omap_dispc_register_isr(dispc_disable_isr,
-                                &frame_done_completion,
-                                DISPC_IRQ_FRAMEDONE);
+                                &frame_done_completion, irq);
 
                 if (r)
                         DSSERR("failed to register FRAMEDONE isr\n");
         }
 
-        _enable_lcd_out(enable);
+        _enable_lcd_out(channel, enable);
 
         if (!enable && is_on) {
                 if (!wait_for_completion_timeout(&frame_done_completion,
@@ -1767,8 +1959,7 @@ static void dispc_enable_lcd_out(bool enable)
                         DSSERR("timeout waiting for FRAME DONE\n");
 
                 r = omap_dispc_unregister_isr(dispc_disable_isr,
-                                &frame_done_completion,
-                                DISPC_IRQ_FRAMEDONE);
+                                &frame_done_completion, irq);
 
                 if (r)
                         DSSERR("failed to unregister FRAMEDONE isr\n");
@@ -1838,6 +2029,8 @@ static void dispc_enable_digit_out(bool enable)
                 unsigned long flags;
                 spin_lock_irqsave(&dispc.irq_lock, flags);
                 dispc.irq_error_mask = DISPC_IRQ_MASK_ERROR;
+                if (dss_has_feature(FEAT_MGR_LCD2))
+                        dispc.irq_error_mask |= DISPC_IRQ_SYNC_LOST2;
                 dispc_write_reg(DISPC_IRQSTATUS, DISPC_IRQ_SYNC_LOST_DIGIT);
                 _omap_dispc_set_irqs();
                 spin_unlock_irqrestore(&dispc.irq_lock, flags);
@@ -1852,14 +2045,17 @@ bool dispc_is_channel_enabled(enum omap_channel channel)
                 return !!REG_GET(DISPC_CONTROL, 0, 0);
         else if (channel == OMAP_DSS_CHANNEL_DIGIT)
                 return !!REG_GET(DISPC_CONTROL, 1, 1);
+        else if (channel == OMAP_DSS_CHANNEL_LCD2)
+                return !!REG_GET(DISPC_CONTROL2, 0, 0);
         else
                 BUG();
 }
 
 void dispc_enable_channel(enum omap_channel channel, bool enable)
 {
-        if (channel == OMAP_DSS_CHANNEL_LCD)
-                dispc_enable_lcd_out(enable);
+        if (channel == OMAP_DSS_CHANNEL_LCD ||
+                        channel == OMAP_DSS_CHANNEL_LCD2)
+                dispc_enable_lcd_out(channel, enable);
         else if (channel == OMAP_DSS_CHANNEL_DIGIT)
                 dispc_enable_digit_out(enable);
         else
@@ -1868,6 +2064,9 @@ void dispc_enable_channel(enum omap_channel channel, bool enable)
 
 void dispc_lcd_enable_signal_polarity(bool act_high)
 {
+        if (!dss_has_feature(FEAT_LCDENABLEPOL))
+                return;
+
         enable_clocks(1);
         REG_FLD_MOD(DISPC_CONTROL, act_high ? 1 : 0, 29, 29);
         enable_clocks(0);
@@ -1875,6 +2074,9 @@ void dispc_lcd_enable_signal_polarity(bool act_high)
 
 void dispc_lcd_enable_signal(bool enable)
 {
+        if (!dss_has_feature(FEAT_LCDENABLESIGNAL))
+                return;
+
         enable_clocks(1);
         REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 28, 28);
         enable_clocks(0);
@@ -1882,20 +2084,27 @@ void dispc_lcd_enable_signal(bool enable)
 
 void dispc_pck_free_enable(bool enable)
 {
+        if (!dss_has_feature(FEAT_PCKFREEENABLE))
+                return;
+
         enable_clocks(1);
         REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 27, 27);
         enable_clocks(0);
 }
 
-void dispc_enable_fifohandcheck(bool enable)
+void dispc_enable_fifohandcheck(enum omap_channel channel, bool enable)
 {
         enable_clocks(1);
-        REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 16, 16);
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                REG_FLD_MOD(DISPC_CONFIG2, enable ? 1 : 0, 16, 16);
+        else
+                REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 16, 16);
         enable_clocks(0);
 }
 
 
-void dispc_set_lcd_display_type(enum omap_lcd_display_type type)
+void dispc_set_lcd_display_type(enum omap_channel channel,
+                enum omap_lcd_display_type type)
 {
         int mode;
 
@@ -1914,7 +2123,10 @@ void dispc_set_lcd_display_type(enum omap_lcd_display_type type)
         }
 
         enable_clocks(1);
-        REG_FLD_MOD(DISPC_CONTROL, mode, 3, 3);
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                REG_FLD_MOD(DISPC_CONTROL2, mode, 3, 3);
+        else
+                REG_FLD_MOD(DISPC_CONTROL, mode, 3, 3);
         enable_clocks(0);
 }
 
@@ -1928,25 +2140,21 @@ void dispc_set_loadmode(enum omap_dss_load_mode mode)
 
 void dispc_set_default_color(enum omap_channel channel, u32 color)
 {
-        const struct dispc_reg def_reg[] = { DISPC_DEFAULT_COLOR0,
-                                DISPC_DEFAULT_COLOR1 };
-
         enable_clocks(1);
-        dispc_write_reg(def_reg[channel], color);
+        dispc_write_reg(DISPC_DEFAULT_COLOR(channel), color);
         enable_clocks(0);
 }
 
 u32 dispc_get_default_color(enum omap_channel channel)
 {
-        const struct dispc_reg def_reg[] = { DISPC_DEFAULT_COLOR0,
-                                DISPC_DEFAULT_COLOR1 };
         u32 l;
 
         BUG_ON(channel != OMAP_DSS_CHANNEL_DIGIT &&
-               channel != OMAP_DSS_CHANNEL_LCD);
+                channel != OMAP_DSS_CHANNEL_LCD &&
+                channel != OMAP_DSS_CHANNEL_LCD2);
 
         enable_clocks(1);
-        l = dispc_read_reg(def_reg[channel]);
+        l = dispc_read_reg(DISPC_DEFAULT_COLOR(channel));
         enable_clocks(0);
 
         return l;
@@ -1956,16 +2164,15 @@ void dispc_set_trans_key(enum omap_channel ch,
                 enum omap_dss_trans_key_type type,
                 u32 trans_key)
 {
-        const struct dispc_reg tr_reg[] = {
-                DISPC_TRANS_COLOR0, DISPC_TRANS_COLOR1 };
-
         enable_clocks(1);
         if (ch == OMAP_DSS_CHANNEL_LCD)
                 REG_FLD_MOD(DISPC_CONFIG, type, 11, 11);
-        else /* OMAP_DSS_CHANNEL_DIGIT */
+        else if (ch == OMAP_DSS_CHANNEL_DIGIT)
                 REG_FLD_MOD(DISPC_CONFIG, type, 13, 13);
+        else /* OMAP_DSS_CHANNEL_LCD2 */
+                REG_FLD_MOD(DISPC_CONFIG2, type, 11, 11);
 
-        dispc_write_reg(tr_reg[ch], trans_key);
+        dispc_write_reg(DISPC_TRANS_COLOR(ch), trans_key);
         enable_clocks(0);
 }
 
@@ -1973,21 +2180,20 @@ void dispc_get_trans_key(enum omap_channel ch,
                 enum omap_dss_trans_key_type *type,
                 u32 *trans_key)
 {
-        const struct dispc_reg tr_reg[] = {
-                DISPC_TRANS_COLOR0, DISPC_TRANS_COLOR1 };
-
         enable_clocks(1);
         if (type) {
                 if (ch == OMAP_DSS_CHANNEL_LCD)
                         *type = REG_GET(DISPC_CONFIG, 11, 11);
                 else if (ch == OMAP_DSS_CHANNEL_DIGIT)
                         *type = REG_GET(DISPC_CONFIG, 13, 13);
+                else if (ch == OMAP_DSS_CHANNEL_LCD2)
+                        *type = REG_GET(DISPC_CONFIG2, 11, 11);
                 else
                         BUG();
         }
 
         if (trans_key)
-                *trans_key = dispc_read_reg(tr_reg[ch]);
+                *trans_key = dispc_read_reg(DISPC_TRANS_COLOR(ch));
         enable_clocks(0);
 }
 
@@ -1996,40 +2202,45 @@ void dispc_enable_trans_key(enum omap_channel ch, bool enable)
         enable_clocks(1);
         if (ch == OMAP_DSS_CHANNEL_LCD)
                 REG_FLD_MOD(DISPC_CONFIG, enable, 10, 10);
-        else /* OMAP_DSS_CHANNEL_DIGIT */
+        else if (ch == OMAP_DSS_CHANNEL_DIGIT)
                 REG_FLD_MOD(DISPC_CONFIG, enable, 12, 12);
+        else /* OMAP_DSS_CHANNEL_LCD2 */
+                REG_FLD_MOD(DISPC_CONFIG2, enable, 10, 10);
         enable_clocks(0);
 }
 void dispc_enable_alpha_blending(enum omap_channel ch, bool enable)
 {
-        if (cpu_is_omap24xx())
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
                 return;
 
         enable_clocks(1);
         if (ch == OMAP_DSS_CHANNEL_LCD)
                 REG_FLD_MOD(DISPC_CONFIG, enable, 18, 18);
-        else /* OMAP_DSS_CHANNEL_DIGIT */
+        else if (ch == OMAP_DSS_CHANNEL_DIGIT)
                 REG_FLD_MOD(DISPC_CONFIG, enable, 19, 19);
+        else /* OMAP_DSS_CHANNEL_LCD2 */
+                REG_FLD_MOD(DISPC_CONFIG2, enable, 18, 18);
         enable_clocks(0);
 }
 bool dispc_alpha_blending_enabled(enum omap_channel ch)
 {
         bool enabled;
 
-        if (cpu_is_omap24xx())
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA))
                 return false;
 
         enable_clocks(1);
         if (ch == OMAP_DSS_CHANNEL_LCD)
                 enabled = REG_GET(DISPC_CONFIG, 18, 18);
         else if (ch == OMAP_DSS_CHANNEL_DIGIT)
-                enabled = REG_GET(DISPC_CONFIG, 18, 18);
+                enabled = REG_GET(DISPC_CONFIG, 19, 19);
+        else if (ch == OMAP_DSS_CHANNEL_LCD2)
+                enabled = REG_GET(DISPC_CONFIG2, 18, 18);
         else
                 BUG();
         enable_clocks(0);
 
         return enabled;
-
 }
 
 
@@ -2042,6 +2253,8 @@ bool dispc_trans_key_enabled(enum omap_channel ch)
                 enabled = REG_GET(DISPC_CONFIG, 10, 10);
         else if (ch == OMAP_DSS_CHANNEL_DIGIT)
                 enabled = REG_GET(DISPC_CONFIG, 12, 12);
+        else if (ch == OMAP_DSS_CHANNEL_LCD2)
+                enabled = REG_GET(DISPC_CONFIG2, 10, 10);
         else
                 BUG();
         enable_clocks(0);
@@ -2050,7 +2263,7 @@ bool dispc_trans_key_enabled(enum omap_channel ch)
 }
 
 
-void dispc_set_tft_data_lines(u8 data_lines)
+void dispc_set_tft_data_lines(enum omap_channel channel, u8 data_lines)
 {
         int code;
 
@@ -2073,11 +2286,15 @@ void dispc_set_tft_data_lines(u8 data_lines)
         }
 
         enable_clocks(1);
-        REG_FLD_MOD(DISPC_CONTROL, code, 9, 8);
+        if (channel == OMAP_DSS_CHANNEL_LCD2)
+                REG_FLD_MOD(DISPC_CONTROL2, code, 9, 8);
+        else
+                REG_FLD_MOD(DISPC_CONTROL, code, 9, 8);
         enable_clocks(0);
 }
 
-void dispc_set_parallel_interface_mode(enum omap_parallel_interface_mode mode)
+void dispc_set_parallel_interface_mode(enum omap_channel channel,
+                enum omap_parallel_interface_mode mode)
 {
         u32 l;
         int stallmode;
@@ -2107,13 +2324,17 @@ void dispc_set_parallel_interface_mode(enum omap_parallel_interface_mode mode)
 
         enable_clocks(1);
 
-        l = dispc_read_reg(DISPC_CONTROL);
-
-        l = FLD_MOD(l, stallmode, 11, 11);
-        l = FLD_MOD(l, gpout0, 15, 15);
-        l = FLD_MOD(l, gpout1, 16, 16);
-
-        dispc_write_reg(DISPC_CONTROL, l);
+        if (channel == OMAP_DSS_CHANNEL_LCD2) {
+                l = dispc_read_reg(DISPC_CONTROL2);
+                l = FLD_MOD(l, stallmode, 11, 11);
+                dispc_write_reg(DISPC_CONTROL2, l);
+        } else {
+                l = dispc_read_reg(DISPC_CONTROL);
+                l = FLD_MOD(l, stallmode, 11, 11);
+                l = FLD_MOD(l, gpout0, 15, 15);
+                l = FLD_MOD(l, gpout1, 16, 16);
+                dispc_write_reg(DISPC_CONTROL, l);
+        }
 
         enable_clocks(0);
 }
@@ -2149,8 +2370,8 @@ bool dispc_lcd_timings_ok(struct omap_video_timings *timings)
                         timings->vfp, timings->vbp);
 }
 
-static void _dispc_set_lcd_timings(int hsw, int hfp, int hbp,
-                                   int vsw, int vfp, int vbp)
+static void _dispc_set_lcd_timings(enum omap_channel channel, int hsw,
+                int hfp, int hbp, int vsw, int vfp, int vbp)
 {
         u32 timing_h, timing_v;
 
@@ -2169,13 +2390,14 @@ static void _dispc_set_lcd_timings(int hsw, int hfp, int hbp,
         }
 
         enable_clocks(1);
-        dispc_write_reg(DISPC_TIMING_H, timing_h);
-        dispc_write_reg(DISPC_TIMING_V, timing_v);
+        dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
+        dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
         enable_clocks(0);
 }
 
 /* change name to mode? */
-void dispc_set_lcd_timings(struct omap_video_timings *timings)
+void dispc_set_lcd_timings(enum omap_channel channel,
+                struct omap_video_timings *timings)
 {
         unsigned xtot, ytot;
         unsigned long ht, vt;
@@ -2185,10 +2407,11 @@ void dispc_set_lcd_timings(struct omap_video_timings *timings)
                                 timings->vfp, timings->vbp))
                 BUG();
 
-        _dispc_set_lcd_timings(timings->hsw, timings->hfp, timings->hbp,
-                        timings->vsw, timings->vfp, timings->vbp);
+        _dispc_set_lcd_timings(channel, timings->hsw, timings->hfp,
+                        timings->hbp, timings->vsw, timings->vfp,
+                        timings->vbp);
 
-        dispc_set_lcd_size(timings->x_res, timings->y_res);
+        dispc_set_lcd_size(channel, timings->x_res, timings->y_res);
 
         xtot = timings->x_res + timings->hfp + timings->hsw + timings->hbp;
         ytot = timings->y_res + timings->vfp + timings->vsw + timings->vbp;
@@ -2196,7 +2419,8 @@ void dispc_set_lcd_timings(struct omap_video_timings *timings)
         ht = (timings->pixel_clock * 1000) / xtot;
         vt = (timings->pixel_clock * 1000) / xtot / ytot;
 
-        DSSDBG("xres %u yres %u\n", timings->x_res, timings->y_res);
+        DSSDBG("channel %d xres %u yres %u\n", channel, timings->x_res,
+                        timings->y_res);
         DSSDBG("pck %u\n", timings->pixel_clock);
         DSSDBG("hsw %d hfp %d hbp %d vsw %d vfp %d vbp %d\n",
                         timings->hsw, timings->hfp, timings->hbp,
@@ -2205,89 +2429,151 @@ void dispc_set_lcd_timings(struct omap_video_timings *timings)
         DSSDBG("hsync %luHz, vsync %luHz\n", ht, vt);
 }
 
-static void dispc_set_lcd_divisor(u16 lck_div, u16 pck_div)
+static void dispc_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
+                u16 pck_div)
 {
         BUG_ON(lck_div < 1);
         BUG_ON(pck_div < 2);
 
         enable_clocks(1);
-        dispc_write_reg(DISPC_DIVISOR,
+        dispc_write_reg(DISPC_DIVISORo(channel),
                         FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
         enable_clocks(0);
 }
 
-static void dispc_get_lcd_divisor(int *lck_div, int *pck_div)
+static void dispc_get_lcd_divisor(enum omap_channel channel, int *lck_div,
+                int *pck_div)
 {
         u32 l;
-        l = dispc_read_reg(DISPC_DIVISOR);
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
         *lck_div = FLD_GET(l, 23, 16);
         *pck_div = FLD_GET(l, 7, 0);
 }
 
 unsigned long dispc_fclk_rate(void)
 {
+        struct platform_device *dsidev;
         unsigned long r = 0;
 
-        if (dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK)
-                r = dss_clk_get_rate(DSS_CLK_FCK1);
-        else
-#ifdef CONFIG_OMAP2_DSS_DSI
-                r = dsi_get_dsi1_pll_rate();
-#else
-        BUG();
-#endif
+        switch (dss_get_dispc_clk_source()) {
+        case OMAP_DSS_CLK_SRC_FCK:
+                r = dss_clk_get_rate(DSS_CLK_FCK);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                dsidev = dsi_get_dsidev_from_id(0);
+                r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                dsidev = dsi_get_dsidev_from_id(1);
+                r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+                break;
+        default:
+                BUG();
+        }
+
         return r;
 }
 
-unsigned long dispc_lclk_rate(void)
+unsigned long dispc_lclk_rate(enum omap_channel channel)
 {
+        struct platform_device *dsidev;
         int lcd;
         unsigned long r;
         u32 l;
 
-        l = dispc_read_reg(DISPC_DIVISOR);
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
 
         lcd = FLD_GET(l, 23, 16);
 
-        r = dispc_fclk_rate();
+        switch (dss_get_lcd_clk_source(channel)) {
+        case OMAP_DSS_CLK_SRC_FCK:
+                r = dss_clk_get_rate(DSS_CLK_FCK);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                dsidev = dsi_get_dsidev_from_id(0);
+                r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                dsidev = dsi_get_dsidev_from_id(1);
+                r = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+                break;
+        default:
+                BUG();
+        }
 
         return r / lcd;
 }
 
-unsigned long dispc_pclk_rate(void)
+unsigned long dispc_pclk_rate(enum omap_channel channel)
 {
-        int lcd, pcd;
+        int pcd;
         unsigned long r;
         u32 l;
 
-        l = dispc_read_reg(DISPC_DIVISOR);
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
 
-        lcd = FLD_GET(l, 23, 16);
         pcd = FLD_GET(l, 7, 0);
 
-        r = dispc_fclk_rate();
+        r = dispc_lclk_rate(channel);
 
-        return r / lcd / pcd;
+        return r / pcd;
 }
 
 void dispc_dump_clocks(struct seq_file *s)
 {
         int lcd, pcd;
+        u32 l;
+        enum omap_dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
+        enum omap_dss_clk_source lcd_clk_src;
 
         enable_clocks(1);
 
-        dispc_get_lcd_divisor(&lcd, &pcd);
-
         seq_printf(s, "- DISPC -\n");
 
-        seq_printf(s, "dispc fclk source = %s\n",
-                        dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-                        "dss1_alwon_fclk" : "dsi1_pll_fclk");
+        seq_printf(s, "dispc fclk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(dispc_clk_src),
+                        dss_feat_get_clk_source_name(dispc_clk_src));
 
         seq_printf(s, "fck\t\t%-16lu\n", dispc_fclk_rate());
-        seq_printf(s, "lck\t\t%-16lulck div\t%u\n", dispc_lclk_rate(), lcd);
-        seq_printf(s, "pck\t\t%-16lupck div\t%u\n", dispc_pclk_rate(), pcd);
 
+        if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+                seq_printf(s, "- DISPC-CORE-CLK -\n");
+                l = dispc_read_reg(DISPC_DIVISOR);
+                lcd = FLD_GET(l, 23, 16);
+
+                seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+                                (dispc_fclk_rate()/lcd), lcd);
+        }
+        seq_printf(s, "- LCD1 -\n");
+
+        lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD);
+
+        seq_printf(s, "lcd1_clk source = %s (%s)\n",
+                dss_get_generic_clk_source_name(lcd_clk_src),
+                dss_feat_get_clk_source_name(lcd_clk_src));
+
+        dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD, &lcd, &pcd);
+
+        seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+                        dispc_lclk_rate(OMAP_DSS_CHANNEL_LCD), lcd);
+        seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
+                        dispc_pclk_rate(OMAP_DSS_CHANNEL_LCD), pcd);
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                seq_printf(s, "- LCD2 -\n");
+
+                lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD2);
+
+                seq_printf(s, "lcd2_clk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(lcd_clk_src),
+                        dss_feat_get_clk_source_name(lcd_clk_src));
+
+                dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD2, &lcd, &pcd);
+
+                seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+                                dispc_lclk_rate(OMAP_DSS_CHANNEL_LCD2), lcd);
+                seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
+                                dispc_pclk_rate(OMAP_DSS_CHANNEL_LCD2), pcd);
+        }
         enable_clocks(0);
 }
 
@@ -2329,15 +2615,21 @@ void dispc_dump_irqs(struct seq_file *s)
         PIS(SYNC_LOST);
         PIS(SYNC_LOST_DIGIT);
         PIS(WAKEUP);
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                PIS(FRAMEDONE2);
+                PIS(VSYNC2);
+                PIS(ACBIAS_COUNT_STAT2);
+                PIS(SYNC_LOST2);
+        }
 #undef PIS
 }
 #endif
 
 void dispc_dump_regs(struct seq_file *s)
 {
-#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dispc_read_reg(r))
+#define DUMPREG(r) seq_printf(s, "%-50s %08x\n", #r, dispc_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(DISPC_REVISION);
         DUMPREG(DISPC_SYSCONFIG);
@@ -2347,139 +2639,234 @@ void dispc_dump_regs(struct seq_file *s)
         DUMPREG(DISPC_CONTROL);
         DUMPREG(DISPC_CONFIG);
         DUMPREG(DISPC_CAPABLE);
-        DUMPREG(DISPC_DEFAULT_COLOR0);
-        DUMPREG(DISPC_DEFAULT_COLOR1);
-        DUMPREG(DISPC_TRANS_COLOR0);
-        DUMPREG(DISPC_TRANS_COLOR1);
+        DUMPREG(DISPC_DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_DEFAULT_COLOR(OMAP_DSS_CHANNEL_DIGIT));
+        DUMPREG(DISPC_TRANS_COLOR(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_TRANS_COLOR(OMAP_DSS_CHANNEL_DIGIT));
         DUMPREG(DISPC_LINE_STATUS);
         DUMPREG(DISPC_LINE_NUMBER);
-        DUMPREG(DISPC_TIMING_H);
-        DUMPREG(DISPC_TIMING_V);
-        DUMPREG(DISPC_POL_FREQ);
-        DUMPREG(DISPC_DIVISOR);
+        DUMPREG(DISPC_TIMING_H(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_TIMING_V(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_POL_FREQ(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_DIVISORo(OMAP_DSS_CHANNEL_LCD));
         DUMPREG(DISPC_GLOBAL_ALPHA);
-        DUMPREG(DISPC_SIZE_DIG);
-        DUMPREG(DISPC_SIZE_LCD);
-
-        DUMPREG(DISPC_GFX_BA0);
-        DUMPREG(DISPC_GFX_BA1);
-        DUMPREG(DISPC_GFX_POSITION);
-        DUMPREG(DISPC_GFX_SIZE);
-        DUMPREG(DISPC_GFX_ATTRIBUTES);
-        DUMPREG(DISPC_GFX_FIFO_THRESHOLD);
-        DUMPREG(DISPC_GFX_FIFO_SIZE_STATUS);
-        DUMPREG(DISPC_GFX_ROW_INC);
-        DUMPREG(DISPC_GFX_PIXEL_INC);
-        DUMPREG(DISPC_GFX_WINDOW_SKIP);
-        DUMPREG(DISPC_GFX_TABLE_BA);
-
-        DUMPREG(DISPC_DATA_CYCLE1);
-        DUMPREG(DISPC_DATA_CYCLE2);
-        DUMPREG(DISPC_DATA_CYCLE3);
-
-        DUMPREG(DISPC_CPR_COEF_R);
-        DUMPREG(DISPC_CPR_COEF_G);
-        DUMPREG(DISPC_CPR_COEF_B);
-
-        DUMPREG(DISPC_GFX_PRELOAD);
-
-        DUMPREG(DISPC_VID_BA0(0));
-        DUMPREG(DISPC_VID_BA1(0));
-        DUMPREG(DISPC_VID_POSITION(0));
-        DUMPREG(DISPC_VID_SIZE(0));
-        DUMPREG(DISPC_VID_ATTRIBUTES(0));
-        DUMPREG(DISPC_VID_FIFO_THRESHOLD(0));
-        DUMPREG(DISPC_VID_FIFO_SIZE_STATUS(0));
-        DUMPREG(DISPC_VID_ROW_INC(0));
-        DUMPREG(DISPC_VID_PIXEL_INC(0));
-        DUMPREG(DISPC_VID_FIR(0));
-        DUMPREG(DISPC_VID_PICTURE_SIZE(0));
-        DUMPREG(DISPC_VID_ACCU0(0));
-        DUMPREG(DISPC_VID_ACCU1(0));
-
-        DUMPREG(DISPC_VID_BA0(1));
-        DUMPREG(DISPC_VID_BA1(1));
-        DUMPREG(DISPC_VID_POSITION(1));
-        DUMPREG(DISPC_VID_SIZE(1));
-        DUMPREG(DISPC_VID_ATTRIBUTES(1));
-        DUMPREG(DISPC_VID_FIFO_THRESHOLD(1));
-        DUMPREG(DISPC_VID_FIFO_SIZE_STATUS(1));
-        DUMPREG(DISPC_VID_ROW_INC(1));
-        DUMPREG(DISPC_VID_PIXEL_INC(1));
-        DUMPREG(DISPC_VID_FIR(1));
-        DUMPREG(DISPC_VID_PICTURE_SIZE(1));
-        DUMPREG(DISPC_VID_ACCU0(1));
-        DUMPREG(DISPC_VID_ACCU1(1));
-
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 0));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 1));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 2));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 3));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 5));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 6));
-        DUMPREG(DISPC_VID_FIR_COEF_H(0, 7));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 0));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 1));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 2));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 3));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 5));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 6));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(0, 7));
-        DUMPREG(DISPC_VID_CONV_COEF(0, 0));
-        DUMPREG(DISPC_VID_CONV_COEF(0, 1));
-        DUMPREG(DISPC_VID_CONV_COEF(0, 2));
-        DUMPREG(DISPC_VID_CONV_COEF(0, 3));
-        DUMPREG(DISPC_VID_CONV_COEF(0, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 0));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 1));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 2));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 3));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 5));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 6));
-        DUMPREG(DISPC_VID_FIR_COEF_V(0, 7));
-
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 0));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 1));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 2));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 3));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 5));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 6));
-        DUMPREG(DISPC_VID_FIR_COEF_H(1, 7));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 0));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 1));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 2));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 3));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 5));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 6));
-        DUMPREG(DISPC_VID_FIR_COEF_HV(1, 7));
-        DUMPREG(DISPC_VID_CONV_COEF(1, 0));
-        DUMPREG(DISPC_VID_CONV_COEF(1, 1));
-        DUMPREG(DISPC_VID_CONV_COEF(1, 2));
-        DUMPREG(DISPC_VID_CONV_COEF(1, 3));
-        DUMPREG(DISPC_VID_CONV_COEF(1, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 0));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 1));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 2));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 3));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 4));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 5));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 6));
-        DUMPREG(DISPC_VID_FIR_COEF_V(1, 7));
-
-        DUMPREG(DISPC_VID_PRELOAD(0));
-        DUMPREG(DISPC_VID_PRELOAD(1));
-
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        DUMPREG(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_DIGIT));
+        DUMPREG(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_LCD));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                DUMPREG(DISPC_CONTROL2);
+                DUMPREG(DISPC_CONFIG2);
+                DUMPREG(DISPC_DEFAULT_COLOR(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_TRANS_COLOR(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_TIMING_H(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_TIMING_V(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_POL_FREQ(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_DIVISORo(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_SIZE_MGR(OMAP_DSS_CHANNEL_LCD2));
+        }
+
+        DUMPREG(DISPC_OVL_BA0(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_BA1(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_POSITION(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_SIZE(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_ATTRIBUTES(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_FIFO_THRESHOLD(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_FIFO_SIZE_STATUS(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_ROW_INC(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_PIXEL_INC(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_WINDOW_SKIP(OMAP_DSS_GFX));
+        DUMPREG(DISPC_OVL_TABLE_BA(OMAP_DSS_GFX));
+
+        DUMPREG(DISPC_DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD));
+
+        DUMPREG(DISPC_CPR_COEF_R(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_CPR_COEF_G(OMAP_DSS_CHANNEL_LCD));
+        DUMPREG(DISPC_CPR_COEF_B(OMAP_DSS_CHANNEL_LCD));
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                DUMPREG(DISPC_DATA_CYCLE1(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_DATA_CYCLE2(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_DATA_CYCLE3(OMAP_DSS_CHANNEL_LCD2));
+
+                DUMPREG(DISPC_CPR_COEF_R(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_CPR_COEF_G(OMAP_DSS_CHANNEL_LCD2));
+                DUMPREG(DISPC_CPR_COEF_B(OMAP_DSS_CHANNEL_LCD2));
+        }
+
+        DUMPREG(DISPC_OVL_PRELOAD(OMAP_DSS_GFX));
+
+        DUMPREG(DISPC_OVL_BA0(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_BA1(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_POSITION(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_SIZE(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_FIFO_SIZE_STATUS(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_ROW_INC(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_PIXEL_INC(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_FIR(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_PICTURE_SIZE(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_ACCU0(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_ACCU1(OMAP_DSS_VIDEO1));
+
+        DUMPREG(DISPC_OVL_BA0(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_BA1(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_POSITION(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_SIZE(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_ATTRIBUTES(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_FIFO_THRESHOLD(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_FIFO_SIZE_STATUS(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_ROW_INC(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_PIXEL_INC(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_FIR(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_PICTURE_SIZE(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_ACCU0(OMAP_DSS_VIDEO2));
+        DUMPREG(DISPC_OVL_ACCU1(OMAP_DSS_VIDEO2));
+
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 0));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 1));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 2));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 3));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 5));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 6));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO1, 7));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 0));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 1));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 2));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 3));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 5));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 6));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO1, 7));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 0));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 1));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 2));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 3));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO1, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 0));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 1));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 2));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 3));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 5));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 6));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO1, 7));
+
+        if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                DUMPREG(DISPC_OVL_BA0_UV(OMAP_DSS_VIDEO1));
+                DUMPREG(DISPC_OVL_BA1_UV(OMAP_DSS_VIDEO1));
+                DUMPREG(DISPC_OVL_FIR2(OMAP_DSS_VIDEO1));
+                DUMPREG(DISPC_OVL_ACCU2_0(OMAP_DSS_VIDEO1));
+                DUMPREG(DISPC_OVL_ACCU2_1(OMAP_DSS_VIDEO1));
+
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 0));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 1));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 2));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 3));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 4));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 5));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 6));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO1, 7));
+
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 0));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 1));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 2));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 3));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 4));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 5));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 6));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO1, 7));
+
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 0));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 1));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 2));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 3));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 4));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 5));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 6));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO1, 7));
+        }
+        if (dss_has_feature(FEAT_ATTR2))
+                DUMPREG(DISPC_OVL_ATTRIBUTES2(OMAP_DSS_VIDEO1));
+
+
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 0));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 1));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 2));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 3));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 5));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 6));
+        DUMPREG(DISPC_OVL_FIR_COEF_H(OMAP_DSS_VIDEO2, 7));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 0));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 1));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 2));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 3));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 5));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 6));
+        DUMPREG(DISPC_OVL_FIR_COEF_HV(OMAP_DSS_VIDEO2, 7));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 0));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 1));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 2));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 3));
+        DUMPREG(DISPC_OVL_CONV_COEF(OMAP_DSS_VIDEO2, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 0));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 1));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 2));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 3));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 4));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 5));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 6));
+        DUMPREG(DISPC_OVL_FIR_COEF_V(OMAP_DSS_VIDEO2, 7));
+
+        if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                DUMPREG(DISPC_OVL_BA0_UV(OMAP_DSS_VIDEO2));
+                DUMPREG(DISPC_OVL_BA1_UV(OMAP_DSS_VIDEO2));
+                DUMPREG(DISPC_OVL_FIR2(OMAP_DSS_VIDEO2));
+                DUMPREG(DISPC_OVL_ACCU2_0(OMAP_DSS_VIDEO2));
+                DUMPREG(DISPC_OVL_ACCU2_1(OMAP_DSS_VIDEO2));
+
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 0));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 1));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 2));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 3));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 4));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 5));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 6));
+                DUMPREG(DISPC_OVL_FIR_COEF_H2(OMAP_DSS_VIDEO2, 7));
+
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 0));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 1));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 2));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 3));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 4));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 5));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 6));
+                DUMPREG(DISPC_OVL_FIR_COEF_HV2(OMAP_DSS_VIDEO2, 7));
+
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 0));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 1));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 2));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 3));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 4));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 5));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 6));
+                DUMPREG(DISPC_OVL_FIR_COEF_V2(OMAP_DSS_VIDEO2, 7));
+        }
+        if (dss_has_feature(FEAT_ATTR2))
+                DUMPREG(DISPC_OVL_ATTRIBUTES2(OMAP_DSS_VIDEO2));
+
+        DUMPREG(DISPC_OVL_PRELOAD(OMAP_DSS_VIDEO1));
+        DUMPREG(DISPC_OVL_PRELOAD(OMAP_DSS_VIDEO2));
+
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
-static void _dispc_set_pol_freq(bool onoff, bool rf, bool ieo, bool ipc,
-                                bool ihs, bool ivs, u8 acbi, u8 acb)
+static void _dispc_set_pol_freq(enum omap_channel channel, bool onoff, bool rf,
+                bool ieo, bool ipc, bool ihs, bool ivs, u8 acbi, u8 acb)
 {
         u32 l = 0;
 
@@ -2496,13 +2883,14 @@ static void _dispc_set_pol_freq(bool onoff, bool rf, bool ieo, bool ipc,
         l |= FLD_VAL(acb, 7, 0);
 
         enable_clocks(1);
-        dispc_write_reg(DISPC_POL_FREQ, l);
+        dispc_write_reg(DISPC_POL_FREQ(channel), l);
         enable_clocks(0);
 }
 
-void dispc_set_pol_freq(enum omap_panel_config config, u8 acbi, u8 acb)
+void dispc_set_pol_freq(enum omap_channel channel,
+                enum omap_panel_config config, u8 acbi, u8 acb)
 {
-        _dispc_set_pol_freq((config & OMAP_DSS_LCD_ONOFF) != 0,
+        _dispc_set_pol_freq(channel, (config & OMAP_DSS_LCD_ONOFF) != 0,
                         (config & OMAP_DSS_LCD_RF) != 0,
                         (config & OMAP_DSS_LCD_IEO) != 0,
                         (config & OMAP_DSS_LCD_IPC) != 0,
@@ -2571,24 +2959,26 @@ int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
         return 0;
 }
 
-int dispc_set_clock_div(struct dispc_clock_info *cinfo)
+int dispc_set_clock_div(enum omap_channel channel,
+                struct dispc_clock_info *cinfo)
 {
         DSSDBG("lck = %lu (%u)\n", cinfo->lck, cinfo->lck_div);
         DSSDBG("pck = %lu (%u)\n", cinfo->pck, cinfo->pck_div);
 
-        dispc_set_lcd_divisor(cinfo->lck_div, cinfo->pck_div);
+        dispc_set_lcd_divisor(channel, cinfo->lck_div, cinfo->pck_div);
 
         return 0;
 }
 
-int dispc_get_clock_div(struct dispc_clock_info *cinfo)
+int dispc_get_clock_div(enum omap_channel channel,
+                struct dispc_clock_info *cinfo)
 {
         unsigned long fck;
 
         fck = dispc_fclk_rate();
 
-        cinfo->lck_div = REG_GET(DISPC_DIVISOR, 23, 16);
-        cinfo->pck_div = REG_GET(DISPC_DIVISOR, 7, 0);
+        cinfo->lck_div = REG_GET(DISPC_DIVISORo(channel), 23, 16);
+        cinfo->pck_div = REG_GET(DISPC_DIVISORo(channel), 7, 0);
 
         cinfo->lck = fck / cinfo->lck_div;
         cinfo->pck = cinfo->lck / cinfo->pck_div;
@@ -2665,6 +3055,9 @@ int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
                 break;
         }
 
+        if (ret)
+                goto err;
+
         _omap_dispc_set_irqs();
 
         spin_unlock_irqrestore(&dispc.irq_lock, flags);
@@ -2728,6 +3121,8 @@ static void print_irq_status(u32 status)
         PIS(VID2_FIFO_UNDERFLOW);
         PIS(SYNC_LOST);
         PIS(SYNC_LOST_DIGIT);
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                PIS(SYNC_LOST2);
 #undef PIS
 
         printk("\n");
@@ -2738,10 +3133,10 @@ static void print_irq_status(u32 status)
  * but we presume they are on because we got an IRQ. However,
  * an irq handler may turn the clocks off, so we may not have
  * clock later in the function. */
-void dispc_irq_handler(void)
+static irqreturn_t omap_dispc_irq_handler(int irq, void *arg)
 {
         int i;
-        u32 irqstatus;
+        u32 irqstatus, irqenable;
         u32 handledirqs = 0;
         u32 unhandled_errors;
         struct omap_dispc_isr_data *isr_data;
@@ -2750,6 +3145,13 @@ void dispc_irq_handler(void)
         spin_lock(&dispc.irq_lock);
 
         irqstatus = dispc_read_reg(DISPC_IRQSTATUS);
+        irqenable = dispc_read_reg(DISPC_IRQENABLE);
+
+        /* IRQ is not for us */
+        if (!(irqstatus & irqenable)) {
+                spin_unlock(&dispc.irq_lock);
+                return IRQ_NONE;
+        }
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
         spin_lock(&dispc.irq_stats_lock);
@@ -2801,6 +3203,8 @@ void dispc_irq_handler(void)
         }
 
         spin_unlock(&dispc.irq_lock);
+
+        return IRQ_HANDLED;
 }
 
 static void dispc_error_worker(struct work_struct *work)
@@ -2946,6 +3350,45 @@ static void dispc_error_worker(struct work_struct *work)
                 }
         }
 
+        if (errors & DISPC_IRQ_SYNC_LOST2) {
+                struct omap_overlay_manager *manager = NULL;
+                bool enable = false;
+
+                DSSERR("SYNC_LOST for LCD2, disabling LCD2\n");
+
+                for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+                        struct omap_overlay_manager *mgr;
+                        mgr = omap_dss_get_overlay_manager(i);
+
+                        if (mgr->id == OMAP_DSS_CHANNEL_LCD2) {
+                                manager = mgr;
+                                enable = mgr->device->state ==
+                                                OMAP_DSS_DISPLAY_ACTIVE;
+                                mgr->device->driver->disable(mgr->device);
+                                break;
+                        }
+                }
+
+                if (manager) {
+                        struct omap_dss_device *dssdev = manager->device;
+                        for (i = 0; i < omap_dss_get_num_overlays(); ++i) {
+                                struct omap_overlay *ovl;
+                                ovl = omap_dss_get_overlay(i);
+
+                                if (!(ovl->caps & OMAP_DSS_OVL_CAP_DISPC))
+                                        continue;
+
+                                if (ovl->id != 0 && ovl->manager == manager)
+                                        dispc_enable_plane(ovl->id, 0);
+                        }
+
+                        dispc_go(manager->id);
+                        mdelay(50);
+                        if (enable)
+                                dssdev->driver->enable(dssdev);
+                }
+        }
+
         if (errors & DISPC_IRQ_OCP_ERR) {
                 DSSERR("OCP_ERR\n");
                 for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
@@ -3053,6 +3496,8 @@ static void _omap_dispc_initialize_irq(void)
         memset(dispc.registered_isr, 0, sizeof(dispc.registered_isr));
 
         dispc.irq_error_mask = DISPC_IRQ_MASK_ERROR;
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                dispc.irq_error_mask |= DISPC_IRQ_SYNC_LOST2;
 
         /* there's SYNC_LOST_DIGIT waiting after enabling the DSS,
          * so clear it */
@@ -3084,8 +3529,18 @@ static void _omap_dispc_initial_config(void)
         l = FLD_MOD(l, 1, 0, 0);        /* AUTOIDLE */
         dispc_write_reg(DISPC_SYSCONFIG, l);
 
+        /* Exclusively enable DISPC_CORE_CLK and set divider to 1 */
+        if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+                l = dispc_read_reg(DISPC_DIVISOR);
+                /* Use DISPC_DIVISOR.LCD, instead of DISPC_DIVISOR1.LCD */
+                l = FLD_MOD(l, 1, 0, 0);
+                l = FLD_MOD(l, 1, 23, 16);
+                dispc_write_reg(DISPC_DIVISOR, l);
+        }
+
         /* FUNCGATED */
-        REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
+        if (dss_has_feature(FEAT_FUNCGATED))
+                REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
 
         /* L3 firewall setting: enable access to OCM RAM */
         /* XXX this should be somewhere in plat-omap */
@@ -3099,47 +3554,6 @@ static void _omap_dispc_initial_config(void)
         dispc_read_plane_fifo_sizes();
 }
 
-int dispc_init(void)
-{
-        u32 rev;
-
-        spin_lock_init(&dispc.irq_lock);
-
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-        spin_lock_init(&dispc.irq_stats_lock);
-        dispc.irq_stats.last_reset = jiffies;
-#endif
-
-        INIT_WORK(&dispc.error_work, dispc_error_worker);
-
-        dispc.base = ioremap(DISPC_BASE, DISPC_SZ_REGS);
-        if (!dispc.base) {
-                DSSERR("can't ioremap DISPC\n");
-                return -ENOMEM;
-        }
-
-        enable_clocks(1);
-
-        _omap_dispc_initial_config();
-
-        _omap_dispc_initialize_irq();
-
-        dispc_save_context();
-
-        rev = dispc_read_reg(DISPC_REVISION);
-        printk(KERN_INFO "OMAP DISPC rev %d.%d\n",
-               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-        enable_clocks(0);
-
-        return 0;
-}
-
-void dispc_exit(void)
-{
-        iounmap(dispc.base);
-}
-
 int dispc_enable_plane(enum omap_plane plane, bool enable)
 {
         DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
@@ -3159,17 +3573,19 @@ int dispc_setup_plane(enum omap_plane plane,
                        enum omap_color_mode color_mode,
                        bool ilace,
                        enum omap_dss_rotation_type rotation_type,
-                       u8 rotation, bool mirror, u8 global_alpha)
+                       u8 rotation, bool mirror, u8 global_alpha,
+                       u8 pre_mult_alpha, enum omap_channel channel,
+                       u32 puv_addr)
 {
         int r = 0;
 
-        DSSDBG("dispc_setup_plane %d, pa %x, sw %d, %d,%d, %dx%d -> "
-               "%dx%d, ilace %d, cmode %x, rot %d, mir %d\n",
+        DSSDBG("dispc_setup_plane %d, pa %x, sw %d, %d, %d, %dx%d -> "
+               "%dx%d, ilace %d, cmode %x, rot %d, mir %d chan %d\n",
                plane, paddr, screen_width, pos_x, pos_y,
                width, height,
                out_width, out_height,
                ilace, color_mode,
-               rotation, mirror);
+               rotation, mirror, channel);
 
         enable_clocks(1);
 
@@ -3181,9 +3597,102 @@ int dispc_setup_plane(enum omap_plane plane,
                            color_mode, ilace,
                            rotation_type,
                            rotation, mirror,
-                           global_alpha);
+                           global_alpha,
+                           pre_mult_alpha,
+                           channel, puv_addr);
 
         enable_clocks(0);
 
         return r;
 }
+
+/* DISPC HW IP initialisation */
+static int omap_dispchw_probe(struct platform_device *pdev)
+{
+        u32 rev;
+        int r = 0;
+        struct resource *dispc_mem;
+
+        dispc.pdev = pdev;
+
+        spin_lock_init(&dispc.irq_lock);
+
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+        spin_lock_init(&dispc.irq_stats_lock);
+        dispc.irq_stats.last_reset = jiffies;
+#endif
+
+        INIT_WORK(&dispc.error_work, dispc_error_worker);
+
+        dispc_mem = platform_get_resource(dispc.pdev, IORESOURCE_MEM, 0);
+        if (!dispc_mem) {
+                DSSERR("can't get IORESOURCE_MEM DISPC\n");
+                r = -EINVAL;
+                goto fail0;
+        }
+        dispc.base = ioremap(dispc_mem->start, resource_size(dispc_mem));
+        if (!dispc.base) {
+                DSSERR("can't ioremap DISPC\n");
+                r = -ENOMEM;
+                goto fail0;
+        }
+        dispc.irq = platform_get_irq(dispc.pdev, 0);
+        if (dispc.irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                r = -ENODEV;
+                goto fail1;
+        }
+
+        r = request_irq(dispc.irq, omap_dispc_irq_handler, IRQF_SHARED,
+                "OMAP DISPC", dispc.pdev);
+        if (r < 0) {
+                DSSERR("request_irq failed\n");
+                goto fail1;
+        }
+
+        enable_clocks(1);
+
+        _omap_dispc_initial_config();
+
+        _omap_dispc_initialize_irq();
+
+        dispc_save_context();
+
+        rev = dispc_read_reg(DISPC_REVISION);
+        dev_dbg(&pdev->dev, "OMAP DISPC rev %d.%d\n",
+               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+        enable_clocks(0);
+
+        return 0;
+fail1:
+        iounmap(dispc.base);
+fail0:
+        return r;
+}
+
+static int omap_dispchw_remove(struct platform_device *pdev)
+{
+        free_irq(dispc.irq, dispc.pdev);
+        iounmap(dispc.base);
+        return 0;
+}
+
+static struct platform_driver omap_dispchw_driver = {
+        .probe          = omap_dispchw_probe,
+        .remove         = omap_dispchw_remove,
+        .driver         = {
+                .name   = "omapdss_dispc",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int dispc_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dispchw_driver);
+}
+
+void dispc_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_dispchw_driver);
+}
diff --git a/drivers/video/omap2/dss/dispc.h b/drivers/video/omap2/dss/dispc.h
new file mode 100644
index 000000000000..6c9ee0a0efb3
--- /dev/null
+++ b/drivers/video/omap2/dss/dispc.h
@@ -0,0 +1,691 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc.h
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Archit Taneja <archit@ti.com>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OMAP2_DISPC_REG_H
+#define __OMAP2_DISPC_REG_H
+
+/* DISPC common registers */
+#define DISPC_REVISION                  0x0000
+#define DISPC_SYSCONFIG                 0x0010
+#define DISPC_SYSSTATUS                 0x0014
+#define DISPC_IRQSTATUS                 0x0018
+#define DISPC_IRQENABLE                 0x001C
+#define DISPC_CONTROL                   0x0040
+#define DISPC_CONFIG                    0x0044
+#define DISPC_CAPABLE                   0x0048
+#define DISPC_LINE_STATUS               0x005C
+#define DISPC_LINE_NUMBER               0x0060
+#define DISPC_GLOBAL_ALPHA              0x0074
+#define DISPC_CONTROL2                  0x0238
+#define DISPC_CONFIG2                   0x0620
+#define DISPC_DIVISOR                   0x0804
+
+/* DISPC overlay registers */
+#define DISPC_OVL_BA0(n)                (DISPC_OVL_BASE(n) + \
+                                        DISPC_BA0_OFFSET(n))
+#define DISPC_OVL_BA1(n)                (DISPC_OVL_BASE(n) + \
+                                        DISPC_BA1_OFFSET(n))
+#define DISPC_OVL_BA0_UV(n)             (DISPC_OVL_BASE(n) + \
+                                        DISPC_BA0_UV_OFFSET(n))
+#define DISPC_OVL_BA1_UV(n)             (DISPC_OVL_BASE(n) + \
+                                        DISPC_BA1_UV_OFFSET(n))
+#define DISPC_OVL_POSITION(n)           (DISPC_OVL_BASE(n) + \
+                                        DISPC_POS_OFFSET(n))
+#define DISPC_OVL_SIZE(n)               (DISPC_OVL_BASE(n) + \
+                                        DISPC_SIZE_OFFSET(n))
+#define DISPC_OVL_ATTRIBUTES(n)         (DISPC_OVL_BASE(n) + \
+                                        DISPC_ATTR_OFFSET(n))
+#define DISPC_OVL_ATTRIBUTES2(n)        (DISPC_OVL_BASE(n) + \
+                                        DISPC_ATTR2_OFFSET(n))
+#define DISPC_OVL_FIFO_THRESHOLD(n)     (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIFO_THRESH_OFFSET(n))
+#define DISPC_OVL_FIFO_SIZE_STATUS(n)   (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIFO_SIZE_STATUS_OFFSET(n))
+#define DISPC_OVL_ROW_INC(n)            (DISPC_OVL_BASE(n) + \
+                                        DISPC_ROW_INC_OFFSET(n))
+#define DISPC_OVL_PIXEL_INC(n)          (DISPC_OVL_BASE(n) + \
+                                        DISPC_PIX_INC_OFFSET(n))
+#define DISPC_OVL_WINDOW_SKIP(n)        (DISPC_OVL_BASE(n) + \
+                                        DISPC_WINDOW_SKIP_OFFSET(n))
+#define DISPC_OVL_TABLE_BA(n)           (DISPC_OVL_BASE(n) + \
+                                        DISPC_TABLE_BA_OFFSET(n))
+#define DISPC_OVL_FIR(n)                (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_OFFSET(n))
+#define DISPC_OVL_FIR2(n)               (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR2_OFFSET(n))
+#define DISPC_OVL_PICTURE_SIZE(n)       (DISPC_OVL_BASE(n) + \
+                                        DISPC_PIC_SIZE_OFFSET(n))
+#define DISPC_OVL_ACCU0(n)              (DISPC_OVL_BASE(n) + \
+                                        DISPC_ACCU0_OFFSET(n))
+#define DISPC_OVL_ACCU1(n)              (DISPC_OVL_BASE(n) + \
+                                        DISPC_ACCU1_OFFSET(n))
+#define DISPC_OVL_ACCU2_0(n)            (DISPC_OVL_BASE(n) + \
+                                        DISPC_ACCU2_0_OFFSET(n))
+#define DISPC_OVL_ACCU2_1(n)            (DISPC_OVL_BASE(n) + \
+                                        DISPC_ACCU2_1_OFFSET(n))
+#define DISPC_OVL_FIR_COEF_H(n, i)      (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_COEF_H_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_HV(n, i)     (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_COEF_HV_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_H2(n, i)     (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_COEF_H2_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_HV2(n, i)    (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_COEF_HV2_OFFSET(n, i))
+#define DISPC_OVL_CONV_COEF(n, i)       (DISPC_OVL_BASE(n) + \
+                                        DISPC_CONV_COEF_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_V(n, i)      (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_COEF_V_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_V2(n, i)     (DISPC_OVL_BASE(n) + \
+                                        DISPC_FIR_COEF_V2_OFFSET(n, i))
+#define DISPC_OVL_PRELOAD(n)            (DISPC_OVL_BASE(n) + \
+                                        DISPC_PRELOAD_OFFSET(n))
+
+/* DISPC manager/channel specific registers */
+static inline u16 DISPC_DEFAULT_COLOR(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x004C;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                return 0x0050;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03AC;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_TRANS_COLOR(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0054;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                return 0x0058;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03B0;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_TIMING_H(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0064;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x0400;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_TIMING_V(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0068;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x0404;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_POL_FREQ(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x006C;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x0408;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_DIVISORo(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0070;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x040C;
+        default:
+                BUG();
+        }
+}
+
+/* Named as DISPC_SIZE_LCD, DISPC_SIZE_DIGIT and DISPC_SIZE_LCD2 in TRM */
+static inline u16 DISPC_SIZE_MGR(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x007C;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                return 0x0078;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03CC;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_DATA_CYCLE1(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x01D4;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03C0;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_DATA_CYCLE2(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x01D8;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03C4;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_DATA_CYCLE3(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x01DC;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03C8;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_CPR_COEF_R(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0220;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03BC;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_CPR_COEF_G(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0224;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03B8;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_CPR_COEF_B(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0228;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03B4;
+        default:
+                BUG();
+        }
+}
+
+/* DISPC overlay register base addresses */
+static inline u16 DISPC_OVL_BASE(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0080;
+        case OMAP_DSS_VIDEO1:
+                return 0x00BC;
+        case OMAP_DSS_VIDEO2:
+                return 0x014C;
+        default:
+                BUG();
+        }
+}
+
+/* DISPC overlay register offsets */
+static inline u16 DISPC_BA0_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0000;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_BA1_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0004;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_BA0_UV_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0544;
+        case OMAP_DSS_VIDEO2:
+                return 0x04BC;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_BA1_UV_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0548;
+        case OMAP_DSS_VIDEO2:
+                return 0x04C0;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_POS_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0008;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_SIZE_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x000C;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_ATTR_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0020;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0010;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_ATTR2_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0568;
+        case OMAP_DSS_VIDEO2:
+                return 0x04DC;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_FIFO_THRESH_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0024;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0014;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_FIFO_SIZE_STATUS_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0028;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0018;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_ROW_INC_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x002C;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x001C;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_PIX_INC_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0030;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0020;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_WINDOW_SKIP_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0034;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                BUG();
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_TABLE_BA_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0038;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                BUG();
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_FIR_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0024;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_FIR2_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0580;
+        case OMAP_DSS_VIDEO2:
+                return 0x055C;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_PIC_SIZE_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0028;
+        default:
+                BUG();
+        }
+}
+
+
+static inline u16 DISPC_ACCU0_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x002C;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_ACCU2_0_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0584;
+        case OMAP_DSS_VIDEO2:
+                return 0x0560;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_ACCU1_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0030;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_ACCU2_1_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0588;
+        case OMAP_DSS_VIDEO2:
+                return 0x0564;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_H_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0034 + i * 0x8;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_H2_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x058C + i * 0x8;
+        case OMAP_DSS_VIDEO2:
+                return 0x0568 + i * 0x8;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_HV_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0038 + i * 0x8;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_HV2_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0590 + i * 8;
+        case OMAP_DSS_VIDEO2:
+                return 0x056C + i * 0x8;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4,} */
+static inline u16 DISPC_CONV_COEF_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0074 + i * 0x4;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_V_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x0124 + i * 0x4;
+        case OMAP_DSS_VIDEO2:
+                return 0x00B4 + i * 0x4;
+        default:
+                BUG();
+        }
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_V2_OFFSET(enum omap_plane plane, u16 i)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+        case OMAP_DSS_VIDEO1:
+                return 0x05CC + i * 0x4;
+        case OMAP_DSS_VIDEO2:
+                return 0x05A8 + i * 0x4;
+        default:
+                BUG();
+        }
+}
+
+static inline u16 DISPC_PRELOAD_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x01AC;
+        case OMAP_DSS_VIDEO1:
+                return 0x0174;
+        case OMAP_DSS_VIDEO2:
+                return 0x00E8;
+        default:
+                BUG();
+        }
+}
+#endif
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 22dd7a474f79..c2dfc8c50057 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -25,14 +25,11 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/jiffies.h>
-#include <linux/list.h>
 #include <linux/platform_device.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include "dss.h"
 
-static LIST_HEAD(display_list);
-
 static ssize_t display_enabled_show(struct device *dev,
                 struct device_attribute *attr, char *buf)
 {
@@ -47,9 +44,13 @@ static ssize_t display_enabled_store(struct device *dev,
                 const char *buf, size_t size)
 {
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        bool enabled, r;
+        int r, enabled;
+
+        r = kstrtoint(buf, 0, &enabled);
+        if (r)
+                return r;
 
-        enabled = simple_strtoul(buf, NULL, 10);
+        enabled = !!enabled;
 
         if (enabled != (dssdev->state != OMAP_DSS_DISPLAY_DISABLED)) {
                 if (enabled) {
@@ -85,7 +86,9 @@ static ssize_t display_upd_mode_store(struct device *dev,
         if (!dssdev->driver->set_update_mode)
                 return -EINVAL;
 
-        val = simple_strtoul(buf, NULL, 10);
+        r = kstrtoint(buf, 0, &val);
+        if (r)
+                return r;
 
         switch (val) {
         case OMAP_DSS_UPDATE_DISABLED:
@@ -117,13 +120,16 @@ static ssize_t display_tear_store(struct device *dev,
                 struct device_attribute *attr, const char *buf, size_t size)
 {
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        unsigned long te;
-        int r;
+        int te, r;
 
         if (!dssdev->driver->enable_te || !dssdev->driver->get_te)
                 return -ENOENT;
 
-        te = simple_strtoul(buf, NULL, 0);
+        r = kstrtoint(buf, 0, &te);
+        if (r)
+                return r;
+
+        te = !!te;
 
         r = dssdev->driver->enable_te(dssdev, te);
         if (r)
@@ -199,13 +205,14 @@ static ssize_t display_rotate_store(struct device *dev,
                 struct device_attribute *attr, const char *buf, size_t size)
 {
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        unsigned long rot;
-        int r;
+        int rot, r;
 
         if (!dssdev->driver->set_rotate || !dssdev->driver->get_rotate)
                 return -ENOENT;
 
-        rot = simple_strtoul(buf, NULL, 0);
+        r = kstrtoint(buf, 0, &rot);
+        if (r)
+                return r;
 
         r = dssdev->driver->set_rotate(dssdev, rot);
         if (r)
@@ -229,13 +236,16 @@ static ssize_t display_mirror_store(struct device *dev,
                 struct device_attribute *attr, const char *buf, size_t size)
 {
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        unsigned long mirror;
-        int r;
+        int mirror, r;
 
         if (!dssdev->driver->set_mirror || !dssdev->driver->get_mirror)
                 return -ENOENT;
 
-        mirror = simple_strtoul(buf, NULL, 0);
+        r = kstrtoint(buf, 0, &mirror);
+        if (r)
+                return r;
+
+        mirror = !!mirror;
 
         r = dssdev->driver->set_mirror(dssdev, mirror);
         if (r)
@@ -262,14 +272,15 @@ static ssize_t display_wss_store(struct device *dev,
                 struct device_attribute *attr, const char *buf, size_t size)
 {
         struct omap_dss_device *dssdev = to_dss_device(dev);
-        unsigned long wss;
+        u32 wss;
         int r;
 
         if (!dssdev->driver->get_wss || !dssdev->driver->set_wss)
                 return -ENOENT;
 
-        if (strict_strtoul(buf, 0, &wss))
-                return -EINVAL;
+        r = kstrtou32(buf, 0, &wss);
+        if (r)
+                return r;
 
         if (wss > 0xfffff)
                 return -EINVAL;
@@ -345,6 +356,7 @@ int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
                         return 16;
         case OMAP_DISPLAY_TYPE_VENC:
         case OMAP_DISPLAY_TYPE_SDI:
+        case OMAP_DISPLAY_TYPE_HDMI:
                 return 24;
         default:
                 BUG();
@@ -371,6 +383,7 @@ bool dss_use_replication(struct omap_dss_device *dssdev,
         case OMAP_DISPLAY_TYPE_DPI:
                 bpp = dssdev->phy.dpi.data_lines;
                 break;
+        case OMAP_DISPLAY_TYPE_HDMI:
         case OMAP_DISPLAY_TYPE_VENC:
         case OMAP_DISPLAY_TYPE_SDI:
                 bpp = 24;
@@ -396,29 +409,6 @@ void dss_init_device(struct platform_device *pdev,
         switch (dssdev->type) {
 #ifdef CONFIG_OMAP2_DSS_DPI
         case OMAP_DISPLAY_TYPE_DPI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_RFBI
-        case OMAP_DISPLAY_TYPE_DBI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_SDI
-        case OMAP_DISPLAY_TYPE_SDI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_DSI
-        case OMAP_DISPLAY_TYPE_DSI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_VENC
-        case OMAP_DISPLAY_TYPE_VENC:
-#endif
-                break;
-        default:
-                DSSERR("Support for display '%s' not compiled in.\n",
-                                dssdev->name);
-                return;
-        }
-
-        switch (dssdev->type) {
-#ifdef CONFIG_OMAP2_DSS_DPI
-        case OMAP_DISPLAY_TYPE_DPI:
                 r = dpi_init_display(dssdev);
                 break;
 #endif
@@ -442,8 +432,13 @@ void dss_init_device(struct platform_device *pdev,
                 r = dsi_init_display(dssdev);
                 break;
 #endif
+        case OMAP_DISPLAY_TYPE_HDMI:
+                r = hdmi_init_display(dssdev);
+                break;
         default:
-                BUG();
+                DSSERR("Support for display '%s' not compiled in.\n",
+                                dssdev->name);
+                return;
         }
 
         if (r) {
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 960e977a8bf0..ff6bd30132df 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -30,47 +30,73 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
 
 static struct {
         struct regulator *vdds_dsi_reg;
+        struct platform_device *dsidev;
 } dpi;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-static int dpi_set_dsi_clk(bool is_tft, unsigned long pck_req,
-                unsigned long *fck, int *lck_div, int *pck_div)
+static struct platform_device *dpi_get_dsidev(enum omap_dss_clk_source clk)
+{
+        int dsi_module;
+
+        dsi_module = clk == OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC ? 0 : 1;
+
+        return dsi_get_dsidev_from_id(dsi_module);
+}
+
+static bool dpi_use_dsi_pll(struct omap_dss_device *dssdev)
+{
+        if (dssdev->clocks.dispc.dispc_fclk_src ==
+                        OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC ||
+                        dssdev->clocks.dispc.dispc_fclk_src ==
+                        OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC ||
+                        dssdev->clocks.dispc.channel.lcd_clk_src ==
+                        OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC ||
+                        dssdev->clocks.dispc.channel.lcd_clk_src ==
+                        OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC)
+                return true;
+        else
+                return false;
+}
+
+static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
+                unsigned long pck_req, unsigned long *fck, int *lck_div,
+                int *pck_div)
 {
         struct dsi_clock_info dsi_cinfo;
         struct dispc_clock_info dispc_cinfo;
         int r;
 
-        r = dsi_pll_calc_clock_div_pck(is_tft, pck_req, &dsi_cinfo,
-                        &dispc_cinfo);
+        r = dsi_pll_calc_clock_div_pck(dpi.dsidev, is_tft, pck_req,
+                        &dsi_cinfo, &dispc_cinfo);
         if (r)
                 return r;
 
-        r = dsi_pll_set_clock_div(&dsi_cinfo);
+        r = dsi_pll_set_clock_div(dpi.dsidev, &dsi_cinfo);
         if (r)
                 return r;
 
-        dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
+        dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
 
-        r = dispc_set_clock_div(&dispc_cinfo);
+        r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
         if (r)
                 return r;
 
-        *fck = dsi_cinfo.dsi1_pll_fclk;
+        *fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
         *lck_div = dispc_cinfo.lck_div;
         *pck_div = dispc_cinfo.pck_div;
 
         return 0;
 }
-#else
-static int dpi_set_dispc_clk(bool is_tft, unsigned long pck_req,
-                unsigned long *fck, int *lck_div, int *pck_div)
+
+static int dpi_set_dispc_clk(struct omap_dss_device *dssdev, bool is_tft,
+                unsigned long pck_req, unsigned long *fck, int *lck_div,
+                int *pck_div)
 {
         struct dss_clock_info dss_cinfo;
         struct dispc_clock_info dispc_cinfo;
@@ -84,7 +110,7 @@ static int dpi_set_dispc_clk(bool is_tft, unsigned long pck_req,
         if (r)
                 return r;
 
-        r = dispc_set_clock_div(&dispc_cinfo);
+        r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
         if (r)
                 return r;
 
@@ -94,31 +120,29 @@ static int dpi_set_dispc_clk(bool is_tft, unsigned long pck_req,
 
         return 0;
 }
-#endif
 
 static int dpi_set_mode(struct omap_dss_device *dssdev)
 {
         struct omap_video_timings *t = &dssdev->panel.timings;
-        int lck_div, pck_div;
-        unsigned long fck;
+        int lck_div = 0, pck_div = 0;
+        unsigned long fck = 0;
         unsigned long pck;
         bool is_tft;
         int r = 0;
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
-        dispc_set_pol_freq(dssdev->panel.config, dssdev->panel.acbi,
-                        dssdev->panel.acb);
+        dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
+                        dssdev->panel.acbi, dssdev->panel.acb);
 
         is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        r = dpi_set_dsi_clk(is_tft, t->pixel_clock * 1000,
-                        &fck, &lck_div, &pck_div);
-#else
-        r = dpi_set_dispc_clk(is_tft, t->pixel_clock * 1000,
-                        &fck, &lck_div, &pck_div);
-#endif
+        if (dpi_use_dsi_pll(dssdev))
+                r = dpi_set_dsi_clk(dssdev, is_tft, t->pixel_clock * 1000,
+                                &fck, &lck_div, &pck_div);
+        else
+                r = dpi_set_dispc_clk(dssdev, is_tft, t->pixel_clock * 1000,
+                                &fck, &lck_div, &pck_div);
         if (r)
                 goto err0;
 
@@ -132,10 +156,10 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
                 t->pixel_clock = pck;
         }
 
-        dispc_set_lcd_timings(t);
+        dispc_set_lcd_timings(dssdev->manager->id, t);
 
 err0:
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
         return r;
 }
 
@@ -145,10 +169,12 @@ static int dpi_basic_init(struct omap_dss_device *dssdev)
 
         is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
 
-        dispc_set_parallel_interface_mode(OMAP_DSS_PARALLELMODE_BYPASS);
-        dispc_set_lcd_display_type(is_tft ? OMAP_DSS_LCD_DISPLAY_TFT :
-                        OMAP_DSS_LCD_DISPLAY_STN);
-        dispc_set_tft_data_lines(dssdev->phy.dpi.data_lines);
+        dispc_set_parallel_interface_mode(dssdev->manager->id,
+                        OMAP_DSS_PARALLELMODE_BYPASS);
+        dispc_set_lcd_display_type(dssdev->manager->id, is_tft ?
+                        OMAP_DSS_LCD_DISPLAY_TFT : OMAP_DSS_LCD_DISPLAY_STN);
+        dispc_set_tft_data_lines(dssdev->manager->id,
+                        dssdev->phy.dpi.data_lines);
 
         return 0;
 }
@@ -169,18 +195,19 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
                         goto err1;
         }
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         r = dpi_basic_init(dssdev);
         if (r)
                 goto err2;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        dss_clk_enable(DSS_CLK_FCK2);
-        r = dsi_pll_init(dssdev, 0, 1);
-        if (r)
-                goto err3;
-#endif
+        if (dpi_use_dsi_pll(dssdev)) {
+                dss_clk_enable(DSS_CLK_SYSCK);
+                r = dsi_pll_init(dpi.dsidev, 0, 1);
+                if (r)
+                        goto err3;
+        }
+
         r = dpi_set_mode(dssdev);
         if (r)
                 goto err4;
@@ -192,13 +219,13 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
         return 0;
 
 err4:
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        dsi_pll_uninit();
+        if (dpi_use_dsi_pll(dssdev))
+                dsi_pll_uninit(dpi.dsidev, true);
 err3:
-        dss_clk_disable(DSS_CLK_FCK2);
-#endif
+        if (dpi_use_dsi_pll(dssdev))
+                dss_clk_disable(DSS_CLK_SYSCK);
 err2:
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
         if (cpu_is_omap34xx())
                 regulator_disable(dpi.vdds_dsi_reg);
 err1:
@@ -212,13 +239,13 @@ void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 {
         dssdev->manager->disable(dssdev->manager);
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-        dsi_pll_uninit();
-        dss_clk_disable(DSS_CLK_FCK2);
-#endif
+        if (dpi_use_dsi_pll(dssdev)) {
+                dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+                dsi_pll_uninit(dpi.dsidev, true);
+                dss_clk_disable(DSS_CLK_SYSCK);
+        }
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         if (cpu_is_omap34xx())
                 regulator_disable(dpi.vdds_dsi_reg);
@@ -234,7 +261,7 @@ void dpi_set_timings(struct omap_dss_device *dssdev,
         dssdev->panel.timings = *timings;
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
                 dpi_set_mode(dssdev);
-                dispc_go(OMAP_DSS_CHANNEL_LCD);
+                dispc_go(dssdev->manager->id);
         }
 }
 EXPORT_SYMBOL(dpi_set_timings);
@@ -247,6 +274,7 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
         int lck_div, pck_div;
         unsigned long fck;
         unsigned long pck;
+        struct dispc_clock_info dispc_cinfo;
 
         if (!dispc_lcd_timings_ok(timings))
                 return -EINVAL;
@@ -256,25 +284,18 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 
         is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
 
-#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        {
+        if (dpi_use_dsi_pll(dssdev)) {
                 struct dsi_clock_info dsi_cinfo;
-                struct dispc_clock_info dispc_cinfo;
-                r = dsi_pll_calc_clock_div_pck(is_tft,
+                r = dsi_pll_calc_clock_div_pck(dpi.dsidev, is_tft,
                                 timings->pixel_clock * 1000,
                                 &dsi_cinfo, &dispc_cinfo);
 
                 if (r)
                         return r;
 
-                fck = dsi_cinfo.dsi1_pll_fclk;
-                lck_div = dispc_cinfo.lck_div;
-                pck_div = dispc_cinfo.pck_div;
-        }
-#else
-        {
+                fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
+        } else {
                 struct dss_clock_info dss_cinfo;
-                struct dispc_clock_info dispc_cinfo;
                 r = dss_calc_clock_div(is_tft, timings->pixel_clock * 1000,
                                 &dss_cinfo, &dispc_cinfo);
 
@@ -282,10 +303,10 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
                         return r;
 
                 fck = dss_cinfo.fck;
-                lck_div = dispc_cinfo.lck_div;
-                pck_div = dispc_cinfo.pck_div;
         }
-#endif
+
+        lck_div = dispc_cinfo.lck_div;
+        pck_div = dispc_cinfo.pck_div;
 
         pck = fck / lck_div / pck_div / 1000;
 
@@ -299,22 +320,33 @@ int dpi_init_display(struct omap_dss_device *dssdev)
 {
         DSSDBG("init_display\n");
 
-        return 0;
-}
+        if (cpu_is_omap34xx() && dpi.vdds_dsi_reg == NULL) {
+                struct regulator *vdds_dsi;
 
-int dpi_init(struct platform_device *pdev)
-{
-        if (cpu_is_omap34xx()) {
-                dpi.vdds_dsi_reg = dss_get_vdds_dsi();
-                if (IS_ERR(dpi.vdds_dsi_reg)) {
+                vdds_dsi = dss_get_vdds_dsi();
+
+                if (IS_ERR(vdds_dsi)) {
                         DSSERR("can't get VDDS_DSI regulator\n");
-                        return PTR_ERR(dpi.vdds_dsi_reg);
+                        return PTR_ERR(vdds_dsi);
                 }
+
+                dpi.vdds_dsi_reg = vdds_dsi;
+        }
+
+        if (dpi_use_dsi_pll(dssdev)) {
+                enum omap_dss_clk_source dispc_fclk_src =
+                        dssdev->clocks.dispc.dispc_fclk_src;
+                dpi.dsidev = dpi_get_dsidev(dispc_fclk_src);
         }
 
         return 0;
 }
 
+int dpi_init(void)
+{
+        return 0;
+}
+
 void dpi_exit(void)
 {
 }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index b3fa3a7db911..345757cfcbee 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -33,17 +33,19 @@
 #include <linux/regulator/consumer.h>
 #include <linux/wait.h>
 #include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/clock.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 /*#define VERBOSE_IRQ*/
 #define DSI_CATCH_MISSING_TE
 
-#define DSI_BASE                0x4804FC00
-
 struct dsi_reg { u16 idx; };
 
 #define DSI_REG(idx)            ((const struct dsi_reg) { idx })
@@ -57,6 +59,7 @@ struct dsi_reg { u16 idx; };
 #define DSI_IRQSTATUS                   DSI_REG(0x0018)
 #define DSI_IRQENABLE                   DSI_REG(0x001C)
 #define DSI_CTRL                        DSI_REG(0x0040)
+#define DSI_GNQ                         DSI_REG(0x0044)
 #define DSI_COMPLEXIO_CFG1              DSI_REG(0x0048)
 #define DSI_COMPLEXIO_IRQ_STATUS        DSI_REG(0x004C)
 #define DSI_COMPLEXIO_IRQ_ENABLE        DSI_REG(0x0050)
@@ -91,6 +94,7 @@ struct dsi_reg { u16 idx; };
 #define DSI_DSIPHY_CFG1                 DSI_REG(0x200 + 0x0004)
 #define DSI_DSIPHY_CFG2                 DSI_REG(0x200 + 0x0008)
 #define DSI_DSIPHY_CFG5                 DSI_REG(0x200 + 0x0014)
+#define DSI_DSIPHY_CFG10                DSI_REG(0x200 + 0x0028)
 
 /* DSI_PLL_CTRL_SCP */
 
@@ -100,11 +104,11 @@ struct dsi_reg { u16 idx; };
 #define DSI_PLL_CONFIGURATION1          DSI_REG(0x300 + 0x000C)
 #define DSI_PLL_CONFIGURATION2          DSI_REG(0x300 + 0x0010)
 
-#define REG_GET(idx, start, end) \
-        FLD_GET(dsi_read_reg(idx), start, end)
+#define REG_GET(dsidev, idx, start, end) \
+        FLD_GET(dsi_read_reg(dsidev, idx), start, end)
 
-#define REG_FLD_MOD(idx, val, start, end) \
-        dsi_write_reg(idx, FLD_MOD(dsi_read_reg(idx), val, start, end))
+#define REG_FLD_MOD(dsidev, idx, val, start, end) \
+        dsi_write_reg(dsidev, idx, FLD_MOD(dsi_read_reg(dsidev, idx), val, start, end))
 
 /* Global interrupts */
 #define DSI_IRQ_VC0             (1 << 0)
@@ -148,31 +152,50 @@ struct dsi_reg { u16 idx; };
 #define DSI_CIO_IRQ_ERRSYNCESC1         (1 << 0)
 #define DSI_CIO_IRQ_ERRSYNCESC2         (1 << 1)
 #define DSI_CIO_IRQ_ERRSYNCESC3         (1 << 2)
+#define DSI_CIO_IRQ_ERRSYNCESC4         (1 << 3)
+#define DSI_CIO_IRQ_ERRSYNCESC5         (1 << 4)
 #define DSI_CIO_IRQ_ERRESC1             (1 << 5)
 #define DSI_CIO_IRQ_ERRESC2             (1 << 6)
 #define DSI_CIO_IRQ_ERRESC3             (1 << 7)
+#define DSI_CIO_IRQ_ERRESC4             (1 << 8)
+#define DSI_CIO_IRQ_ERRESC5             (1 << 9)
 #define DSI_CIO_IRQ_ERRCONTROL1         (1 << 10)
 #define DSI_CIO_IRQ_ERRCONTROL2         (1 << 11)
 #define DSI_CIO_IRQ_ERRCONTROL3         (1 << 12)
+#define DSI_CIO_IRQ_ERRCONTROL4         (1 << 13)
+#define DSI_CIO_IRQ_ERRCONTROL5         (1 << 14)
 #define DSI_CIO_IRQ_STATEULPS1          (1 << 15)
 #define DSI_CIO_IRQ_STATEULPS2          (1 << 16)
 #define DSI_CIO_IRQ_STATEULPS3          (1 << 17)
+#define DSI_CIO_IRQ_STATEULPS4          (1 << 18)
+#define DSI_CIO_IRQ_STATEULPS5          (1 << 19)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP0_1  (1 << 20)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_1  (1 << 21)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP0_2  (1 << 22)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_2  (1 << 23)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP0_3  (1 << 24)
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_3  (1 << 25)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_4  (1 << 26)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_4  (1 << 27)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_5  (1 << 28)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_5  (1 << 29)
 #define DSI_CIO_IRQ_ULPSACTIVENOT_ALL0  (1 << 30)
 #define DSI_CIO_IRQ_ULPSACTIVENOT_ALL1  (1 << 31)
 #define DSI_CIO_IRQ_ERROR_MASK \
         (DSI_CIO_IRQ_ERRSYNCESC1 | DSI_CIO_IRQ_ERRSYNCESC2 | \
-         DSI_CIO_IRQ_ERRSYNCESC3 | DSI_CIO_IRQ_ERRESC1 | DSI_CIO_IRQ_ERRESC2 | \
-         DSI_CIO_IRQ_ERRESC3 | DSI_CIO_IRQ_ERRCONTROL1 | \
-         DSI_CIO_IRQ_ERRCONTROL2 | DSI_CIO_IRQ_ERRCONTROL3 | \
+         DSI_CIO_IRQ_ERRSYNCESC3 | DSI_CIO_IRQ_ERRSYNCESC4 | \
+         DSI_CIO_IRQ_ERRSYNCESC5 | \
+         DSI_CIO_IRQ_ERRESC1 | DSI_CIO_IRQ_ERRESC2 | \
+         DSI_CIO_IRQ_ERRESC3 | DSI_CIO_IRQ_ERRESC4 | \
+         DSI_CIO_IRQ_ERRESC5 | \
+         DSI_CIO_IRQ_ERRCONTROL1 | DSI_CIO_IRQ_ERRCONTROL2 | \
+         DSI_CIO_IRQ_ERRCONTROL3 | DSI_CIO_IRQ_ERRCONTROL4 | \
+         DSI_CIO_IRQ_ERRCONTROL5 | \
          DSI_CIO_IRQ_ERRCONTENTIONLP0_1 | DSI_CIO_IRQ_ERRCONTENTIONLP1_1 | \
          DSI_CIO_IRQ_ERRCONTENTIONLP0_2 | DSI_CIO_IRQ_ERRCONTENTIONLP1_2 | \
-         DSI_CIO_IRQ_ERRCONTENTIONLP0_3 | DSI_CIO_IRQ_ERRCONTENTIONLP1_3)
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_3 | DSI_CIO_IRQ_ERRCONTENTIONLP1_3 | \
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_4 | DSI_CIO_IRQ_ERRCONTENTIONLP1_4 | \
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_5 | DSI_CIO_IRQ_ERRCONTENTIONLP1_5)
 
 #define DSI_DT_DCS_SHORT_WRITE_0        0x05
 #define DSI_DT_DCS_SHORT_WRITE_1        0x15
@@ -186,13 +209,15 @@ struct dsi_reg { u16 idx; };
 #define DSI_DT_RX_SHORT_READ_1          0x21
 #define DSI_DT_RX_SHORT_READ_2          0x22
 
-#define FINT_MAX 2100000
-#define FINT_MIN 750000
-#define REGN_MAX (1 << 7)
-#define REGM_MAX ((1 << 11) - 1)
-#define REGM3_MAX (1 << 4)
-#define REGM4_MAX (1 << 4)
-#define LP_DIV_MAX ((1 << 13) - 1)
+typedef void (*omap_dsi_isr_t) (void *arg, u32 mask);
+
+#define DSI_MAX_NR_ISRS                2
+
+struct dsi_isr_data {
+        omap_dsi_isr_t  isr;
+        void            *arg;
+        u32             mask;
+};
 
 enum fifo_size {
         DSI_FIFO_SIZE_0         = 0,
@@ -207,6 +232,19 @@ enum dsi_vc_mode {
         DSI_VC_MODE_VP,
 };
 
+enum dsi_lane {
+        DSI_CLK_P       = 1 << 0,
+        DSI_CLK_N       = 1 << 1,
+        DSI_DATA1_P     = 1 << 2,
+        DSI_DATA1_N     = 1 << 3,
+        DSI_DATA2_P     = 1 << 4,
+        DSI_DATA2_N     = 1 << 5,
+        DSI_DATA3_P     = 1 << 6,
+        DSI_DATA3_N     = 1 << 7,
+        DSI_DATA4_P     = 1 << 8,
+        DSI_DATA4_N     = 1 << 9,
+};
+
 struct dsi_update_region {
         u16 x, y, w, h;
         struct omap_dss_device *device;
@@ -220,18 +258,29 @@ struct dsi_irq_stats {
         unsigned cio_irqs[32];
 };
 
-static struct
-{
+struct dsi_isr_tables {
+        struct dsi_isr_data isr_table[DSI_MAX_NR_ISRS];
+        struct dsi_isr_data isr_table_vc[4][DSI_MAX_NR_ISRS];
+        struct dsi_isr_data isr_table_cio[DSI_MAX_NR_ISRS];
+};
+
+struct dsi_data {
+        struct platform_device *pdev;
         void __iomem    *base;
+        int irq;
+
+        void (*dsi_mux_pads)(bool enable);
 
         struct dsi_clock_info current_cinfo;
 
+        bool vdds_dsi_enabled;
         struct regulator *vdds_dsi_reg;
 
         struct {
                 enum dsi_vc_mode mode;
                 struct omap_dss_device *dssdev;
                 enum fifo_size fifo_size;
+                int vc_id;
         } vc[4];
 
         struct mutex lock;
@@ -239,15 +288,16 @@ static struct
 
         unsigned pll_locked;
 
-        struct completion bta_completion;
-        void (*bta_callback)(void);
+        spinlock_t irq_lock;
+        struct dsi_isr_tables isr_tables;
+        /* space for a copy used by the interrupt handler */
+        struct dsi_isr_tables isr_tables_copy;
 
         int update_channel;
         struct dsi_update_region update_region;
 
         bool te_enabled;
-
-        struct workqueue_struct *workqueue;
+        bool ulps_enabled;
 
         void (*framedone_callback)(int, void *);
         void *framedone_data;
@@ -275,21 +325,68 @@ static struct
         spinlock_t irq_stats_lock;
         struct dsi_irq_stats irq_stats;
 #endif
-} dsi;
+        /* DSI PLL Parameter Ranges */
+        unsigned long regm_max, regn_max;
+        unsigned long  regm_dispc_max, regm_dsi_max;
+        unsigned long  fint_min, fint_max;
+        unsigned long lpdiv_max;
+
+        int num_data_lanes;
+
+        unsigned scp_clk_refcount;
+};
+
+struct dsi_packet_sent_handler_data {
+        struct platform_device *dsidev;
+        struct completion *completion;
+};
+
+static struct platform_device *dsi_pdev_map[MAX_NUM_DSI];
 
 #ifdef DEBUG
 static unsigned int dsi_perf;
 module_param_named(dsi_perf, dsi_perf, bool, 0644);
 #endif
 
-static inline void dsi_write_reg(const struct dsi_reg idx, u32 val)
+static inline struct dsi_data *dsi_get_dsidrv_data(struct platform_device *dsidev)
+{
+        return dev_get_drvdata(&dsidev->dev);
+}
+
+static inline struct platform_device *dsi_get_dsidev_from_dssdev(struct omap_dss_device *dssdev)
 {
-        __raw_writel(val, dsi.base + idx.idx);
+        return dsi_pdev_map[dssdev->phy.dsi.module];
 }
 
-static inline u32 dsi_read_reg(const struct dsi_reg idx)
+struct platform_device *dsi_get_dsidev_from_id(int module)
 {
-        return __raw_readl(dsi.base + idx.idx);
+        return dsi_pdev_map[module];
+}
+
+static int dsi_get_dsidev_id(struct platform_device *dsidev)
+{
+        /* TEMP: Pass 0 as the dsi module index till the time the dsi platform
+         * device names aren't changed to the form "omapdss_dsi.0",
+         * "omapdss_dsi.1" and so on */
+        BUG_ON(dsidev->id != -1);
+
+        return 0;
+}
+
+static inline void dsi_write_reg(struct platform_device *dsidev,
+                const struct dsi_reg idx, u32 val)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        __raw_writel(val, dsi->base + idx.idx);
+}
+
+static inline u32 dsi_read_reg(struct platform_device *dsidev,
+                const struct dsi_reg idx)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return __raw_readl(dsi->base + idx.idx);
 }
 
 
@@ -301,29 +398,42 @@ void dsi_restore_context(void)
 {
 }
 
-void dsi_bus_lock(void)
+void dsi_bus_lock(struct omap_dss_device *dssdev)
 {
-        down(&dsi.bus_lock);
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        down(&dsi->bus_lock);
 }
 EXPORT_SYMBOL(dsi_bus_lock);
 
-void dsi_bus_unlock(void)
+void dsi_bus_unlock(struct omap_dss_device *dssdev)
 {
-        up(&dsi.bus_lock);
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        up(&dsi->bus_lock);
 }
 EXPORT_SYMBOL(dsi_bus_unlock);
 
-static bool dsi_bus_is_locked(void)
+static bool dsi_bus_is_locked(struct platform_device *dsidev)
 {
-        return dsi.bus_lock.count == 0;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->bus_lock.count == 0;
 }
 
-static inline int wait_for_bit_change(const struct dsi_reg idx, int bitnum,
-                int value)
+static void dsi_completion_handler(void *data, u32 mask)
+{
+        complete((struct completion *)data);
+}
+
+static inline int wait_for_bit_change(struct platform_device *dsidev,
+                const struct dsi_reg idx, int bitnum, int value)
 {
         int t = 100000;
 
-        while (REG_GET(idx, bitnum, bitnum) != value) {
+        while (REG_GET(dsidev, idx, bitnum, bitnum) != value) {
                 if (--t == 0)
                         return !value;
         }
@@ -332,18 +442,21 @@ static inline int wait_for_bit_change(const struct dsi_reg idx, int bitnum,
 }
 
 #ifdef DEBUG
-static void dsi_perf_mark_setup(void)
+static void dsi_perf_mark_setup(struct platform_device *dsidev)
 {
-        dsi.perf_setup_time = ktime_get();
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        dsi->perf_setup_time = ktime_get();
 }
 
-static void dsi_perf_mark_start(void)
+static void dsi_perf_mark_start(struct platform_device *dsidev)
 {
-        dsi.perf_start_time = ktime_get();
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        dsi->perf_start_time = ktime_get();
 }
 
-static void dsi_perf_show(const char *name)
+static void dsi_perf_show(struct platform_device *dsidev, const char *name)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         ktime_t t, setup_time, trans_time;
         u32 total_bytes;
         u32 setup_us, trans_us, total_us;
@@ -353,21 +466,21 @@ static void dsi_perf_show(const char *name)
 
         t = ktime_get();
 
-        setup_time = ktime_sub(dsi.perf_start_time, dsi.perf_setup_time);
+        setup_time = ktime_sub(dsi->perf_start_time, dsi->perf_setup_time);
         setup_us = (u32)ktime_to_us(setup_time);
         if (setup_us == 0)
                 setup_us = 1;
 
-        trans_time = ktime_sub(t, dsi.perf_start_time);
+        trans_time = ktime_sub(t, dsi->perf_start_time);
         trans_us = (u32)ktime_to_us(trans_time);
         if (trans_us == 0)
                 trans_us = 1;
 
         total_us = setup_us + trans_us;
 
-        total_bytes = dsi.update_region.w *
-                dsi.update_region.h *
-                dsi.update_region.device->ctrl.pixel_size / 8;
+        total_bytes = dsi->update_region.w *
+                dsi->update_region.h *
+                dsi->update_region.device->ctrl.pixel_size / 8;
 
         printk(KERN_INFO "DSI(%s): %u us + %u us = %u us (%uHz), "
                         "%u bytes, %u kbytes/sec\n",
@@ -380,13 +493,16 @@ static void dsi_perf_show(const char *name)
                         total_bytes * 1000 / total_us);
 }
 #else
-#define dsi_perf_mark_setup()
-#define dsi_perf_mark_start()
-#define dsi_perf_show(x)
+#define dsi_perf_mark_setup(x)
+#define dsi_perf_mark_start(x)
+#define dsi_perf_show(x, y)
 #endif
 
 static void print_irq_status(u32 status)
 {
+        if (status == 0)
+                return;
+
 #ifndef VERBOSE_IRQ
         if ((status & ~DSI_IRQ_CHANNEL_MASK) == 0)
                 return;
@@ -422,6 +538,9 @@ static void print_irq_status(u32 status)
 
 static void print_irq_status_vc(int channel, u32 status)
 {
+        if (status == 0)
+                return;
+
 #ifndef VERBOSE_IRQ
         if ((status & ~DSI_VC_IRQ_PACKET_SENT) == 0)
                 return;
@@ -448,6 +567,9 @@ static void print_irq_status_vc(int channel, u32 status)
 
 static void print_irq_status_cio(u32 status)
 {
+        if (status == 0)
+                return;
+
         printk(KERN_DEBUG "DSI CIO IRQ 0x%x: ", status);
 
 #define PIS(x) \
@@ -478,192 +600,476 @@ static void print_irq_status_cio(u32 status)
         printk("\n");
 }
 
-static int debug_irq;
-
-/* called from dss */
-void dsi_irq_handler(void)
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dsi_collect_irq_stats(struct platform_device *dsidev, u32 irqstatus,
+                u32 *vcstatus, u32 ciostatus)
 {
-        u32 irqstatus, vcstatus, ciostatus;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int i;
 
-        irqstatus = dsi_read_reg(DSI_IRQSTATUS);
+        spin_lock(&dsi->irq_stats_lock);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-        spin_lock(&dsi.irq_stats_lock);
-        dsi.irq_stats.irq_count++;
-        dss_collect_irq_stats(irqstatus, dsi.irq_stats.dsi_irqs);
+        dsi->irq_stats.irq_count++;
+        dss_collect_irq_stats(irqstatus, dsi->irq_stats.dsi_irqs);
+
+        for (i = 0; i < 4; ++i)
+                dss_collect_irq_stats(vcstatus[i], dsi->irq_stats.vc_irqs[i]);
+
+        dss_collect_irq_stats(ciostatus, dsi->irq_stats.cio_irqs);
+
+        spin_unlock(&dsi->irq_stats_lock);
+}
+#else
+#define dsi_collect_irq_stats(dsidev, irqstatus, vcstatus, ciostatus)
 #endif
 
+static int debug_irq;
+
+static void dsi_handle_irq_errors(struct platform_device *dsidev, u32 irqstatus,
+                u32 *vcstatus, u32 ciostatus)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int i;
+
         if (irqstatus & DSI_IRQ_ERROR_MASK) {
                 DSSERR("DSI error, irqstatus %x\n", irqstatus);
                 print_irq_status(irqstatus);
-                spin_lock(&dsi.errors_lock);
-                dsi.errors |= irqstatus & DSI_IRQ_ERROR_MASK;
-                spin_unlock(&dsi.errors_lock);
+                spin_lock(&dsi->errors_lock);
+                dsi->errors |= irqstatus & DSI_IRQ_ERROR_MASK;
+                spin_unlock(&dsi->errors_lock);
         } else if (debug_irq) {
                 print_irq_status(irqstatus);
         }
 
-#ifdef DSI_CATCH_MISSING_TE
-        if (irqstatus & DSI_IRQ_TE_TRIGGER)
-                del_timer(&dsi.te_timer);
-#endif
+        for (i = 0; i < 4; ++i) {
+                if (vcstatus[i] & DSI_VC_IRQ_ERROR_MASK) {
+                        DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
+                                       i, vcstatus[i]);
+                        print_irq_status_vc(i, vcstatus[i]);
+                } else if (debug_irq) {
+                        print_irq_status_vc(i, vcstatus[i]);
+                }
+        }
+
+        if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
+                DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
+                print_irq_status_cio(ciostatus);
+        } else if (debug_irq) {
+                print_irq_status_cio(ciostatus);
+        }
+}
+
+static void dsi_call_isrs(struct dsi_isr_data *isr_array,
+                unsigned isr_array_size, u32 irqstatus)
+{
+        struct dsi_isr_data *isr_data;
+        int i;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+                if (isr_data->isr && isr_data->mask & irqstatus)
+                        isr_data->isr(isr_data->arg, irqstatus);
+        }
+}
+
+static void dsi_handle_isrs(struct dsi_isr_tables *isr_tables,
+                u32 irqstatus, u32 *vcstatus, u32 ciostatus)
+{
+        int i;
+
+        dsi_call_isrs(isr_tables->isr_table,
+                        ARRAY_SIZE(isr_tables->isr_table),
+                        irqstatus);
 
         for (i = 0; i < 4; ++i) {
-                if ((irqstatus & (1<<i)) == 0)
+                if (vcstatus[i] == 0)
                         continue;
+                dsi_call_isrs(isr_tables->isr_table_vc[i],
+                                ARRAY_SIZE(isr_tables->isr_table_vc[i]),
+                                vcstatus[i]);
+        }
 
-                vcstatus = dsi_read_reg(DSI_VC_IRQSTATUS(i));
+        if (ciostatus != 0)
+                dsi_call_isrs(isr_tables->isr_table_cio,
+                                ARRAY_SIZE(isr_tables->isr_table_cio),
+                                ciostatus);
+}
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-                dss_collect_irq_stats(vcstatus, dsi.irq_stats.vc_irqs[i]);
-#endif
+static irqreturn_t omap_dsi_irq_handler(int irq, void *arg)
+{
+        struct platform_device *dsidev;
+        struct dsi_data *dsi;
+        u32 irqstatus, vcstatus[4], ciostatus;
+        int i;
 
-                if (vcstatus & DSI_VC_IRQ_BTA) {
-                        complete(&dsi.bta_completion);
+        dsidev = (struct platform_device *) arg;
+        dsi = dsi_get_dsidrv_data(dsidev);
 
-                        if (dsi.bta_callback)
-                                dsi.bta_callback();
-                }
+        spin_lock(&dsi->irq_lock);
 
-                if (vcstatus & DSI_VC_IRQ_ERROR_MASK) {
-                        DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
-                                       i, vcstatus);
-                        print_irq_status_vc(i, vcstatus);
-                } else if (debug_irq) {
-                        print_irq_status_vc(i, vcstatus);
+        irqstatus = dsi_read_reg(dsidev, DSI_IRQSTATUS);
+
+        /* IRQ is not for us */
+        if (!irqstatus) {
+                spin_unlock(&dsi->irq_lock);
+                return IRQ_NONE;
+        }
+
+        dsi_write_reg(dsidev, DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
+        /* flush posted write */
+        dsi_read_reg(dsidev, DSI_IRQSTATUS);
+
+        for (i = 0; i < 4; ++i) {
+                if ((irqstatus & (1 << i)) == 0) {
+                        vcstatus[i] = 0;
+                        continue;
                 }
 
-                dsi_write_reg(DSI_VC_IRQSTATUS(i), vcstatus);
+                vcstatus[i] = dsi_read_reg(dsidev, DSI_VC_IRQSTATUS(i));
+
+                dsi_write_reg(dsidev, DSI_VC_IRQSTATUS(i), vcstatus[i]);
                 /* flush posted write */
-                dsi_read_reg(DSI_VC_IRQSTATUS(i));
+                dsi_read_reg(dsidev, DSI_VC_IRQSTATUS(i));
         }
 
         if (irqstatus & DSI_IRQ_COMPLEXIO_ERR) {
-                ciostatus = dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
+                ciostatus = dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-                dss_collect_irq_stats(ciostatus, dsi.irq_stats.cio_irqs);
+                dsi_write_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS, ciostatus);
+                /* flush posted write */
+                dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
+        } else {
+                ciostatus = 0;
+        }
+
+#ifdef DSI_CATCH_MISSING_TE
+        if (irqstatus & DSI_IRQ_TE_TRIGGER)
+                del_timer(&dsi->te_timer);
 #endif
 
-                dsi_write_reg(DSI_COMPLEXIO_IRQ_STATUS, ciostatus);
-                /* flush posted write */
-                dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
+        /* make a copy and unlock, so that isrs can unregister
+         * themselves */
+        memcpy(&dsi->isr_tables_copy, &dsi->isr_tables,
+                sizeof(dsi->isr_tables));
 
-                if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
-                        DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
-                        print_irq_status_cio(ciostatus);
-                } else if (debug_irq) {
-                        print_irq_status_cio(ciostatus);
-                }
+        spin_unlock(&dsi->irq_lock);
+
+        dsi_handle_isrs(&dsi->isr_tables_copy, irqstatus, vcstatus, ciostatus);
+
+        dsi_handle_irq_errors(dsidev, irqstatus, vcstatus, ciostatus);
+
+        dsi_collect_irq_stats(dsidev, irqstatus, vcstatus, ciostatus);
+
+        return IRQ_HANDLED;
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_configure_irqs(struct platform_device *dsidev,
+                struct dsi_isr_data *isr_array,
+                unsigned isr_array_size, u32 default_mask,
+                const struct dsi_reg enable_reg,
+                const struct dsi_reg status_reg)
+{
+        struct dsi_isr_data *isr_data;
+        u32 mask;
+        u32 old_mask;
+        int i;
+
+        mask = default_mask;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+
+                if (isr_data->isr == NULL)
+                        continue;
+
+                mask |= isr_data->mask;
         }
 
-        dsi_write_reg(DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
-        /* flush posted write */
-        dsi_read_reg(DSI_IRQSTATUS);
+        old_mask = dsi_read_reg(dsidev, enable_reg);
+        /* clear the irqstatus for newly enabled irqs */
+        dsi_write_reg(dsidev, status_reg, (mask ^ old_mask) & mask);
+        dsi_write_reg(dsidev, enable_reg, mask);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-        spin_unlock(&dsi.irq_stats_lock);
+        /* flush posted writes */
+        dsi_read_reg(dsidev, enable_reg);
+        dsi_read_reg(dsidev, status_reg);
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        u32 mask = DSI_IRQ_ERROR_MASK;
+#ifdef DSI_CATCH_MISSING_TE
+        mask |= DSI_IRQ_TE_TRIGGER;
 #endif
+        _omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table), mask,
+                        DSI_IRQENABLE, DSI_IRQSTATUS);
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_vc(struct platform_device *dsidev, int vc)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        _omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table_vc[vc],
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_vc[vc]),
+                        DSI_VC_IRQ_ERROR_MASK,
+                        DSI_VC_IRQENABLE(vc), DSI_VC_IRQSTATUS(vc));
 }
 
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_cio(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        _omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_cio),
+                        DSI_CIO_IRQ_ERROR_MASK,
+                        DSI_COMPLEXIO_IRQ_ENABLE, DSI_COMPLEXIO_IRQ_STATUS);
+}
 
-static void _dsi_initialize_irq(void)
+static void _dsi_initialize_irq(struct platform_device *dsidev)
 {
-        u32 l;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int vc;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        memset(&dsi->isr_tables, 0, sizeof(dsi->isr_tables));
+
+        _omap_dsi_set_irqs(dsidev);
+        for (vc = 0; vc < 4; ++vc)
+                _omap_dsi_set_irqs_vc(dsidev, vc);
+        _omap_dsi_set_irqs_cio(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+}
+
+static int _dsi_register_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+                struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+        struct dsi_isr_data *isr_data;
+        int free_idx;
         int i;
 
-        /* disable all interrupts */
-        dsi_write_reg(DSI_IRQENABLE, 0);
-        for (i = 0; i < 4; ++i)
-                dsi_write_reg(DSI_VC_IRQENABLE(i), 0);
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, 0);
+        BUG_ON(isr == NULL);
 
-        /* clear interrupt status */
-        l = dsi_read_reg(DSI_IRQSTATUS);
-        dsi_write_reg(DSI_IRQSTATUS, l & ~DSI_IRQ_CHANNEL_MASK);
+        /* check for duplicate entry and find a free slot */
+        free_idx = -1;
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
 
-        for (i = 0; i < 4; ++i) {
-                l = dsi_read_reg(DSI_VC_IRQSTATUS(i));
-                dsi_write_reg(DSI_VC_IRQSTATUS(i), l);
+                if (isr_data->isr == isr && isr_data->arg == arg &&
+                                isr_data->mask == mask) {
+                        return -EINVAL;
+                }
+
+                if (isr_data->isr == NULL && free_idx == -1)
+                        free_idx = i;
         }
 
-        l = dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_STATUS, l);
+        if (free_idx == -1)
+                return -EBUSY;
 
-        /* enable error irqs */
-        l = DSI_IRQ_ERROR_MASK;
-#ifdef DSI_CATCH_MISSING_TE
-        l |= DSI_IRQ_TE_TRIGGER;
-#endif
-        dsi_write_reg(DSI_IRQENABLE, l);
+        isr_data = &isr_array[free_idx];
+        isr_data->isr = isr;
+        isr_data->arg = arg;
+        isr_data->mask = mask;
 
-        l = DSI_VC_IRQ_ERROR_MASK;
-        for (i = 0; i < 4; ++i)
-                dsi_write_reg(DSI_VC_IRQENABLE(i), l);
+        return 0;
+}
+
+static int _dsi_unregister_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+                struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+        struct dsi_isr_data *isr_data;
+        int i;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+                if (isr_data->isr != isr || isr_data->arg != arg ||
+                                isr_data->mask != mask)
+                        continue;
+
+                isr_data->isr = NULL;
+                isr_data->arg = NULL;
+                isr_data->mask = 0;
 
-        l = DSI_CIO_IRQ_ERROR_MASK;
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, l);
+                return 0;
+        }
+
+        return -EINVAL;
 }
 
-static u32 dsi_get_errors(void)
+static int dsi_register_isr(struct platform_device *dsidev, omap_dsi_isr_t isr,
+                void *arg, u32 mask)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         unsigned long flags;
-        u32 e;
-        spin_lock_irqsave(&dsi.errors_lock, flags);
-        e = dsi.errors;
-        dsi.errors = 0;
-        spin_unlock_irqrestore(&dsi.errors_lock, flags);
-        return e;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table));
+
+        if (r == 0)
+                _omap_dsi_set_irqs(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
 }
 
-static void dsi_vc_enable_bta_irq(int channel)
+static int dsi_unregister_isr(struct platform_device *dsidev,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
 {
-        u32 l;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
 
-        dsi_write_reg(DSI_VC_IRQSTATUS(channel), DSI_VC_IRQ_BTA);
+        spin_lock_irqsave(&dsi->irq_lock, flags);
 
-        l = dsi_read_reg(DSI_VC_IRQENABLE(channel));
-        l |= DSI_VC_IRQ_BTA;
-        dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
+        r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table));
+
+        if (r == 0)
+                _omap_dsi_set_irqs(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
 }
 
-static void dsi_vc_disable_bta_irq(int channel)
+static int dsi_register_isr_vc(struct platform_device *dsidev, int channel,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
 {
-        u32 l;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask,
+                        dsi->isr_tables.isr_table_vc[channel],
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_vc[channel]));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_vc(dsidev, channel);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_unregister_isr_vc(struct platform_device *dsidev, int channel,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask,
+                        dsi->isr_tables.isr_table_vc[channel],
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_vc[channel]));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_vc(dsidev, channel);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
+}
 
-        l = dsi_read_reg(DSI_VC_IRQENABLE(channel));
-        l &= ~DSI_VC_IRQ_BTA;
-        dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
+static int dsi_register_isr_cio(struct platform_device *dsidev,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_cio(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_unregister_isr_cio(struct platform_device *dsidev,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_cio(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
 }
 
-/* DSI func clock. this could also be DSI2_PLL_FCLK */
+static u32 dsi_get_errors(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        u32 e;
+        spin_lock_irqsave(&dsi->errors_lock, flags);
+        e = dsi->errors;
+        dsi->errors = 0;
+        spin_unlock_irqrestore(&dsi->errors_lock, flags);
+        return e;
+}
+
+/* DSI func clock. this could also be dsi_pll_hsdiv_dsi_clk */
 static inline void enable_clocks(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 /* source clock for DSI PLL. this could also be PCLKFREE */
-static inline void dsi_enable_pll_clock(bool enable)
+static inline void dsi_enable_pll_clock(struct platform_device *dsidev,
+                bool enable)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
         if (enable)
-                dss_clk_enable(DSS_CLK_FCK2);
+                dss_clk_enable(DSS_CLK_SYSCK);
         else
-                dss_clk_disable(DSS_CLK_FCK2);
+                dss_clk_disable(DSS_CLK_SYSCK);
 
-        if (enable && dsi.pll_locked) {
-                if (wait_for_bit_change(DSI_PLL_STATUS, 1, 1) != 1)
+        if (enable && dsi->pll_locked) {
+                if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 1, 1) != 1)
                         DSSERR("cannot lock PLL when enabling clocks\n");
         }
 }
 
 #ifdef DEBUG
-static void _dsi_print_reset_status(void)
+static void _dsi_print_reset_status(struct platform_device *dsidev)
 {
         u32 l;
+        int b0, b1, b2;
 
         if (!dss_debug)
                 return;
@@ -671,35 +1077,47 @@ static void _dsi_print_reset_status(void)
         /* A dummy read using the SCP interface to any DSIPHY register is
          * required after DSIPHY reset to complete the reset of the DSI complex
          * I/O. */
-        l = dsi_read_reg(DSI_DSIPHY_CFG5);
+        l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
 
         printk(KERN_DEBUG "DSI resets: ");
 
-        l = dsi_read_reg(DSI_PLL_STATUS);
+        l = dsi_read_reg(dsidev, DSI_PLL_STATUS);
         printk("PLL (%d) ", FLD_GET(l, 0, 0));
 
-        l = dsi_read_reg(DSI_COMPLEXIO_CFG1);
+        l = dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1);
         printk("CIO (%d) ", FLD_GET(l, 29, 29));
 
-        l = dsi_read_reg(DSI_DSIPHY_CFG5);
-        printk("PHY (%x, %d, %d, %d)\n",
-                        FLD_GET(l, 28, 26),
+        if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC)) {
+                b0 = 28;
+                b1 = 27;
+                b2 = 26;
+        } else {
+                b0 = 24;
+                b1 = 25;
+                b2 = 26;
+        }
+
+        l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+        printk("PHY (%x%x%x, %d, %d, %d)\n",
+                        FLD_GET(l, b0, b0),
+                        FLD_GET(l, b1, b1),
+                        FLD_GET(l, b2, b2),
                         FLD_GET(l, 29, 29),
                         FLD_GET(l, 30, 30),
                         FLD_GET(l, 31, 31));
 }
 #else
-#define _dsi_print_reset_status()
+#define _dsi_print_reset_status(x)
 #endif
 
-static inline int dsi_if_enable(bool enable)
+static inline int dsi_if_enable(struct platform_device *dsidev, bool enable)
 {
         DSSDBG("dsi_if_enable(%d)\n", enable);
 
         enable = enable ? 1 : 0;
-        REG_FLD_MOD(DSI_CTRL, enable, 0, 0); /* IF_EN */
+        REG_FLD_MOD(dsidev, DSI_CTRL, enable, 0, 0); /* IF_EN */
 
-        if (wait_for_bit_change(DSI_CTRL, 0, enable) != enable) {
+        if (wait_for_bit_change(dsidev, DSI_CTRL, 0, enable) != enable) {
                         DSSERR("Failed to set dsi_if_enable to %d\n", enable);
                         return -EIO;
         }
@@ -707,31 +1125,38 @@ static inline int dsi_if_enable(bool enable)
         return 0;
 }
 
-unsigned long dsi_get_dsi1_pll_rate(void)
+unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev)
 {
-        return dsi.current_cinfo.dsi1_pll_fclk;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->current_cinfo.dsi_pll_hsdiv_dispc_clk;
 }
 
-static unsigned long dsi_get_dsi2_pll_rate(void)
+static unsigned long dsi_get_pll_hsdiv_dsi_rate(struct platform_device *dsidev)
 {
-        return dsi.current_cinfo.dsi2_pll_fclk;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->current_cinfo.dsi_pll_hsdiv_dsi_clk;
 }
 
-static unsigned long dsi_get_txbyteclkhs(void)
+static unsigned long dsi_get_txbyteclkhs(struct platform_device *dsidev)
 {
-        return dsi.current_cinfo.clkin4ddr / 16;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->current_cinfo.clkin4ddr / 16;
 }
 
-static unsigned long dsi_fclk_rate(void)
+static unsigned long dsi_fclk_rate(struct platform_device *dsidev)
 {
         unsigned long r;
+        int dsi_module = dsi_get_dsidev_id(dsidev);
 
-        if (dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK) {
-                /* DSI FCLK source is DSS1_ALWON_FCK, which is dss1_fck */
-                r = dss_clk_get_rate(DSS_CLK_FCK1);
+        if (dss_get_dsi_clk_source(dsi_module) == OMAP_DSS_CLK_SRC_FCK) {
+                /* DSI FCLK source is DSS_CLK_FCK */
+                r = dss_clk_get_rate(DSS_CLK_FCK);
         } else {
-                /* DSI FCLK source is DSI2_PLL_FCLK */
-                r = dsi_get_dsi2_pll_rate();
+                /* DSI FCLK source is dsi_pll_hsdiv_dsi_clk */
+                r = dsi_get_pll_hsdiv_dsi_rate(dsidev);
         }
 
         return r;
@@ -739,31 +1164,50 @@ static unsigned long dsi_fclk_rate(void)
 
 static int dsi_set_lp_clk_divisor(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         unsigned long dsi_fclk;
         unsigned lp_clk_div;
         unsigned long lp_clk;
 
-        lp_clk_div = dssdev->phy.dsi.div.lp_clk_div;
+        lp_clk_div = dssdev->clocks.dsi.lp_clk_div;
 
-        if (lp_clk_div == 0 || lp_clk_div > LP_DIV_MAX)
+        if (lp_clk_div == 0 || lp_clk_div > dsi->lpdiv_max)
                 return -EINVAL;
 
-        dsi_fclk = dsi_fclk_rate();
+        dsi_fclk = dsi_fclk_rate(dsidev);
 
         lp_clk = dsi_fclk / 2 / lp_clk_div;
 
         DSSDBG("LP_CLK_DIV %u, LP_CLK %lu\n", lp_clk_div, lp_clk);
-        dsi.current_cinfo.lp_clk = lp_clk;
-        dsi.current_cinfo.lp_clk_div = lp_clk_div;
+        dsi->current_cinfo.lp_clk = lp_clk;
+        dsi->current_cinfo.lp_clk_div = lp_clk_div;
 
-        REG_FLD_MOD(DSI_CLK_CTRL, lp_clk_div, 12, 0);   /* LP_CLK_DIVISOR */
+        /* LP_CLK_DIVISOR */
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, lp_clk_div, 12, 0);
 
-        REG_FLD_MOD(DSI_CLK_CTRL, dsi_fclk > 30000000 ? 1 : 0,
-                        21, 21);                /* LP_RX_SYNCHRO_ENABLE */
+        /* LP_RX_SYNCHRO_ENABLE */
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, dsi_fclk > 30000000 ? 1 : 0, 21, 21);
 
         return 0;
 }
 
+static void dsi_enable_scp_clk(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->scp_clk_refcount++ == 0)
+                REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dsi_disable_scp_clk(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        WARN_ON(dsi->scp_clk_refcount == 0);
+        if (--dsi->scp_clk_refcount == 0)
+                REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 14, 14); /* CIO_CLK_ICG */
+}
 
 enum dsi_pll_power_state {
         DSI_PLL_POWER_OFF       = 0x0,
@@ -772,14 +1216,21 @@ enum dsi_pll_power_state {
         DSI_PLL_POWER_ON_DIV    = 0x3,
 };
 
-static int dsi_pll_power(enum dsi_pll_power_state state)
+static int dsi_pll_power(struct platform_device *dsidev,
+                enum dsi_pll_power_state state)
 {
         int t = 0;
 
-        REG_FLD_MOD(DSI_CLK_CTRL, state, 31, 30);       /* PLL_PWR_CMD */
+        /* DSI-PLL power command 0x3 is not working */
+        if (dss_has_feature(FEAT_DSI_PLL_PWR_BUG) &&
+                        state == DSI_PLL_POWER_ON_DIV)
+                state = DSI_PLL_POWER_ON_ALL;
+
+        /* PLL_PWR_CMD */
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, state, 31, 30);
 
         /* PLL_PWR_STATUS */
-        while (FLD_GET(dsi_read_reg(DSI_CLK_CTRL), 29, 28) != state) {
+        while (FLD_GET(dsi_read_reg(dsidev, DSI_CLK_CTRL), 29, 28) != state) {
                 if (++t > 1000) {
                         DSSERR("Failed to set DSI PLL power mode to %d\n",
                                         state);
@@ -792,27 +1243,31 @@ static int dsi_pll_power(enum dsi_pll_power_state state)
 }
 
 /* calculate clock rates using dividers in cinfo */
-static int dsi_calc_clock_rates(struct dsi_clock_info *cinfo)
+static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
+                struct dsi_clock_info *cinfo)
 {
-        if (cinfo->regn == 0 || cinfo->regn > REGN_MAX)
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (cinfo->regn == 0 || cinfo->regn > dsi->regn_max)
                 return -EINVAL;
 
-        if (cinfo->regm == 0 || cinfo->regm > REGM_MAX)
+        if (cinfo->regm == 0 || cinfo->regm > dsi->regm_max)
                 return -EINVAL;
 
-        if (cinfo->regm3 > REGM3_MAX)
+        if (cinfo->regm_dispc > dsi->regm_dispc_max)
                 return -EINVAL;
 
-        if (cinfo->regm4 > REGM4_MAX)
+        if (cinfo->regm_dsi > dsi->regm_dsi_max)
                 return -EINVAL;
 
-        if (cinfo->use_dss2_fck) {
-                cinfo->clkin = dss_clk_get_rate(DSS_CLK_FCK2);
+        if (cinfo->use_sys_clk) {
+                cinfo->clkin = dss_clk_get_rate(DSS_CLK_SYSCK);
                 /* XXX it is unclear if highfreq should be used
-                 * with DSS2_FCK source also */
+                 * with DSS_SYS_CLK source also */
                 cinfo->highfreq = 0;
         } else {
-                cinfo->clkin = dispc_pclk_rate();
+                cinfo->clkin = dispc_pclk_rate(dssdev->manager->id);
 
                 if (cinfo->clkin < 32000000)
                         cinfo->highfreq = 0;
@@ -822,7 +1277,7 @@ static int dsi_calc_clock_rates(struct dsi_clock_info *cinfo)
 
         cinfo->fint = cinfo->clkin / (cinfo->regn * (cinfo->highfreq ? 2 : 1));
 
-        if (cinfo->fint > FINT_MAX || cinfo->fint < FINT_MIN)
+        if (cinfo->fint > dsi->fint_max || cinfo->fint < dsi->fint_min)
                 return -EINVAL;
 
         cinfo->clkin4ddr = 2 * cinfo->regm * cinfo->fint;
@@ -830,44 +1285,49 @@ static int dsi_calc_clock_rates(struct dsi_clock_info *cinfo)
         if (cinfo->clkin4ddr > 1800 * 1000 * 1000)
                 return -EINVAL;
 
-        if (cinfo->regm3 > 0)
-                cinfo->dsi1_pll_fclk = cinfo->clkin4ddr / cinfo->regm3;
+        if (cinfo->regm_dispc > 0)
+                cinfo->dsi_pll_hsdiv_dispc_clk =
+                        cinfo->clkin4ddr / cinfo->regm_dispc;
         else
-                cinfo->dsi1_pll_fclk = 0;
+                cinfo->dsi_pll_hsdiv_dispc_clk = 0;
 
-        if (cinfo->regm4 > 0)
-                cinfo->dsi2_pll_fclk = cinfo->clkin4ddr / cinfo->regm4;
+        if (cinfo->regm_dsi > 0)
+                cinfo->dsi_pll_hsdiv_dsi_clk =
+                        cinfo->clkin4ddr / cinfo->regm_dsi;
         else
-                cinfo->dsi2_pll_fclk = 0;
+                cinfo->dsi_pll_hsdiv_dsi_clk = 0;
 
         return 0;
 }
 
-int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
-                struct dsi_clock_info *dsi_cinfo,
+int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
+                unsigned long req_pck, struct dsi_clock_info *dsi_cinfo,
                 struct dispc_clock_info *dispc_cinfo)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         struct dsi_clock_info cur, best;
         struct dispc_clock_info best_dispc;
         int min_fck_per_pck;
         int match = 0;
-        unsigned long dss_clk_fck2;
+        unsigned long dss_sys_clk, max_dss_fck;
+
+        dss_sys_clk = dss_clk_get_rate(DSS_CLK_SYSCK);
 
-        dss_clk_fck2 = dss_clk_get_rate(DSS_CLK_FCK2);
+        max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
 
-        if (req_pck == dsi.cache_req_pck &&
-                        dsi.cache_cinfo.clkin == dss_clk_fck2) {
+        if (req_pck == dsi->cache_req_pck &&
+                        dsi->cache_cinfo.clkin == dss_sys_clk) {
                 DSSDBG("DSI clock info found from cache\n");
-                *dsi_cinfo = dsi.cache_cinfo;
-                dispc_find_clk_divs(is_tft, req_pck, dsi_cinfo->dsi1_pll_fclk,
-                                dispc_cinfo);
+                *dsi_cinfo = dsi->cache_cinfo;
+                dispc_find_clk_divs(is_tft, req_pck,
+                        dsi_cinfo->dsi_pll_hsdiv_dispc_clk, dispc_cinfo);
                 return 0;
         }
 
         min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
         if (min_fck_per_pck &&
-                req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+                req_pck * min_fck_per_pck > max_dss_fck) {
                 DSSERR("Requested pixel clock not possible with the current "
                                 "OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
                                 "the constraint off.\n");
@@ -881,24 +1341,24 @@ retry:
         memset(&best_dispc, 0, sizeof(best_dispc));
 
         memset(&cur, 0, sizeof(cur));
-        cur.clkin = dss_clk_fck2;
-        cur.use_dss2_fck = 1;
+        cur.clkin = dss_sys_clk;
+        cur.use_sys_clk = 1;
         cur.highfreq = 0;
 
         /* no highfreq: 0.75MHz < Fint = clkin / regn < 2.1MHz */
         /* highfreq: 0.75MHz < Fint = clkin / (2*regn) < 2.1MHz */
         /* To reduce PLL lock time, keep Fint high (around 2 MHz) */
-        for (cur.regn = 1; cur.regn < REGN_MAX; ++cur.regn) {
+        for (cur.regn = 1; cur.regn < dsi->regn_max; ++cur.regn) {
                 if (cur.highfreq == 0)
                         cur.fint = cur.clkin / cur.regn;
                 else
                         cur.fint = cur.clkin / (2 * cur.regn);
 
-                if (cur.fint > FINT_MAX || cur.fint < FINT_MIN)
+                if (cur.fint > dsi->fint_max || cur.fint < dsi->fint_min)
                         continue;
 
                 /* DSIPHY(MHz) = (2 * regm / regn) * (clkin / (highfreq + 1)) */
-                for (cur.regm = 1; cur.regm < REGM_MAX; ++cur.regm) {
+                for (cur.regm = 1; cur.regm < dsi->regm_max; ++cur.regm) {
                         unsigned long a, b;
 
                         a = 2 * cur.regm * (cur.clkin/1000);
@@ -908,30 +1368,32 @@ retry:
                         if (cur.clkin4ddr > 1800 * 1000 * 1000)
                                 break;
 
-                        /* DSI1_PLL_FCLK(MHz) = DSIPHY(MHz) / regm3  < 173MHz */
-                        for (cur.regm3 = 1; cur.regm3 < REGM3_MAX;
-                                        ++cur.regm3) {
+                        /* dsi_pll_hsdiv_dispc_clk(MHz) =
+                         * DSIPHY(MHz) / regm_dispc  < 173MHz/186Mhz */
+                        for (cur.regm_dispc = 1; cur.regm_dispc <
+                                        dsi->regm_dispc_max; ++cur.regm_dispc) {
                                 struct dispc_clock_info cur_dispc;
-                                cur.dsi1_pll_fclk = cur.clkin4ddr / cur.regm3;
+                                cur.dsi_pll_hsdiv_dispc_clk =
+                                        cur.clkin4ddr / cur.regm_dispc;
 
                                 /* this will narrow down the search a bit,
                                  * but still give pixclocks below what was
                                  * requested */
-                                if (cur.dsi1_pll_fclk  < req_pck)
+                                if (cur.dsi_pll_hsdiv_dispc_clk  < req_pck)
                                         break;
 
-                                if (cur.dsi1_pll_fclk > DISPC_MAX_FCK)
+                                if (cur.dsi_pll_hsdiv_dispc_clk > max_dss_fck)
                                         continue;
 
                                 if (min_fck_per_pck &&
-                                        cur.dsi1_pll_fclk <
+                                        cur.dsi_pll_hsdiv_dispc_clk <
                                                 req_pck * min_fck_per_pck)
                                         continue;
 
                                 match = 1;
 
                                 dispc_find_clk_divs(is_tft, req_pck,
-                                                cur.dsi1_pll_fclk,
+                                                cur.dsi_pll_hsdiv_dispc_clk,
                                                 &cur_dispc);
 
                                 if (abs(cur_dispc.pck - req_pck) <
@@ -960,44 +1422,53 @@ found:
                 return -EINVAL;
         }
 
-        /* DSI2_PLL_FCLK (regm4) is not used */
-        best.regm4 = 0;
-        best.dsi2_pll_fclk = 0;
+        /* dsi_pll_hsdiv_dsi_clk (regm_dsi) is not used */
+        best.regm_dsi = 0;
+        best.dsi_pll_hsdiv_dsi_clk = 0;
 
         if (dsi_cinfo)
                 *dsi_cinfo = best;
         if (dispc_cinfo)
                 *dispc_cinfo = best_dispc;
 
-        dsi.cache_req_pck = req_pck;
-        dsi.cache_clk_freq = 0;
-        dsi.cache_cinfo = best;
+        dsi->cache_req_pck = req_pck;
+        dsi->cache_clk_freq = 0;
+        dsi->cache_cinfo = best;
 
         return 0;
 }
 
-int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
+int dsi_pll_set_clock_div(struct platform_device *dsidev,
+                struct dsi_clock_info *cinfo)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int r = 0;
         u32 l;
-        int f;
+        int f = 0;
+        u8 regn_start, regn_end, regm_start, regm_end;
+        u8 regm_dispc_start, regm_dispc_end, regm_dsi_start, regm_dsi_end;
 
         DSSDBGF();
 
-        dsi.current_cinfo.fint = cinfo->fint;
-        dsi.current_cinfo.clkin4ddr = cinfo->clkin4ddr;
-        dsi.current_cinfo.dsi1_pll_fclk = cinfo->dsi1_pll_fclk;
-        dsi.current_cinfo.dsi2_pll_fclk = cinfo->dsi2_pll_fclk;
+        dsi->current_cinfo.use_sys_clk = cinfo->use_sys_clk;
+        dsi->current_cinfo.highfreq = cinfo->highfreq;
 
-        dsi.current_cinfo.regn = cinfo->regn;
-        dsi.current_cinfo.regm = cinfo->regm;
-        dsi.current_cinfo.regm3 = cinfo->regm3;
-        dsi.current_cinfo.regm4 = cinfo->regm4;
+        dsi->current_cinfo.fint = cinfo->fint;
+        dsi->current_cinfo.clkin4ddr = cinfo->clkin4ddr;
+        dsi->current_cinfo.dsi_pll_hsdiv_dispc_clk =
+                        cinfo->dsi_pll_hsdiv_dispc_clk;
+        dsi->current_cinfo.dsi_pll_hsdiv_dsi_clk =
+                        cinfo->dsi_pll_hsdiv_dsi_clk;
+
+        dsi->current_cinfo.regn = cinfo->regn;
+        dsi->current_cinfo.regm = cinfo->regm;
+        dsi->current_cinfo.regm_dispc = cinfo->regm_dispc;
+        dsi->current_cinfo.regm_dsi = cinfo->regm_dsi;
 
         DSSDBG("DSI Fint %ld\n", cinfo->fint);
 
         DSSDBG("clkin (%s) rate %ld, highfreq %d\n",
-                        cinfo->use_dss2_fck ? "dss2_fck" : "pclkfree",
+                        cinfo->use_sys_clk ? "dss_sys_clk" : "pclkfree",
                         cinfo->clkin,
                         cinfo->highfreq);
 
@@ -1014,63 +1485,79 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
         DSSDBG("Clock lane freq %ld Hz\n", cinfo->clkin4ddr / 4);
 
-        DSSDBG("regm3 = %d, dsi1_pll_fclk = %lu\n",
-                        cinfo->regm3, cinfo->dsi1_pll_fclk);
-        DSSDBG("regm4 = %d, dsi2_pll_fclk = %lu\n",
-                        cinfo->regm4, cinfo->dsi2_pll_fclk);
-
-        REG_FLD_MOD(DSI_PLL_CONTROL, 0, 0, 0); /* DSI_PLL_AUTOMODE = manual */
-
-        l = dsi_read_reg(DSI_PLL_CONFIGURATION1);
+        DSSDBG("regm_dispc = %d, %s (%s) = %lu\n", cinfo->regm_dispc,
+                dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+                dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+                cinfo->dsi_pll_hsdiv_dispc_clk);
+        DSSDBG("regm_dsi = %d, %s (%s) = %lu\n", cinfo->regm_dsi,
+                dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+                dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+                cinfo->dsi_pll_hsdiv_dsi_clk);
+
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGN, &regn_start, &regn_end);
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM, &regm_start, &regm_end);
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM_DISPC, &regm_dispc_start,
+                        &regm_dispc_end);
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM_DSI, &regm_dsi_start,
+                        &regm_dsi_end);
+
+        /* DSI_PLL_AUTOMODE = manual */
+        REG_FLD_MOD(dsidev, DSI_PLL_CONTROL, 0, 0, 0);
+
+        l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION1);
         l = FLD_MOD(l, 1, 0, 0);                /* DSI_PLL_STOPMODE */
-        l = FLD_MOD(l, cinfo->regn - 1, 7, 1);  /* DSI_PLL_REGN */
-        l = FLD_MOD(l, cinfo->regm, 18, 8);     /* DSI_PLL_REGM */
-        l = FLD_MOD(l, cinfo->regm3 > 0 ? cinfo->regm3 - 1 : 0,
-                        22, 19);                /* DSI_CLOCK_DIV */
-        l = FLD_MOD(l, cinfo->regm4 > 0 ? cinfo->regm4 - 1 : 0,
-                        26, 23);                /* DSIPROTO_CLOCK_DIV */
-        dsi_write_reg(DSI_PLL_CONFIGURATION1, l);
-
-        BUG_ON(cinfo->fint < 750000 || cinfo->fint > 2100000);
-        if (cinfo->fint < 1000000)
-                f = 0x3;
-        else if (cinfo->fint < 1250000)
-                f = 0x4;
-        else if (cinfo->fint < 1500000)
-                f = 0x5;
-        else if (cinfo->fint < 1750000)
-                f = 0x6;
-        else
-                f = 0x7;
+        /* DSI_PLL_REGN */
+        l = FLD_MOD(l, cinfo->regn - 1, regn_start, regn_end);
+        /* DSI_PLL_REGM */
+        l = FLD_MOD(l, cinfo->regm, regm_start, regm_end);
+        /* DSI_CLOCK_DIV */
+        l = FLD_MOD(l, cinfo->regm_dispc > 0 ? cinfo->regm_dispc - 1 : 0,
+                        regm_dispc_start, regm_dispc_end);
+        /* DSIPROTO_CLOCK_DIV */
+        l = FLD_MOD(l, cinfo->regm_dsi > 0 ? cinfo->regm_dsi - 1 : 0,
+                        regm_dsi_start, regm_dsi_end);
+        dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION1, l);
+
+        BUG_ON(cinfo->fint < dsi->fint_min || cinfo->fint > dsi->fint_max);
+
+        if (dss_has_feature(FEAT_DSI_PLL_FREQSEL)) {
+                f = cinfo->fint < 1000000 ? 0x3 :
+                        cinfo->fint < 1250000 ? 0x4 :
+                        cinfo->fint < 1500000 ? 0x5 :
+                        cinfo->fint < 1750000 ? 0x6 :
+                        0x7;
+        }
 
-        l = dsi_read_reg(DSI_PLL_CONFIGURATION2);
-        l = FLD_MOD(l, f, 4, 1);                /* DSI_PLL_FREQSEL */
-        l = FLD_MOD(l, cinfo->use_dss2_fck ? 0 : 1,
+        l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
+
+        if (dss_has_feature(FEAT_DSI_PLL_FREQSEL))
+                l = FLD_MOD(l, f, 4, 1);        /* DSI_PLL_FREQSEL */
+        l = FLD_MOD(l, cinfo->use_sys_clk ? 0 : 1,
                         11, 11);                /* DSI_PLL_CLKSEL */
         l = FLD_MOD(l, cinfo->highfreq,
                         12, 12);                /* DSI_PLL_HIGHFREQ */
         l = FLD_MOD(l, 1, 13, 13);              /* DSI_PLL_REFEN */
         l = FLD_MOD(l, 0, 14, 14);              /* DSIPHY_CLKINEN */
         l = FLD_MOD(l, 1, 20, 20);              /* DSI_HSDIVBYPASS */
-        dsi_write_reg(DSI_PLL_CONFIGURATION2, l);
+        dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION2, l);
 
-        REG_FLD_MOD(DSI_PLL_GO, 1, 0, 0);       /* DSI_PLL_GO */
+        REG_FLD_MOD(dsidev, DSI_PLL_GO, 1, 0, 0);       /* DSI_PLL_GO */
 
-        if (wait_for_bit_change(DSI_PLL_GO, 0, 0) != 0) {
+        if (wait_for_bit_change(dsidev, DSI_PLL_GO, 0, 0) != 0) {
                 DSSERR("dsi pll go bit not going down.\n");
                 r = -EIO;
                 goto err;
         }
 
-        if (wait_for_bit_change(DSI_PLL_STATUS, 1, 1) != 1) {
+        if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 1, 1) != 1) {
                 DSSERR("cannot lock PLL\n");
                 r = -EIO;
                 goto err;
         }
 
-        dsi.pll_locked = 1;
+        dsi->pll_locked = 1;
 
-        l = dsi_read_reg(DSI_PLL_CONFIGURATION2);
+        l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
         l = FLD_MOD(l, 0, 0, 0);        /* DSI_PLL_IDLE */
         l = FLD_MOD(l, 0, 5, 5);        /* DSI_PLL_PLLLPMODE */
         l = FLD_MOD(l, 0, 6, 6);        /* DSI_PLL_LOWCURRSTBY */
@@ -1085,32 +1572,53 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
         l = FLD_MOD(l, 1, 18, 18);      /* DSI_PROTO_CLOCK_EN */
         l = FLD_MOD(l, 0, 19, 19);      /* DSI_PROTO_CLOCK_PWDN */
         l = FLD_MOD(l, 0, 20, 20);      /* DSI_HSDIVBYPASS */
-        dsi_write_reg(DSI_PLL_CONFIGURATION2, l);
+        dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION2, l);
 
         DSSDBG("PLL config done\n");
 err:
         return r;
 }
 
-int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
+int dsi_pll_init(struct platform_device *dsidev, bool enable_hsclk,
                 bool enable_hsdiv)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int r = 0;
         enum dsi_pll_power_state pwstate;
 
         DSSDBG("PLL init\n");
 
+        if (dsi->vdds_dsi_reg == NULL) {
+                struct regulator *vdds_dsi;
+
+                vdds_dsi = regulator_get(&dsi->pdev->dev, "vdds_dsi");
+
+                if (IS_ERR(vdds_dsi)) {
+                        DSSERR("can't get VDDS_DSI regulator\n");
+                        return PTR_ERR(vdds_dsi);
+                }
+
+                dsi->vdds_dsi_reg = vdds_dsi;
+        }
+
         enable_clocks(1);
-        dsi_enable_pll_clock(1);
+        dsi_enable_pll_clock(dsidev, 1);
+        /*
+         * Note: SCP CLK is not required on OMAP3, but it is required on OMAP4.
+         */
+        dsi_enable_scp_clk(dsidev);
 
-        r = regulator_enable(dsi.vdds_dsi_reg);
-        if (r)
-                goto err0;
+        if (!dsi->vdds_dsi_enabled) {
+                r = regulator_enable(dsi->vdds_dsi_reg);
+                if (r)
+                        goto err0;
+                dsi->vdds_dsi_enabled = true;
+        }
 
         /* XXX PLL does not come out of reset without this... */
         dispc_pck_free_enable(1);
 
-        if (wait_for_bit_change(DSI_PLL_STATUS, 0, 1) != 1) {
+        if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 0, 1) != 1) {
                 DSSERR("PLL not coming out of reset.\n");
                 r = -ENODEV;
                 dispc_pck_free_enable(0);
@@ -1130,7 +1638,7 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
         else
                 pwstate = DSI_PLL_POWER_OFF;
 
-        r = dsi_pll_power(pwstate);
+        r = dsi_pll_power(dsidev, pwstate);
 
         if (r)
                 goto err1;
@@ -1139,92 +1647,121 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
 
         return 0;
 err1:
-        regulator_disable(dsi.vdds_dsi_reg);
+        if (dsi->vdds_dsi_enabled) {
+                regulator_disable(dsi->vdds_dsi_reg);
+                dsi->vdds_dsi_enabled = false;
+        }
 err0:
+        dsi_disable_scp_clk(dsidev);
         enable_clocks(0);
-        dsi_enable_pll_clock(0);
+        dsi_enable_pll_clock(dsidev, 0);
         return r;
 }
 
-void dsi_pll_uninit(void)
+void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi->pll_locked = 0;
+        dsi_pll_power(dsidev, DSI_PLL_POWER_OFF);
+        if (disconnect_lanes) {
+                WARN_ON(!dsi->vdds_dsi_enabled);
+                regulator_disable(dsi->vdds_dsi_reg);
+                dsi->vdds_dsi_enabled = false;
+        }
+
+        dsi_disable_scp_clk(dsidev);
         enable_clocks(0);
-        dsi_enable_pll_clock(0);
+        dsi_enable_pll_clock(dsidev, 0);
 
-        dsi.pll_locked = 0;
-        dsi_pll_power(DSI_PLL_POWER_OFF);
-        regulator_disable(dsi.vdds_dsi_reg);
         DSSDBG("PLL uninit done\n");
 }
 
-void dsi_dump_clocks(struct seq_file *s)
+static void dsi_dump_dsidev_clocks(struct platform_device *dsidev,
+                struct seq_file *s)
 {
-        int clksel;
-        struct dsi_clock_info *cinfo = &dsi.current_cinfo;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dsi_clock_info *cinfo = &dsi->current_cinfo;
+        enum omap_dss_clk_source dispc_clk_src, dsi_clk_src;
+        int dsi_module = dsi_get_dsidev_id(dsidev);
 
-        enable_clocks(1);
+        dispc_clk_src = dss_get_dispc_clk_source();
+        dsi_clk_src = dss_get_dsi_clk_source(dsi_module);
 
-        clksel = REG_GET(DSI_PLL_CONFIGURATION2, 11, 11);
+        enable_clocks(1);
 
-        seq_printf(s,   "- DSI PLL -\n");
+        seq_printf(s,   "- DSI%d PLL -\n", dsi_module + 1);
 
         seq_printf(s,   "dsi pll source = %s\n",
-                        clksel == 0 ?
-                        "dss2_alwon_fclk" : "pclkfree");
+                        cinfo->use_sys_clk ? "dss_sys_clk" : "pclkfree");
 
         seq_printf(s,   "Fint\t\t%-16luregn %u\n", cinfo->fint, cinfo->regn);
 
         seq_printf(s,   "CLKIN4DDR\t%-16luregm %u\n",
                         cinfo->clkin4ddr, cinfo->regm);
 
-        seq_printf(s,   "dsi1_pll_fck\t%-16luregm3 %u\t(%s)\n",
-                        cinfo->dsi1_pll_fclk,
-                        cinfo->regm3,
-                        dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+        seq_printf(s,   "%s (%s)\t%-16luregm_dispc %u\t(%s)\n",
+                        dss_get_generic_clk_source_name(dispc_clk_src),
+                        dss_feat_get_clk_source_name(dispc_clk_src),
+                        cinfo->dsi_pll_hsdiv_dispc_clk,
+                        cinfo->regm_dispc,
+                        dispc_clk_src == OMAP_DSS_CLK_SRC_FCK ?
                         "off" : "on");
 
-        seq_printf(s,   "dsi2_pll_fck\t%-16luregm4 %u\t(%s)\n",
-                        cinfo->dsi2_pll_fclk,
-                        cinfo->regm4,
-                        dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+        seq_printf(s,   "%s (%s)\t%-16luregm_dsi %u\t(%s)\n",
+                        dss_get_generic_clk_source_name(dsi_clk_src),
+                        dss_feat_get_clk_source_name(dsi_clk_src),
+                        cinfo->dsi_pll_hsdiv_dsi_clk,
+                        cinfo->regm_dsi,
+                        dsi_clk_src == OMAP_DSS_CLK_SRC_FCK ?
                         "off" : "on");
 
-        seq_printf(s,   "- DSI -\n");
+        seq_printf(s,   "- DSI%d -\n", dsi_module + 1);
 
-        seq_printf(s,   "dsi fclk source = %s\n",
-                        dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-                        "dss1_alwon_fclk" : "dsi2_pll_fclk");
+        seq_printf(s,   "dsi fclk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(dsi_clk_src),
+                        dss_feat_get_clk_source_name(dsi_clk_src));
 
-        seq_printf(s,   "DSI_FCLK\t%lu\n", dsi_fclk_rate());
+        seq_printf(s,   "DSI_FCLK\t%lu\n", dsi_fclk_rate(dsidev));
 
         seq_printf(s,   "DDR_CLK\t\t%lu\n",
                         cinfo->clkin4ddr / 4);
 
-        seq_printf(s,   "TxByteClkHS\t%lu\n", dsi_get_txbyteclkhs());
+        seq_printf(s,   "TxByteClkHS\t%lu\n", dsi_get_txbyteclkhs(dsidev));
 
         seq_printf(s,   "LP_CLK\t\t%lu\n", cinfo->lp_clk);
 
-        seq_printf(s,   "VP_CLK\t\t%lu\n"
-                        "VP_PCLK\t\t%lu\n",
-                        dispc_lclk_rate(),
-                        dispc_pclk_rate());
-
         enable_clocks(0);
 }
 
+void dsi_dump_clocks(struct seq_file *s)
+{
+        struct platform_device *dsidev;
+        int i;
+
+        for  (i = 0; i < MAX_NUM_DSI; i++) {
+                dsidev = dsi_get_dsidev_from_id(i);
+                if (dsidev)
+                        dsi_dump_dsidev_clocks(dsidev, s);
+        }
+}
+
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-void dsi_dump_irqs(struct seq_file *s)
+static void dsi_dump_dsidev_irqs(struct platform_device *dsidev,
+                struct seq_file *s)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         unsigned long flags;
         struct dsi_irq_stats stats;
+        int dsi_module = dsi_get_dsidev_id(dsidev);
 
-        spin_lock_irqsave(&dsi.irq_stats_lock, flags);
+        spin_lock_irqsave(&dsi->irq_stats_lock, flags);
 
-        stats = dsi.irq_stats;
-        memset(&dsi.irq_stats, 0, sizeof(dsi.irq_stats));
-        dsi.irq_stats.last_reset = jiffies;
+        stats = dsi->irq_stats;
+        memset(&dsi->irq_stats, 0, sizeof(dsi->irq_stats));
+        dsi->irq_stats.last_reset = jiffies;
 
-        spin_unlock_irqrestore(&dsi.irq_stats_lock, flags);
+        spin_unlock_irqrestore(&dsi->irq_stats_lock, flags);
 
         seq_printf(s, "period %u ms\n",
                         jiffies_to_msecs(jiffies - stats.last_reset));
@@ -1233,7 +1770,7 @@ void dsi_dump_irqs(struct seq_file *s)
 #define PIS(x) \
         seq_printf(s, "%-20s %10d\n", #x, stats.dsi_irqs[ffs(DSI_IRQ_##x)-1]);
 
-        seq_printf(s, "-- DSI interrupts --\n");
+        seq_printf(s, "-- DSI%d interrupts --\n", dsi_module + 1);
         PIS(VC0);
         PIS(VC1);
         PIS(VC2);
@@ -1299,13 +1836,45 @@ void dsi_dump_irqs(struct seq_file *s)
         PIS(ULPSACTIVENOT_ALL1);
 #undef PIS
 }
+
+static void dsi1_dump_irqs(struct seq_file *s)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_id(0);
+
+        dsi_dump_dsidev_irqs(dsidev, s);
+}
+
+static void dsi2_dump_irqs(struct seq_file *s)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_id(1);
+
+        dsi_dump_dsidev_irqs(dsidev, s);
+}
+
+void dsi_create_debugfs_files_irq(struct dentry *debugfs_dir,
+                const struct file_operations *debug_fops)
+{
+        struct platform_device *dsidev;
+
+        dsidev = dsi_get_dsidev_from_id(0);
+        if (dsidev)
+                debugfs_create_file("dsi1_irqs", S_IRUGO, debugfs_dir,
+                        &dsi1_dump_irqs, debug_fops);
+
+        dsidev = dsi_get_dsidev_from_id(1);
+        if (dsidev)
+                debugfs_create_file("dsi2_irqs", S_IRUGO, debugfs_dir,
+                        &dsi2_dump_irqs, debug_fops);
+}
 #endif
 
-void dsi_dump_regs(struct seq_file *s)
+static void dsi_dump_dsidev_regs(struct platform_device *dsidev,
+                struct seq_file *s)
 {
-#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(r))
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(dsidev, r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
+        dsi_enable_scp_clk(dsidev);
 
         DUMPREG(DSI_REVISION);
         DUMPREG(DSI_SYSCONFIG);
@@ -1377,25 +1946,57 @@ void dsi_dump_regs(struct seq_file *s)
         DUMPREG(DSI_PLL_CONFIGURATION1);
         DUMPREG(DSI_PLL_CONFIGURATION2);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dsi_disable_scp_clk(dsidev);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
-enum dsi_complexio_power_state {
+static void dsi1_dump_regs(struct seq_file *s)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_id(0);
+
+        dsi_dump_dsidev_regs(dsidev, s);
+}
+
+static void dsi2_dump_regs(struct seq_file *s)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_id(1);
+
+        dsi_dump_dsidev_regs(dsidev, s);
+}
+
+void dsi_create_debugfs_files_reg(struct dentry *debugfs_dir,
+                const struct file_operations *debug_fops)
+{
+        struct platform_device *dsidev;
+
+        dsidev = dsi_get_dsidev_from_id(0);
+        if (dsidev)
+                debugfs_create_file("dsi1_regs", S_IRUGO, debugfs_dir,
+                        &dsi1_dump_regs, debug_fops);
+
+        dsidev = dsi_get_dsidev_from_id(1);
+        if (dsidev)
+                debugfs_create_file("dsi2_regs", S_IRUGO, debugfs_dir,
+                        &dsi2_dump_regs, debug_fops);
+}
+enum dsi_cio_power_state {
         DSI_COMPLEXIO_POWER_OFF         = 0x0,
         DSI_COMPLEXIO_POWER_ON          = 0x1,
         DSI_COMPLEXIO_POWER_ULPS        = 0x2,
 };
 
-static int dsi_complexio_power(enum dsi_complexio_power_state state)
+static int dsi_cio_power(struct platform_device *dsidev,
+                enum dsi_cio_power_state state)
 {
         int t = 0;
 
         /* PWR_CMD */
-        REG_FLD_MOD(DSI_COMPLEXIO_CFG1, state, 28, 27);
+        REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG1, state, 28, 27);
 
         /* PWR_STATUS */
-        while (FLD_GET(dsi_read_reg(DSI_COMPLEXIO_CFG1), 26, 25) != state) {
+        while (FLD_GET(dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1),
+                        26, 25) != state) {
                 if (++t > 1000) {
                         DSSERR("failed to set complexio power state to "
                                         "%d\n", state);
@@ -1407,9 +2008,70 @@ static int dsi_complexio_power(enum dsi_complexio_power_state state)
         return 0;
 }
 
-static void dsi_complexio_config(struct omap_dss_device *dssdev)
+/* Number of data lanes present on DSI interface */
+static inline int dsi_get_num_data_lanes(struct platform_device *dsidev)
 {
+        /* DSI on OMAP3 doesn't have register DSI_GNQ, set number
+         * of data lanes as 2 by default */
+        if (dss_has_feature(FEAT_DSI_GNQ))
+                return REG_GET(dsidev, DSI_GNQ, 11, 9); /* NB_DATA_LANES */
+        else
+                return 2;
+}
+
+/* Number of data lanes used by the dss device */
+static inline int dsi_get_num_data_lanes_dssdev(struct omap_dss_device *dssdev)
+{
+        int num_data_lanes = 0;
+
+        if (dssdev->phy.dsi.data1_lane != 0)
+                num_data_lanes++;
+        if (dssdev->phy.dsi.data2_lane != 0)
+                num_data_lanes++;
+        if (dssdev->phy.dsi.data3_lane != 0)
+                num_data_lanes++;
+        if (dssdev->phy.dsi.data4_lane != 0)
+                num_data_lanes++;
+
+        return num_data_lanes;
+}
+
+static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
+{
+        int val;
+
+        /* line buffer on OMAP3 is 1024 x 24bits */
+        /* XXX: for some reason using full buffer size causes
+         * considerable TX slowdown with update sizes that fill the
+         * whole buffer */
+        if (!dss_has_feature(FEAT_DSI_GNQ))
+                return 1023 * 3;
+
+        val = REG_GET(dsidev, DSI_GNQ, 14, 12); /* VP1_LINE_BUFFER_SIZE */
+
+        switch (val) {
+        case 1:
+                return 512 * 3;         /* 512x24 bits */
+        case 2:
+                return 682 * 3;         /* 682x24 bits */
+        case 3:
+                return 853 * 3;         /* 853x24 bits */
+        case 4:
+                return 1024 * 3;        /* 1024x24 bits */
+        case 5:
+                return 1194 * 3;        /* 1194x24 bits */
+        case 6:
+                return 1365 * 3;        /* 1365x24 bits */
+        default:
+                BUG();
+        }
+}
+
+static void dsi_set_lane_config(struct omap_dss_device *dssdev)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         u32 r;
+        int num_data_lanes_dssdev = dsi_get_num_data_lanes_dssdev(dssdev);
 
         int clk_lane   = dssdev->phy.dsi.clk_lane;
         int data1_lane = dssdev->phy.dsi.data1_lane;
@@ -1418,14 +2080,28 @@ static void dsi_complexio_config(struct omap_dss_device *dssdev)
         int data1_pol  = dssdev->phy.dsi.data1_pol;
         int data2_pol  = dssdev->phy.dsi.data2_pol;
 
-        r = dsi_read_reg(DSI_COMPLEXIO_CFG1);
+        r = dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1);
         r = FLD_MOD(r, clk_lane, 2, 0);
         r = FLD_MOD(r, clk_pol, 3, 3);
         r = FLD_MOD(r, data1_lane, 6, 4);
         r = FLD_MOD(r, data1_pol, 7, 7);
         r = FLD_MOD(r, data2_lane, 10, 8);
         r = FLD_MOD(r, data2_pol, 11, 11);
-        dsi_write_reg(DSI_COMPLEXIO_CFG1, r);
+        if (num_data_lanes_dssdev > 2) {
+                int data3_lane  = dssdev->phy.dsi.data3_lane;
+                int data3_pol  = dssdev->phy.dsi.data3_pol;
+
+                r = FLD_MOD(r, data3_lane, 14, 12);
+                r = FLD_MOD(r, data3_pol, 15, 15);
+        }
+        if (num_data_lanes_dssdev > 3) {
+                int data4_lane  = dssdev->phy.dsi.data4_lane;
+                int data4_pol  = dssdev->phy.dsi.data4_pol;
+
+                r = FLD_MOD(r, data4_lane, 18, 16);
+                r = FLD_MOD(r, data4_pol, 19, 19);
+        }
+        dsi_write_reg(dsidev, DSI_COMPLEXIO_CFG1, r);
 
         /* The configuration of the DSI complex I/O (number of data lanes,
            position, differential order) should not be changed while
@@ -1439,27 +2115,31 @@ static void dsi_complexio_config(struct omap_dss_device *dssdev)
            DSI complex I/O configuration is unknown. */
 
         /*
-        REG_FLD_MOD(DSI_CTRL, 1, 0, 0);
-        REG_FLD_MOD(DSI_CTRL, 0, 0, 0);
-        REG_FLD_MOD(DSI_CLK_CTRL, 1, 20, 20);
-        REG_FLD_MOD(DSI_CTRL, 1, 0, 0);
+        REG_FLD_MOD(dsidev, DSI_CTRL, 1, 0, 0);
+        REG_FLD_MOD(dsidev, DSI_CTRL, 0, 0, 0);
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20);
+        REG_FLD_MOD(dsidev, DSI_CTRL, 1, 0, 0);
         */
 }
 
-static inline unsigned ns2ddr(unsigned ns)
+static inline unsigned ns2ddr(struct platform_device *dsidev, unsigned ns)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
         /* convert time in ns to ddr ticks, rounding up */
-        unsigned long ddr_clk = dsi.current_cinfo.clkin4ddr / 4;
+        unsigned long ddr_clk = dsi->current_cinfo.clkin4ddr / 4;
         return (ns * (ddr_clk / 1000 / 1000) + 999) / 1000;
 }
 
-static inline unsigned ddr2ns(unsigned ddr)
+static inline unsigned ddr2ns(struct platform_device *dsidev, unsigned ddr)
 {
-        unsigned long ddr_clk = dsi.current_cinfo.clkin4ddr / 4;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        unsigned long ddr_clk = dsi->current_cinfo.clkin4ddr / 4;
         return ddr * 1000 * 1000 / (ddr_clk / 1000);
 }
 
-static void dsi_complexio_timings(void)
+static void dsi_cio_timings(struct platform_device *dsidev)
 {
         u32 r;
         u32 ths_prepare, ths_prepare_ths_zero, ths_trail, ths_exit;
@@ -1471,139 +2151,323 @@ static void dsi_complexio_timings(void)
         /* 1 * DDR_CLK = 2 * UI */
 
         /* min 40ns + 4*UI      max 85ns + 6*UI */
-        ths_prepare = ns2ddr(70) + 2;
+        ths_prepare = ns2ddr(dsidev, 70) + 2;
 
         /* min 145ns + 10*UI */
-        ths_prepare_ths_zero = ns2ddr(175) + 2;
+        ths_prepare_ths_zero = ns2ddr(dsidev, 175) + 2;
 
         /* min max(8*UI, 60ns+4*UI) */
-        ths_trail = ns2ddr(60) + 5;
+        ths_trail = ns2ddr(dsidev, 60) + 5;
 
         /* min 100ns */
-        ths_exit = ns2ddr(145);
+        ths_exit = ns2ddr(dsidev, 145);
 
         /* tlpx min 50n */
-        tlpx_half = ns2ddr(25);
+        tlpx_half = ns2ddr(dsidev, 25);
 
         /* min 60ns */
-        tclk_trail = ns2ddr(60) + 2;
+        tclk_trail = ns2ddr(dsidev, 60) + 2;
 
         /* min 38ns, max 95ns */
-        tclk_prepare = ns2ddr(65);
+        tclk_prepare = ns2ddr(dsidev, 65);
 
         /* min tclk-prepare + tclk-zero = 300ns */
-        tclk_zero = ns2ddr(260);
+        tclk_zero = ns2ddr(dsidev, 260);
 
         DSSDBG("ths_prepare %u (%uns), ths_prepare_ths_zero %u (%uns)\n",
-                ths_prepare, ddr2ns(ths_prepare),
-                ths_prepare_ths_zero, ddr2ns(ths_prepare_ths_zero));
+                ths_prepare, ddr2ns(dsidev, ths_prepare),
+                ths_prepare_ths_zero, ddr2ns(dsidev, ths_prepare_ths_zero));
         DSSDBG("ths_trail %u (%uns), ths_exit %u (%uns)\n",
-                        ths_trail, ddr2ns(ths_trail),
-                        ths_exit, ddr2ns(ths_exit));
+                        ths_trail, ddr2ns(dsidev, ths_trail),
+                        ths_exit, ddr2ns(dsidev, ths_exit));
 
         DSSDBG("tlpx_half %u (%uns), tclk_trail %u (%uns), "
                         "tclk_zero %u (%uns)\n",
-                        tlpx_half, ddr2ns(tlpx_half),
-                        tclk_trail, ddr2ns(tclk_trail),
-                        tclk_zero, ddr2ns(tclk_zero));
+                        tlpx_half, ddr2ns(dsidev, tlpx_half),
+                        tclk_trail, ddr2ns(dsidev, tclk_trail),
+                        tclk_zero, ddr2ns(dsidev, tclk_zero));
         DSSDBG("tclk_prepare %u (%uns)\n",
-                        tclk_prepare, ddr2ns(tclk_prepare));
+                        tclk_prepare, ddr2ns(dsidev, tclk_prepare));
 
         /* program timings */
 
-        r = dsi_read_reg(DSI_DSIPHY_CFG0);
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
         r = FLD_MOD(r, ths_prepare, 31, 24);
         r = FLD_MOD(r, ths_prepare_ths_zero, 23, 16);
         r = FLD_MOD(r, ths_trail, 15, 8);
         r = FLD_MOD(r, ths_exit, 7, 0);
-        dsi_write_reg(DSI_DSIPHY_CFG0, r);
+        dsi_write_reg(dsidev, DSI_DSIPHY_CFG0, r);
 
-        r = dsi_read_reg(DSI_DSIPHY_CFG1);
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
         r = FLD_MOD(r, tlpx_half, 22, 16);
         r = FLD_MOD(r, tclk_trail, 15, 8);
         r = FLD_MOD(r, tclk_zero, 7, 0);
-        dsi_write_reg(DSI_DSIPHY_CFG1, r);
+        dsi_write_reg(dsidev, DSI_DSIPHY_CFG1, r);
 
-        r = dsi_read_reg(DSI_DSIPHY_CFG2);
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
         r = FLD_MOD(r, tclk_prepare, 7, 0);
-        dsi_write_reg(DSI_DSIPHY_CFG2, r);
+        dsi_write_reg(dsidev, DSI_DSIPHY_CFG2, r);
+}
+
+static void dsi_cio_enable_lane_override(struct omap_dss_device *dssdev,
+                enum dsi_lane lanes)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int clk_lane   = dssdev->phy.dsi.clk_lane;
+        int data1_lane = dssdev->phy.dsi.data1_lane;
+        int data2_lane = dssdev->phy.dsi.data2_lane;
+        int data3_lane = dssdev->phy.dsi.data3_lane;
+        int data4_lane = dssdev->phy.dsi.data4_lane;
+        int clk_pol    = dssdev->phy.dsi.clk_pol;
+        int data1_pol  = dssdev->phy.dsi.data1_pol;
+        int data2_pol  = dssdev->phy.dsi.data2_pol;
+        int data3_pol  = dssdev->phy.dsi.data3_pol;
+        int data4_pol  = dssdev->phy.dsi.data4_pol;
+
+        u32 l = 0;
+        u8 lptxscp_start = dsi->num_data_lanes == 2 ? 22 : 26;
+
+        if (lanes & DSI_CLK_P)
+                l |= 1 << ((clk_lane - 1) * 2 + (clk_pol ? 0 : 1));
+        if (lanes & DSI_CLK_N)
+                l |= 1 << ((clk_lane - 1) * 2 + (clk_pol ? 1 : 0));
+
+        if (lanes & DSI_DATA1_P)
+                l |= 1 << ((data1_lane - 1) * 2 + (data1_pol ? 0 : 1));
+        if (lanes & DSI_DATA1_N)
+                l |= 1 << ((data1_lane - 1) * 2 + (data1_pol ? 1 : 0));
+
+        if (lanes & DSI_DATA2_P)
+                l |= 1 << ((data2_lane - 1) * 2 + (data2_pol ? 0 : 1));
+        if (lanes & DSI_DATA2_N)
+                l |= 1 << ((data2_lane - 1) * 2 + (data2_pol ? 1 : 0));
+
+        if (lanes & DSI_DATA3_P)
+                l |= 1 << ((data3_lane - 1) * 2 + (data3_pol ? 0 : 1));
+        if (lanes & DSI_DATA3_N)
+                l |= 1 << ((data3_lane - 1) * 2 + (data3_pol ? 1 : 0));
+
+        if (lanes & DSI_DATA4_P)
+                l |= 1 << ((data4_lane - 1) * 2 + (data4_pol ? 0 : 1));
+        if (lanes & DSI_DATA4_N)
+                l |= 1 << ((data4_lane - 1) * 2 + (data4_pol ? 1 : 0));
+        /*
+         * Bits in REGLPTXSCPDAT4TO0DXDY:
+         * 17: DY0 18: DX0
+         * 19: DY1 20: DX1
+         * 21: DY2 22: DX2
+         * 23: DY3 24: DX3
+         * 25: DY4 26: DX4
+         */
+
+        /* Set the lane override configuration */
+
+        /* REGLPTXSCPDAT4TO0DXDY */
+        REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, l, lptxscp_start, 17);
+
+        /* Enable lane override */
+
+        /* ENLPTXSCPDAT */
+        REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 1, 27, 27);
 }
 
+static void dsi_cio_disable_lane_override(struct platform_device *dsidev)
+{
+        /* Disable lane override */
+        REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 27, 27); /* ENLPTXSCPDAT */
+        /* Reset the lane override configuration */
+        /* REGLPTXSCPDAT4TO0DXDY */
+        REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 22, 17);
+}
 
-static int dsi_complexio_init(struct omap_dss_device *dssdev)
+static int dsi_cio_wait_tx_clk_esc_reset(struct omap_dss_device *dssdev)
 {
-        int r = 0;
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        int t;
+        int bits[3];
+        bool in_use[3];
+
+        if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC)) {
+                bits[0] = 28;
+                bits[1] = 27;
+                bits[2] = 26;
+        } else {
+                bits[0] = 24;
+                bits[1] = 25;
+                bits[2] = 26;
+        }
+
+        in_use[0] = false;
+        in_use[1] = false;
+        in_use[2] = false;
+
+        if (dssdev->phy.dsi.clk_lane != 0)
+                in_use[dssdev->phy.dsi.clk_lane - 1] = true;
+        if (dssdev->phy.dsi.data1_lane != 0)
+                in_use[dssdev->phy.dsi.data1_lane - 1] = true;
+        if (dssdev->phy.dsi.data2_lane != 0)
+                in_use[dssdev->phy.dsi.data2_lane - 1] = true;
+
+        t = 100000;
+        while (true) {
+                u32 l;
+                int i;
+                int ok;
+
+                l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+
+                ok = 0;
+                for (i = 0; i < 3; ++i) {
+                        if (!in_use[i] || (l & (1 << bits[i])))
+                                ok++;
+                }
+
+                if (ok == 3)
+                        break;
+
+                if (--t == 0) {
+                        for (i = 0; i < 3; ++i) {
+                                if (!in_use[i] || (l & (1 << bits[i])))
+                                        continue;
+
+                                DSSERR("CIO TXCLKESC%d domain not coming " \
+                                                "out of reset\n", i);
+                        }
+                        return -EIO;
+                }
+        }
+
+        return 0;
+}
+
+static int dsi_cio_init(struct omap_dss_device *dssdev)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int r;
+        int num_data_lanes_dssdev = dsi_get_num_data_lanes_dssdev(dssdev);
+        u32 l;
+
+        DSSDBGF();
 
-        DSSDBG("dsi_complexio_init\n");
+        if (dsi->dsi_mux_pads)
+                dsi->dsi_mux_pads(true);
 
-        /* CIO_CLK_ICG, enable L3 clk to CIO */
-        REG_FLD_MOD(DSI_CLK_CTRL, 1, 14, 14);
+        dsi_enable_scp_clk(dsidev);
 
         /* A dummy read using the SCP interface to any DSIPHY register is
          * required after DSIPHY reset to complete the reset of the DSI complex
          * I/O. */
-        dsi_read_reg(DSI_DSIPHY_CFG5);
+        dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
 
-        if (wait_for_bit_change(DSI_DSIPHY_CFG5, 30, 1) != 1) {
-                DSSERR("ComplexIO PHY not coming out of reset.\n");
-                r = -ENODEV;
-                goto err;
+        if (wait_for_bit_change(dsidev, DSI_DSIPHY_CFG5, 30, 1) != 1) {
+                DSSERR("CIO SCP Clock domain not coming out of reset.\n");
+                r = -EIO;
+                goto err_scp_clk_dom;
         }
 
-        dsi_complexio_config(dssdev);
+        dsi_set_lane_config(dssdev);
+
+        /* set TX STOP MODE timer to maximum for this operation */
+        l = dsi_read_reg(dsidev, DSI_TIMING1);
+        l = FLD_MOD(l, 1, 15, 15);      /* FORCE_TX_STOP_MODE_IO */
+        l = FLD_MOD(l, 1, 14, 14);      /* STOP_STATE_X16_IO */
+        l = FLD_MOD(l, 1, 13, 13);      /* STOP_STATE_X4_IO */
+        l = FLD_MOD(l, 0x1fff, 12, 0);  /* STOP_STATE_COUNTER_IO */
+        dsi_write_reg(dsidev, DSI_TIMING1, l);
+
+        if (dsi->ulps_enabled) {
+                u32 lane_mask = DSI_CLK_P | DSI_DATA1_P | DSI_DATA2_P;
+
+                DSSDBG("manual ulps exit\n");
+
+                /* ULPS is exited by Mark-1 state for 1ms, followed by
+                 * stop state. DSS HW cannot do this via the normal
+                 * ULPS exit sequence, as after reset the DSS HW thinks
+                 * that we are not in ULPS mode, and refuses to send the
+                 * sequence. So we need to send the ULPS exit sequence
+                 * manually.
+                 */
+
+                if (num_data_lanes_dssdev > 2)
+                        lane_mask |= DSI_DATA3_P;
+
+                if (num_data_lanes_dssdev > 3)
+                        lane_mask |= DSI_DATA4_P;
 
-        r = dsi_complexio_power(DSI_COMPLEXIO_POWER_ON);
+                dsi_cio_enable_lane_override(dssdev, lane_mask);
+        }
 
+        r = dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ON);
         if (r)
-                goto err;
+                goto err_cio_pwr;
 
-        if (wait_for_bit_change(DSI_COMPLEXIO_CFG1, 29, 1) != 1) {
-                DSSERR("ComplexIO not coming out of reset.\n");
+        if (wait_for_bit_change(dsidev, DSI_COMPLEXIO_CFG1, 29, 1) != 1) {
+                DSSERR("CIO PWR clock domain not coming out of reset.\n");
                 r = -ENODEV;
-                goto err;
+                goto err_cio_pwr_dom;
         }
 
-        if (wait_for_bit_change(DSI_COMPLEXIO_CFG1, 21, 1) != 1) {
-                DSSERR("ComplexIO LDO power down.\n");
-                r = -ENODEV;
-                goto err;
+        dsi_if_enable(dsidev, true);
+        dsi_if_enable(dsidev, false);
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
+
+        r = dsi_cio_wait_tx_clk_esc_reset(dssdev);
+        if (r)
+                goto err_tx_clk_esc_rst;
+
+        if (dsi->ulps_enabled) {
+                /* Keep Mark-1 state for 1ms (as per DSI spec) */
+                ktime_t wait = ns_to_ktime(1000 * 1000);
+                set_current_state(TASK_UNINTERRUPTIBLE);
+                schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+
+                /* Disable the override. The lanes should be set to Mark-11
+                 * state by the HW */
+                dsi_cio_disable_lane_override(dsidev);
         }
 
-        dsi_complexio_timings();
+        /* FORCE_TX_STOP_MODE_IO */
+        REG_FLD_MOD(dsidev, DSI_TIMING1, 0, 15, 15);
 
-        /*
-           The configuration of the DSI complex I/O (number of data lanes,
-           position, differential order) should not be changed while
-           DSS.DSI_CLK_CRTRL[20] LP_CLK_ENABLE bit is set to 1. For the
-           hardware to recognize a new configuration of the complex I/O (done
-           in DSS.DSI_COMPLEXIO_CFG1 register), it is recommended to follow
-           this sequence: First set the DSS.DSI_CTRL[0] IF_EN bit to 1, next
-           reset the DSS.DSI_CTRL[0] IF_EN to 0, then set DSS.DSI_CLK_CTRL[20]
-           LP_CLK_ENABLE to 1, and finally, set again the DSS.DSI_CTRL[0] IF_EN
-           bit to 1. If the sequence is not followed, the DSi complex I/O
-           configuration is undetermined.
-           */
-        dsi_if_enable(1);
-        dsi_if_enable(0);
-        REG_FLD_MOD(DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
-        dsi_if_enable(1);
-        dsi_if_enable(0);
+        dsi_cio_timings(dsidev);
+
+        dsi->ulps_enabled = false;
 
         DSSDBG("CIO init done\n");
-err:
+
+        return 0;
+
+err_tx_clk_esc_rst:
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 20, 20); /* LP_CLK_ENABLE */
+err_cio_pwr_dom:
+        dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+err_cio_pwr:
+        if (dsi->ulps_enabled)
+                dsi_cio_disable_lane_override(dsidev);
+err_scp_clk_dom:
+        dsi_disable_scp_clk(dsidev);
+        if (dsi->dsi_mux_pads)
+                dsi->dsi_mux_pads(false);
         return r;
 }
 
-static void dsi_complexio_uninit(void)
+static void dsi_cio_uninit(struct platform_device *dsidev)
 {
-        dsi_complexio_power(DSI_COMPLEXIO_POWER_OFF);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+        dsi_disable_scp_clk(dsidev);
+        if (dsi->dsi_mux_pads)
+                dsi->dsi_mux_pads(false);
 }
 
-static int _dsi_wait_reset(void)
+static int _dsi_wait_reset(struct platform_device *dsidev)
 {
         int t = 0;
 
-        while (REG_GET(DSI_SYSSTATUS, 0, 0) == 0) {
+        while (REG_GET(dsidev, DSI_SYSSTATUS, 0, 0) == 0) {
                 if (++t > 5) {
                         DSSERR("soft reset failed\n");
                         return -ENODEV;
@@ -1614,42 +2478,30 @@ static int _dsi_wait_reset(void)
         return 0;
 }
 
-static int _dsi_reset(void)
+static int _dsi_reset(struct platform_device *dsidev)
 {
         /* Soft reset */
-        REG_FLD_MOD(DSI_SYSCONFIG, 1, 1, 1);
-        return _dsi_wait_reset();
-}
-
-static void dsi_reset_tx_fifo(int channel)
-{
-        u32 mask;
-        u32 l;
-
-        /* set fifosize of the channel to 0, then return the old size */
-        l = dsi_read_reg(DSI_TX_FIFO_VC_SIZE);
-
-        mask = FLD_MASK((8 * channel) + 7, (8 * channel) + 4);
-        dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l & ~mask);
-
-        dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l);
+        REG_FLD_MOD(dsidev, DSI_SYSCONFIG, 1, 1, 1);
+        return _dsi_wait_reset(dsidev);
 }
 
-static void dsi_config_tx_fifo(enum fifo_size size1, enum fifo_size size2,
+static void dsi_config_tx_fifo(struct platform_device *dsidev,
+                enum fifo_size size1, enum fifo_size size2,
                 enum fifo_size size3, enum fifo_size size4)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u32 r = 0;
         int add = 0;
         int i;
 
-        dsi.vc[0].fifo_size = size1;
-        dsi.vc[1].fifo_size = size2;
-        dsi.vc[2].fifo_size = size3;
-        dsi.vc[3].fifo_size = size4;
+        dsi->vc[0].fifo_size = size1;
+        dsi->vc[1].fifo_size = size2;
+        dsi->vc[2].fifo_size = size3;
+        dsi->vc[3].fifo_size = size4;
 
         for (i = 0; i < 4; i++) {
                 u8 v;
-                int size = dsi.vc[i].fifo_size;
+                int size = dsi->vc[i].fifo_size;
 
                 if (add + size > 4) {
                         DSSERR("Illegal FIFO configuration\n");
@@ -1662,24 +2514,26 @@ static void dsi_config_tx_fifo(enum fifo_size size1, enum fifo_size size2,
                 add += size;
         }
 
-        dsi_write_reg(DSI_TX_FIFO_VC_SIZE, r);
+        dsi_write_reg(dsidev, DSI_TX_FIFO_VC_SIZE, r);
 }
 
-static void dsi_config_rx_fifo(enum fifo_size size1, enum fifo_size size2,
+static void dsi_config_rx_fifo(struct platform_device *dsidev,
+                enum fifo_size size1, enum fifo_size size2,
                 enum fifo_size size3, enum fifo_size size4)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u32 r = 0;
         int add = 0;
         int i;
 
-        dsi.vc[0].fifo_size = size1;
-        dsi.vc[1].fifo_size = size2;
-        dsi.vc[2].fifo_size = size3;
-        dsi.vc[3].fifo_size = size4;
+        dsi->vc[0].fifo_size = size1;
+        dsi->vc[1].fifo_size = size2;
+        dsi->vc[2].fifo_size = size3;
+        dsi->vc[3].fifo_size = size4;
 
         for (i = 0; i < 4; i++) {
                 u8 v;
-                int size = dsi.vc[i].fifo_size;
+                int size = dsi->vc[i].fifo_size;
 
                 if (add + size > 4) {
                         DSSERR("Illegal FIFO configuration\n");
@@ -1692,18 +2546,18 @@ static void dsi_config_rx_fifo(enum fifo_size size1, enum fifo_size size2,
                 add += size;
         }
 
-        dsi_write_reg(DSI_RX_FIFO_VC_SIZE, r);
+        dsi_write_reg(dsidev, DSI_RX_FIFO_VC_SIZE, r);
 }
 
-static int dsi_force_tx_stop_mode_io(void)
+static int dsi_force_tx_stop_mode_io(struct platform_device *dsidev)
 {
         u32 r;
 
-        r = dsi_read_reg(DSI_TIMING1);
+        r = dsi_read_reg(dsidev, DSI_TIMING1);
         r = FLD_MOD(r, 1, 15, 15);      /* FORCE_TX_STOP_MODE_IO */
-        dsi_write_reg(DSI_TIMING1, r);
+        dsi_write_reg(dsidev, DSI_TIMING1, r);
 
-        if (wait_for_bit_change(DSI_TIMING1, 15, 0) != 0) {
+        if (wait_for_bit_change(dsidev, DSI_TIMING1, 15, 0) != 0) {
                 DSSERR("TX_STOP bit not going down\n");
                 return -EIO;
         }
@@ -1711,16 +2565,135 @@ static int dsi_force_tx_stop_mode_io(void)
         return 0;
 }
 
-static int dsi_vc_enable(int channel, bool enable)
+static bool dsi_vc_is_enabled(struct platform_device *dsidev, int channel)
+{
+        return REG_GET(dsidev, DSI_VC_CTRL(channel), 0, 0);
+}
+
+static void dsi_packet_sent_handler_vp(void *data, u32 mask)
+{
+        struct dsi_packet_sent_handler_data *vp_data =
+                (struct dsi_packet_sent_handler_data *) data;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(vp_data->dsidev);
+        const int channel = dsi->update_channel;
+        u8 bit = dsi->te_enabled ? 30 : 31;
+
+        if (REG_GET(vp_data->dsidev, DSI_VC_TE(channel), bit, bit) == 0)
+                complete(vp_data->completion);
+}
+
+static int dsi_sync_vc_vp(struct platform_device *dsidev, int channel)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        DECLARE_COMPLETION_ONSTACK(completion);
+        struct dsi_packet_sent_handler_data vp_data = { dsidev, &completion };
+        int r = 0;
+        u8 bit;
+
+        bit = dsi->te_enabled ? 30 : 31;
+
+        r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+                &vp_data, DSI_VC_IRQ_PACKET_SENT);
+        if (r)
+                goto err0;
+
+        /* Wait for completion only if TE_EN/TE_START is still set */
+        if (REG_GET(dsidev, DSI_VC_TE(channel), bit, bit)) {
+                if (wait_for_completion_timeout(&completion,
+                                msecs_to_jiffies(10)) == 0) {
+                        DSSERR("Failed to complete previous frame transfer\n");
+                        r = -EIO;
+                        goto err1;
+                }
+        }
+
+        dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+                &vp_data, DSI_VC_IRQ_PACKET_SENT);
+
+        return 0;
+err1:
+        dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+                &vp_data, DSI_VC_IRQ_PACKET_SENT);
+err0:
+        return r;
+}
+
+static void dsi_packet_sent_handler_l4(void *data, u32 mask)
+{
+        struct dsi_packet_sent_handler_data *l4_data =
+                (struct dsi_packet_sent_handler_data *) data;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(l4_data->dsidev);
+        const int channel = dsi->update_channel;
+
+        if (REG_GET(l4_data->dsidev, DSI_VC_CTRL(channel), 5, 5) == 0)
+                complete(l4_data->completion);
+}
+
+static int dsi_sync_vc_l4(struct platform_device *dsidev, int channel)
+{
+        DECLARE_COMPLETION_ONSTACK(completion);
+        struct dsi_packet_sent_handler_data l4_data = { dsidev, &completion };
+        int r = 0;
+
+        r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+                &l4_data, DSI_VC_IRQ_PACKET_SENT);
+        if (r)
+                goto err0;
+
+        /* Wait for completion only if TX_FIFO_NOT_EMPTY is still set */
+        if (REG_GET(dsidev, DSI_VC_CTRL(channel), 5, 5)) {
+                if (wait_for_completion_timeout(&completion,
+                                msecs_to_jiffies(10)) == 0) {
+                        DSSERR("Failed to complete previous l4 transfer\n");
+                        r = -EIO;
+                        goto err1;
+                }
+        }
+
+        dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+                &l4_data, DSI_VC_IRQ_PACKET_SENT);
+
+        return 0;
+err1:
+        dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+                &l4_data, DSI_VC_IRQ_PACKET_SENT);
+err0:
+        return r;
+}
+
+static int dsi_sync_vc(struct platform_device *dsidev, int channel)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        WARN_ON(!dsi_bus_is_locked(dsidev));
+
+        WARN_ON(in_interrupt());
+
+        if (!dsi_vc_is_enabled(dsidev, channel))
+                return 0;
+
+        switch (dsi->vc[channel].mode) {
+        case DSI_VC_MODE_VP:
+                return dsi_sync_vc_vp(dsidev, channel);
+        case DSI_VC_MODE_L4:
+                return dsi_sync_vc_l4(dsidev, channel);
+        default:
+                BUG();
+        }
+}
+
+static int dsi_vc_enable(struct platform_device *dsidev, int channel,
+                bool enable)
 {
         DSSDBG("dsi_vc_enable channel %d, enable %d\n",
                         channel, enable);
 
         enable = enable ? 1 : 0;
 
-        REG_FLD_MOD(DSI_VC_CTRL(channel), enable, 0, 0);
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 0, 0);
 
-        if (wait_for_bit_change(DSI_VC_CTRL(channel), 0, enable) != enable) {
+        if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel),
+                0, enable) != enable) {
                         DSSERR("Failed to set dsi_vc_enable to %d\n", enable);
                         return -EIO;
         }
@@ -1728,13 +2701,13 @@ static int dsi_vc_enable(int channel, bool enable)
         return 0;
 }
 
-static void dsi_vc_initial_config(int channel)
+static void dsi_vc_initial_config(struct platform_device *dsidev, int channel)
 {
         u32 r;
 
         DSSDBGF("%d", channel);
 
-        r = dsi_read_reg(DSI_VC_CTRL(channel));
+        r = dsi_read_reg(dsidev, DSI_VC_CTRL(channel));
 
         if (FLD_GET(r, 15, 15)) /* VC_BUSY */
                 DSSERR("VC(%d) busy when trying to configure it!\n",
@@ -1747,87 +2720,107 @@ static void dsi_vc_initial_config(int channel)
         r = FLD_MOD(r, 1, 7, 7); /* CS_TX_EN */
         r = FLD_MOD(r, 1, 8, 8); /* ECC_TX_EN */
         r = FLD_MOD(r, 0, 9, 9); /* MODE_SPEED, high speed on/off */
+        if (dss_has_feature(FEAT_DSI_VC_OCP_WIDTH))
+                r = FLD_MOD(r, 3, 11, 10);      /* OCP_WIDTH = 32 bit */
 
         r = FLD_MOD(r, 4, 29, 27); /* DMA_RX_REQ_NB = no dma */
         r = FLD_MOD(r, 4, 23, 21); /* DMA_TX_REQ_NB = no dma */
 
-        dsi_write_reg(DSI_VC_CTRL(channel), r);
-
-        dsi.vc[channel].mode = DSI_VC_MODE_L4;
+        dsi_write_reg(dsidev, DSI_VC_CTRL(channel), r);
 }
 
-static int dsi_vc_config_l4(int channel)
+static int dsi_vc_config_l4(struct platform_device *dsidev, int channel)
 {
-        if (dsi.vc[channel].mode == DSI_VC_MODE_L4)
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->vc[channel].mode == DSI_VC_MODE_L4)
                 return 0;
 
         DSSDBGF("%d", channel);
 
-        dsi_vc_enable(channel, 0);
+        dsi_sync_vc(dsidev, channel);
+
+        dsi_vc_enable(dsidev, channel, 0);
 
         /* VC_BUSY */
-        if (wait_for_bit_change(DSI_VC_CTRL(channel), 15, 0) != 0) {
+        if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel), 15, 0) != 0) {
                 DSSERR("vc(%d) busy when trying to config for L4\n", channel);
                 return -EIO;
         }
 
-        REG_FLD_MOD(DSI_VC_CTRL(channel), 0, 1, 1); /* SOURCE, 0 = L4 */
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 0, 1, 1); /* SOURCE, 0 = L4 */
+
+        /* DCS_CMD_ENABLE */
+        if (dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC))
+                REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 0, 30, 30);
 
-        dsi_vc_enable(channel, 1);
+        dsi_vc_enable(dsidev, channel, 1);
 
-        dsi.vc[channel].mode = DSI_VC_MODE_L4;
+        dsi->vc[channel].mode = DSI_VC_MODE_L4;
 
         return 0;
 }
 
-static int dsi_vc_config_vp(int channel)
+static int dsi_vc_config_vp(struct platform_device *dsidev, int channel)
 {
-        if (dsi.vc[channel].mode == DSI_VC_MODE_VP)
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->vc[channel].mode == DSI_VC_MODE_VP)
                 return 0;
 
         DSSDBGF("%d", channel);
 
-        dsi_vc_enable(channel, 0);
+        dsi_sync_vc(dsidev, channel);
+
+        dsi_vc_enable(dsidev, channel, 0);
 
         /* VC_BUSY */
-        if (wait_for_bit_change(DSI_VC_CTRL(channel), 15, 0) != 0) {
+        if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel), 15, 0) != 0) {
                 DSSERR("vc(%d) busy when trying to config for VP\n", channel);
                 return -EIO;
         }
 
-        REG_FLD_MOD(DSI_VC_CTRL(channel), 1, 1, 1); /* SOURCE, 1 = video port */
+        /* SOURCE, 1 = video port */
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 1, 1);
+
+        /* DCS_CMD_ENABLE */
+        if (dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC))
+                REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 30, 30);
 
-        dsi_vc_enable(channel, 1);
+        dsi_vc_enable(dsidev, channel, 1);
 
-        dsi.vc[channel].mode = DSI_VC_MODE_VP;
+        dsi->vc[channel].mode = DSI_VC_MODE_VP;
 
         return 0;
 }
 
 
-void omapdss_dsi_vc_enable_hs(int channel, bool enable)
+void omapdss_dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
+                bool enable)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
         DSSDBG("dsi_vc_enable_hs(%d, %d)\n", channel, enable);
 
-        WARN_ON(!dsi_bus_is_locked());
+        WARN_ON(!dsi_bus_is_locked(dsidev));
 
-        dsi_vc_enable(channel, 0);
-        dsi_if_enable(0);
+        dsi_vc_enable(dsidev, channel, 0);
+        dsi_if_enable(dsidev, 0);
 
-        REG_FLD_MOD(DSI_VC_CTRL(channel), enable, 9, 9);
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 9, 9);
 
-        dsi_vc_enable(channel, 1);
-        dsi_if_enable(1);
+        dsi_vc_enable(dsidev, channel, 1);
+        dsi_if_enable(dsidev, 1);
 
-        dsi_force_tx_stop_mode_io();
+        dsi_force_tx_stop_mode_io(dsidev);
 }
 EXPORT_SYMBOL(omapdss_dsi_vc_enable_hs);
 
-static void dsi_vc_flush_long_data(int channel)
+static void dsi_vc_flush_long_data(struct platform_device *dsidev, int channel)
 {
-        while (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
+        while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
                 u32 val;
-                val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
+                val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
                 DSSDBG("\t\tb1 %#02x b2 %#02x b3 %#02x b4 %#02x\n",
                                 (val >> 0) & 0xff,
                                 (val >> 8) & 0xff,
@@ -1873,13 +2866,14 @@ static void dsi_show_rx_ack_with_err(u16 err)
                 DSSERR("\t\tDSI Protocol Violation\n");
 }
 
-static u16 dsi_vc_flush_receive_data(int channel)
+static u16 dsi_vc_flush_receive_data(struct platform_device *dsidev,
+                int channel)
 {
         /* RX_FIFO_NOT_EMPTY */
-        while (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
+        while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
                 u32 val;
                 u8 dt;
-                val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
+                val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
                 DSSERR("\trawval %#08x\n", val);
                 dt = FLD_GET(val, 5, 0);
                 if (dt == DSI_DT_RX_ACK_WITH_ERR) {
@@ -1894,7 +2888,7 @@ static u16 dsi_vc_flush_receive_data(int channel)
                 } else if (dt == DSI_DT_RX_DCS_LONG_READ) {
                         DSSERR("\tDCS long response, len %d\n",
                                         FLD_GET(val, 23, 8));
-                        dsi_vc_flush_long_data(channel);
+                        dsi_vc_flush_long_data(dsidev, channel);
                 } else {
                         DSSERR("\tunknown datatype 0x%02x\n", dt);
                 }
@@ -1902,74 +2896,90 @@ static u16 dsi_vc_flush_receive_data(int channel)
         return 0;
 }
 
-static int dsi_vc_send_bta(int channel)
+static int dsi_vc_send_bta(struct platform_device *dsidev, int channel)
 {
-        if (dsi.debug_write || dsi.debug_read)
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->debug_write || dsi->debug_read)
                 DSSDBG("dsi_vc_send_bta %d\n", channel);
 
-        WARN_ON(!dsi_bus_is_locked());
+        WARN_ON(!dsi_bus_is_locked(dsidev));
 
-        if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {    /* RX_FIFO_NOT_EMPTY */
+        /* RX_FIFO_NOT_EMPTY */
+        if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
                 DSSERR("rx fifo not empty when sending BTA, dumping data:\n");
-                dsi_vc_flush_receive_data(channel);
+                dsi_vc_flush_receive_data(dsidev, channel);
         }
 
-        REG_FLD_MOD(DSI_VC_CTRL(channel), 1, 6, 6); /* BTA_EN */
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 6, 6); /* BTA_EN */
 
         return 0;
 }
 
-int dsi_vc_send_bta_sync(int channel)
+int dsi_vc_send_bta_sync(struct omap_dss_device *dssdev, int channel)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        DECLARE_COMPLETION_ONSTACK(completion);
         int r = 0;
         u32 err;
 
-        INIT_COMPLETION(dsi.bta_completion);
+        r = dsi_register_isr_vc(dsidev, channel, dsi_completion_handler,
+                        &completion, DSI_VC_IRQ_BTA);
+        if (r)
+                goto err0;
 
-        dsi_vc_enable_bta_irq(channel);
+        r = dsi_register_isr(dsidev, dsi_completion_handler, &completion,
+                        DSI_IRQ_ERROR_MASK);
+        if (r)
+                goto err1;
 
-        r = dsi_vc_send_bta(channel);
+        r = dsi_vc_send_bta(dsidev, channel);
         if (r)
-                goto err;
+                goto err2;
 
-        if (wait_for_completion_timeout(&dsi.bta_completion,
+        if (wait_for_completion_timeout(&completion,
                                 msecs_to_jiffies(500)) == 0) {
                 DSSERR("Failed to receive BTA\n");
                 r = -EIO;
-                goto err;
+                goto err2;
         }
 
-        err = dsi_get_errors();
+        err = dsi_get_errors(dsidev);
         if (err) {
                 DSSERR("Error while sending BTA: %x\n", err);
                 r = -EIO;
-                goto err;
+                goto err2;
         }
-err:
-        dsi_vc_disable_bta_irq(channel);
-
+err2:
+        dsi_unregister_isr(dsidev, dsi_completion_handler, &completion,
+                        DSI_IRQ_ERROR_MASK);
+err1:
+        dsi_unregister_isr_vc(dsidev, channel, dsi_completion_handler,
+                        &completion, DSI_VC_IRQ_BTA);
+err0:
         return r;
 }
 EXPORT_SYMBOL(dsi_vc_send_bta_sync);
 
-static inline void dsi_vc_write_long_header(int channel, u8 data_type,
-                u16 len, u8 ecc)
+static inline void dsi_vc_write_long_header(struct platform_device *dsidev,
+                int channel, u8 data_type, u16 len, u8 ecc)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u32 val;
         u8 data_id;
 
-        WARN_ON(!dsi_bus_is_locked());
+        WARN_ON(!dsi_bus_is_locked(dsidev));
 
-        data_id = data_type | channel << 6;
+        data_id = data_type | dsi->vc[channel].vc_id << 6;
 
         val = FLD_VAL(data_id, 7, 0) | FLD_VAL(len, 23, 8) |
                 FLD_VAL(ecc, 31, 24);
 
-        dsi_write_reg(DSI_VC_LONG_PACKET_HEADER(channel), val);
+        dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_HEADER(channel), val);
 }
 
-static inline void dsi_vc_write_long_payload(int channel,
-                u8 b1, u8 b2, u8 b3, u8 b4)
+static inline void dsi_vc_write_long_payload(struct platform_device *dsidev,
+                int channel, u8 b1, u8 b2, u8 b3, u8 b4)
 {
         u32 val;
 
@@ -1978,34 +2988,35 @@ static inline void dsi_vc_write_long_payload(int channel,
 /*      DSSDBG("\twriting %02x, %02x, %02x, %02x (%#010x)\n",
                         b1, b2, b3, b4, val); */
 
-        dsi_write_reg(DSI_VC_LONG_PACKET_PAYLOAD(channel), val);
+        dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_PAYLOAD(channel), val);
 }
 
-static int dsi_vc_send_long(int channel, u8 data_type, u8 *data, u16 len,
-                u8 ecc)
+static int dsi_vc_send_long(struct platform_device *dsidev, int channel,
+                u8 data_type, u8 *data, u16 len, u8 ecc)
 {
         /*u32 val; */
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int i;
         u8 *p;
         int r = 0;
         u8 b1, b2, b3, b4;
 
-        if (dsi.debug_write)
+        if (dsi->debug_write)
                 DSSDBG("dsi_vc_send_long, %d bytes\n", len);
 
         /* len + header */
-        if (dsi.vc[channel].fifo_size * 32 * 4 < len + 4) {
+        if (dsi->vc[channel].fifo_size * 32 * 4 < len + 4) {
                 DSSERR("unable to send long packet: packet too long.\n");
                 return -EINVAL;
         }
 
-        dsi_vc_config_l4(channel);
+        dsi_vc_config_l4(dsidev, channel);
 
-        dsi_vc_write_long_header(channel, data_type, len, ecc);
+        dsi_vc_write_long_header(dsidev, channel, data_type, len, ecc);
 
         p = data;
         for (i = 0; i < len >> 2; i++) {
-                if (dsi.debug_write)
+                if (dsi->debug_write)
                         DSSDBG("\tsending full packet %d\n", i);
 
                 b1 = *p++;
@@ -2013,14 +3024,14 @@ static int dsi_vc_send_long(int channel, u8 data_type, u8 *data, u16 len,
                 b3 = *p++;
                 b4 = *p++;
 
-                dsi_vc_write_long_payload(channel, b1, b2, b3, b4);
+                dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, b4);
         }
 
         i = len % 4;
         if (i) {
                 b1 = 0; b2 = 0; b3 = 0;
 
-                if (dsi.debug_write)
+                if (dsi->debug_write)
                         DSSDBG("\tsending remainder bytes %d\n", i);
 
                 switch (i) {
@@ -2038,62 +3049,69 @@ static int dsi_vc_send_long(int channel, u8 data_type, u8 *data, u16 len,
                         break;
                 }
 
-                dsi_vc_write_long_payload(channel, b1, b2, b3, 0);
+                dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, 0);
         }
 
         return r;
 }
 
-static int dsi_vc_send_short(int channel, u8 data_type, u16 data, u8 ecc)
+static int dsi_vc_send_short(struct platform_device *dsidev, int channel,
+                u8 data_type, u16 data, u8 ecc)
 {
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u32 r;
         u8 data_id;
 
-        WARN_ON(!dsi_bus_is_locked());
+        WARN_ON(!dsi_bus_is_locked(dsidev));
 
-        if (dsi.debug_write)
+        if (dsi->debug_write)
                 DSSDBG("dsi_vc_send_short(ch%d, dt %#x, b1 %#x, b2 %#x)\n",
                                 channel,
                                 data_type, data & 0xff, (data >> 8) & 0xff);
 
-        dsi_vc_config_l4(channel);
+        dsi_vc_config_l4(dsidev, channel);
 
-        if (FLD_GET(dsi_read_reg(DSI_VC_CTRL(channel)), 16, 16)) {
+        if (FLD_GET(dsi_read_reg(dsidev, DSI_VC_CTRL(channel)), 16, 16)) {
                 DSSERR("ERROR FIFO FULL, aborting transfer\n");
                 return -EINVAL;
         }
 
-        data_id = data_type | channel << 6;
+        data_id = data_type | dsi->vc[channel].vc_id << 6;
 
         r = (data_id << 0) | (data << 8) | (ecc << 24);
 
-        dsi_write_reg(DSI_VC_SHORT_PACKET_HEADER(channel), r);
+        dsi_write_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel), r);
 
         return 0;
 }
 
-int dsi_vc_send_null(int channel)
+int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         u8 nullpkg[] = {0, 0, 0, 0};
-        return dsi_vc_send_long(channel, DSI_DT_NULL_PACKET, nullpkg, 4, 0);
+
+        return dsi_vc_send_long(dsidev, channel, DSI_DT_NULL_PACKET, nullpkg,
+                4, 0);
 }
 EXPORT_SYMBOL(dsi_vc_send_null);
 
-int dsi_vc_dcs_write_nosync(int channel, u8 *data, int len)
+int dsi_vc_dcs_write_nosync(struct omap_dss_device *dssdev, int channel,
+                u8 *data, int len)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         int r;
 
         BUG_ON(len == 0);
 
         if (len == 1) {
-                r = dsi_vc_send_short(channel, DSI_DT_DCS_SHORT_WRITE_0,
+                r = dsi_vc_send_short(dsidev, channel, DSI_DT_DCS_SHORT_WRITE_0,
                                 data[0], 0);
         } else if (len == 2) {
-                r = dsi_vc_send_short(channel, DSI_DT_DCS_SHORT_WRITE_1,
+                r = dsi_vc_send_short(dsidev, channel, DSI_DT_DCS_SHORT_WRITE_1,
                                 data[0] | (data[1] << 8), 0);
         } else {
                 /* 0x39 = DCS Long Write */
-                r = dsi_vc_send_long(channel, DSI_DT_DCS_LONG_WRITE,
+                r = dsi_vc_send_long(dsidev, channel, DSI_DT_DCS_LONG_WRITE,
                                 data, len, 0);
         }
 
@@ -2101,21 +3119,24 @@ int dsi_vc_dcs_write_nosync(int channel, u8 *data, int len)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_nosync);
 
-int dsi_vc_dcs_write(int channel, u8 *data, int len)
+int dsi_vc_dcs_write(struct omap_dss_device *dssdev, int channel, u8 *data,
+                int len)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         int r;
 
-        r = dsi_vc_dcs_write_nosync(channel, data, len);
+        r = dsi_vc_dcs_write_nosync(dssdev, channel, data, len);
         if (r)
                 goto err;
 
-        r = dsi_vc_send_bta_sync(channel);
+        r = dsi_vc_send_bta_sync(dssdev, channel);
         if (r)
                 goto err;
 
-        if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {    /* RX_FIFO_NOT_EMPTY */
+        /* RX_FIFO_NOT_EMPTY */
+        if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
                 DSSERR("rx fifo not empty after write, dumping data:\n");
-                dsi_vc_flush_receive_data(channel);
+                dsi_vc_flush_receive_data(dsidev, channel);
                 r = -EIO;
                 goto err;
         }
@@ -2128,47 +3149,51 @@ err:
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write);
 
-int dsi_vc_dcs_write_0(int channel, u8 dcs_cmd)
+int dsi_vc_dcs_write_0(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd)
 {
-        return dsi_vc_dcs_write(channel, &dcs_cmd, 1);
+        return dsi_vc_dcs_write(dssdev, channel, &dcs_cmd, 1);
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_0);
 
-int dsi_vc_dcs_write_1(int channel, u8 dcs_cmd, u8 param)
+int dsi_vc_dcs_write_1(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+                u8 param)
 {
         u8 buf[2];
         buf[0] = dcs_cmd;
         buf[1] = param;
-        return dsi_vc_dcs_write(channel, buf, 2);
+        return dsi_vc_dcs_write(dssdev, channel, buf, 2);
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_1);
 
-int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
+int dsi_vc_dcs_read(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+                u8 *buf, int buflen)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         u32 val;
         u8 dt;
         int r;
 
-        if (dsi.debug_read)
+        if (dsi->debug_read)
                 DSSDBG("dsi_vc_dcs_read(ch%d, dcs_cmd %x)\n", channel, dcs_cmd);
 
-        r = dsi_vc_send_short(channel, DSI_DT_DCS_READ, dcs_cmd, 0);
+        r = dsi_vc_send_short(dsidev, channel, DSI_DT_DCS_READ, dcs_cmd, 0);
         if (r)
                 goto err;
 
-        r = dsi_vc_send_bta_sync(channel);
+        r = dsi_vc_send_bta_sync(dssdev, channel);
         if (r)
                 goto err;
 
         /* RX_FIFO_NOT_EMPTY */
-        if (REG_GET(DSI_VC_CTRL(channel), 20, 20) == 0) {
+        if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20) == 0) {
                 DSSERR("RX fifo empty when trying to read.\n");
                 r = -EIO;
                 goto err;
         }
 
-        val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
-        if (dsi.debug_read)
+        val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+        if (dsi->debug_read)
                 DSSDBG("\theader: %08x\n", val);
         dt = FLD_GET(val, 5, 0);
         if (dt == DSI_DT_RX_ACK_WITH_ERR) {
@@ -2179,7 +3204,7 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
 
         } else if (dt == DSI_DT_RX_SHORT_READ_1) {
                 u8 data = FLD_GET(val, 15, 8);
-                if (dsi.debug_read)
+                if (dsi->debug_read)
                         DSSDBG("\tDCS short response, 1 byte: %02x\n", data);
 
                 if (buflen < 1) {
@@ -2192,7 +3217,7 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
                 return 1;
         } else if (dt == DSI_DT_RX_SHORT_READ_2) {
                 u16 data = FLD_GET(val, 23, 8);
-                if (dsi.debug_read)
+                if (dsi->debug_read)
                         DSSDBG("\tDCS short response, 2 byte: %04x\n", data);
 
                 if (buflen < 2) {
@@ -2207,7 +3232,7 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
         } else if (dt == DSI_DT_RX_DCS_LONG_READ) {
                 int w;
                 int len = FLD_GET(val, 23, 8);
-                if (dsi.debug_read)
+                if (dsi->debug_read)
                         DSSDBG("\tDCS long response, len %d\n", len);
 
                 if (len > buflen) {
@@ -2218,8 +3243,9 @@ int dsi_vc_dcs_read(int channel, u8 dcs_cmd, u8 *buf, int buflen)
                 /* two byte checksum ends the packet, not included in len */
                 for (w = 0; w < len + 2;) {
                         int b;
-                        val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
-                        if (dsi.debug_read)
+                        val = dsi_read_reg(dsidev,
+                                DSI_VC_SHORT_PACKET_HEADER(channel));
+                        if (dsi->debug_read)
                                 DSSDBG("\t\t%02x %02x %02x %02x\n",
                                                 (val >> 0) & 0xff,
                                                 (val >> 8) & 0xff,
@@ -2250,11 +3276,12 @@ err:
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read);
 
-int dsi_vc_dcs_read_1(int channel, u8 dcs_cmd, u8 *data)
+int dsi_vc_dcs_read_1(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+                u8 *data)
 {
         int r;
 
-        r = dsi_vc_dcs_read(channel, dcs_cmd, data, 1);
+        r = dsi_vc_dcs_read(dssdev, channel, dcs_cmd, data, 1);
 
         if (r < 0)
                 return r;
@@ -2266,12 +3293,13 @@ int dsi_vc_dcs_read_1(int channel, u8 dcs_cmd, u8 *data)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_1);
 
-int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u8 *data1, u8 *data2)
+int dsi_vc_dcs_read_2(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+                u8 *data1, u8 *data2)
 {
         u8 buf[2];
         int r;
 
-        r = dsi_vc_dcs_read(channel, dcs_cmd, buf, 2);
+        r = dsi_vc_dcs_read(dssdev, channel, dcs_cmd, buf, 2);
 
         if (r < 0)
                 return r;
@@ -2286,14 +3314,94 @@ int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u8 *data1, u8 *data2)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_2);
 
-int dsi_vc_set_max_rx_packet_size(int channel, u16 len)
+int dsi_vc_set_max_rx_packet_size(struct omap_dss_device *dssdev, int channel,
+                u16 len)
 {
-        return dsi_vc_send_short(channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+        return dsi_vc_send_short(dsidev, channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
                         len, 0);
 }
 EXPORT_SYMBOL(dsi_vc_set_max_rx_packet_size);
 
-static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
+static int dsi_enter_ulps(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        DECLARE_COMPLETION_ONSTACK(completion);
+        int r;
+
+        DSSDBGF();
+
+        WARN_ON(!dsi_bus_is_locked(dsidev));
+
+        WARN_ON(dsi->ulps_enabled);
+
+        if (dsi->ulps_enabled)
+                return 0;
+
+        if (REG_GET(dsidev, DSI_CLK_CTRL, 13, 13)) {
+                DSSERR("DDR_CLK_ALWAYS_ON enabled when entering ULPS\n");
+                return -EIO;
+        }
+
+        dsi_sync_vc(dsidev, 0);
+        dsi_sync_vc(dsidev, 1);
+        dsi_sync_vc(dsidev, 2);
+        dsi_sync_vc(dsidev, 3);
+
+        dsi_force_tx_stop_mode_io(dsidev);
+
+        dsi_vc_enable(dsidev, 0, false);
+        dsi_vc_enable(dsidev, 1, false);
+        dsi_vc_enable(dsidev, 2, false);
+        dsi_vc_enable(dsidev, 3, false);
+
+        if (REG_GET(dsidev, DSI_COMPLEXIO_CFG2, 16, 16)) {      /* HS_BUSY */
+                DSSERR("HS busy when enabling ULPS\n");
+                return -EIO;
+        }
+
+        if (REG_GET(dsidev, DSI_COMPLEXIO_CFG2, 17, 17)) {      /* LP_BUSY */
+                DSSERR("LP busy when enabling ULPS\n");
+                return -EIO;
+        }
+
+        r = dsi_register_isr_cio(dsidev, dsi_completion_handler, &completion,
+                        DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+        if (r)
+                return r;
+
+        /* Assert TxRequestEsc for data lanes and TxUlpsClk for clk lane */
+        /* LANEx_ULPS_SIG2 */
+        REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, (1 << 0) | (1 << 1) | (1 << 2),
+                7, 5);
+
+        if (wait_for_completion_timeout(&completion,
+                                msecs_to_jiffies(1000)) == 0) {
+                DSSERR("ULPS enable timeout\n");
+                r = -EIO;
+                goto err;
+        }
+
+        dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+                        DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+
+        dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ULPS);
+
+        dsi_if_enable(dsidev, false);
+
+        dsi->ulps_enabled = true;
+
+        return 0;
+
+err:
+        dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+                        DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+        return r;
+}
+
+static void dsi_set_lp_rx_timeout(struct platform_device *dsidev,
+                unsigned ticks, bool x4, bool x16)
 {
         unsigned long fck;
         unsigned long total_ticks;
@@ -2302,14 +3410,14 @@ static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
         BUG_ON(ticks > 0x1fff);
 
         /* ticks in DSI_FCK */
-        fck = dsi_fclk_rate();
+        fck = dsi_fclk_rate(dsidev);
 
-        r = dsi_read_reg(DSI_TIMING2);
+        r = dsi_read_reg(dsidev, DSI_TIMING2);
         r = FLD_MOD(r, 1, 15, 15);      /* LP_RX_TO */
         r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);    /* LP_RX_TO_X16 */
         r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);     /* LP_RX_TO_X4 */
         r = FLD_MOD(r, ticks, 12, 0);   /* LP_RX_COUNTER */
-        dsi_write_reg(DSI_TIMING2, r);
+        dsi_write_reg(dsidev, DSI_TIMING2, r);
 
         total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
 
@@ -2319,7 +3427,8 @@ static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
                         (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
+static void dsi_set_ta_timeout(struct platform_device *dsidev, unsigned ticks,
+                bool x8, bool x16)
 {
         unsigned long fck;
         unsigned long total_ticks;
@@ -2328,14 +3437,14 @@ static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
         BUG_ON(ticks > 0x1fff);
 
         /* ticks in DSI_FCK */
-        fck = dsi_fclk_rate();
+        fck = dsi_fclk_rate(dsidev);
 
-        r = dsi_read_reg(DSI_TIMING1);
+        r = dsi_read_reg(dsidev, DSI_TIMING1);
         r = FLD_MOD(r, 1, 31, 31);      /* TA_TO */
         r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);    /* TA_TO_X16 */
         r = FLD_MOD(r, x8 ? 1 : 0, 29, 29);     /* TA_TO_X8 */
         r = FLD_MOD(r, ticks, 28, 16);  /* TA_TO_COUNTER */
-        dsi_write_reg(DSI_TIMING1, r);
+        dsi_write_reg(dsidev, DSI_TIMING1, r);
 
         total_ticks = ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1);
 
@@ -2345,7 +3454,8 @@ static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
                         (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
+static void dsi_set_stop_state_counter(struct platform_device *dsidev,
+                unsigned ticks, bool x4, bool x16)
 {
         unsigned long fck;
         unsigned long total_ticks;
@@ -2354,14 +3464,14 @@ static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
         BUG_ON(ticks > 0x1fff);
 
         /* ticks in DSI_FCK */
-        fck = dsi_fclk_rate();
+        fck = dsi_fclk_rate(dsidev);
 
-        r = dsi_read_reg(DSI_TIMING1);
+        r = dsi_read_reg(dsidev, DSI_TIMING1);
         r = FLD_MOD(r, 1, 15, 15);      /* FORCE_TX_STOP_MODE_IO */
         r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);    /* STOP_STATE_X16_IO */
         r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);     /* STOP_STATE_X4_IO */
         r = FLD_MOD(r, ticks, 12, 0);   /* STOP_STATE_COUNTER_IO */
-        dsi_write_reg(DSI_TIMING1, r);
+        dsi_write_reg(dsidev, DSI_TIMING1, r);
 
         total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
 
@@ -2371,7 +3481,8 @@ static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
                         (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
+static void dsi_set_hs_tx_timeout(struct platform_device *dsidev,
+                unsigned ticks, bool x4, bool x16)
 {
         unsigned long fck;
         unsigned long total_ticks;
@@ -2380,14 +3491,14 @@ static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
         BUG_ON(ticks > 0x1fff);
 
         /* ticks in TxByteClkHS */
-        fck = dsi_get_txbyteclkhs();
+        fck = dsi_get_txbyteclkhs(dsidev);
 
-        r = dsi_read_reg(DSI_TIMING2);
+        r = dsi_read_reg(dsidev, DSI_TIMING2);
         r = FLD_MOD(r, 1, 31, 31);      /* HS_TX_TO */
         r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);    /* HS_TX_TO_X16 */
         r = FLD_MOD(r, x4 ? 1 : 0, 29, 29);     /* HS_TX_TO_X8 (4 really) */
         r = FLD_MOD(r, ticks, 28, 16);  /* HS_TX_TO_COUNTER */
-        dsi_write_reg(DSI_TIMING2, r);
+        dsi_write_reg(dsidev, DSI_TIMING2, r);
 
         total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
 
@@ -2398,24 +3509,25 @@ static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
 }
 static int dsi_proto_config(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         u32 r;
         int buswidth = 0;
 
-        dsi_config_tx_fifo(DSI_FIFO_SIZE_32,
+        dsi_config_tx_fifo(dsidev, DSI_FIFO_SIZE_32,
                         DSI_FIFO_SIZE_32,
                         DSI_FIFO_SIZE_32,
                         DSI_FIFO_SIZE_32);
 
-        dsi_config_rx_fifo(DSI_FIFO_SIZE_32,
+        dsi_config_rx_fifo(dsidev, DSI_FIFO_SIZE_32,
                         DSI_FIFO_SIZE_32,
                         DSI_FIFO_SIZE_32,
                         DSI_FIFO_SIZE_32);
 
         /* XXX what values for the timeouts? */
-        dsi_set_stop_state_counter(0x1000, false, false);
-        dsi_set_ta_timeout(0x1fff, true, true);
-        dsi_set_lp_rx_timeout(0x1fff, true, true);
-        dsi_set_hs_tx_timeout(0x1fff, true, true);
+        dsi_set_stop_state_counter(dsidev, 0x1000, false, false);
+        dsi_set_ta_timeout(dsidev, 0x1fff, true, true);
+        dsi_set_lp_rx_timeout(dsidev, 0x1fff, true, true);
+        dsi_set_hs_tx_timeout(dsidev, 0x1fff, true, true);
 
         switch (dssdev->ctrl.pixel_size) {
         case 16:
@@ -2431,7 +3543,7 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
                 BUG();
         }
 
-        r = dsi_read_reg(DSI_CTRL);
+        r = dsi_read_reg(dsidev, DSI_CTRL);
         r = FLD_MOD(r, 1, 1, 1);        /* CS_RX_EN */
         r = FLD_MOD(r, 1, 2, 2);        /* ECC_RX_EN */
         r = FLD_MOD(r, 1, 3, 3);        /* TX_FIFO_ARBITRATION */
@@ -2441,21 +3553,25 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
         r = FLD_MOD(r, 2, 13, 12);      /* LINE_BUFFER, 2 lines */
         r = FLD_MOD(r, 1, 14, 14);      /* TRIGGER_RESET_MODE */
         r = FLD_MOD(r, 1, 19, 19);      /* EOT_ENABLE */
-        r = FLD_MOD(r, 1, 24, 24);      /* DCS_CMD_ENABLE */
-        r = FLD_MOD(r, 0, 25, 25);      /* DCS_CMD_CODE, 1=start, 0=continue */
+        if (!dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC)) {
+                r = FLD_MOD(r, 1, 24, 24);      /* DCS_CMD_ENABLE */
+                /* DCS_CMD_CODE, 1=start, 0=continue */
+                r = FLD_MOD(r, 0, 25, 25);
+        }
 
-        dsi_write_reg(DSI_CTRL, r);
+        dsi_write_reg(dsidev, DSI_CTRL, r);
 
-        dsi_vc_initial_config(0);
-        dsi_vc_initial_config(1);
-        dsi_vc_initial_config(2);
-        dsi_vc_initial_config(3);
+        dsi_vc_initial_config(dsidev, 0);
+        dsi_vc_initial_config(dsidev, 1);
+        dsi_vc_initial_config(dsidev, 2);
+        dsi_vc_initial_config(dsidev, 3);
 
         return 0;
 }
 
 static void dsi_proto_timings(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         unsigned tlpx, tclk_zero, tclk_prepare, tclk_trail;
         unsigned tclk_pre, tclk_post;
         unsigned ths_prepare, ths_prepare_ths_zero, ths_zero;
@@ -2465,32 +3581,27 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
         unsigned ths_eot;
         u32 r;
 
-        r = dsi_read_reg(DSI_DSIPHY_CFG0);
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
         ths_prepare = FLD_GET(r, 31, 24);
         ths_prepare_ths_zero = FLD_GET(r, 23, 16);
         ths_zero = ths_prepare_ths_zero - ths_prepare;
         ths_trail = FLD_GET(r, 15, 8);
         ths_exit = FLD_GET(r, 7, 0);
 
-        r = dsi_read_reg(DSI_DSIPHY_CFG1);
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
         tlpx = FLD_GET(r, 22, 16) * 2;
         tclk_trail = FLD_GET(r, 15, 8);
         tclk_zero = FLD_GET(r, 7, 0);
 
-        r = dsi_read_reg(DSI_DSIPHY_CFG2);
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
         tclk_prepare = FLD_GET(r, 7, 0);
 
         /* min 8*UI */
         tclk_pre = 20;
         /* min 60ns + 52*UI */
-        tclk_post = ns2ddr(60) + 26;
+        tclk_post = ns2ddr(dsidev, 60) + 26;
 
-        /* ths_eot is 2 for 2 datalanes and 4 for 1 datalane */
-        if (dssdev->phy.dsi.data1_lane != 0 &&
-                        dssdev->phy.dsi.data2_lane != 0)
-                ths_eot = 2;
-        else
-                ths_eot = 4;
+        ths_eot = DIV_ROUND_UP(4, dsi_get_num_data_lanes_dssdev(dssdev));
 
         ddr_clk_pre = DIV_ROUND_UP(tclk_pre + tlpx + tclk_zero + tclk_prepare,
                         4);
@@ -2499,10 +3610,10 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
         BUG_ON(ddr_clk_pre == 0 || ddr_clk_pre > 255);
         BUG_ON(ddr_clk_post == 0 || ddr_clk_post > 255);
 
-        r = dsi_read_reg(DSI_CLK_TIMING);
+        r = dsi_read_reg(dsidev, DSI_CLK_TIMING);
         r = FLD_MOD(r, ddr_clk_pre, 15, 8);
         r = FLD_MOD(r, ddr_clk_post, 7, 0);
-        dsi_write_reg(DSI_CLK_TIMING, r);
+        dsi_write_reg(dsidev, DSI_CLK_TIMING, r);
 
         DSSDBG("ddr_clk_pre %u, ddr_clk_post %u\n",
                         ddr_clk_pre,
@@ -2516,7 +3627,7 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 
         r = FLD_VAL(enter_hs_mode_lat, 31, 16) |
                 FLD_VAL(exit_hs_mode_lat, 15, 0);
-        dsi_write_reg(DSI_VM_TIMING7, r);
+        dsi_write_reg(dsidev, DSI_VM_TIMING7, r);
 
         DSSDBG("enter_hs_mode_lat %u, exit_hs_mode_lat %u\n",
                         enter_hs_mode_lat, exit_hs_mode_lat);
@@ -2526,25 +3637,27 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 #define DSI_DECL_VARS \
         int __dsi_cb = 0; u32 __dsi_cv = 0;
 
-#define DSI_FLUSH(ch) \
+#define DSI_FLUSH(dsidev, ch) \
         if (__dsi_cb > 0) { \
                 /*DSSDBG("sending long packet %#010x\n", __dsi_cv);*/ \
-                dsi_write_reg(DSI_VC_LONG_PACKET_PAYLOAD(ch), __dsi_cv); \
+                dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_PAYLOAD(ch), __dsi_cv); \
                 __dsi_cb = __dsi_cv = 0; \
         }
 
-#define DSI_PUSH(ch, data) \
+#define DSI_PUSH(dsidev, ch, data) \
         do { \
                 __dsi_cv |= (data) << (__dsi_cb * 8); \
                 /*DSSDBG("cv = %#010x, cb = %d\n", __dsi_cv, __dsi_cb);*/ \
                 if (++__dsi_cb > 3) \
-                        DSI_FLUSH(ch); \
+                        DSI_FLUSH(dsidev, ch); \
         } while (0)
 
 static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
                         int x, int y, int w, int h)
 {
         /* Note: supports only 24bit colors in 32bit container */
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int first = 1;
         int fifo_stalls = 0;
         int max_dsi_packet_size;
@@ -2583,7 +3696,7 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
          * in fifo */
 
         /* When using CPU, max long packet size is TX buffer size */
-        max_dsi_packet_size = dsi.vc[0].fifo_size * 32 * 4;
+        max_dsi_packet_size = dsi->vc[0].fifo_size * 32 * 4;
 
         /* we seem to get better perf if we divide the tx fifo to half,
            and while the other half is being sent, we fill the other half
@@ -2612,35 +3725,36 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 #if 1
                 /* using fifo not empty */
                 /* TX_FIFO_NOT_EMPTY */
-                while (FLD_GET(dsi_read_reg(DSI_VC_CTRL(0)), 5, 5)) {
+                while (FLD_GET(dsi_read_reg(dsidev, DSI_VC_CTRL(0)), 5, 5)) {
                         fifo_stalls++;
                         if (fifo_stalls > 0xfffff) {
                                 DSSERR("fifo stalls overflow, pixels left %d\n",
                                                 pixels_left);
-                                dsi_if_enable(0);
+                                dsi_if_enable(dsidev, 0);
                                 return -EIO;
                         }
                         udelay(1);
                 }
 #elif 1
                 /* using fifo emptiness */
-                while ((REG_GET(DSI_TX_FIFO_VC_EMPTINESS, 7, 0)+1)*4 <
+                while ((REG_GET(dsidev, DSI_TX_FIFO_VC_EMPTINESS, 7, 0)+1)*4 <
                                 max_dsi_packet_size) {
                         fifo_stalls++;
                         if (fifo_stalls > 0xfffff) {
                                 DSSERR("fifo stalls overflow, pixels left %d\n",
                                                pixels_left);
-                                dsi_if_enable(0);
+                                dsi_if_enable(dsidev, 0);
                                 return -EIO;
                         }
                 }
 #else
-                while ((REG_GET(DSI_TX_FIFO_VC_EMPTINESS, 7, 0)+1)*4 == 0) {
+                while ((REG_GET(dsidev, DSI_TX_FIFO_VC_EMPTINESS,
+                                7, 0) + 1) * 4 == 0) {
                         fifo_stalls++;
                         if (fifo_stalls > 0xfffff) {
                                 DSSERR("fifo stalls overflow, pixels left %d\n",
                                                pixels_left);
-                                dsi_if_enable(0);
+                                dsi_if_enable(dsidev, 0);
                                 return -EIO;
                         }
                 }
@@ -2649,17 +3763,17 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 
                 pixels_left -= pixels;
 
-                dsi_vc_write_long_header(0, DSI_DT_DCS_LONG_WRITE,
+                dsi_vc_write_long_header(dsidev, 0, DSI_DT_DCS_LONG_WRITE,
                                 1 + pixels * bytespp, 0);
 
-                DSI_PUSH(0, dcs_cmd);
+                DSI_PUSH(dsidev, 0, dcs_cmd);
 
                 while (pixels-- > 0) {
                         u32 pix = __raw_readl(data++);
 
-                        DSI_PUSH(0, (pix >> 16) & 0xff);
-                        DSI_PUSH(0, (pix >> 8) & 0xff);
-                        DSI_PUSH(0, (pix >> 0) & 0xff);
+                        DSI_PUSH(dsidev, 0, (pix >> 16) & 0xff);
+                        DSI_PUSH(dsidev, 0, (pix >> 8) & 0xff);
+                        DSI_PUSH(dsidev, 0, (pix >> 0) & 0xff);
 
                         current_x++;
                         if (current_x == x+w) {
@@ -2668,7 +3782,7 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
                         }
                 }
 
-                DSI_FLUSH(0);
+                DSI_FLUSH(dsidev, 0);
         }
 
         return 0;
@@ -2677,6 +3791,8 @@ static int dsi_update_screen_l4(struct omap_dss_device *dssdev,
 static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
                 u16 x, u16 y, u16 w, u16 h)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         unsigned bytespp;
         unsigned bytespl;
         unsigned bytespf;
@@ -2685,16 +3801,13 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
         unsigned packet_len;
         u32 l;
         int r;
-        const unsigned channel = dsi.update_channel;
-        /* line buffer is 1024 x 24bits */
-        /* XXX: for some reason using full buffer size causes considerable TX
-         * slowdown with update sizes that fill the whole buffer */
-        const unsigned line_buf_size = 1023 * 3;
+        const unsigned channel = dsi->update_channel;
+        const unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
 
         DSSDBG("dsi_update_screen_dispc(%d,%d %dx%d)\n",
                         x, y, w, h);
 
-        dsi_vc_config_vp(channel);
+        dsi_vc_config_vp(dsidev, channel);
 
         bytespp = dssdev->ctrl.pixel_size / 8;
         bytespl = w * bytespp;
@@ -2715,15 +3828,16 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
                 total_len += (bytespf % packet_payload) + 1;
 
         l = FLD_VAL(total_len, 23, 0); /* TE_SIZE */
-        dsi_write_reg(DSI_VC_TE(channel), l);
+        dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
 
-        dsi_vc_write_long_header(channel, DSI_DT_DCS_LONG_WRITE, packet_len, 0);
+        dsi_vc_write_long_header(dsidev, channel, DSI_DT_DCS_LONG_WRITE,
+                packet_len, 0);
 
-        if (dsi.te_enabled)
+        if (dsi->te_enabled)
                 l = FLD_MOD(l, 1, 30, 30); /* TE_EN */
         else
                 l = FLD_MOD(l, 1, 31, 31); /* TE_START */
-        dsi_write_reg(DSI_VC_TE(channel), l);
+        dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
 
         /* We put SIDLEMODE to no-idle for the duration of the transfer,
          * because DSS interrupts are not capable of waking up the CPU and the
@@ -2733,23 +3847,23 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
          */
         dispc_disable_sidle();
 
-        dsi_perf_mark_start();
+        dsi_perf_mark_start(dsidev);
 
-        r = queue_delayed_work(dsi.workqueue, &dsi.framedone_timeout_work,
-                        msecs_to_jiffies(250));
+        r = schedule_delayed_work(&dsi->framedone_timeout_work,
+                msecs_to_jiffies(250));
         BUG_ON(r == 0);
 
         dss_start_update(dssdev);
 
-        if (dsi.te_enabled) {
+        if (dsi->te_enabled) {
                 /* disable LP_RX_TO, so that we can receive TE.  Time to wait
                  * for TE is longer than the timer allows */
-                REG_FLD_MOD(DSI_TIMING2, 0, 15, 15); /* LP_RX_TO */
+                REG_FLD_MOD(dsidev, DSI_TIMING2, 0, 15, 15); /* LP_RX_TO */
 
-                dsi_vc_send_bta(channel);
+                dsi_vc_send_bta(dsidev, channel);
 
 #ifdef DSI_CATCH_MISSING_TE
-                mod_timer(&dsi.te_timer, jiffies + msecs_to_jiffies(250));
+                mod_timer(&dsi->te_timer, jiffies + msecs_to_jiffies(250));
 #endif
         }
 }
@@ -2761,40 +3875,28 @@ static void dsi_te_timeout(unsigned long arg)
 }
 #endif
 
-static void dsi_handle_framedone(int error)
+static void dsi_handle_framedone(struct platform_device *dsidev, int error)
 {
-        const int channel = dsi.update_channel;
-
-        cancel_delayed_work(&dsi.framedone_timeout_work);
-
-        dsi_vc_disable_bta_irq(channel);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
         /* SIDLEMODE back to smart-idle */
         dispc_enable_sidle();
 
-        dsi.bta_callback = NULL;
-
-        if (dsi.te_enabled) {
+        if (dsi->te_enabled) {
                 /* enable LP_RX_TO again after the TE */
-                REG_FLD_MOD(DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
-        }
-
-        /* RX_FIFO_NOT_EMPTY */
-        if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
-                DSSERR("Received error during frame transfer:\n");
-                dsi_vc_flush_receive_data(channel);
-                if (!error)
-                        error = -EIO;
+                REG_FLD_MOD(dsidev, DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
         }
 
-        dsi.framedone_callback(error, dsi.framedone_data);
+        dsi->framedone_callback(error, dsi->framedone_data);
 
         if (!error)
-                dsi_perf_show("DISPC");
+                dsi_perf_show(dsidev, "DISPC");
 }
 
 static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 {
+        struct dsi_data *dsi = container_of(work, struct dsi_data,
+                        framedone_timeout_work.work);
         /* XXX While extremely unlikely, we could get FRAMEDONE interrupt after
          * 250ms which would conflict with this timeout work. What should be
          * done is first cancel the transfer on the HW, and then cancel the
@@ -2804,66 +3906,34 @@ static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 
         DSSERR("Framedone not received for 250ms!\n");
 
-        dsi_handle_framedone(-ETIMEDOUT);
-}
-
-static void dsi_framedone_bta_callback(void)
-{
-        dsi_handle_framedone(0);
-
-#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
-        dispc_fake_vsync_irq();
-#endif
+        dsi_handle_framedone(dsi->pdev, -ETIMEDOUT);
 }
 
 static void dsi_framedone_irq_callback(void *data, u32 mask)
 {
-        const int channel = dsi.update_channel;
-        int r;
+        struct omap_dss_device *dssdev = (struct omap_dss_device *) data;
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
         /* Note: We get FRAMEDONE when DISPC has finished sending pixels and
          * turns itself off. However, DSI still has the pixels in its buffers,
          * and is sending the data.
          */
 
-        if (dsi.te_enabled) {
-                /* enable LP_RX_TO again after the TE */
-                REG_FLD_MOD(DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
-        }
-
-        /* Send BTA after the frame. We need this for the TE to work, as TE
-         * trigger is only sent for BTAs without preceding packet. Thus we need
-         * to BTA after the pixel packets so that next BTA will cause TE
-         * trigger.
-         *
-         * This is not needed when TE is not in use, but we do it anyway to
-         * make sure that the transfer has been completed. It would be more
-         * optimal, but more complex, to wait only just before starting next
-         * transfer.
-         *
-         * Also, as there's no interrupt telling when the transfer has been
-         * done and the channel could be reconfigured, the only way is to
-         * busyloop until TE_SIZE is zero. With BTA we can do this
-         * asynchronously.
-         * */
-
-        dsi.bta_callback = dsi_framedone_bta_callback;
-
-        barrier();
+        __cancel_delayed_work(&dsi->framedone_timeout_work);
 
-        dsi_vc_enable_bta_irq(channel);
+        dsi_handle_framedone(dsidev, 0);
 
-        r = dsi_vc_send_bta(channel);
-        if (r) {
-                DSSERR("BTA after framedone failed\n");
-                dsi_handle_framedone(-EIO);
-        }
+#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
+        dispc_fake_vsync_irq();
+#endif
 }
 
 int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
                                     u16 *x, u16 *y, u16 *w, u16 *h,
                                     bool enlarge_update_area)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         u16 dw, dh;
 
         dssdev->driver->get_resolution(dssdev, &dw, &dh);
@@ -2883,12 +3953,12 @@ int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
         if (*w == 0 || *h == 0)
                 return -EINVAL;
 
-        dsi_perf_mark_setup();
+        dsi_perf_mark_setup(dsidev);
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
                 dss_setup_partial_planes(dssdev, x, y, w, h,
                                 enlarge_update_area);
-                dispc_set_lcd_size(*w, *h);
+                dispc_set_lcd_size(dssdev->manager->id, *w, *h);
         }
 
         return 0;
@@ -2900,7 +3970,10 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
                 u16 x, u16 y, u16 w, u16 h,
                 void (*callback)(int, void *), void *data)
 {
-        dsi.update_channel = channel;
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi->update_channel = channel;
 
         /* OMAP DSS cannot send updates of odd widths.
          * omap_dsi_prepare_update() makes the widths even, but add a BUG_ON
@@ -2909,14 +3982,14 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
         BUG_ON(x % 2 == 1);
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-                dsi.framedone_callback = callback;
-                dsi.framedone_data = data;
+                dsi->framedone_callback = callback;
+                dsi->framedone_data = data;
 
-                dsi.update_region.x = x;
-                dsi.update_region.y = y;
-                dsi.update_region.w = w;
-                dsi.update_region.h = h;
-                dsi.update_region.device = dssdev;
+                dsi->update_region.x = x;
+                dsi->update_region.y = y;
+                dsi->update_region.w = w;
+                dsi->update_region.h = h;
+                dsi->update_region.device = dssdev;
 
                 dsi_update_screen_dispc(dssdev, x, y, w, h);
         } else {
@@ -2926,7 +3999,7 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
                 if (r)
                         return r;
 
-                dsi_perf_show("L4");
+                dsi_perf_show(dsidev, "L4");
                 callback(0, data);
         }
 
@@ -2939,20 +4012,26 @@ EXPORT_SYMBOL(omap_dsi_update);
 static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 {
         int r;
+        u32 irq;
+
+        irq = dssdev->manager->id == OMAP_DSS_CHANNEL_LCD ?
+                DISPC_IRQ_FRAMEDONE : DISPC_IRQ_FRAMEDONE2;
 
-        r = omap_dispc_register_isr(dsi_framedone_irq_callback, NULL,
-                        DISPC_IRQ_FRAMEDONE);
+        r = omap_dispc_register_isr(dsi_framedone_irq_callback, (void *) dssdev,
+                        irq);
         if (r) {
                 DSSERR("can't get FRAMEDONE irq\n");
                 return r;
         }
 
-        dispc_set_lcd_display_type(OMAP_DSS_LCD_DISPLAY_TFT);
+        dispc_set_lcd_display_type(dssdev->manager->id,
+                        OMAP_DSS_LCD_DISPLAY_TFT);
 
-        dispc_set_parallel_interface_mode(OMAP_DSS_PARALLELMODE_DSI);
-        dispc_enable_fifohandcheck(1);
+        dispc_set_parallel_interface_mode(dssdev->manager->id,
+                        OMAP_DSS_PARALLELMODE_DSI);
+        dispc_enable_fifohandcheck(dssdev->manager->id, 1);
 
-        dispc_set_tft_data_lines(dssdev->ctrl.pixel_size);
+        dispc_set_tft_data_lines(dssdev->manager->id, dssdev->ctrl.pixel_size);
 
         {
                 struct omap_video_timings timings = {
@@ -2964,7 +4043,7 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
                         .vbp            = 0,
                 };
 
-                dispc_set_lcd_timings(&timings);
+                dispc_set_lcd_timings(dssdev->manager->id, &timings);
         }
 
         return 0;
@@ -2972,28 +4051,34 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 
 static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
 {
-        omap_dispc_unregister_isr(dsi_framedone_irq_callback, NULL,
-                        DISPC_IRQ_FRAMEDONE);
+        u32 irq;
+
+        irq = dssdev->manager->id == OMAP_DSS_CHANNEL_LCD ?
+                DISPC_IRQ_FRAMEDONE : DISPC_IRQ_FRAMEDONE2;
+
+        omap_dispc_unregister_isr(dsi_framedone_irq_callback, (void *) dssdev,
+                        irq);
 }
 
 static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dsi_clock_info cinfo;
         int r;
 
-        /* we always use DSS2_FCK as input clock */
-        cinfo.use_dss2_fck = true;
-        cinfo.regn  = dssdev->phy.dsi.div.regn;
-        cinfo.regm  = dssdev->phy.dsi.div.regm;
-        cinfo.regm3 = dssdev->phy.dsi.div.regm3;
-        cinfo.regm4 = dssdev->phy.dsi.div.regm4;
-        r = dsi_calc_clock_rates(&cinfo);
+        /* we always use DSS_CLK_SYSCK as input clock */
+        cinfo.use_sys_clk = true;
+        cinfo.regn  = dssdev->clocks.dsi.regn;
+        cinfo.regm  = dssdev->clocks.dsi.regm;
+        cinfo.regm_dispc = dssdev->clocks.dsi.regm_dispc;
+        cinfo.regm_dsi = dssdev->clocks.dsi.regm_dsi;
+        r = dsi_calc_clock_rates(dssdev, &cinfo);
         if (r) {
                 DSSERR("Failed to calc dsi clocks\n");
                 return r;
         }
 
-        r = dsi_pll_set_clock_div(&cinfo);
+        r = dsi_pll_set_clock_div(dsidev, &cinfo);
         if (r) {
                 DSSERR("Failed to set dsi clocks\n");
                 return r;
@@ -3004,14 +4089,15 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 
 static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
         struct dispc_clock_info dispc_cinfo;
         int r;
         unsigned long long fck;
 
-        fck = dsi_get_dsi1_pll_rate();
+        fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
 
-        dispc_cinfo.lck_div = dssdev->phy.dsi.div.lck_div;
-        dispc_cinfo.pck_div = dssdev->phy.dsi.div.pck_div;
+        dispc_cinfo.lck_div = dssdev->clocks.dispc.channel.lck_div;
+        dispc_cinfo.pck_div = dssdev->clocks.dispc.channel.pck_div;
 
         r = dispc_calc_clock_rates(fck, &dispc_cinfo);
         if (r) {
@@ -3019,7 +4105,7 @@ static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
                 return r;
         }
 
-        r = dispc_set_clock_div(&dispc_cinfo);
+        r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
         if (r) {
                 DSSERR("Failed to set dispc clocks\n");
                 return r;
@@ -3030,11 +4116,11 @@ static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
 
 static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        int dsi_module = dsi_get_dsidev_id(dsidev);
         int r;
 
-        _dsi_print_reset_status();
-
-        r = dsi_pll_init(dssdev, true, true);
+        r = dsi_pll_init(dsidev, true, true);
         if (r)
                 goto err0;
 
@@ -3042,8 +4128,10 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
         if (r)
                 goto err1;
 
-        dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
-        dss_select_dsi_clk_source(DSS_SRC_DSI2_PLL_FCLK);
+        dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
+        dss_select_dsi_clk_source(dsi_module, dssdev->clocks.dsi.dsi_fclk_src);
+        dss_select_lcd_clk_source(dssdev->manager->id,
+                        dssdev->clocks.dispc.channel.lcd_clk_src);
 
         DSSDBG("PLL OK\n");
 
@@ -3051,82 +4139,92 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
         if (r)
                 goto err2;
 
-        r = dsi_complexio_init(dssdev);
+        r = dsi_cio_init(dssdev);
         if (r)
                 goto err2;
 
-        _dsi_print_reset_status();
+        _dsi_print_reset_status(dsidev);
 
         dsi_proto_timings(dssdev);
         dsi_set_lp_clk_divisor(dssdev);
 
         if (1)
-                _dsi_print_reset_status();
+                _dsi_print_reset_status(dsidev);
 
         r = dsi_proto_config(dssdev);
         if (r)
                 goto err3;
 
         /* enable interface */
-        dsi_vc_enable(0, 1);
-        dsi_vc_enable(1, 1);
-        dsi_vc_enable(2, 1);
-        dsi_vc_enable(3, 1);
-        dsi_if_enable(1);
-        dsi_force_tx_stop_mode_io();
+        dsi_vc_enable(dsidev, 0, 1);
+        dsi_vc_enable(dsidev, 1, 1);
+        dsi_vc_enable(dsidev, 2, 1);
+        dsi_vc_enable(dsidev, 3, 1);
+        dsi_if_enable(dsidev, 1);
+        dsi_force_tx_stop_mode_io(dsidev);
 
         return 0;
 err3:
-        dsi_complexio_uninit();
+        dsi_cio_uninit(dsidev);
 err2:
-        dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-        dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+        dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+        dss_select_dsi_clk_source(dsi_module, OMAP_DSS_CLK_SRC_FCK);
 err1:
-        dsi_pll_uninit();
+        dsi_pll_uninit(dsidev, true);
 err0:
         return r;
 }
 
-static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev)
+static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
+                bool disconnect_lanes, bool enter_ulps)
 {
-        /* 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);
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int dsi_module = dsi_get_dsidev_id(dsidev);
+
+        if (enter_ulps && !dsi->ulps_enabled)
+                dsi_enter_ulps(dsidev);
 
-        dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-        dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-        dsi_complexio_uninit();
-        dsi_pll_uninit();
+        /* disable interface */
+        dsi_if_enable(dsidev, 0);
+        dsi_vc_enable(dsidev, 0, 0);
+        dsi_vc_enable(dsidev, 1, 0);
+        dsi_vc_enable(dsidev, 2, 0);
+        dsi_vc_enable(dsidev, 3, 0);
+
+        dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+        dss_select_dsi_clk_source(dsi_module, OMAP_DSS_CLK_SRC_FCK);
+        dsi_cio_uninit(dsidev);
+        dsi_pll_uninit(dsidev, disconnect_lanes);
 }
 
-static int dsi_core_init(void)
+static int dsi_core_init(struct platform_device *dsidev)
 {
         /* Autoidle */
-        REG_FLD_MOD(DSI_SYSCONFIG, 1, 0, 0);
+        REG_FLD_MOD(dsidev, DSI_SYSCONFIG, 1, 0, 0);
 
         /* ENWAKEUP */
-        REG_FLD_MOD(DSI_SYSCONFIG, 1, 2, 2);
+        REG_FLD_MOD(dsidev, DSI_SYSCONFIG, 1, 2, 2);
 
         /* SIDLEMODE smart-idle */
-        REG_FLD_MOD(DSI_SYSCONFIG, 2, 4, 3);
+        REG_FLD_MOD(dsidev, DSI_SYSCONFIG, 2, 4, 3);
 
-        _dsi_initialize_irq();
+        _dsi_initialize_irq(dsidev);
 
         return 0;
 }
 
 int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
         int r = 0;
 
         DSSDBG("dsi_display_enable\n");
 
-        WARN_ON(!dsi_bus_is_locked());
+        WARN_ON(!dsi_bus_is_locked(dsidev));
 
-        mutex_lock(&dsi.lock);
+        mutex_lock(&dsi->lock);
 
         r = omap_dss_start_device(dssdev);
         if (r) {
@@ -3135,13 +4233,13 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
         }
 
         enable_clocks(1);
-        dsi_enable_pll_clock(1);
+        dsi_enable_pll_clock(dsidev, 1);
 
-        r = _dsi_reset();
+        r = _dsi_reset(dsidev);
         if (r)
                 goto err1;
 
-        dsi_core_init();
+        dsi_core_init(dsidev);
 
         r = dsi_display_init_dispc(dssdev);
         if (r)
@@ -3151,7 +4249,7 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
         if (r)
                 goto err2;
 
-        mutex_unlock(&dsi.lock);
+        mutex_unlock(&dsi->lock);
 
         return 0;
 
@@ -3159,39 +4257,46 @@ err2:
         dsi_display_uninit_dispc(dssdev);
 err1:
         enable_clocks(0);
-        dsi_enable_pll_clock(0);
+        dsi_enable_pll_clock(dsidev, 0);
         omap_dss_stop_device(dssdev);
 err0:
-        mutex_unlock(&dsi.lock);
+        mutex_unlock(&dsi->lock);
         DSSDBG("dsi_display_enable FAILED\n");
         return r;
 }
 EXPORT_SYMBOL(omapdss_dsi_display_enable);
 
-void omapdss_dsi_display_disable(struct omap_dss_device *dssdev)
+void omapdss_dsi_display_disable(struct omap_dss_device *dssdev,
+                bool disconnect_lanes, bool enter_ulps)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
         DSSDBG("dsi_display_disable\n");
 
-        WARN_ON(!dsi_bus_is_locked());
+        WARN_ON(!dsi_bus_is_locked(dsidev));
 
-        mutex_lock(&dsi.lock);
+        mutex_lock(&dsi->lock);
 
         dsi_display_uninit_dispc(dssdev);
 
-        dsi_display_uninit_dsi(dssdev);
+        dsi_display_uninit_dsi(dssdev, disconnect_lanes, enter_ulps);
 
         enable_clocks(0);
-        dsi_enable_pll_clock(0);
+        dsi_enable_pll_clock(dsidev, 0);
 
         omap_dss_stop_device(dssdev);
 
-        mutex_unlock(&dsi.lock);
+        mutex_unlock(&dsi->lock);
 }
 EXPORT_SYMBOL(omapdss_dsi_display_disable);
 
 int omapdss_dsi_enable_te(struct omap_dss_device *dssdev, bool enable)
 {
-        dsi.te_enabled = enable;
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi->te_enabled = enable;
         return 0;
 }
 EXPORT_SYMBOL(omapdss_dsi_enable_te);
@@ -3211,97 +4316,284 @@ void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
 
 int dsi_init_display(struct omap_dss_device *dssdev)
 {
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int dsi_module = dsi_get_dsidev_id(dsidev);
+
         DSSDBG("DSI init\n");
 
         /* XXX these should be figured out dynamically */
         dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
                 OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
 
-        dsi.vc[0].dssdev = dssdev;
-        dsi.vc[1].dssdev = dssdev;
+        if (dsi->vdds_dsi_reg == NULL) {
+                struct regulator *vdds_dsi;
+
+                vdds_dsi = regulator_get(&dsi->pdev->dev, "vdds_dsi");
+
+                if (IS_ERR(vdds_dsi)) {
+                        DSSERR("can't get VDDS_DSI regulator\n");
+                        return PTR_ERR(vdds_dsi);
+                }
+
+                dsi->vdds_dsi_reg = vdds_dsi;
+        }
+
+        if (dsi_get_num_data_lanes_dssdev(dssdev) > dsi->num_data_lanes) {
+                DSSERR("DSI%d can't support more than %d data lanes\n",
+                        dsi_module + 1, dsi->num_data_lanes);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+int omap_dsi_request_vc(struct omap_dss_device *dssdev, int *channel)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+                if (!dsi->vc[i].dssdev) {
+                        dsi->vc[i].dssdev = dssdev;
+                        *channel = i;
+                        return 0;
+                }
+        }
+
+        DSSERR("cannot get VC for display %s", dssdev->name);
+        return -ENOSPC;
+}
+EXPORT_SYMBOL(omap_dsi_request_vc);
+
+int omap_dsi_set_vc_id(struct omap_dss_device *dssdev, int channel, int vc_id)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (vc_id < 0 || vc_id > 3) {
+                DSSERR("VC ID out of range\n");
+                return -EINVAL;
+        }
+
+        if (channel < 0 || channel > 3) {
+                DSSERR("Virtual Channel out of range\n");
+                return -EINVAL;
+        }
+
+        if (dsi->vc[channel].dssdev != dssdev) {
+                DSSERR("Virtual Channel not allocated to display %s\n",
+                        dssdev->name);
+                return -EINVAL;
+        }
+
+        dsi->vc[channel].vc_id = vc_id;
 
         return 0;
 }
+EXPORT_SYMBOL(omap_dsi_set_vc_id);
+
+void omap_dsi_release_vc(struct omap_dss_device *dssdev, int channel)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if ((channel >= 0 && channel <= 3) &&
+                dsi->vc[channel].dssdev == dssdev) {
+                dsi->vc[channel].dssdev = NULL;
+                dsi->vc[channel].vc_id = 0;
+        }
+}
+EXPORT_SYMBOL(omap_dsi_release_vc);
+
+void dsi_wait_pll_hsdiv_dispc_active(struct platform_device *dsidev)
+{
+        if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 7, 1) != 1)
+                DSSERR("%s (%s) not active\n",
+                        dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+                        dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC));
+}
 
-void dsi_wait_dsi1_pll_active(void)
+void dsi_wait_pll_hsdiv_dsi_active(struct platform_device *dsidev)
 {
-        if (wait_for_bit_change(DSI_PLL_STATUS, 7, 1) != 1)
-                DSSERR("DSI1 PLL clock not active\n");
+        if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 8, 1) != 1)
+                DSSERR("%s (%s) not active\n",
+                        dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+                        dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI));
 }
 
-void dsi_wait_dsi2_pll_active(void)
+static void dsi_calc_clock_param_ranges(struct platform_device *dsidev)
 {
-        if (wait_for_bit_change(DSI_PLL_STATUS, 8, 1) != 1)
-                DSSERR("DSI2 PLL clock not active\n");
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi->regn_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGN);
+        dsi->regm_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM);
+        dsi->regm_dispc_max =
+                dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM_DISPC);
+        dsi->regm_dsi_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM_DSI);
+        dsi->fint_min = dss_feat_get_param_min(FEAT_PARAM_DSIPLL_FINT);
+        dsi->fint_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_FINT);
+        dsi->lpdiv_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_LPDIV);
 }
 
-int dsi_init(struct platform_device *pdev)
+static int dsi_init(struct platform_device *dsidev)
 {
+        struct omap_display_platform_data *dss_plat_data;
+        struct omap_dss_board_info *board_info;
         u32 rev;
-        int r;
+        int r, i, dsi_module = dsi_get_dsidev_id(dsidev);
+        struct resource *dsi_mem;
+        struct dsi_data *dsi;
 
-        spin_lock_init(&dsi.errors_lock);
-        dsi.errors = 0;
+        dsi = kzalloc(sizeof(*dsi), GFP_KERNEL);
+        if (!dsi) {
+                r = -ENOMEM;
+                goto err0;
+        }
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-        spin_lock_init(&dsi.irq_stats_lock);
-        dsi.irq_stats.last_reset = jiffies;
-#endif
+        dsi->pdev = dsidev;
+        dsi_pdev_map[dsi_module] = dsidev;
+        dev_set_drvdata(&dsidev->dev, dsi);
 
-        init_completion(&dsi.bta_completion);
+        dss_plat_data = dsidev->dev.platform_data;
+        board_info = dss_plat_data->board_data;
+        dsi->dsi_mux_pads = board_info->dsi_mux_pads;
 
-        mutex_init(&dsi.lock);
-        sema_init(&dsi.bus_lock, 1);
+        spin_lock_init(&dsi->irq_lock);
+        spin_lock_init(&dsi->errors_lock);
+        dsi->errors = 0;
 
-        dsi.workqueue = create_singlethread_workqueue("dsi");
-        if (dsi.workqueue == NULL)
-                return -ENOMEM;
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+        spin_lock_init(&dsi->irq_stats_lock);
+        dsi->irq_stats.last_reset = jiffies;
+#endif
 
-        INIT_DELAYED_WORK_DEFERRABLE(&dsi.framedone_timeout_work,
+        mutex_init(&dsi->lock);
+        sema_init(&dsi->bus_lock, 1);
+
+        INIT_DELAYED_WORK_DEFERRABLE(&dsi->framedone_timeout_work,
                         dsi_framedone_timeout_work_callback);
 
 #ifdef DSI_CATCH_MISSING_TE
-        init_timer(&dsi.te_timer);
-        dsi.te_timer.function = dsi_te_timeout;
-        dsi.te_timer.data = 0;
+        init_timer(&dsi->te_timer);
+        dsi->te_timer.function = dsi_te_timeout;
+        dsi->te_timer.data = 0;
 #endif
-        dsi.base = ioremap(DSI_BASE, DSI_SZ_REGS);
-        if (!dsi.base) {
+        dsi_mem = platform_get_resource(dsi->pdev, IORESOURCE_MEM, 0);
+        if (!dsi_mem) {
+                DSSERR("can't get IORESOURCE_MEM DSI\n");
+                r = -EINVAL;
+                goto err1;
+        }
+        dsi->base = ioremap(dsi_mem->start, resource_size(dsi_mem));
+        if (!dsi->base) {
                 DSSERR("can't ioremap DSI\n");
                 r = -ENOMEM;
                 goto err1;
         }
+        dsi->irq = platform_get_irq(dsi->pdev, 0);
+        if (dsi->irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                r = -ENODEV;
+                goto err2;
+        }
 
-        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);
+        r = request_irq(dsi->irq, omap_dsi_irq_handler, IRQF_SHARED,
+                dev_name(&dsidev->dev), dsi->pdev);
+        if (r < 0) {
+                DSSERR("request_irq failed\n");
                 goto err2;
         }
 
+        /* DSI VCs initialization */
+        for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+                dsi->vc[i].mode = DSI_VC_MODE_L4;
+                dsi->vc[i].dssdev = NULL;
+                dsi->vc[i].vc_id = 0;
+        }
+
+        dsi_calc_clock_param_ranges(dsidev);
+
         enable_clocks(1);
 
-        rev = dsi_read_reg(DSI_REVISION);
-        printk(KERN_INFO "OMAP DSI rev %d.%d\n",
+        rev = dsi_read_reg(dsidev, DSI_REVISION);
+        dev_dbg(&dsidev->dev, "OMAP DSI rev %d.%d\n",
                FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
+        dsi->num_data_lanes = dsi_get_num_data_lanes(dsidev);
+
         enable_clocks(0);
 
         return 0;
 err2:
-        iounmap(dsi.base);
+        iounmap(dsi->base);
 err1:
-        destroy_workqueue(dsi.workqueue);
+        kfree(dsi);
+err0:
         return r;
 }
 
-void dsi_exit(void)
+static void dsi_exit(struct platform_device *dsidev)
 {
-        iounmap(dsi.base);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->vdds_dsi_reg != NULL) {
+                if (dsi->vdds_dsi_enabled) {
+                        regulator_disable(dsi->vdds_dsi_reg);
+                        dsi->vdds_dsi_enabled = false;
+                }
+
+                regulator_put(dsi->vdds_dsi_reg);
+                dsi->vdds_dsi_reg = NULL;
+        }
 
-        destroy_workqueue(dsi.workqueue);
+        free_irq(dsi->irq, dsi->pdev);
+        iounmap(dsi->base);
+
+        kfree(dsi);
 
         DSSDBG("omap_dsi_exit\n");
 }
 
+/* DSI1 HW IP initialisation */
+static int omap_dsi1hw_probe(struct platform_device *dsidev)
+{
+        int r;
+
+        r = dsi_init(dsidev);
+        if (r) {
+                DSSERR("Failed to initialize DSI\n");
+                goto err_dsi;
+        }
+err_dsi:
+        return r;
+}
+
+static int omap_dsi1hw_remove(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi_exit(dsidev);
+        WARN_ON(dsi->scp_clk_refcount > 0);
+        return 0;
+}
+
+static struct platform_driver omap_dsi1hw_driver = {
+        .probe          = omap_dsi1hw_probe,
+        .remove         = omap_dsi1hw_remove,
+        .driver         = {
+                .name   = "omapdss_dsi1",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int dsi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dsi1hw_driver);
+}
+
+void dsi_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_dsi1hw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 77c3621c9171..d9489d5c4f08 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -26,14 +26,13 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/delay.h>
-#include <linux/interrupt.h>
 #include <linux/seq_file.h>
 #include <linux/clk.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
+#include <plat/clock.h>
 #include "dss.h"
-
-#define DSS_BASE                        0x48050000
+#include "dss_features.h"
 
 #define DSS_SZ_REGS                     SZ_512
 
@@ -46,7 +45,6 @@ struct dss_reg {
 #define DSS_REVISION                    DSS_REG(0x0000)
 #define DSS_SYSCONFIG                   DSS_REG(0x0010)
 #define DSS_SYSSTATUS                   DSS_REG(0x0014)
-#define DSS_IRQSTATUS                   DSS_REG(0x0018)
 #define DSS_CONTROL                     DSS_REG(0x0040)
 #define DSS_SDI_CONTROL                 DSS_REG(0x0044)
 #define DSS_PLL_CONTROL                 DSS_REG(0x0048)
@@ -59,21 +57,41 @@ struct dss_reg {
         dss_write_reg(idx, FLD_MOD(dss_read_reg(idx), val, start, end))
 
 static struct {
+        struct platform_device *pdev;
         void __iomem    *base;
+        int             ctx_id;
 
         struct clk      *dpll4_m4_ck;
+        struct clk      *dss_ick;
+        struct clk      *dss_fck;
+        struct clk      *dss_sys_clk;
+        struct clk      *dss_tv_fck;
+        struct clk      *dss_video_fck;
+        unsigned        num_clks_enabled;
 
         unsigned long   cache_req_pck;
         unsigned long   cache_prate;
         struct dss_clock_info cache_dss_cinfo;
         struct dispc_clock_info cache_dispc_cinfo;
 
-        enum dss_clk_source dsi_clk_source;
-        enum dss_clk_source dispc_clk_source;
+        enum omap_dss_clk_source dsi_clk_source[MAX_NUM_DSI];
+        enum omap_dss_clk_source dispc_clk_source;
+        enum omap_dss_clk_source lcd_clk_source[MAX_DSS_LCD_MANAGERS];
 
         u32             ctx[DSS_SZ_REGS / sizeof(u32)];
 } dss;
 
+static const char * const dss_generic_clk_source_names[] = {
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]  = "DSI_PLL_HSDIV_DISPC",
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]    = "DSI_PLL_HSDIV_DSI",
+        [OMAP_DSS_CLK_SRC_FCK]                  = "DSS_FCK",
+};
+
+static void dss_clk_enable_all_no_ctx(void);
+static void dss_clk_disable_all_no_ctx(void);
+static void dss_clk_enable_no_ctx(enum dss_clock clks);
+static void dss_clk_disable_no_ctx(enum dss_clock clks);
+
 static int _omap_dss_wait_reset(void);
 
 static inline void dss_write_reg(const struct dss_reg idx, u32 val)
@@ -99,10 +117,11 @@ void dss_save_context(void)
         SR(SYSCONFIG);
         SR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-        SR(SDI_CONTROL);
-        SR(PLL_CONTROL);
-#endif
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                SR(SDI_CONTROL);
+                SR(PLL_CONTROL);
+        }
 }
 
 void dss_restore_context(void)
@@ -113,10 +132,11 @@ void dss_restore_context(void)
         RR(SYSCONFIG);
         RR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-        RR(SDI_CONTROL);
-        RR(PLL_CONTROL);
-#endif
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                RR(SDI_CONTROL);
+                RR(PLL_CONTROL);
+        }
 }
 
 #undef SR
@@ -209,125 +229,235 @@ void dss_sdi_disable(void)
         REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
 }
 
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src)
+{
+        return dss_generic_clk_source_names[clk_src];
+}
+
 void dss_dump_clocks(struct seq_file *s)
 {
         unsigned long dpll4_ck_rate;
         unsigned long dpll4_m4_ck_rate;
+        const char *fclk_name, *fclk_real_name;
+        unsigned long fclk_rate;
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-        dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-        dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         seq_printf(s, "- DSS -\n");
 
-        seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
+        fclk_name = dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+        fclk_real_name = dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+        fclk_rate = dss_clk_get_rate(DSS_CLK_FCK);
 
-        if (cpu_is_omap3630())
-                seq_printf(s, "dss1_alwon_fclk = %lu / %lu  = %lu\n",
-                        dpll4_ck_rate,
-                        dpll4_ck_rate / dpll4_m4_ck_rate,
-                        dss_clk_get_rate(DSS_CLK_FCK1));
-        else
-                seq_printf(s, "dss1_alwon_fclk = %lu / %lu * 2 = %lu\n",
-                        dpll4_ck_rate,
-                        dpll4_ck_rate / dpll4_m4_ck_rate,
-                        dss_clk_get_rate(DSS_CLK_FCK1));
+        if (dss.dpll4_m4_ck) {
+                dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+                dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
+
+                seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
+                        seq_printf(s, "%s (%s) = %lu / %lu  = %lu\n",
+                                        fclk_name, fclk_real_name,
+                                        dpll4_ck_rate,
+                                        dpll4_ck_rate / dpll4_m4_ck_rate,
+                                        fclk_rate);
+                else
+                        seq_printf(s, "%s (%s) = %lu / %lu * 2 = %lu\n",
+                                        fclk_name, fclk_real_name,
+                                        dpll4_ck_rate,
+                                        dpll4_ck_rate / dpll4_m4_ck_rate,
+                                        fclk_rate);
+        } else {
+                seq_printf(s, "%s (%s) = %lu\n",
+                                fclk_name, fclk_real_name,
+                                fclk_rate);
+        }
+
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 void dss_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dss_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(DSS_REVISION);
         DUMPREG(DSS_SYSCONFIG);
         DUMPREG(DSS_SYSSTATUS);
-        DUMPREG(DSS_IRQSTATUS);
         DUMPREG(DSS_CONTROL);
-        DUMPREG(DSS_SDI_CONTROL);
-        DUMPREG(DSS_PLL_CONTROL);
-        DUMPREG(DSS_SDI_STATUS);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                DUMPREG(DSS_SDI_CONTROL);
+                DUMPREG(DSS_PLL_CONTROL);
+                DUMPREG(DSS_SDI_STATUS);
+        }
+
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
-void dss_select_dispc_clk_source(enum dss_clk_source clk_src)
+void dss_select_dispc_clk_source(enum omap_dss_clk_source clk_src)
 {
+        struct platform_device *dsidev;
         int b;
+        u8 start, end;
+
+        switch (clk_src) {
+        case OMAP_DSS_CLK_SRC_FCK:
+                b = 0;
+                break;
+        case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                b = 1;
+                dsidev = dsi_get_dsidev_from_id(0);
+                dsi_wait_pll_hsdiv_dispc_active(dsidev);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                b = 2;
+                dsidev = dsi_get_dsidev_from_id(1);
+                dsi_wait_pll_hsdiv_dispc_active(dsidev);
+                break;
+        default:
+                BUG();
+        }
 
-        BUG_ON(clk_src != DSS_SRC_DSI1_PLL_FCLK &&
-                        clk_src != DSS_SRC_DSS1_ALWON_FCLK);
-
-        b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
-
-        if (clk_src == DSS_SRC_DSI1_PLL_FCLK)
-                dsi_wait_dsi1_pll_active();
+        dss_feat_get_reg_field(FEAT_REG_DISPC_CLK_SWITCH, &start, &end);
 
-        REG_FLD_MOD(DSS_CONTROL, b, 0, 0);      /* DISPC_CLK_SWITCH */
+        REG_FLD_MOD(DSS_CONTROL, b, start, end);        /* DISPC_CLK_SWITCH */
 
         dss.dispc_clk_source = clk_src;
 }
 
-void dss_select_dsi_clk_source(enum dss_clk_source clk_src)
+void dss_select_dsi_clk_source(int dsi_module,
+                enum omap_dss_clk_source clk_src)
 {
+        struct platform_device *dsidev;
         int b;
 
-        BUG_ON(clk_src != DSS_SRC_DSI2_PLL_FCLK &&
-                        clk_src != DSS_SRC_DSS1_ALWON_FCLK);
+        switch (clk_src) {
+        case OMAP_DSS_CLK_SRC_FCK:
+                b = 0;
+                break;
+        case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI:
+                BUG_ON(dsi_module != 0);
+                b = 1;
+                dsidev = dsi_get_dsidev_from_id(0);
+                dsi_wait_pll_hsdiv_dsi_active(dsidev);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI:
+                BUG_ON(dsi_module != 1);
+                b = 1;
+                dsidev = dsi_get_dsidev_from_id(1);
+                dsi_wait_pll_hsdiv_dsi_active(dsidev);
+                break;
+        default:
+                BUG();
+        }
 
-        b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
+        REG_FLD_MOD(DSS_CONTROL, b, 1, 1);      /* DSI_CLK_SWITCH */
 
-        if (clk_src == DSS_SRC_DSI2_PLL_FCLK)
-                dsi_wait_dsi2_pll_active();
+        dss.dsi_clk_source[dsi_module] = clk_src;
+}
 
-        REG_FLD_MOD(DSS_CONTROL, b, 1, 1);      /* DSI_CLK_SWITCH */
+void dss_select_lcd_clk_source(enum omap_channel channel,
+                enum omap_dss_clk_source clk_src)
+{
+        struct platform_device *dsidev;
+        int b, ix, pos;
 
-        dss.dsi_clk_source = clk_src;
+        if (!dss_has_feature(FEAT_LCD_CLK_SRC))
+                return;
+
+        switch (clk_src) {
+        case OMAP_DSS_CLK_SRC_FCK:
+                b = 0;
+                break;
+        case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                BUG_ON(channel != OMAP_DSS_CHANNEL_LCD);
+                b = 1;
+                dsidev = dsi_get_dsidev_from_id(0);
+                dsi_wait_pll_hsdiv_dispc_active(dsidev);
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                BUG_ON(channel != OMAP_DSS_CHANNEL_LCD2);
+                b = 1;
+                dsidev = dsi_get_dsidev_from_id(1);
+                dsi_wait_pll_hsdiv_dispc_active(dsidev);
+                break;
+        default:
+                BUG();
+        }
+
+        pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 12;
+        REG_FLD_MOD(DSS_CONTROL, b, pos, pos);  /* LCDx_CLK_SWITCH */
+
+        ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+        dss.lcd_clk_source[ix] = clk_src;
 }
 
-enum dss_clk_source dss_get_dispc_clk_source(void)
+enum omap_dss_clk_source dss_get_dispc_clk_source(void)
 {
         return dss.dispc_clk_source;
 }
 
-enum dss_clk_source dss_get_dsi_clk_source(void)
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module)
 {
-        return dss.dsi_clk_source;
+        return dss.dsi_clk_source[dsi_module];
+}
+
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
+{
+        if (dss_has_feature(FEAT_LCD_CLK_SRC)) {
+                int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+                return dss.lcd_clk_source[ix];
+        } else {
+                /* LCD_CLK source is the same as DISPC_FCLK source for
+                 * OMAP2 and OMAP3 */
+                return dss.dispc_clk_source;
+        }
 }
 
 /* calculate clock rates using dividers in cinfo */
 int dss_calc_clock_rates(struct dss_clock_info *cinfo)
 {
-        unsigned long prate;
+        if (dss.dpll4_m4_ck) {
+                unsigned long prate;
+                u16 fck_div_max = 16;
 
-        if (cinfo->fck_div > (cpu_is_omap3630() ? 32 : 16) ||
-                                                cinfo->fck_div == 0)
-                return -EINVAL;
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
+                        fck_div_max = 32;
+
+                if (cinfo->fck_div > fck_div_max || cinfo->fck_div == 0)
+                        return -EINVAL;
 
-        prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+                prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
 
-        cinfo->fck = prate / cinfo->fck_div;
+                cinfo->fck = prate / cinfo->fck_div;
+        } else {
+                if (cinfo->fck_div != 0)
+                        return -EINVAL;
+                cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK);
+        }
 
         return 0;
 }
 
 int dss_set_clock_div(struct dss_clock_info *cinfo)
 {
-        unsigned long prate;
-        int r;
+        if (dss.dpll4_m4_ck) {
+                unsigned long prate;
+                int r;
 
-        if (cpu_is_omap34xx()) {
                 prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
                 DSSDBG("dpll4_m4 = %ld\n", prate);
 
                 r = clk_set_rate(dss.dpll4_m4_ck, prate / cinfo->fck_div);
                 if (r)
                         return r;
+        } else {
+                if (cinfo->fck_div != 0)
+                        return -EINVAL;
         }
 
         DSSDBG("fck = %ld (%d)\n", cinfo->fck, cinfo->fck_div);
@@ -337,12 +467,14 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 
 int dss_get_clock_div(struct dss_clock_info *cinfo)
 {
-        cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK1);
+        cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK);
 
-        if (cpu_is_omap34xx()) {
+        if (dss.dpll4_m4_ck) {
                 unsigned long prate;
+
                 prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-                if (cpu_is_omap3630())
+
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
                         cinfo->fck_div = prate / (cinfo->fck);
                 else
                         cinfo->fck_div = prate / (cinfo->fck / 2);
@@ -355,7 +487,7 @@ int dss_get_clock_div(struct dss_clock_info *cinfo)
 
 unsigned long dss_get_dpll4_rate(void)
 {
-        if (cpu_is_omap34xx())
+        if (dss.dpll4_m4_ck)
                 return clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
         else
                 return 0;
@@ -369,16 +501,18 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
         struct dss_clock_info best_dss;
         struct dispc_clock_info best_dispc;
 
-        unsigned long fck;
+        unsigned long fck, max_dss_fck;
 
-        u16 fck_div;
+        u16 fck_div, fck_div_max = 16;
 
         int match = 0;
         int min_fck_per_pck;
 
         prate = dss_get_dpll4_rate();
 
-        fck = dss_clk_get_rate(DSS_CLK_FCK1);
+        max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+        fck = dss_clk_get_rate(DSS_CLK_FCK);
         if (req_pck == dss.cache_req_pck &&
                         ((cpu_is_omap34xx() && prate == dss.cache_prate) ||
                          dss.cache_dss_cinfo.fck == fck)) {
@@ -391,7 +525,7 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
         min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
         if (min_fck_per_pck &&
-                req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+                req_pck * min_fck_per_pck > max_dss_fck) {
                 DSSERR("Requested pixel clock not possible with the current "
                                 "OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
                                 "the constraint off.\n");
@@ -402,10 +536,10 @@ retry:
         memset(&best_dss, 0, sizeof(best_dss));
         memset(&best_dispc, 0, sizeof(best_dispc));
 
-        if (cpu_is_omap24xx()) {
+        if (dss.dpll4_m4_ck == NULL) {
                 struct dispc_clock_info cur_dispc;
                 /* XXX can we change the clock on omap2? */
-                fck = dss_clk_get_rate(DSS_CLK_FCK1);
+                fck = dss_clk_get_rate(DSS_CLK_FCK);
                 fck_div = 1;
 
                 dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
@@ -417,17 +551,19 @@ retry:
                 best_dispc = cur_dispc;
 
                 goto found;
-        } else if (cpu_is_omap34xx()) {
-                for (fck_div = (cpu_is_omap3630() ? 32 : 16);
-                                        fck_div > 0; --fck_div) {
+        } else {
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
+                        fck_div_max = 32;
+
+                for (fck_div = fck_div_max; fck_div > 0; --fck_div) {
                         struct dispc_clock_info cur_dispc;
 
-                        if (cpu_is_omap3630())
+                        if (fck_div_max == 32)
                                 fck = prate / fck_div;
                         else
                                 fck = prate / fck_div * 2;
 
-                        if (fck > DISPC_MAX_FCK)
+                        if (fck > max_dss_fck)
                                 continue;
 
                         if (min_fck_per_pck &&
@@ -450,8 +586,6 @@ retry:
                                         goto found;
                         }
                 }
-        } else {
-                BUG();
         }
 
 found:
@@ -482,31 +616,6 @@ found:
         return 0;
 }
 
-
-
-static irqreturn_t dss_irq_handler_omap2(int irq, void *arg)
-{
-        dispc_irq_handler();
-
-        return IRQ_HANDLED;
-}
-
-static irqreturn_t dss_irq_handler_omap3(int irq, void *arg)
-{
-        u32 irqstatus;
-
-        irqstatus = dss_read_reg(DSS_IRQSTATUS);
-
-        if (irqstatus & (1<<0)) /* DISPC_IRQ */
-                dispc_irq_handler();
-#ifdef CONFIG_OMAP2_DSS_DSI
-        if (irqstatus & (1<<1)) /* DSI_IRQ */
-                dsi_irq_handler();
-#endif
-
-        return IRQ_HANDLED;
-}
-
 static int _omap_dss_wait_reset(void)
 {
         int t = 0;
@@ -549,34 +658,50 @@ void dss_set_dac_pwrdn_bgz(bool enable)
         REG_FLD_MOD(DSS_CONTROL, enable, 5, 5); /* DAC Power-Down Control */
 }
 
-int dss_init(bool skip_init)
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select hdmi)
+{
+        REG_FLD_MOD(DSS_CONTROL, hdmi, 15, 15); /* VENC_HDMI_SWITCH */
+}
+
+static int dss_init(void)
 {
         int r;
         u32 rev;
+        struct resource *dss_mem;
+        struct clk *dpll4_m4_ck;
 
-        dss.base = ioremap(DSS_BASE, DSS_SZ_REGS);
+        dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
+        if (!dss_mem) {
+                DSSERR("can't get IORESOURCE_MEM DSS\n");
+                r = -EINVAL;
+                goto fail0;
+        }
+        dss.base = ioremap(dss_mem->start, resource_size(dss_mem));
         if (!dss.base) {
                 DSSERR("can't ioremap DSS\n");
                 r = -ENOMEM;
                 goto fail0;
         }
 
-        if (!skip_init) {
-                /* disable LCD and DIGIT output. This seems to fix the synclost
-                 * problem that we get, if the bootloader starts the DSS and
-                 * the kernel resets it */
-                omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
-
-                /* We need to wait here a bit, otherwise we sometimes start to
-                 * get synclost errors, and after that only power cycle will
-                 * restore DSS functionality. I have no idea why this happens.
-                 * And we have to wait _before_ resetting the DSS, but after
-                 * enabling clocks.
-                 */
-                msleep(50);
+        /* disable LCD and DIGIT output. This seems to fix the synclost
+         * problem that we get, if the bootloader starts the DSS and
+         * the kernel resets it */
+        omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
+
+#ifdef CONFIG_OMAP2_DSS_SLEEP_BEFORE_RESET
+        /* We need to wait here a bit, otherwise we sometimes start to
+         * get synclost errors, and after that only power cycle will
+         * restore DSS functionality. I have no idea why this happens.
+         * And we have to wait _before_ resetting the DSS, but after
+         * enabling clocks.
+         *
+         * This bug was at least present on OMAP3430. It's unknown
+         * if it happens on OMAP2 or OMAP3630.
+         */
+        msleep(50);
+#endif
 
-                _omap_dss_reset();
-        }
+        _omap_dss_reset();
 
         /* autoidle */
         REG_FLD_MOD(DSS_SYSCONFIG, 1, 0, 0);
@@ -589,29 +714,31 @@ int dss_init(bool skip_init)
         REG_FLD_MOD(DSS_CONTROL, 1, 3, 3);      /* venc clock 4x enable */
         REG_FLD_MOD(DSS_CONTROL, 0, 2, 2);      /* venc clock mode = normal */
 #endif
-
-        r = request_irq(INT_24XX_DSS_IRQ,
-                        cpu_is_omap24xx()
-                        ? dss_irq_handler_omap2
-                        : dss_irq_handler_omap3,
-                        0, "OMAP DSS", NULL);
-
-        if (r < 0) {
-                DSSERR("omap2 dss: request_irq failed\n");
-                goto fail1;
-        }
-
         if (cpu_is_omap34xx()) {
-                dss.dpll4_m4_ck = clk_get(NULL, "dpll4_m4_ck");
-                if (IS_ERR(dss.dpll4_m4_ck)) {
+                dpll4_m4_ck = clk_get(NULL, "dpll4_m4_ck");
+                if (IS_ERR(dpll4_m4_ck)) {
                         DSSERR("Failed to get dpll4_m4_ck\n");
-                        r = PTR_ERR(dss.dpll4_m4_ck);
-                        goto fail2;
+                        r = PTR_ERR(dpll4_m4_ck);
+                        goto fail1;
                 }
+        } else if (cpu_is_omap44xx()) {
+                dpll4_m4_ck = clk_get(NULL, "dpll_per_m5x2_ck");
+                if (IS_ERR(dpll4_m4_ck)) {
+                        DSSERR("Failed to get dpll4_m4_ck\n");
+                        r = PTR_ERR(dpll4_m4_ck);
+                        goto fail1;
+                }
+        } else { /* omap24xx */
+                dpll4_m4_ck = NULL;
         }
 
-        dss.dsi_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
-        dss.dispc_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
+        dss.dpll4_m4_ck = dpll4_m4_ck;
+
+        dss.dsi_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
+        dss.dsi_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
+        dss.dispc_clk_source = OMAP_DSS_CLK_SRC_FCK;
+        dss.lcd_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
+        dss.lcd_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
 
         dss_save_context();
 
@@ -621,21 +748,427 @@ int dss_init(bool skip_init)
 
         return 0;
 
-fail2:
-        free_irq(INT_24XX_DSS_IRQ, NULL);
 fail1:
         iounmap(dss.base);
 fail0:
         return r;
 }
 
-void dss_exit(void)
+static void dss_exit(void)
 {
-        if (cpu_is_omap34xx())
+        if (dss.dpll4_m4_ck)
                 clk_put(dss.dpll4_m4_ck);
 
-        free_irq(INT_24XX_DSS_IRQ, NULL);
-
         iounmap(dss.base);
 }
 
+/* CONTEXT */
+static int dss_get_ctx_id(void)
+{
+        struct omap_display_platform_data *pdata = dss.pdev->dev.platform_data;
+        int r;
+
+        if (!pdata->board_data->get_last_off_on_transaction_id)
+                return 0;
+        r = pdata->board_data->get_last_off_on_transaction_id(&dss.pdev->dev);
+        if (r < 0) {
+                dev_err(&dss.pdev->dev, "getting transaction ID failed, "
+                                "will force context restore\n");
+                r = -1;
+        }
+        return r;
+}
+
+int dss_need_ctx_restore(void)
+{
+        int id = dss_get_ctx_id();
+
+        if (id < 0 || id != dss.ctx_id) {
+                DSSDBG("ctx id %d -> id %d\n",
+                                dss.ctx_id, id);
+                dss.ctx_id = id;
+                return 1;
+        } else {
+                return 0;
+        }
+}
+
+static void save_all_ctx(void)
+{
+        DSSDBG("save context\n");
+
+        dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK);
+
+        dss_save_context();
+        dispc_save_context();
+#ifdef CONFIG_OMAP2_DSS_DSI
+        dsi_save_context();
+#endif
+
+        dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK);
+}
+
+static void restore_all_ctx(void)
+{
+        DSSDBG("restore context\n");
+
+        dss_clk_enable_all_no_ctx();
+
+        dss_restore_context();
+        dispc_restore_context();
+#ifdef CONFIG_OMAP2_DSS_DSI
+        dsi_restore_context();
+#endif
+
+        dss_clk_disable_all_no_ctx();
+}
+
+static int dss_get_clock(struct clk **clock, const char *clk_name)
+{
+        struct clk *clk;
+
+        clk = clk_get(&dss.pdev->dev, clk_name);
+
+        if (IS_ERR(clk)) {
+                DSSERR("can't get clock %s", clk_name);
+                return PTR_ERR(clk);
+        }
+
+        *clock = clk;
+
+        DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
+
+        return 0;
+}
+
+static int dss_get_clocks(void)
+{
+        int r;
+        struct omap_display_platform_data *pdata = dss.pdev->dev.platform_data;
+
+        dss.dss_ick = NULL;
+        dss.dss_fck = NULL;
+        dss.dss_sys_clk = NULL;
+        dss.dss_tv_fck = NULL;
+        dss.dss_video_fck = NULL;
+
+        r = dss_get_clock(&dss.dss_ick, "ick");
+        if (r)
+                goto err;
+
+        r = dss_get_clock(&dss.dss_fck, "fck");
+        if (r)
+                goto err;
+
+        if (!pdata->opt_clock_available) {
+                r = -ENODEV;
+                goto err;
+        }
+
+        if (pdata->opt_clock_available("sys_clk")) {
+                r = dss_get_clock(&dss.dss_sys_clk, "sys_clk");
+                if (r)
+                        goto err;
+        }
+
+        if (pdata->opt_clock_available("tv_clk")) {
+                r = dss_get_clock(&dss.dss_tv_fck, "tv_clk");
+                if (r)
+                        goto err;
+        }
+
+        if (pdata->opt_clock_available("video_clk")) {
+                r = dss_get_clock(&dss.dss_video_fck, "video_clk");
+                if (r)
+                        goto err;
+        }
+
+        return 0;
+
+err:
+        if (dss.dss_ick)
+                clk_put(dss.dss_ick);
+        if (dss.dss_fck)
+                clk_put(dss.dss_fck);
+        if (dss.dss_sys_clk)
+                clk_put(dss.dss_sys_clk);
+        if (dss.dss_tv_fck)
+                clk_put(dss.dss_tv_fck);
+        if (dss.dss_video_fck)
+                clk_put(dss.dss_video_fck);
+
+        return r;
+}
+
+static void dss_put_clocks(void)
+{
+        if (dss.dss_video_fck)
+                clk_put(dss.dss_video_fck);
+        if (dss.dss_tv_fck)
+                clk_put(dss.dss_tv_fck);
+        if (dss.dss_sys_clk)
+                clk_put(dss.dss_sys_clk);
+        clk_put(dss.dss_fck);
+        clk_put(dss.dss_ick);
+}
+
+unsigned long dss_clk_get_rate(enum dss_clock clk)
+{
+        switch (clk) {
+        case DSS_CLK_ICK:
+                return clk_get_rate(dss.dss_ick);
+        case DSS_CLK_FCK:
+                return clk_get_rate(dss.dss_fck);
+        case DSS_CLK_SYSCK:
+                return clk_get_rate(dss.dss_sys_clk);
+        case DSS_CLK_TVFCK:
+                return clk_get_rate(dss.dss_tv_fck);
+        case DSS_CLK_VIDFCK:
+                return clk_get_rate(dss.dss_video_fck);
+        }
+
+        BUG();
+        return 0;
+}
+
+static unsigned count_clk_bits(enum dss_clock clks)
+{
+        unsigned num_clks = 0;
+
+        if (clks & DSS_CLK_ICK)
+                ++num_clks;
+        if (clks & DSS_CLK_FCK)
+                ++num_clks;
+        if (clks & DSS_CLK_SYSCK)
+                ++num_clks;
+        if (clks & DSS_CLK_TVFCK)
+                ++num_clks;
+        if (clks & DSS_CLK_VIDFCK)
+                ++num_clks;
+
+        return num_clks;
+}
+
+static void dss_clk_enable_no_ctx(enum dss_clock clks)
+{
+        unsigned num_clks = count_clk_bits(clks);
+
+        if (clks & DSS_CLK_ICK)
+                clk_enable(dss.dss_ick);
+        if (clks & DSS_CLK_FCK)
+                clk_enable(dss.dss_fck);
+        if ((clks & DSS_CLK_SYSCK) && dss.dss_sys_clk)
+                clk_enable(dss.dss_sys_clk);
+        if ((clks & DSS_CLK_TVFCK) && dss.dss_tv_fck)
+                clk_enable(dss.dss_tv_fck);
+        if ((clks & DSS_CLK_VIDFCK) && dss.dss_video_fck)
+                clk_enable(dss.dss_video_fck);
+
+        dss.num_clks_enabled += num_clks;
+}
+
+void dss_clk_enable(enum dss_clock clks)
+{
+        bool check_ctx = dss.num_clks_enabled == 0;
+
+        dss_clk_enable_no_ctx(clks);
+
+        /*
+         * HACK: On omap4 the registers may not be accessible right after
+         * enabling the clocks. At some point this will be handled by
+         * pm_runtime, but for the time begin this should make things work.
+         */
+        if (cpu_is_omap44xx() && check_ctx)
+                udelay(10);
+
+        if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
+                restore_all_ctx();
+}
+
+static void dss_clk_disable_no_ctx(enum dss_clock clks)
+{
+        unsigned num_clks = count_clk_bits(clks);
+
+        if (clks & DSS_CLK_ICK)
+                clk_disable(dss.dss_ick);
+        if (clks & DSS_CLK_FCK)
+                clk_disable(dss.dss_fck);
+        if ((clks & DSS_CLK_SYSCK) && dss.dss_sys_clk)
+                clk_disable(dss.dss_sys_clk);
+        if ((clks & DSS_CLK_TVFCK) && dss.dss_tv_fck)
+                clk_disable(dss.dss_tv_fck);
+        if ((clks & DSS_CLK_VIDFCK) && dss.dss_video_fck)
+                clk_disable(dss.dss_video_fck);
+
+        dss.num_clks_enabled -= num_clks;
+}
+
+void dss_clk_disable(enum dss_clock clks)
+{
+        if (cpu_is_omap34xx()) {
+                unsigned num_clks = count_clk_bits(clks);
+
+                BUG_ON(dss.num_clks_enabled < num_clks);
+
+                if (dss.num_clks_enabled == num_clks)
+                        save_all_ctx();
+        }
+
+        dss_clk_disable_no_ctx(clks);
+}
+
+static void dss_clk_enable_all_no_ctx(void)
+{
+        enum dss_clock clks;
+
+        clks = DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_SYSCK | DSS_CLK_TVFCK;
+        if (cpu_is_omap34xx())
+                clks |= DSS_CLK_VIDFCK;
+        dss_clk_enable_no_ctx(clks);
+}
+
+static void dss_clk_disable_all_no_ctx(void)
+{
+        enum dss_clock clks;
+
+        clks = DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_SYSCK | DSS_CLK_TVFCK;
+        if (cpu_is_omap34xx())
+                clks |= DSS_CLK_VIDFCK;
+        dss_clk_disable_no_ctx(clks);
+}
+
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+/* CLOCKS */
+static void core_dump_clocks(struct seq_file *s)
+{
+        int i;
+        struct clk *clocks[5] = {
+                dss.dss_ick,
+                dss.dss_fck,
+                dss.dss_sys_clk,
+                dss.dss_tv_fck,
+                dss.dss_video_fck
+        };
+
+        const char *names[5] = {
+                "ick",
+                "fck",
+                "sys_clk",
+                "tv_fck",
+                "video_fck"
+        };
+
+        seq_printf(s, "- CORE -\n");
+
+        seq_printf(s, "internal clk count\t\t%u\n", dss.num_clks_enabled);
+
+        for (i = 0; i < 5; i++) {
+                if (!clocks[i])
+                        continue;
+                seq_printf(s, "%s (%s)%*s\t%lu\t%d\n",
+                                names[i],
+                                clocks[i]->name,
+                                24 - strlen(names[i]) - strlen(clocks[i]->name),
+                                "",
+                                clk_get_rate(clocks[i]),
+                                clocks[i]->usecount);
+        }
+}
+#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
+
+/* DEBUGFS */
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s)
+{
+        core_dump_clocks(s);
+        dss_dump_clocks(s);
+        dispc_dump_clocks(s);
+#ifdef CONFIG_OMAP2_DSS_DSI
+        dsi_dump_clocks(s);
+#endif
+}
+#endif
+
+
+/* DSS HW IP initialisation */
+static int omap_dsshw_probe(struct platform_device *pdev)
+{
+        int r;
+
+        dss.pdev = pdev;
+
+        r = dss_get_clocks();
+        if (r)
+                goto err_clocks;
+
+        dss_clk_enable_all_no_ctx();
+
+        dss.ctx_id = dss_get_ctx_id();
+        DSSDBG("initial ctx id %u\n", dss.ctx_id);
+
+        r = dss_init();
+        if (r) {
+                DSSERR("Failed to initialize DSS\n");
+                goto err_dss;
+        }
+
+        r = dpi_init();
+        if (r) {
+                DSSERR("Failed to initialize DPI\n");
+                goto err_dpi;
+        }
+
+        r = sdi_init();
+        if (r) {
+                DSSERR("Failed to initialize SDI\n");
+                goto err_sdi;
+        }
+
+        dss_clk_disable_all_no_ctx();
+        return 0;
+err_sdi:
+        dpi_exit();
+err_dpi:
+        dss_exit();
+err_dss:
+        dss_clk_disable_all_no_ctx();
+        dss_put_clocks();
+err_clocks:
+        return r;
+}
+
+static int omap_dsshw_remove(struct platform_device *pdev)
+{
+
+        dss_exit();
+
+        /*
+         * As part of hwmod changes, DSS is not the only controller of dss
+         * clocks; hwmod framework itself will also enable clocks during hwmod
+         * init for dss, and autoidle is set in h/w for DSS. Hence, there's no
+         * need to disable clocks if their usecounts > 1.
+         */
+        WARN_ON(dss.num_clks_enabled > 0);
+
+        dss_put_clocks();
+        return 0;
+}
+
+static struct platform_driver omap_dsshw_driver = {
+        .probe          = omap_dsshw_probe,
+        .remove         = omap_dsshw_remove,
+        .driver         = {
+                .name   = "omapdss_dss",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int dss_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dsshw_driver);
+}
+
+void dss_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_dsshw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index 5c7940d5f282..8ab6d43329bb 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -97,8 +97,6 @@ extern unsigned int dss_debug;
 #define FLD_MOD(orig, val, start, end) \
         (((orig) & ~FLD_MASK(start, end)) | FLD_VAL(val, start, end))
 
-#define DISPC_MAX_FCK 173000000
-
 enum omap_burst_size {
         OMAP_DSS_BURST_4x32 = 0,
         OMAP_DSS_BURST_8x32 = 1,
@@ -112,17 +110,16 @@ enum omap_parallel_interface_mode {
 };
 
 enum dss_clock {
-        DSS_CLK_ICK     = 1 << 0,
-        DSS_CLK_FCK1    = 1 << 1,
-        DSS_CLK_FCK2    = 1 << 2,
-        DSS_CLK_54M     = 1 << 3,
-        DSS_CLK_96M     = 1 << 4,
+        DSS_CLK_ICK     = 1 << 0,       /* DSS_L3_ICLK and DSS_L4_ICLK */
+        DSS_CLK_FCK     = 1 << 1,       /* DSS1_ALWON_FCLK */
+        DSS_CLK_SYSCK   = 1 << 2,       /* DSS2_ALWON_FCLK */
+        DSS_CLK_TVFCK   = 1 << 3,       /* DSS_TV_FCLK */
+        DSS_CLK_VIDFCK  = 1 << 4,       /* DSS_96M_FCLK*/
 };
 
-enum dss_clk_source {
-        DSS_SRC_DSI1_PLL_FCLK,
-        DSS_SRC_DSI2_PLL_FCLK,
-        DSS_SRC_DSS1_ALWON_FCLK,
+enum dss_hdmi_venc_clk_source_select {
+        DSS_VENC_TV_CLK = 0,
+        DSS_HDMI_M_PCLK = 1,
 };
 
 struct dss_clock_info {
@@ -148,36 +145,42 @@ struct dsi_clock_info {
         unsigned long fint;
         unsigned long clkin4ddr;
         unsigned long clkin;
-        unsigned long dsi1_pll_fclk;
-        unsigned long dsi2_pll_fclk;
-
+        unsigned long dsi_pll_hsdiv_dispc_clk;  /* OMAP3: DSI1_PLL_CLK
+                                                 * OMAP4: PLLx_CLK1 */
+        unsigned long dsi_pll_hsdiv_dsi_clk;    /* OMAP3: DSI2_PLL_CLK
+                                                 * OMAP4: PLLx_CLK2 */
         unsigned long lp_clk;
 
         /* dividers */
         u16 regn;
         u16 regm;
-        u16 regm3;
-        u16 regm4;
-
+        u16 regm_dispc; /* OMAP3: REGM3
+                         * OMAP4: REGM4 */
+        u16 regm_dsi;   /* OMAP3: REGM4
+                         * OMAP4: REGM5 */
         u16 lp_clk_div;
 
         u8 highfreq;
-        bool use_dss2_fck;
+        bool use_sys_clk;
+};
+
+/* HDMI PLL structure */
+struct hdmi_pll_info {
+        u16 regn;
+        u16 regm;
+        u32 regmf;
+        u16 regm2;
+        u16 regsd;
+        u16 dcofreq;
 };
 
 struct seq_file;
 struct platform_device;
 
 /* core */
-void dss_clk_enable(enum dss_clock clks);
-void dss_clk_disable(enum dss_clock clks);
-unsigned long dss_clk_get_rate(enum dss_clock clk);
-int dss_need_ctx_restore(void);
-void dss_dump_clocks(struct seq_file *s);
 struct bus_type *dss_get_bus(void);
 struct regulator *dss_get_vdds_dsi(void);
 struct regulator *dss_get_vdds_sdi(void);
-struct regulator *dss_get_vdda_dac(void);
 
 /* display */
 int dss_suspend_all_devices(void);
@@ -214,22 +217,36 @@ void dss_overlay_setup_l4_manager(struct omap_overlay_manager *mgr);
 void dss_recheck_connections(struct omap_dss_device *dssdev, bool force);
 
 /* DSS */
-int dss_init(bool skip_init);
-void dss_exit(void);
+int dss_init_platform_driver(void);
+void dss_uninit_platform_driver(void);
 
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
 void dss_save_context(void);
 void dss_restore_context(void);
+void dss_clk_enable(enum dss_clock clks);
+void dss_clk_disable(enum dss_clock clks);
+unsigned long dss_clk_get_rate(enum dss_clock clk);
+int dss_need_ctx_restore(void);
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src);
+void dss_dump_clocks(struct seq_file *s);
 
 void dss_dump_regs(struct seq_file *s);
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s);
+#endif
 
 void dss_sdi_init(u8 datapairs);
 int dss_sdi_enable(void);
 void dss_sdi_disable(void);
 
-void dss_select_dispc_clk_source(enum dss_clk_source clk_src);
-void dss_select_dsi_clk_source(enum dss_clk_source clk_src);
-enum dss_clk_source dss_get_dispc_clk_source(void);
-enum dss_clk_source dss_get_dsi_clk_source(void);
+void dss_select_dispc_clk_source(enum omap_dss_clk_source clk_src);
+void dss_select_dsi_clk_source(int dsi_module,
+                enum omap_dss_clk_source clk_src);
+void dss_select_lcd_clk_source(enum omap_channel channel,
+                enum omap_dss_clk_source clk_src);
+enum omap_dss_clk_source dss_get_dispc_clk_source(void);
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module);
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel);
 
 void dss_set_venc_output(enum omap_dss_venc_type type);
 void dss_set_dac_pwrdn_bgz(bool enable);
@@ -244,11 +261,11 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 
 /* SDI */
 #ifdef CONFIG_OMAP2_DSS_SDI
-int sdi_init(bool skip_init);
+int sdi_init(void);
 void sdi_exit(void);
 int sdi_init_display(struct omap_dss_device *display);
 #else
-static inline int sdi_init(bool skip_init)
+static inline int sdi_init(void)
 {
         return 0;
 }
@@ -259,54 +276,97 @@ static inline void sdi_exit(void)
 
 /* DSI */
 #ifdef CONFIG_OMAP2_DSS_DSI
-int dsi_init(struct platform_device *pdev);
-void dsi_exit(void);
+
+struct dentry;
+struct file_operations;
+
+int dsi_init_platform_driver(void);
+void dsi_uninit_platform_driver(void);
 
 void dsi_dump_clocks(struct seq_file *s);
-void dsi_dump_irqs(struct seq_file *s);
-void dsi_dump_regs(struct seq_file *s);
+void dsi_create_debugfs_files_irq(struct dentry *debugfs_dir,
+                const struct file_operations *debug_fops);
+void dsi_create_debugfs_files_reg(struct dentry *debugfs_dir,
+                const struct file_operations *debug_fops);
 
 void dsi_save_context(void);
 void dsi_restore_context(void);
 
 int dsi_init_display(struct omap_dss_device *display);
 void dsi_irq_handler(void);
-unsigned long dsi_get_dsi1_pll_rate(void);
-int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo);
-int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
-                struct dsi_clock_info *cinfo,
+unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev);
+int dsi_pll_set_clock_div(struct platform_device *dsidev,
+                struct dsi_clock_info *cinfo);
+int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
+                unsigned long req_pck, struct dsi_clock_info *cinfo,
                 struct dispc_clock_info *dispc_cinfo);
-int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
+int dsi_pll_init(struct platform_device *dsidev, bool enable_hsclk,
                 bool enable_hsdiv);
-void dsi_pll_uninit(void);
+void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes);
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
                 u32 fifo_size, enum omap_burst_size *burst_size,
                 u32 *fifo_low, u32 *fifo_high);
-void dsi_wait_dsi1_pll_active(void);
-void dsi_wait_dsi2_pll_active(void);
+void dsi_wait_pll_hsdiv_dispc_active(struct platform_device *dsidev);
+void dsi_wait_pll_hsdiv_dsi_active(struct platform_device *dsidev);
+struct platform_device *dsi_get_dsidev_from_id(int module);
 #else
-static inline int dsi_init(struct platform_device *pdev)
+static inline int dsi_init_platform_driver(void)
 {
         return 0;
 }
-static inline void dsi_exit(void)
+static inline void dsi_uninit_platform_driver(void)
+{
+}
+static inline unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev)
 {
+        WARN("%s: DSI not compiled in, returning rate as 0\n", __func__);
+        return 0;
 }
-static inline void dsi_wait_dsi1_pll_active(void)
+static inline int dsi_pll_set_clock_div(struct platform_device *dsidev,
+                struct dsi_clock_info *cinfo)
 {
+        WARN("%s: DSI not compiled in\n", __func__);
+        return -ENODEV;
 }
-static inline void dsi_wait_dsi2_pll_active(void)
+static inline int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
+                bool is_tft, unsigned long req_pck,
+                struct dsi_clock_info *dsi_cinfo,
+                struct dispc_clock_info *dispc_cinfo)
 {
+        WARN("%s: DSI not compiled in\n", __func__);
+        return -ENODEV;
+}
+static inline int dsi_pll_init(struct platform_device *dsidev,
+                bool enable_hsclk, bool enable_hsdiv)
+{
+        WARN("%s: DSI not compiled in\n", __func__);
+        return -ENODEV;
+}
+static inline void dsi_pll_uninit(struct platform_device *dsidev,
+                bool disconnect_lanes)
+{
+}
+static inline void dsi_wait_pll_hsdiv_dispc_active(struct platform_device *dsidev)
+{
+}
+static inline void dsi_wait_pll_hsdiv_dsi_active(struct platform_device *dsidev)
+{
+}
+static inline struct platform_device *dsi_get_dsidev_from_id(int module)
+{
+        WARN("%s: DSI not compiled in, returning platform device as NULL\n",
+                        __func__);
+        return NULL;
 }
 #endif
 
 /* DPI */
 #ifdef CONFIG_OMAP2_DSS_DPI
-int dpi_init(struct platform_device *pdev);
+int dpi_init(void);
 void dpi_exit(void);
 int dpi_init_display(struct omap_dss_device *dssdev);
 #else
-static inline int dpi_init(struct platform_device *pdev)
+static inline int dpi_init(void)
 {
         return 0;
 }
@@ -316,8 +376,8 @@ static inline void dpi_exit(void)
 #endif
 
 /* DISPC */
-int dispc_init(void);
-void dispc_exit(void);
+int dispc_init_platform_driver(void);
+void dispc_uninit_platform_driver(void);
 void dispc_dump_clocks(struct seq_file *s);
 void dispc_dump_irqs(struct seq_file *s);
 void dispc_dump_regs(struct seq_file *s);
@@ -333,9 +393,9 @@ void dispc_disable_sidle(void);
 void dispc_lcd_enable_signal_polarity(bool act_high);
 void dispc_lcd_enable_signal(bool enable);
 void dispc_pck_free_enable(bool enable);
-void dispc_enable_fifohandcheck(bool enable);
+void dispc_enable_fifohandcheck(enum omap_channel channel, bool enable);
 
-void dispc_set_lcd_size(u16 width, u16 height);
+void dispc_set_lcd_size(enum omap_channel channel, u16 width, u16 height);
 void dispc_set_digit_size(u16 width, u16 height);
 u32 dispc_get_plane_fifo_size(enum omap_plane plane);
 void dispc_setup_plane_fifo(enum omap_plane plane, u32 low, u32 high);
@@ -350,6 +410,7 @@ void dispc_set_plane_size(enum omap_plane plane, u16 width, u16 height);
 void dispc_set_channel_out(enum omap_plane plane,
                 enum omap_channel channel_out);
 
+void dispc_enable_gamma_table(bool enable);
 int dispc_setup_plane(enum omap_plane plane,
                       u32 paddr, u16 screen_width,
                       u16 pos_x, u16 pos_y,
@@ -359,7 +420,9 @@ int dispc_setup_plane(enum omap_plane plane,
                       bool ilace,
                       enum omap_dss_rotation_type rotation_type,
                       u8 rotation, bool mirror,
-                      u8 global_alpha);
+                      u8 global_alpha, u8 pre_mult_alpha,
+                      enum omap_channel channel,
+                      u32 puv_addr);
 
 bool dispc_go_busy(enum omap_channel channel);
 void dispc_go(enum omap_channel channel);
@@ -368,9 +431,11 @@ bool dispc_is_channel_enabled(enum omap_channel channel);
 int dispc_enable_plane(enum omap_plane plane, bool enable);
 void dispc_enable_replication(enum omap_plane plane, bool enable);
 
-void dispc_set_parallel_interface_mode(enum omap_parallel_interface_mode mode);
-void dispc_set_tft_data_lines(u8 data_lines);
-void dispc_set_lcd_display_type(enum omap_lcd_display_type type);
+void dispc_set_parallel_interface_mode(enum omap_channel channel,
+                enum omap_parallel_interface_mode mode);
+void dispc_set_tft_data_lines(enum omap_channel channel, u8 data_lines);
+void dispc_set_lcd_display_type(enum omap_channel channel,
+                enum omap_lcd_display_type type);
 void dispc_set_loadmode(enum omap_dss_load_mode mode);
 
 void dispc_set_default_color(enum omap_channel channel, u32 color);
@@ -387,54 +452,77 @@ bool dispc_trans_key_enabled(enum omap_channel ch);
 bool dispc_alpha_blending_enabled(enum omap_channel ch);
 
 bool dispc_lcd_timings_ok(struct omap_video_timings *timings);
-void dispc_set_lcd_timings(struct omap_video_timings *timings);
+void dispc_set_lcd_timings(enum omap_channel channel,
+                struct omap_video_timings *timings);
 unsigned long dispc_fclk_rate(void);
-unsigned long dispc_lclk_rate(void);
-unsigned long dispc_pclk_rate(void);
-void dispc_set_pol_freq(enum omap_panel_config config, u8 acbi, u8 acb);
+unsigned long dispc_lclk_rate(enum omap_channel channel);
+unsigned long dispc_pclk_rate(enum omap_channel channel);
+void dispc_set_pol_freq(enum omap_channel channel,
+                enum omap_panel_config config, u8 acbi, u8 acb);
 void dispc_find_clk_divs(bool is_tft, unsigned long req_pck, unsigned long fck,
                 struct dispc_clock_info *cinfo);
 int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
                 struct dispc_clock_info *cinfo);
-int dispc_set_clock_div(struct dispc_clock_info *cinfo);
-int dispc_get_clock_div(struct dispc_clock_info *cinfo);
+int dispc_set_clock_div(enum omap_channel channel,
+                struct dispc_clock_info *cinfo);
+int dispc_get_clock_div(enum omap_channel channel,
+                struct dispc_clock_info *cinfo);
 
 
 /* VENC */
 #ifdef CONFIG_OMAP2_DSS_VENC
-int venc_init(struct platform_device *pdev);
-void venc_exit(void);
+int venc_init_platform_driver(void);
+void venc_uninit_platform_driver(void);
 void venc_dump_regs(struct seq_file *s);
 int venc_init_display(struct omap_dss_device *display);
 #else
-static inline int venc_init(struct platform_device *pdev)
+static inline int venc_init_platform_driver(void)
 {
         return 0;
 }
-static inline void venc_exit(void)
+static inline void venc_uninit_platform_driver(void)
 {
 }
 #endif
 
+/* HDMI */
+#ifdef CONFIG_OMAP4_DSS_HDMI
+int hdmi_init_platform_driver(void);
+void hdmi_uninit_platform_driver(void);
+int hdmi_init_display(struct omap_dss_device *dssdev);
+#else
+static inline int hdmi_init_display(struct omap_dss_device *dssdev)
+{
+        return 0;
+}
+static inline int hdmi_init_platform_driver(void)
+{
+        return 0;
+}
+static inline void hdmi_uninit_platform_driver(void)
+{
+}
+#endif
+int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev);
+int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
+                                        struct omap_video_timings *timings);
+int hdmi_panel_init(void);
+void hdmi_panel_exit(void);
+
 /* RFBI */
 #ifdef CONFIG_OMAP2_DSS_RFBI
-int rfbi_init(void);
-void rfbi_exit(void);
+int rfbi_init_platform_driver(void);
+void rfbi_uninit_platform_driver(void);
 void rfbi_dump_regs(struct seq_file *s);
-
-int rfbi_configure(int rfbi_module, int bpp, int lines);
-void rfbi_enable_rfbi(bool enable);
-void rfbi_transfer_area(u16 width, u16 height,
-                             void (callback)(void *data), void *data);
-void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t);
-unsigned long rfbi_get_max_tx_rate(void);
 int rfbi_init_display(struct omap_dss_device *display);
 #else
-static inline int rfbi_init(void)
+static inline int rfbi_init_platform_driver(void)
 {
         return 0;
 }
-static inline void rfbi_exit(void)
+static inline void rfbi_uninit_platform_driver(void)
 {
 }
 #endif
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
new file mode 100644
index 000000000000..1c18888e5df3
--- /dev/null
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -0,0 +1,433 @@
+/*
+ * 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 <video/omapdss.h>
+#include <plat/cpu.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+/* Defines a generic omap register field */
+struct dss_reg_field {
+        u8 start, end;
+};
+
+struct dss_param_range {
+        int min, max;
+};
+
+struct omap_dss_features {
+        const struct dss_reg_field *reg_fields;
+        const int num_reg_fields;
+
+        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;
+        const char * const *clksrc_names;
+        const struct dss_param_range *dss_params;
+};
+
+/* This struct is assigned to one of the below during initialization */
+static const struct omap_dss_features *omap_current_dss_features;
+
+static const struct dss_reg_field omap2_dss_reg_fields[] = {
+        [FEAT_REG_FIRHINC]                      = { 11, 0 },
+        [FEAT_REG_FIRVINC]                      = { 27, 16 },
+        [FEAT_REG_FIFOLOWTHRESHOLD]             = { 8, 0 },
+        [FEAT_REG_FIFOHIGHTHRESHOLD]            = { 24, 16 },
+        [FEAT_REG_FIFOSIZE]                     = { 8, 0 },
+        [FEAT_REG_HORIZONTALACCU]               = { 9, 0 },
+        [FEAT_REG_VERTICALACCU]                 = { 25, 16 },
+        [FEAT_REG_DISPC_CLK_SWITCH]             = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGN]                  = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGM]                  = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGM_DISPC]            = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGM_DSI]              = { 0, 0 },
+};
+
+static const struct dss_reg_field omap3_dss_reg_fields[] = {
+        [FEAT_REG_FIRHINC]                      = { 12, 0 },
+        [FEAT_REG_FIRVINC]                      = { 28, 16 },
+        [FEAT_REG_FIFOLOWTHRESHOLD]             = { 11, 0 },
+        [FEAT_REG_FIFOHIGHTHRESHOLD]            = { 27, 16 },
+        [FEAT_REG_FIFOSIZE]                     = { 10, 0 },
+        [FEAT_REG_HORIZONTALACCU]               = { 9, 0 },
+        [FEAT_REG_VERTICALACCU]                 = { 25, 16 },
+        [FEAT_REG_DISPC_CLK_SWITCH]             = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGN]                  = { 7, 1 },
+        [FEAT_REG_DSIPLL_REGM]                  = { 18, 8 },
+        [FEAT_REG_DSIPLL_REGM_DISPC]            = { 22, 19 },
+        [FEAT_REG_DSIPLL_REGM_DSI]              = { 26, 23 },
+};
+
+static const struct dss_reg_field omap4_dss_reg_fields[] = {
+        [FEAT_REG_FIRHINC]                      = { 12, 0 },
+        [FEAT_REG_FIRVINC]                      = { 28, 16 },
+        [FEAT_REG_FIFOLOWTHRESHOLD]             = { 15, 0 },
+        [FEAT_REG_FIFOHIGHTHRESHOLD]            = { 31, 16 },
+        [FEAT_REG_FIFOSIZE]                     = { 15, 0 },
+        [FEAT_REG_HORIZONTALACCU]               = { 10, 0 },
+        [FEAT_REG_VERTICALACCU]                 = { 26, 16 },
+        [FEAT_REG_DISPC_CLK_SWITCH]             = { 9, 8 },
+        [FEAT_REG_DSIPLL_REGN]                  = { 8, 1 },
+        [FEAT_REG_DSIPLL_REGM]                  = { 20, 9 },
+        [FEAT_REG_DSIPLL_REGM_DISPC]            = { 25, 21 },
+        [FEAT_REG_DSIPLL_REGM_DSI]              = { 30, 26 },
+};
+
+static const enum omap_display_type omap2_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3430_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3630_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_DSI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap4_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DBI | OMAP_DISPLAY_TYPE_DSI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC | OMAP_DISPLAY_TYPE_HDMI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_DSI,
+};
+
+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,
+};
+
+static const enum omap_color_mode omap4_dss_supported_color_modes[] = {
+        /* OMAP_DSS_GFX */
+        OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
+        OMAP_DSS_COLOR_CLUT4 | OMAP_DSS_COLOR_CLUT8 |
+        OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_ARGB16 |
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_ARGB32 |
+        OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32 |
+        OMAP_DSS_COLOR_ARGB16_1555,
+
+        /* OMAP_DSS_VIDEO1 */
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+        OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+        OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+        OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+        OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+        OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+        OMAP_DSS_COLOR_RGBX32,
+
+       /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+        OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+        OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+        OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+        OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+        OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+        OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+        OMAP_DSS_COLOR_RGBX32,
+};
+
+static const char * const omap2_dss_clk_source_names[] = {
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]  = "N/A",
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]    = "N/A",
+        [OMAP_DSS_CLK_SRC_FCK]                  = "DSS_FCLK1",
+};
+
+static const char * const omap3_dss_clk_source_names[] = {
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]  = "DSI1_PLL_FCLK",
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]    = "DSI2_PLL_FCLK",
+        [OMAP_DSS_CLK_SRC_FCK]                  = "DSS1_ALWON_FCLK",
+};
+
+static const char * const omap4_dss_clk_source_names[] = {
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]  = "PLL1_CLK1",
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]    = "PLL1_CLK2",
+        [OMAP_DSS_CLK_SRC_FCK]                  = "DSS_FCLK",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC] = "PLL2_CLK1",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]   = "PLL2_CLK2",
+};
+
+static const struct dss_param_range omap2_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 173000000 },
+        [FEAT_PARAM_DSIPLL_REGN]                = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_REGM]                = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_REGM_DISPC]          = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_FINT]                = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, 0 },
+};
+
+static const struct dss_param_range omap3_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 173000000 },
+        [FEAT_PARAM_DSIPLL_REGN]                = { 0, (1 << 7) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM]                = { 0, (1 << 11) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DISPC]          = { 0, (1 << 4) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, (1 << 4) - 1 },
+        [FEAT_PARAM_DSIPLL_FINT]                = { 750000, 2100000 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 1, (1 << 13) - 1},
+};
+
+static const struct dss_param_range omap4_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 186000000 },
+        [FEAT_PARAM_DSIPLL_REGN]                = { 0, (1 << 8) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM]                = { 0, (1 << 12) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DISPC]          = { 0, (1 << 5) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, (1 << 5) - 1 },
+        [FEAT_PARAM_DSIPLL_FINT]                = { 500000, 2500000 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, (1 << 13) - 1 },
+};
+
+/* OMAP2 DSS Features */
+static const struct omap_dss_features omap2_dss_features = {
+        .reg_fields = omap2_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap2_dss_reg_fields),
+
+        .has_feature    =
+                FEAT_LCDENABLEPOL | FEAT_LCDENABLESIGNAL |
+                FEAT_PCKFREEENABLE | FEAT_FUNCGATED |
+                FEAT_ROWREPEATENABLE | FEAT_RESIZECONF,
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap2_dss_supported_displays,
+        .supported_color_modes = omap2_dss_supported_color_modes,
+        .clksrc_names = omap2_dss_clk_source_names,
+        .dss_params = omap2_dss_param_range,
+};
+
+/* OMAP3 DSS Features */
+static const struct omap_dss_features omap3430_dss_features = {
+        .reg_fields = omap3_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+        .has_feature    =
+                FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
+                FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
+                FEAT_FUNCGATED | FEAT_ROWREPEATENABLE |
+                FEAT_LINEBUFFERSPLIT | FEAT_RESIZECONF |
+                FEAT_DSI_PLL_FREQSEL | FEAT_DSI_REVERSE_TXCLKESC,
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3430_dss_supported_displays,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
+};
+
+static const struct omap_dss_features omap3630_dss_features = {
+        .reg_fields = omap3_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+        .has_feature    =
+                FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
+                FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
+                FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED |
+                FEAT_ROWREPEATENABLE | FEAT_LINEBUFFERSPLIT |
+                FEAT_RESIZECONF | FEAT_DSI_PLL_PWR_BUG |
+                FEAT_DSI_PLL_FREQSEL,
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3630_dss_supported_displays,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
+};
+
+/* OMAP4 DSS Features */
+/* For OMAP4430 ES 1.0 revision */
+static const struct omap_dss_features omap4430_es1_0_dss_features  = {
+        .reg_fields = omap4_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+        .has_feature    =
+                FEAT_GLOBAL_ALPHA | FEAT_PRE_MULT_ALPHA |
+                FEAT_MGR_LCD2 | FEAT_GLOBAL_ALPHA_VID1 |
+                FEAT_CORE_CLK_DIV | FEAT_LCD_CLK_SRC |
+                FEAT_DSI_DCS_CMD_CONFIG_VC | FEAT_DSI_VC_OCP_WIDTH |
+                FEAT_DSI_GNQ | FEAT_HANDLE_UV_SEPARATE | FEAT_ATTR2,
+
+        .num_mgrs = 3,
+        .num_ovls = 3,
+        .supported_displays = omap4_dss_supported_displays,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .clksrc_names = omap4_dss_clk_source_names,
+        .dss_params = omap4_dss_param_range,
+};
+
+/* For all the other OMAP4 versions */
+static const struct omap_dss_features omap4_dss_features = {
+        .reg_fields = omap4_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+        .has_feature    =
+                FEAT_GLOBAL_ALPHA | FEAT_PRE_MULT_ALPHA |
+                FEAT_MGR_LCD2 | FEAT_GLOBAL_ALPHA_VID1 |
+                FEAT_CORE_CLK_DIV | FEAT_LCD_CLK_SRC |
+                FEAT_DSI_DCS_CMD_CONFIG_VC | FEAT_DSI_VC_OCP_WIDTH |
+                FEAT_DSI_GNQ | FEAT_HDMI_CTS_SWMODE |
+                FEAT_HANDLE_UV_SEPARATE | FEAT_ATTR2,
+
+        .num_mgrs = 3,
+        .num_ovls = 3,
+        .supported_displays = omap4_dss_supported_displays,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .clksrc_names = omap4_dss_clk_source_names,
+        .dss_params = omap4_dss_param_range,
+};
+
+/* 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;
+}
+
+unsigned long dss_feat_get_param_min(enum dss_range_param param)
+{
+        return omap_current_dss_features->dss_params[param].min;
+}
+
+unsigned long dss_feat_get_param_max(enum dss_range_param param)
+{
+        return omap_current_dss_features->dss_params[param].max;
+}
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel)
+{
+        return omap_current_dss_features->supported_displays[channel];
+}
+
+enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
+{
+        return omap_current_dss_features->supported_color_modes[plane];
+}
+
+bool dss_feat_color_mode_supported(enum omap_plane plane,
+                enum omap_color_mode color_mode)
+{
+        return omap_current_dss_features->supported_color_modes[plane] &
+                        color_mode;
+}
+
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id)
+{
+        return omap_current_dss_features->clksrc_names[id];
+}
+
+/* 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 if (cpu_is_omap3630())
+                omap_current_dss_features = &omap3630_dss_features;
+        else if (cpu_is_omap34xx())
+                omap_current_dss_features = &omap3430_dss_features;
+        else if (omap_rev() == OMAP4430_REV_ES1_0)
+                omap_current_dss_features = &omap4430_es1_0_dss_features;
+        else if (cpu_is_omap44xx())
+                omap_current_dss_features = &omap4_dss_features;
+        else
+                DSSWARN("Unsupported OMAP version");
+}
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
new file mode 100644
index 000000000000..07b346f7d916
--- /dev/null
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -0,0 +1,96 @@
+/*
+ * 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        3
+#define MAX_DSS_OVERLAYS        3
+#define MAX_DSS_LCD_MANAGERS    2
+#define MAX_NUM_DSI             2
+
+/* DSS has feature id */
+enum dss_feat_id {
+        FEAT_GLOBAL_ALPHA               = 1 << 0,
+        FEAT_GLOBAL_ALPHA_VID1          = 1 << 1,
+        FEAT_PRE_MULT_ALPHA             = 1 << 2,
+        FEAT_LCDENABLEPOL               = 1 << 3,
+        FEAT_LCDENABLESIGNAL            = 1 << 4,
+        FEAT_PCKFREEENABLE              = 1 << 5,
+        FEAT_FUNCGATED                  = 1 << 6,
+        FEAT_MGR_LCD2                   = 1 << 7,
+        FEAT_LINEBUFFERSPLIT            = 1 << 8,
+        FEAT_ROWREPEATENABLE            = 1 << 9,
+        FEAT_RESIZECONF                 = 1 << 10,
+        /* Independent core clk divider */
+        FEAT_CORE_CLK_DIV               = 1 << 11,
+        FEAT_LCD_CLK_SRC                = 1 << 12,
+        /* DSI-PLL power command 0x3 is not working */
+        FEAT_DSI_PLL_PWR_BUG            = 1 << 13,
+        FEAT_DSI_PLL_FREQSEL            = 1 << 14,
+        FEAT_DSI_DCS_CMD_CONFIG_VC      = 1 << 15,
+        FEAT_DSI_VC_OCP_WIDTH           = 1 << 16,
+        FEAT_DSI_REVERSE_TXCLKESC       = 1 << 17,
+        FEAT_DSI_GNQ                    = 1 << 18,
+        FEAT_HDMI_CTS_SWMODE            = 1 << 19,
+        FEAT_HANDLE_UV_SEPARATE         = 1 << 20,
+        FEAT_ATTR2                      = 1 << 21,
+};
+
+/* DSS register field id */
+enum dss_feat_reg_field {
+        FEAT_REG_FIRHINC,
+        FEAT_REG_FIRVINC,
+        FEAT_REG_FIFOHIGHTHRESHOLD,
+        FEAT_REG_FIFOLOWTHRESHOLD,
+        FEAT_REG_FIFOSIZE,
+        FEAT_REG_HORIZONTALACCU,
+        FEAT_REG_VERTICALACCU,
+        FEAT_REG_DISPC_CLK_SWITCH,
+        FEAT_REG_DSIPLL_REGN,
+        FEAT_REG_DSIPLL_REGM,
+        FEAT_REG_DSIPLL_REGM_DISPC,
+        FEAT_REG_DSIPLL_REGM_DSI,
+};
+
+enum dss_range_param {
+        FEAT_PARAM_DSS_FCK,
+        FEAT_PARAM_DSIPLL_REGN,
+        FEAT_PARAM_DSIPLL_REGM,
+        FEAT_PARAM_DSIPLL_REGM_DISPC,
+        FEAT_PARAM_DSIPLL_REGM_DSI,
+        FEAT_PARAM_DSIPLL_FINT,
+        FEAT_PARAM_DSIPLL_LPDIV,
+};
+
+/* DSS Feature Functions */
+int dss_feat_get_num_mgrs(void);
+int dss_feat_get_num_ovls(void);
+unsigned long dss_feat_get_param_min(enum dss_range_param param);
+unsigned long dss_feat_get_param_max(enum dss_range_param param);
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
+enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane);
+bool dss_feat_color_mode_supported(enum omap_plane plane,
+                enum omap_color_mode color_mode);
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id);
+
+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/hdmi.c b/drivers/video/omap2/dss/hdmi.c
new file mode 100644
index 000000000000..b0555f4f0a78
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -0,0 +1,1763 @@
+/*
+ * hdmi.c
+ *
+ * HDMI interface DSS driver setting for TI's OMAP4 family of processor.
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors: Yong Zhi
+ *      Mythri pk <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <video/omapdss.h>
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+        defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+#include <sound/soc.h>
+#include <sound/pcm_params.h>
+#endif
+
+#include "dss.h"
+#include "hdmi.h"
+#include "dss_features.h"
+
+static struct {
+        struct mutex lock;
+        struct omap_display_platform_data *pdata;
+        struct platform_device *pdev;
+        void __iomem *base_wp;  /* HDMI wrapper */
+        int code;
+        int mode;
+        u8 edid[HDMI_EDID_MAX_LENGTH];
+        u8 edid_set;
+        bool custom_set;
+        struct hdmi_config cfg;
+} hdmi;
+
+/*
+ * Logic for the below structure :
+ * user enters the CEA or VESA timings by specifying the HDMI/DVI code.
+ * There is a correspondence between CEA/VESA timing and code, please
+ * refer to section 6.3 in HDMI 1.3 specification for timing code.
+ *
+ * In the below structure, cea_vesa_timings corresponds to all OMAP4
+ * supported CEA and VESA timing values.code_cea corresponds to the CEA
+ * code, It is used to get the timing from cea_vesa_timing array.Similarly
+ * with code_vesa. Code_index is used for back mapping, that is once EDID
+ * is read from the TV, EDID is parsed to find the timing values and then
+ * map it to corresponding CEA or VESA index.
+ */
+
+static const struct hdmi_timings cea_vesa_timings[OMAP_HDMI_TIMINGS_NB] = {
+        { {640, 480, 25200, 96, 16, 48, 2, 10, 33} , 0 , 0},
+        { {1280, 720, 74250, 40, 440, 220, 5, 5, 20}, 1, 1},
+        { {1280, 720, 74250, 40, 110, 220, 5, 5, 20}, 1, 1},
+        { {720, 480, 27027, 62, 16, 60, 6, 9, 30}, 0, 0},
+        { {2880, 576, 108000, 256, 48, 272, 5, 5, 39}, 0, 0},
+        { {1440, 240, 27027, 124, 38, 114, 3, 4, 15}, 0, 0},
+        { {1440, 288, 27000, 126, 24, 138, 3, 2, 19}, 0, 0},
+        { {1920, 540, 74250, 44, 528, 148, 5, 2, 15}, 1, 1},
+        { {1920, 540, 74250, 44, 88, 148, 5, 2, 15}, 1, 1},
+        { {1920, 1080, 148500, 44, 88, 148, 5, 4, 36}, 1, 1},
+        { {720, 576, 27000, 64, 12, 68, 5, 5, 39}, 0, 0},
+        { {1440, 576, 54000, 128, 24, 136, 5, 5, 39}, 0, 0},
+        { {1920, 1080, 148500, 44, 528, 148, 5, 4, 36}, 1, 1},
+        { {2880, 480, 108108, 248, 64, 240, 6, 9, 30}, 0, 0},
+        { {1920, 1080, 74250, 44, 638, 148, 5, 4, 36}, 1, 1},
+        /* VESA From Here */
+        { {640, 480, 25175, 96, 16, 48, 2 , 11, 31}, 0, 0},
+        { {800, 600, 40000, 128, 40, 88, 4 , 1, 23}, 1, 1},
+        { {848, 480, 33750, 112, 16, 112, 8 , 6, 23}, 1, 1},
+        { {1280, 768, 79500, 128, 64, 192, 7 , 3, 20}, 1, 0},
+        { {1280, 800, 83500, 128, 72, 200, 6 , 3, 22}, 1, 0},
+        { {1360, 768, 85500, 112, 64, 256, 6 , 3, 18}, 1, 1},
+        { {1280, 960, 108000, 112, 96, 312, 3 , 1, 36}, 1, 1},
+        { {1280, 1024, 108000, 112, 48, 248, 3 , 1, 38}, 1, 1},
+        { {1024, 768, 65000, 136, 24, 160, 6, 3, 29}, 0, 0},
+        { {1400, 1050, 121750, 144, 88, 232, 4, 3, 32}, 1, 0},
+        { {1440, 900, 106500, 152, 80, 232, 6, 3, 25}, 1, 0},
+        { {1680, 1050, 146250, 176 , 104, 280, 6, 3, 30}, 1, 0},
+        { {1366, 768, 85500, 143, 70, 213, 3, 3, 24}, 1, 1},
+        { {1920, 1080, 148500, 44, 148, 80, 5, 4, 36}, 1, 1},
+        { {1280, 768, 68250, 32, 48, 80, 7, 3, 12}, 0, 1},
+        { {1400, 1050, 101000, 32, 48, 80, 4, 3, 23}, 0, 1},
+        { {1680, 1050, 119000, 32, 48, 80, 6, 3, 21}, 0, 1},
+        { {1280, 800, 79500, 32, 48, 80, 6, 3, 14}, 0, 1},
+        { {1280, 720, 74250, 40, 110, 220, 5, 5, 20}, 1, 1}
+};
+
+/*
+ * This is a static mapping array which maps the timing values
+ * with corresponding CEA / VESA code
+ */
+static const int code_index[OMAP_HDMI_TIMINGS_NB] = {
+        1, 19, 4, 2, 37, 6, 21, 20, 5, 16, 17, 29, 31, 35, 32,
+        /* <--15 CEA 17--> vesa*/
+        4, 9, 0xE, 0x17, 0x1C, 0x27, 0x20, 0x23, 0x10, 0x2A,
+        0X2F, 0x3A, 0X51, 0X52, 0x16, 0x29, 0x39, 0x1B
+};
+
+/*
+ * This is reverse static mapping which maps the CEA / VESA code
+ * to the corresponding timing values
+ */
+static const int code_cea[39] = {
+        -1,  0,  3,  3,  2,  8,  5,  5, -1, -1,
+        -1, -1, -1, -1, -1, -1,  9, 10, 10,  1,
+        7,   6,  6, -1, -1, -1, -1, -1, -1, 11,
+        11, 12, 14, -1, -1, 13, 13,  4,  4
+};
+
+static const int code_vesa[85] = {
+        -1, -1, -1, -1, 15, -1, -1, -1, -1, 16,
+        -1, -1, -1, -1, 17, -1, 23, -1, -1, -1,
+        -1, -1, 29, 18, -1, -1, -1, 32, 19, -1,
+        -1, -1, 21, -1, -1, 22, -1, -1, -1, 20,
+        -1, 30, 24, -1, -1, -1, -1, 25, -1, -1,
+        -1, -1, -1, -1, -1, -1, -1, 31, 26, -1,
+        -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+        -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+        -1, 27, 28, -1, 33};
+
+static const u8 edid_header[8] = {0x0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0};
+
+static inline void hdmi_write_reg(const struct hdmi_reg idx, u32 val)
+{
+        __raw_writel(val, hdmi.base_wp + idx.idx);
+}
+
+static inline u32 hdmi_read_reg(const struct hdmi_reg idx)
+{
+        return __raw_readl(hdmi.base_wp + idx.idx);
+}
+
+static inline int hdmi_wait_for_bit_change(const struct hdmi_reg idx,
+                                int b2, int b1, u32 val)
+{
+        u32 t = 0;
+        while (val != REG_GET(idx, b2, b1)) {
+                udelay(1);
+                if (t++ > 10000)
+                        return !val;
+        }
+        return val;
+}
+
+int hdmi_init_display(struct omap_dss_device *dssdev)
+{
+        DSSDBG("init_display\n");
+
+        return 0;
+}
+
+static int hdmi_pll_init(enum hdmi_clk_refsel refsel, int dcofreq,
+                struct hdmi_pll_info *fmt, u16 sd)
+{
+        u32 r;
+
+        /* PLL start always use manual mode */
+        REG_FLD_MOD(PLLCTRL_PLL_CONTROL, 0x0, 0, 0);
+
+        r = hdmi_read_reg(PLLCTRL_CFG1);
+        r = FLD_MOD(r, fmt->regm, 20, 9); /* CFG1_PLL_REGM */
+        r = FLD_MOD(r, fmt->regn, 8, 1);  /* CFG1_PLL_REGN */
+
+        hdmi_write_reg(PLLCTRL_CFG1, r);
+
+        r = hdmi_read_reg(PLLCTRL_CFG2);
+
+        r = FLD_MOD(r, 0x0, 12, 12); /* PLL_HIGHFREQ divide by 2 */
+        r = FLD_MOD(r, 0x1, 13, 13); /* PLL_REFEN */
+        r = FLD_MOD(r, 0x0, 14, 14); /* PHY_CLKINEN de-assert during locking */
+
+        if (dcofreq) {
+                /* divider programming for frequency beyond 1000Mhz */
+                REG_FLD_MOD(PLLCTRL_CFG3, sd, 17, 10);
+                r = FLD_MOD(r, 0x4, 3, 1); /* 1000MHz and 2000MHz */
+        } else {
+                r = FLD_MOD(r, 0x2, 3, 1); /* 500MHz and 1000MHz */
+        }
+
+        hdmi_write_reg(PLLCTRL_CFG2, r);
+
+        r = hdmi_read_reg(PLLCTRL_CFG4);
+        r = FLD_MOD(r, fmt->regm2, 24, 18);
+        r = FLD_MOD(r, fmt->regmf, 17, 0);
+
+        hdmi_write_reg(PLLCTRL_CFG4, r);
+
+        /* go now */
+        REG_FLD_MOD(PLLCTRL_PLL_GO, 0x1, 0, 0);
+
+        /* wait for bit change */
+        if (hdmi_wait_for_bit_change(PLLCTRL_PLL_GO, 0, 0, 1) != 1) {
+                DSSERR("PLL GO bit not set\n");
+                return -ETIMEDOUT;
+        }
+
+        /* Wait till the lock bit is set in PLL status */
+        if (hdmi_wait_for_bit_change(PLLCTRL_PLL_STATUS, 1, 1, 1) != 1) {
+                DSSWARN("cannot lock PLL\n");
+                DSSWARN("CFG1 0x%x\n",
+                        hdmi_read_reg(PLLCTRL_CFG1));
+                DSSWARN("CFG2 0x%x\n",
+                        hdmi_read_reg(PLLCTRL_CFG2));
+                DSSWARN("CFG4 0x%x\n",
+                        hdmi_read_reg(PLLCTRL_CFG4));
+                return -ETIMEDOUT;
+        }
+
+        DSSDBG("PLL locked!\n");
+
+        return 0;
+}
+
+/* PHY_PWR_CMD */
+static int hdmi_set_phy_pwr(enum hdmi_phy_pwr val)
+{
+        /* Command for power control of HDMI PHY */
+        REG_FLD_MOD(HDMI_WP_PWR_CTRL, val, 7, 6);
+
+        /* Status of the power control of HDMI PHY */
+        if (hdmi_wait_for_bit_change(HDMI_WP_PWR_CTRL, 5, 4, val) != val) {
+                DSSERR("Failed to set PHY power mode to %d\n", val);
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+/* PLL_PWR_CMD */
+static int hdmi_set_pll_pwr(enum hdmi_pll_pwr val)
+{
+        /* Command for power control of HDMI PLL */
+        REG_FLD_MOD(HDMI_WP_PWR_CTRL, val, 3, 2);
+
+        /* wait till PHY_PWR_STATUS is set */
+        if (hdmi_wait_for_bit_change(HDMI_WP_PWR_CTRL, 1, 0, val) != val) {
+                DSSERR("Failed to set PHY_PWR_STATUS\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_pll_reset(void)
+{
+        /* SYSRESET  controlled by power FSM */
+        REG_FLD_MOD(PLLCTRL_PLL_CONTROL, 0x0, 3, 3);
+
+        /* READ 0x0 reset is in progress */
+        if (hdmi_wait_for_bit_change(PLLCTRL_PLL_STATUS, 0, 0, 1) != 1) {
+                DSSERR("Failed to sysreset PLL\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_phy_init(void)
+{
+        u16 r = 0;
+
+        r = hdmi_set_phy_pwr(HDMI_PHYPWRCMD_LDOON);
+        if (r)
+                return r;
+
+        r = hdmi_set_phy_pwr(HDMI_PHYPWRCMD_TXON);
+        if (r)
+                return r;
+
+        /*
+         * Read address 0 in order to get the SCP reset done completed
+         * Dummy access performed to make sure reset is done
+         */
+        hdmi_read_reg(HDMI_TXPHY_TX_CTRL);
+
+        /*
+         * Write to phy address 0 to configure the clock
+         * use HFBITCLK write HDMI_TXPHY_TX_CONTROL_FREQOUT field
+         */
+        REG_FLD_MOD(HDMI_TXPHY_TX_CTRL, 0x1, 31, 30);
+
+        /* Write to phy address 1 to start HDMI line (TXVALID and TMDSCLKEN) */
+        hdmi_write_reg(HDMI_TXPHY_DIGITAL_CTRL, 0xF0000000);
+
+        /* Setup max LDO voltage */
+        REG_FLD_MOD(HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
+
+        /* Write to phy address 3 to change the polarity control */
+        REG_FLD_MOD(HDMI_TXPHY_PAD_CFG_CTRL, 0x1, 27, 27);
+
+        return 0;
+}
+
+static int hdmi_wait_softreset(void)
+{
+        /* reset W1 */
+        REG_FLD_MOD(HDMI_WP_SYSCONFIG, 0x1, 0, 0);
+
+        /* wait till SOFTRESET == 0 */
+        if (hdmi_wait_for_bit_change(HDMI_WP_SYSCONFIG, 0, 0, 0) != 0) {
+                DSSERR("sysconfig reset failed\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_pll_program(struct hdmi_pll_info *fmt)
+{
+        u16 r = 0;
+        enum hdmi_clk_refsel refsel;
+
+        /* wait for wrapper reset */
+        r = hdmi_wait_softreset();
+        if (r)
+                return r;
+
+        r = hdmi_set_pll_pwr(HDMI_PLLPWRCMD_ALLOFF);
+        if (r)
+                return r;
+
+        r = hdmi_set_pll_pwr(HDMI_PLLPWRCMD_BOTHON_ALLCLKS);
+        if (r)
+                return r;
+
+        r = hdmi_pll_reset();
+        if (r)
+                return r;
+
+        refsel = HDMI_REFSEL_SYSCLK;
+
+        r = hdmi_pll_init(refsel, fmt->dcofreq, fmt, fmt->regsd);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void hdmi_phy_off(void)
+{
+        hdmi_set_phy_pwr(HDMI_PHYPWRCMD_OFF);
+}
+
+static int hdmi_core_ddc_edid(u8 *pedid, int ext)
+{
+        u32 i, j;
+        char checksum = 0;
+        u32 offset = 0;
+
+        /* Turn on CLK for DDC */
+        REG_FLD_MOD(HDMI_CORE_AV_DPD, 0x7, 2, 0);
+
+        /*
+         * SW HACK : Without the Delay DDC(i2c bus) reads 0 values /
+         * right shifted values( The behavior is not consistent and seen only
+         * with some TV's)
+         */
+        usleep_range(800, 1000);
+
+        if (!ext) {
+                /* Clk SCL Devices */
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0xA, 3, 0);
+
+                /* HDMI_CORE_DDC_STATUS_IN_PROG */
+                if (hdmi_wait_for_bit_change(HDMI_CORE_DDC_STATUS,
+                                                4, 4, 0) != 0) {
+                        DSSERR("Failed to program DDC\n");
+                        return -ETIMEDOUT;
+                }
+
+                /* Clear FIFO */
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x9, 3, 0);
+
+                /* HDMI_CORE_DDC_STATUS_IN_PROG */
+                if (hdmi_wait_for_bit_change(HDMI_CORE_DDC_STATUS,
+                                                4, 4, 0) != 0) {
+                        DSSERR("Failed to program DDC\n");
+                        return -ETIMEDOUT;
+                }
+
+        } else {
+                if (ext % 2 != 0)
+                        offset = 0x80;
+        }
+
+        /* Load Segment Address Register */
+        REG_FLD_MOD(HDMI_CORE_DDC_SEGM, ext/2, 7, 0);
+
+        /* Load Slave Address Register */
+        REG_FLD_MOD(HDMI_CORE_DDC_ADDR, 0xA0 >> 1, 7, 1);
+
+        /* Load Offset Address Register */
+        REG_FLD_MOD(HDMI_CORE_DDC_OFFSET, offset, 7, 0);
+
+        /* Load Byte Count */
+        REG_FLD_MOD(HDMI_CORE_DDC_COUNT1, 0x80, 7, 0);
+        REG_FLD_MOD(HDMI_CORE_DDC_COUNT2, 0x0, 1, 0);
+
+        /* Set DDC_CMD */
+        if (ext)
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x4, 3, 0);
+        else
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x2, 3, 0);
+
+        /* HDMI_CORE_DDC_STATUS_BUS_LOW */
+        if (REG_GET(HDMI_CORE_DDC_STATUS, 6, 6) == 1) {
+                DSSWARN("I2C Bus Low?\n");
+                return -EIO;
+        }
+        /* HDMI_CORE_DDC_STATUS_NO_ACK */
+        if (REG_GET(HDMI_CORE_DDC_STATUS, 5, 5) == 1) {
+                DSSWARN("I2C No Ack\n");
+                return -EIO;
+        }
+
+        i = ext * 128;
+        j = 0;
+        while (((REG_GET(HDMI_CORE_DDC_STATUS, 4, 4) == 1) ||
+                        (REG_GET(HDMI_CORE_DDC_STATUS, 2, 2) == 0)) &&
+                        j < 128) {
+
+                if (REG_GET(HDMI_CORE_DDC_STATUS, 2, 2) == 0) {
+                        /* FIFO not empty */
+                        pedid[i++] = REG_GET(HDMI_CORE_DDC_DATA, 7, 0);
+                        j++;
+                }
+        }
+
+        for (j = 0; j < 128; j++)
+                checksum += pedid[j];
+
+        if (checksum != 0) {
+                DSSERR("E-EDID checksum failed!!\n");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int read_edid(u8 *pedid, u16 max_length)
+{
+        int r = 0, n = 0, i = 0;
+        int max_ext_blocks = (max_length / 128) - 1;
+
+        r = hdmi_core_ddc_edid(pedid, 0);
+        if (r) {
+                return r;
+        } else {
+                n = pedid[0x7e];
+
+                /*
+                 * README: need to comply with max_length set by the caller.
+                 * Better implementation should be to allocate necessary
+                 * memory to store EDID according to nb_block field found
+                 * in first block
+                 */
+                if (n > max_ext_blocks)
+                        n = max_ext_blocks;
+
+                for (i = 1; i <= n; i++) {
+                        r = hdmi_core_ddc_edid(pedid, i);
+                        if (r)
+                                return r;
+                }
+        }
+        return 0;
+}
+
+static int get_timings_index(void)
+{
+        int code;
+
+        if (hdmi.mode == 0)
+                code = code_vesa[hdmi.code];
+        else
+                code = code_cea[hdmi.code];
+
+        if (code == -1) {
+                /* HDMI code 4 corresponds to 640 * 480 VGA */
+                hdmi.code = 4;
+                /* DVI mode 1 corresponds to HDMI 0 to DVI */
+                hdmi.mode = HDMI_DVI;
+
+                code = code_vesa[hdmi.code];
+        }
+        return code;
+}
+
+static struct hdmi_cm hdmi_get_code(struct omap_video_timings *timing)
+{
+        int i = 0, code = -1, temp_vsync = 0, temp_hsync = 0;
+        int timing_vsync = 0, timing_hsync = 0;
+        struct omap_video_timings temp;
+        struct hdmi_cm cm = {-1};
+        DSSDBG("hdmi_get_code\n");
+
+        for (i = 0; i < OMAP_HDMI_TIMINGS_NB; i++) {
+                temp = cea_vesa_timings[i].timings;
+                if ((temp.pixel_clock == timing->pixel_clock) &&
+                        (temp.x_res == timing->x_res) &&
+                        (temp.y_res == timing->y_res)) {
+
+                        temp_hsync = temp.hfp + temp.hsw + temp.hbp;
+                        timing_hsync = timing->hfp + timing->hsw + timing->hbp;
+                        temp_vsync = temp.vfp + temp.vsw + temp.vbp;
+                        timing_vsync = timing->vfp + timing->vsw + timing->vbp;
+
+                        DSSDBG("temp_hsync = %d , temp_vsync = %d"
+                                "timing_hsync = %d, timing_vsync = %d\n",
+                                temp_hsync, temp_hsync,
+                                timing_hsync, timing_vsync);
+
+                        if ((temp_hsync == timing_hsync) &&
+                                        (temp_vsync == timing_vsync)) {
+                                code = i;
+                                cm.code = code_index[i];
+                                if (code < 14)
+                                        cm.mode = HDMI_HDMI;
+                                else
+                                        cm.mode = HDMI_DVI;
+                                DSSDBG("Hdmi_code = %d mode = %d\n",
+                                         cm.code, cm.mode);
+                                break;
+                         }
+                }
+        }
+
+        return cm;
+}
+
+static void get_horz_vert_timing_info(int current_descriptor_addrs, u8 *edid ,
+                struct omap_video_timings *timings)
+{
+        /* X and Y resolution */
+        timings->x_res = (((edid[current_descriptor_addrs + 4] & 0xF0) << 4) |
+                         edid[current_descriptor_addrs + 2]);
+        timings->y_res = (((edid[current_descriptor_addrs + 7] & 0xF0) << 4) |
+                         edid[current_descriptor_addrs + 5]);
+
+        timings->pixel_clock = ((edid[current_descriptor_addrs + 1] << 8) |
+                                edid[current_descriptor_addrs]);
+
+        timings->pixel_clock = 10 * timings->pixel_clock;
+
+        /* HORIZONTAL FRONT PORCH */
+        timings->hfp = edid[current_descriptor_addrs + 8] |
+                        ((edid[current_descriptor_addrs + 11] & 0xc0) << 2);
+        /* HORIZONTAL SYNC WIDTH */
+        timings->hsw = edid[current_descriptor_addrs + 9] |
+                        ((edid[current_descriptor_addrs + 11] & 0x30) << 4);
+        /* HORIZONTAL BACK PORCH */
+        timings->hbp = (((edid[current_descriptor_addrs + 4] & 0x0F) << 8) |
+                        edid[current_descriptor_addrs + 3]) -
+                        (timings->hfp + timings->hsw);
+        /* VERTICAL FRONT PORCH */
+        timings->vfp = ((edid[current_descriptor_addrs + 10] & 0xF0) >> 4) |
+                        ((edid[current_descriptor_addrs + 11] & 0x0f) << 2);
+        /* VERTICAL SYNC WIDTH */
+        timings->vsw = (edid[current_descriptor_addrs + 10] & 0x0F) |
+                        ((edid[current_descriptor_addrs + 11] & 0x03) << 4);
+        /* VERTICAL BACK PORCH */
+        timings->vbp = (((edid[current_descriptor_addrs + 7] & 0x0F) << 8) |
+                        edid[current_descriptor_addrs + 6]) -
+                        (timings->vfp + timings->vsw);
+
+}
+
+/* Description : This function gets the resolution information from EDID */
+static void get_edid_timing_data(u8 *edid)
+{
+        u8 count;
+        u16 current_descriptor_addrs;
+        struct hdmi_cm cm;
+        struct omap_video_timings edid_timings;
+
+        /* search block 0, there are 4 DTDs arranged in priority order */
+        for (count = 0; count < EDID_SIZE_BLOCK0_TIMING_DESCRIPTOR; count++) {
+                current_descriptor_addrs =
+                        EDID_DESCRIPTOR_BLOCK0_ADDRESS +
+                        count * EDID_TIMING_DESCRIPTOR_SIZE;
+                get_horz_vert_timing_info(current_descriptor_addrs,
+                                edid, &edid_timings);
+                cm = hdmi_get_code(&edid_timings);
+                DSSDBG("Block0[%d] value matches code = %d , mode = %d\n",
+                        count, cm.code, cm.mode);
+                if (cm.code == -1) {
+                        continue;
+                } else {
+                        hdmi.code = cm.code;
+                        hdmi.mode = cm.mode;
+                        DSSDBG("code = %d , mode = %d\n",
+                                hdmi.code, hdmi.mode);
+                        return;
+                }
+        }
+        if (edid[0x7e] != 0x00) {
+                for (count = 0; count < EDID_SIZE_BLOCK1_TIMING_DESCRIPTOR;
+                        count++) {
+                        current_descriptor_addrs =
+                        EDID_DESCRIPTOR_BLOCK1_ADDRESS +
+                        count * EDID_TIMING_DESCRIPTOR_SIZE;
+                        get_horz_vert_timing_info(current_descriptor_addrs,
+                                                edid, &edid_timings);
+                        cm = hdmi_get_code(&edid_timings);
+                        DSSDBG("Block1[%d] value matches code = %d, mode = %d",
+                                count, cm.code, cm.mode);
+                        if (cm.code == -1) {
+                                continue;
+                        } else {
+                                hdmi.code = cm.code;
+                                hdmi.mode = cm.mode;
+                                DSSDBG("code = %d , mode = %d\n",
+                                        hdmi.code, hdmi.mode);
+                                return;
+                        }
+                }
+        }
+
+        DSSINFO("no valid timing found , falling back to VGA\n");
+        hdmi.code = 4; /* setting default value of 640 480 VGA */
+        hdmi.mode = HDMI_DVI;
+}
+
+static void hdmi_read_edid(struct omap_video_timings *dp)
+{
+        int ret = 0, code;
+
+        memset(hdmi.edid, 0, HDMI_EDID_MAX_LENGTH);
+
+        if (!hdmi.edid_set)
+                ret = read_edid(hdmi.edid, HDMI_EDID_MAX_LENGTH);
+
+        if (!ret) {
+                if (!memcmp(hdmi.edid, edid_header, sizeof(edid_header))) {
+                        /* search for timings of default resolution */
+                        get_edid_timing_data(hdmi.edid);
+                        hdmi.edid_set = true;
+                }
+        } else {
+                DSSWARN("failed to read E-EDID\n");
+        }
+
+        if (!hdmi.edid_set) {
+                DSSINFO("fallback to VGA\n");
+                hdmi.code = 4; /* setting default value of 640 480 VGA */
+                hdmi.mode = HDMI_DVI;
+        }
+
+        code = get_timings_index();
+
+        *dp = cea_vesa_timings[code].timings;
+}
+
+static void hdmi_core_init(struct hdmi_core_video_config *video_cfg,
+                        struct hdmi_core_infoframe_avi *avi_cfg,
+                        struct hdmi_core_packet_enable_repeat *repeat_cfg)
+{
+        DSSDBG("Enter hdmi_core_init\n");
+
+        /* video core */
+        video_cfg->ip_bus_width = HDMI_INPUT_8BIT;
+        video_cfg->op_dither_truc = HDMI_OUTPUTTRUNCATION_8BIT;
+        video_cfg->deep_color_pkt = HDMI_DEEPCOLORPACKECTDISABLE;
+        video_cfg->pkt_mode = HDMI_PACKETMODERESERVEDVALUE;
+        video_cfg->hdmi_dvi = HDMI_DVI;
+        video_cfg->tclk_sel_clkmult = HDMI_FPLL10IDCK;
+
+        /* info frame */
+        avi_cfg->db1_format = 0;
+        avi_cfg->db1_active_info = 0;
+        avi_cfg->db1_bar_info_dv = 0;
+        avi_cfg->db1_scan_info = 0;
+        avi_cfg->db2_colorimetry = 0;
+        avi_cfg->db2_aspect_ratio = 0;
+        avi_cfg->db2_active_fmt_ar = 0;
+        avi_cfg->db3_itc = 0;
+        avi_cfg->db3_ec = 0;
+        avi_cfg->db3_q_range = 0;
+        avi_cfg->db3_nup_scaling = 0;
+        avi_cfg->db4_videocode = 0;
+        avi_cfg->db5_pixel_repeat = 0;
+        avi_cfg->db6_7_line_eoftop = 0 ;
+        avi_cfg->db8_9_line_sofbottom = 0;
+        avi_cfg->db10_11_pixel_eofleft = 0;
+        avi_cfg->db12_13_pixel_sofright = 0;
+
+        /* packet enable and repeat */
+        repeat_cfg->audio_pkt = 0;
+        repeat_cfg->audio_pkt_repeat = 0;
+        repeat_cfg->avi_infoframe = 0;
+        repeat_cfg->avi_infoframe_repeat = 0;
+        repeat_cfg->gen_cntrl_pkt = 0;
+        repeat_cfg->gen_cntrl_pkt_repeat = 0;
+        repeat_cfg->generic_pkt = 0;
+        repeat_cfg->generic_pkt_repeat = 0;
+}
+
+static void hdmi_core_powerdown_disable(void)
+{
+        DSSDBG("Enter hdmi_core_powerdown_disable\n");
+        REG_FLD_MOD(HDMI_CORE_CTRL1, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_release(void)
+{
+        DSSDBG("Enter hdmi_core_swreset_release\n");
+        REG_FLD_MOD(HDMI_CORE_SYS_SRST, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_assert(void)
+{
+        DSSDBG("Enter hdmi_core_swreset_assert\n");
+        REG_FLD_MOD(HDMI_CORE_SYS_SRST, 0x1, 0, 0);
+}
+
+/* DSS_HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_video_config *cfg)
+{
+        u32 r = 0;
+
+        /* sys_ctrl1 default configuration not tunable */
+        r = hdmi_read_reg(HDMI_CORE_CTRL1);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_VEN_FOLLOWVSYNC, 5, 5);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_HEN_FOLLOWHSYNC, 4, 4);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_BSEL_24BITBUS, 2, 2);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_EDGE_RISINGEDGE, 1, 1);
+        hdmi_write_reg(HDMI_CORE_CTRL1, r);
+
+        REG_FLD_MOD(HDMI_CORE_SYS_VID_ACEN, cfg->ip_bus_width, 7, 6);
+
+        /* Vid_Mode */
+        r = hdmi_read_reg(HDMI_CORE_SYS_VID_MODE);
+
+        /* dither truncation configuration */
+        if (cfg->op_dither_truc > HDMI_OUTPUTTRUNCATION_12BIT) {
+                r = FLD_MOD(r, cfg->op_dither_truc - 3, 7, 6);
+                r = FLD_MOD(r, 1, 5, 5);
+        } else {
+                r = FLD_MOD(r, cfg->op_dither_truc, 7, 6);
+                r = FLD_MOD(r, 0, 5, 5);
+        }
+        hdmi_write_reg(HDMI_CORE_SYS_VID_MODE, r);
+
+        /* HDMI_Ctrl */
+        r = hdmi_read_reg(HDMI_CORE_AV_HDMI_CTRL);
+        r = FLD_MOD(r, cfg->deep_color_pkt, 6, 6);
+        r = FLD_MOD(r, cfg->pkt_mode, 5, 3);
+        r = FLD_MOD(r, cfg->hdmi_dvi, 0, 0);
+        hdmi_write_reg(HDMI_CORE_AV_HDMI_CTRL, r);
+
+        /* TMDS_CTRL */
+        REG_FLD_MOD(HDMI_CORE_SYS_TMDS_CTRL,
+                cfg->tclk_sel_clkmult, 6, 5);
+}
+
+static void hdmi_core_aux_infoframe_avi_config(
+                struct hdmi_core_infoframe_avi info_avi)
+{
+        u32 val;
+        char sum = 0, checksum = 0;
+
+        sum += 0x82 + 0x002 + 0x00D;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_TYPE, 0x082);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_VERS, 0x002);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_LEN, 0x00D);
+
+        val = (info_avi.db1_format << 5) |
+                (info_avi.db1_active_info << 4) |
+                (info_avi.db1_bar_info_dv << 2) |
+                (info_avi.db1_scan_info);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(0), val);
+        sum += val;
+
+        val = (info_avi.db2_colorimetry << 6) |
+                (info_avi.db2_aspect_ratio << 4) |
+                (info_avi.db2_active_fmt_ar);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(1), val);
+        sum += val;
+
+        val = (info_avi.db3_itc << 7) |
+                (info_avi.db3_ec << 4) |
+                (info_avi.db3_q_range << 2) |
+                (info_avi.db3_nup_scaling);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(2), val);
+        sum += val;
+
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(3), info_avi.db4_videocode);
+        sum += info_avi.db4_videocode;
+
+        val = info_avi.db5_pixel_repeat;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(4), val);
+        sum += val;
+
+        val = info_avi.db6_7_line_eoftop & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(5), val);
+        sum += val;
+
+        val = ((info_avi.db6_7_line_eoftop >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(6), val);
+        sum += val;
+
+        val = info_avi.db8_9_line_sofbottom & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(7), val);
+        sum += val;
+
+        val = ((info_avi.db8_9_line_sofbottom >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(8), val);
+        sum += val;
+
+        val = info_avi.db10_11_pixel_eofleft & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(9), val);
+        sum += val;
+
+        val = ((info_avi.db10_11_pixel_eofleft >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(10), val);
+        sum += val;
+
+        val = info_avi.db12_13_pixel_sofright & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(11), val);
+        sum += val;
+
+        val = ((info_avi.db12_13_pixel_sofright >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(12), val);
+        sum += val;
+
+        checksum = 0x100 - sum;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_CHSUM, checksum);
+}
+
+static void hdmi_core_av_packet_config(
+                struct hdmi_core_packet_enable_repeat repeat_cfg)
+{
+        /* enable/repeat the infoframe */
+        hdmi_write_reg(HDMI_CORE_AV_PB_CTRL1,
+                (repeat_cfg.audio_pkt << 5) |
+                (repeat_cfg.audio_pkt_repeat << 4) |
+                (repeat_cfg.avi_infoframe << 1) |
+                (repeat_cfg.avi_infoframe_repeat));
+
+        /* enable/repeat the packet */
+        hdmi_write_reg(HDMI_CORE_AV_PB_CTRL2,
+                (repeat_cfg.gen_cntrl_pkt << 3) |
+                (repeat_cfg.gen_cntrl_pkt_repeat << 2) |
+                (repeat_cfg.generic_pkt << 1) |
+                (repeat_cfg.generic_pkt_repeat));
+}
+
+static void hdmi_wp_init(struct omap_video_timings *timings,
+                        struct hdmi_video_format *video_fmt,
+                        struct hdmi_video_interface *video_int)
+{
+        DSSDBG("Enter hdmi_wp_init\n");
+
+        timings->hbp = 0;
+        timings->hfp = 0;
+        timings->hsw = 0;
+        timings->vbp = 0;
+        timings->vfp = 0;
+        timings->vsw = 0;
+
+        video_fmt->packing_mode = HDMI_PACK_10b_RGB_YUV444;
+        video_fmt->y_res = 0;
+        video_fmt->x_res = 0;
+
+        video_int->vsp = 0;
+        video_int->hsp = 0;
+
+        video_int->interlacing = 0;
+        video_int->tm = 0; /* HDMI_TIMING_SLAVE */
+
+}
+
+static void hdmi_wp_video_start(bool start)
+{
+        REG_FLD_MOD(HDMI_WP_VIDEO_CFG, start, 31, 31);
+}
+
+static void hdmi_wp_video_init_format(struct hdmi_video_format *video_fmt,
+        struct omap_video_timings *timings, struct hdmi_config *param)
+{
+        DSSDBG("Enter hdmi_wp_video_init_format\n");
+
+        video_fmt->y_res = param->timings.timings.y_res;
+        video_fmt->x_res = param->timings.timings.x_res;
+
+        timings->hbp = param->timings.timings.hbp;
+        timings->hfp = param->timings.timings.hfp;
+        timings->hsw = param->timings.timings.hsw;
+        timings->vbp = param->timings.timings.vbp;
+        timings->vfp = param->timings.timings.vfp;
+        timings->vsw = param->timings.timings.vsw;
+}
+
+static void hdmi_wp_video_config_format(
+                struct hdmi_video_format *video_fmt)
+{
+        u32 l = 0;
+
+        REG_FLD_MOD(HDMI_WP_VIDEO_CFG, video_fmt->packing_mode, 10, 8);
+
+        l |= FLD_VAL(video_fmt->y_res, 31, 16);
+        l |= FLD_VAL(video_fmt->x_res, 15, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_SIZE, l);
+}
+
+static void hdmi_wp_video_config_interface(
+                struct hdmi_video_interface *video_int)
+{
+        u32 r;
+        DSSDBG("Enter hdmi_wp_video_config_interface\n");
+
+        r = hdmi_read_reg(HDMI_WP_VIDEO_CFG);
+        r = FLD_MOD(r, video_int->vsp, 7, 7);
+        r = FLD_MOD(r, video_int->hsp, 6, 6);
+        r = FLD_MOD(r, video_int->interlacing, 3, 3);
+        r = FLD_MOD(r, video_int->tm, 1, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_CFG, r);
+}
+
+static void hdmi_wp_video_config_timing(
+                struct omap_video_timings *timings)
+{
+        u32 timing_h = 0;
+        u32 timing_v = 0;
+
+        DSSDBG("Enter hdmi_wp_video_config_timing\n");
+
+        timing_h |= FLD_VAL(timings->hbp, 31, 20);
+        timing_h |= FLD_VAL(timings->hfp, 19, 8);
+        timing_h |= FLD_VAL(timings->hsw, 7, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_TIMING_H, timing_h);
+
+        timing_v |= FLD_VAL(timings->vbp, 31, 20);
+        timing_v |= FLD_VAL(timings->vfp, 19, 8);
+        timing_v |= FLD_VAL(timings->vsw, 7, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_TIMING_V, timing_v);
+}
+
+static void hdmi_basic_configure(struct hdmi_config *cfg)
+{
+        /* HDMI */
+        struct omap_video_timings video_timing;
+        struct hdmi_video_format video_format;
+        struct hdmi_video_interface video_interface;
+        /* HDMI core */
+        struct hdmi_core_infoframe_avi avi_cfg;
+        struct hdmi_core_video_config v_core_cfg;
+        struct hdmi_core_packet_enable_repeat repeat_cfg;
+
+        hdmi_wp_init(&video_timing, &video_format,
+                &video_interface);
+
+        hdmi_core_init(&v_core_cfg,
+                &avi_cfg,
+                &repeat_cfg);
+
+        hdmi_wp_video_init_format(&video_format,
+                        &video_timing, cfg);
+
+        hdmi_wp_video_config_timing(&video_timing);
+
+        /* video config */
+        video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+        hdmi_wp_video_config_format(&video_format);
+
+        video_interface.vsp = cfg->timings.vsync_pol;
+        video_interface.hsp = cfg->timings.hsync_pol;
+        video_interface.interlacing = cfg->interlace;
+        video_interface.tm = 1 ; /* HDMI_TIMING_MASTER_24BIT */
+
+        hdmi_wp_video_config_interface(&video_interface);
+
+        /*
+         * configure core video part
+         * set software reset in the core
+         */
+        hdmi_core_swreset_assert();
+
+        /* power down off */
+        hdmi_core_powerdown_disable();
+
+        v_core_cfg.pkt_mode = HDMI_PACKETMODE24BITPERPIXEL;
+        v_core_cfg.hdmi_dvi = cfg->cm.mode;
+
+        hdmi_core_video_config(&v_core_cfg);
+
+        /* release software reset in the core */
+        hdmi_core_swreset_release();
+
+        /*
+         * configure packet
+         * info frame video see doc CEA861-D page 65
+         */
+        avi_cfg.db1_format = HDMI_INFOFRAME_AVI_DB1Y_RGB;
+        avi_cfg.db1_active_info =
+                HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF;
+        avi_cfg.db1_bar_info_dv = HDMI_INFOFRAME_AVI_DB1B_NO;
+        avi_cfg.db1_scan_info = HDMI_INFOFRAME_AVI_DB1S_0;
+        avi_cfg.db2_colorimetry = HDMI_INFOFRAME_AVI_DB2C_NO;
+        avi_cfg.db2_aspect_ratio = HDMI_INFOFRAME_AVI_DB2M_NO;
+        avi_cfg.db2_active_fmt_ar = HDMI_INFOFRAME_AVI_DB2R_SAME;
+        avi_cfg.db3_itc = HDMI_INFOFRAME_AVI_DB3ITC_NO;
+        avi_cfg.db3_ec = HDMI_INFOFRAME_AVI_DB3EC_XVYUV601;
+        avi_cfg.db3_q_range = HDMI_INFOFRAME_AVI_DB3Q_DEFAULT;
+        avi_cfg.db3_nup_scaling = HDMI_INFOFRAME_AVI_DB3SC_NO;
+        avi_cfg.db4_videocode = cfg->cm.code;
+        avi_cfg.db5_pixel_repeat = HDMI_INFOFRAME_AVI_DB5PR_NO;
+        avi_cfg.db6_7_line_eoftop = 0;
+        avi_cfg.db8_9_line_sofbottom = 0;
+        avi_cfg.db10_11_pixel_eofleft = 0;
+        avi_cfg.db12_13_pixel_sofright = 0;
+
+        hdmi_core_aux_infoframe_avi_config(avi_cfg);
+
+        /* enable/repeat the infoframe */
+        repeat_cfg.avi_infoframe = HDMI_PACKETENABLE;
+        repeat_cfg.avi_infoframe_repeat = HDMI_PACKETREPEATON;
+        /* wakeup */
+        repeat_cfg.audio_pkt = HDMI_PACKETENABLE;
+        repeat_cfg.audio_pkt_repeat = HDMI_PACKETREPEATON;
+        hdmi_core_av_packet_config(repeat_cfg);
+}
+
+static void update_hdmi_timings(struct hdmi_config *cfg,
+                struct omap_video_timings *timings, int code)
+{
+        cfg->timings.timings.x_res = timings->x_res;
+        cfg->timings.timings.y_res = timings->y_res;
+        cfg->timings.timings.hbp = timings->hbp;
+        cfg->timings.timings.hfp = timings->hfp;
+        cfg->timings.timings.hsw = timings->hsw;
+        cfg->timings.timings.vbp = timings->vbp;
+        cfg->timings.timings.vfp = timings->vfp;
+        cfg->timings.timings.vsw = timings->vsw;
+        cfg->timings.timings.pixel_clock = timings->pixel_clock;
+        cfg->timings.vsync_pol = cea_vesa_timings[code].vsync_pol;
+        cfg->timings.hsync_pol = cea_vesa_timings[code].hsync_pol;
+}
+
+static void hdmi_compute_pll(struct omap_dss_device *dssdev, int phy,
+                struct hdmi_pll_info *pi)
+{
+        unsigned long clkin, refclk;
+        u32 mf;
+
+        clkin = dss_clk_get_rate(DSS_CLK_SYSCK) / 10000;
+        /*
+         * Input clock is predivided by N + 1
+         * out put of which is reference clk
+         */
+        pi->regn = dssdev->clocks.hdmi.regn;
+        refclk = clkin / (pi->regn + 1);
+
+        /*
+         * multiplier is pixel_clk/ref_clk
+         * Multiplying by 100 to avoid fractional part removal
+         */
+        pi->regm = (phy * 100 / (refclk)) / 100;
+        pi->regm2 = dssdev->clocks.hdmi.regm2;
+
+        /*
+         * fractional multiplier is remainder of the difference between
+         * multiplier and actual phy(required pixel clock thus should be
+         * multiplied by 2^18(262144) divided by the reference clock
+         */
+        mf = (phy - pi->regm * refclk) * 262144;
+        pi->regmf = mf / (refclk);
+
+        /*
+         * Dcofreq should be set to 1 if required pixel clock
+         * is greater than 1000MHz
+         */
+        pi->dcofreq = phy > 1000 * 100;
+        pi->regsd = ((pi->regm * clkin / 10) / ((pi->regn + 1) * 250) + 5) / 10;
+
+        DSSDBG("M = %d Mf = %d\n", pi->regm, pi->regmf);
+        DSSDBG("range = %d sd = %d\n", pi->dcofreq, pi->regsd);
+}
+
+static void hdmi_enable_clocks(int enable)
+{
+        if (enable)
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK |
+                                DSS_CLK_SYSCK | DSS_CLK_VIDFCK);
+        else
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK |
+                                DSS_CLK_SYSCK | DSS_CLK_VIDFCK);
+}
+
+static int hdmi_power_on(struct omap_dss_device *dssdev)
+{
+        int r, code = 0;
+        struct hdmi_pll_info pll_data;
+        struct omap_video_timings *p;
+        unsigned long phy;
+
+        hdmi_enable_clocks(1);
+
+        dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 0);
+
+        p = &dssdev->panel.timings;
+
+        DSSDBG("hdmi_power_on x_res= %d y_res = %d\n",
+                dssdev->panel.timings.x_res,
+                dssdev->panel.timings.y_res);
+
+        if (!hdmi.custom_set) {
+                DSSDBG("Read EDID as no EDID is not set on poweron\n");
+                hdmi_read_edid(p);
+        }
+        code = get_timings_index();
+        dssdev->panel.timings = cea_vesa_timings[code].timings;
+        update_hdmi_timings(&hdmi.cfg, p, code);
+
+        phy = p->pixel_clock;
+
+        hdmi_compute_pll(dssdev, phy, &pll_data);
+
+        hdmi_wp_video_start(0);
+
+        /* config the PLL and PHY first */
+        r = hdmi_pll_program(&pll_data);
+        if (r) {
+                DSSDBG("Failed to lock PLL\n");
+                goto err;
+        }
+
+        r = hdmi_phy_init();
+        if (r) {
+                DSSDBG("Failed to start PHY\n");
+                goto err;
+        }
+
+        hdmi.cfg.cm.mode = hdmi.mode;
+        hdmi.cfg.cm.code = hdmi.code;
+        hdmi_basic_configure(&hdmi.cfg);
+
+        /* Make selection of HDMI in DSS */
+        dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+        /* Select the dispc clock source as PRCM clock, to ensure that it is not
+         * DSI PLL source as the clock selected by DSI PLL might not be
+         * sufficient for the resolution selected / that can be changed
+         * dynamically by user. This can be moved to single location , say
+         * Boardfile.
+         */
+        dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
+
+        /* bypass TV gamma table */
+        dispc_enable_gamma_table(0);
+
+        /* tv size */
+        dispc_set_digit_size(dssdev->panel.timings.x_res,
+                        dssdev->panel.timings.y_res);
+
+        dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 1);
+
+        hdmi_wp_video_start(1);
+
+        return 0;
+err:
+        hdmi_enable_clocks(0);
+        return -EIO;
+}
+
+static void hdmi_power_off(struct omap_dss_device *dssdev)
+{
+        dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 0);
+
+        hdmi_wp_video_start(0);
+        hdmi_phy_off();
+        hdmi_set_pll_pwr(HDMI_PLLPWRCMD_ALLOFF);
+        hdmi_enable_clocks(0);
+
+        hdmi.edid_set = 0;
+}
+
+int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
+                                        struct omap_video_timings *timings)
+{
+        struct hdmi_cm cm;
+
+        cm = hdmi_get_code(timings);
+        if (cm.code == -1) {
+                DSSERR("Invalid timing entered\n");
+                return -EINVAL;
+        }
+
+        return 0;
+
+}
+
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev)
+{
+        struct hdmi_cm cm;
+
+        hdmi.custom_set = 1;
+        cm = hdmi_get_code(&dssdev->panel.timings);
+        hdmi.code = cm.code;
+        hdmi.mode = cm.mode;
+        omapdss_hdmi_display_enable(dssdev);
+        hdmi.custom_set = 0;
+}
+
+int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        DSSDBG("ENTER hdmi_display_enable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        r = omap_dss_start_device(dssdev);
+        if (r) {
+                DSSERR("failed to start device\n");
+                goto err0;
+        }
+
+        if (dssdev->platform_enable) {
+                r = dssdev->platform_enable(dssdev);
+                if (r) {
+                        DSSERR("failed to enable GPIO's\n");
+                        goto err1;
+                }
+        }
+
+        r = hdmi_power_on(dssdev);
+        if (r) {
+                DSSERR("failed to power on device\n");
+                goto err2;
+        }
+
+        mutex_unlock(&hdmi.lock);
+        return 0;
+
+err2:
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+err1:
+        omap_dss_stop_device(dssdev);
+err0:
+        mutex_unlock(&hdmi.lock);
+        return r;
+}
+
+void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+        DSSDBG("Enter hdmi_display_disable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        hdmi_power_off(dssdev);
+
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+
+        omap_dss_stop_device(dssdev);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+        defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+static void hdmi_wp_audio_config_format(
+                struct hdmi_audio_format *aud_fmt)
+{
+        u32 r;
+
+        DSSDBG("Enter hdmi_wp_audio_config_format\n");
+
+        r = hdmi_read_reg(HDMI_WP_AUDIO_CFG);
+        r = FLD_MOD(r, aud_fmt->stereo_channels, 26, 24);
+        r = FLD_MOD(r, aud_fmt->active_chnnls_msk, 23, 16);
+        r = FLD_MOD(r, aud_fmt->en_sig_blk_strt_end, 5, 5);
+        r = FLD_MOD(r, aud_fmt->type, 4, 4);
+        r = FLD_MOD(r, aud_fmt->justification, 3, 3);
+        r = FLD_MOD(r, aud_fmt->sample_order, 2, 2);
+        r = FLD_MOD(r, aud_fmt->samples_per_word, 1, 1);
+        r = FLD_MOD(r, aud_fmt->sample_size, 0, 0);
+        hdmi_write_reg(HDMI_WP_AUDIO_CFG, r);
+}
+
+static void hdmi_wp_audio_config_dma(struct hdmi_audio_dma *aud_dma)
+{
+        u32 r;
+
+        DSSDBG("Enter hdmi_wp_audio_config_dma\n");
+
+        r = hdmi_read_reg(HDMI_WP_AUDIO_CFG2);
+        r = FLD_MOD(r, aud_dma->transfer_size, 15, 8);
+        r = FLD_MOD(r, aud_dma->block_size, 7, 0);
+        hdmi_write_reg(HDMI_WP_AUDIO_CFG2, r);
+
+        r = hdmi_read_reg(HDMI_WP_AUDIO_CTRL);
+        r = FLD_MOD(r, aud_dma->mode, 9, 9);
+        r = FLD_MOD(r, aud_dma->fifo_threshold, 8, 0);
+        hdmi_write_reg(HDMI_WP_AUDIO_CTRL, r);
+}
+
+static void hdmi_core_audio_config(struct hdmi_core_audio_config *cfg)
+{
+        u32 r;
+
+        /* audio clock recovery parameters */
+        r = hdmi_read_reg(HDMI_CORE_AV_ACR_CTRL);
+        r = FLD_MOD(r, cfg->use_mclk, 2, 2);
+        r = FLD_MOD(r, cfg->en_acr_pkt, 1, 1);
+        r = FLD_MOD(r, cfg->cts_mode, 0, 0);
+        hdmi_write_reg(HDMI_CORE_AV_ACR_CTRL, r);
+
+        REG_FLD_MOD(HDMI_CORE_AV_N_SVAL1, cfg->n, 7, 0);
+        REG_FLD_MOD(HDMI_CORE_AV_N_SVAL2, cfg->n >> 8, 7, 0);
+        REG_FLD_MOD(HDMI_CORE_AV_N_SVAL3, cfg->n >> 16, 7, 0);
+
+        if (cfg->cts_mode == HDMI_AUDIO_CTS_MODE_SW) {
+                REG_FLD_MOD(HDMI_CORE_AV_CTS_SVAL1, cfg->cts, 7, 0);
+                REG_FLD_MOD(HDMI_CORE_AV_CTS_SVAL2, cfg->cts >> 8, 7, 0);
+                REG_FLD_MOD(HDMI_CORE_AV_CTS_SVAL3, cfg->cts >> 16, 7, 0);
+        } else {
+                /*
+                 * HDMI IP uses this configuration to divide the MCLK to
+                 * update CTS value.
+                 */
+                REG_FLD_MOD(HDMI_CORE_AV_FREQ_SVAL, cfg->mclk_mode, 2, 0);
+
+                /* Configure clock for audio packets */
+                REG_FLD_MOD(HDMI_CORE_AV_AUD_PAR_BUSCLK_1,
+                        cfg->aud_par_busclk, 7, 0);
+                REG_FLD_MOD(HDMI_CORE_AV_AUD_PAR_BUSCLK_2,
+                        (cfg->aud_par_busclk >> 8), 7, 0);
+                REG_FLD_MOD(HDMI_CORE_AV_AUD_PAR_BUSCLK_3,
+                        (cfg->aud_par_busclk >> 16), 7, 0);
+        }
+
+        /* Override of SPDIF sample frequency with value in I2S_CHST4 */
+        REG_FLD_MOD(HDMI_CORE_AV_SPDIF_CTRL, cfg->fs_override, 1, 1);
+
+        /* I2S parameters */
+        REG_FLD_MOD(HDMI_CORE_AV_I2S_CHST4, cfg->freq_sample, 3, 0);
+
+        r = hdmi_read_reg(HDMI_CORE_AV_I2S_IN_CTRL);
+        r = FLD_MOD(r, cfg->i2s_cfg.en_high_bitrate_aud, 7, 7);
+        r = FLD_MOD(r, cfg->i2s_cfg.sck_edge_mode, 6, 6);
+        r = FLD_MOD(r, cfg->i2s_cfg.cbit_order, 5, 5);
+        r = FLD_MOD(r, cfg->i2s_cfg.vbit, 4, 4);
+        r = FLD_MOD(r, cfg->i2s_cfg.ws_polarity, 3, 3);
+        r = FLD_MOD(r, cfg->i2s_cfg.justification, 2, 2);
+        r = FLD_MOD(r, cfg->i2s_cfg.direction, 1, 1);
+        r = FLD_MOD(r, cfg->i2s_cfg.shift, 0, 0);
+        hdmi_write_reg(HDMI_CORE_AV_I2S_IN_CTRL, r);
+
+        r = hdmi_read_reg(HDMI_CORE_AV_I2S_CHST5);
+        r = FLD_MOD(r, cfg->freq_sample, 7, 4);
+        r = FLD_MOD(r, cfg->i2s_cfg.word_length, 3, 1);
+        r = FLD_MOD(r, cfg->i2s_cfg.word_max_length, 0, 0);
+        hdmi_write_reg(HDMI_CORE_AV_I2S_CHST5, r);
+
+        REG_FLD_MOD(HDMI_CORE_AV_I2S_IN_LEN, cfg->i2s_cfg.in_length_bits, 3, 0);
+
+        /* Audio channels and mode parameters */
+        REG_FLD_MOD(HDMI_CORE_AV_HDMI_CTRL, cfg->layout, 2, 1);
+        r = hdmi_read_reg(HDMI_CORE_AV_AUD_MODE);
+        r = FLD_MOD(r, cfg->i2s_cfg.active_sds, 7, 4);
+        r = FLD_MOD(r, cfg->en_dsd_audio, 3, 3);
+        r = FLD_MOD(r, cfg->en_parallel_aud_input, 2, 2);
+        r = FLD_MOD(r, cfg->en_spdif, 1, 1);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_MODE, r);
+}
+
+static void hdmi_core_audio_infoframe_config(
+                struct hdmi_core_infoframe_audio *info_aud)
+{
+        u8 val;
+        u8 sum = 0, checksum = 0;
+
+        /*
+         * Set audio info frame type, version and length as
+         * described in HDMI 1.4a Section 8.2.2 specification.
+         * Checksum calculation is defined in Section 5.3.5.
+         */
+        hdmi_write_reg(HDMI_CORE_AV_AUDIO_TYPE, 0x84);
+        hdmi_write_reg(HDMI_CORE_AV_AUDIO_VERS, 0x01);
+        hdmi_write_reg(HDMI_CORE_AV_AUDIO_LEN, 0x0a);
+        sum += 0x84 + 0x001 + 0x00a;
+
+        val = (info_aud->db1_coding_type << 4)
+                        | (info_aud->db1_channel_count - 1);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(0), val);
+        sum += val;
+
+        val = (info_aud->db2_sample_freq << 2) | info_aud->db2_sample_size;
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(1), val);
+        sum += val;
+
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(2), 0x00);
+
+        val = info_aud->db4_channel_alloc;
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(3), val);
+        sum += val;
+
+        val = (info_aud->db5_downmix_inh << 7) | (info_aud->db5_lsv << 3);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(4), val);
+        sum += val;
+
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(5), 0x00);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(6), 0x00);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(7), 0x00);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(8), 0x00);
+        hdmi_write_reg(HDMI_CORE_AV_AUD_DBYTE(9), 0x00);
+
+        checksum = 0x100 - sum;
+        hdmi_write_reg(HDMI_CORE_AV_AUDIO_CHSUM, checksum);
+
+        /*
+         * TODO: Add MPEG and SPD enable and repeat cfg when EDID parsing
+         * is available.
+         */
+}
+
+static int hdmi_config_audio_acr(u32 sample_freq, u32 *n, u32 *cts)
+{
+        u32 r;
+        u32 deep_color = 0;
+        u32 pclk = hdmi.cfg.timings.timings.pixel_clock;
+
+        if (n == NULL || cts == NULL)
+                return -EINVAL;
+        /*
+         * Obtain current deep color configuration. This needed
+         * to calculate the TMDS clock based on the pixel clock.
+         */
+        r = REG_GET(HDMI_WP_VIDEO_CFG, 1, 0);
+        switch (r) {
+        case 1: /* No deep color selected */
+                deep_color = 100;
+                break;
+        case 2: /* 10-bit deep color selected */
+                deep_color = 125;
+                break;
+        case 3: /* 12-bit deep color selected */
+                deep_color = 150;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        switch (sample_freq) {
+        case 32000:
+                if ((deep_color == 125) && ((pclk == 54054)
+                                || (pclk == 74250)))
+                        *n = 8192;
+                else
+                        *n = 4096;
+                break;
+        case 44100:
+                *n = 6272;
+                break;
+        case 48000:
+                if ((deep_color == 125) && ((pclk == 54054)
+                                || (pclk == 74250)))
+                        *n = 8192;
+                else
+                        *n = 6144;
+                break;
+        default:
+                *n = 0;
+                return -EINVAL;
+        }
+
+        /* Calculate CTS. See HDMI 1.3a or 1.4a specifications */
+        *cts = pclk * (*n / 128) * deep_color / (sample_freq / 10);
+
+        return 0;
+}
+
+static int hdmi_audio_hw_params(struct snd_pcm_substream *substream,
+                                    struct snd_pcm_hw_params *params,
+                                    struct snd_soc_dai *dai)
+{
+        struct hdmi_audio_format audio_format;
+        struct hdmi_audio_dma audio_dma;
+        struct hdmi_core_audio_config core_cfg;
+        struct hdmi_core_infoframe_audio aud_if_cfg;
+        int err, n, cts;
+        enum hdmi_core_audio_sample_freq sample_freq;
+
+        switch (params_format(params)) {
+        case SNDRV_PCM_FORMAT_S16_LE:
+                core_cfg.i2s_cfg.word_max_length =
+                        HDMI_AUDIO_I2S_MAX_WORD_20BITS;
+                core_cfg.i2s_cfg.word_length = HDMI_AUDIO_I2S_CHST_WORD_16_BITS;
+                core_cfg.i2s_cfg.in_length_bits =
+                        HDMI_AUDIO_I2S_INPUT_LENGTH_16;
+                core_cfg.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+                audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
+                audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
+                audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+                audio_dma.transfer_size = 0x10;
+                break;
+        case SNDRV_PCM_FORMAT_S24_LE:
+                core_cfg.i2s_cfg.word_max_length =
+                        HDMI_AUDIO_I2S_MAX_WORD_24BITS;
+                core_cfg.i2s_cfg.word_length = HDMI_AUDIO_I2S_CHST_WORD_24_BITS;
+                core_cfg.i2s_cfg.in_length_bits =
+                        HDMI_AUDIO_I2S_INPUT_LENGTH_24;
+                audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_ONESAMPLE;
+                audio_format.sample_size = HDMI_AUDIO_SAMPLE_24BITS;
+                audio_format.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+                core_cfg.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+                audio_dma.transfer_size = 0x20;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        switch (params_rate(params)) {
+        case 32000:
+                sample_freq = HDMI_AUDIO_FS_32000;
+                break;
+        case 44100:
+                sample_freq = HDMI_AUDIO_FS_44100;
+                break;
+        case 48000:
+                sample_freq = HDMI_AUDIO_FS_48000;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        err = hdmi_config_audio_acr(params_rate(params), &n, &cts);
+        if (err < 0)
+                return err;
+
+        /* Audio wrapper config */
+        audio_format.stereo_channels = HDMI_AUDIO_STEREO_ONECHANNEL;
+        audio_format.active_chnnls_msk = 0x03;
+        audio_format.type = HDMI_AUDIO_TYPE_LPCM;
+        audio_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
+        /* Disable start/stop signals of IEC 60958 blocks */
+        audio_format.en_sig_blk_strt_end = HDMI_AUDIO_BLOCK_SIG_STARTEND_OFF;
+
+        audio_dma.block_size = 0xC0;
+        audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+        audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+        hdmi_wp_audio_config_dma(&audio_dma);
+        hdmi_wp_audio_config_format(&audio_format);
+
+        /*
+         * I2S config
+         */
+        core_cfg.i2s_cfg.en_high_bitrate_aud = false;
+        /* Only used with high bitrate audio */
+        core_cfg.i2s_cfg.cbit_order = false;
+        /* Serial data and word select should change on sck rising edge */
+        core_cfg.i2s_cfg.sck_edge_mode = HDMI_AUDIO_I2S_SCK_EDGE_RISING;
+        core_cfg.i2s_cfg.vbit = HDMI_AUDIO_I2S_VBIT_FOR_PCM;
+        /* Set I2S word select polarity */
+        core_cfg.i2s_cfg.ws_polarity = HDMI_AUDIO_I2S_WS_POLARITY_LOW_IS_LEFT;
+        core_cfg.i2s_cfg.direction = HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST;
+        /* Set serial data to word select shift. See Phillips spec. */
+        core_cfg.i2s_cfg.shift = HDMI_AUDIO_I2S_FIRST_BIT_SHIFT;
+        /* Enable one of the four available serial data channels */
+        core_cfg.i2s_cfg.active_sds = HDMI_AUDIO_I2S_SD0_EN;
+
+        /* Core audio config */
+        core_cfg.freq_sample = sample_freq;
+        core_cfg.n = n;
+        core_cfg.cts = cts;
+        if (dss_has_feature(FEAT_HDMI_CTS_SWMODE)) {
+                core_cfg.aud_par_busclk = 0;
+                core_cfg.cts_mode = HDMI_AUDIO_CTS_MODE_SW;
+                core_cfg.use_mclk = false;
+        } else {
+                core_cfg.aud_par_busclk = (((128 * 31) - 1) << 8);
+                core_cfg.cts_mode = HDMI_AUDIO_CTS_MODE_HW;
+                core_cfg.use_mclk = true;
+                core_cfg.mclk_mode = HDMI_AUDIO_MCLK_128FS;
+        }
+        core_cfg.layout = HDMI_AUDIO_LAYOUT_2CH;
+        core_cfg.en_spdif = false;
+        /* Use sample frequency from channel status word */
+        core_cfg.fs_override = true;
+        /* Enable ACR packets */
+        core_cfg.en_acr_pkt = true;
+        /* Disable direct streaming digital audio */
+        core_cfg.en_dsd_audio = false;
+        /* Use parallel audio interface */
+        core_cfg.en_parallel_aud_input = true;
+
+        hdmi_core_audio_config(&core_cfg);
+
+        /*
+         * Configure packet
+         * info frame audio see doc CEA861-D page 74
+         */
+        aud_if_cfg.db1_coding_type = HDMI_INFOFRAME_AUDIO_DB1CT_FROM_STREAM;
+        aud_if_cfg.db1_channel_count = 2;
+        aud_if_cfg.db2_sample_freq = HDMI_INFOFRAME_AUDIO_DB2SF_FROM_STREAM;
+        aud_if_cfg.db2_sample_size = HDMI_INFOFRAME_AUDIO_DB2SS_FROM_STREAM;
+        aud_if_cfg.db4_channel_alloc = 0x00;
+        aud_if_cfg.db5_downmix_inh = false;
+        aud_if_cfg.db5_lsv = 0;
+
+        hdmi_core_audio_infoframe_config(&aud_if_cfg);
+        return 0;
+}
+
+static int hdmi_audio_trigger(struct snd_pcm_substream *substream, int cmd,
+                                  struct snd_soc_dai *dai)
+{
+        int err = 0;
+        switch (cmd) {
+        case SNDRV_PCM_TRIGGER_START:
+        case SNDRV_PCM_TRIGGER_RESUME:
+        case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+                REG_FLD_MOD(HDMI_CORE_AV_AUD_MODE, 1, 0, 0);
+                REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 1, 31, 31);
+                REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 1, 30, 30);
+                break;
+
+        case SNDRV_PCM_TRIGGER_STOP:
+        case SNDRV_PCM_TRIGGER_SUSPEND:
+        case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+                REG_FLD_MOD(HDMI_CORE_AV_AUD_MODE, 0, 0, 0);
+                REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 0, 30, 30);
+                REG_FLD_MOD(HDMI_WP_AUDIO_CTRL, 0, 31, 31);
+                break;
+        default:
+                err = -EINVAL;
+        }
+        return err;
+}
+
+static int hdmi_audio_startup(struct snd_pcm_substream *substream,
+                                  struct snd_soc_dai *dai)
+{
+        if (!hdmi.mode) {
+                pr_err("Current video settings do not support audio.\n");
+                return -EIO;
+        }
+        return 0;
+}
+
+static struct snd_soc_codec_driver hdmi_audio_codec_drv = {
+};
+
+static struct snd_soc_dai_ops hdmi_audio_codec_ops = {
+        .hw_params = hdmi_audio_hw_params,
+        .trigger = hdmi_audio_trigger,
+        .startup = hdmi_audio_startup,
+};
+
+static struct snd_soc_dai_driver hdmi_codec_dai_drv = {
+                .name = "hdmi-audio-codec",
+                .playback = {
+                        .channels_min = 2,
+                        .channels_max = 2,
+                        .rates = SNDRV_PCM_RATE_32000 |
+                                SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000,
+                        .formats = SNDRV_PCM_FMTBIT_S16_LE |
+                                SNDRV_PCM_FMTBIT_S24_LE,
+                },
+                .ops = &hdmi_audio_codec_ops,
+};
+#endif
+
+/* HDMI HW IP initialisation */
+static int omapdss_hdmihw_probe(struct platform_device *pdev)
+{
+        struct resource *hdmi_mem;
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+        defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+        int ret;
+#endif
+
+        hdmi.pdata = pdev->dev.platform_data;
+        hdmi.pdev = pdev;
+
+        mutex_init(&hdmi.lock);
+
+        hdmi_mem = platform_get_resource(hdmi.pdev, IORESOURCE_MEM, 0);
+        if (!hdmi_mem) {
+                DSSERR("can't get IORESOURCE_MEM HDMI\n");
+                return -EINVAL;
+        }
+
+        /* Base address taken from platform */
+        hdmi.base_wp = ioremap(hdmi_mem->start, resource_size(hdmi_mem));
+        if (!hdmi.base_wp) {
+                DSSERR("can't ioremap WP\n");
+                return -ENOMEM;
+        }
+
+        hdmi_panel_init();
+
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+        defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+
+        /* Register ASoC codec DAI */
+        ret = snd_soc_register_codec(&pdev->dev, &hdmi_audio_codec_drv,
+                                        &hdmi_codec_dai_drv, 1);
+        if (ret) {
+                DSSERR("can't register ASoC HDMI audio codec\n");
+                return ret;
+        }
+#endif
+        return 0;
+}
+
+static int omapdss_hdmihw_remove(struct platform_device *pdev)
+{
+        hdmi_panel_exit();
+
+#if defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI) || \
+        defined(CONFIG_SND_OMAP_SOC_OMAP4_HDMI_MODULE)
+        snd_soc_unregister_codec(&pdev->dev);
+#endif
+
+        iounmap(hdmi.base_wp);
+
+        return 0;
+}
+
+static struct platform_driver omapdss_hdmihw_driver = {
+        .probe          = omapdss_hdmihw_probe,
+        .remove         = omapdss_hdmihw_remove,
+        .driver         = {
+                .name   = "omapdss_hdmi",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int hdmi_init_platform_driver(void)
+{
+        return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/omap2/dss/hdmi.h b/drivers/video/omap2/dss/hdmi.h
new file mode 100644
index 000000000000..c885f9cb0659
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi.h
@@ -0,0 +1,631 @@
+/*
+ * hdmi.h
+ *
+ * HDMI driver definition for TI OMAP4 processors.
+ *
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _OMAP4_DSS_HDMI_H_
+#define _OMAP4_DSS_HDMI_H_
+
+#include <linux/string.h>
+#include <video/omapdss.h>
+
+#define HDMI_WP         0x0
+#define HDMI_CORE_SYS           0x400
+#define HDMI_CORE_AV            0x900
+#define HDMI_PLLCTRL            0x200
+#define HDMI_PHY                0x300
+
+struct hdmi_reg { u16 idx; };
+
+#define HDMI_REG(idx)                   ((const struct hdmi_reg) { idx })
+
+/* HDMI Wrapper */
+#define HDMI_WP_REG(idx)                        HDMI_REG(HDMI_WP + idx)
+
+#define HDMI_WP_REVISION                        HDMI_WP_REG(0x0)
+#define HDMI_WP_SYSCONFIG                       HDMI_WP_REG(0x10)
+#define HDMI_WP_IRQSTATUS_RAW                   HDMI_WP_REG(0x24)
+#define HDMI_WP_IRQSTATUS                       HDMI_WP_REG(0x28)
+#define HDMI_WP_PWR_CTRL                        HDMI_WP_REG(0x40)
+#define HDMI_WP_IRQENABLE_SET                   HDMI_WP_REG(0x2C)
+#define HDMI_WP_VIDEO_CFG                       HDMI_WP_REG(0x50)
+#define HDMI_WP_VIDEO_SIZE                      HDMI_WP_REG(0x60)
+#define HDMI_WP_VIDEO_TIMING_H                  HDMI_WP_REG(0x68)
+#define HDMI_WP_VIDEO_TIMING_V                  HDMI_WP_REG(0x6C)
+#define HDMI_WP_WP_CLK                          HDMI_WP_REG(0x70)
+#define HDMI_WP_AUDIO_CFG                       HDMI_WP_REG(0x80)
+#define HDMI_WP_AUDIO_CFG2                      HDMI_WP_REG(0x84)
+#define HDMI_WP_AUDIO_CTRL                      HDMI_WP_REG(0x88)
+#define HDMI_WP_AUDIO_DATA                      HDMI_WP_REG(0x8C)
+
+/* HDMI IP Core System */
+#define HDMI_CORE_SYS_REG(idx)                  HDMI_REG(HDMI_CORE_SYS + idx)
+
+#define HDMI_CORE_SYS_VND_IDL                   HDMI_CORE_SYS_REG(0x0)
+#define HDMI_CORE_SYS_DEV_IDL                   HDMI_CORE_SYS_REG(0x8)
+#define HDMI_CORE_SYS_DEV_IDH                   HDMI_CORE_SYS_REG(0xC)
+#define HDMI_CORE_SYS_DEV_REV                   HDMI_CORE_SYS_REG(0x10)
+#define HDMI_CORE_SYS_SRST                      HDMI_CORE_SYS_REG(0x14)
+#define HDMI_CORE_CTRL1                 HDMI_CORE_SYS_REG(0x20)
+#define HDMI_CORE_SYS_SYS_STAT                  HDMI_CORE_SYS_REG(0x24)
+#define HDMI_CORE_SYS_VID_ACEN                  HDMI_CORE_SYS_REG(0x124)
+#define HDMI_CORE_SYS_VID_MODE                  HDMI_CORE_SYS_REG(0x128)
+#define HDMI_CORE_SYS_INTR_STATE                HDMI_CORE_SYS_REG(0x1C0)
+#define HDMI_CORE_SYS_INTR1                     HDMI_CORE_SYS_REG(0x1C4)
+#define HDMI_CORE_SYS_INTR2                     HDMI_CORE_SYS_REG(0x1C8)
+#define HDMI_CORE_SYS_INTR3                     HDMI_CORE_SYS_REG(0x1CC)
+#define HDMI_CORE_SYS_INTR4                     HDMI_CORE_SYS_REG(0x1D0)
+#define HDMI_CORE_SYS_UMASK1                    HDMI_CORE_SYS_REG(0x1D4)
+#define HDMI_CORE_SYS_TMDS_CTRL         HDMI_CORE_SYS_REG(0x208)
+#define HDMI_CORE_SYS_DE_DLY                    HDMI_CORE_SYS_REG(0xC8)
+#define HDMI_CORE_SYS_DE_CTRL                   HDMI_CORE_SYS_REG(0xCC)
+#define HDMI_CORE_SYS_DE_TOP                    HDMI_CORE_SYS_REG(0xD0)
+#define HDMI_CORE_SYS_DE_CNTL                   HDMI_CORE_SYS_REG(0xD8)
+#define HDMI_CORE_SYS_DE_CNTH                   HDMI_CORE_SYS_REG(0xDC)
+#define HDMI_CORE_SYS_DE_LINL                   HDMI_CORE_SYS_REG(0xE0)
+#define HDMI_CORE_SYS_DE_LINH_1         HDMI_CORE_SYS_REG(0xE4)
+#define HDMI_CORE_CTRL1_VEN_FOLLOWVSYNC 0x1
+#define HDMI_CORE_CTRL1_HEN_FOLLOWHSYNC 0x1
+#define HDMI_CORE_CTRL1_BSEL_24BITBUS           0x1
+#define HDMI_CORE_CTRL1_EDGE_RISINGEDGE 0x1
+
+/* HDMI DDC E-DID */
+#define HDMI_CORE_DDC_CMD                       HDMI_CORE_SYS_REG(0x3CC)
+#define HDMI_CORE_DDC_STATUS                    HDMI_CORE_SYS_REG(0x3C8)
+#define HDMI_CORE_DDC_ADDR                      HDMI_CORE_SYS_REG(0x3B4)
+#define HDMI_CORE_DDC_OFFSET                    HDMI_CORE_SYS_REG(0x3BC)
+#define HDMI_CORE_DDC_COUNT1                    HDMI_CORE_SYS_REG(0x3C0)
+#define HDMI_CORE_DDC_COUNT2                    HDMI_CORE_SYS_REG(0x3C4)
+#define HDMI_CORE_DDC_DATA                      HDMI_CORE_SYS_REG(0x3D0)
+#define HDMI_CORE_DDC_SEGM                      HDMI_CORE_SYS_REG(0x3B8)
+
+/* HDMI IP Core Audio Video */
+#define HDMI_CORE_AV_REG(idx)                   HDMI_REG(HDMI_CORE_AV + idx)
+
+#define HDMI_CORE_AV_HDMI_CTRL                  HDMI_CORE_AV_REG(0xBC)
+#define HDMI_CORE_AV_DPD                        HDMI_CORE_AV_REG(0xF4)
+#define HDMI_CORE_AV_PB_CTRL1                   HDMI_CORE_AV_REG(0xF8)
+#define HDMI_CORE_AV_PB_CTRL2                   HDMI_CORE_AV_REG(0xFC)
+#define HDMI_CORE_AV_AVI_TYPE                   HDMI_CORE_AV_REG(0x100)
+#define HDMI_CORE_AV_AVI_VERS                   HDMI_CORE_AV_REG(0x104)
+#define HDMI_CORE_AV_AVI_LEN                    HDMI_CORE_AV_REG(0x108)
+#define HDMI_CORE_AV_AVI_CHSUM                  HDMI_CORE_AV_REG(0x10C)
+#define HDMI_CORE_AV_AVI_DBYTE(n)               HDMI_CORE_AV_REG(n * 4 + 0x110)
+#define HDMI_CORE_AV_AVI_DBYTE_NELEMS           HDMI_CORE_AV_REG(15)
+#define HDMI_CORE_AV_SPD_DBYTE                  HDMI_CORE_AV_REG(0x190)
+#define HDMI_CORE_AV_SPD_DBYTE_NELEMS           HDMI_CORE_AV_REG(27)
+#define HDMI_CORE_AV_AUD_DBYTE(n)               HDMI_CORE_AV_REG(n * 4 + 0x210)
+#define HDMI_CORE_AV_AUD_DBYTE_NELEMS           HDMI_CORE_AV_REG(10)
+#define HDMI_CORE_AV_MPEG_DBYTE         HDMI_CORE_AV_REG(0x290)
+#define HDMI_CORE_AV_MPEG_DBYTE_NELEMS          HDMI_CORE_AV_REG(27)
+#define HDMI_CORE_AV_GEN_DBYTE                  HDMI_CORE_AV_REG(0x300)
+#define HDMI_CORE_AV_GEN_DBYTE_NELEMS           HDMI_CORE_AV_REG(31)
+#define HDMI_CORE_AV_GEN2_DBYTE         HDMI_CORE_AV_REG(0x380)
+#define HDMI_CORE_AV_GEN2_DBYTE_NELEMS          HDMI_CORE_AV_REG(31)
+#define HDMI_CORE_AV_ACR_CTRL                   HDMI_CORE_AV_REG(0x4)
+#define HDMI_CORE_AV_FREQ_SVAL                  HDMI_CORE_AV_REG(0x8)
+#define HDMI_CORE_AV_N_SVAL1                    HDMI_CORE_AV_REG(0xC)
+#define HDMI_CORE_AV_N_SVAL2                    HDMI_CORE_AV_REG(0x10)
+#define HDMI_CORE_AV_N_SVAL3                    HDMI_CORE_AV_REG(0x14)
+#define HDMI_CORE_AV_CTS_SVAL1                  HDMI_CORE_AV_REG(0x18)
+#define HDMI_CORE_AV_CTS_SVAL2                  HDMI_CORE_AV_REG(0x1C)
+#define HDMI_CORE_AV_CTS_SVAL3                  HDMI_CORE_AV_REG(0x20)
+#define HDMI_CORE_AV_CTS_HVAL1                  HDMI_CORE_AV_REG(0x24)
+#define HDMI_CORE_AV_CTS_HVAL2                  HDMI_CORE_AV_REG(0x28)
+#define HDMI_CORE_AV_CTS_HVAL3                  HDMI_CORE_AV_REG(0x2C)
+#define HDMI_CORE_AV_AUD_MODE                   HDMI_CORE_AV_REG(0x50)
+#define HDMI_CORE_AV_SPDIF_CTRL         HDMI_CORE_AV_REG(0x54)
+#define HDMI_CORE_AV_HW_SPDIF_FS                HDMI_CORE_AV_REG(0x60)
+#define HDMI_CORE_AV_SWAP_I2S                   HDMI_CORE_AV_REG(0x64)
+#define HDMI_CORE_AV_SPDIF_ERTH         HDMI_CORE_AV_REG(0x6C)
+#define HDMI_CORE_AV_I2S_IN_MAP         HDMI_CORE_AV_REG(0x70)
+#define HDMI_CORE_AV_I2S_IN_CTRL                HDMI_CORE_AV_REG(0x74)
+#define HDMI_CORE_AV_I2S_CHST0                  HDMI_CORE_AV_REG(0x78)
+#define HDMI_CORE_AV_I2S_CHST1                  HDMI_CORE_AV_REG(0x7C)
+#define HDMI_CORE_AV_I2S_CHST2                  HDMI_CORE_AV_REG(0x80)
+#define HDMI_CORE_AV_I2S_CHST4                  HDMI_CORE_AV_REG(0x84)
+#define HDMI_CORE_AV_I2S_CHST5                  HDMI_CORE_AV_REG(0x88)
+#define HDMI_CORE_AV_ASRC                       HDMI_CORE_AV_REG(0x8C)
+#define HDMI_CORE_AV_I2S_IN_LEN         HDMI_CORE_AV_REG(0x90)
+#define HDMI_CORE_AV_HDMI_CTRL                  HDMI_CORE_AV_REG(0xBC)
+#define HDMI_CORE_AV_AUDO_TXSTAT                HDMI_CORE_AV_REG(0xC0)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_1           HDMI_CORE_AV_REG(0xCC)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_2           HDMI_CORE_AV_REG(0xD0)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_3           HDMI_CORE_AV_REG(0xD4)
+#define HDMI_CORE_AV_TEST_TXCTRL                HDMI_CORE_AV_REG(0xF0)
+#define HDMI_CORE_AV_DPD                        HDMI_CORE_AV_REG(0xF4)
+#define HDMI_CORE_AV_PB_CTRL1                   HDMI_CORE_AV_REG(0xF8)
+#define HDMI_CORE_AV_PB_CTRL2                   HDMI_CORE_AV_REG(0xFC)
+#define HDMI_CORE_AV_AVI_TYPE                   HDMI_CORE_AV_REG(0x100)
+#define HDMI_CORE_AV_AVI_VERS                   HDMI_CORE_AV_REG(0x104)
+#define HDMI_CORE_AV_AVI_LEN                    HDMI_CORE_AV_REG(0x108)
+#define HDMI_CORE_AV_AVI_CHSUM                  HDMI_CORE_AV_REG(0x10C)
+#define HDMI_CORE_AV_SPD_TYPE                   HDMI_CORE_AV_REG(0x180)
+#define HDMI_CORE_AV_SPD_VERS                   HDMI_CORE_AV_REG(0x184)
+#define HDMI_CORE_AV_SPD_LEN                    HDMI_CORE_AV_REG(0x188)
+#define HDMI_CORE_AV_SPD_CHSUM                  HDMI_CORE_AV_REG(0x18C)
+#define HDMI_CORE_AV_AUDIO_TYPE         HDMI_CORE_AV_REG(0x200)
+#define HDMI_CORE_AV_AUDIO_VERS         HDMI_CORE_AV_REG(0x204)
+#define HDMI_CORE_AV_AUDIO_LEN                  HDMI_CORE_AV_REG(0x208)
+#define HDMI_CORE_AV_AUDIO_CHSUM                HDMI_CORE_AV_REG(0x20C)
+#define HDMI_CORE_AV_MPEG_TYPE                  HDMI_CORE_AV_REG(0x280)
+#define HDMI_CORE_AV_MPEG_VERS                  HDMI_CORE_AV_REG(0x284)
+#define HDMI_CORE_AV_MPEG_LEN                   HDMI_CORE_AV_REG(0x288)
+#define HDMI_CORE_AV_MPEG_CHSUM         HDMI_CORE_AV_REG(0x28C)
+#define HDMI_CORE_AV_CP_BYTE1                   HDMI_CORE_AV_REG(0x37C)
+#define HDMI_CORE_AV_CEC_ADDR_ID                HDMI_CORE_AV_REG(0x3FC)
+#define HDMI_CORE_AV_SPD_DBYTE_ELSIZE           0x4
+#define HDMI_CORE_AV_GEN2_DBYTE_ELSIZE          0x4
+#define HDMI_CORE_AV_MPEG_DBYTE_ELSIZE          0x4
+#define HDMI_CORE_AV_GEN_DBYTE_ELSIZE           0x4
+
+/* PLL */
+#define HDMI_PLL_REG(idx)                       HDMI_REG(HDMI_PLLCTRL + idx)
+
+#define PLLCTRL_PLL_CONTROL                     HDMI_PLL_REG(0x0)
+#define PLLCTRL_PLL_STATUS                      HDMI_PLL_REG(0x4)
+#define PLLCTRL_PLL_GO                          HDMI_PLL_REG(0x8)
+#define PLLCTRL_CFG1                            HDMI_PLL_REG(0xC)
+#define PLLCTRL_CFG2                            HDMI_PLL_REG(0x10)
+#define PLLCTRL_CFG3                            HDMI_PLL_REG(0x14)
+#define PLLCTRL_CFG4                            HDMI_PLL_REG(0x20)
+
+/* HDMI PHY */
+#define HDMI_PHY_REG(idx)                       HDMI_REG(HDMI_PHY + idx)
+
+#define HDMI_TXPHY_TX_CTRL                      HDMI_PHY_REG(0x0)
+#define HDMI_TXPHY_DIGITAL_CTRL         HDMI_PHY_REG(0x4)
+#define HDMI_TXPHY_POWER_CTRL                   HDMI_PHY_REG(0x8)
+#define HDMI_TXPHY_PAD_CFG_CTRL         HDMI_PHY_REG(0xC)
+
+/* HDMI EDID Length  */
+#define HDMI_EDID_MAX_LENGTH                    256
+#define EDID_TIMING_DESCRIPTOR_SIZE             0x12
+#define EDID_DESCRIPTOR_BLOCK0_ADDRESS          0x36
+#define EDID_DESCRIPTOR_BLOCK1_ADDRESS          0x80
+#define EDID_SIZE_BLOCK0_TIMING_DESCRIPTOR      4
+#define EDID_SIZE_BLOCK1_TIMING_DESCRIPTOR      4
+
+#define OMAP_HDMI_TIMINGS_NB                    34
+
+#define REG_FLD_MOD(idx, val, start, end) \
+        hdmi_write_reg(idx, FLD_MOD(hdmi_read_reg(idx), val, start, end))
+#define REG_GET(idx, start, end) \
+        FLD_GET(hdmi_read_reg(idx), start, end)
+
+/* HDMI timing structure */
+struct hdmi_timings {
+        struct omap_video_timings timings;
+        int vsync_pol;
+        int hsync_pol;
+};
+
+enum hdmi_phy_pwr {
+        HDMI_PHYPWRCMD_OFF = 0,
+        HDMI_PHYPWRCMD_LDOON = 1,
+        HDMI_PHYPWRCMD_TXON = 2
+};
+
+enum hdmi_pll_pwr {
+        HDMI_PLLPWRCMD_ALLOFF = 0,
+        HDMI_PLLPWRCMD_PLLONLY = 1,
+        HDMI_PLLPWRCMD_BOTHON_ALLCLKS = 2,
+        HDMI_PLLPWRCMD_BOTHON_NOPHYCLK = 3
+};
+
+enum hdmi_clk_refsel {
+        HDMI_REFSEL_PCLK = 0,
+        HDMI_REFSEL_REF1 = 1,
+        HDMI_REFSEL_REF2 = 2,
+        HDMI_REFSEL_SYSCLK = 3
+};
+
+enum hdmi_core_inputbus_width {
+        HDMI_INPUT_8BIT = 0,
+        HDMI_INPUT_10BIT = 1,
+        HDMI_INPUT_12BIT = 2
+};
+
+enum hdmi_core_dither_trunc {
+        HDMI_OUTPUTTRUNCATION_8BIT = 0,
+        HDMI_OUTPUTTRUNCATION_10BIT = 1,
+        HDMI_OUTPUTTRUNCATION_12BIT = 2,
+        HDMI_OUTPUTDITHER_8BIT = 3,
+        HDMI_OUTPUTDITHER_10BIT = 4,
+        HDMI_OUTPUTDITHER_12BIT = 5
+};
+
+enum hdmi_core_deepcolor_ed {
+        HDMI_DEEPCOLORPACKECTDISABLE = 0,
+        HDMI_DEEPCOLORPACKECTENABLE = 1
+};
+
+enum hdmi_core_packet_mode {
+        HDMI_PACKETMODERESERVEDVALUE = 0,
+        HDMI_PACKETMODE24BITPERPIXEL = 4,
+        HDMI_PACKETMODE30BITPERPIXEL = 5,
+        HDMI_PACKETMODE36BITPERPIXEL = 6,
+        HDMI_PACKETMODE48BITPERPIXEL = 7
+};
+
+enum hdmi_core_hdmi_dvi {
+        HDMI_DVI = 0,
+        HDMI_HDMI = 1
+};
+
+enum hdmi_core_tclkselclkmult {
+        HDMI_FPLL05IDCK = 0,
+        HDMI_FPLL10IDCK = 1,
+        HDMI_FPLL20IDCK = 2,
+        HDMI_FPLL40IDCK = 3
+};
+
+enum hdmi_core_packet_ctrl {
+        HDMI_PACKETENABLE = 1,
+        HDMI_PACKETDISABLE = 0,
+        HDMI_PACKETREPEATON = 1,
+        HDMI_PACKETREPEATOFF = 0
+};
+
+/* INFOFRAME_AVI_ and INFOFRAME_AUDIO_ definitions */
+enum hdmi_core_infoframe {
+        HDMI_INFOFRAME_AVI_DB1Y_RGB = 0,
+        HDMI_INFOFRAME_AVI_DB1Y_YUV422 = 1,
+        HDMI_INFOFRAME_AVI_DB1Y_YUV444 = 2,
+        HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF = 0,
+        HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_ON =  1,
+        HDMI_INFOFRAME_AVI_DB1B_NO = 0,
+        HDMI_INFOFRAME_AVI_DB1B_VERT = 1,
+        HDMI_INFOFRAME_AVI_DB1B_HORI = 2,
+        HDMI_INFOFRAME_AVI_DB1B_VERTHORI = 3,
+        HDMI_INFOFRAME_AVI_DB1S_0 = 0,
+        HDMI_INFOFRAME_AVI_DB1S_1 = 1,
+        HDMI_INFOFRAME_AVI_DB1S_2 = 2,
+        HDMI_INFOFRAME_AVI_DB2C_NO = 0,
+        HDMI_INFOFRAME_AVI_DB2C_ITU601 = 1,
+        HDMI_INFOFRAME_AVI_DB2C_ITU709 = 2,
+        HDMI_INFOFRAME_AVI_DB2C_EC_EXTENDED = 3,
+        HDMI_INFOFRAME_AVI_DB2M_NO = 0,
+        HDMI_INFOFRAME_AVI_DB2M_43 = 1,
+        HDMI_INFOFRAME_AVI_DB2M_169 = 2,
+        HDMI_INFOFRAME_AVI_DB2R_SAME = 8,
+        HDMI_INFOFRAME_AVI_DB2R_43 = 9,
+        HDMI_INFOFRAME_AVI_DB2R_169 = 10,
+        HDMI_INFOFRAME_AVI_DB2R_149 = 11,
+        HDMI_INFOFRAME_AVI_DB3ITC_NO = 0,
+        HDMI_INFOFRAME_AVI_DB3ITC_YES = 1,
+        HDMI_INFOFRAME_AVI_DB3EC_XVYUV601 = 0,
+        HDMI_INFOFRAME_AVI_DB3EC_XVYUV709 = 1,
+        HDMI_INFOFRAME_AVI_DB3Q_DEFAULT = 0,
+        HDMI_INFOFRAME_AVI_DB3Q_LR = 1,
+        HDMI_INFOFRAME_AVI_DB3Q_FR = 2,
+        HDMI_INFOFRAME_AVI_DB3SC_NO = 0,
+        HDMI_INFOFRAME_AVI_DB3SC_HORI = 1,
+        HDMI_INFOFRAME_AVI_DB3SC_VERT = 2,
+        HDMI_INFOFRAME_AVI_DB3SC_HORIVERT = 3,
+        HDMI_INFOFRAME_AVI_DB5PR_NO = 0,
+        HDMI_INFOFRAME_AVI_DB5PR_2 = 1,
+        HDMI_INFOFRAME_AVI_DB5PR_3 = 2,
+        HDMI_INFOFRAME_AVI_DB5PR_4 = 3,
+        HDMI_INFOFRAME_AVI_DB5PR_5 = 4,
+        HDMI_INFOFRAME_AVI_DB5PR_6 = 5,
+        HDMI_INFOFRAME_AVI_DB5PR_7 = 6,
+        HDMI_INFOFRAME_AVI_DB5PR_8 = 7,
+        HDMI_INFOFRAME_AVI_DB5PR_9 = 8,
+        HDMI_INFOFRAME_AVI_DB5PR_10 = 9,
+        HDMI_INFOFRAME_AUDIO_DB1CT_FROM_STREAM = 0,
+        HDMI_INFOFRAME_AUDIO_DB1CT_IEC60958 = 1,
+        HDMI_INFOFRAME_AUDIO_DB1CT_AC3 = 2,
+        HDMI_INFOFRAME_AUDIO_DB1CT_MPEG1 = 3,
+        HDMI_INFOFRAME_AUDIO_DB1CT_MP3 = 4,
+        HDMI_INFOFRAME_AUDIO_DB1CT_MPEG2_MULTICH = 5,
+        HDMI_INFOFRAME_AUDIO_DB1CT_AAC = 6,
+        HDMI_INFOFRAME_AUDIO_DB1CT_DTS = 7,
+        HDMI_INFOFRAME_AUDIO_DB1CT_ATRAC = 8,
+        HDMI_INFOFRAME_AUDIO_DB1CT_ONEBIT = 9,
+        HDMI_INFOFRAME_AUDIO_DB1CT_DOLBY_DIGITAL_PLUS = 10,
+        HDMI_INFOFRAME_AUDIO_DB1CT_DTS_HD = 11,
+        HDMI_INFOFRAME_AUDIO_DB1CT_MAT = 12,
+        HDMI_INFOFRAME_AUDIO_DB1CT_DST = 13,
+        HDMI_INFOFRAME_AUDIO_DB1CT_WMA_PRO = 14,
+        HDMI_INFOFRAME_AUDIO_DB2SF_FROM_STREAM = 0,
+        HDMI_INFOFRAME_AUDIO_DB2SF_32000 = 1,
+        HDMI_INFOFRAME_AUDIO_DB2SF_44100 = 2,
+        HDMI_INFOFRAME_AUDIO_DB2SF_48000 = 3,
+        HDMI_INFOFRAME_AUDIO_DB2SF_88200 = 4,
+        HDMI_INFOFRAME_AUDIO_DB2SF_96000 = 5,
+        HDMI_INFOFRAME_AUDIO_DB2SF_176400 = 6,
+        HDMI_INFOFRAME_AUDIO_DB2SF_192000 = 7,
+        HDMI_INFOFRAME_AUDIO_DB2SS_FROM_STREAM = 0,
+        HDMI_INFOFRAME_AUDIO_DB2SS_16BIT = 1,
+        HDMI_INFOFRAME_AUDIO_DB2SS_20BIT = 2,
+        HDMI_INFOFRAME_AUDIO_DB2SS_24BIT = 3,
+        HDMI_INFOFRAME_AUDIO_DB5_DM_INH_PERMITTED = 0,
+        HDMI_INFOFRAME_AUDIO_DB5_DM_INH_PROHIBITED = 1
+};
+
+enum hdmi_packing_mode {
+        HDMI_PACK_10b_RGB_YUV444 = 0,
+        HDMI_PACK_24b_RGB_YUV444_YUV422 = 1,
+        HDMI_PACK_20b_YUV422 = 2,
+        HDMI_PACK_ALREADYPACKED = 7
+};
+
+enum hdmi_core_audio_sample_freq {
+        HDMI_AUDIO_FS_32000 = 0x3,
+        HDMI_AUDIO_FS_44100 = 0x0,
+        HDMI_AUDIO_FS_48000 = 0x2,
+        HDMI_AUDIO_FS_88200 = 0x8,
+        HDMI_AUDIO_FS_96000 = 0xA,
+        HDMI_AUDIO_FS_176400 = 0xC,
+        HDMI_AUDIO_FS_192000 = 0xE,
+        HDMI_AUDIO_FS_NOT_INDICATED = 0x1
+};
+
+enum hdmi_core_audio_layout {
+        HDMI_AUDIO_LAYOUT_2CH = 0,
+        HDMI_AUDIO_LAYOUT_8CH = 1
+};
+
+enum hdmi_core_cts_mode {
+        HDMI_AUDIO_CTS_MODE_HW = 0,
+        HDMI_AUDIO_CTS_MODE_SW = 1
+};
+
+enum hdmi_stereo_channels {
+        HDMI_AUDIO_STEREO_NOCHANNELS = 0,
+        HDMI_AUDIO_STEREO_ONECHANNEL = 1,
+        HDMI_AUDIO_STEREO_TWOCHANNELS = 2,
+        HDMI_AUDIO_STEREO_THREECHANNELS = 3,
+        HDMI_AUDIO_STEREO_FOURCHANNELS = 4
+};
+
+enum hdmi_audio_type {
+        HDMI_AUDIO_TYPE_LPCM = 0,
+        HDMI_AUDIO_TYPE_IEC = 1
+};
+
+enum hdmi_audio_justify {
+        HDMI_AUDIO_JUSTIFY_LEFT = 0,
+        HDMI_AUDIO_JUSTIFY_RIGHT = 1
+};
+
+enum hdmi_audio_sample_order {
+        HDMI_AUDIO_SAMPLE_RIGHT_FIRST = 0,
+        HDMI_AUDIO_SAMPLE_LEFT_FIRST = 1
+};
+
+enum hdmi_audio_samples_perword {
+        HDMI_AUDIO_ONEWORD_ONESAMPLE = 0,
+        HDMI_AUDIO_ONEWORD_TWOSAMPLES = 1
+};
+
+enum hdmi_audio_sample_size {
+        HDMI_AUDIO_SAMPLE_16BITS = 0,
+        HDMI_AUDIO_SAMPLE_24BITS = 1
+};
+
+enum hdmi_audio_transf_mode {
+        HDMI_AUDIO_TRANSF_DMA = 0,
+        HDMI_AUDIO_TRANSF_IRQ = 1
+};
+
+enum hdmi_audio_blk_strt_end_sig {
+        HDMI_AUDIO_BLOCK_SIG_STARTEND_ON = 0,
+        HDMI_AUDIO_BLOCK_SIG_STARTEND_OFF = 1
+};
+
+enum hdmi_audio_i2s_config {
+        HDMI_AUDIO_I2S_WS_POLARITY_LOW_IS_LEFT = 0,
+        HDMI_AUDIO_I2S_WS_POLARIT_YLOW_IS_RIGHT = 1,
+        HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST = 0,
+        HDMI_AUDIO_I2S_LSB_SHIFTED_FIRST = 1,
+        HDMI_AUDIO_I2S_MAX_WORD_20BITS = 0,
+        HDMI_AUDIO_I2S_MAX_WORD_24BITS = 1,
+        HDMI_AUDIO_I2S_CHST_WORD_NOT_SPECIFIED = 0,
+        HDMI_AUDIO_I2S_CHST_WORD_16_BITS = 1,
+        HDMI_AUDIO_I2S_CHST_WORD_17_BITS = 6,
+        HDMI_AUDIO_I2S_CHST_WORD_18_BITS = 2,
+        HDMI_AUDIO_I2S_CHST_WORD_19_BITS = 4,
+        HDMI_AUDIO_I2S_CHST_WORD_20_BITS_20MAX = 5,
+        HDMI_AUDIO_I2S_CHST_WORD_20_BITS_24MAX = 1,
+        HDMI_AUDIO_I2S_CHST_WORD_21_BITS = 6,
+        HDMI_AUDIO_I2S_CHST_WORD_22_BITS = 2,
+        HDMI_AUDIO_I2S_CHST_WORD_23_BITS = 4,
+        HDMI_AUDIO_I2S_CHST_WORD_24_BITS = 5,
+        HDMI_AUDIO_I2S_SCK_EDGE_FALLING = 0,
+        HDMI_AUDIO_I2S_SCK_EDGE_RISING = 1,
+        HDMI_AUDIO_I2S_VBIT_FOR_PCM = 0,
+        HDMI_AUDIO_I2S_VBIT_FOR_COMPRESSED = 1,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_NA = 0,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_16 = 2,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_17 = 12,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_18 = 4,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_19 = 8,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_20 = 10,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_21 = 13,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_22 = 5,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_23 = 9,
+        HDMI_AUDIO_I2S_INPUT_LENGTH_24 = 11,
+        HDMI_AUDIO_I2S_FIRST_BIT_SHIFT = 0,
+        HDMI_AUDIO_I2S_FIRST_BIT_NO_SHIFT = 1,
+        HDMI_AUDIO_I2S_SD0_EN = 1,
+        HDMI_AUDIO_I2S_SD1_EN = 1 << 1,
+        HDMI_AUDIO_I2S_SD2_EN = 1 << 2,
+        HDMI_AUDIO_I2S_SD3_EN = 1 << 3,
+};
+
+enum hdmi_audio_mclk_mode {
+        HDMI_AUDIO_MCLK_128FS = 0,
+        HDMI_AUDIO_MCLK_256FS = 1,
+        HDMI_AUDIO_MCLK_384FS = 2,
+        HDMI_AUDIO_MCLK_512FS = 3,
+        HDMI_AUDIO_MCLK_768FS = 4,
+        HDMI_AUDIO_MCLK_1024FS = 5,
+        HDMI_AUDIO_MCLK_1152FS = 6,
+        HDMI_AUDIO_MCLK_192FS = 7
+};
+
+struct hdmi_core_video_config {
+        enum hdmi_core_inputbus_width   ip_bus_width;
+        enum hdmi_core_dither_trunc     op_dither_truc;
+        enum hdmi_core_deepcolor_ed     deep_color_pkt;
+        enum hdmi_core_packet_mode      pkt_mode;
+        enum hdmi_core_hdmi_dvi         hdmi_dvi;
+        enum hdmi_core_tclkselclkmult   tclk_sel_clkmult;
+};
+
+/*
+ * Refer to section 8.2 in HDMI 1.3 specification for
+ * details about infoframe databytes
+ */
+struct hdmi_core_infoframe_avi {
+        u8      db1_format;
+                /* Y0, Y1 rgb,yCbCr */
+        u8      db1_active_info;
+                /* A0  Active information Present */
+        u8      db1_bar_info_dv;
+                /* B0, B1 Bar info data valid */
+        u8      db1_scan_info;
+                /* S0, S1 scan information */
+        u8      db2_colorimetry;
+                /* C0, C1 colorimetry */
+        u8      db2_aspect_ratio;
+                /* M0, M1 Aspect ratio (4:3, 16:9) */
+        u8      db2_active_fmt_ar;
+                /* R0...R3 Active format aspect ratio */
+        u8      db3_itc;
+                /* ITC IT content. */
+        u8      db3_ec;
+                /* EC0, EC1, EC2 Extended colorimetry */
+        u8      db3_q_range;
+                /* Q1, Q0 Quantization range */
+        u8      db3_nup_scaling;
+                /* SC1, SC0 Non-uniform picture scaling */
+        u8      db4_videocode;
+                /* VIC0..6 Video format identification */
+        u8      db5_pixel_repeat;
+                /* PR0..PR3 Pixel repetition factor */
+        u16     db6_7_line_eoftop;
+                /* Line number end of top bar */
+        u16     db8_9_line_sofbottom;
+                /* Line number start of bottom bar */
+        u16     db10_11_pixel_eofleft;
+                /* Pixel number end of left bar */
+        u16     db12_13_pixel_sofright;
+                /* Pixel number start of right bar */
+};
+/*
+ * Refer to section 8.2 in HDMI 1.3 specification for
+ * details about infoframe databytes
+ */
+struct hdmi_core_infoframe_audio {
+        u8 db1_coding_type;
+        u8 db1_channel_count;
+        u8 db2_sample_freq;
+        u8 db2_sample_size;
+        u8 db4_channel_alloc;
+        bool db5_downmix_inh;
+        u8 db5_lsv;     /* Level shift values for downmix */
+};
+
+struct hdmi_core_packet_enable_repeat {
+        u32     audio_pkt;
+        u32     audio_pkt_repeat;
+        u32     avi_infoframe;
+        u32     avi_infoframe_repeat;
+        u32     gen_cntrl_pkt;
+        u32     gen_cntrl_pkt_repeat;
+        u32     generic_pkt;
+        u32     generic_pkt_repeat;
+};
+
+struct hdmi_video_format {
+        enum hdmi_packing_mode  packing_mode;
+        u32                     y_res;  /* Line per panel */
+        u32                     x_res;  /* pixel per line */
+};
+
+struct hdmi_video_interface {
+        int     vsp;    /* Vsync polarity */
+        int     hsp;    /* Hsync polarity */
+        int     interlacing;
+        int     tm;     /* Timing mode */
+};
+
+struct hdmi_cm {
+        int     code;
+        int     mode;
+};
+
+struct hdmi_config {
+        struct hdmi_timings timings;
+        u16     interlace;
+        struct hdmi_cm cm;
+};
+
+struct hdmi_audio_format {
+        enum hdmi_stereo_channels               stereo_channels;
+        u8                                      active_chnnls_msk;
+        enum hdmi_audio_type                    type;
+        enum hdmi_audio_justify                 justification;
+        enum hdmi_audio_sample_order            sample_order;
+        enum hdmi_audio_samples_perword         samples_per_word;
+        enum hdmi_audio_sample_size             sample_size;
+        enum hdmi_audio_blk_strt_end_sig        en_sig_blk_strt_end;
+};
+
+struct hdmi_audio_dma {
+        u8                              transfer_size;
+        u8                              block_size;
+        enum hdmi_audio_transf_mode     mode;
+        u16                             fifo_threshold;
+};
+
+struct hdmi_core_audio_i2s_config {
+        u8 word_max_length;
+        u8 word_length;
+        u8 in_length_bits;
+        u8 justification;
+        u8 en_high_bitrate_aud;
+        u8 sck_edge_mode;
+        u8 cbit_order;
+        u8 vbit;
+        u8 ws_polarity;
+        u8 direction;
+        u8 shift;
+        u8 active_sds;
+};
+
+struct hdmi_core_audio_config {
+        struct hdmi_core_audio_i2s_config       i2s_cfg;
+        enum hdmi_core_audio_sample_freq        freq_sample;
+        bool                                    fs_override;
+        u32                                     n;
+        u32                                     cts;
+        u32                                     aud_par_busclk;
+        enum hdmi_core_audio_layout             layout;
+        enum hdmi_core_cts_mode                 cts_mode;
+        bool                                    use_mclk;
+        enum hdmi_audio_mclk_mode               mclk_mode;
+        bool                                    en_acr_pkt;
+        bool                                    en_dsd_audio;
+        bool                                    en_parallel_aud_input;
+        bool                                    en_spdif;
+};
+#endif
diff --git a/drivers/video/omap2/dss/hdmi_omap4_panel.c b/drivers/video/omap2/dss/hdmi_omap4_panel.c
new file mode 100644
index 000000000000..7d4f2bd7c506
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi_omap4_panel.c
@@ -0,0 +1,222 @@
+/*
+ * hdmi_omap4_panel.c
+ *
+ * HDMI library support functions for TI OMAP4 processors.
+ *
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors:     Mythri P k <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static struct {
+        struct mutex hdmi_lock;
+} hdmi;
+
+
+static int hdmi_panel_probe(struct omap_dss_device *dssdev)
+{
+        DSSDBG("ENTER hdmi_panel_probe\n");
+
+        dssdev->panel.config = OMAP_DSS_LCD_TFT |
+                        OMAP_DSS_LCD_IVS | OMAP_DSS_LCD_IHS;
+
+        /*
+         * Initialize the timings to 640 * 480
+         * This is only for framebuffer update not for TV timing setting
+         * Setting TV timing will be done only on enable
+         */
+        dssdev->panel.timings.x_res = 640;
+        dssdev->panel.timings.y_res = 480;
+
+        DSSDBG("hdmi_panel_probe x_res= %d y_res = %d\n",
+                dssdev->panel.timings.x_res,
+                dssdev->panel.timings.y_res);
+        return 0;
+}
+
+static void hdmi_panel_remove(struct omap_dss_device *dssdev)
+{
+
+}
+
+static int hdmi_panel_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+        DSSDBG("ENTER hdmi_panel_enable\n");
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        r = omapdss_hdmi_display_enable(dssdev);
+        if (r) {
+                DSSERR("failed to power on\n");
+                goto err;
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+
+        return r;
+}
+
+static void hdmi_panel_disable(struct omap_dss_device *dssdev)
+{
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                omapdss_hdmi_display_disable(dssdev);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static int hdmi_panel_suspend(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+        omapdss_hdmi_display_disable(dssdev);
+
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+
+        return r;
+}
+
+static int hdmi_panel_resume(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_SUSPENDED) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        r = omapdss_hdmi_display_enable(dssdev);
+        if (r) {
+                DSSERR("failed to power on\n");
+                goto err;
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+
+        return r;
+}
+
+static void hdmi_get_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        mutex_lock(&hdmi.hdmi_lock);
+
+        *timings = dssdev->panel.timings;
+
+        mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static void hdmi_set_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        DSSDBG("hdmi_set_timings\n");
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        dssdev->panel.timings = *timings;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+                /* turn the hdmi off and on to get new timings to use */
+                omapdss_hdmi_display_disable(dssdev);
+                omapdss_hdmi_display_set_timing(dssdev);
+        }
+
+        mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static int hdmi_check_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        int r = 0;
+
+        DSSDBG("hdmi_check_timings\n");
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        r = omapdss_hdmi_display_check_timing(dssdev, timings);
+        if (r) {
+                DSSERR("Timing cannot be applied\n");
+                goto err;
+        }
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+        return r;
+}
+
+static struct omap_dss_driver hdmi_driver = {
+        .probe          = hdmi_panel_probe,
+        .remove         = hdmi_panel_remove,
+        .enable         = hdmi_panel_enable,
+        .disable        = hdmi_panel_disable,
+        .suspend        = hdmi_panel_suspend,
+        .resume         = hdmi_panel_resume,
+        .get_timings    = hdmi_get_timings,
+        .set_timings    = hdmi_set_timings,
+        .check_timings  = hdmi_check_timings,
+        .driver                 = {
+                .name   = "hdmi_panel",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int hdmi_panel_init(void)
+{
+        mutex_init(&hdmi.hdmi_lock);
+
+        omap_dss_register_driver(&hdmi_driver);
+
+        return 0;
+}
+
+void hdmi_panel_exit(void)
+{
+        omap_dss_unregister_driver(&hdmi_driver);
+
+}
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 6a649ab5539e..9aeea50e33ff 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -29,10 +29,11 @@
 #include <linux/spinlock.h>
 #include <linux/jiffies.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static int num_managers;
 static struct list_head manager_list;
@@ -392,6 +393,7 @@ struct overlay_cache_data {
 
         u32 paddr;
         void __iomem *vaddr;
+        u32 p_uv_addr; /* relevant for NV12 format only */
         u16 screen_width;
         u16 width;
         u16 height;
@@ -405,6 +407,7 @@ struct overlay_cache_data {
         u16 out_width;  /* if 0, out_width == width */
         u16 out_height; /* if 0, out_height == height */
         u8 global_alpha;
+        u8 pre_mult_alpha;
 
         enum omap_channel channel;
         bool replication;
@@ -448,8 +451,8 @@ struct manager_cache_data {
 
 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;
@@ -511,11 +514,16 @@ static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
         unsigned long timeout = msecs_to_jiffies(500);
         u32 irq;
 
-        if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC)
+        if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC) {
                 irq = DISPC_IRQ_EVSYNC_ODD;
-        else
-                irq = DISPC_IRQ_VSYNC;
-
+        } else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI) {
+                irq = DISPC_IRQ_EVSYNC_EVEN;
+        } else {
+                if (mgr->id == OMAP_DSS_CHANNEL_LCD)
+                        irq = DISPC_IRQ_VSYNC;
+                else
+                        irq = DISPC_IRQ_VSYNC2;
+        }
         return omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
 }
 
@@ -523,7 +531,6 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 {
         unsigned long timeout = msecs_to_jiffies(500);
         struct manager_cache_data *mc;
-        enum omap_channel channel;
         u32 irq;
         int r;
         int i;
@@ -532,9 +539,9 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
         if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
-        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+                        || dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
                 irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
-                channel = OMAP_DSS_CHANNEL_DIGIT;
         } else {
                 if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
                         enum omap_dss_update_mode mode;
@@ -542,11 +549,14 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
                         if (mode != OMAP_DSS_UPDATE_AUTO)
                                 return 0;
 
-                        irq = DISPC_IRQ_FRAMEDONE;
+                        irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
+                                DISPC_IRQ_FRAMEDONE
+                                : DISPC_IRQ_FRAMEDONE2;
                 } else {
-                        irq = DISPC_IRQ_VSYNC;
+                        irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
+                                DISPC_IRQ_VSYNC
+                                : DISPC_IRQ_VSYNC2;
                 }
-                channel = OMAP_DSS_CHANNEL_LCD;
         }
 
         mc = &dss_cache.manager_cache[mgr->id];
@@ -593,7 +603,6 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
 {
         unsigned long timeout = msecs_to_jiffies(500);
-        enum omap_channel channel;
         struct overlay_cache_data *oc;
         struct omap_dss_device *dssdev;
         u32 irq;
@@ -608,9 +617,9 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
         if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
-        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+                        || dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
                 irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
-                channel = OMAP_DSS_CHANNEL_DIGIT;
         } else {
                 if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
                         enum omap_dss_update_mode mode;
@@ -618,11 +627,14 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
                         if (mode != OMAP_DSS_UPDATE_AUTO)
                                 return 0;
 
-                        irq = DISPC_IRQ_FRAMEDONE;
+                        irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
+                                DISPC_IRQ_FRAMEDONE
+                                : DISPC_IRQ_FRAMEDONE2;
                 } else {
-                        irq = DISPC_IRQ_VSYNC;
+                        irq = (dssdev->manager->id == OMAP_DSS_CHANNEL_LCD) ?
+                                DISPC_IRQ_VSYNC
+                                : DISPC_IRQ_VSYNC2;
                 }
-                channel = OMAP_DSS_CHANNEL_LCD;
         }
 
         oc = &dss_cache.overlay_cache[ovl->id];
@@ -764,10 +776,17 @@ static int configure_overlay(enum omap_plane plane)
                 }
 
                 switch (c->color_mode) {
+                case OMAP_DSS_COLOR_NV12:
+                        bpp = 8;
+                        break;
                 case OMAP_DSS_COLOR_RGB16:
                 case OMAP_DSS_COLOR_ARGB16:
                 case OMAP_DSS_COLOR_YUV2:
                 case OMAP_DSS_COLOR_UYVY:
+                case OMAP_DSS_COLOR_RGBA16:
+                case OMAP_DSS_COLOR_RGBX16:
+                case OMAP_DSS_COLOR_ARGB16_1555:
+                case OMAP_DSS_COLOR_XRGB16_1555:
                         bpp = 16;
                         break;
 
@@ -841,7 +860,10 @@ static int configure_overlay(enum omap_plane plane)
                         c->rotation_type,
                         c->rotation,
                         c->mirror,
-                        c->global_alpha);
+                        c->global_alpha,
+                        c->pre_mult_alpha,
+                        c->channel,
+                        c->p_uv_addr);
 
         if (r) {
                 /* this shouldn't happen */
@@ -882,21 +904,21 @@ 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;
         busy = false;
 
-        mgr_busy[0] = dispc_go_busy(0);
-        mgr_busy[1] = dispc_go_busy(1);
-        mgr_go[0] = false;
-        mgr_go[1] = false;
+        for (i = 0; i < num_mgrs; i++) {
+                mgr_busy[i] = dispc_go_busy(i);
+                mgr_go[i] = false;
+        }
 
         /* Commit overlay settings */
         for (i = 0; i < num_ovls; ++i) {
@@ -989,7 +1011,7 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
 {
         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;
@@ -1121,8 +1143,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;
 
@@ -1151,13 +1173,14 @@ 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];
+        u32 irq_mask;
 
-        mgr_busy[0] = dispc_go_busy(0);
-        mgr_busy[1] = dispc_go_busy(1);
+        for (i = 0; i < num_mgrs; i++)
+                mgr_busy[i] = dispc_go_busy(i);
 
         spin_lock(&dss_cache.lock);
 
@@ -1178,8 +1201,8 @@ static void dss_apply_irq_handler(void *data, u32 mask)
                 goto end;
 
         /* re-read busy flags */
-        mgr_busy[0] = dispc_go_busy(0);
-        mgr_busy[1] = dispc_go_busy(1);
+        for (i = 0; i < num_mgrs; i++)
+                mgr_busy[i] = dispc_go_busy(i);
 
         /* keep running as long as there are busy managers, so that
          * we can collect overlay-applied information */
@@ -1188,9 +1211,12 @@ static void dss_apply_irq_handler(void *data, u32 mask)
                         goto end;
         }
 
-        omap_dispc_unregister_isr(dss_apply_irq_handler, NULL,
-                        DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_ODD |
-                        DISPC_IRQ_EVSYNC_EVEN);
+        irq_mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_ODD |
+                        DISPC_IRQ_EVSYNC_EVEN;
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                irq_mask |= DISPC_IRQ_VSYNC2;
+
+        omap_dispc_unregister_isr(dss_apply_irq_handler, NULL, irq_mask);
         dss_cache.irq_enabled = false;
 
 end:
@@ -1252,6 +1278,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 
                 oc->paddr = ovl->info.paddr;
                 oc->vaddr = ovl->info.vaddr;
+                oc->p_uv_addr = ovl->info.p_uv_addr;
                 oc->screen_width = ovl->info.screen_width;
                 oc->width = ovl->info.width;
                 oc->height = ovl->info.height;
@@ -1264,6 +1291,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
                 oc->out_width = ovl->info.out_width;
                 oc->out_height = ovl->info.out_height;
                 oc->global_alpha = ovl->info.global_alpha;
+                oc->pre_mult_alpha = ovl->info.pre_mult_alpha;
 
                 oc->replication =
                         dss_use_replication(dssdev, ovl->info.color_mode);
@@ -1363,6 +1391,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
                 case OMAP_DISPLAY_TYPE_DBI:
                 case OMAP_DISPLAY_TYPE_SDI:
                 case OMAP_DISPLAY_TYPE_VENC:
+                case OMAP_DISPLAY_TYPE_HDMI:
                         default_get_overlay_fifo_thresholds(ovl->id, size,
                                         &oc->burst_size, &oc->fifo_low,
                                         &oc->fifo_high);
@@ -1380,15 +1409,20 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
         }
 
         r = 0;
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         if (!dss_cache.irq_enabled) {
-                r = omap_dispc_register_isr(dss_apply_irq_handler, NULL,
-                                DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_ODD |
-                                DISPC_IRQ_EVSYNC_EVEN);
+                u32 mask;
+
+                mask = DISPC_IRQ_VSYNC  | DISPC_IRQ_EVSYNC_ODD |
+                        DISPC_IRQ_EVSYNC_EVEN;
+                if (dss_has_feature(FEAT_MGR_LCD2))
+                        mask |= DISPC_IRQ_VSYNC2;
+
+                r = omap_dispc_register_isr(dss_apply_irq_handler, NULL, mask);
                 dss_cache.irq_enabled = true;
         }
         configure_dispc();
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         spin_unlock_irqrestore(&dss_cache.lock, flags);
 
@@ -1461,7 +1495,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);
 
@@ -1471,14 +1505,14 @@ 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;
+                case 2:
+                        mgr->name = "lcd2";
+                        mgr->id = OMAP_DSS_CHANNEL_LCD2;
                         break;
                 }
 
@@ -1494,6 +1528,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 244dca81a399..0f08025b1f0e 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -31,10 +31,11 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static int num_overlays;
 static struct list_head overlay_list;
@@ -200,12 +201,16 @@ static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
 static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
                 size_t size)
 {
-        int r;
+        int r, enable;
         struct omap_overlay_info info;
 
         ovl->get_overlay_info(ovl, &info);
 
-        info.enabled = simple_strtoul(buf, NULL, 10);
+        r = kstrtoint(buf, 0, &enable);
+        if (r)
+                return r;
+
+        info.enabled = !!enable;
 
         r = ovl->set_overlay_info(ovl, &info);
         if (r)
@@ -230,17 +235,65 @@ static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
                 const char *buf, size_t size)
 {
         int r;
+        u8 alpha;
         struct omap_overlay_info info;
 
+        r = kstrtou8(buf, 0, &alpha);
+        if (r)
+                return r;
+
         ovl->get_overlay_info(ovl, &info);
 
         /* 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);
+                info.global_alpha = alpha;
+
+        r = ovl->set_overlay_info(ovl, &info);
+        if (r)
+                return r;
+
+        if (ovl->manager) {
+                r = ovl->manager->apply(ovl->manager);
+                if (r)
+                        return r;
+        }
+
+        return size;
+}
+
+static ssize_t overlay_pre_mult_alpha_show(struct omap_overlay *ovl,
+                char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        ovl->info.pre_mult_alpha);
+}
+
+static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        u8 alpha;
+        struct omap_overlay_info info;
+
+        r = kstrtou8(buf, 0, &alpha);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        /* only GFX and Video2 plane support pre alpha multiplied
+         * set zero for Video1 plane
+         */
+        if (!dss_has_feature(FEAT_GLOBAL_ALPHA_VID1) &&
+                ovl->id == OMAP_DSS_VIDEO1)
+                info.pre_mult_alpha = 0;
+        else
+                info.pre_mult_alpha = alpha;
 
         r = ovl->set_overlay_info(ovl, &info);
         if (r)
@@ -278,6 +331,9 @@ static OVERLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
                 overlay_enabled_show, overlay_enabled_store);
 static OVERLAY_ATTR(global_alpha, S_IRUGO|S_IWUSR,
                 overlay_global_alpha_show, overlay_global_alpha_store);
+static OVERLAY_ATTR(pre_mult_alpha, S_IRUGO|S_IWUSR,
+                overlay_pre_mult_alpha_show,
+                overlay_pre_mult_alpha_store);
 
 static struct attribute *overlay_sysfs_attrs[] = {
         &overlay_attr_name.attr,
@@ -288,6 +344,7 @@ static struct attribute *overlay_sysfs_attrs[] = {
         &overlay_attr_output_size.attr,
         &overlay_attr_enabled.attr,
         &overlay_attr_global_alpha.attr,
+        &overlay_attr_pre_mult_alpha.attr,
         NULL
 };
 
@@ -447,16 +504,21 @@ static int omap_dss_set_manager(struct omap_overlay *ovl,
 
         ovl->manager = mgr;
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-        /* XXX: on manual update display, in auto update mode, a bug happens
-         * here. When an overlay is first enabled on LCD, then it's disabled,
-         * and the manager is changed to TV, we sometimes get SYNC_LOST_DIGIT
-         * errors. Waiting before changing the channel_out fixes it. I'm
-         * guessing that the overlay is still somehow being used for the LCD,
-         * but I don't understand how or why. */
-        msleep(40);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
+        /* XXX: When there is an overlay on a DSI manual update display, and
+         * the overlay is first disabled, then moved to tv, and enabled, we
+         * seem to get SYNC_LOST_DIGIT error.
+         *
+         * Waiting doesn't seem to help, but updating the manual update display
+         * after disabling the overlay seems to fix this. This hints that the
+         * overlay is perhaps somehow tied to the LCD output until the output
+         * is updated.
+         *
+         * Userspace workaround for this is to update the LCD after disabling
+         * the overlay, but before moving the overlay to TV.
+         */
         dispc_set_channel_out(ovl->id, mgr->id);
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         return 0;
 }
@@ -510,11 +572,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 +597,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 +607,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 +620,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 +632,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,
@@ -627,12 +683,23 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
         int i;
         struct omap_overlay_manager *lcd_mgr;
         struct omap_overlay_manager *tv_mgr;
+        struct omap_overlay_manager *lcd2_mgr = NULL;
         struct omap_overlay_manager *mgr = NULL;
 
         lcd_mgr = omap_dss_get_overlay_manager(OMAP_DSS_OVL_MGR_LCD);
         tv_mgr = omap_dss_get_overlay_manager(OMAP_DSS_OVL_MGR_TV);
-
-        if (dssdev->type != OMAP_DISPLAY_TYPE_VENC) {
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                lcd2_mgr = omap_dss_get_overlay_manager(OMAP_DSS_OVL_MGR_LCD2);
+
+        if (dssdev->channel == OMAP_DSS_CHANNEL_LCD2) {
+                if (!lcd2_mgr->device || force) {
+                        if (lcd2_mgr->device)
+                                lcd2_mgr->unset_device(lcd2_mgr);
+                        lcd2_mgr->set_device(lcd2_mgr, dssdev);
+                        mgr = lcd2_mgr;
+                }
+        } else if (dssdev->type != OMAP_DISPLAY_TYPE_VENC
+                        && dssdev->type != OMAP_DISPLAY_TYPE_HDMI) {
                 if (!lcd_mgr->device || force) {
                         if (lcd_mgr->device)
                                 lcd_mgr->unset_device(lcd_mgr);
@@ -641,7 +708,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
                 }
         }
 
-        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+                        || dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
                 if (!tv_mgr->device || force) {
                         if (tv_mgr->device)
                                 tv_mgr->unset_device(tv_mgr);
@@ -651,7 +719,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 bbe62464e92d..c06fbe0bc678 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -32,12 +32,11 @@
 #include <linux/ktime.h>
 #include <linux/hrtimer.h>
 #include <linux/seq_file.h>
+#include <linux/semaphore.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include "dss.h"
 
-#define RFBI_BASE               0x48050800
-
 struct rfbi_reg { u16 idx; };
 
 #define RFBI_REG(idx)           ((const struct rfbi_reg) { idx })
@@ -67,9 +66,6 @@ struct rfbi_reg { u16 idx; };
 #define REG_FLD_MOD(idx, val, start, end) \
         rfbi_write_reg(idx, FLD_MOD(rfbi_read_reg(idx), val, start, end))
 
-/* To work around an RFBI transfer rate limitation */
-#define OMAP_RFBI_RATE_LIMIT    1
-
 enum omap_rfbi_cycleformat {
         OMAP_DSS_RFBI_CYCLEFORMAT_1_1 = 0,
         OMAP_DSS_RFBI_CYCLEFORMAT_2_1 = 1,
@@ -91,15 +87,11 @@ enum omap_rfbi_parallelmode {
         OMAP_DSS_RFBI_PARALLELMODE_16 = 3,
 };
 
-enum update_cmd {
-        RFBI_CMD_UPDATE = 0,
-        RFBI_CMD_SYNC   = 1,
-};
-
 static int rfbi_convert_timings(struct rfbi_timings *t);
 static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div);
 
 static struct {
+        struct platform_device *pdev;
         void __iomem    *base;
 
         unsigned long   l4_khz;
@@ -115,20 +107,9 @@ static struct {
 
         struct omap_dss_device *dssdev[2];
 
-        struct kfifo      cmd_fifo;
-        spinlock_t        cmd_lock;
-        struct completion cmd_done;
-        atomic_t          cmd_fifo_full;
-        atomic_t          cmd_pending;
+        struct semaphore bus_lock;
 } rfbi;
 
-struct update_region {
-        u16     x;
-        u16     y;
-        u16     w;
-        u16     h;
-};
-
 static inline void rfbi_write_reg(const struct rfbi_reg idx, u32 val)
 {
         __raw_writel(val, rfbi.base + idx.idx);
@@ -142,14 +123,25 @@ static inline u32 rfbi_read_reg(const struct rfbi_reg idx)
 static void rfbi_enable_clocks(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
+}
+
+void rfbi_bus_lock(void)
+{
+        down(&rfbi.bus_lock);
+}
+EXPORT_SYMBOL(rfbi_bus_lock);
+
+void rfbi_bus_unlock(void)
+{
+        up(&rfbi.bus_lock);
 }
+EXPORT_SYMBOL(rfbi_bus_unlock);
 
 void omap_rfbi_write_command(const void *buf, u32 len)
 {
-        rfbi_enable_clocks(1);
         switch (rfbi.parallelmode) {
         case OMAP_DSS_RFBI_PARALLELMODE_8:
         {
@@ -173,13 +165,11 @@ void omap_rfbi_write_command(const void *buf, u32 len)
         default:
                 BUG();
         }
-        rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_write_command);
 
 void omap_rfbi_read_data(void *buf, u32 len)
 {
-        rfbi_enable_clocks(1);
         switch (rfbi.parallelmode) {
         case OMAP_DSS_RFBI_PARALLELMODE_8:
         {
@@ -207,13 +197,11 @@ void omap_rfbi_read_data(void *buf, u32 len)
         default:
                 BUG();
         }
-        rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_read_data);
 
 void omap_rfbi_write_data(const void *buf, u32 len)
 {
-        rfbi_enable_clocks(1);
         switch (rfbi.parallelmode) {
         case OMAP_DSS_RFBI_PARALLELMODE_8:
         {
@@ -238,7 +226,6 @@ void omap_rfbi_write_data(const void *buf, u32 len)
                 BUG();
 
         }
-        rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_write_data);
 
@@ -250,8 +237,6 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
         int horiz_offset = scr_width - w;
         int i;
 
-        rfbi_enable_clocks(1);
-
         if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_16 &&
            rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_8) {
                 const u16 __iomem *pd = buf;
@@ -296,13 +281,11 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
         } else {
                 BUG();
         }
-
-        rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omap_rfbi_write_pixels);
 
-void rfbi_transfer_area(u16 width, u16 height,
-                             void (callback)(void *data), void *data)
+static void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
+                u16 height, void (*callback)(void *data), void *data)
 {
         u32 l;
 
@@ -311,15 +294,13 @@ void rfbi_transfer_area(u16 width, u16 height,
 
         DSSDBG("rfbi_transfer_area %dx%d\n", width, height);
 
-        dispc_set_lcd_size(width, height);
+        dispc_set_lcd_size(dssdev->manager->id, width, height);
 
-        dispc_enable_channel(OMAP_DSS_CHANNEL_LCD, true);
+        dispc_enable_channel(dssdev->manager->id, true);
 
         rfbi.framedone_callback = callback;
         rfbi.framedone_callback_data = data;
 
-        rfbi_enable_clocks(1);
-
         rfbi_write_reg(RFBI_PIXEL_CNT, width * height);
 
         l = rfbi_read_reg(RFBI_CONTROL);
@@ -338,15 +319,11 @@ static void framedone_callback(void *data, u32 mask)
 
         REG_FLD_MOD(RFBI_CONTROL, 0, 0, 0);
 
-        rfbi_enable_clocks(0);
-
         callback = rfbi.framedone_callback;
         rfbi.framedone_callback = NULL;
 
         if (callback != NULL)
                 callback(rfbi.framedone_callback_data);
-
-        atomic_set(&rfbi.cmd_pending, 0);
 }
 
 #if 1 /* VERBOSE */
@@ -436,7 +413,7 @@ static int calc_extif_timings(struct rfbi_timings *t)
 }
 
 
-void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
+static void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
 {
         int r;
 
@@ -448,7 +425,6 @@ void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
 
         BUG_ON(!t->converted);
 
-        rfbi_enable_clocks(1);
         rfbi_write_reg(RFBI_ONOFF_TIME(rfbi_module), t->tim[0]);
         rfbi_write_reg(RFBI_CYCLE_TIME(rfbi_module), t->tim[1]);
 
@@ -457,7 +433,6 @@ void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
                     (t->tim[2] ? 1 : 0), 4, 4);
 
         rfbi_print_timings();
-        rfbi_enable_clocks(0);
 }
 
 static int ps_to_rfbi_ticks(int time, int div)
@@ -473,59 +448,6 @@ static int ps_to_rfbi_ticks(int time, int div)
         return ret;
 }
 
-#ifdef OMAP_RFBI_RATE_LIMIT
-unsigned long rfbi_get_max_tx_rate(void)
-{
-        unsigned long   l4_rate, dss1_rate;
-        int             min_l4_ticks = 0;
-        int             i;
-
-        /* According to TI this can't be calculated so make the
-         * adjustments for a couple of known frequencies and warn for
-         * others.
-         */
-        static const struct {
-                unsigned long l4_clk;           /* HZ */
-                unsigned long dss1_clk;         /* HZ */
-                unsigned long min_l4_ticks;
-        } ftab[] = {
-                { 55,   132,    7, },           /* 7.86 MPix/s */
-                { 110,  110,    12, },          /* 9.16 MPix/s */
-                { 110,  132,    10, },          /* 11   Mpix/s */
-                { 120,  120,    10, },          /* 12   Mpix/s */
-                { 133,  133,    10, },          /* 13.3 Mpix/s */
-        };
-
-        l4_rate = rfbi.l4_khz / 1000;
-        dss1_rate = dss_clk_get_rate(DSS_CLK_FCK1) / 1000000;
-
-        for (i = 0; i < ARRAY_SIZE(ftab); i++) {
-                /* Use a window instead of an exact match, to account
-                 * for different DPLL multiplier / divider pairs.
-                 */
-                if (abs(ftab[i].l4_clk - l4_rate) < 3 &&
-                    abs(ftab[i].dss1_clk - dss1_rate) < 3) {
-                        min_l4_ticks = ftab[i].min_l4_ticks;
-                        break;
-                }
-        }
-        if (i == ARRAY_SIZE(ftab)) {
-                /* Can't be sure, return anyway the maximum not
-                 * rate-limited. This might cause a problem only for the
-                 * tearing synchronisation.
-                 */
-                DSSERR("can't determine maximum RFBI transfer rate\n");
-                return rfbi.l4_khz * 1000;
-        }
-        return rfbi.l4_khz * 1000 / min_l4_ticks;
-}
-#else
-int rfbi_get_max_tx_rate(void)
-{
-        return rfbi.l4_khz * 1000;
-}
-#endif
-
 static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div)
 {
         *clk_period = 1000000000 / rfbi.l4_khz;
@@ -645,7 +567,6 @@ int omap_rfbi_setup_te(enum omap_rfbi_te_mode mode,
         DSSDBG("setup_te: mode %d hs %d vs %d hs_inv %d vs_inv %d\n",
                 mode, hs, vs, hs_pol_inv, vs_pol_inv);
 
-        rfbi_enable_clocks(1);
         rfbi_write_reg(RFBI_HSYNC_WIDTH, hs);
         rfbi_write_reg(RFBI_VSYNC_WIDTH, vs);
 
@@ -658,7 +579,6 @@ int omap_rfbi_setup_te(enum omap_rfbi_te_mode mode,
                 l &= ~(1 << 20);
         else
                 l |= 1 << 20;
-        rfbi_enable_clocks(0);
 
         return 0;
 }
@@ -673,7 +593,6 @@ int omap_rfbi_enable_te(bool enable, unsigned line)
         if (line > (1 << 11) - 1)
                 return -EINVAL;
 
-        rfbi_enable_clocks(1);
         l = rfbi_read_reg(RFBI_CONFIG(0));
         l &= ~(0x3 << 2);
         if (enable) {
@@ -683,50 +602,12 @@ int omap_rfbi_enable_te(bool enable, unsigned line)
                 rfbi.te_enabled = 0;
         rfbi_write_reg(RFBI_CONFIG(0), l);
         rfbi_write_reg(RFBI_LINE_NUMBER, line);
-        rfbi_enable_clocks(0);
 
         return 0;
 }
 EXPORT_SYMBOL(omap_rfbi_enable_te);
 
-#if 0
-static void rfbi_enable_config(int enable1, int enable2)
-{
-        u32 l;
-        int cs = 0;
-
-        if (enable1)
-                cs |= 1<<0;
-        if (enable2)
-                cs |= 1<<1;
-
-        rfbi_enable_clocks(1);
-
-        l = rfbi_read_reg(RFBI_CONTROL);
-
-        l = FLD_MOD(l, cs, 3, 2);
-        l = FLD_MOD(l, 0, 1, 1);
-
-        rfbi_write_reg(RFBI_CONTROL, l);
-
-
-        l = rfbi_read_reg(RFBI_CONFIG(0));
-        l = FLD_MOD(l, 0, 3, 2); /* TRIGGERMODE: ITE */
-        /*l |= FLD_VAL(2, 8, 7); */ /* L4FORMAT, 2pix/L4 */
-        /*l |= FLD_VAL(0, 8, 7); */ /* L4FORMAT, 1pix/L4 */
-
-        l = FLD_MOD(l, 0, 16, 16); /* A0POLARITY */
-        l = FLD_MOD(l, 1, 20, 20); /* TE_VSYNC_POLARITY */
-        l = FLD_MOD(l, 1, 21, 21); /* HSYNCPOLARITY */
-
-        l = FLD_MOD(l, OMAP_DSS_RFBI_PARALLELMODE_8, 1, 0);
-        rfbi_write_reg(RFBI_CONFIG(0), l);
-
-        rfbi_enable_clocks(0);
-}
-#endif
-
-int rfbi_configure(int rfbi_module, int bpp, int lines)
+static int rfbi_configure(int rfbi_module, int bpp, int lines)
 {
         u32 l;
         int cycle1 = 0, cycle2 = 0, cycle3 = 0;
@@ -822,8 +703,6 @@ int rfbi_configure(int rfbi_module, int bpp, int lines)
                 break;
         }
 
-        rfbi_enable_clocks(1);
-
         REG_FLD_MOD(RFBI_CONTROL, 0, 3, 2); /* clear CS */
 
         l = 0;
@@ -857,11 +736,15 @@ int rfbi_configure(int rfbi_module, int bpp, int lines)
         DSSDBG("RFBI config: bpp %d, lines %d, cycles: 0x%x 0x%x 0x%x\n",
                bpp, lines, cycle1, cycle2, cycle3);
 
-        rfbi_enable_clocks(0);
-
         return 0;
 }
-EXPORT_SYMBOL(rfbi_configure);
+
+int omap_rfbi_configure(struct omap_dss_device *dssdev, int pixel_size,
+                int data_lines)
+{
+        return rfbi_configure(dssdev->phy.rfbi.channel, pixel_size, data_lines);
+}
+EXPORT_SYMBOL(omap_rfbi_configure);
 
 int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
                 u16 *x, u16 *y, u16 *w, u16 *h)
@@ -887,7 +770,7 @@ int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
                 dss_setup_partial_planes(dssdev, x, y, w, h, true);
-                dispc_set_lcd_size(*w, *h);
+                dispc_set_lcd_size(dssdev->manager->id, *w, *h);
         }
 
         return 0;
@@ -899,7 +782,7 @@ int omap_rfbi_update(struct omap_dss_device *dssdev,
                 void (*callback)(void *), void *data)
 {
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-                rfbi_transfer_area(w, h, callback, data);
+                rfbi_transfer_area(dssdev, w, h, callback, data);
         } else {
                 struct omap_overlay *ovl;
                 void __iomem *addr;
@@ -922,7 +805,7 @@ void rfbi_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, rfbi_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(RFBI_REVISION);
         DUMPREG(RFBI_SYSCONFIG);
@@ -953,58 +836,16 @@ void rfbi_dump_regs(struct seq_file *s)
         DUMPREG(RFBI_VSYNC_WIDTH);
         DUMPREG(RFBI_HSYNC_WIDTH);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
-int rfbi_init(void)
-{
-        u32 rev;
-        u32 l;
-
-        spin_lock_init(&rfbi.cmd_lock);
-
-        init_completion(&rfbi.cmd_done);
-        atomic_set(&rfbi.cmd_fifo_full, 0);
-        atomic_set(&rfbi.cmd_pending, 0);
-
-        rfbi.base = ioremap(RFBI_BASE, SZ_256);
-        if (!rfbi.base) {
-                DSSERR("can't ioremap RFBI\n");
-                return -ENOMEM;
-        }
-
-        rfbi_enable_clocks(1);
-
-        msleep(10);
-
-        rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
-
-        /* Enable autoidle and smart-idle */
-        l = rfbi_read_reg(RFBI_SYSCONFIG);
-        l |= (1 << 0) | (2 << 3);
-        rfbi_write_reg(RFBI_SYSCONFIG, l);
-
-        rev = rfbi_read_reg(RFBI_REVISION);
-        printk(KERN_INFO "OMAP RFBI rev %d.%d\n",
-               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-        rfbi_enable_clocks(0);
-
-        return 0;
-}
-
-void rfbi_exit(void)
-{
-        DSSDBG("rfbi_exit\n");
-
-        iounmap(rfbi.base);
-}
-
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
         int r;
 
+        rfbi_enable_clocks(1);
+
         r = omap_dss_start_device(dssdev);
         if (r) {
                 DSSERR("failed to start device\n");
@@ -1018,11 +859,13 @@ int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
                 goto err1;
         }
 
-        dispc_set_lcd_display_type(OMAP_DSS_LCD_DISPLAY_TFT);
+        dispc_set_lcd_display_type(dssdev->manager->id,
+                        OMAP_DSS_LCD_DISPLAY_TFT);
 
-        dispc_set_parallel_interface_mode(OMAP_DSS_PARALLELMODE_RFBI);
+        dispc_set_parallel_interface_mode(dssdev->manager->id,
+                        OMAP_DSS_PARALLELMODE_RFBI);
 
-        dispc_set_tft_data_lines(dssdev->ctrl.pixel_size);
+        dispc_set_tft_data_lines(dssdev->manager->id, dssdev->ctrl.pixel_size);
 
         rfbi_configure(dssdev->phy.rfbi.channel,
                                dssdev->ctrl.pixel_size,
@@ -1045,6 +888,8 @@ void omapdss_rfbi_display_disable(struct omap_dss_device *dssdev)
         omap_dispc_unregister_isr(framedone_callback, NULL,
                         DISPC_IRQ_FRAMEDONE);
         omap_dss_stop_device(dssdev);
+
+        rfbi_enable_clocks(0);
 }
 EXPORT_SYMBOL(omapdss_rfbi_display_disable);
 
@@ -1054,3 +899,70 @@ int rfbi_init_display(struct omap_dss_device *dssdev)
         dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
         return 0;
 }
+
+/* RFBI HW IP initialisation */
+static int omap_rfbihw_probe(struct platform_device *pdev)
+{
+        u32 rev;
+        u32 l;
+        struct resource *rfbi_mem;
+
+        rfbi.pdev = pdev;
+
+        sema_init(&rfbi.bus_lock, 1);
+
+        rfbi_mem = platform_get_resource(rfbi.pdev, IORESOURCE_MEM, 0);
+        if (!rfbi_mem) {
+                DSSERR("can't get IORESOURCE_MEM RFBI\n");
+                return -EINVAL;
+        }
+        rfbi.base = ioremap(rfbi_mem->start, resource_size(rfbi_mem));
+        if (!rfbi.base) {
+                DSSERR("can't ioremap RFBI\n");
+                return -ENOMEM;
+        }
+
+        rfbi_enable_clocks(1);
+
+        msleep(10);
+
+        rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
+
+        /* Enable autoidle and smart-idle */
+        l = rfbi_read_reg(RFBI_SYSCONFIG);
+        l |= (1 << 0) | (2 << 3);
+        rfbi_write_reg(RFBI_SYSCONFIG, l);
+
+        rev = rfbi_read_reg(RFBI_REVISION);
+        dev_dbg(&pdev->dev, "OMAP RFBI rev %d.%d\n",
+               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+        rfbi_enable_clocks(0);
+
+        return 0;
+}
+
+static int omap_rfbihw_remove(struct platform_device *pdev)
+{
+        iounmap(rfbi.base);
+        return 0;
+}
+
+static struct platform_driver omap_rfbihw_driver = {
+        .probe          = omap_rfbihw_probe,
+        .remove         = omap_rfbihw_remove,
+        .driver         = {
+                .name   = "omapdss_rfbi",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int rfbi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_rfbihw_driver);
+}
+
+void rfbi_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_rfbihw_driver);
+}
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index ee07a3cc22ef..0bd4b0350f80 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -25,22 +25,25 @@
 #include <linux/err.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 #include "dss.h"
 
 static struct {
-        bool skip_init;
         bool update_enabled;
         struct regulator *vdds_sdi_reg;
 } sdi;
 
-static void sdi_basic_init(void)
+static void sdi_basic_init(struct omap_dss_device *dssdev)
+
 {
-        dispc_set_parallel_interface_mode(OMAP_DSS_PARALLELMODE_BYPASS);
+        dispc_set_parallel_interface_mode(dssdev->manager->id,
+                        OMAP_DSS_PARALLELMODE_BYPASS);
+
+        dispc_set_lcd_display_type(dssdev->manager->id,
+                        OMAP_DSS_LCD_DISPLAY_TFT);
 
-        dispc_set_lcd_display_type(OMAP_DSS_LCD_DISPLAY_TFT);
-        dispc_set_tft_data_lines(24);
+        dispc_set_tft_data_lines(dssdev->manager->id, 24);
         dispc_lcd_enable_signal_polarity(1);
 }
 
@@ -64,26 +67,18 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
         if (r)
                 goto err1;
 
-        /* In case of skip_init sdi_init has already enabled the clocks */
-        if (!sdi.skip_init)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
-        sdi_basic_init();
+        sdi_basic_init(dssdev);
 
         /* 15.5.9.1.2 */
         dssdev->panel.config |= OMAP_DSS_LCD_RF | OMAP_DSS_LCD_ONOFF;
 
-        dispc_set_pol_freq(dssdev->panel.config, dssdev->panel.acbi,
-                        dssdev->panel.acb);
-
-        if (!sdi.skip_init) {
-                r = dss_calc_clock_div(1, t->pixel_clock * 1000,
-                                &dss_cinfo, &dispc_cinfo);
-        } else {
-                r = dss_get_clock_div(&dss_cinfo);
-                r = dispc_get_clock_div(&dispc_cinfo);
-        }
+        dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
+                        dssdev->panel.acbi, dssdev->panel.acb);
 
+        r = dss_calc_clock_div(1, t->pixel_clock * 1000,
+                        &dss_cinfo, &dispc_cinfo);
         if (r)
                 goto err2;
 
@@ -102,31 +97,27 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
         }
 
 
-        dispc_set_lcd_timings(t);
+        dispc_set_lcd_timings(dssdev->manager->id, t);
 
         r = dss_set_clock_div(&dss_cinfo);
         if (r)
                 goto err2;
 
-        r = dispc_set_clock_div(&dispc_cinfo);
+        r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
         if (r)
                 goto err2;
 
-        if (!sdi.skip_init) {
-                dss_sdi_init(dssdev->phy.sdi.datapairs);
-                r = dss_sdi_enable();
-                if (r)
-                        goto err1;
-                mdelay(2);
-        }
+        dss_sdi_init(dssdev->phy.sdi.datapairs);
+        r = dss_sdi_enable();
+        if (r)
+                goto err1;
+        mdelay(2);
 
         dssdev->manager->enable(dssdev->manager);
 
-        sdi.skip_init = 0;
-
         return 0;
 err2:
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
         regulator_disable(sdi.vdds_sdi_reg);
 err1:
         omap_dss_stop_device(dssdev);
@@ -141,7 +132,7 @@ void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 
         dss_sdi_disable();
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         regulator_disable(sdi.vdds_sdi_reg);
 
@@ -153,25 +144,24 @@ int sdi_init_display(struct omap_dss_device *dssdev)
 {
         DSSDBG("SDI init\n");
 
+        if (sdi.vdds_sdi_reg == NULL) {
+                struct regulator *vdds_sdi;
+
+                vdds_sdi = dss_get_vdds_sdi();
+
+                if (IS_ERR(vdds_sdi)) {
+                        DSSERR("can't get VDDS_SDI regulator\n");
+                        return PTR_ERR(vdds_sdi);
+                }
+
+                sdi.vdds_sdi_reg = vdds_sdi;
+        }
+
         return 0;
 }
 
-int sdi_init(bool skip_init)
+int sdi_init(void)
 {
-        /* we store this for first display enable, then clear it */
-        sdi.skip_init = skip_init;
-
-        sdi.vdds_sdi_reg = dss_get_vdds_sdi();
-        if (IS_ERR(sdi.vdds_sdi_reg)) {
-                DSSERR("can't get VDDS_SDI regulator\n");
-                return PTR_ERR(sdi.vdds_sdi_reg);
-        }
-        /*
-         * Enable clocks already here, otherwise there would be a toggle
-         * of them until sdi_display_enable is called.
-         */
-        if (skip_init)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
         return 0;
 }
 
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index eff35050e28a..980f919ed987 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -34,13 +34,11 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/cpu.h>
 
 #include "dss.h"
 
-#define VENC_BASE       0x48050C00
-
 /* Venc registers */
 #define VENC_REV_ID                             0x00
 #define VENC_STATUS                             0x04
@@ -289,6 +287,7 @@ const struct omap_video_timings omap_dss_ntsc_timings = {
 EXPORT_SYMBOL(omap_dss_ntsc_timings);
 
 static struct {
+        struct platform_device *pdev;
         void __iomem *base;
         struct mutex venc_lock;
         u32 wss_data;
@@ -374,18 +373,21 @@ static void venc_reset(void)
                 }
         }
 
+#ifdef CONFIG_OMAP2_DSS_SLEEP_AFTER_VENC_RESET
         /* the magical sleep that makes things work */
+        /* XXX more info? What bug this circumvents? */
         msleep(20);
+#endif
 }
 
 static void venc_enable_clocks(int enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-                                DSS_CLK_96M);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_TVFCK |
+                                DSS_CLK_VIDFCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-                                DSS_CLK_96M);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_TVFCK |
+                                DSS_CLK_VIDFCK);
 }
 
 static const struct venc_config *venc_timings_to_config(
@@ -474,6 +476,12 @@ static int venc_panel_enable(struct omap_dss_device *dssdev)
 
         mutex_lock(&venc.venc_lock);
 
+        r = omap_dss_start_device(dssdev);
+        if (r) {
+                DSSERR("failed to start device\n");
+                goto err0;
+        }
+
         if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
                 r = -EINVAL;
                 goto err1;
@@ -485,10 +493,11 @@ static int venc_panel_enable(struct omap_dss_device *dssdev)
 
         dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
-        /* wait couple of vsyncs until enabling the LCD */
-        msleep(50);
-
+        mutex_unlock(&venc.venc_lock);
+        return 0;
 err1:
+        omap_dss_stop_device(dssdev);
+err0:
         mutex_unlock(&venc.venc_lock);
 
         return r;
@@ -511,10 +520,9 @@ static void venc_panel_disable(struct omap_dss_device *dssdev)
 
         venc_power_off(dssdev);
 
-        /* wait at least 5 vsyncs after disabling the LCD */
-        msleep(100);
-
         dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        omap_dss_stop_device(dssdev);
 end:
         mutex_unlock(&venc.venc_lock);
 }
@@ -641,50 +649,23 @@ static struct omap_dss_driver venc_driver = {
 };
 /* driver end */
 
-
-
-int venc_init(struct platform_device *pdev)
+int venc_init_display(struct omap_dss_device *dssdev)
 {
-        u8 rev_id;
+        DSSDBG("init_display\n");
 
-        mutex_init(&venc.venc_lock);
+        if (venc.vdda_dac_reg == NULL) {
+                struct regulator *vdda_dac;
 
-        venc.wss_data = 0;
+                vdda_dac = regulator_get(&venc.pdev->dev, "vdda_dac");
 
-        venc.base = ioremap(VENC_BASE, SZ_1K);
-        if (!venc.base) {
-                DSSERR("can't ioremap VENC\n");
-                return -ENOMEM;
-        }
+                if (IS_ERR(vdda_dac)) {
+                        DSSERR("can't get VDDA_DAC regulator\n");
+                        return PTR_ERR(vdda_dac);
+                }
 
-        venc.vdda_dac_reg = dss_get_vdda_dac();
-        if (IS_ERR(venc.vdda_dac_reg)) {
-                iounmap(venc.base);
-                DSSERR("can't get VDDA_DAC regulator\n");
-                return PTR_ERR(venc.vdda_dac_reg);
+                venc.vdda_dac_reg = vdda_dac;
         }
 
-        venc_enable_clocks(1);
-
-        rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
-        printk(KERN_INFO "OMAP VENC rev %d\n", rev_id);
-
-        venc_enable_clocks(0);
-
-        return omap_dss_register_driver(&venc_driver);
-}
-
-void venc_exit(void)
-{
-        omap_dss_unregister_driver(&venc_driver);
-
-        iounmap(venc.base);
-}
-
-int venc_init_display(struct omap_dss_device *dssdev)
-{
-        DSSDBG("init_display\n");
-
         return 0;
 }
 
@@ -740,3 +721,73 @@ void venc_dump_regs(struct seq_file *s)
 
 #undef DUMPREG
 }
+
+/* VENC HW IP initialisation */
+static int omap_venchw_probe(struct platform_device *pdev)
+{
+        u8 rev_id;
+        struct resource *venc_mem;
+
+        venc.pdev = pdev;
+
+        mutex_init(&venc.venc_lock);
+
+        venc.wss_data = 0;
+
+        venc_mem = platform_get_resource(venc.pdev, IORESOURCE_MEM, 0);
+        if (!venc_mem) {
+                DSSERR("can't get IORESOURCE_MEM VENC\n");
+                return -EINVAL;
+        }
+        venc.base = ioremap(venc_mem->start, resource_size(venc_mem));
+        if (!venc.base) {
+                DSSERR("can't ioremap VENC\n");
+                return -ENOMEM;
+        }
+
+        venc_enable_clocks(1);
+
+        rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
+        dev_dbg(&pdev->dev, "OMAP VENC rev %d\n", rev_id);
+
+        venc_enable_clocks(0);
+
+        return omap_dss_register_driver(&venc_driver);
+}
+
+static int omap_venchw_remove(struct platform_device *pdev)
+{
+        if (venc.vdda_dac_reg != NULL) {
+                regulator_put(venc.vdda_dac_reg);
+                venc.vdda_dac_reg = NULL;
+        }
+        omap_dss_unregister_driver(&venc_driver);
+
+        iounmap(venc.base);
+        return 0;
+}
+
+static struct platform_driver omap_venchw_driver = {
+        .probe          = omap_venchw_probe,
+        .remove         = omap_venchw_remove,
+        .driver         = {
+                .name   = "omapdss_venc",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int venc_init_platform_driver(void)
+{
+        if (cpu_is_omap44xx())
+                return 0;
+
+        return platform_driver_register(&omap_venchw_driver);
+}
+
+void venc_uninit_platform_driver(void)
+{
+        if (cpu_is_omap44xx())
+                return;
+
+        return platform_driver_unregister(&omap_venchw_driver);
+}
diff --git a/drivers/video/omap2/omapfb/Kconfig b/drivers/video/omap2/omapfb/Kconfig
index 43496d6c377f..aa33386c81ff 100644
--- a/drivers/video/omap2/omapfb/Kconfig
+++ b/drivers/video/omap2/omapfb/Kconfig
@@ -1,17 +1,17 @@
 menuconfig FB_OMAP2
-        tristate "OMAP2/3 frame buffer support (EXPERIMENTAL)"
+        tristate "OMAP2+ frame buffer support (EXPERIMENTAL)"
         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
         help
-          Frame buffer driver for OMAP2/3 based boards.
+          Frame buffer driver for OMAP2+ based boards.
 
 config FB_OMAP2_DEBUG_SUPPORT
-        bool "Debug support for OMAP2/3 FB"
+        bool "Debug support for OMAP2+ FB"
         default y
         depends on FB_OMAP2
         help
diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index 6f435450987e..cff450392b79 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -28,7 +28,7 @@
 #include <linux/omapfb.h>
 #include <linux/vmalloc.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/vrfb.h>
 #include <plat/vram.h>
 
@@ -895,8 +895,16 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
                 p.display_info.xres = xres;
                 p.display_info.yres = yres;
-                p.display_info.width = 0;
-                p.display_info.height = 0;
+
+                if (display->driver->get_dimensions) {
+                        u32 w, h;
+                        display->driver->get_dimensions(display, &w, &h);
+                        p.display_info.width = w;
+                        p.display_info.height = h;
+                } else {
+                        p.display_info.width = 0;
+                        p.display_info.height = 0;
+                }
 
                 if (copy_to_user((void __user *)arg, &p.display_info,
                                         sizeof(p.display_info)))
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 04034d410d6d..505bc12a3031 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -30,7 +30,7 @@
 #include <linux/platform_device.h>
 #include <linux/omapfb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/vram.h>
 #include <plat/vrfb.h>
 
@@ -702,8 +702,16 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
                         var->xres, var->yres,
                         var->xres_virtual, var->yres_virtual);
 
-        var->height             = -1;
-        var->width              = -1;
+        if (display && display->driver->get_dimensions) {
+                u32 w, h;
+                display->driver->get_dimensions(display, &w, &h);
+                var->width = DIV_ROUND_CLOSEST(w, 1000);
+                var->height = DIV_ROUND_CLOSEST(h, 1000);
+        } else {
+                var->height = -1;
+                var->width = -1;
+        }
+
         var->grayscale          = 0;
 
         if (display && display->driver->get_timings) {
@@ -714,10 +722,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 {
@@ -749,35 +757,6 @@ static int omapfb_open(struct fb_info *fbi, int user)
 
 static int omapfb_release(struct fb_info *fbi, int user)
 {
-#if 0
-        struct omapfb_info *ofbi = FB2OFB(fbi);
-        struct omapfb2_device *fbdev = ofbi->fbdev;
-        struct omap_dss_device *display = fb2display(fbi);
-
-        DBG("Closing fb with plane index %d\n", ofbi->id);
-
-        omapfb_lock(fbdev);
-
-        if (display && display->get_update_mode && display->update) {
-                /* XXX this update should be removed, I think. But it's
-                 * good for debugging */
-                if (display->get_update_mode(display) ==
-                                OMAP_DSS_UPDATE_MANUAL) {
-                        u16 w, h;
-
-                        if (display->sync)
-                                display->sync(display);
-
-                        display->get_resolution(display, &w, &h);
-                        display->update(display, 0, 0, w, h);
-                }
-        }
-
-        if (display && display->sync)
-                display->sync(display);
-
-        omapfb_unlock(fbdev);
-#endif
         return 0;
 }
 
@@ -1263,7 +1242,6 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omap_dss_device *display = fb2display(fbi);
-        int do_update = 0;
         int r = 0;
 
         if (!display)
@@ -1279,11 +1257,6 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
                 if (display->driver->resume)
                         r = display->driver->resume(display);
 
-                if (r == 0 && display->driver->get_update_mode &&
-                                display->driver->get_update_mode(display) ==
-                                OMAP_DSS_UPDATE_MANUAL)
-                        do_update = 1;
-
                 break;
 
         case FB_BLANK_NORMAL:
@@ -1307,13 +1280,6 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 exit:
         omapfb_unlock(fbdev);
 
-        if (r == 0 && do_update && display->driver->update) {
-                u16 w, h;
-                display->driver->get_resolution(display, &w, &h);
-
-                r = display->driver->update(display, 0, 0, w, h);
-        }
-
         return r;
 }
 
@@ -2030,9 +1996,9 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
 static int omapfb_mode_to_timings(const char *mode_str,
                 struct omap_video_timings *timings, u8 *bpp)
 {
-        struct fb_info fbi;
-        struct fb_var_screeninfo var;
-        struct fb_ops fbops;
+        struct fb_info *fbi;
+        struct fb_var_screeninfo *var;
+        struct fb_ops *fbops;
         int r;
 
 #ifdef CONFIG_OMAP2_DSS_VENC
@@ -2050,39 +2016,66 @@ static int omapfb_mode_to_timings(const char *mode_str,
         /* this is quite a hack, but I wanted to use the modedb and for
          * that we need fb_info and var, so we create dummy ones */
 
-        memset(&fbi, 0, sizeof(fbi));
-        memset(&var, 0, sizeof(var));
-        memset(&fbops, 0, sizeof(fbops));
-        fbi.fbops = &fbops;
-
-        r = fb_find_mode(&var, &fbi, mode_str, NULL, 0, NULL, 24);
-
-        if (r != 0) {
-                timings->pixel_clock = PICOS2KHZ(var.pixclock);
-                timings->hfp = var.left_margin;
-                timings->hbp = var.right_margin;
-                timings->vfp = var.upper_margin;
-                timings->vbp = var.lower_margin;
-                timings->hsw = var.hsync_len;
-                timings->vsw = var.vsync_len;
-                timings->x_res = var.xres;
-                timings->y_res = var.yres;
-
-                switch (var.bits_per_pixel) {
-                case 16:
-                        *bpp = 16;
-                        break;
-                case 24:
-                case 32:
-                default:
-                        *bpp = 24;
-                        break;
-                }
+        *bpp = 0;
+        fbi = NULL;
+        var = NULL;
+        fbops = NULL;
 
-                return 0;
-        } else {
-                return -EINVAL;
+        fbi = kzalloc(sizeof(*fbi), GFP_KERNEL);
+        if (fbi == NULL) {
+                r = -ENOMEM;
+                goto err;
+        }
+
+        var = kzalloc(sizeof(*var), GFP_KERNEL);
+        if (var == NULL) {
+                r = -ENOMEM;
+                goto err;
+        }
+
+        fbops = kzalloc(sizeof(*fbops), GFP_KERNEL);
+        if (fbops == NULL) {
+                r = -ENOMEM;
+                goto err;
+        }
+
+        fbi->fbops = fbops;
+
+        r = fb_find_mode(var, fbi, mode_str, NULL, 0, NULL, 24);
+        if (r == 0) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        timings->pixel_clock = PICOS2KHZ(var->pixclock);
+        timings->hbp = var->left_margin;
+        timings->hfp = var->right_margin;
+        timings->vbp = var->upper_margin;
+        timings->vfp = var->lower_margin;
+        timings->hsw = var->hsync_len;
+        timings->vsw = var->vsync_len;
+        timings->x_res = var->xres;
+        timings->y_res = var->yres;
+
+        switch (var->bits_per_pixel) {
+        case 16:
+                *bpp = 16;
+                break;
+        case 24:
+        case 32:
+        default:
+                *bpp = 24;
+                break;
         }
+
+        r = 0;
+
+err:
+        kfree(fbi);
+        kfree(var);
+        kfree(fbops);
+
+        return r;
 }
 
 static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
@@ -2090,7 +2083,7 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 {
         int r;
         u8 bpp;
-        struct omap_video_timings timings;
+        struct omap_video_timings timings, temp_timings;
 
         r = omapfb_mode_to_timings(mode_str, &timings, &bpp);
         if (r)
@@ -2100,14 +2093,23 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
         fbdev->bpp_overrides[fbdev->num_bpp_overrides].bpp = bpp;
         ++fbdev->num_bpp_overrides;
 
-        if (!display->driver->check_timings || !display->driver->set_timings)
-                return -EINVAL;
+        if (display->driver->check_timings) {
+                r = display->driver->check_timings(display, &timings);
+                if (r)
+                        return r;
+        } else {
+                /* If check_timings is not present compare xres and yres */
+                if (display->driver->get_timings) {
+                        display->driver->get_timings(display, &temp_timings);
 
-        r = display->driver->check_timings(display, &timings);
-        if (r)
-                return r;
+                        if (temp_timings.x_res != timings.x_res ||
+                                temp_timings.y_res != timings.y_res)
+                                return -EINVAL;
+                }
+        }
 
-        display->driver->set_timings(display, &timings);
+        if (display->driver->set_timings)
+                        display->driver->set_timings(display, &timings);
 
         return 0;
 }
@@ -2132,8 +2134,9 @@ static int omapfb_parse_def_modes(struct omapfb2_device *fbdev)
         char *str, *options, *this_opt;
         int r = 0;
 
-        str = kmalloc(strlen(def_mode) + 1, GFP_KERNEL);
-        strcpy(str, def_mode);
+        str = kstrdup(def_mode, GFP_KERNEL);
+        if (!str)
+                return -ENOMEM;
         options = str;
 
         while (!r && (this_opt = strsep(&options, ",")) != NULL) {
@@ -2175,6 +2178,61 @@ static int omapfb_parse_def_modes(struct omapfb2_device *fbdev)
         return r;
 }
 
+static int omapfb_init_display(struct omapfb2_device *fbdev,
+                struct omap_dss_device *dssdev)
+{
+        struct omap_dss_driver *dssdrv = dssdev->driver;
+        int r;
+
+        r = dssdrv->enable(dssdev);
+        if (r) {
+                dev_warn(fbdev->dev, "Failed to enable display '%s'\n",
+                                dssdev->name);
+                return r;
+        }
+
+        if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
+                u16 w, h;
+                if (dssdrv->enable_te) {
+                        r = dssdrv->enable_te(dssdev, 1);
+                        if (r) {
+                                dev_err(fbdev->dev, "Failed to set TE\n");
+                                return r;
+                        }
+                }
+
+                if (dssdrv->set_update_mode) {
+                        r = dssdrv->set_update_mode(dssdev,
+                                        OMAP_DSS_UPDATE_MANUAL);
+                        if (r) {
+                                dev_err(fbdev->dev,
+                                                "Failed to set update mode\n");
+                                return r;
+                        }
+                }
+
+                dssdrv->get_resolution(dssdev, &w, &h);
+                r = dssdrv->update(dssdev, 0, 0, w, h);
+                if (r) {
+                        dev_err(fbdev->dev,
+                                        "Failed to update display\n");
+                        return r;
+                }
+        } else {
+                if (dssdrv->set_update_mode) {
+                        r = dssdrv->set_update_mode(dssdev,
+                                        OMAP_DSS_UPDATE_AUTO);
+                        if (r) {
+                                dev_err(fbdev->dev,
+                                                "Failed to set update mode\n");
+                                return r;
+                        }
+                }
+        }
+
+        return 0;
+}
+
 static int omapfb_probe(struct platform_device *pdev)
 {
         struct omapfb2_device *fbdev = NULL;
@@ -2198,6 +2256,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;
@@ -2264,30 +2332,13 @@ static int omapfb_probe(struct platform_device *pdev)
         }
 
         if (def_display) {
-                struct omap_dss_driver *dssdrv = def_display->driver;
-
-                r = def_display->driver->enable(def_display);
+                r = omapfb_init_display(fbdev, def_display);
                 if (r) {
-                        dev_warn(fbdev->dev, "Failed to enable display '%s'\n",
-                                        def_display->name);
+                        dev_err(fbdev->dev,
+                                        "failed to initialize default "
+                                        "display\n");
                         goto cleanup;
                 }
-
-                if (def_display->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
-                        u16 w, h;
-                        if (dssdrv->enable_te)
-                                dssdrv->enable_te(def_display, 1);
-                        if (dssdrv->set_update_mode)
-                                dssdrv->set_update_mode(def_display,
-                                                OMAP_DSS_UPDATE_MANUAL);
-
-                        dssdrv->get_resolution(def_display, &w, &h);
-                        def_display->driver->update(def_display, 0, 0, w, h);
-                } else {
-                        if (dssdrv->set_update_mode)
-                                dssdrv->set_update_mode(def_display,
-                                                OMAP_DSS_UPDATE_AUTO);
-                }
         }
 
         DBG("create sysfs for fbs\n");
diff --git a/drivers/video/omap2/omapfb/omapfb-sysfs.c b/drivers/video/omap2/omapfb/omapfb-sysfs.c
index 6f9c72cd6bb0..2f5e817b2a9a 100644
--- a/drivers/video/omap2/omapfb/omapfb-sysfs.c
+++ b/drivers/video/omap2/omapfb/omapfb-sysfs.c
@@ -29,7 +29,7 @@
 #include <linux/mm.h>
 #include <linux/omapfb.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 #include <plat/vrfb.h>
 
 #include "omapfb.h"
@@ -50,10 +50,12 @@ static ssize_t store_rotate_type(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_mem_region *rg;
-        enum omap_dss_rotation_type rot_type;
+        int rot_type;
         int r;
 
-        rot_type = simple_strtoul(buf, NULL, 0);
+        r = kstrtoint(buf, 0, &rot_type);
+        if (r)
+                return r;
 
         if (rot_type != OMAP_DSS_ROT_DMA && rot_type != OMAP_DSS_ROT_VRFB)
                 return -EINVAL;
@@ -102,14 +104,15 @@ static ssize_t store_mirror(struct device *dev,
 {
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        unsigned long mirror;
+        int mirror;
         int r;
         struct fb_var_screeninfo new_var;
 
-        mirror = simple_strtoul(buf, NULL, 0);
+        r = kstrtoint(buf, 0, &mirror);
+        if (r)
+                return r;
 
-        if (mirror != 0 && mirror != 1)
-                return -EINVAL;
+        mirror = !!mirror;
 
         if (!lock_fb_info(fbi))
                 return -ENODEV;
@@ -445,7 +448,11 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
         int r;
         int i;
 
-        size = PAGE_ALIGN(simple_strtoul(buf, NULL, 0));
+        r = kstrtoul(buf, 0, &size);
+        if (r)
+                return r;
+
+        size = PAGE_ALIGN(size);
 
         if (!lock_fb_info(fbi))
                 return -ENODEV;
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index 1305fc9880ba..aa1b1d974276 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -29,13 +29,15 @@
 
 #include <linux/rwsem.h>
 
-#include <plat/display.h>
+#include <video/omapdss.h>
 
 #ifdef DEBUG
 extern unsigned int omapfb_debug;
 #define DBG(format, ...) \
-        if (omapfb_debug) \
-                printk(KERN_DEBUG "OMAPFB: " format, ## __VA_ARGS__)
+        do { \
+                if (omapfb_debug) \
+                        printk(KERN_DEBUG "OMAPFB: " format, ## __VA_ARGS__); \
+        } while (0)
 #else
 #define DBG(format, ...)
 #endif
diff --git a/drivers/video/omap2/vram.c b/drivers/video/omap2/vram.c
index f6fdc2085f3e..9441e2eb3dee 100644
--- a/drivers/video/omap2/vram.c
+++ b/drivers/video/omap2/vram.c
@@ -551,16 +551,18 @@ void __init omap_vram_reserve_sdram_memblock(void)
         if (!size)
                 return;
 
-        size = PAGE_ALIGN(size);
+        size = ALIGN(size, SZ_2M);
 
         if (paddr) {
-                struct memblock_property res;
+                if (paddr & ~PAGE_MASK) {
+                        pr_err("VRAM start address 0x%08x not page aligned\n",
+                                        paddr);
+                        return;
+                }
 
-                res.base = paddr;
-                res.size = size;
-                if ((paddr & ~PAGE_MASK) || memblock_find(&res) ||
-                    res.base != paddr || res.size != size) {
-                        pr_err("Illegal SDRAM region for VRAM\n");
+                if (!memblock_is_region_memory(paddr, size)) {
+                        pr_err("Illegal SDRAM region 0x%08x..0x%08x for VRAM\n",
+                                        paddr, paddr + size - 1);
                         return;
                 }
 
@@ -574,9 +576,12 @@ void __init omap_vram_reserve_sdram_memblock(void)
                         return;
                 }
         } else {
-                paddr = memblock_alloc_base(size, PAGE_SIZE, MEMBLOCK_REAL_LIMIT);
+                paddr = memblock_alloc(size, SZ_2M);
         }
 
+        memblock_free(paddr, size);
+        memblock_remove(paddr, size);
+
         omap_vram_add_region(paddr, size);
 
         pr_info("Reserving %u bytes SDRAM for VRAM\n", size);
diff --git a/drivers/video/p9100.c b/drivers/video/p9100.c
index b6c3fc2db632..d57cc58c5168 100644
--- a/drivers/video/p9100.c
+++ b/drivers/video/p9100.c
@@ -249,7 +249,7 @@ static void p9100_init_fix(struct fb_info *info, int linebytes, struct device_no
         info->fix.accel = FB_ACCEL_SUN_CGTHREE;
 }
 
-static int __devinit p9100_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit p9100_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -352,7 +352,7 @@ static const struct of_device_id p9100_match[] = {
 };
 MODULE_DEVICE_TABLE(of, p9100_match);
 
-static struct of_platform_driver p9100_driver = {
+static struct platform_driver p9100_driver = {
         .driver = {
                 .name = "p9100",
                 .owner = THIS_MODULE,
@@ -367,12 +367,12 @@ static int __init p9100_init(void)
         if (fb_get_options("p9100fb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&p9100_driver);
+        return platform_driver_register(&p9100_driver);
 }
 
 static void __exit p9100_exit(void)
 {
-        of_unregister_platform_driver(&p9100_driver);
+        platform_driver_unregister(&p9100_driver);
 }
 
 module_init(p9100_init);
diff --git a/drivers/video/platinumfb.c b/drivers/video/platinumfb.c
index a50e1977b316..ef532d9d3c99 100644
--- a/drivers/video/platinumfb.c
+++ b/drivers/video/platinumfb.c
@@ -533,8 +533,7 @@ static int __init platinumfb_setup(char *options)
 #define invalidate_cache(addr)
 #endif
 
-static int __devinit platinumfb_probe(struct platform_device* odev,
-                                      const struct of_device_id *match)
+static int __devinit platinumfb_probe(struct platform_device* odev)
 {
         struct device_node      *dp = odev->dev.of_node;
         struct fb_info          *info;
@@ -677,7 +676,7 @@ static struct of_device_id platinumfb_match[] =
         {},
 };
 
-static struct of_platform_driver platinum_driver = 
+static struct platform_driver platinum_driver = 
 {
         .driver = {
                 .name = "platinumfb",
@@ -697,14 +696,14 @@ static int __init platinumfb_init(void)
                 return -ENODEV;
         platinumfb_setup(option);
 #endif
-        of_register_platform_driver(&platinum_driver);
+        platform_driver_register(&platinum_driver);
 
         return 0;
 }
 
 static void __exit platinumfb_exit(void)
 {
-        of_unregister_platform_driver(&platinum_driver);
+        platform_driver_unregister(&platinum_driver);
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/ps3fb.c b/drivers/video/ps3fb.c
index 9c0144ee7ae5..65560a1a0439 100644
--- a/drivers/video/ps3fb.c
+++ b/drivers/video/ps3fb.c
@@ -513,9 +513,9 @@ static int ps3fb_release(struct fb_info *info, int user)
         if (atomic_dec_and_test(&ps3fb.f_count)) {
                 if (atomic_read(&ps3fb.ext_flip)) {
                         atomic_set(&ps3fb.ext_flip, 0);
-                        if (!try_acquire_console_sem()) {
+                        if (console_trylock()) {
                                 ps3fb_sync(info, 0);    /* single buffer */
-                                release_console_sem();
+                                console_unlock();
                         }
                 }
         }
@@ -830,14 +830,14 @@ static int ps3fb_ioctl(struct fb_info *info, unsigned int cmd,
                         if (vmode) {
                                 var = info->var;
                                 fb_videomode_to_var(&var, vmode);
-                                acquire_console_sem();
+                                console_lock();
                                 info->flags |= FBINFO_MISC_USEREVENT;
                                 /* Force, in case only special bits changed */
                                 var.activate |= FB_ACTIVATE_FORCE;
                                 par->new_mode_id = val;
                                 retval = fb_set_var(info, &var);
                                 info->flags &= ~FBINFO_MISC_USEREVENT;
-                                release_console_sem();
+                                console_unlock();
                         }
                         break;
                 }
@@ -881,9 +881,9 @@ static int ps3fb_ioctl(struct fb_info *info, unsigned int cmd,
                         break;
 
                 dev_dbg(info->device, "PS3FB_IOCTL_FSEL:%d\n", val);
-                acquire_console_sem();
+                console_lock();
                 retval = ps3fb_sync(info, val);
-                release_console_sem();
+                console_unlock();
                 break;
 
         default:
@@ -903,9 +903,9 @@ static int ps3fbd(void *arg)
                 set_current_state(TASK_INTERRUPTIBLE);
                 if (ps3fb.is_kicked) {
                         ps3fb.is_kicked = 0;
-                        acquire_console_sem();
+                        console_lock();
                         ps3fb_sync(info, 0);    /* single buffer */
-                        release_console_sem();
+                        console_unlock();
                 }
                 schedule();
         }
diff --git a/drivers/video/pxa168fb.c b/drivers/video/pxa168fb.c
index a31a77ff6f3d..bb95ec56d25d 100644
--- a/drivers/video/pxa168fb.c
+++ b/drivers/video/pxa168fb.c
@@ -623,19 +623,21 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (res == NULL) {
                 dev_err(&pdev->dev, "no IO memory defined\n");
-                return -ENOENT;
+                ret = -ENOENT;
+                goto failed_put_clk;
         }
 
         irq = platform_get_irq(pdev, 0);
         if (irq < 0) {
                 dev_err(&pdev->dev, "no IRQ defined\n");
-                return -ENOENT;
+                ret = -ENOENT;
+                goto failed_put_clk;
         }
 
         info = framebuffer_alloc(sizeof(struct pxa168fb_info), &pdev->dev);
         if (info == NULL) {
-                clk_put(clk);
-                return -ENOMEM;
+                ret = -ENOMEM;
+                goto failed_put_clk;
         }
 
         /* Initialize private data */
@@ -671,7 +673,7 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
         fbi->reg_base = ioremap_nocache(res->start, resource_size(res));
         if (fbi->reg_base == NULL) {
                 ret = -ENOMEM;
-                goto failed;
+                goto failed_free_info;
         }
 
         /*
@@ -683,7 +685,7 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
                                                 &fbi->fb_start_dma, GFP_KERNEL);
         if (info->screen_base == NULL) {
                 ret = -ENOMEM;
-                goto failed;
+                goto failed_free_info;
         }
 
         info->fix.smem_start = (unsigned long)fbi->fb_start_dma;
@@ -701,16 +703,12 @@ static int __devinit pxa168fb_probe(struct platform_device *pdev)
          */
         pxa168fb_init_mode(info, mi);
 
-        ret = pxa168fb_check_var(&info->var, info);
-        if (ret)
-                goto failed_free_fbmem;
-
         /*
          * Fill in sane defaults.
          */
         ret = pxa168fb_check_var(&info->var, info);
         if (ret)
-                goto failed;
+                goto failed_free_fbmem;
 
         /*
          * enable controller clock
@@ -776,20 +774,62 @@ failed_free_clk:
 failed_free_fbmem:
         dma_free_coherent(fbi->dev, info->fix.smem_len,
                         info->screen_base, fbi->fb_start_dma);
-failed:
+failed_free_info:
         kfree(info);
+failed_put_clk:
         clk_put(clk);
 
         dev_err(&pdev->dev, "frame buffer device init failed with %d\n", ret);
         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 __init pxa168fb_init(void)
@@ -798,6 +838,12 @@ static int __init pxa168fb_init(void)
 }
 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/pxa3xx-gcu.c b/drivers/video/pxa3xx-gcu.c
new file mode 100644
index 000000000000..0283c7021090
--- /dev/null
+++ b/drivers/video/pxa3xx-gcu.c
@@ -0,0 +1,772 @@
+/*
+ *  pxa3xx-gcu.c - Linux kernel module for PXA3xx graphics controllers
+ *
+ *  This driver needs a DirectFB counterpart in user space, communication
+ *  is handled via mmap()ed memory areas and an ioctl.
+ *
+ *  Copyright (c) 2009 Daniel Mack <daniel@caiaq.de>
+ *  Copyright (c) 2009 Janine Kropp <nin@directfb.org>
+ *  Copyright (c) 2009 Denis Oliver Kropp <dok@directfb.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/*
+ * WARNING: This controller is attached to System Bus 2 of the PXA which
+ * needs its arbiter to be enabled explicitly (CKENB & 1<<9).
+ * There is currently no way to do this from Linux, so you need to teach
+ * your bootloader for now.
+ */
+
+#include <linux/module.h>
+#include <linux/version.h>
+
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/miscdevice.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/ioctl.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+
+#include "pxa3xx-gcu.h"
+
+#define DRV_NAME        "pxa3xx-gcu"
+#define MISCDEV_MINOR   197
+
+#define REG_GCCR        0x00
+#define GCCR_SYNC_CLR   (1 << 9)
+#define GCCR_BP_RST     (1 << 8)
+#define GCCR_ABORT      (1 << 6)
+#define GCCR_STOP       (1 << 4)
+
+#define REG_GCISCR      0x04
+#define REG_GCIECR      0x08
+#define REG_GCRBBR      0x20
+#define REG_GCRBLR      0x24
+#define REG_GCRBHR      0x28
+#define REG_GCRBTR      0x2C
+#define REG_GCRBEXHR    0x30
+
+#define IE_EOB          (1 << 0)
+#define IE_EEOB         (1 << 5)
+#define IE_ALL          0xff
+
+#define SHARED_SIZE     PAGE_ALIGN(sizeof(struct pxa3xx_gcu_shared))
+
+/* #define PXA3XX_GCU_DEBUG */
+/* #define PXA3XX_GCU_DEBUG_TIMER */
+
+#ifdef PXA3XX_GCU_DEBUG
+#define QDUMP(msg)                                      \
+        do {                                            \
+                QPRINT(priv, KERN_DEBUG, msg);          \
+        } while (0)
+#else
+#define QDUMP(msg)      do {} while (0)
+#endif
+
+#define QERROR(msg)                                     \
+        do {                                            \
+                QPRINT(priv, KERN_ERR, msg);            \
+        } while (0)
+
+struct pxa3xx_gcu_batch {
+        struct pxa3xx_gcu_batch *next;
+        u32                     *ptr;
+        dma_addr_t               phys;
+        unsigned long            length;
+};
+
+struct pxa3xx_gcu_priv {
+        void __iomem             *mmio_base;
+        struct clk               *clk;
+        struct pxa3xx_gcu_shared *shared;
+        dma_addr_t                shared_phys;
+        struct resource          *resource_mem;
+        struct miscdevice         misc_dev;
+        struct file_operations    misc_fops;
+        wait_queue_head_t         wait_idle;
+        wait_queue_head_t         wait_free;
+        spinlock_t                spinlock;
+        struct timeval            base_time;
+
+        struct pxa3xx_gcu_batch *free;
+
+        struct pxa3xx_gcu_batch *ready;
+        struct pxa3xx_gcu_batch *ready_last;
+        struct pxa3xx_gcu_batch *running;
+};
+
+static inline unsigned long
+gc_readl(struct pxa3xx_gcu_priv *priv, unsigned int off)
+{
+        return __raw_readl(priv->mmio_base + off);
+}
+
+static inline void
+gc_writel(struct pxa3xx_gcu_priv *priv, unsigned int off, unsigned long val)
+{
+        __raw_writel(val, priv->mmio_base + off);
+}
+
+#define QPRINT(priv, level, msg)                                        \
+        do {                                                            \
+                struct timeval tv;                                      \
+                struct pxa3xx_gcu_shared *shared = priv->shared;        \
+                u32 base = gc_readl(priv, REG_GCRBBR);                  \
+                                                                        \
+                do_gettimeofday(&tv);                                   \
+                                                                        \
+                printk(level "%ld.%03ld.%03ld - %-17s: %-21s (%s, "     \
+                        "STATUS "                                       \
+                        "0x%02lx, B 0x%08lx [%ld], E %5ld, H %5ld, "    \
+                        "T %5ld)\n",                                    \
+                        tv.tv_sec - priv->base_time.tv_sec,             \
+                        tv.tv_usec / 1000, tv.tv_usec % 1000,           \
+                        __func__, msg,                                  \
+                        shared->hw_running ? "running" : "   idle",     \
+                        gc_readl(priv, REG_GCISCR),                     \
+                        gc_readl(priv, REG_GCRBBR),                     \
+                        gc_readl(priv, REG_GCRBLR),                     \
+                        (gc_readl(priv, REG_GCRBEXHR) - base) / 4,      \
+                        (gc_readl(priv, REG_GCRBHR) - base) / 4,        \
+                        (gc_readl(priv, REG_GCRBTR) - base) / 4);       \
+        } while (0)
+
+static void
+pxa3xx_gcu_reset(struct pxa3xx_gcu_priv *priv)
+{
+        QDUMP("RESET");
+
+        /* disable interrupts */
+        gc_writel(priv, REG_GCIECR, 0);
+
+        /* reset hardware */
+        gc_writel(priv, REG_GCCR, GCCR_ABORT);
+        gc_writel(priv, REG_GCCR, 0);
+
+        memset(priv->shared, 0, SHARED_SIZE);
+        priv->shared->buffer_phys = priv->shared_phys;
+        priv->shared->magic = PXA3XX_GCU_SHARED_MAGIC;
+
+        do_gettimeofday(&priv->base_time);
+
+        /* set up the ring buffer pointers */
+        gc_writel(priv, REG_GCRBLR, 0);
+        gc_writel(priv, REG_GCRBBR, priv->shared_phys);
+        gc_writel(priv, REG_GCRBTR, priv->shared_phys);
+
+        /* enable all IRQs except EOB */
+        gc_writel(priv, REG_GCIECR, IE_ALL & ~IE_EOB);
+}
+
+static void
+dump_whole_state(struct pxa3xx_gcu_priv *priv)
+{
+        struct pxa3xx_gcu_shared *sh = priv->shared;
+        u32 base = gc_readl(priv, REG_GCRBBR);
+
+        QDUMP("DUMP");
+
+        printk(KERN_DEBUG "== PXA3XX-GCU DUMP ==\n"
+                "%s, STATUS 0x%02lx, B 0x%08lx [%ld], E %5ld, H %5ld, T %5ld\n",
+                sh->hw_running ? "running" : "idle   ",
+                gc_readl(priv, REG_GCISCR),
+                gc_readl(priv, REG_GCRBBR),
+                gc_readl(priv, REG_GCRBLR),
+                (gc_readl(priv, REG_GCRBEXHR) - base) / 4,
+                (gc_readl(priv, REG_GCRBHR) - base) / 4,
+                (gc_readl(priv, REG_GCRBTR) - base) / 4);
+}
+
+static void
+flush_running(struct pxa3xx_gcu_priv *priv)
+{
+        struct pxa3xx_gcu_batch *running = priv->running;
+        struct pxa3xx_gcu_batch *next;
+
+        while (running) {
+                next = running->next;
+                running->next = priv->free;
+                priv->free = running;
+                running = next;
+        }
+
+        priv->running = NULL;
+}
+
+static void
+run_ready(struct pxa3xx_gcu_priv *priv)
+{
+        unsigned int num = 0;
+        struct pxa3xx_gcu_shared *shared = priv->shared;
+        struct pxa3xx_gcu_batch *ready = priv->ready;
+
+        QDUMP("Start");
+
+        BUG_ON(!ready);
+
+        shared->buffer[num++] = 0x05000000;
+
+        while (ready) {
+                shared->buffer[num++] = 0x00000001;
+                shared->buffer[num++] = ready->phys;
+                ready = ready->next;
+        }
+
+        shared->buffer[num++] = 0x05000000;
+        priv->running = priv->ready;
+        priv->ready = priv->ready_last = NULL;
+        gc_writel(priv, REG_GCRBLR, 0);
+        shared->hw_running = 1;
+
+        /* ring base address */
+        gc_writel(priv, REG_GCRBBR, shared->buffer_phys);
+
+        /* ring tail address */
+        gc_writel(priv, REG_GCRBTR, shared->buffer_phys + num * 4);
+
+        /* ring length */
+        gc_writel(priv, REG_GCRBLR, ((num + 63) & ~63) * 4);
+}
+
+static irqreturn_t
+pxa3xx_gcu_handle_irq(int irq, void *ctx)
+{
+        struct pxa3xx_gcu_priv *priv = ctx;
+        struct pxa3xx_gcu_shared *shared = priv->shared;
+        u32 status = gc_readl(priv, REG_GCISCR) & IE_ALL;
+
+        QDUMP("-Interrupt");
+
+        if (!status)
+                return IRQ_NONE;
+
+        spin_lock(&priv->spinlock);
+        shared->num_interrupts++;
+
+        if (status & IE_EEOB) {
+                QDUMP(" [EEOB]");
+
+                flush_running(priv);
+                wake_up_all(&priv->wait_free);
+
+                if (priv->ready) {
+                        run_ready(priv);
+                } else {
+                        /* There is no more data prepared by the userspace.
+                         * Set hw_running = 0 and wait for the next userspace
+                         * kick-off */
+                        shared->num_idle++;
+                        shared->hw_running = 0;
+
+                        QDUMP(" '-> Idle.");
+
+                        /* set ring buffer length to zero */
+                        gc_writel(priv, REG_GCRBLR, 0);
+
+                        wake_up_all(&priv->wait_idle);
+                }
+
+                shared->num_done++;
+        } else {
+                QERROR(" [???]");
+                dump_whole_state(priv);
+        }
+
+        /* Clear the interrupt */
+        gc_writel(priv, REG_GCISCR, status);
+        spin_unlock(&priv->spinlock);
+
+        return IRQ_HANDLED;
+}
+
+static int
+pxa3xx_gcu_wait_idle(struct pxa3xx_gcu_priv *priv)
+{
+        int ret = 0;
+
+        QDUMP("Waiting for idle...");
+
+        /* Does not need to be atomic. There's a lock in user space,
+         * but anyhow, this is just for statistics. */
+        priv->shared->num_wait_idle++;
+
+        while (priv->shared->hw_running) {
+                int num = priv->shared->num_interrupts;
+                u32 rbexhr = gc_readl(priv, REG_GCRBEXHR);
+
+                ret = wait_event_interruptible_timeout(priv->wait_idle,
+                                        !priv->shared->hw_running, HZ*4);
+
+                if (ret < 0)
+                        break;
+
+                if (ret > 0)
+                        continue;
+
+                if (gc_readl(priv, REG_GCRBEXHR) == rbexhr &&
+                    priv->shared->num_interrupts == num) {
+                        QERROR("TIMEOUT");
+                        ret = -ETIMEDOUT;
+                        break;
+                }
+        }
+
+        QDUMP("done");
+
+        return ret;
+}
+
+static int
+pxa3xx_gcu_wait_free(struct pxa3xx_gcu_priv *priv)
+{
+        int ret = 0;
+
+        QDUMP("Waiting for free...");
+
+        /* Does not need to be atomic. There's a lock in user space,
+         * but anyhow, this is just for statistics. */
+        priv->shared->num_wait_free++;
+
+        while (!priv->free) {
+                u32 rbexhr = gc_readl(priv, REG_GCRBEXHR);
+
+                ret = wait_event_interruptible_timeout(priv->wait_free,
+                                                       priv->free, HZ*4);
+
+                if (ret < 0)
+                        break;
+
+                if (ret > 0)
+                        continue;
+
+                if (gc_readl(priv, REG_GCRBEXHR) == rbexhr) {
+                        QERROR("TIMEOUT");
+                        ret = -ETIMEDOUT;
+                        break;
+                }
+        }
+
+        QDUMP("done");
+
+        return ret;
+}
+
+/* Misc device layer */
+
+static ssize_t
+pxa3xx_gcu_misc_write(struct file *filp, const char *buff,
+                      size_t count, loff_t *offp)
+{
+        int ret;
+        unsigned long flags;
+        struct pxa3xx_gcu_batch *buffer;
+        struct pxa3xx_gcu_priv *priv =
+                container_of(filp->f_op, struct pxa3xx_gcu_priv, misc_fops);
+
+        int words = count / 4;
+
+        /* Does not need to be atomic. There's a lock in user space,
+         * but anyhow, this is just for statistics. */
+        priv->shared->num_writes++;
+
+        priv->shared->num_words += words;
+
+        /* Last word reserved for batch buffer end command */
+        if (words >= PXA3XX_GCU_BATCH_WORDS)
+                return -E2BIG;
+
+        /* Wait for a free buffer */
+        if (!priv->free) {
+                ret = pxa3xx_gcu_wait_free(priv);
+                if (ret < 0)
+                        return ret;
+        }
+
+        /*
+         * Get buffer from free list
+         */
+        spin_lock_irqsave(&priv->spinlock, flags);
+
+        buffer = priv->free;
+        priv->free = buffer->next;
+
+        spin_unlock_irqrestore(&priv->spinlock, flags);
+
+
+        /* Copy data from user into buffer */
+        ret = copy_from_user(buffer->ptr, buff, words * 4);
+        if (ret) {
+                spin_lock_irqsave(&priv->spinlock, flags);
+                buffer->next = priv->free;
+                priv->free = buffer;
+                spin_unlock_irqrestore(&priv->spinlock, flags);
+                return -EFAULT;
+        }
+
+        buffer->length = words;
+
+        /* Append batch buffer end command */
+        buffer->ptr[words] = 0x01000000;
+
+        /*
+         * Add buffer to ready list
+         */
+        spin_lock_irqsave(&priv->spinlock, flags);
+
+        buffer->next = NULL;
+
+        if (priv->ready) {
+                BUG_ON(priv->ready_last == NULL);
+
+                priv->ready_last->next = buffer;
+        } else
+                priv->ready = buffer;
+
+        priv->ready_last = buffer;
+
+        if (!priv->shared->hw_running)
+                run_ready(priv);
+
+        spin_unlock_irqrestore(&priv->spinlock, flags);
+
+        return words * 4;
+}
+
+
+static long
+pxa3xx_gcu_misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+        unsigned long flags;
+        struct pxa3xx_gcu_priv *priv =
+                container_of(filp->f_op, struct pxa3xx_gcu_priv, misc_fops);
+
+        switch (cmd) {
+        case PXA3XX_GCU_IOCTL_RESET:
+                spin_lock_irqsave(&priv->spinlock, flags);
+                pxa3xx_gcu_reset(priv);
+                spin_unlock_irqrestore(&priv->spinlock, flags);
+                return 0;
+
+        case PXA3XX_GCU_IOCTL_WAIT_IDLE:
+                return pxa3xx_gcu_wait_idle(priv);
+        }
+
+        return -ENOSYS;
+}
+
+static int
+pxa3xx_gcu_misc_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+        unsigned int size = vma->vm_end - vma->vm_start;
+        struct pxa3xx_gcu_priv *priv =
+                container_of(filp->f_op, struct pxa3xx_gcu_priv, misc_fops);
+
+        switch (vma->vm_pgoff) {
+        case 0:
+                /* hand out the shared data area */
+                if (size != SHARED_SIZE)
+                        return -EINVAL;
+
+                return dma_mmap_coherent(NULL, vma,
+                        priv->shared, priv->shared_phys, size);
+
+        case SHARED_SIZE >> PAGE_SHIFT:
+                /* hand out the MMIO base for direct register access
+                 * from userspace */
+                if (size != resource_size(priv->resource_mem))
+                        return -EINVAL;
+
+                vma->vm_flags |= VM_IO;
+                vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+                return io_remap_pfn_range(vma, vma->vm_start,
+                                priv->resource_mem->start >> PAGE_SHIFT,
+                                size, vma->vm_page_prot);
+        }
+
+        return -EINVAL;
+}
+
+
+#ifdef PXA3XX_GCU_DEBUG_TIMER
+static struct timer_list pxa3xx_gcu_debug_timer;
+
+static void pxa3xx_gcu_debug_timedout(unsigned long ptr)
+{
+        struct pxa3xx_gcu_priv *priv = (struct pxa3xx_gcu_priv *) ptr;
+
+        QERROR("Timer DUMP");
+
+        /* init the timer structure */
+        init_timer(&pxa3xx_gcu_debug_timer);
+        pxa3xx_gcu_debug_timer.function = pxa3xx_gcu_debug_timedout;
+        pxa3xx_gcu_debug_timer.data = ptr;
+        pxa3xx_gcu_debug_timer.expires = jiffies + 5*HZ; /* one second */
+
+        add_timer(&pxa3xx_gcu_debug_timer);
+}
+
+static void pxa3xx_gcu_init_debug_timer(void)
+{
+        pxa3xx_gcu_debug_timedout((unsigned long) &pxa3xx_gcu_debug_timer);
+}
+#else
+static inline void pxa3xx_gcu_init_debug_timer(void) {}
+#endif
+
+static int
+add_buffer(struct platform_device *dev,
+           struct pxa3xx_gcu_priv *priv)
+{
+        struct pxa3xx_gcu_batch *buffer;
+
+        buffer = kzalloc(sizeof(struct pxa3xx_gcu_batch), GFP_KERNEL);
+        if (!buffer)
+                return -ENOMEM;
+
+        buffer->ptr = dma_alloc_coherent(&dev->dev, PXA3XX_GCU_BATCH_WORDS * 4,
+                                         &buffer->phys, GFP_KERNEL);
+        if (!buffer->ptr) {
+                kfree(buffer);
+                return -ENOMEM;
+        }
+
+        buffer->next = priv->free;
+
+        priv->free = buffer;
+
+        return 0;
+}
+
+static void
+free_buffers(struct platform_device *dev,
+             struct pxa3xx_gcu_priv *priv)
+{
+        struct pxa3xx_gcu_batch *next, *buffer = priv->free;
+
+        while (buffer) {
+                next = buffer->next;
+
+                dma_free_coherent(&dev->dev, PXA3XX_GCU_BATCH_WORDS * 4,
+                                  buffer->ptr, buffer->phys);
+
+                kfree(buffer);
+
+                buffer = next;
+        }
+
+        priv->free = NULL;
+}
+
+static int __devinit
+pxa3xx_gcu_probe(struct platform_device *dev)
+{
+        int i, ret, irq;
+        struct resource *r;
+        struct pxa3xx_gcu_priv *priv;
+
+        priv = kzalloc(sizeof(struct pxa3xx_gcu_priv), GFP_KERNEL);
+        if (!priv)
+                return -ENOMEM;
+
+        for (i = 0; i < 8; i++) {
+                ret = add_buffer(dev, priv);
+                if (ret) {
+                        dev_err(&dev->dev, "failed to allocate DMA memory\n");
+                        goto err_free_priv;
+                }
+        }
+
+        init_waitqueue_head(&priv->wait_idle);
+        init_waitqueue_head(&priv->wait_free);
+        spin_lock_init(&priv->spinlock);
+
+        /* we allocate the misc device structure as part of our own allocation,
+         * so we can get a pointer to our priv structure later on with
+         * container_of(). This isn't really necessary as we have a fixed minor
+         * number anyway, but this is to avoid statics. */
+
+        priv->misc_fops.owner   = THIS_MODULE;
+        priv->misc_fops.write   = pxa3xx_gcu_misc_write;
+        priv->misc_fops.unlocked_ioctl = pxa3xx_gcu_misc_ioctl;
+        priv->misc_fops.mmap    = pxa3xx_gcu_misc_mmap;
+
+        priv->misc_dev.minor    = MISCDEV_MINOR,
+        priv->misc_dev.name     = DRV_NAME,
+        priv->misc_dev.fops     = &priv->misc_fops,
+
+        /* register misc device */
+        ret = misc_register(&priv->misc_dev);
+        if (ret < 0) {
+                dev_err(&dev->dev, "misc_register() for minor %d failed\n",
+                        MISCDEV_MINOR);
+                goto err_free_priv;
+        }
+
+        /* handle IO resources */
+        r = platform_get_resource(dev, IORESOURCE_MEM, 0);
+        if (r == NULL) {
+                dev_err(&dev->dev, "no I/O memory resource defined\n");
+                ret = -ENODEV;
+                goto err_misc_deregister;
+        }
+
+        if (!request_mem_region(r->start, resource_size(r), dev->name)) {
+                dev_err(&dev->dev, "failed to request I/O memory\n");
+                ret = -EBUSY;
+                goto err_misc_deregister;
+        }
+
+        priv->mmio_base = ioremap_nocache(r->start, resource_size(r));
+        if (!priv->mmio_base) {
+                dev_err(&dev->dev, "failed to map I/O memory\n");
+                ret = -EBUSY;
+                goto err_free_mem_region;
+        }
+
+        /* allocate dma memory */
+        priv->shared = dma_alloc_coherent(&dev->dev, SHARED_SIZE,
+                                          &priv->shared_phys, GFP_KERNEL);
+
+        if (!priv->shared) {
+                dev_err(&dev->dev, "failed to allocate DMA memory\n");
+                ret = -ENOMEM;
+                goto err_free_io;
+        }
+
+        /* enable the clock */
+        priv->clk = clk_get(&dev->dev, NULL);
+        if (IS_ERR(priv->clk)) {
+                dev_err(&dev->dev, "failed to get clock\n");
+                ret = -ENODEV;
+                goto err_free_dma;
+        }
+
+        ret = clk_enable(priv->clk);
+        if (ret < 0) {
+                dev_err(&dev->dev, "failed to enable clock\n");
+                goto err_put_clk;
+        }
+
+        /* request the IRQ */
+        irq = platform_get_irq(dev, 0);
+        if (irq < 0) {
+                dev_err(&dev->dev, "no IRQ defined\n");
+                ret = -ENODEV;
+                goto err_put_clk;
+        }
+
+        ret = request_irq(irq, pxa3xx_gcu_handle_irq,
+                          IRQF_DISABLED, DRV_NAME, priv);
+        if (ret) {
+                dev_err(&dev->dev, "request_irq failed\n");
+                ret = -EBUSY;
+                goto err_put_clk;
+        }
+
+        platform_set_drvdata(dev, priv);
+        priv->resource_mem = r;
+        pxa3xx_gcu_reset(priv);
+        pxa3xx_gcu_init_debug_timer();
+
+        dev_info(&dev->dev, "registered @0x%p, DMA 0x%p (%d bytes), IRQ %d\n",
+                        (void *) r->start, (void *) priv->shared_phys,
+                        SHARED_SIZE, irq);
+        return 0;
+
+err_put_clk:
+        clk_disable(priv->clk);
+        clk_put(priv->clk);
+
+err_free_dma:
+        dma_free_coherent(&dev->dev, SHARED_SIZE,
+                        priv->shared, priv->shared_phys);
+
+err_free_io:
+        iounmap(priv->mmio_base);
+
+err_free_mem_region:
+        release_mem_region(r->start, resource_size(r));
+
+err_misc_deregister:
+        misc_deregister(&priv->misc_dev);
+
+err_free_priv:
+        platform_set_drvdata(dev, NULL);
+        free_buffers(dev, priv);
+        kfree(priv);
+        return ret;
+}
+
+static int __devexit
+pxa3xx_gcu_remove(struct platform_device *dev)
+{
+        struct pxa3xx_gcu_priv *priv = platform_get_drvdata(dev);
+        struct resource *r = priv->resource_mem;
+
+        pxa3xx_gcu_wait_idle(priv);
+
+        misc_deregister(&priv->misc_dev);
+        dma_free_coherent(&dev->dev, SHARED_SIZE,
+                        priv->shared, priv->shared_phys);
+        iounmap(priv->mmio_base);
+        release_mem_region(r->start, resource_size(r));
+        platform_set_drvdata(dev, NULL);
+        clk_disable(priv->clk);
+        free_buffers(dev, priv);
+        kfree(priv);
+
+        return 0;
+}
+
+static struct platform_driver pxa3xx_gcu_driver = {
+        .probe    = pxa3xx_gcu_probe,
+        .remove  = __devexit_p(pxa3xx_gcu_remove),
+        .driver  = {
+                .owner  = THIS_MODULE,
+                .name   = DRV_NAME,
+        },
+};
+
+static int __init
+pxa3xx_gcu_init(void)
+{
+        return platform_driver_register(&pxa3xx_gcu_driver);
+}
+
+static void __exit
+pxa3xx_gcu_exit(void)
+{
+        platform_driver_unregister(&pxa3xx_gcu_driver);
+}
+
+module_init(pxa3xx_gcu_init);
+module_exit(pxa3xx_gcu_exit);
+
+MODULE_DESCRIPTION("PXA3xx graphics controller unit driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(MISCDEV_MINOR);
+MODULE_AUTHOR("Janine Kropp <nin@directfb.org>, "
+                "Denis Oliver Kropp <dok@directfb.org>, "
+                "Daniel Mack <daniel@caiaq.de>");
diff --git a/drivers/video/pxa3xx-gcu.h b/drivers/video/pxa3xx-gcu.h
new file mode 100644
index 000000000000..0428ed03dc49
--- /dev/null
+++ b/drivers/video/pxa3xx-gcu.h
@@ -0,0 +1,38 @@
+#ifndef __PXA3XX_GCU_H__
+#define __PXA3XX_GCU_H__
+
+#include <linux/types.h>
+
+/* Number of 32bit words in display list (ring buffer). */
+#define PXA3XX_GCU_BUFFER_WORDS  ((256 * 1024 - 256) / 4)
+
+/* To be increased when breaking the ABI */
+#define PXA3XX_GCU_SHARED_MAGIC  0x30000001
+
+#define PXA3XX_GCU_BATCH_WORDS   8192
+
+struct pxa3xx_gcu_shared {
+        u32            buffer[PXA3XX_GCU_BUFFER_WORDS];
+
+        bool           hw_running;
+
+        unsigned long  buffer_phys;
+
+        unsigned int   num_words;
+        unsigned int   num_writes;
+        unsigned int   num_done;
+        unsigned int   num_interrupts;
+        unsigned int   num_wait_idle;
+        unsigned int   num_wait_free;
+        unsigned int   num_idle;
+
+        u32            magic;
+};
+
+/* Initialization and synchronization.
+ * Hardware is started upon write(). */
+#define PXA3XX_GCU_IOCTL_RESET          _IO('G', 0)
+#define PXA3XX_GCU_IOCTL_WAIT_IDLE      _IO('G', 2)
+
+#endif /* __PXA3XX_GCU_H__ */
+
diff --git a/drivers/video/pxafb.c b/drivers/video/pxafb.c
index 825b665245bb..0f4e8c942f9e 100644
--- a/drivers/video/pxafb.c
+++ b/drivers/video/pxafb.c
@@ -627,7 +627,12 @@ static void overlay1fb_enable(struct pxafb_layer *ofb)
 
 static void overlay1fb_disable(struct pxafb_layer *ofb)
 {
-        uint32_t lccr5 = lcd_readl(ofb->fbi, LCCR5);
+        uint32_t lccr5;
+
+        if (!(lcd_readl(ofb->fbi, OVL1C1) & OVLxC1_OEN))
+                return;
+
+        lccr5 = lcd_readl(ofb->fbi, LCCR5);
 
         lcd_writel(ofb->fbi, OVL1C1, ofb->control[0] & ~OVLxC1_OEN);
 
@@ -685,7 +690,12 @@ static void overlay2fb_enable(struct pxafb_layer *ofb)
 
 static void overlay2fb_disable(struct pxafb_layer *ofb)
 {
-        uint32_t lccr5 = lcd_readl(ofb->fbi, LCCR5);
+        uint32_t lccr5;
+
+        if (!(lcd_readl(ofb->fbi, OVL2C1) & OVLxC1_OEN))
+                return;
+
+        lccr5 = lcd_readl(ofb->fbi, LCCR5);
 
         lcd_writel(ofb->fbi, OVL2C1, ofb->control[0] & ~OVLxC1_OEN);
 
@@ -720,12 +730,10 @@ static int overlayfb_open(struct fb_info *info, int user)
         if (user == 0)
                 return -ENODEV;
 
-        /* allow only one user at a time */
-        if (atomic_inc_and_test(&ofb->usage))
-                return -EBUSY;
+        if (ofb->usage++ == 0)
+                /* unblank the base framebuffer */
+                fb_blank(&ofb->fbi->fb, FB_BLANK_UNBLANK);
 
-        /* unblank the base framebuffer */
-        fb_blank(&ofb->fbi->fb, FB_BLANK_UNBLANK);
         return 0;
 }
 
@@ -733,12 +741,15 @@ static int overlayfb_release(struct fb_info *info, int user)
 {
         struct pxafb_layer *ofb = (struct pxafb_layer*) info;
 
-        atomic_dec(&ofb->usage);
-        ofb->ops->disable(ofb);
+        if (ofb->usage == 1) {
+                ofb->ops->disable(ofb);
+                ofb->fb.var.height      = -1;
+                ofb->fb.var.width       = -1;
+                ofb->fb.var.xres = ofb->fb.var.xres_virtual = 0;
+                ofb->fb.var.yres = ofb->fb.var.yres_virtual = 0;
 
-        free_pages_exact(ofb->video_mem, ofb->video_mem_size);
-        ofb->video_mem = NULL;
-        ofb->video_mem_size = 0;
+                ofb->usage--;
+        }
         return 0;
 }
 
@@ -750,7 +761,7 @@ static int overlayfb_check_var(struct fb_var_screeninfo *var,
         int xpos, ypos, pfor, bpp;
 
         xpos = NONSTD_TO_XPOS(var->nonstd);
-        ypos = NONSTD_TO_XPOS(var->nonstd);
+        ypos = NONSTD_TO_YPOS(var->nonstd);
         pfor = NONSTD_TO_PFOR(var->nonstd);
 
         bpp = pxafb_var_to_bpp(var);
@@ -794,7 +805,7 @@ static int overlayfb_check_var(struct fb_var_screeninfo *var,
         return 0;
 }
 
-static int overlayfb_map_video_memory(struct pxafb_layer *ofb)
+static int overlayfb_check_video_memory(struct pxafb_layer *ofb)
 {
         struct fb_var_screeninfo *var = &ofb->fb.var;
         int pfor = NONSTD_TO_PFOR(var->nonstd);
@@ -812,27 +823,11 @@ static int overlayfb_map_video_memory(struct pxafb_layer *ofb)
 
         size = PAGE_ALIGN(ofb->fb.fix.line_length * var->yres_virtual);
 
-        /* don't re-allocate if the original video memory is enough */
         if (ofb->video_mem) {
                 if (ofb->video_mem_size >= size)
                         return 0;
-
-                free_pages_exact(ofb->video_mem, ofb->video_mem_size);
         }
-
-        ofb->video_mem = alloc_pages_exact(size, GFP_KERNEL | __GFP_ZERO);
-        if (ofb->video_mem == NULL)
-                return -ENOMEM;
-
-        ofb->video_mem_phys = virt_to_phys(ofb->video_mem);
-        ofb->video_mem_size = size;
-
-        mutex_lock(&ofb->fb.mm_lock);
-        ofb->fb.fix.smem_start  = ofb->video_mem_phys;
-        ofb->fb.fix.smem_len    = ofb->fb.fix.line_length * var->yres_virtual;
-        mutex_unlock(&ofb->fb.mm_lock);
-        ofb->fb.screen_base     = ofb->video_mem;
-        return 0;
+        return -EINVAL;
 }
 
 static int overlayfb_set_par(struct fb_info *info)
@@ -841,13 +836,13 @@ static int overlayfb_set_par(struct fb_info *info)
         struct fb_var_screeninfo *var = &info->var;
         int xpos, ypos, pfor, bpp, ret;
 
-        ret = overlayfb_map_video_memory(ofb);
+        ret = overlayfb_check_video_memory(ofb);
         if (ret)
                 return ret;
 
         bpp  = pxafb_var_to_bpp(var);
         xpos = NONSTD_TO_XPOS(var->nonstd);
-        ypos = NONSTD_TO_XPOS(var->nonstd);
+        ypos = NONSTD_TO_YPOS(var->nonstd);
         pfor = NONSTD_TO_PFOR(var->nonstd);
 
         ofb->control[0] = OVLxC1_PPL(var->xres) | OVLxC1_LPO(var->yres) |
@@ -891,7 +886,7 @@ static void __devinit init_pxafb_overlay(struct pxafb_info *fbi,
 
         ofb->id = id;
         ofb->ops = &ofb_ops[id];
-        atomic_set(&ofb->usage, 0);
+        ofb->usage = 0;
         ofb->fbi = fbi;
         init_completion(&ofb->branch_done);
 }
@@ -904,29 +899,60 @@ static inline int pxafb_overlay_supported(void)
         return 0;
 }
 
-static int __devinit pxafb_overlay_init(struct pxafb_info *fbi)
+static int __devinit pxafb_overlay_map_video_memory(struct pxafb_info *pxafb,
+        struct pxafb_layer *ofb)
+{
+        /* We assume that user will use at most video_mem_size for overlay fb,
+         * anyway, it's useless to use 16bpp main plane and 24bpp overlay
+         */
+        ofb->video_mem = alloc_pages_exact(PAGE_ALIGN(pxafb->video_mem_size),
+                GFP_KERNEL | __GFP_ZERO);
+        if (ofb->video_mem == NULL)
+                return -ENOMEM;
+
+        ofb->video_mem_phys = virt_to_phys(ofb->video_mem);
+        ofb->video_mem_size = PAGE_ALIGN(pxafb->video_mem_size);
+
+        mutex_lock(&ofb->fb.mm_lock);
+        ofb->fb.fix.smem_start  = ofb->video_mem_phys;
+        ofb->fb.fix.smem_len    = pxafb->video_mem_size;
+        mutex_unlock(&ofb->fb.mm_lock);
+
+        ofb->fb.screen_base     = ofb->video_mem;
+
+        return 0;
+}
+
+static void __devinit pxafb_overlay_init(struct pxafb_info *fbi)
 {
         int i, ret;
 
         if (!pxafb_overlay_supported())
-                return 0;
+                return;
 
         for (i = 0; i < 2; i++) {
-                init_pxafb_overlay(fbi, &fbi->overlay[i], i);
-                ret = register_framebuffer(&fbi->overlay[i].fb);
+                struct pxafb_layer *ofb = &fbi->overlay[i];
+                init_pxafb_overlay(fbi, ofb, i);
+                ret = register_framebuffer(&ofb->fb);
                 if (ret) {
                         dev_err(fbi->dev, "failed to register overlay %d\n", i);
-                        return ret;
+                        continue;
                 }
+                ret = pxafb_overlay_map_video_memory(fbi, ofb);
+                if (ret) {
+                        dev_err(fbi->dev,
+                                "failed to map video memory for overlay %d\n",
+                                i);
+                        unregister_framebuffer(&ofb->fb);
+                        continue;
+                }
+                ofb->registered = 1;
         }
 
         /* mask all IU/BS/EOF/SOF interrupts */
         lcd_writel(fbi, LCCR5, ~0);
 
-        /* place overlay(s) on top of base */
-        fbi->lccr0 |= LCCR0_OUC;
         pr_info("PXA Overlay driver loaded successfully!\n");
-        return 0;
 }
 
 static void __devexit pxafb_overlay_exit(struct pxafb_info *fbi)
@@ -936,8 +962,15 @@ static void __devexit pxafb_overlay_exit(struct pxafb_info *fbi)
         if (!pxafb_overlay_supported())
                 return;
 
-        for (i = 0; i < 2; i++)
-                unregister_framebuffer(&fbi->overlay[i].fb);
+        for (i = 0; i < 2; i++) {
+                struct pxafb_layer *ofb = &fbi->overlay[i];
+                if (ofb->registered) {
+                        if (ofb->video_mem)
+                                free_pages_exact(ofb->video_mem,
+                                        ofb->video_mem_size);
+                        unregister_framebuffer(&ofb->fb);
+                }
+        }
 }
 #else
 static inline void pxafb_overlay_init(struct pxafb_info *fbi) {}
@@ -1368,7 +1401,8 @@ static int pxafb_activate_var(struct fb_var_screeninfo *var,
             (lcd_readl(fbi, LCCR3) != fbi->reg_lccr3) ||
             (lcd_readl(fbi, LCCR4) != fbi->reg_lccr4) ||
             (lcd_readl(fbi, FDADR0) != fbi->fdadr[0]) ||
-            (lcd_readl(fbi, FDADR1) != fbi->fdadr[1]))
+            ((fbi->lccr0 & LCCR0_SDS) &&
+            (lcd_readl(fbi, FDADR1) != fbi->fdadr[1])))
                 pxafb_schedule_work(fbi, C_REENABLE);
 
         return 0;
@@ -1420,7 +1454,8 @@ static void pxafb_enable_controller(struct pxafb_info *fbi)
         lcd_writel(fbi, LCCR0, fbi->reg_lccr0 & ~LCCR0_ENB);
 
         lcd_writel(fbi, FDADR0, fbi->fdadr[0]);
-        lcd_writel(fbi, FDADR1, fbi->fdadr[1]);
+        if (fbi->lccr0 & LCCR0_SDS)
+                lcd_writel(fbi, FDADR1, fbi->fdadr[1]);
         lcd_writel(fbi, LCCR0, fbi->reg_lccr0 | LCCR0_ENB);
 }
 
@@ -1613,7 +1648,10 @@ pxafb_freq_transition(struct notifier_block *nb, unsigned long val, void *data)
 
         switch (val) {
         case CPUFREQ_PRECHANGE:
-                set_ctrlr_state(fbi, C_DISABLE_CLKCHANGE);
+#ifdef CONFIG_FB_PXA_OVERLAY
+                if (!(fbi->overlay[0].usage || fbi->overlay[1].usage))
+#endif
+                        set_ctrlr_state(fbi, C_DISABLE_CLKCHANGE);
                 break;
 
         case CPUFREQ_POSTCHANGE:
@@ -1806,6 +1844,12 @@ static struct pxafb_info * __devinit pxafb_init_fbinfo(struct device *dev)
 
         pxafb_decode_mach_info(fbi, inf);
 
+#ifdef CONFIG_FB_PXA_OVERLAY
+        /* place overlay(s) on top of base */
+        if (pxafb_overlay_supported())
+                fbi->lccr0 |= LCCR0_OUC;
+#endif
+
         init_waitqueue_head(&fbi->ctrlr_wait);
         INIT_WORK(&fbi->task, pxafb_task);
         mutex_init(&fbi->ctrlr_lock);
diff --git a/drivers/video/pxafb.h b/drivers/video/pxafb.h
index 2353521c5c8c..26ba9fa3f737 100644
--- a/drivers/video/pxafb.h
+++ b/drivers/video/pxafb.h
@@ -92,7 +92,8 @@ struct pxafb_layer_ops {
 struct pxafb_layer {
         struct fb_info          fb;
         int                     id;
-        atomic_t                usage;
+        int                     registered;
+        uint32_t                usage;
         uint32_t                control[2];
 
         struct pxafb_layer_ops  *ops;
diff --git a/drivers/video/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/riva/fbdev.c b/drivers/video/riva/fbdev.c
index 618f36bec10d..d8ab7be4fd6b 100644
--- a/drivers/video/riva/fbdev.c
+++ b/drivers/video/riva/fbdev.c
@@ -331,7 +331,7 @@ static int riva_bl_get_brightness(struct backlight_device *bd)
         return bd->props.brightness;
 }
 
-static struct backlight_ops riva_bl_ops = {
+static const struct backlight_ops riva_bl_ops = {
         .get_brightness = riva_bl_get_brightness,
         .update_status  = riva_bl_update_status,
 };
@@ -355,6 +355,7 @@ static void riva_bl_init(struct riva_par *par)
         snprintf(name, sizeof(name), "rivabl%d", info->node);
 
         memset(&props, 0, sizeof(struct backlight_properties));
+        props.type = BACKLIGHT_RAW;
         props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
         bd = backlight_device_register(name, info->dev, par, &riva_bl_ops,
                                        &props);
diff --git a/drivers/video/riva/rivafb-i2c.c b/drivers/video/riva/rivafb-i2c.c
index a0e22ac483a3..167400e2a182 100644
--- a/drivers/video/riva/rivafb-i2c.c
+++ b/drivers/video/riva/rivafb-i2c.c
@@ -94,7 +94,6 @@ static int __devinit riva_setup_i2c_bus(struct riva_i2c_chan *chan,
 
         strcpy(chan->adapter.name, name);
         chan->adapter.owner             = THIS_MODULE;
-        chan->adapter.id                = I2C_HW_B_RIVA;
         chan->adapter.class             = i2c_class;
         chan->adapter.algo_data         = &chan->algo;
         chan->adapter.dev.parent        = &chan->par->pdev->dev;
diff --git a/drivers/video/s1d13xxxfb.c b/drivers/video/s1d13xxxfb.c
index a6247fc081ab..28b1c6c3d8ac 100644
--- a/drivers/video/s1d13xxxfb.c
+++ b/drivers/video/s1d13xxxfb.c
@@ -410,28 +410,6 @@ s1d13xxxfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
  ************************************************************/
 
 /**
- *      bltbit_wait_bitset - waits for change in register value
- *      @info : framebuffer structure
- *      @bit  : value expected in register
- *      @timeout : ...
- *
- *      waits until value changes INTO bit
- */
-static u8
-bltbit_wait_bitset(struct fb_info *info, u8 bit, int timeout)
-{
-        while (!(s1d13xxxfb_readreg(info->par, S1DREG_BBLT_CTL0) & bit)) {
-                udelay(10);
-                if (!--timeout) {
-                        dbg_blit("wait_bitset timeout\n");
-                        break;
-                }
-        }
-
-        return timeout;
-}
-
-/**
  *      bltbit_wait_bitclear - waits for change in register value
  *      @info : frambuffer structure
  *      @bit  : value currently in register
@@ -454,34 +432,6 @@ bltbit_wait_bitclear(struct fb_info *info, u8 bit, int timeout)
         return timeout;
 }
 
-/**
- *      bltbit_fifo_status - checks the current status of the fifo
- *      @info : framebuffer structure
- *
- *      returns number of free words in buffer
- */
-static u8
-bltbit_fifo_status(struct fb_info *info)
-{
-        u8 status;
-
-        status = s1d13xxxfb_readreg(info->par, S1DREG_BBLT_CTL0);
-
-        /* its empty so room for 16 words */
-        if (status & BBLT_FIFO_EMPTY)
-                return 16;
-
-        /* its full so we dont want to add */
-        if (status & BBLT_FIFO_FULL)
-                return 0;
-
-        /* its atleast half full but we can add one atleast */
-        if (status & BBLT_FIFO_NOT_FULL)
-                return 1;
-
-        return 0;
-}
-
 /*
  *      s1d13xxxfb_bitblt_copyarea - accelerated copyarea function
  *      @info : framebuffer structure
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index f9aca9d13d1b..4aecf213c9be 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -23,6 +23,7 @@
 #include <linux/io.h>
 #include <linux/uaccess.h>
 #include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
 
 #include <mach/map.h>
 #include <plat/regs-fb-v4.h>
@@ -47,7 +48,7 @@
 #undef writel
 #define writel(v, r) do { \
         printk(KERN_DEBUG "%s: %08x => %p\n", __func__, (unsigned int)v, r); \
-        __raw_writel(v, r); } while(0)
+        __raw_writel(v, r); } while (0)
 #endif /* FB_S3C_DEBUG_REGWRITE */
 
 /* irq_flags bits */
@@ -181,6 +182,7 @@ struct s3c_fb_vsync {
 
 /**
  * struct s3c_fb - overall hardware state of the hardware
+ * @slock: The spinlock protection for this data sturcture.
  * @dev: The device that we bound to, for printing, etc.
  * @regs_res: The resource we claimed for the IO registers.
  * @bus_clk: The clk (hclk) feeding our interface and possibly pixclk.
@@ -194,6 +196,7 @@ struct s3c_fb_vsync {
  * @vsync_info: VSYNC-related information (count, queues...)
  */
 struct s3c_fb {
+        spinlock_t              slock;
         struct device           *dev;
         struct resource         *regs_res;
         struct clk              *bus_clk;
@@ -232,13 +235,12 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
                             struct fb_info *info)
 {
         struct s3c_fb_win *win = info->par;
-        struct s3c_fb_pd_win *windata = win->windata;
         struct s3c_fb *sfb = win->parent;
 
         dev_dbg(sfb->dev, "checking parameters\n");
 
-        var->xres_virtual = max((unsigned int)windata->virtual_x, var->xres);
-        var->yres_virtual = max((unsigned int)windata->virtual_y, var->yres);
+        var->xres_virtual = max(var->xres_virtual, var->xres);
+        var->yres_virtual = max(var->yres_virtual, var->yres);
 
         if (!s3c_fb_validate_win_bpp(win, var->bits_per_pixel)) {
                 dev_dbg(sfb->dev, "win %d: unsupported bpp %d\n",
@@ -299,6 +301,7 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
                 var->blue.length        = 5;
                 break;
 
+        case 32:
         case 28:
         case 25:
                 var->transp.length      = var->bits_per_pixel - 24;
@@ -307,7 +310,6 @@ static int s3c_fb_check_var(struct fb_var_screeninfo *var,
         case 24:
                 /* our 24bpp is unpacked, so 32bpp */
                 var->bits_per_pixel     = 32;
-        case 32:
                 var->red.offset         = 16;
                 var->red.length         = 8;
                 var->green.offset       = 8;
@@ -517,7 +519,7 @@ static int s3c_fb_set_par(struct fb_info *info)
 
                 data = VIDTCON2_LINEVAL(var->yres - 1) |
                        VIDTCON2_HOZVAL(var->xres - 1);
-                writel(data, regs +sfb->variant.vidtcon + 8 );
+                writel(data, regs + sfb->variant.vidtcon + 8);
         }
 
         /* write the buffer address */
@@ -555,6 +557,13 @@ static int s3c_fb_set_par(struct fb_info *info)
         vidosd_set_alpha(win, alpha);
         vidosd_set_size(win, data);
 
+        /* Enable DMA channel for this window */
+        if (sfb->variant.has_shadowcon) {
+                data = readl(sfb->regs + SHADOWCON);
+                data |= SHADOWCON_CHx_ENABLE(win_no);
+                writel(data, sfb->regs + SHADOWCON);
+        }
+
         data = WINCONx_ENWIN;
 
         /* note, since we have to round up the bits-per-pixel, we end up
@@ -634,13 +643,6 @@ static int s3c_fb_set_par(struct fb_info *info)
         writel(data, regs + sfb->variant.wincon + (win_no * 4));
         writel(0x0, regs + sfb->variant.winmap + (win_no * 4));
 
-        /* Enable DMA channel for this window */
-        if (sfb->variant.has_shadowcon) {
-                data = readl(sfb->regs + SHADOWCON);
-                data |= SHADOWCON_CHx_ENABLE(win_no);
-                writel(data, sfb->regs + SHADOWCON);
-        }
-
         shadow_protect_win(win, 0);
 
         return 0;
@@ -946,6 +948,8 @@ static irqreturn_t s3c_fb_irq(int irq, void *dev_id)
         void __iomem  *regs = sfb->regs;
         u32 irq_sts_reg;
 
+        spin_lock(&sfb->slock);
+
         irq_sts_reg = readl(regs + VIDINTCON1);
 
         if (irq_sts_reg & VIDINTCON1_INT_FRAME) {
@@ -962,6 +966,7 @@ static irqreturn_t s3c_fb_irq(int irq, void *dev_id)
          */
         s3c_fb_disable_irq(sfb);
 
+        spin_unlock(&sfb->slock);
         return IRQ_HANDLED;
 }
 
@@ -1013,8 +1018,30 @@ static int s3c_fb_ioctl(struct fb_info *info, unsigned int cmd,
         return ret;
 }
 
+static int s3c_fb_open(struct fb_info *info, int user)
+{
+        struct s3c_fb_win *win = info->par;
+        struct s3c_fb *sfb = win->parent;
+
+        pm_runtime_get_sync(sfb->dev);
+
+        return 0;
+}
+
+static int s3c_fb_release(struct fb_info *info, int user)
+{
+        struct s3c_fb_win *win = info->par;
+        struct s3c_fb *sfb = win->parent;
+
+        pm_runtime_put_sync(sfb->dev);
+
+        return 0;
+}
+
 static struct fb_ops s3c_fb_ops = {
         .owner          = THIS_MODULE,
+        .fb_open        = s3c_fb_open,
+        .fb_release     = s3c_fb_release,
         .fb_check_var   = s3c_fb_check_var,
         .fb_set_par     = s3c_fb_set_par,
         .fb_blank       = s3c_fb_blank,
@@ -1281,6 +1308,7 @@ static void s3c_fb_clear_win(struct s3c_fb *sfb, int win)
 
 static int __devinit s3c_fb_probe(struct platform_device *pdev)
 {
+        const struct platform_device_id *platid;
         struct s3c_fb_driverdata *fbdrv;
         struct device *dev = &pdev->dev;
         struct s3c_fb_platdata *pd;
@@ -1289,7 +1317,8 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
         int win;
         int ret = 0;
 
-        fbdrv = (struct s3c_fb_driverdata *)platform_get_device_id(pdev)->driver_data;
+        platid = platform_get_device_id(pdev);
+        fbdrv = (struct s3c_fb_driverdata *)platid->driver_data;
 
         if (fbdrv->variant.nr_windows > S3C_FB_MAX_WIN) {
                 dev_err(dev, "too many windows, cannot attach\n");
@@ -1314,14 +1343,19 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
         sfb->pdata = pd;
         sfb->variant = fbdrv->variant;
 
+        spin_lock_init(&sfb->slock);
+
         sfb->bus_clk = clk_get(dev, "lcd");
         if (IS_ERR(sfb->bus_clk)) {
                 dev_err(dev, "failed to get bus clock\n");
+                ret = PTR_ERR(sfb->bus_clk);
                 goto err_sfb;
         }
 
         clk_enable(sfb->bus_clk);
 
+        pm_runtime_enable(sfb->dev);
+
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (!res) {
                 dev_err(dev, "failed to find registers\n");
@@ -1360,6 +1394,9 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 
         dev_dbg(dev, "got resources (regs %p), probing windows\n", sfb->regs);
 
+        platform_set_drvdata(pdev, sfb);
+        pm_runtime_get_sync(sfb->dev);
+
         /* setup gpio and output polarity controls */
 
         pd->setup_gpio();
@@ -1400,6 +1437,7 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
         }
 
         platform_set_drvdata(pdev, sfb);
+        pm_runtime_put_sync(sfb->dev);
 
         return 0;
 
@@ -1410,8 +1448,7 @@ err_ioremap:
         iounmap(sfb->regs);
 
 err_req_region:
-        release_resource(sfb->regs_res);
-        kfree(sfb->regs_res);
+        release_mem_region(sfb->regs_res->start, resource_size(sfb->regs_res));
 
 err_clk:
         clk_disable(sfb->bus_clk);
@@ -1434,6 +1471,8 @@ static int __devexit s3c_fb_remove(struct platform_device *pdev)
         struct s3c_fb *sfb = platform_get_drvdata(pdev);
         int win;
 
+        pm_runtime_get_sync(sfb->dev);
+
         for (win = 0; win < S3C_FB_MAX_WIN; win++)
                 if (sfb->windows[win])
                         s3c_fb_release_win(sfb, sfb->windows[win]);
@@ -1445,17 +1484,78 @@ static int __devexit s3c_fb_remove(struct platform_device *pdev)
         clk_disable(sfb->bus_clk);
         clk_put(sfb->bus_clk);
 
-        release_resource(sfb->regs_res);
-        kfree(sfb->regs_res);
+        release_mem_region(sfb->regs_res->start, resource_size(sfb->regs_res));
 
-        kfree(sfb);
+        pm_runtime_put_sync(sfb->dev);
+        pm_runtime_disable(sfb->dev);
 
+        kfree(sfb);
         return 0;
 }
 
 #ifdef CONFIG_PM
-static int s3c_fb_suspend(struct platform_device *pdev, pm_message_t state)
+static int s3c_fb_suspend(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct s3c_fb *sfb = platform_get_drvdata(pdev);
+        struct s3c_fb_win *win;
+        int win_no;
+
+        for (win_no = S3C_FB_MAX_WIN - 1; win_no >= 0; win_no--) {
+                win = sfb->windows[win_no];
+                if (!win)
+                        continue;
+
+                /* use the blank function to push into power-down */
+                s3c_fb_blank(FB_BLANK_POWERDOWN, win->fbinfo);
+        }
+
+        clk_disable(sfb->bus_clk);
+        return 0;
+}
+
+static int s3c_fb_resume(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct s3c_fb *sfb = platform_get_drvdata(pdev);
+        struct s3c_fb_platdata *pd = sfb->pdata;
+        struct s3c_fb_win *win;
+        int win_no;
+
+        clk_enable(sfb->bus_clk);
+
+        /* setup gpio and output polarity controls */
+        pd->setup_gpio();
+        writel(pd->vidcon1, sfb->regs + VIDCON1);
+
+        /* zero all windows before we do anything */
+        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];
+                if (!win)
+                        continue;
+
+                dev_dbg(&pdev->dev, "resuming window %d\n", win_no);
+                s3c_fb_set_par(win->fbinfo);
+        }
+
+        return 0;
+}
+
+static int s3c_fb_runtime_suspend(struct device *dev)
 {
+        struct platform_device *pdev = to_platform_device(dev);
         struct s3c_fb *sfb = platform_get_drvdata(pdev);
         struct s3c_fb_win *win;
         int win_no;
@@ -1473,8 +1573,9 @@ static int s3c_fb_suspend(struct platform_device *pdev, pm_message_t state)
         return 0;
 }
 
-static int s3c_fb_resume(struct platform_device *pdev)
+static int s3c_fb_runtime_resume(struct device *dev)
 {
+        struct platform_device *pdev = to_platform_device(dev);
         struct s3c_fb *sfb = platform_get_drvdata(pdev);
         struct s3c_fb_platdata *pd = sfb->pdata;
         struct s3c_fb_win *win;
@@ -1482,7 +1583,8 @@ static int s3c_fb_resume(struct platform_device *pdev)
 
         clk_enable(sfb->bus_clk);
 
-        /* setup registers */
+        /* setup gpio and output polarity controls */
+        pd->setup_gpio();
         writel(pd->vidcon1, sfb->regs + VIDCON1);
 
         /* zero all windows before we do anything */
@@ -1509,9 +1611,12 @@ static int s3c_fb_resume(struct platform_device *pdev)
 
         return 0;
 }
+
 #else
 #define s3c_fb_suspend NULL
 #define s3c_fb_resume  NULL
+#define s3c_fb_runtime_suspend NULL
+#define s3c_fb_runtime_resume NULL
 #endif
 
 
@@ -1523,28 +1628,31 @@ static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
                 .has_osd_c      = 1,
                 .osd_size_off   = 0x8,
                 .palette_sz     = 256,
-                .valid_bpp      = VALID_BPP1248 | VALID_BPP(16) | VALID_BPP(24),
+                .valid_bpp      = (VALID_BPP1248 | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(24)),
         },
         [1] = {
                 .has_osd_c      = 1,
                 .has_osd_d      = 1,
-                .osd_size_off   = 0x12,
+                .osd_size_off   = 0xc,
                 .has_osd_alpha  = 1,
                 .palette_sz     = 256,
                 .valid_bpp      = (VALID_BPP1248 | VALID_BPP(16) |
                                    VALID_BPP(18) | VALID_BPP(19) |
-                                   VALID_BPP(24) | VALID_BPP(25)),
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(28)),
         },
         [2] = {
                 .has_osd_c      = 1,
                 .has_osd_d      = 1,
-                .osd_size_off   = 0x12,
+                .osd_size_off   = 0xc,
                 .has_osd_alpha  = 1,
                 .palette_sz     = 16,
                 .palette_16bpp  = 1,
                 .valid_bpp      = (VALID_BPP1248 | VALID_BPP(16) |
                                    VALID_BPP(18) | VALID_BPP(19) |
-                                   VALID_BPP(24) | VALID_BPP(25)),
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(28)),
         },
         [3] = {
                 .has_osd_c      = 1,
@@ -1553,7 +1661,8 @@ static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
                 .palette_16bpp  = 1,
                 .valid_bpp      = (VALID_BPP124  | VALID_BPP(16) |
                                    VALID_BPP(18) | VALID_BPP(19) |
-                                   VALID_BPP(24) | VALID_BPP(25)),
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(28)),
         },
         [4] = {
                 .has_osd_c      = 1,
@@ -1562,7 +1671,65 @@ static struct s3c_fb_win_variant s3c_fb_data_64xx_wins[] = {
                 .palette_16bpp  = 1,
                 .valid_bpp      = (VALID_BPP(1) | VALID_BPP(2) |
                                    VALID_BPP(16) | VALID_BPP(18) |
-                                   VALID_BPP(24) | VALID_BPP(25)),
+                                   VALID_BPP(19) | VALID_BPP(24) |
+                                   VALID_BPP(25) | VALID_BPP(28)),
+        },
+};
+
+static struct s3c_fb_win_variant s3c_fb_data_s5p_wins[] = {
+        [0] = {
+                .has_osd_c      = 1,
+                .osd_size_off   = 0x8,
+                .palette_sz     = 256,
+                .valid_bpp      = (VALID_BPP1248 | VALID_BPP(13) |
+                                   VALID_BPP(15) | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(19) |
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(32)),
+        },
+        [1] = {
+                .has_osd_c      = 1,
+                .has_osd_d      = 1,
+                .osd_size_off   = 0xc,
+                .has_osd_alpha  = 1,
+                .palette_sz     = 256,
+                .valid_bpp      = (VALID_BPP1248 | VALID_BPP(13) |
+                                   VALID_BPP(15) | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(19) |
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(32)),
+        },
+        [2] = {
+                .has_osd_c      = 1,
+                .has_osd_d      = 1,
+                .osd_size_off   = 0xc,
+                .has_osd_alpha  = 1,
+                .palette_sz     = 256,
+                .valid_bpp      = (VALID_BPP1248 | VALID_BPP(13) |
+                                   VALID_BPP(15) | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(19) |
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(32)),
+        },
+        [3] = {
+                .has_osd_c      = 1,
+                .has_osd_alpha  = 1,
+                .palette_sz     = 256,
+                .valid_bpp      = (VALID_BPP1248 | VALID_BPP(13) |
+                                   VALID_BPP(15) | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(19) |
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(32)),
+        },
+        [4] = {
+                .has_osd_c      = 1,
+                .has_osd_alpha  = 1,
+                .palette_sz     = 256,
+                .valid_bpp      = (VALID_BPP1248 | VALID_BPP(13) |
+                                   VALID_BPP(15) | VALID_BPP(16) |
+                                   VALID_BPP(18) | VALID_BPP(19) |
+                                   VALID_BPP(24) | VALID_BPP(25) |
+                                   VALID_BPP(32)),
         },
 };
 
@@ -1619,11 +1786,11 @@ static struct s3c_fb_driverdata s3c_fb_data_s5pc100 = {
 
                 .has_prtcon     = 1,
         },
-        .win[0] = &s3c_fb_data_64xx_wins[0],
-        .win[1] = &s3c_fb_data_64xx_wins[1],
-        .win[2] = &s3c_fb_data_64xx_wins[2],
-        .win[3] = &s3c_fb_data_64xx_wins[3],
-        .win[4] = &s3c_fb_data_64xx_wins[4],
+        .win[0] = &s3c_fb_data_s5p_wins[0],
+        .win[1] = &s3c_fb_data_s5p_wins[1],
+        .win[2] = &s3c_fb_data_s5p_wins[2],
+        .win[3] = &s3c_fb_data_s5p_wins[3],
+        .win[4] = &s3c_fb_data_s5p_wins[4],
 };
 
 static struct s3c_fb_driverdata s3c_fb_data_s5pv210 = {
@@ -1649,11 +1816,11 @@ static struct s3c_fb_driverdata s3c_fb_data_s5pv210 = {
 
                 .has_shadowcon  = 1,
         },
-        .win[0] = &s3c_fb_data_64xx_wins[0],
-        .win[1] = &s3c_fb_data_64xx_wins[1],
-        .win[2] = &s3c_fb_data_64xx_wins[2],
-        .win[3] = &s3c_fb_data_64xx_wins[3],
-        .win[4] = &s3c_fb_data_64xx_wins[4],
+        .win[0] = &s3c_fb_data_s5p_wins[0],
+        .win[1] = &s3c_fb_data_s5p_wins[1],
+        .win[2] = &s3c_fb_data_s5p_wins[2],
+        .win[3] = &s3c_fb_data_s5p_wins[3],
+        .win[4] = &s3c_fb_data_s5p_wins[4],
 };
 
 /* S3C2443/S3C2416 style hardware */
@@ -1710,15 +1877,21 @@ static struct platform_device_id s3c_fb_driver_ids[] = {
 };
 MODULE_DEVICE_TABLE(platform, s3c_fb_driver_ids);
 
+static const struct dev_pm_ops s3cfb_pm_ops = {
+        .suspend        = s3c_fb_suspend,
+        .resume         = s3c_fb_resume,
+        .runtime_suspend        = s3c_fb_runtime_suspend,
+        .runtime_resume         = s3c_fb_runtime_resume,
+};
+
 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,
+                .pm     = &s3cfb_pm_ops,
         },
 };
 
diff --git a/drivers/video/s3c2410fb.c b/drivers/video/s3c2410fb.c
index 46b430978bcc..0aa13761de6e 100644
--- a/drivers/video/s3c2410fb.c
+++ b/drivers/video/s3c2410fb.c
@@ -13,6 +13,7 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
+#include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/string.h>
 #include <linux/mm.h>
@@ -866,7 +867,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
                 goto dealloc_fb;
         }
 
-        size = (res->end - res->start) + 1;
+        size = resource_size(res);
         info->mem = request_mem_region(res->start, size, pdev->name);
         if (info->mem == NULL) {
                 dev_err(&pdev->dev, "failed to get memory region\n");
@@ -918,9 +919,9 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
         }
 
         info->clk = clk_get(NULL, "lcd");
-        if (!info->clk || IS_ERR(info->clk)) {
+        if (IS_ERR(info->clk)) {
                 printk(KERN_ERR "failed to get lcd clock source\n");
-                ret = -ENOENT;
+                ret = PTR_ERR(info->clk);
                 goto release_irq;
         }
 
@@ -996,8 +997,7 @@ release_irq:
 release_regs:
         iounmap(info->io);
 release_mem:
-        release_resource(info->mem);
-        kfree(info->mem);
+        release_mem_region(res->start, size);
 dealloc_fb:
         platform_set_drvdata(pdev, NULL);
         framebuffer_release(fbinfo);
@@ -1043,8 +1043,7 @@ static int __devexit s3c2410fb_remove(struct platform_device *pdev)
 
         iounmap(info->io);
 
-        release_resource(info->mem);
-        kfree(info->mem);
+        release_mem_region(info->mem->start, resource_size(info->mem));
 
         platform_set_drvdata(pdev, NULL);
         framebuffer_release(fbinfo);
diff --git a/drivers/video/s3fb.c b/drivers/video/s3fb.c
index dce8c97b4333..4ca5d0c8fe84 100644
--- a/drivers/video/s3fb.c
+++ b/drivers/video/s3fb.c
@@ -22,9 +22,12 @@
 #include <linux/svga.h>
 #include <linux/init.h>
 #include <linux/pci.h>
-#include <linux/console.h> /* Why should fb driver call console functions? because acquire_console_sem() */
+#include <linux/console.h> /* Why should fb driver call console functions? because console_lock() */
 #include <video/vga.h>
 
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
 #ifdef CONFIG_MTRR
 #include <asm/mtrr.h>
 #endif
@@ -36,6 +39,12 @@ struct s3fb_info {
         struct mutex open_lock;
         unsigned int ref_count;
         u32 pseudo_palette[16];
+#ifdef CONFIG_FB_S3_DDC
+        u8 __iomem *mmio;
+        bool ddc_registered;
+        struct i2c_adapter ddc_adapter;
+        struct i2c_algo_bit_data ddc_algo;
+#endif
 };
 
 
@@ -64,15 +73,18 @@ static const struct svga_fb_format s3fb_formats[] = {
 
 static const struct svga_pll s3_pll = {3, 129, 3, 33, 0, 3,
         35000, 240000, 14318};
+static const struct svga_pll s3_trio3d_pll = {3, 129, 3, 31, 0, 4,
+        230000, 460000, 14318};
 
 static const int s3_memsizes[] = {4096, 0, 3072, 8192, 2048, 6144, 1024, 512};
 
 static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64", "S3 Trio64V+",
                         "S3 Trio64UV+", "S3 Trio64V2/DX", "S3 Trio64V2/GX",
-                        "S3 Plato/PX", "S3 Aurora64VP", "S3 Virge",
+                        "S3 Plato/PX", "S3 Aurora64V+", "S3 Virge",
                         "S3 Virge/VX", "S3 Virge/DX", "S3 Virge/GX",
-                        "S3 Virge/GX2", "S3 Virge/GX2P", "S3 Virge/GX2P",
-                        "S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X"};
+                        "S3 Virge/GX2", "S3 Virge/GX2+", "",
+                        "S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X",
+                        "S3 Trio3D"};
 
 #define CHIP_UNKNOWN            0x00
 #define CHIP_732_TRIO32         0x01
@@ -87,12 +99,12 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_988_VIRGE_VX       0x0A
 #define CHIP_375_VIRGE_DX       0x0B
 #define CHIP_385_VIRGE_GX       0x0C
-#define CHIP_356_VIRGE_GX2      0x0D
-#define CHIP_357_VIRGE_GX2P     0x0E
-#define CHIP_359_VIRGE_GX2P     0x0F
+#define CHIP_357_VIRGE_GX2      0x0D
+#define CHIP_359_VIRGE_GX2P     0x0E
 #define CHIP_360_TRIO3D_1X      0x10
 #define CHIP_362_TRIO3D_2X      0x11
 #define CHIP_368_TRIO3D_2X      0x12
+#define CHIP_365_TRIO3D         0x13
 
 #define CHIP_XXX_TRIO           0x80
 #define CHIP_XXX_TRIO64V2_DXGX  0x81
@@ -102,6 +114,9 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_UNDECIDED_FLAG     0x80
 #define CHIP_MASK               0xFF
 
+#define MMIO_OFFSET             0x1000000
+#define MMIO_SIZE               0x10000
+
 /* CRT timing register sets */
 
 static const struct vga_regset s3_h_total_regs[]        = {{0x00, 0, 7}, {0x5D, 0, 0}, VGA_REGSET_END};
@@ -119,9 +134,11 @@ static const struct vga_regset s3_v_sync_start_regs[]   = {{0x10, 0, 7}, {0x07,
 static const struct vga_regset s3_v_sync_end_regs[]     = {{0x11, 0, 3}, VGA_REGSET_END};
 
 static const struct vga_regset s3_line_compare_regs[]   = {{0x18, 0, 7}, {0x07, 4, 4}, {0x09, 6, 6}, {0x5E, 6, 6}, VGA_REGSET_END};
-static const struct vga_regset s3_start_address_regs[]  = {{0x0d, 0, 7}, {0x0c, 0, 7}, {0x31, 4, 5}, {0x51, 0, 1}, VGA_REGSET_END};
+static const struct vga_regset s3_start_address_regs[]  = {{0x0d, 0, 7}, {0x0c, 0, 7}, {0x69, 0, 4}, VGA_REGSET_END};
 static const struct vga_regset s3_offset_regs[]         = {{0x13, 0, 7}, {0x51, 4, 5}, VGA_REGSET_END}; /* set 0x43 bit 2 to 0 */
 
+static const struct vga_regset s3_dtpc_regs[]           = {{0x3B, 0, 7}, {0x5D, 6, 6}, VGA_REGSET_END};
+
 static const struct svga_timing_regs s3_timing_regs     = {
         s3_h_total_regs, s3_h_display_regs, s3_h_blank_start_regs,
         s3_h_blank_end_regs, s3_h_sync_start_regs, s3_h_sync_end_regs,
@@ -135,7 +152,7 @@ static const struct svga_timing_regs s3_timing_regs     = {
 /* Module parameters */
 
 
-static char *mode_option __devinitdata = "640x480-8@60";
+static char *mode_option __devinitdata;
 
 #ifdef CONFIG_MTRR
 static int mtrr __devinitdata = 1;
@@ -164,6 +181,119 @@ MODULE_PARM_DESC(fasttext, "Enable S3 fast text mode (1=enable, 0=disable, defau
 
 /* ------------------------------------------------------------------------- */
 
+#ifdef CONFIG_FB_S3_DDC
+
+#define DDC_REG         0xaa            /* Trio 3D/1X/2X */
+#define DDC_MMIO_REG    0xff20          /* all other chips */
+#define DDC_SCL_OUT     (1 << 0)
+#define DDC_SDA_OUT     (1 << 1)
+#define DDC_SCL_IN      (1 << 2)
+#define DDC_SDA_IN      (1 << 3)
+#define DDC_DRIVE_EN    (1 << 4)
+
+static bool s3fb_ddc_needs_mmio(int chip)
+{
+        return !(chip == CHIP_360_TRIO3D_1X  ||
+                 chip == CHIP_362_TRIO3D_2X  ||
+                 chip == CHIP_368_TRIO3D_2X);
+}
+
+static u8 s3fb_ddc_read(struct s3fb_info *par)
+{
+        if (s3fb_ddc_needs_mmio(par->chip))
+                return readb(par->mmio + DDC_MMIO_REG);
+        else
+                return vga_rcrt(par->state.vgabase, DDC_REG);
+}
+
+static void s3fb_ddc_write(struct s3fb_info *par, u8 val)
+{
+        if (s3fb_ddc_needs_mmio(par->chip))
+                writeb(val, par->mmio + DDC_MMIO_REG);
+        else
+                vga_wcrt(par->state.vgabase, DDC_REG, val);
+}
+
+static void s3fb_ddc_setscl(void *data, int val)
+{
+        struct s3fb_info *par = data;
+        unsigned char reg;
+
+        reg = s3fb_ddc_read(par) | DDC_DRIVE_EN;
+        if (val)
+                reg |= DDC_SCL_OUT;
+        else
+                reg &= ~DDC_SCL_OUT;
+        s3fb_ddc_write(par, reg);
+}
+
+static void s3fb_ddc_setsda(void *data, int val)
+{
+        struct s3fb_info *par = data;
+        unsigned char reg;
+
+        reg = s3fb_ddc_read(par) | DDC_DRIVE_EN;
+        if (val)
+                reg |= DDC_SDA_OUT;
+        else
+                reg &= ~DDC_SDA_OUT;
+        s3fb_ddc_write(par, reg);
+}
+
+static int s3fb_ddc_getscl(void *data)
+{
+        struct s3fb_info *par = data;
+
+        return !!(s3fb_ddc_read(par) & DDC_SCL_IN);
+}
+
+static int s3fb_ddc_getsda(void *data)
+{
+        struct s3fb_info *par = data;
+
+        return !!(s3fb_ddc_read(par) & DDC_SDA_IN);
+}
+
+static int __devinit s3fb_setup_ddc_bus(struct fb_info *info)
+{
+        struct s3fb_info *par = info->par;
+
+        strlcpy(par->ddc_adapter.name, info->fix.id,
+                sizeof(par->ddc_adapter.name));
+        par->ddc_adapter.owner          = THIS_MODULE;
+        par->ddc_adapter.class          = I2C_CLASS_DDC;
+        par->ddc_adapter.algo_data      = &par->ddc_algo;
+        par->ddc_adapter.dev.parent     = info->device;
+        par->ddc_algo.setsda            = s3fb_ddc_setsda;
+        par->ddc_algo.setscl            = s3fb_ddc_setscl;
+        par->ddc_algo.getsda            = s3fb_ddc_getsda;
+        par->ddc_algo.getscl            = s3fb_ddc_getscl;
+        par->ddc_algo.udelay            = 10;
+        par->ddc_algo.timeout           = 20;
+        par->ddc_algo.data              = par;
+
+        i2c_set_adapdata(&par->ddc_adapter, par);
+
+        /*
+         * some Virge cards have external MUX to switch chip I2C bus between
+         * DDC and extension pins - switch it do DDC
+         */
+/*      vga_wseq(par->state.vgabase, 0x08, 0x06); - not needed, already unlocked */
+        if (par->chip == CHIP_357_VIRGE_GX2 ||
+            par->chip == CHIP_359_VIRGE_GX2P)
+                svga_wseq_mask(par->state.vgabase, 0x0d, 0x01, 0x03);
+        else
+                svga_wseq_mask(par->state.vgabase, 0x0d, 0x00, 0x03);
+        /* some Virge need this or the DDC is ignored */
+        svga_wcrt_mask(par->state.vgabase, 0x5c, 0x03, 0x03);
+
+        return i2c_bit_add_bus(&par->ddc_adapter);
+}
+#endif /* CONFIG_FB_S3_DDC */
+
+
+/* ------------------------------------------------------------------------- */
+
 /* Set font in S3 fast text mode */
 
 static void s3fb_settile_fast(struct fb_info *info, struct fb_tilemap *map)
@@ -188,12 +318,19 @@ static void s3fb_settile_fast(struct fb_info *info, struct fb_tilemap *map)
         }
 }
 
+static void s3fb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+        struct s3fb_info *par = info->par;
+
+        svga_tilecursor(par->state.vgabase, info, cursor);
+}
+
 static struct fb_tile_ops s3fb_tile_ops = {
         .fb_settile     = svga_settile,
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = s3fb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -202,7 +339,7 @@ static struct fb_tile_ops s3fb_fast_tile_ops = {
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = s3fb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -334,33 +471,36 @@ static void s3_set_pixclock(struct fb_info *info, u32 pixclock)
         u8 regval;
         int rv;
 
-        rv = svga_compute_pll(&s3_pll, 1000000000 / pixclock, &m, &n, &r, info->node);
+        rv = svga_compute_pll((par->chip == CHIP_365_TRIO3D) ? &s3_trio3d_pll : &s3_pll,
+                              1000000000 / pixclock, &m, &n, &r, info->node);
         if (rv < 0) {
                 printk(KERN_ERR "fb%d: cannot set requested pixclock, keeping old value\n", info->node);
                 return;
         }
 
         /* Set VGA misc register  */
-        regval = vga_r(NULL, VGA_MIS_R);
-        vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+        regval = vga_r(par->state.vgabase, VGA_MIS_R);
+        vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
         /* Set S3 clock registers */
-        if (par->chip == CHIP_360_TRIO3D_1X ||
+        if (par->chip == CHIP_357_VIRGE_GX2 ||
+            par->chip == CHIP_359_VIRGE_GX2P ||
+            par->chip == CHIP_360_TRIO3D_1X ||
             par->chip == CHIP_362_TRIO3D_2X ||
             par->chip == CHIP_368_TRIO3D_2X) {
-                vga_wseq(NULL, 0x12, (n - 2) | ((r & 3) << 6)); /* n and two bits of r */
-                vga_wseq(NULL, 0x29, r >> 2); /* remaining highest bit of r */
+                vga_wseq(par->state.vgabase, 0x12, (n - 2) | ((r & 3) << 6));   /* n and two bits of r */
+                vga_wseq(par->state.vgabase, 0x29, r >> 2); /* remaining highest bit of r */
         } else
-                vga_wseq(NULL, 0x12, (n - 2) | (r << 5));
-        vga_wseq(NULL, 0x13, m - 2);
+                vga_wseq(par->state.vgabase, 0x12, (n - 2) | (r << 5));
+        vga_wseq(par->state.vgabase, 0x13, m - 2);
 
         udelay(1000);
 
         /* Activate clock - write 0, 1, 0 to seq/15 bit 5 */
-        regval = vga_rseq (NULL, 0x15); /* | 0x80; */
-        vga_wseq(NULL, 0x15, regval & ~(1<<5));
-        vga_wseq(NULL, 0x15, regval |  (1<<5));
-        vga_wseq(NULL, 0x15, regval & ~(1<<5));
+        regval = vga_rseq (par->state.vgabase, 0x15); /* | 0x80; */
+        vga_wseq(par->state.vgabase, 0x15, regval & ~(1<<5));
+        vga_wseq(par->state.vgabase, 0x15, regval |  (1<<5));
+        vga_wseq(par->state.vgabase, 0x15, regval & ~(1<<5));
 }
 
 
@@ -372,7 +512,10 @@ static int s3fb_open(struct fb_info *info, int user)
 
         mutex_lock(&(par->open_lock));
         if (par->ref_count == 0) {
+                void __iomem *vgabase = par->state.vgabase;
+
                 memset(&(par->state), 0, sizeof(struct vgastate));
+                par->state.vgabase = vgabase;
                 par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
                 par->state.num_crtc = 0x70;
                 par->state.num_seq = 0x20;
@@ -470,6 +613,7 @@ static int s3fb_set_par(struct fb_info *info)
         struct s3fb_info *par = info->par;
         u32 value, mode, hmul, offset_value, screen_size, multiplex, dbytes;
         u32 bpp = info->var.bits_per_pixel;
+        u32 htotal, hsstart;
 
         if (bpp != 0) {
                 info->fix.ypanstep = 1;
@@ -504,99 +648,115 @@ static int s3fb_set_par(struct fb_info *info)
         info->var.activate = FB_ACTIVATE_NOW;
 
         /* Unlock registers */
-        vga_wcrt(NULL, 0x38, 0x48);
-        vga_wcrt(NULL, 0x39, 0xA5);
-        vga_wseq(NULL, 0x08, 0x06);
-        svga_wcrt_mask(0x11, 0x00, 0x80);
+        vga_wcrt(par->state.vgabase, 0x38, 0x48);
+        vga_wcrt(par->state.vgabase, 0x39, 0xA5);
+        vga_wseq(par->state.vgabase, 0x08, 0x06);
+        svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
 
         /* Blank screen and turn off sync */
-        svga_wseq_mask(0x01, 0x20, 0x20);
-        svga_wcrt_mask(0x17, 0x00, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
         /* Set default values */
-        svga_set_default_gfx_regs();
-        svga_set_default_atc_regs();
-        svga_set_default_seq_regs();
-        svga_set_default_crt_regs();
-        svga_wcrt_multi(s3_line_compare_regs, 0xFFFFFFFF);
-        svga_wcrt_multi(s3_start_address_regs, 0);
+        svga_set_default_gfx_regs(par->state.vgabase);
+        svga_set_default_atc_regs(par->state.vgabase);
+        svga_set_default_seq_regs(par->state.vgabase);
+        svga_set_default_crt_regs(par->state.vgabase);
+        svga_wcrt_multi(par->state.vgabase, s3_line_compare_regs, 0xFFFFFFFF);
+        svga_wcrt_multi(par->state.vgabase, s3_start_address_regs, 0);
 
         /* S3 specific initialization */
-        svga_wcrt_mask(0x58, 0x10, 0x10); /* enable linear framebuffer */
-        svga_wcrt_mask(0x31, 0x08, 0x08); /* enable sequencer access to framebuffer above 256 kB */
+        svga_wcrt_mask(par->state.vgabase, 0x58, 0x10, 0x10); /* enable linear framebuffer */
+        svga_wcrt_mask(par->state.vgabase, 0x31, 0x08, 0x08); /* enable sequencer access to framebuffer above 256 kB */
 
-/*      svga_wcrt_mask(0x33, 0x08, 0x08); */ /* DDR ?   */
-/*      svga_wcrt_mask(0x43, 0x01, 0x01); */ /* DDR ?   */
-        svga_wcrt_mask(0x33, 0x00, 0x08); /* no DDR ?   */
-        svga_wcrt_mask(0x43, 0x00, 0x01); /* no DDR ?   */
+/*      svga_wcrt_mask(par->state.vgabase, 0x33, 0x08, 0x08); */ /* DDR ?       */
+/*      svga_wcrt_mask(par->state.vgabase, 0x43, 0x01, 0x01); */ /* DDR ?       */
+        svga_wcrt_mask(par->state.vgabase, 0x33, 0x00, 0x08); /* no DDR ?       */
+        svga_wcrt_mask(par->state.vgabase, 0x43, 0x00, 0x01); /* no DDR ?       */
 
-        svga_wcrt_mask(0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
+        svga_wcrt_mask(par->state.vgabase, 0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
 
-/*      svga_wcrt_mask(0x58, 0x03, 0x03); */
+/*      svga_wcrt_mask(par->state.vgabase, 0x58, 0x03, 0x03); */
 
-/*      svga_wcrt_mask(0x53, 0x12, 0x13); */ /* enable MMIO */
-/*      svga_wcrt_mask(0x40, 0x08, 0x08); */ /* enable write buffer */
+/*      svga_wcrt_mask(par->state.vgabase, 0x53, 0x12, 0x13); */ /* enable MMIO */
+/*      svga_wcrt_mask(par->state.vgabase, 0x40, 0x08, 0x08); */ /* enable write buffer */
 
 
         /* Set the offset register */
         pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
-        svga_wcrt_multi(s3_offset_regs, offset_value);
+        svga_wcrt_multi(par->state.vgabase, s3_offset_regs, offset_value);
 
-        if (par->chip != CHIP_360_TRIO3D_1X &&
+        if (par->chip != CHIP_357_VIRGE_GX2 &&
+            par->chip != CHIP_359_VIRGE_GX2P &&
+            par->chip != CHIP_360_TRIO3D_1X &&
             par->chip != CHIP_362_TRIO3D_2X &&
             par->chip != CHIP_368_TRIO3D_2X) {
-                vga_wcrt(NULL, 0x54, 0x18); /* M parameter */
-                vga_wcrt(NULL, 0x60, 0xff); /* N parameter */
-                vga_wcrt(NULL, 0x61, 0xff); /* L parameter */
-                vga_wcrt(NULL, 0x62, 0xff); /* L parameter */
+                vga_wcrt(par->state.vgabase, 0x54, 0x18); /* M parameter */
+                vga_wcrt(par->state.vgabase, 0x60, 0xff); /* N parameter */
+                vga_wcrt(par->state.vgabase, 0x61, 0xff); /* L parameter */
+                vga_wcrt(par->state.vgabase, 0x62, 0xff); /* L parameter */
         }
 
-        vga_wcrt(NULL, 0x3A, 0x35);
-        svga_wattr(0x33, 0x00);
+        vga_wcrt(par->state.vgabase, 0x3A, 0x35);
+        svga_wattr(par->state.vgabase, 0x33, 0x00);
 
         if (info->var.vmode & FB_VMODE_DOUBLE)
-                svga_wcrt_mask(0x09, 0x80, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
         else
-                svga_wcrt_mask(0x09, 0x00, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
         if (info->var.vmode & FB_VMODE_INTERLACED)
-                svga_wcrt_mask(0x42, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x42, 0x20, 0x20);
         else
-                svga_wcrt_mask(0x42, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x42, 0x00, 0x20);
 
         /* Disable hardware graphics cursor */
-        svga_wcrt_mask(0x45, 0x00, 0x01);
+        svga_wcrt_mask(par->state.vgabase, 0x45, 0x00, 0x01);
         /* Disable Streams engine */
-        svga_wcrt_mask(0x67, 0x00, 0x0C);
+        svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0x0C);
 
         mode = svga_match_format(s3fb_formats, &(info->var), &(info->fix));
 
         /* S3 virge DX hack */
         if (par->chip == CHIP_375_VIRGE_DX) {
-                vga_wcrt(NULL, 0x86, 0x80);
-                vga_wcrt(NULL, 0x90, 0x00);
+                vga_wcrt(par->state.vgabase, 0x86, 0x80);
+                vga_wcrt(par->state.vgabase, 0x90, 0x00);
         }
 
         /* S3 virge VX hack */
         if (par->chip == CHIP_988_VIRGE_VX) {
-                vga_wcrt(NULL, 0x50, 0x00);
-                vga_wcrt(NULL, 0x67, 0x50);
+                vga_wcrt(par->state.vgabase, 0x50, 0x00);
+                vga_wcrt(par->state.vgabase, 0x67, 0x50);
 
-                vga_wcrt(NULL, 0x63, (mode <= 2) ? 0x90 : 0x09);
-                vga_wcrt(NULL, 0x66, 0x90);
+                vga_wcrt(par->state.vgabase, 0x63, (mode <= 2) ? 0x90 : 0x09);
+                vga_wcrt(par->state.vgabase, 0x66, 0x90);
         }
 
-        if (par->chip == CHIP_360_TRIO3D_1X ||
+        if (par->chip == CHIP_357_VIRGE_GX2 ||
+            par->chip == CHIP_359_VIRGE_GX2P ||
+            par->chip == CHIP_360_TRIO3D_1X ||
             par->chip == CHIP_362_TRIO3D_2X ||
-            par->chip == CHIP_368_TRIO3D_2X) {
+            par->chip == CHIP_368_TRIO3D_2X ||
+            par->chip == CHIP_365_TRIO3D    ||
+            par->chip == CHIP_375_VIRGE_DX  ||
+            par->chip == CHIP_385_VIRGE_GX) {
                 dbytes = info->var.xres * ((bpp+7)/8);
-                vga_wcrt(NULL, 0x91, (dbytes + 7) / 8);
-                vga_wcrt(NULL, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
+                vga_wcrt(par->state.vgabase, 0x91, (dbytes + 7) / 8);
+                vga_wcrt(par->state.vgabase, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
 
-                vga_wcrt(NULL, 0x66, 0x81);
+                vga_wcrt(par->state.vgabase, 0x66, 0x81);
         }
 
-        svga_wcrt_mask(0x31, 0x00, 0x40);
+        if (par->chip == CHIP_357_VIRGE_GX2  ||
+            par->chip == CHIP_359_VIRGE_GX2P ||
+            par->chip == CHIP_360_TRIO3D_1X ||
+            par->chip == CHIP_362_TRIO3D_2X ||
+            par->chip == CHIP_368_TRIO3D_2X)
+                vga_wcrt(par->state.vgabase, 0x34, 0x00);
+        else    /* enable Data Transfer Position Control (DTPC) */
+                vga_wcrt(par->state.vgabase, 0x34, 0x10);
+
+        svga_wcrt_mask(par->state.vgabase, 0x31, 0x00, 0x40);
         multiplex = 0;
         hmul = 1;
 
@@ -604,51 +764,53 @@ static int s3fb_set_par(struct fb_info *info)
         switch (mode) {
         case 0:
                 pr_debug("fb%d: text mode\n", info->node);
-                svga_set_textmode_vga_regs();
+                svga_set_textmode_vga_regs(par->state.vgabase);
 
                 /* Set additional registers like in 8-bit mode */
-                svga_wcrt_mask(0x50, 0x00, 0x30);
-                svga_wcrt_mask(0x67, 0x00, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
                 /* Disable enhanced mode */
-                svga_wcrt_mask(0x3A, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
 
                 if (fasttext) {
                         pr_debug("fb%d: high speed text mode set\n", info->node);
-                        svga_wcrt_mask(0x31, 0x40, 0x40);
+                        svga_wcrt_mask(par->state.vgabase, 0x31, 0x40, 0x40);
                 }
                 break;
         case 1:
                 pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-                vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
+                vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
 
                 /* Set additional registers like in 8-bit mode */
-                svga_wcrt_mask(0x50, 0x00, 0x30);
-                svga_wcrt_mask(0x67, 0x00, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
                 /* disable enhanced mode */
-                svga_wcrt_mask(0x3A, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
                 break;
         case 2:
                 pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
 
                 /* Set additional registers like in 8-bit mode */
-                svga_wcrt_mask(0x50, 0x00, 0x30);
-                svga_wcrt_mask(0x67, 0x00, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
                 /* disable enhanced mode */
-                svga_wcrt_mask(0x3A, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-                svga_wcrt_mask(0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
                 if (info->var.pixclock > 20000 ||
+                    par->chip == CHIP_357_VIRGE_GX2 ||
+                    par->chip == CHIP_359_VIRGE_GX2P ||
                     par->chip == CHIP_360_TRIO3D_1X ||
                     par->chip == CHIP_362_TRIO3D_2X ||
                     par->chip == CHIP_368_TRIO3D_2X)
-                        svga_wcrt_mask(0x67, 0x00, 0xF0);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
                 else {
-                        svga_wcrt_mask(0x67, 0x10, 0xF0);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x10, 0xF0);
                         multiplex = 1;
                 }
                 break;
@@ -656,13 +818,24 @@ static int s3fb_set_par(struct fb_info *info)
                 pr_debug("fb%d: 5/5/5 truecolor\n", info->node);
                 if (par->chip == CHIP_988_VIRGE_VX) {
                         if (info->var.pixclock > 20000)
-                                svga_wcrt_mask(0x67, 0x20, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x20, 0xF0);
                         else
-                                svga_wcrt_mask(0x67, 0x30, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+                } else if (par->chip == CHIP_365_TRIO3D) {
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        if (info->var.pixclock > 8695) {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+                                hmul = 2;
+                        } else {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x20, 0xF0);
+                                multiplex = 1;
+                        }
                 } else {
-                        svga_wcrt_mask(0x50, 0x10, 0x30);
-                        svga_wcrt_mask(0x67, 0x30, 0xF0);
-                        if (par->chip != CHIP_360_TRIO3D_1X &&
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+                        if (par->chip != CHIP_357_VIRGE_GX2 &&
+                            par->chip != CHIP_359_VIRGE_GX2P &&
+                            par->chip != CHIP_360_TRIO3D_1X &&
                             par->chip != CHIP_362_TRIO3D_2X &&
                             par->chip != CHIP_368_TRIO3D_2X)
                                 hmul = 2;
@@ -672,13 +845,24 @@ static int s3fb_set_par(struct fb_info *info)
                 pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
                 if (par->chip == CHIP_988_VIRGE_VX) {
                         if (info->var.pixclock > 20000)
-                                svga_wcrt_mask(0x67, 0x40, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x40, 0xF0);
                         else
-                                svga_wcrt_mask(0x67, 0x50, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+                } else if (par->chip == CHIP_365_TRIO3D) {
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        if (info->var.pixclock > 8695) {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+                                hmul = 2;
+                        } else {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x40, 0xF0);
+                                multiplex = 1;
+                        }
                 } else {
-                        svga_wcrt_mask(0x50, 0x10, 0x30);
-                        svga_wcrt_mask(0x67, 0x50, 0xF0);
-                        if (par->chip != CHIP_360_TRIO3D_1X &&
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+                        if (par->chip != CHIP_357_VIRGE_GX2 &&
+                            par->chip != CHIP_359_VIRGE_GX2P &&
+                            par->chip != CHIP_360_TRIO3D_1X &&
                             par->chip != CHIP_362_TRIO3D_2X &&
                             par->chip != CHIP_368_TRIO3D_2X)
                                 hmul = 2;
@@ -687,12 +871,12 @@ static int s3fb_set_par(struct fb_info *info)
         case 6:
                 /* VIRGE VX case */
                 pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
-                svga_wcrt_mask(0x67, 0xD0, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0xD0, 0xF0);
                 break;
         case 7:
                 pr_debug("fb%d: 8/8/8/8 truecolor\n", info->node);
-                svga_wcrt_mask(0x50, 0x30, 0x30);
-                svga_wcrt_mask(0x67, 0xD0, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x30, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0xD0, 0xF0);
                 break;
         default:
                 printk(KERN_ERR "fb%d: unsupported mode - bug\n", info->node);
@@ -700,25 +884,30 @@ static int s3fb_set_par(struct fb_info *info)
         }
 
         if (par->chip != CHIP_988_VIRGE_VX) {
-                svga_wseq_mask(0x15, multiplex ? 0x10 : 0x00, 0x10);
-                svga_wseq_mask(0x18, multiplex ? 0x80 : 0x00, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x15, multiplex ? 0x10 : 0x00, 0x10);
+                svga_wseq_mask(par->state.vgabase, 0x18, multiplex ? 0x80 : 0x00, 0x80);
         }
 
         s3_set_pixclock(info, info->var.pixclock);
-        svga_set_timings(&s3_timing_regs, &(info->var), hmul, 1,
+        svga_set_timings(par->state.vgabase, &s3_timing_regs, &(info->var), hmul, 1,
                          (info->var.vmode & FB_VMODE_DOUBLE)     ? 2 : 1,
                          (info->var.vmode & FB_VMODE_INTERLACED) ? 2 : 1,
                          hmul, info->node);
 
         /* Set interlaced mode start/end register */
-        value = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
-        value = ((value * hmul) / 8) - 5;
-        vga_wcrt(NULL, 0x3C, (value + 1) / 2);
+        htotal = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
+        htotal = ((htotal * hmul) / 8) - 5;
+        vga_wcrt(par->state.vgabase, 0x3C, (htotal + 1) / 2);
+
+        /* Set Data Transfer Position */
+        hsstart = ((info->var.xres + info->var.right_margin) * hmul) / 8;
+        value = clamp((htotal + hsstart + 1) / 2, hsstart + 4, htotal + 1);
+        svga_wcrt_multi(par->state.vgabase, s3_dtpc_regs, value);
 
         memset_io(info->screen_base, 0x00, screen_size);
         /* Device and screen back on */
-        svga_wcrt_mask(0x17, 0x80, 0x80);
-        svga_wseq_mask(0x01, 0x00, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
         return 0;
 }
@@ -788,31 +977,33 @@ static int s3fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int s3fb_blank(int blank_mode, struct fb_info *info)
 {
+        struct s3fb_info *par = info->par;
+
         switch (blank_mode) {
         case FB_BLANK_UNBLANK:
                 pr_debug("fb%d: unblank\n", info->node);
-                svga_wcrt_mask(0x56, 0x00, 0x06);
-                svga_wseq_mask(0x01, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x00, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
                 break;
         case FB_BLANK_NORMAL:
                 pr_debug("fb%d: blank\n", info->node);
-                svga_wcrt_mask(0x56, 0x00, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x00, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_HSYNC_SUSPEND:
                 pr_debug("fb%d: hsync\n", info->node);
-                svga_wcrt_mask(0x56, 0x02, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x02, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_VSYNC_SUSPEND:
                 pr_debug("fb%d: vsync\n", info->node);
-                svga_wcrt_mask(0x56, 0x04, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x04, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_POWERDOWN:
                 pr_debug("fb%d: sync down\n", info->node);
-                svga_wcrt_mask(0x56, 0x06, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x06, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         }
 
@@ -822,8 +1013,9 @@ static int s3fb_blank(int blank_mode, struct fb_info *info)
 
 /* Pan the display */
 
-static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info) {
-
+static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+        struct s3fb_info *par = info->par;
         unsigned int offset;
 
         /* Calculate the offset */
@@ -837,7 +1029,7 @@ static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
         }
 
         /* Set the offset */
-        svga_wcrt_multi(s3_start_address_regs, offset);
+        svga_wcrt_multi(par->state.vgabase, s3_start_address_regs, offset);
 
         return 0;
 }
@@ -863,12 +1055,14 @@ static struct fb_ops s3fb_ops = {
 
 /* ------------------------------------------------------------------------- */
 
-static int __devinit s3_identification(int chip)
+static int __devinit s3_identification(struct s3fb_info *par)
 {
+        int chip = par->chip;
+
         if (chip == CHIP_XXX_TRIO) {
-                u8 cr30 = vga_rcrt(NULL, 0x30);
-                u8 cr2e = vga_rcrt(NULL, 0x2e);
-                u8 cr2f = vga_rcrt(NULL, 0x2f);
+                u8 cr30 = vga_rcrt(par->state.vgabase, 0x30);
+                u8 cr2e = vga_rcrt(par->state.vgabase, 0x2e);
+                u8 cr2f = vga_rcrt(par->state.vgabase, 0x2f);
 
                 if ((cr30 == 0xE0) || (cr30 == 0xE1)) {
                         if (cr2e == 0x10)
@@ -883,7 +1077,7 @@ static int __devinit s3_identification(int chip)
         }
 
         if (chip == CHIP_XXX_TRIO64V2_DXGX) {
-                u8 cr6f = vga_rcrt(NULL, 0x6f);
+                u8 cr6f = vga_rcrt(par->state.vgabase, 0x6f);
 
                 if (! (cr6f & 0x01))
                         return CHIP_775_TRIO64V2_DX;
@@ -892,7 +1086,7 @@ static int __devinit s3_identification(int chip)
         }
 
         if (chip == CHIP_XXX_VIRGE_DXGX) {
-                u8 cr6f = vga_rcrt(NULL, 0x6f);
+                u8 cr6f = vga_rcrt(par->state.vgabase, 0x6f);
 
                 if (! (cr6f & 0x01))
                         return CHIP_375_VIRGE_DX;
@@ -901,7 +1095,7 @@ static int __devinit s3_identification(int chip)
         }
 
         if (chip == CHIP_36X_TRIO3D_1X_2X) {
-                switch (vga_rcrt(NULL, 0x2f)) {
+                switch (vga_rcrt(par->state.vgabase, 0x2f)) {
                 case 0x00:
                         return CHIP_360_TRIO3D_1X;
                 case 0x01:
@@ -919,10 +1113,13 @@ static int __devinit s3_identification(int chip)
 
 static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+        struct pci_bus_region bus_reg;
+        struct resource vga_res;
         struct fb_info *info;
         struct s3fb_info *par;
         int rc;
         u8 regval, cr38, cr39;
+        bool found = false;
 
         /* Ignore secondary VGA device because there is no VGA arbitration */
         if (! svga_primary_device(dev)) {
@@ -968,47 +1165,68 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
                 goto err_iomap;
         }
 
+        bus_reg.start = 0;
+        bus_reg.end = 64 * 1024;
+
+        vga_res.flags = IORESOURCE_IO;
+
+        pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+        par->state.vgabase = (void __iomem *) vga_res.start;
+
         /* Unlock regs */
-        cr38 = vga_rcrt(NULL, 0x38);
-        cr39 = vga_rcrt(NULL, 0x39);
-        vga_wseq(NULL, 0x08, 0x06);
-        vga_wcrt(NULL, 0x38, 0x48);
-        vga_wcrt(NULL, 0x39, 0xA5);
+        cr38 = vga_rcrt(par->state.vgabase, 0x38);
+        cr39 = vga_rcrt(par->state.vgabase, 0x39);
+        vga_wseq(par->state.vgabase, 0x08, 0x06);
+        vga_wcrt(par->state.vgabase, 0x38, 0x48);
+        vga_wcrt(par->state.vgabase, 0x39, 0xA5);
 
         /* Identify chip type */
         par->chip = id->driver_data & CHIP_MASK;
-        par->rev = vga_rcrt(NULL, 0x2f);
+        par->rev = vga_rcrt(par->state.vgabase, 0x2f);
         if (par->chip & CHIP_UNDECIDED_FLAG)
-                par->chip = s3_identification(par->chip);
+                par->chip = s3_identification(par);
 
         /* Find how many physical memory there is on card */
         /* 0x36 register is accessible even if other registers are locked */
-        regval = vga_rcrt(NULL, 0x36);
+        regval = vga_rcrt(par->state.vgabase, 0x36);
         if (par->chip == CHIP_360_TRIO3D_1X ||
             par->chip == CHIP_362_TRIO3D_2X ||
-            par->chip == CHIP_368_TRIO3D_2X) {
+            par->chip == CHIP_368_TRIO3D_2X ||
+            par->chip == CHIP_365_TRIO3D) {
                 switch ((regval & 0xE0) >> 5) {
                 case 0: /* 8MB -- only 4MB usable for display */
                 case 1: /* 4MB with 32-bit bus */
                 case 2: /* 4MB */
                         info->screen_size = 4 << 20;
                         break;
+                case 4: /* 2MB on 365 Trio3D */
                 case 6: /* 2MB */
                         info->screen_size = 2 << 20;
                         break;
                 }
+        } else if (par->chip == CHIP_357_VIRGE_GX2 ||
+                   par->chip == CHIP_359_VIRGE_GX2P) {
+                switch ((regval & 0xC0) >> 6) {
+                case 1: /* 4MB */
+                        info->screen_size = 4 << 20;
+                        break;
+                case 3: /* 2MB */
+                        info->screen_size = 2 << 20;
+                        break;
+                }
         } else
                 info->screen_size = s3_memsizes[regval >> 5] << 10;
         info->fix.smem_len = info->screen_size;
 
         /* Find MCLK frequency */
-        regval = vga_rseq(NULL, 0x10);
-        par->mclk_freq = ((vga_rseq(NULL, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
+        regval = vga_rseq(par->state.vgabase, 0x10);
+        par->mclk_freq = ((vga_rseq(par->state.vgabase, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
         par->mclk_freq = par->mclk_freq >> (regval >> 5);
 
         /* Restore locks */
-        vga_wcrt(NULL, 0x38, cr38);
-        vga_wcrt(NULL, 0x39, cr39);
+        vga_wcrt(par->state.vgabase, 0x38, cr38);
+        vga_wcrt(par->state.vgabase, 0x39, cr39);
 
         strcpy(info->fix.id, s3_names [par->chip]);
         info->fix.mmio_start = 0;
@@ -1018,12 +1236,77 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
         info->fix.ypanstep = 0;
         info->fix.accel = FB_ACCEL_NONE;
         info->pseudo_palette = (void*) (par->pseudo_palette);
+        info->var.bits_per_pixel = 8;
+
+#ifdef CONFIG_FB_S3_DDC
+        /* Enable MMIO if needed */
+        if (s3fb_ddc_needs_mmio(par->chip)) {
+                par->mmio = ioremap(info->fix.smem_start + MMIO_OFFSET, MMIO_SIZE);
+                if (par->mmio)
+                        svga_wcrt_mask(par->state.vgabase, 0x53, 0x08, 0x08);   /* enable MMIO */
+                else
+                        dev_err(info->device, "unable to map MMIO at 0x%lx, disabling DDC",
+                                info->fix.smem_start + MMIO_OFFSET);
+        }
+        if (!s3fb_ddc_needs_mmio(par->chip) || par->mmio)
+                if (s3fb_setup_ddc_bus(info) == 0) {
+                        u8 *edid = fb_ddc_read(&par->ddc_adapter);
+                        par->ddc_registered = true;
+                        if (edid) {
+                                fb_edid_to_monspecs(edid, &info->monspecs);
+                                kfree(edid);
+                                if (!info->monspecs.modedb)
+                                        dev_err(info->device, "error getting mode database\n");
+                                else {
+                                        const struct fb_videomode *m;
+
+                                        fb_videomode_to_modelist(info->monspecs.modedb,
+                                                                 info->monspecs.modedb_len,
+                                                                 &info->modelist);
+                                        m = fb_find_best_display(&info->monspecs, &info->modelist);
+                                        if (m) {
+                                                fb_videomode_to_var(&info->var, m);
+                                                /* fill all other info->var's fields */
+                                                if (s3fb_check_var(&info->var, info) == 0)
+                                                        found = true;
+                                        }
+                                }
+                        }
+                }
+#endif
+        if (!mode_option && !found)
+                mode_option = "640x480-8@60";
 
         /* Prepare startup mode */
-        rc = fb_find_mode(&(info->var), info, mode_option, NULL, 0, NULL, 8);
-        if (! ((rc == 1) || (rc == 2))) {
-                rc = -EINVAL;
-                dev_err(info->device, "mode %s not found\n", mode_option);
+        if (mode_option) {
+                rc = fb_find_mode(&info->var, info, mode_option,
+                                   info->monspecs.modedb, info->monspecs.modedb_len,
+                                   NULL, info->var.bits_per_pixel);
+                if (!rc || rc == 4) {
+                        rc = -EINVAL;
+                        dev_err(info->device, "mode %s not found\n", mode_option);
+                        fb_destroy_modedb(info->monspecs.modedb);
+                        info->monspecs.modedb = NULL;
+                        goto err_find_mode;
+                }
+        }
+
+        fb_destroy_modedb(info->monspecs.modedb);
+        info->monspecs.modedb = NULL;
+
+        /* maximize virtual vertical size for fast scrolling */
+        info->var.yres_virtual = info->fix.smem_len * 8 /
+                        (info->var.bits_per_pixel * info->var.xres_virtual);
+        if (info->var.yres_virtual < info->var.yres) {
+                dev_err(info->device, "virtual vertical size smaller than real\n");
+                goto err_find_mode;
+        }
+
+        /* maximize virtual vertical size for fast scrolling */
+        info->var.yres_virtual = info->fix.smem_len * 8 /
+                        (info->var.bits_per_pixel * info->var.xres_virtual);
+        if (info->var.yres_virtual < info->var.yres) {
+                dev_err(info->device, "virtual vertical size smaller than real\n");
                 goto err_find_mode;
         }
 
@@ -1044,8 +1327,8 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
 
         if (par->chip == CHIP_UNKNOWN)
                 printk(KERN_INFO "fb%d: unknown chip, CR2D=%x, CR2E=%x, CRT2F=%x, CRT30=%x\n",
-                        info->node, vga_rcrt(NULL, 0x2d), vga_rcrt(NULL, 0x2e),
-                        vga_rcrt(NULL, 0x2f), vga_rcrt(NULL, 0x30));
+                        info->node, vga_rcrt(par->state.vgabase, 0x2d), vga_rcrt(par->state.vgabase, 0x2e),
+                        vga_rcrt(par->state.vgabase, 0x2f), vga_rcrt(par->state.vgabase, 0x30));
 
         /* Record a reference to the driver data */
         pci_set_drvdata(dev, info);
@@ -1064,6 +1347,12 @@ err_reg_fb:
         fb_dealloc_cmap(&info->cmap);
 err_alloc_cmap:
 err_find_mode:
+#ifdef CONFIG_FB_S3_DDC
+        if (par->ddc_registered)
+                i2c_del_adapter(&par->ddc_adapter);
+        if (par->mmio)
+                iounmap(par->mmio);
+#endif
         pci_iounmap(dev, info->screen_base);
 err_iomap:
         pci_release_regions(dev);
@@ -1080,12 +1369,11 @@ err_enable_device:
 static void __devexit s3_pci_remove(struct pci_dev *dev)
 {
         struct fb_info *info = pci_get_drvdata(dev);
+        struct s3fb_info __maybe_unused *par = info->par;
 
         if (info) {
 
 #ifdef CONFIG_MTRR
-                struct s3fb_info *par = info->par;
-
                 if (par->mtrr_reg >= 0) {
                         mtrr_del(par->mtrr_reg, 0, 0);
                         par->mtrr_reg = -1;
@@ -1095,6 +1383,13 @@ static void __devexit s3_pci_remove(struct pci_dev *dev)
                 unregister_framebuffer(info);
                 fb_dealloc_cmap(&info->cmap);
 
+#ifdef CONFIG_FB_S3_DDC
+                if (par->ddc_registered)
+                        i2c_del_adapter(&par->ddc_adapter);
+                if (par->mmio)
+                        iounmap(par->mmio);
+#endif
+
                 pci_iounmap(dev, info->screen_base);
                 pci_release_regions(dev);
 /*              pci_disable_device(dev); */
@@ -1113,12 +1408,12 @@ static int s3_pci_suspend(struct pci_dev* dev, pm_message_t state)
 
         dev_info(info->device, "suspend\n");
 
-        acquire_console_sem();
+        console_lock();
         mutex_lock(&(par->open_lock));
 
         if ((state.event == PM_EVENT_FREEZE) || (par->ref_count == 0)) {
                 mutex_unlock(&(par->open_lock));
-                release_console_sem();
+                console_unlock();
                 return 0;
         }
 
@@ -1129,7 +1424,7 @@ static int s3_pci_suspend(struct pci_dev* dev, pm_message_t state)
         pci_set_power_state(dev, pci_choose_state(dev, state));
 
         mutex_unlock(&(par->open_lock));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1145,12 +1440,12 @@ static int s3_pci_resume(struct pci_dev* dev)
 
         dev_info(info->device, "resume\n");
 
-        acquire_console_sem();
+        console_lock();
         mutex_lock(&(par->open_lock));
 
         if (par->ref_count == 0) {
                 mutex_unlock(&(par->open_lock));
-                release_console_sem();
+                console_unlock();
                 return 0;
         }
 
@@ -1159,7 +1454,7 @@ static int s3_pci_resume(struct pci_dev* dev)
         err = pci_enable_device(dev);
         if (err) {
                 mutex_unlock(&(par->open_lock));
-                release_console_sem();
+                console_unlock();
                 dev_err(info->device, "error %d enabling device for resume\n", err);
                 return err;
         }
@@ -1169,7 +1464,7 @@ static int s3_pci_resume(struct pci_dev* dev)
         fb_set_suspend(info, 0);
 
         mutex_unlock(&(par->open_lock));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -1188,10 +1483,11 @@ static struct pci_device_id s3_devices[] __devinitdata = {
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x5631), .driver_data = CHIP_325_VIRGE},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x883D), .driver_data = CHIP_988_VIRGE_VX},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A01), .driver_data = CHIP_XXX_VIRGE_DXGX},
-        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A10), .driver_data = CHIP_356_VIRGE_GX2},
-        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A11), .driver_data = CHIP_357_VIRGE_GX2P},
+        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A10), .driver_data = CHIP_357_VIRGE_GX2},
+        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A11), .driver_data = CHIP_359_VIRGE_GX2P},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A12), .driver_data = CHIP_359_VIRGE_GX2P},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A13), .driver_data = CHIP_36X_TRIO3D_1X_2X},
+        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8904), .driver_data = CHIP_365_TRIO3D},
 
         {0, 0, 0, 0, 0, 0, 0}
 };
diff --git a/drivers/video/savage/savagefb-i2c.c b/drivers/video/savage/savagefb-i2c.c
index ed371c868b3a..80fa87e2ae2f 100644
--- a/drivers/video/savage/savagefb-i2c.c
+++ b/drivers/video/savage/savagefb-i2c.c
@@ -159,8 +159,7 @@ static int savage_setup_i2c_bus(struct savagefb_i2c_chan *chan,
                 else
                         dev_warn(&chan->par->pcidev->dev,
                                  "Failed to register I2C bus %s.\n", name);
-        } else
-                chan->par = NULL;
+        }
 
         return rc;
 }
@@ -170,9 +169,10 @@ void savagefb_create_i2c_busses(struct fb_info *info)
         struct savagefb_par *par = info->par;
         par->chan.par   = par;
 
-        switch(info->fix.accel) {
-        case FB_ACCEL_PROSAVAGE_DDRK:
-        case FB_ACCEL_PROSAVAGE_PM:
+        switch (par->chip) {
+        case S3_PROSAVAGE:
+        case S3_PROSAVAGEDDR:
+        case S3_TWISTER:
                 par->chan.reg         = CR_SERIAL2;
                 par->chan.ioaddr      = par->mmio.vbase;
                 par->chan.algo.setsda = prosavage_gpio_setsda;
@@ -180,9 +180,18 @@ void savagefb_create_i2c_busses(struct fb_info *info)
                 par->chan.algo.getsda = prosavage_gpio_getsda;
                 par->chan.algo.getscl = prosavage_gpio_getscl;
                 break;
-        case FB_ACCEL_SAVAGE4:
-        case FB_ACCEL_SAVAGE2000:
-                par->chan.reg         = 0xff20;
+        case S3_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 S3_SAVAGE2000:
+                par->chan.reg         = MM_SERIAL1;
                 par->chan.ioaddr      = par->mmio.vbase;
                 par->chan.algo.setsda = savage4_gpio_setsda;
                 par->chan.algo.setscl = savage4_gpio_setscl;
diff --git a/drivers/video/savage/savagefb.h b/drivers/video/savage/savagefb.h
index 8bfdfc3c5234..32549d177b19 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>
@@ -37,7 +36,6 @@
 #define PCI_CHIP_SAVAGE_IX    0x8c13
 #define PCI_CHIP_PROSAVAGE_PM 0x8a25
 #define PCI_CHIP_PROSAVAGE_KM 0x8a26
- /* Twister is a code name; hope I get the real name soon. */
 #define PCI_CHIP_S3TWISTER_P  0x8d01
 #define PCI_CHIP_S3TWISTER_K  0x8d02
 #define PCI_CHIP_PROSAVAGE_DDR          0x8d03
@@ -53,14 +51,15 @@
 #define PCI_CHIP_SUPSAV_IXCDDR          0x8c2f
 
 
+#define S3_SAVAGE_SERIES(chip)    ((chip>=S3_SAVAGE3D) && (chip<=S3_SAVAGE2000))
 
 #define S3_SAVAGE3D_SERIES(chip)  ((chip>=S3_SAVAGE3D) && (chip<=S3_SAVAGE_MX))
 
-#define S3_SAVAGE4_SERIES(chip)   ((chip==S3_SAVAGE4) || (chip==S3_PROSAVAGE))
+#define S3_SAVAGE4_SERIES(chip)   ((chip>=S3_SAVAGE4) || (chip<=S3_PROSAVAGEDDR))
 
 #define S3_SAVAGE_MOBILE_SERIES(chip)  ((chip==S3_SAVAGE_MX) || (chip==S3_SUPERSAVAGE))
 
-#define S3_SAVAGE_SERIES(chip)    ((chip>=S3_SAVAGE3D) && (chip<=S3_SAVAGE2000))
+#define S3_MOBILE_TWISTER_SERIES(chip) ((chip==S3_TWISTER) || (chip==S3_PROSAVAGEDDR))
 
 /* Chip tags.  These are used to group the adapters into
  * related families.
@@ -72,6 +71,8 @@ typedef enum {
   S3_SAVAGE_MX,
   S3_SAVAGE4,
   S3_PROSAVAGE,
+  S3_TWISTER,
+  S3_PROSAVAGEDDR,
   S3_SUPERSAVAGE,
   S3_SAVAGE2000,
   S3_LAST
@@ -154,7 +155,7 @@ struct savage_reg {
         unsigned char CRTC[25];       /* Crtc Controller */
         unsigned char Sequencer[5];   /* Video Sequencer */
         unsigned char Graphics[9];    /* Video Graphics */
-        unsigned char Attribute[21];  /* Video Atribute */
+        unsigned char Attribute[21];  /* Video Attribute */
 
         unsigned int mode, refresh;
         unsigned char SR08, SR0E, SR0F;
diff --git a/drivers/video/savage/savagefb_driver.c b/drivers/video/savage/savagefb_driver.c
index 842d157e1025..4de541ca9c52 100644
--- a/drivers/video/savage/savagefb_driver.c
+++ b/drivers/video/savage/savagefb_driver.c
@@ -328,7 +328,9 @@ SavageSetup2DEngine(struct savagefb_par  *par)
                 savage_out32(0x48C18, savage_in32(0x48C18, par) | 0x0C, par);
                 break;
         case S3_SAVAGE4:
+        case S3_TWISTER:
         case S3_PROSAVAGE:
+        case S3_PROSAVAGEDDR:
         case S3_SUPERSAVAGE:
                 /* Disable BCI */
                 savage_out32(0x48C18, savage_in32(0x48C18, par) & 0x3FF0, par);
@@ -385,7 +387,7 @@ SavageSetup2DEngine(struct savagefb_par  *par)
         BCI_SEND(GlobalBitmapDescriptor);
 
         /*
-         * I don't know why, sending this twice fixes the intial black screen,
+         * I don't know why, sending this twice fixes the initial black screen,
          * prevents X from crashing at least in Toshiba laptops with SavageIX.
          * --Tony
          */
@@ -1886,6 +1888,8 @@ static int savage_init_hw(struct savagefb_par *par)
                 break;
 
         case S3_PROSAVAGE:
+        case S3_PROSAVAGEDDR:
+        case S3_TWISTER:
                 videoRam = RamSavageNB[(config1 & 0xE0) >> 5] * 1024;
                 break;
 
@@ -1963,7 +1967,8 @@ static int savage_init_hw(struct savagefb_par *par)
                 }
         }
 
-        if (S3_SAVAGE_MOBILE_SERIES(par->chip) && !par->crtonly)
+        if ((S3_SAVAGE_MOBILE_SERIES(par->chip) ||
+             S3_MOBILE_TWISTER_SERIES(par->chip)) && !par->crtonly)
                 par->display_type = DISP_LCD;
         else if (dvi || (par->chip == S3_SAVAGE4 && par->dvi))
                 par->display_type = DISP_DFP;
@@ -2111,19 +2116,19 @@ static int __devinit savage_init_fb_info(struct fb_info *info,
                 snprintf(info->fix.id, 16, "ProSavageKM");
                 break;
         case FB_ACCEL_S3TWISTER_P:
-                par->chip = S3_PROSAVAGE;
+                par->chip = S3_TWISTER;
                 snprintf(info->fix.id, 16, "TwisterP");
                 break;
         case FB_ACCEL_S3TWISTER_K:
-                par->chip = S3_PROSAVAGE;
+                par->chip = S3_TWISTER;
                 snprintf(info->fix.id, 16, "TwisterK");
                 break;
         case FB_ACCEL_PROSAVAGE_DDR:
-                par->chip = S3_PROSAVAGE;
+                par->chip = S3_PROSAVAGEDDR;
                 snprintf(info->fix.id, 16, "ProSavageDDR");
                 break;
         case FB_ACCEL_PROSAVAGE_DDRK:
-                par->chip = S3_PROSAVAGE;
+                par->chip = S3_PROSAVAGEDDR;
                 snprintf(info->fix.id, 16, "ProSavage8");
                 break;
         }
@@ -2211,7 +2216,7 @@ static int __devinit savagefb_probe(struct pci_dev* dev,
                 goto failed_mmio;
 
         video_len = savage_init_hw(par);
-        /* FIXME: cant be negative */
+        /* FIXME: can't be negative */
         if (video_len < 0) {
                 err = video_len;
                 goto failed_mmio;
@@ -2232,6 +2237,22 @@ static int __devinit savagefb_probe(struct pci_dev* dev,
                                  &info->modelist);
 #endif
         info->var = savagefb_var800x600x8;
+        /* if a panel was detected, default to a CVT mode instead */
+        if (par->SavagePanelWidth) {
+                struct fb_videomode cvt_mode;
+
+                memset(&cvt_mode, 0, sizeof(cvt_mode));
+                cvt_mode.xres = par->SavagePanelWidth;
+                cvt_mode.yres = par->SavagePanelHeight;
+                cvt_mode.refresh = 60;
+                /* FIXME: if we know there is only the panel
+                 * we can enable reduced blanking as well */
+                if (fb_find_mode_cvt(&cvt_mode, 0, 0))
+                        printk(KERN_WARNING "No CVT mode found for panel\n");
+                else if (fb_find_mode(&info->var, info, NULL, NULL, 0,
+                                      &cvt_mode, 0) != 3)
+                        info->var = savagefb_var800x600x8;
+        }
 
         if (mode_option) {
                 fb_find_mode(&info->var, info, mode_option,
@@ -2373,7 +2394,7 @@ static int savagefb_suspend(struct pci_dev *dev, pm_message_t mesg)
         if (mesg.event == PM_EVENT_FREEZE)
                 return 0;
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(info, 1);
 
         if (info->fbops->fb_sync)
@@ -2385,7 +2406,7 @@ static int savagefb_suspend(struct pci_dev *dev, pm_message_t mesg)
         pci_save_state(dev);
         pci_disable_device(dev);
         pci_set_power_state(dev, pci_choose_state(dev, mesg));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -2409,7 +2430,7 @@ static int savagefb_resume(struct pci_dev* dev)
                 return 0;
         }
 
-        acquire_console_sem();
+        console_lock();
 
         pci_set_power_state(dev, PCI_D0);
         pci_restore_state(dev);
@@ -2423,7 +2444,7 @@ static int savagefb_resume(struct pci_dev* dev)
         savagefb_set_par(info);
         fb_set_suspend(info, 0);
         savagefb_blank(FB_BLANK_UNBLANK, info);
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
diff --git a/drivers/video/sh7760fb.c b/drivers/video/sh7760fb.c
index bea38fce2470..45e47d847163 100644
--- a/drivers/video/sh7760fb.c
+++ b/drivers/video/sh7760fb.c
@@ -459,14 +459,14 @@ static int __devinit sh7760fb_probe(struct platform_device *pdev)
         }
 
         par->ioarea = request_mem_region(res->start,
-                                         (res->end - res->start), pdev->name);
+                                         resource_size(res), pdev->name);
         if (!par->ioarea) {
                 dev_err(&pdev->dev, "mmio area busy\n");
                 ret = -EBUSY;
                 goto out_fb;
         }
 
-        par->base = ioremap_nocache(res->start, res->end - res->start + 1);
+        par->base = ioremap_nocache(res->start, resource_size(res));
         if (!par->base) {
                 dev_err(&pdev->dev, "cannot remap\n");
                 ret = -ENODEV;
@@ -551,8 +551,7 @@ out_unmap:
                 free_irq(par->irq, &par->vsync);
         iounmap(par->base);
 out_res:
-        release_resource(par->ioarea);
-        kfree(par->ioarea);
+        release_mem_region(res->start, resource_size(res));
 out_fb:
         framebuffer_release(info);
         return ret;
@@ -570,8 +569,7 @@ static int __devexit sh7760fb_remove(struct platform_device *dev)
         if (par->irq >= 0)
                 free_irq(par->irq, par);
         iounmap(par->base);
-        release_resource(par->ioarea);
-        kfree(par->ioarea);
+        release_mem_region(par->ioarea->start, resource_size(par->ioarea));
         framebuffer_release(info);
         platform_set_drvdata(dev, NULL);
 
diff --git a/drivers/video/sh_mipi_dsi.c b/drivers/video/sh_mipi_dsi.c
index 5699ce0c1780..24640c8458ab 100644
--- a/drivers/video/sh_mipi_dsi.c
+++ b/drivers/video/sh_mipi_dsi.c
@@ -13,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/types.h>
@@ -21,18 +22,40 @@
 #include <video/sh_mipi_dsi.h>
 #include <video/sh_mobile_lcdc.h>
 
-#define CMTSRTCTR       0x80d0
-#define CMTSRTREQ       0x8070
-
+#define SYSCTRL         0x0000
+#define SYSCONF         0x0004
+#define TIMSET          0x0008
+#define RESREQSET0      0x0018
+#define RESREQSET1      0x001c
+#define HSTTOVSET       0x0020
+#define LPRTOVSET       0x0024
+#define TATOVSET        0x0028
+#define PRTOVSET        0x002c
+#define DSICTRL         0x0030
 #define DSIINTE         0x0060
+#define PHYCTRL         0x0070
+
+/* relative to linkbase */
+#define DTCTR           0x0000
+#define VMCTR1          0x0020
+#define VMCTR2          0x0024
+#define VMLEN1          0x0028
+#define CMTSRTREQ       0x0070
+#define CMTSRTCTR       0x00d0
 
 /* E.g., sh7372 has 2 MIPI-DSIs - one for each LCDC */
 #define MAX_SH_MIPI_DSI 2
 
 struct sh_mipi {
         void __iomem    *base;
+        void __iomem    *linkbase;
         struct clk      *dsit_clk;
         struct clk      *dsip_clk;
+        struct device   *dev;
+
+        void    *next_board_data;
+        void    (*next_display_on)(void *board_data, struct fb_info *info);
+        void    (*next_display_off)(void *board_data);
 };
 
 static struct sh_mipi *mipi_dsi[MAX_SH_MIPI_DSI];
@@ -55,10 +78,10 @@ static int sh_mipi_send_short(struct sh_mipi *mipi, u8 dsi_cmd,
         int cnt = 100;
 
         /* transmit a short packet to LCD panel */
-        iowrite32(1 | data, mipi->base + 0x80d0); /* CMTSRTCTR */
-        iowrite32(1, mipi->base + 0x8070); /* CMTSRTREQ */
+        iowrite32(1 | data, mipi->linkbase + CMTSRTCTR);
+        iowrite32(1, mipi->linkbase + CMTSRTREQ);
 
-        while ((ioread32(mipi->base + 0x8070) & 1) && --cnt)
+        while ((ioread32(mipi->linkbase + CMTSRTREQ) & 1) && --cnt)
                 udelay(1);
 
         return cnt ? 0 : -ETIMEDOUT;
@@ -90,7 +113,7 @@ 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 */
+        iowrite32(0x00000002 | enable, mipi->linkbase + DTCTR);
 }
 
 static void sh_mipi_shutdown(struct platform_device *pdev)
@@ -104,14 +127,22 @@ static void mipi_display_on(void *arg, struct fb_info *info)
 {
         struct sh_mipi *mipi = arg;
 
+        pm_runtime_get_sync(mipi->dev);
         sh_mipi_dsi_enable(mipi, true);
+
+        if (mipi->next_display_on)
+                mipi->next_display_on(mipi->next_board_data, info);
 }
 
 static void mipi_display_off(void *arg)
 {
         struct sh_mipi *mipi = arg;
 
+        if (mipi->next_display_off)
+                mipi->next_display_off(mipi->next_board_data);
+
         sh_mipi_dsi_enable(mipi, false);
+        pm_runtime_put(mipi->dev);
 }
 
 static int __init sh_mipi_setup(struct sh_mipi *mipi,
@@ -119,87 +150,90 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
 {
         void __iomem *base = mipi->base;
         struct sh_mobile_lcdc_chan_cfg *ch = pdata->lcd_chan;
-        u32 pctype, datatype, pixfmt;
-        u32 linelength;
+        u32 pctype, datatype, pixfmt, linelength, vmctr2 = 0x00e00000;
         bool yuv;
 
-        /* Select data format */
+        /*
+         * Select data format. MIPI DSI is not hot-pluggable, so, we just use
+         * the default videomode. If this ever becomes a problem, We'll have to
+         * move this to mipi_display_on() above and use info->var.xres
+         */
         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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = (ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = (ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = ch->lcd_cfg[0].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;
+                linelength = (ch->lcd_cfg[0].xres * 12 + 7) / 8;
                 yuv = true;
                 break;
         case MIPI_YUV420:
@@ -207,7 +241,7 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
                 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;
+                linelength = (ch->lcd_cfg[0].xres + 1) / 2;
                 yuv = true;
                 break;
         default:
@@ -219,10 +253,10 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
                 return -EINVAL;
 
         /* reset DSI link */
-        iowrite32(0x00000001, base); /* SYSCTRL */
+        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 */
+        iowrite32(0x00000000, base + SYSCTRL);
 
         /* setup DSI link */
 
@@ -234,7 +268,7 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
          *      ECC check enable
          * additionally enable first two lanes
          */
-        iowrite32(0x00003703, base + 0x04); /* SYSCONF */
+        iowrite32(0x00003703, base + SYSCONF);
         /*
          * T_wakeup = 0x7000
          * T_hs-trail = 3
@@ -242,28 +276,28 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
          * T_clk-trail = 3
          * T_clk-prepare = 2
          */
-        iowrite32(0x70003332, base + 0x08); /* TIMSET */
+        iowrite32(0x70003332, base + TIMSET);
         /* no responses requested */
-        iowrite32(0x00000000, base + 0x18); /* RESREQSET0 */
+        iowrite32(0x00000000, base + RESREQSET0);
         /* request response to packets of type 0x28 */
-        iowrite32(0x00000100, base + 0x1c); /* RESREQSET1 */
+        iowrite32(0x00000100, base + RESREQSET1);
         /* High-speed transmission timeout, default 0xffffffff */
-        iowrite32(0x0fffffff, base + 0x20); /* HSTTOVSET */
+        iowrite32(0x0fffffff, base + HSTTOVSET);
         /* LP reception timeout, default 0xffffffff */
-        iowrite32(0x0fffffff, base + 0x24); /* LPRTOVSET */
+        iowrite32(0x0fffffff, base + LPRTOVSET);
         /* Turn-around timeout, default 0xffffffff */
-        iowrite32(0x0fffffff, base + 0x28); /* TATOVSET */
+        iowrite32(0x0fffffff, base + TATOVSET);
         /* Peripheral reset timeout, default 0xffffffff */
-        iowrite32(0x0fffffff, base + 0x2c); /* PRTOVSET */
+        iowrite32(0x0fffffff, base + PRTOVSET);
         /* Enable timeout counters */
-        iowrite32(0x00000f00, base + 0x30); /* DSICTRL */
+        iowrite32(0x00000f00, base + DSICTRL);
         /* Interrupts not used, disable all */
         iowrite32(0, base + DSIINTE);
         /* DSI-Tx bias on */
-        iowrite32(0x00000001, base + 0x70); /* PHYCTRL */
+        iowrite32(0x00000001, base + PHYCTRL);
         udelay(200);
         /* Deassert resets, power on, set multiplier */
-        iowrite32(0x03070b01, base + 0x70); /* PHYCTRL */
+        iowrite32(0x03070b01, base + PHYCTRL);
 
         /* setup l-bridge */
 
@@ -271,20 +305,28 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
          * Enable transmission of all packets,
          * transmit LPS after each HS packet completion
          */
-        iowrite32(0x00000006, base + 0x8000); /* DTCTR */
+        iowrite32(0x00000006, mipi->linkbase + DTCTR);
         /* VSYNC width = 2 (<< 17) */
-        iowrite32(0x00040000 | (pctype << 12) | datatype, base + 0x8020); /* VMCTR1 */
+        iowrite32((ch->lcd_cfg[0].vsync_len << pdata->vsynw_offset) |
+                  (pdata->clksrc << 16) | (pctype << 12) | datatype,
+                  mipi->linkbase + VMCTR1);
+
         /*
          * Non-burst mode with sync pulses: VSE and HSE are output,
          * HSA period allowed, no commands in LP
          */
-        iowrite32(0x00e00000, base + 0x8024); /* VMCTR2 */
+        if (pdata->flags & SH_MIPI_DSI_HSABM)
+                vmctr2 |= 0x20;
+        if (pdata->flags & SH_MIPI_DSI_HSPBM)
+                vmctr2 |= 0x10;
+        iowrite32(vmctr2, mipi->linkbase + 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)
+         * sh_mobile_lcdc_info.ch[0].lcd_cfg[0].xres), HSALEN = 1 - default
+         * (unused if VMCTR2[HSABM] = 0)
          */
-        iowrite32(1 | (linelength << 16), base + 0x8028); /* VMLEN1 */
+        iowrite32(1 | (linelength << 16), mipi->linkbase + VMLEN1);
 
         msleep(5);
 
@@ -317,11 +359,12 @@ 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);
+        struct resource *res2 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
         unsigned long rate, f_current;
         int idx = pdev->id, ret;
         char dsip_clk[] = "dsi.p_clk";
 
-        if (!res || idx >= ARRAY_SIZE(mipi_dsi) || !pdata)
+        if (!res || !res2 || idx >= ARRAY_SIZE(mipi_dsi) || !pdata)
                 return -ENODEV;
 
         mutex_lock(&array_lock);
@@ -352,6 +395,20 @@ static int __init sh_mipi_probe(struct platform_device *pdev)
                 goto emap;
         }
 
+        if (!request_mem_region(res2->start, resource_size(res2), pdev->name)) {
+                dev_err(&pdev->dev, "MIPI register region 2 already claimed\n");
+                ret = -EBUSY;
+                goto ereqreg2;
+        }
+
+        mipi->linkbase = ioremap(res2->start, resource_size(res2));
+        if (!mipi->linkbase) {
+                ret = -ENOMEM;
+                goto emap2;
+        }
+
+        mipi->dev = &pdev->dev;
+
         mipi->dsit_clk = clk_get(&pdev->dev, "dsit_clk");
         if (IS_ERR(mipi->dsit_clk)) {
                 ret = PTR_ERR(mipi->dsit_clk);
@@ -401,6 +458,9 @@ static int __init sh_mipi_probe(struct platform_device *pdev)
 
         mipi_dsi[idx] = mipi;
 
+        pm_runtime_enable(&pdev->dev);
+        pm_runtime_resume(&pdev->dev);
+
         ret = sh_mipi_setup(mipi, pdata);
         if (ret < 0)
                 goto emipisetup;
@@ -408,15 +468,22 @@ static int __init sh_mipi_probe(struct platform_device *pdev)
         mutex_unlock(&array_lock);
         platform_set_drvdata(pdev, mipi);
 
+        /* Save original LCDC callbacks */
+        mipi->next_board_data = pdata->lcd_chan->board_cfg.board_data;
+        mipi->next_display_on = pdata->lcd_chan->board_cfg.display_on;
+        mipi->next_display_off = pdata->lcd_chan->board_cfg.display_off;
+
         /* 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;
+        pdata->lcd_chan->board_cfg.owner = THIS_MODULE;
 
         return 0;
 
 emipisetup:
         mipi_dsi[idx] = NULL;
+        pm_runtime_disable(&pdev->dev);
         clk_disable(mipi->dsip_clk);
 eclkpon:
         clk_disable(mipi->dsit_clk);
@@ -427,6 +494,10 @@ eclkpget:
 esettrate:
         clk_put(mipi->dsit_clk);
 eclktget:
+        iounmap(mipi->linkbase);
+emap2:
+        release_mem_region(res2->start, resource_size(res2));
+ereqreg2:
         iounmap(mipi->base);
 emap:
         release_mem_region(res->start, resource_size(res));
@@ -443,6 +514,7 @@ 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 resource *res2 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
         struct sh_mipi *mipi = platform_get_drvdata(pdev);
         int i, ret;
 
@@ -463,14 +535,19 @@ static int __exit sh_mipi_remove(struct platform_device *pdev)
         if (ret < 0)
                 return ret;
 
+        pdata->lcd_chan->board_cfg.owner = NULL;
         pdata->lcd_chan->board_cfg.display_on = NULL;
         pdata->lcd_chan->board_cfg.display_off = NULL;
         pdata->lcd_chan->board_cfg.board_data = NULL;
 
+        pm_runtime_disable(&pdev->dev);
         clk_disable(mipi->dsip_clk);
         clk_disable(mipi->dsit_clk);
         clk_put(mipi->dsit_clk);
         clk_put(mipi->dsip_clk);
+        iounmap(mipi->linkbase);
+        if (res2)
+                release_mem_region(res2->start, resource_size(res2));
         iounmap(mipi->base);
         if (res)
                 release_mem_region(res->start, resource_size(res));
diff --git a/drivers/video/sh_mobile_hdmi.c b/drivers/video/sh_mobile_hdmi.c
index 2fde08cc66bf..7d54e2c612f7 100644
--- a/drivers/video/sh_mobile_hdmi.c
+++ b/drivers/video/sh_mobile_hdmi.c
@@ -22,10 +22,15 @@
 #include <linux/slab.h>
 #include <linux/types.h>
 #include <linux/workqueue.h>
+#include <sound/soc.h>
+#include <sound/soc-dapm.h>
+#include <sound/initval.h>
 
 #include <video/sh_mobile_hdmi.h>
 #include <video/sh_mobile_lcdc.h>
 
+#include "sh_mobile_lcdcfb.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 */
@@ -204,12 +209,20 @@ enum hotplug_state {
 
 struct sh_hdmi {
         void __iomem *base;
-        enum hotplug_state hp_state;
+        enum hotplug_state hp_state;    /* hot-plug status */
+        u8 preprogrammed_vic;           /* use a pre-programmed VIC or
+                                           the external mode */
+        u8 edid_block_addr;
+        u8 edid_segment_nr;
+        u8 edid_blocks;
         struct clk *hdmi_clk;
         struct device *dev;
         struct fb_info *info;
+        struct mutex mutex;             /* Protect the info pointer */
         struct delayed_work edid_work;
         struct fb_var_screeninfo var;
+        struct fb_monspecs monspec;
+        struct notifier_block notifier;
 };
 
 static void hdmi_write(struct sh_hdmi *hdmi, u8 data, u8 reg)
@@ -222,8 +235,60 @@ 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)
+static void sh_hdmi_external_video_param(struct sh_hdmi *hdmi)
 {
         struct fb_var_screeninfo *var = &hdmi->var;
         u16 htotal, hblank, hdelay, vtotal, vblank, vdelay, voffset;
@@ -255,9 +320,9 @@ static void hdmi_external_video_param(struct sh_hdmi *hdmi)
         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);
+        dev_dbg(hdmi->dev, "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);
 
@@ -282,7 +347,10 @@ static void hdmi_external_video_param(struct sh_hdmi *hdmi)
 
         hdmi_write(hdmi, var->vsync_len, HDMI_EXTERNAL_V_DURATION);
 
-        /* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for manual mode */
+        /* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for external mode */
+        if (!hdmi->preprogrammed_vic)
+                hdmi_write(hdmi, sync | 1 | (voffset << 4),
+                           HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
 }
 
 /**
@@ -318,6 +386,9 @@ static void sh_hdmi_video_config(struct sh_hdmi *hdmi)
  */
 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]
@@ -335,7 +406,23 @@ static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
          * [6:5] set required down sampling rate if required
          * [4:3] set required audio source
          */
-        hdmi_write(hdmi, 0x00, HDMI_AUDIO_SETTING_1);
+        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);
@@ -381,21 +468,72 @@ static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
 }
 
 /**
- * sh_hdmi_phy_config()
+ * sh_hdmi_phy_config() - configure the HDMI PHY for the used video mode
  */
 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);
+        if (hdmi->var.pixclock < 10000) {
+                /* for 1080p8bit 148MHz */
+                hdmi_write(hdmi, 0x1d, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
+                hdmi_write(hdmi, 0x4c, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+                hdmi_write(hdmi, 0x1e, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+                hdmi_write(hdmi, 0x48, 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);
+        } else if (hdmi->var.pixclock < 30000) {
+                /* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
+                /*
+                 * [1:0]        Speed_A
+                 * [3:2]        Speed_B
+                 * [4]          PLLA_Bypass
+                 * [6]          DRV_TEST_EN
+                 * [7]          DRV_TEST_IN
+                 */
+                hdmi_write(hdmi, 0x0f, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+                /* PLLB_CONFIG[17], PLLA_CONFIG[17] - not in PHY datasheet */
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+                /*
+                 * [2:0]        BGR_I_OFFSET
+                 * [6:4]        BGR_V_OFFSET
+                 */
+                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);
+                /*
+                 * PLLA_CONFIG[15:8]: regulator voltage[0], CP current,
+                 * LPF capacitance, LPF resistance[1]
+                 */
+                hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+                /* PLLB_CONFIG[7:0]: LPF resistance[0], VCO offset, VCO gain */
+                hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+                /*
+                 * PLLB_CONFIG[15:8]: regulator voltage[0], CP current,
+                 * LPF capacitance, LPF resistance[1]
+                 */
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+                /* DRV_CONFIG, PE_CONFIG */
+                hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+                /*
+                 * [2:0]        AMON_SEL (4 == LPF voltage)
+                 * [4]          PLLA_CONFIG[16]
+                 * [5]          PLLB_CONFIG[16]
+                 */
+                hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+        } else {
+                /* for 480p8bit 27MHz */
+                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);
+                hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+                hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+                hdmi_write(hdmi, 0x48, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+                hdmi_write(hdmi, 0x0F, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+                hdmi_write(hdmi, 0x20, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+                hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+        }
 }
 
 /**
@@ -403,6 +541,8 @@ static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
  */
 static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
 {
+        u8 vic;
+
         /* AVI InfoFrame */
         hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_INDEX);
 
@@ -427,9 +567,9 @@ static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
         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
+         * [7:6] C = Colorimetry: no data
+         * [5:4] M = 2: 16:9, 1: 4:3 Picture Aspect Ratio
+         * [3:0] R = 8: Active Frame Aspect Ratio: same as picture aspect ratio
          */
         hdmi_write(hdmi, 0x28, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
 
@@ -442,10 +582,14 @@ static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
         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
+         * VIC should be ignored if external config is used, so, we could just use 0,
+         * but play safe and use a valid value in any case just in case
          */
-        hdmi_write(hdmi, 4, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
+        if (hdmi->preprogrammed_vic)
+                vic = hdmi->preprogrammed_vic;
+        else
+                vic = 4;
+        hdmi_write(hdmi, vic, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
 
         /* PR = No Repetition */
         hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
@@ -519,100 +663,6 @@ static void sh_hdmi_audio_infoframe_setup(struct sh_hdmi *hdmi)
 }
 
 /**
- * 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)
@@ -632,18 +682,6 @@ static void sh_hdmi_configure(struct sh_hdmi *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
@@ -661,17 +699,53 @@ static void sh_hdmi_configure(struct sh_hdmi *hdmi)
         hdmi_write(hdmi, 0x40, HDMI_SYSTEM_CTRL);
 }
 
-static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
+static unsigned long sh_hdmi_rate_error(struct sh_hdmi *hdmi,
+                const struct fb_videomode *mode,
+                unsigned long *hdmi_rate, unsigned long *parent_rate)
 {
-        struct fb_var_screeninfo *var = &hdmi->var;
+        unsigned long target = PICOS2KHZ(mode->pixclock) * 1000, rate_error;
         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;
+
+        *hdmi_rate = clk_round_rate(hdmi->hdmi_clk, target);
+        if ((long)*hdmi_rate < 0)
+                *hdmi_rate = clk_get_rate(hdmi->hdmi_clk);
+
+        rate_error = (long)*hdmi_rate > 0 ? abs(*hdmi_rate - target) : ULONG_MAX;
+        if (rate_error && pdata->clk_optimize_parent)
+                rate_error = pdata->clk_optimize_parent(target, hdmi_rate, parent_rate);
+        else if (clk_get_parent(hdmi->hdmi_clk))
+                *parent_rate = clk_get_rate(clk_get_parent(hdmi->hdmi_clk));
+
+        dev_dbg(hdmi->dev, "%u-%u-%u-%u x %u-%u-%u-%u\n",
+                mode->left_margin, mode->xres,
+                mode->right_margin, mode->hsync_len,
+                mode->upper_margin, mode->yres,
+                mode->lower_margin, mode->vsync_len);
+
+        dev_dbg(hdmi->dev, "\t@%lu(+/-%lu)Hz, e=%lu / 1000, r=%uHz, p=%luHz\n", target,
+                rate_error, rate_error ? 10000 / (10 * target / rate_error) : 0,
+                mode->refresh, *parent_rate);
+
+        return rate_error;
+}
+
+static int sh_hdmi_read_edid(struct sh_hdmi *hdmi, unsigned long *hdmi_rate,
+                             unsigned long *parent_rate)
+{
+        struct fb_var_screeninfo tmpvar;
+        struct fb_var_screeninfo *var = &tmpvar;
+        const struct fb_videomode *mode, *found = NULL;
+        struct fb_info *info = hdmi->info;
+        struct fb_modelist *modelist = NULL;
+        unsigned int f_width = 0, f_height = 0, f_refresh = 0;
+        unsigned long found_rate_error = ULONG_MAX; /* silly compiler... */
+        bool scanning = false, preferred_bad = false;
         u8 edid[128];
+        char *forced;
+        int i;
 
         /* Read EDID */
-        pr_debug("Read back EDID code:");
+        dev_dbg(hdmi->dev, "Read back EDID code:");
         for (i = 0; i < 128; i++) {
                 edid[i] = hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
 #ifdef DEBUG
@@ -686,29 +760,167 @@ static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
 #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);
+
+        if (!hdmi->edid_blocks) {
+                fb_edid_to_monspecs(edid, &hdmi->monspec);
+                hdmi->edid_blocks = edid[126] + 1;
+
+                dev_dbg(hdmi->dev, "%d main modes, %d extension blocks\n",
+                        hdmi->monspec.modedb_len, hdmi->edid_blocks - 1);
+        } else {
+                dev_dbg(hdmi->dev, "Extension %u detected, DTD start %u\n",
+                        edid[0], edid[2]);
+                fb_edid_add_monspecs(edid, &hdmi->monspec);
+        }
+
+        if (hdmi->edid_blocks > hdmi->edid_segment_nr * 2 +
+            (hdmi->edid_block_addr >> 7) + 1) {
+                /* More blocks to read */
+                if (hdmi->edid_block_addr) {
+                        hdmi->edid_block_addr = 0;
+                        hdmi->edid_segment_nr++;
+                } else {
+                        hdmi->edid_block_addr = 0x80;
+                }
+                /* Set EDID word address  */
+                hdmi_write(hdmi, hdmi->edid_block_addr, HDMI_EDID_WORD_ADDRESS);
+                /* Enable EDID interrupt */
+                hdmi_write(hdmi, 0xC6, HDMI_INTERRUPT_MASK_1);
+                /* Set EDID segment pointer - starts reading EDID */
+                hdmi_write(hdmi, hdmi->edid_segment_nr, HDMI_EDID_SEGMENT_POINTER);
+                return -EAGAIN;
+        }
+
+        /* All E-EDID blocks ready */
+        dev_dbg(hdmi->dev, "%d main and extended modes\n", hdmi->monspec.modedb_len);
+
+        fb_get_options("sh_mobile_lcdc", &forced);
+        if (forced && *forced) {
+                /* Only primitive parsing so far */
+                i = sscanf(forced, "%ux%u@%u",
+                           &f_width, &f_height, &f_refresh);
+                if (i < 2) {
+                        f_width = 0;
+                        f_height = 0;
+                } else {
+                        /* The user wants us to use the EDID data */
+                        scanning = true;
+                }
+                dev_dbg(hdmi->dev, "Forced mode %ux%u@%uHz\n",
+                        f_width, f_height, f_refresh);
+        }
+
+        /* Walk monitor modes to find the best or the exact match */
+        for (i = 0, mode = hdmi->monspec.modedb;
+             i < hdmi->monspec.modedb_len && scanning;
+             i++, mode++) {
+                unsigned long rate_error;
+
+                if (!f_width && !f_height) {
+                        /*
+                         * A parameter string "video=sh_mobile_lcdc:0x0" means
+                         * use the preferred EDID mode. If it is rejected by
+                         * .fb_check_var(), keep looking, until an acceptable
+                         * one is found.
+                         */
+                        if ((mode->flag & FB_MODE_IS_FIRST) || preferred_bad)
+                                scanning = false;
+                        else
+                                continue;
+                } else if (f_width != mode->xres || f_height != mode->yres) {
+                        /* No interest in unmatching modes */
+                        continue;
+                }
+
+                rate_error = sh_hdmi_rate_error(hdmi, mode, hdmi_rate, parent_rate);
+
+                if (scanning) {
+                        if (f_refresh == mode->refresh || (!f_refresh && !rate_error))
+                                /*
+                                 * Exact match if either the refresh rate
+                                 * matches or it hasn't been specified and we've
+                                 * found a mode, for which we can configure the
+                                 * clock precisely
+                                 */
+                                scanning = false;
+                        else if (found && found_rate_error <= rate_error)
+                                /*
+                                 * We otherwise search for the closest matching
+                                 * clock rate - either if no refresh rate has
+                                 * been specified or we cannot find an exactly
+                                 * matching one
+                                 */
+                                continue;
+                }
+
+                /* Check if supported: sufficient fb memory, supported clock-rate */
+                fb_videomode_to_var(var, mode);
+
+                var->bits_per_pixel = info->var.bits_per_pixel;
+
+                if (info && info->fbops->fb_check_var &&
+                    info->fbops->fb_check_var(var, info)) {
+                        scanning = true;
+                        preferred_bad = true;
+                        continue;
+                }
+
+                found = mode;
+                found_rate_error = rate_error;
+        }
+
+        hdmi->var.width = hdmi->monspec.max_x * 10;
+        hdmi->var.height = hdmi->monspec.max_y * 10;
+
+        /*
+         * TODO 1: if no ->info is present, postpone running the config until
+         * after ->info first gets registered.
+         * TODO 2: consider registering the HDMI platform device from the LCDC
+         * driver, and passing ->info with HDMI platform data.
+         */
+        if (info && !found) {
+                modelist = info->modelist.next &&
+                        !list_empty(&info->modelist) ?
+                        list_entry(info->modelist.next,
+                                   struct fb_modelist, list) :
+                        NULL;
+
+                if (modelist) {
+                        found = &modelist->mode;
+                        found_rate_error = sh_hdmi_rate_error(hdmi, found, hdmi_rate, parent_rate);
+                }
+        }
+
+        /* No cookie today */
+        if (!found)
+                return -ENXIO;
+
+        if (found->xres == 640 && found->yres == 480 && found->refresh == 60)
+                hdmi->preprogrammed_vic = 1;
+        else if (found->xres == 720 && found->yres == 480 && found->refresh == 60)
+                hdmi->preprogrammed_vic = 2;
+        else if (found->xres == 720 && found->yres == 576 && found->refresh == 50)
+                hdmi->preprogrammed_vic = 17;
+        else if (found->xres == 1280 && found->yres == 720 && found->refresh == 60)
+                hdmi->preprogrammed_vic = 4;
+        else if (found->xres == 1920 && found->yres == 1080 && found->refresh == 24)
+                hdmi->preprogrammed_vic = 32;
+        else if (found->xres == 1920 && found->yres == 1080 && found->refresh == 50)
+                hdmi->preprogrammed_vic = 31;
+        else if (found->xres == 1920 && found->yres == 1080 && found->refresh == 60)
+                hdmi->preprogrammed_vic = 16;
+        else
+                hdmi->preprogrammed_vic = 0;
+
+        dev_dbg(hdmi->dev, "Using %s %s mode %ux%u@%uHz (%luHz), clock error %luHz\n",
+                modelist ? "default" : "EDID", hdmi->preprogrammed_vic ? "VIC" : "external",
+                found->xres, found->yres, found->refresh,
+                PICOS2KHZ(found->pixclock) * 1000, found_rate_error);
+
+        fb_videomode_to_var(&hdmi->var, found);
+        sh_hdmi_external_video_param(hdmi);
+
+        return 0;
 }
 
 static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
@@ -736,8 +948,8 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
         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);
+                dev_dbg(hdmi->dev, "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;
@@ -748,136 +960,283 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
                 udelay(500);
 
                 msens = hdmi_read(hdmi, HDMI_HOT_PLUG_MSENS_STATUS);
-                pr_debug("MSENS 0x%x\n", msens);
+                dev_dbg(hdmi->dev, "MSENS 0x%x\n", msens);
                 /* Check, if hot plug & MSENS pin status are both high */
                 if ((msens & 0xC0) == 0xC0) {
                         /* Display plug in */
+                        hdmi->edid_segment_nr = 0;
+                        hdmi->edid_block_addr = 0;
+                        hdmi->edid_blocks = 0;
                         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);
+                        /* Set EDID segment pointer - starts reading EDID */
+                        hdmi_write(hdmi, 0x00, HDMI_EDID_SEGMENT_POINTER);
                 } else if (!(status1 & 0x80)) {
                         /* Display unplug, beware multiple interrupts */
-                        if (hdmi->hp_state != HDMI_HOTPLUG_DISCONNECTED)
+                        if (hdmi->hp_state != HDMI_HOTPLUG_DISCONNECTED) {
+                                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);
+                hdmi_write(hdmi, hdmi->edid_block_addr, HDMI_EDID_WORD_ADDRESS);
                 /* Set EDID segment pointer */
-                hdmi_write(hdmi, 0x00, HDMI_EDID_SEGMENT_POINTER);
+                hdmi_write(hdmi, hdmi->edid_segment_nr, 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)
+/* locking:     called with info->lock held, or before register_framebuffer() */
+static void sh_hdmi_display_on(void *arg, struct fb_info *info)
 {
+        /*
+         * info is guaranteed to be valid, when we are called, because our
+         * FB_EVENT_FB_UNBIND notify is also called with info->lock held
+         */
         struct sh_hdmi *hdmi = arg;
         struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+        struct sh_mobile_lcdc_chan *ch = info->par;
 
-        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;
-        }
+        dev_dbg(hdmi->dev, "%s(%p): state %x\n", __func__,
+                pdata->lcd_dev, info->state);
+
+        /* No need to lock */
+        hdmi->info = info;
 
-        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?
+         * hp_state can be set to
+         * HDMI_HOTPLUG_DISCONNECTED:   on monitor unplug
+         * HDMI_HOTPLUG_CONNECTED:      on monitor plug-in
+         * HDMI_HOTPLUG_EDID_DONE:      on EDID read completion
          */
-        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");
+                dev_dbg(hdmi->dev, "HDMI running\n");
                 break;
         case HDMI_HOTPLUG_DISCONNECTED:
                 info->state = FBINFO_STATE_SUSPENDED;
         default:
-                hdmi->var = info->var;
+                hdmi->var = ch->display_var;
         }
 }
 
-static void hdmi_display_off(void *arg)
+/* locking: called with info->lock held */
+static void sh_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);
+        dev_dbg(hdmi->dev, "%s(%p)\n", __func__, pdata->lcd_dev);
         /* PS mode e->a */
         hdmi_write(hdmi, 0x10, HDMI_SYSTEM_CTRL);
 }
 
+static bool sh_hdmi_must_reconfigure(struct sh_hdmi *hdmi)
+{
+        struct fb_info *info = hdmi->info;
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct fb_var_screeninfo *new_var = &hdmi->var, *old_var = &ch->display_var;
+        struct fb_videomode mode1, mode2;
+
+        fb_var_to_videomode(&mode1, old_var);
+        fb_var_to_videomode(&mode2, new_var);
+
+        dev_dbg(info->dev, "Old %ux%u, new %ux%u\n",
+                mode1.xres, mode1.yres, mode2.xres, mode2.yres);
+
+        if (fb_mode_is_equal(&mode1, &mode2)) {
+                /* It can be a different monitor with an equal video-mode */
+                old_var->width = new_var->width;
+                old_var->height = new_var->height;
+                return false;
+        }
+
+        dev_dbg(info->dev, "Switching %u -> %u lines\n",
+                mode1.yres, mode2.yres);
+        *old_var = *new_var;
+
+        return true;
+}
+
+/**
+ * sh_hdmi_clk_configure() - set HDMI clock frequency and enable the clock
+ * @hdmi:               driver context
+ * @hdmi_rate:          HDMI clock frequency in Hz
+ * @parent_rate:        if != 0 - set parent clock rate for optimal precision
+ * return:              configured positive rate if successful
+ *                      0 if couldn't set the rate, but managed to enable the
+ *                      clock, negative error, if couldn't enable the clock
+ */
+static long sh_hdmi_clk_configure(struct sh_hdmi *hdmi, unsigned long hdmi_rate,
+                                  unsigned long parent_rate)
+{
+        int ret;
+
+        if (parent_rate && clk_get_parent(hdmi->hdmi_clk)) {
+                ret = clk_set_rate(clk_get_parent(hdmi->hdmi_clk), parent_rate);
+                if (ret < 0) {
+                        dev_warn(hdmi->dev, "Cannot set parent rate %ld: %d\n", parent_rate, ret);
+                        hdmi_rate = clk_round_rate(hdmi->hdmi_clk, hdmi_rate);
+                } else {
+                        dev_dbg(hdmi->dev, "HDMI set parent frequency %lu\n", parent_rate);
+                }
+        }
+
+        ret = clk_set_rate(hdmi->hdmi_clk, hdmi_rate);
+        if (ret < 0) {
+                dev_warn(hdmi->dev, "Cannot set rate %ld: %d\n", hdmi_rate, ret);
+                hdmi_rate = 0;
+        } else {
+                dev_dbg(hdmi->dev, "HDMI set frequency %lu\n", hdmi_rate);
+        }
+
+        return hdmi_rate;
+}
+
 /* Hotplug interrupt occurred, read EDID */
-static void edid_work_fn(struct work_struct *work)
+static void sh_hdmi_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;
+        struct sh_mobile_lcdc_chan *ch;
+        int ret;
 
-        pr_debug("%s(%p): begin, hotplug status %d\n", __func__,
-                 pdata->lcd_dev, hdmi->hp_state);
+        dev_dbg(hdmi->dev, "%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);
+        mutex_lock(&hdmi->mutex);
+
+        if (hdmi->hp_state == HDMI_HOTPLUG_CONNECTED) {
+                struct fb_info *info = hdmi->info;
+                unsigned long parent_rate = 0, hdmi_rate;
+
+                ret = sh_hdmi_read_edid(hdmi, &hdmi_rate, &parent_rate);
+                if (ret < 0)
+                        goto out;
+
+                hdmi->hp_state = HDMI_HOTPLUG_EDID_DONE;
+
+                /* Reconfigure the clock */
+                ret = sh_hdmi_clk_configure(hdmi, hdmi_rate, parent_rate);
+                if (ret < 0)
+                        goto out;
+
                 msleep(10);
                 sh_hdmi_configure(hdmi);
                 /* Switched to another (d) power-save mode */
                 msleep(10);
 
-                if (!hdmi->info)
-                        return;
+                if (!info)
+                        goto out;
+
+                ch = info->par;
 
-                acquire_console_sem();
+                console_lock();
 
                 /* 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);
+                if (!sh_hdmi_must_reconfigure(hdmi) &&
+                    info->state == FBINFO_STATE_RUNNING) {
+                        /*
+                         * First activation with the default monitor - just turn
+                         * on, if we run a resume here, the logo disappears
+                         */
+                        if (lock_fb_info(info)) {
+                                info->var.width = hdmi->var.width;
+                                info->var.height = hdmi->var.height;
+                                sh_hdmi_display_on(hdmi, info);
+                                unlock_fb_info(info);
+                        }
+                } else {
+                        /* New monitor or have to wake up */
+                        fb_set_suspend(info, 0);
+                }
 
-                release_console_sem();
+                console_unlock();
         } else {
+                ret = 0;
                 if (!hdmi->info)
-                        return;
+                        goto out;
+
+                hdmi->monspec.modedb_len = 0;
+                fb_destroy_modedb(hdmi->monspec.modedb);
+                hdmi->monspec.modedb = NULL;
 
-                acquire_console_sem();
+                console_lock();
 
                 /* HDMI disconnect */
                 fb_set_suspend(hdmi->info, 1);
 
-                release_console_sem();
-                pm_runtime_put(hdmi->dev);
+                console_unlock();
         }
 
-        pr_debug("%s(%p): end\n", __func__, pdata->lcd_dev);
+out:
+        if (ret < 0 && ret != -EAGAIN)
+                hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
+        mutex_unlock(&hdmi->mutex);
+
+        dev_dbg(hdmi->dev, "%s(%p): end\n", __func__, pdata->lcd_dev);
+}
+
+static int sh_hdmi_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 sh_hdmi *hdmi = board_cfg->board_data;
+
+        if (!hdmi || nb != &hdmi->notifier || hdmi->info != info)
+                return NOTIFY_DONE;
+
+        switch(action) {
+        case FB_EVENT_FB_REGISTERED:
+                /* Unneeded, activation taken care by sh_hdmi_display_on() */
+                break;
+        case FB_EVENT_FB_UNREGISTERED:
+                /*
+                 * We are called from unregister_framebuffer() with the
+                 * info->lock held. This is bad for us, because we can race with
+                 * the scheduled work, which has to call fb_set_suspend(), which
+                 * takes info->lock internally, so, sh_hdmi_edid_work_fn()
+                 * cannot take and hold info->lock for the whole function
+                 * duration. Using an additional lock creates a classical AB-BA
+                 * lock up. Therefore, we have to release the info->lock
+                 * temporarily, synchronise with the work queue and re-acquire
+                 * the info->lock.
+                 */
+                unlock_fb_info(info);
+                mutex_lock(&hdmi->mutex);
+                hdmi->info = NULL;
+                mutex_unlock(&hdmi->mutex);
+                lock_fb_info(info);
+                return NOTIFY_OK;
+        }
+        return NOTIFY_DONE;
 }
 
 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);
+        struct sh_mobile_lcdc_board_cfg *board_cfg;
         int irq = platform_get_irq(pdev, 0), ret;
         struct sh_hdmi *hdmi;
         long rate;
@@ -891,6 +1250,8 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
                 return -ENOMEM;
         }
 
+        mutex_init(&hdmi->mutex);
+
         hdmi->dev = &pdev->dev;
 
         hdmi->hdmi_clk = clk_get(&pdev->dev, "ick");
@@ -900,30 +1261,23 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
                 goto egetclk;
         }
 
-        rate = PICOS2KHZ(pdata->lcd_chan->lcd_cfg.pixclock) * 1000;
+        /* An arbitrary relaxed pixclock just to get things started: from standard 480p */
+        rate = clk_round_rate(hdmi->hdmi_clk, PICOS2KHZ(37037));
+        if (rate > 0)
+                rate = sh_hdmi_clk_configure(hdmi, rate, 0);
 
-        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_err(hdmi->dev, "Cannot enable clock: %d\n", ret);
+                goto erate;
         }
 
-        dev_info(&pdev->dev, "Enabled HDMI clock at %luHz\n", rate);
+        dev_dbg(&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");
@@ -940,21 +1294,21 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 
         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;
+        board_cfg = &pdata->lcd_chan->board_cfg;
+        board_cfg->owner = THIS_MODULE;
+        board_cfg->board_data = hdmi;
+        board_cfg->display_on = sh_hdmi_display_on;
+        board_cfg->display_off = sh_hdmi_display_off;
 
-        INIT_DELAYED_WORK(&hdmi->edid_work, edid_work_fn);
+        INIT_DELAYED_WORK(&hdmi->edid_work, sh_hdmi_edid_work_fn);
 
         pm_runtime_enable(&pdev->dev);
-        pm_runtime_resume(&pdev->dev);
+        pm_runtime_get_sync(&pdev->dev);
+
+        /* Product and revision IDs are 0 in sh-mobile version */
+        dev_info(&pdev->dev, "Detected HDMI controller 0x%x:0x%x\n",
+                 hdmi_read(hdmi, HDMI_PRODUCT_ID), hdmi_read(hdmi, HDMI_REVISION_ID));
 
         ret = request_irq(irq, sh_hdmi_hotplug, 0,
                           dev_name(&pdev->dev), hdmi);
@@ -963,19 +1317,32 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
                 goto ereqirq;
         }
 
+        ret = snd_soc_register_codec(&pdev->dev,
+                        &soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "codec registration failed\n");
+                goto ecodec;
+        }
+
+        hdmi->notifier.notifier_call = sh_hdmi_notify;
+        fb_register_client(&hdmi->notifier);
+
         return 0;
 
+ecodec:
+        free_irq(irq, hdmi);
 ereqirq:
+        pm_runtime_put(&pdev->dev);
         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:
+        mutex_destroy(&hdmi->mutex);
         kfree(hdmi);
 
         return ret;
@@ -986,19 +1353,29 @@ 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);
+        struct sh_mobile_lcdc_board_cfg *board_cfg = &pdata->lcd_chan->board_cfg;
         int irq = platform_get_irq(pdev, 0);
 
-        pdata->lcd_chan->board_cfg.display_on = NULL;
-        pdata->lcd_chan->board_cfg.display_off = NULL;
-        pdata->lcd_chan->board_cfg.board_data = NULL;
+        snd_soc_unregister_codec(&pdev->dev);
+
+        fb_unregister_client(&hdmi->notifier);
 
+        board_cfg->display_on = NULL;
+        board_cfg->display_off = NULL;
+        board_cfg->board_data = NULL;
+        board_cfg->owner = NULL;
+
+        /* No new work will be scheduled, wait for running ISR */
         free_irq(irq, hdmi);
-        pm_runtime_disable(&pdev->dev);
+        /* Wait for already scheduled work */
         cancel_delayed_work_sync(&hdmi->edid_work);
+        pm_runtime_put(&pdev->dev);
+        pm_runtime_disable(&pdev->dev);
         clk_disable(hdmi->hdmi_clk);
         clk_put(hdmi->hdmi_clk);
         iounmap(hdmi->base);
         release_mem_region(res->start, resource_size(res));
+        mutex_destroy(&hdmi->mutex);
         kfree(hdmi);
 
         return 0;
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index d72075a9f01c..019dbd3f12b2 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -12,7 +12,6 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/mm.h>
-#include <linux/fb.h>
 #include <linux/clk.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_device.h>
@@ -21,10 +20,15 @@
 #include <linux/vmalloc.h>
 #include <linux/ioctl.h>
 #include <linux/slab.h>
+#include <linux/console.h>
+#include <linux/backlight.h>
+#include <linux/gpio.h>
 #include <video/sh_mobile_lcdc.h>
 #include <asm/atomic.h>
 
-#define PALETTE_NR 16
+#include "sh_mobile_lcdcfb.h"
+#include "sh_mobile_meram.h"
+
 #define SIDE_B_OFFSET 0x1000
 #define MIRROR_OFFSET 0x2000
 
@@ -53,11 +57,8 @@ static int lcdc_shared_regs[] = {
 };
 #define NR_SHARED_REGS ARRAY_SIZE(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 };
+#define MAX_XRES 1920
+#define MAX_YRES 1080
 
 static unsigned long lcdc_offs_mainlcd[NR_CH_REGS] = {
         [LDDCKPAT1R] = 0x400,
@@ -69,6 +70,7 @@ static unsigned long lcdc_offs_mainlcd[NR_CH_REGS] = {
         [LDSM1R] = 0x428,
         [LDSM2R] = 0x42c,
         [LDSA1R] = 0x430,
+        [LDSA2R] = 0x434,
         [LDMLSR] = 0x438,
         [LDHCNR] = 0x448,
         [LDHSYNR] = 0x44c,
@@ -112,23 +114,22 @@ static unsigned long lcdc_offs_sublcd[NR_CH_REGS] = {
 #define LDRCNTR_MRC     0x00000001
 #define LDSR_MRS        0x00000100
 
-struct sh_mobile_lcdc_priv;
-struct sh_mobile_lcdc_chan {
-        struct sh_mobile_lcdc_priv *lcdc;
-        unsigned long *reg_offs;
-        unsigned long ldmt1r_value;
-        unsigned long enabled; /* ME and SE in LDCNT2R */
-        struct sh_mobile_lcdc_chan_cfg cfg;
-        u32 pseudo_palette[PALETTE_NR];
-        unsigned long saved_ch_regs[NR_CH_REGS];
-        struct fb_info *info;
-        dma_addr_t dma_handle;
-        struct fb_deferred_io defio;
-        struct scatterlist *sglist;
-        unsigned long frame_end;
-        unsigned long pan_offset;
-        wait_queue_head_t frame_end_wait;
-        struct completion vsync_completion;
+static const struct fb_videomode default_720p = {
+        .name = "HDMI 720p",
+        .xres = 1280,
+        .yres = 720,
+
+        .left_margin = 220,
+        .right_margin = 110,
+        .hsync_len = 40,
+
+        .upper_margin = 20,
+        .lower_margin = 5,
+        .vsync_len = 5,
+
+        .pixclock = 13468,
+        .refresh = 60,
+        .sync = FB_SYNC_VERT_HIGH_ACT | FB_SYNC_HOR_HIGH_ACT,
 };
 
 struct sh_mobile_lcdc_priv {
@@ -142,6 +143,8 @@ struct sh_mobile_lcdc_priv {
         struct notifier_block notifier;
         unsigned long saved_shared_regs[NR_SHARED_REGS];
         int started;
+        int forced_bpp; /* 2 channel LCDC must share bpp setting */
+        struct sh_mobile_meram_info *meram_dev;
 };
 
 static bool banked(int reg_nr)
@@ -153,6 +156,7 @@ static bool banked(int reg_nr)
         case LDDFR:
         case LDSM1R:
         case LDSA1R:
+        case LDSA2R:
         case LDMLSR:
         case LDHCNR:
         case LDHSYNR:
@@ -409,8 +413,8 @@ static void sh_mobile_lcdc_start_stop(struct sh_mobile_lcdc_priv *priv,
 
 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;
+        struct fb_var_screeninfo *var = &ch->info->var, *display_var = &ch->display_var;
+        unsigned long h_total, hsync_pos, display_h_total;
         u32 tmp;
 
         tmp = ch->ldmt1r_value;
@@ -428,31 +432,33 @@ static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
         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;
+        h_total = display_var->xres + display_var->hsync_len +
+                display_var->left_margin + display_var->right_margin;
         tmp = h_total / 8; /* HTCN */
-        tmp |= (var->xres / 8) << 16; /* HDCN */
+        tmp |= (min(display_var->xres, var->xres) / 8) << 16; /* HDCN */
         lcdc_write_chan(ch, LDHCNR, tmp);
 
-        hsync_pos = var->xres + var->right_margin;
+        hsync_pos = display_var->xres + display_var->right_margin;
         tmp = hsync_pos / 8; /* HSYNP */
-        tmp |= (var->hsync_len / 8) << 16; /* HSYNW */
+        tmp |= (display_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 */
+        tmp = display_var->yres + display_var->vsync_len +
+                display_var->upper_margin + display_var->lower_margin; /* VTLN */
+        tmp |= min(display_var->yres, var->yres) << 16; /* VDLN */
         lcdc_write_chan(ch, LDVLNR, tmp);
 
-        tmp = var->yres + var->lower_margin; /* VSYNP */
-        tmp |= var->vsync_len << 16; /* VSYNW */
+        tmp = display_var->yres + display_var->lower_margin; /* VSYNP */
+        tmp |= display_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) |
+        display_h_total = display_var->xres + display_var->hsync_len +
+                display_var->left_margin + display_var->right_margin;
+        tmp = ((display_var->xres & 7) << 24) |
+                ((display_h_total & 7) << 16) |
+                ((display_var->hsync_len & 7) << 8) |
                 hsync_pos;
         lcdc_write_chan(ch, LDHAJR, tmp);
 }
@@ -460,16 +466,20 @@ static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
 static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 {
         struct sh_mobile_lcdc_chan *ch;
-        struct fb_videomode *lcd_cfg;
         struct sh_mobile_lcdc_board_cfg *board_cfg;
         unsigned long tmp;
-        int k, m;
-        int ret = 0;
+        int bpp = 0;
+        unsigned long ldddsr;
+        int k, m, ret;
 
         /* enable clocks before accessing the hardware */
-        for (k = 0; k < ARRAY_SIZE(priv->ch); k++)
-                if (priv->ch[k].enabled)
+        for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+                if (priv->ch[k].enabled) {
                         sh_mobile_lcdc_clk_on(priv);
+                        if (!bpp)
+                                bpp = priv->ch[k].info->var.bits_per_pixel;
+                }
+        }
 
         /* reset */
         lcdc_write(priv, _LDCNT2R, lcdc_read(priv, _LDCNT2R) | LCDC_RESET);
@@ -503,7 +513,8 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
                         m = 1 << 6;
                 tmp |= m << (lcdc_chan_is_sublcd(ch) ? 8 : 0);
 
-                lcdc_write_chan(ch, LDDCKPAT1R, 0x00000000);
+                /* FIXME: sh7724 can only use 42, 48, 54 and 60 for the divider denominator */
+                lcdc_write_chan(ch, LDDCKPAT1R, 0);
                 lcdc_write_chan(ch, LDDCKPAT2R, (1 << (m/2)) - 1);
         }
 
@@ -518,7 +529,6 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
         for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
                 ch = &priv->ch[k];
-                lcd_cfg = &ch->cfg.lcd_cfg;
 
                 if (!ch->enabled)
                         continue;
@@ -529,17 +539,36 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
                 lcdc_write_chan(ch, LDPMR, 0);
 
                 board_cfg = &ch->cfg.board_cfg;
-                if (board_cfg->setup_sys)
-                        ret = board_cfg->setup_sys(board_cfg->board_data, ch,
-                                                   &sh_mobile_lcdc_sys_bus_ops);
-                if (ret)
-                        return ret;
+                if (board_cfg->setup_sys) {
+                        ret = board_cfg->setup_sys(board_cfg->board_data,
+                                                ch, &sh_mobile_lcdc_sys_bus_ops);
+                        if (ret)
+                                return ret;
+                }
         }
 
         /* word and long word swap */
-        lcdc_write(priv, _LDDDSR, lcdc_read(priv, _LDDDSR) | 6);
+        ldddsr = lcdc_read(priv, _LDDDSR);
+        if  (priv->ch[0].info->var.nonstd)
+                lcdc_write(priv, _LDDDSR, ldddsr | 7);
+        else {
+                switch (bpp) {
+                case 16:
+                        lcdc_write(priv, _LDDDSR, ldddsr | 6);
+                        break;
+                case 24:
+                        lcdc_write(priv, _LDDDSR, ldddsr | 7);
+                        break;
+                case 32:
+                        lcdc_write(priv, _LDDDSR, ldddsr | 4);
+                        break;
+                }
+        }
 
         for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
+                unsigned long base_addr_y;
+                unsigned long base_addr_c = 0;
+                int pitch;
                 ch = &priv->ch[k];
 
                 if (!priv->ch[k].enabled)
@@ -547,15 +576,95 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
                 /* set bpp format in PKF[4:0] */
                 tmp = lcdc_read_chan(ch, LDDFR);
-                tmp &= ~(0x0001001f);
-                tmp |= (ch->info->var.bits_per_pixel == 16) ? 3 : 0;
+                tmp &= ~0x0003031f;
+                if (ch->info->var.nonstd) {
+                        tmp |= (ch->info->var.nonstd << 16);
+                        switch (ch->info->var.bits_per_pixel) {
+                        case 12:
+                                break;
+                        case 16:
+                                tmp |= (0x1 << 8);
+                                break;
+                        case 24:
+                                tmp |= (0x2 << 8);
+                                break;
+                        }
+                } else {
+                        switch (ch->info->var.bits_per_pixel) {
+                        case 16:
+                                tmp |= 0x03;
+                                break;
+                        case 24:
+                                tmp |= 0x0b;
+                                break;
+                        case 32:
+                                break;
+                        }
+                }
                 lcdc_write_chan(ch, LDDFR, tmp);
 
+                base_addr_y = ch->info->fix.smem_start;
+                base_addr_c = base_addr_y +
+                                ch->info->var.xres *
+                                ch->info->var.yres_virtual;
+                pitch = ch->info->fix.line_length;
+
+                /* test if we can enable meram */
+                if (ch->cfg.meram_cfg && priv->meram_dev &&
+                                priv->meram_dev->ops) {
+                        struct sh_mobile_meram_cfg *cfg;
+                        struct sh_mobile_meram_info *mdev;
+                        unsigned long icb_addr_y, icb_addr_c;
+                        int icb_pitch;
+                        int pf;
+
+                        cfg = ch->cfg.meram_cfg;
+                        mdev = priv->meram_dev;
+                        /* we need to de-init configured ICBs before we
+                         * we can re-initialize them.
+                         */
+                        if (ch->meram_enabled)
+                                mdev->ops->meram_unregister(mdev, cfg);
+
+                        ch->meram_enabled = 0;
+
+                        if (ch->info->var.nonstd) {
+                                if (ch->info->var.bits_per_pixel == 24)
+                                        pf = SH_MOBILE_MERAM_PF_NV24;
+                                else
+                                        pf = SH_MOBILE_MERAM_PF_NV;
+                        } else {
+                                pf = SH_MOBILE_MERAM_PF_RGB;
+                        }
+
+                        ret = mdev->ops->meram_register(mdev, cfg, pitch,
+                                                ch->info->var.yres,
+                                                pf,
+                                                base_addr_y,
+                                                base_addr_c,
+                                                &icb_addr_y,
+                                                &icb_addr_c,
+                                                &icb_pitch);
+                        if (!ret)  {
+                                /* set LDSA1R value */
+                                base_addr_y = icb_addr_y;
+                                pitch = icb_pitch;
+
+                                /* set LDSA2R value if required */
+                                if (base_addr_c)
+                                        base_addr_c = icb_addr_c;
+
+                                ch->meram_enabled = 1;
+                        }
+                }
+
                 /* point out our frame buffer */
-                lcdc_write_chan(ch, LDSA1R, ch->info->fix.smem_start);
+                lcdc_write_chan(ch, LDSA1R, base_addr_y);
+                if (ch->info->var.nonstd)
+                        lcdc_write_chan(ch, LDSA2R, base_addr_c);
 
                 /* set line size */
-                lcdc_write_chan(ch, LDMLSR, ch->info->fix.line_length);
+                lcdc_write_chan(ch, LDMLSR, pitch);
 
                 /* setup deferred io if SYS bus */
                 tmp = ch->cfg.sys_bus_cfg.deferred_io_msec;
@@ -591,8 +700,15 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
                         continue;
 
                 board_cfg = &ch->cfg.board_cfg;
-                if (board_cfg->display_on)
+                if (board_cfg->display_on && try_module_get(board_cfg->owner)) {
                         board_cfg->display_on(board_cfg->board_data, ch->info);
+                        module_put(board_cfg->owner);
+                }
+
+                if (ch->bl) {
+                        ch->bl->props.power = FB_BLANK_UNBLANK;
+                        backlight_update_status(ch->bl);
+                }
         }
 
         return 0;
@@ -614,7 +730,7 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
                  * flush frame, and wait for frame end interrupt
                  * clean up deferred io and enable clock
                  */
-                if (ch->info->fbdefio) {
+                if (ch->info && ch->info->fbdefio) {
                         ch->frame_end = 0;
                         schedule_delayed_work(&ch->info->deferred_work, 0);
                         wait_event(ch->frame_end_wait, ch->frame_end);
@@ -623,9 +739,27 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
                         sh_mobile_lcdc_clk_on(priv);
                 }
 
+                if (ch->bl) {
+                        ch->bl->props.power = FB_BLANK_POWERDOWN;
+                        backlight_update_status(ch->bl);
+                }
+
                 board_cfg = &ch->cfg.board_cfg;
-                if (board_cfg->display_off)
+                if (board_cfg->display_off && try_module_get(board_cfg->owner)) {
                         board_cfg->display_off(board_cfg->board_data);
+                        module_put(board_cfg->owner);
+                }
+
+                /* disable the meram */
+                if (ch->meram_enabled) {
+                        struct sh_mobile_meram_cfg *cfg;
+                        struct sh_mobile_meram_info *mdev;
+                        cfg = ch->cfg.meram_cfg;
+                        mdev = priv->meram_dev;
+                        mdev->ops->meram_unregister(mdev, cfg);
+                        ch->meram_enabled = 0;
+                }
+
         }
 
         /* stop the lcdc */
@@ -704,7 +838,6 @@ static int sh_mobile_lcdc_setup_clocks(struct platform_device *pdev,
                         return PTR_ERR(priv->dot_clk);
                 }
         }
-        atomic_set(&priv->hw_usecnt, -1);
 
         /* Runtime PM support involves two step for this driver:
          * 1) Enable Runtime PM
@@ -778,9 +911,15 @@ static int sh_mobile_fb_pan_display(struct fb_var_screeninfo *var,
         struct sh_mobile_lcdc_priv *priv = ch->lcdc;
         unsigned long ldrcntr;
         unsigned long new_pan_offset;
+        unsigned long base_addr_y, base_addr_c;
+        unsigned long c_offset;
 
-        new_pan_offset = (var->yoffset * info->fix.line_length) +
-                (var->xoffset * (info->var.bits_per_pixel / 8));
+        if (!var->nonstd)
+                new_pan_offset = (var->yoffset * info->fix.line_length) +
+                        (var->xoffset * (info->var.bits_per_pixel / 8));
+        else
+                new_pan_offset = (var->yoffset * info->fix.line_length) +
+                        (var->xoffset);
 
         if (new_pan_offset == ch->pan_offset)
                 return 0;       /* No change, do nothing */
@@ -788,7 +927,46 @@ static int sh_mobile_fb_pan_display(struct fb_var_screeninfo *var,
         ldrcntr = lcdc_read(priv, _LDRCNTR);
 
         /* Set the source address for the next refresh */
-        lcdc_write_chan_mirror(ch, LDSA1R, ch->dma_handle + new_pan_offset);
+        base_addr_y = ch->dma_handle + new_pan_offset;
+        if (var->nonstd) {
+                /* Set y offset */
+                c_offset = (var->yoffset *
+                        info->fix.line_length *
+                        (info->var.bits_per_pixel - 8)) / 8;
+                base_addr_c = ch->dma_handle + var->xres * var->yres_virtual +
+                        c_offset;
+                /* Set x offset */
+                if (info->var.bits_per_pixel == 24)
+                        base_addr_c += 2 * var->xoffset;
+                else
+                        base_addr_c += var->xoffset;
+        } else
+                base_addr_c = 0;
+
+        if (!ch->meram_enabled) {
+                lcdc_write_chan_mirror(ch, LDSA1R, base_addr_y);
+                if (base_addr_c)
+                        lcdc_write_chan_mirror(ch, LDSA2R, base_addr_c);
+        } else {
+                struct sh_mobile_meram_cfg *cfg;
+                struct sh_mobile_meram_info *mdev;
+                unsigned long icb_addr_y, icb_addr_c;
+                int ret;
+
+                cfg = ch->cfg.meram_cfg;
+                mdev = priv->meram_dev;
+                ret = mdev->ops->meram_update(mdev, cfg,
+                                        base_addr_y, base_addr_c,
+                                        &icb_addr_y, &icb_addr_c);
+                if (ret)
+                        return ret;
+
+                lcdc_write_chan_mirror(ch, LDSA1R, icb_addr_y);
+                if (icb_addr_c)
+                        lcdc_write_chan_mirror(ch, LDSA2R, icb_addr_c);
+
+        }
+
         if (lcdc_chan_is_sublcd(ch))
                 lcdc_write(ch->lcdc, _LDRCNTR, ldrcntr ^ LDRCNTR_SRS);
         else
@@ -837,6 +1015,153 @@ static int sh_mobile_ioctl(struct fb_info *info, unsigned int cmd,
         return retval;
 }
 
+static void sh_mobile_fb_reconfig(struct fb_info *info)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct fb_videomode mode1, mode2;
+        struct fb_event event;
+        int evnt = FB_EVENT_MODE_CHANGE_ALL;
+
+        if (ch->use_count > 1 || (ch->use_count == 1 && !info->fbcon_par))
+                /* More framebuffer users are active */
+                return;
+
+        fb_var_to_videomode(&mode1, &ch->display_var);
+        fb_var_to_videomode(&mode2, &info->var);
+
+        if (fb_mode_is_equal(&mode1, &mode2))
+                return;
+
+        /* Display has been re-plugged, framebuffer is free now, reconfigure */
+        if (fb_set_var(info, &ch->display_var) < 0)
+                /* Couldn't reconfigure, hopefully, can continue as before */
+                return;
+
+        if (info->var.nonstd)
+                info->fix.line_length = mode1.xres;
+        else
+                info->fix.line_length = mode1.xres * (ch->cfg.bpp / 8);
+
+        /*
+         * fb_set_var() calls the notifier change internally, only if
+         * FBINFO_MISC_USEREVENT flag is set. Since we do not want to fake a
+         * user event, we have to call the chain ourselves.
+         */
+        event.info = info;
+        event.data = &mode1;
+        fb_notifier_call_chain(evnt, &event);
+}
+
+/*
+ * Locking: both .fb_release() and .fb_open() are called with info->lock held if
+ * user == 1, or with console sem held, if user == 0.
+ */
+static int sh_mobile_release(struct fb_info *info, int user)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+
+        mutex_lock(&ch->open_lock);
+        dev_dbg(info->dev, "%s(): %d users\n", __func__, ch->use_count);
+
+        ch->use_count--;
+
+        /* Nothing to reconfigure, when called from fbcon */
+        if (user) {
+                console_lock();
+                sh_mobile_fb_reconfig(info);
+                console_unlock();
+        }
+
+        mutex_unlock(&ch->open_lock);
+
+        return 0;
+}
+
+static int sh_mobile_open(struct fb_info *info, int user)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+
+        mutex_lock(&ch->open_lock);
+        ch->use_count++;
+
+        dev_dbg(info->dev, "%s(): %d users\n", __func__, ch->use_count);
+        mutex_unlock(&ch->open_lock);
+
+        return 0;
+}
+
+static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct sh_mobile_lcdc_priv *p = ch->lcdc;
+
+        if (var->xres > MAX_XRES || var->yres > MAX_YRES ||
+            var->xres * var->yres * (ch->cfg.bpp / 8) * 2 > info->fix.smem_len) {
+                dev_warn(info->dev, "Invalid info: %u-%u-%u-%u x %u-%u-%u-%u @ %lukHz!\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));
+                return -EINVAL;
+        }
+
+        /* only accept the forced_bpp for dual channel configurations */
+        if (p->forced_bpp && p->forced_bpp != var->bits_per_pixel)
+                return -EINVAL;
+
+        switch (var->bits_per_pixel) {
+        case 16: /* PKF[4:0] = 00011 - RGB 565 */
+        case 24: /* PKF[4:0] = 01011 - RGB 888 */
+        case 32: /* PKF[4:0] = 00000 - RGBA 888 */
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/*
+ * Screen blanking. Behavior is as follows:
+ * FB_BLANK_UNBLANK: screen unblanked, clocks enabled
+ * FB_BLANK_NORMAL: screen blanked, clocks enabled
+ * FB_BLANK_VSYNC,
+ * FB_BLANK_HSYNC,
+ * FB_BLANK_POWEROFF: screen blanked, clocks disabled
+ */
+static int sh_mobile_lcdc_blank(int blank, struct fb_info *info)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct sh_mobile_lcdc_priv *p = ch->lcdc;
+
+        /* blank the screen? */
+        if (blank > FB_BLANK_UNBLANK && ch->blank_status == FB_BLANK_UNBLANK) {
+                struct fb_fillrect rect = {
+                        .width = info->var.xres,
+                        .height = info->var.yres,
+                };
+                sh_mobile_lcdc_fillrect(info, &rect);
+        }
+        /* turn clocks on? */
+        if (blank <= FB_BLANK_NORMAL && ch->blank_status > FB_BLANK_NORMAL) {
+                sh_mobile_lcdc_clk_on(p);
+        }
+        /* turn clocks off? */
+        if (blank > FB_BLANK_NORMAL && ch->blank_status <= FB_BLANK_NORMAL) {
+                /* make sure the screen is updated with the black fill before
+                 * switching the clocks off. one vsync is not enough since
+                 * blanking may occur in the middle of a refresh. deferred io
+                 * mode will reenable the clocks and update the screen in time,
+                 * so it does not need this. */
+                if (!info->fbdefio) {
+                        sh_mobile_wait_for_vsync(info);
+                        sh_mobile_wait_for_vsync(info);
+                }
+                sh_mobile_lcdc_clk_off(p);
+        }
+
+        ch->blank_status = blank;
+        return 0;
+}
 
 static struct fb_ops sh_mobile_lcdc_ops = {
         .owner          = THIS_MODULE,
@@ -846,12 +1171,89 @@ static struct fb_ops sh_mobile_lcdc_ops = {
         .fb_fillrect    = sh_mobile_lcdc_fillrect,
         .fb_copyarea    = sh_mobile_lcdc_copyarea,
         .fb_imageblit   = sh_mobile_lcdc_imageblit,
+        .fb_blank       = sh_mobile_lcdc_blank,
         .fb_pan_display = sh_mobile_fb_pan_display,
         .fb_ioctl       = sh_mobile_ioctl,
+        .fb_open        = sh_mobile_open,
+        .fb_release     = sh_mobile_release,
+        .fb_check_var   = sh_mobile_check_var,
 };
 
-static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp)
+static int sh_mobile_lcdc_update_bl(struct backlight_device *bdev)
+{
+        struct sh_mobile_lcdc_chan *ch = bl_get_data(bdev);
+        struct sh_mobile_lcdc_board_cfg *cfg = &ch->cfg.board_cfg;
+        int brightness = bdev->props.brightness;
+
+        if (bdev->props.power != FB_BLANK_UNBLANK ||
+            bdev->props.state & (BL_CORE_SUSPENDED | BL_CORE_FBBLANK))
+                brightness = 0;
+
+        return cfg->set_brightness(cfg->board_data, brightness);
+}
+
+static int sh_mobile_lcdc_get_brightness(struct backlight_device *bdev)
+{
+        struct sh_mobile_lcdc_chan *ch = bl_get_data(bdev);
+        struct sh_mobile_lcdc_board_cfg *cfg = &ch->cfg.board_cfg;
+
+        return cfg->get_brightness(cfg->board_data);
+}
+
+static int sh_mobile_lcdc_check_fb(struct backlight_device *bdev,
+                                   struct fb_info *info)
 {
+        return (info->bl_dev == bdev);
+}
+
+static struct backlight_ops sh_mobile_lcdc_bl_ops = {
+        .options        = BL_CORE_SUSPENDRESUME,
+        .update_status  = sh_mobile_lcdc_update_bl,
+        .get_brightness = sh_mobile_lcdc_get_brightness,
+        .check_fb       = sh_mobile_lcdc_check_fb,
+};
+
+static struct backlight_device *sh_mobile_lcdc_bl_probe(struct device *parent,
+                                               struct sh_mobile_lcdc_chan *ch)
+{
+        struct backlight_device *bl;
+
+        bl = backlight_device_register(ch->cfg.bl_info.name, parent, ch,
+                                       &sh_mobile_lcdc_bl_ops, NULL);
+        if (IS_ERR(bl)) {
+                dev_err(parent, "unable to register backlight device: %ld\n",
+                        PTR_ERR(bl));
+                return NULL;
+        }
+
+        bl->props.max_brightness = ch->cfg.bl_info.max_brightness;
+        bl->props.brightness = bl->props.max_brightness;
+        backlight_update_status(bl);
+
+        return bl;
+}
+
+static void sh_mobile_lcdc_bl_remove(struct backlight_device *bdev)
+{
+        backlight_device_unregister(bdev);
+}
+
+static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp,
+                                   int nonstd)
+{
+        if (nonstd) {
+                switch (bpp) {
+                case 12:
+                case 16:
+                case 24:
+                        var->bits_per_pixel = bpp;
+                        var->nonstd = nonstd;
+                        return 0;
+                default:
+                        return -EINVAL;
+                }
+        }
+
         switch (bpp) {
         case 16: /* PKF[4:0] = 00011 - RGB 565 */
                 var->red.offset = 11;
@@ -864,19 +1266,27 @@ static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp)
                 var->transp.length = 0;
                 break;
 
-        case 32: /* PKF[4:0] = 00000 - RGB 888
-                  * sh7722 pdf says 00RRGGBB but reality is GGBB00RR
-                  * this may be because LDDDSR has word swap enabled..
-                  */
-                var->red.offset = 0;
+        case 24: /* PKF[4:0] = 01011 - RGB 888 */
+                var->red.offset = 16;
                 var->red.length = 8;
-                var->green.offset = 24;
+                var->green.offset = 8;
                 var->green.length = 8;
-                var->blue.offset = 16;
+                var->blue.offset = 0;
                 var->blue.length = 8;
                 var->transp.offset = 0;
                 var->transp.length = 0;
                 break;
+
+        case 32: /* PKF[4:0] = 00000 - RGBA 888 */
+                var->red.offset = 16;
+                var->red.length = 8;
+                var->green.offset = 8;
+                var->green.length = 8;
+                var->blue.offset = 0;
+                var->blue.length = 8;
+                var->transp.offset = 24;
+                var->transp.length = 8;
+                break;
         default:
                 return -EINVAL;
         }
@@ -958,6 +1368,7 @@ static const struct dev_pm_ops sh_mobile_lcdc_dev_pm_ops = {
         .runtime_resume = sh_mobile_lcdc_runtime_resume,
 };
 
+/* locking: called with info->lock held */
 static int sh_mobile_lcdc_notify(struct notifier_block *nb,
                                  unsigned long action, void *data)
 {
@@ -965,53 +1376,36 @@ static int sh_mobile_lcdc_notify(struct notifier_block *nb,
         struct fb_info *info = event->info;
         struct sh_mobile_lcdc_chan *ch = info->par;
         struct sh_mobile_lcdc_board_cfg *board_cfg = &ch->cfg.board_cfg;
-        struct fb_var_screeninfo *var;
 
         if (&ch->lcdc->notifier != nb)
-                return 0;
+                return NOTIFY_DONE;
 
         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)
+                if (board_cfg->display_off && try_module_get(board_cfg->owner)) {
                         board_cfg->display_off(board_cfg->board_data);
-                pm_runtime_put(info->device);
+                        module_put(board_cfg->owner);
+                }
+                sh_mobile_lcdc_stop(ch->lcdc);
                 break;
         case FB_EVENT_RESUME:
-                var = &info->var;
+                mutex_lock(&ch->open_lock);
+                sh_mobile_fb_reconfig(info);
+                mutex_unlock(&ch->open_lock);
 
                 /* 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;
+                if (board_cfg->display_on && try_module_get(board_cfg->owner)) {
+                        board_cfg->display_on(board_cfg->board_data, info);
+                        module_put(board_cfg->owner);
                 }
 
-                pm_runtime_get_sync(info->device);
-
-                sh_mobile_lcdc_geometry(ch);
-
-                break;
+                sh_mobile_lcdc_start(ch->lcdc);
         }
 
-        return 0;
+        return NOTIFY_OK;
 }
 
 static int sh_mobile_lcdc_remove(struct platform_device *pdev);
@@ -1020,14 +1414,13 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 {
         struct fb_info *info;
         struct sh_mobile_lcdc_priv *priv;
-        struct sh_mobile_lcdc_info *pdata;
-        struct sh_mobile_lcdc_chan_cfg *cfg;
+        struct sh_mobile_lcdc_info *pdata = pdev->dev.platform_data;
         struct resource *res;
         int error;
         void *buf;
         int i, j;
 
-        if (!pdev->dev.platform_data) {
+        if (!pdata) {
                 dev_err(&pdev->dev, "no platform data defined\n");
                 return -EINVAL;
         }
@@ -1055,31 +1448,37 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
         }
 
         priv->irq = i;
-        pdata = pdev->dev.platform_data;
+        atomic_set(&priv->hw_usecnt, -1);
 
         j = 0;
         for (i = 0; i < ARRAY_SIZE(pdata->ch); i++) {
-                priv->ch[j].lcdc = priv;
-                memcpy(&priv->ch[j].cfg, &pdata->ch[i], sizeof(pdata->ch[i]));
+                struct sh_mobile_lcdc_chan *ch = priv->ch + j;
+
+                ch->lcdc = priv;
+                memcpy(&ch->cfg, &pdata->ch[i], sizeof(pdata->ch[i]));
 
-                error = sh_mobile_lcdc_check_interface(&priv->ch[j]);
+                error = sh_mobile_lcdc_check_interface(ch);
                 if (error) {
                         dev_err(&pdev->dev, "unsupported interface type\n");
                         goto err1;
                 }
-                init_waitqueue_head(&priv->ch[j].frame_end_wait);
-                init_completion(&priv->ch[j].vsync_completion);
-                priv->ch[j].pan_offset = 0;
+                init_waitqueue_head(&ch->frame_end_wait);
+                init_completion(&ch->vsync_completion);
+                ch->pan_offset = 0;
+
+                /* probe the backlight is there is one defined */
+                if (ch->cfg.bl_info.max_brightness)
+                        ch->bl = sh_mobile_lcdc_bl_probe(&pdev->dev, ch);
 
                 switch (pdata->ch[i].chan) {
                 case LCDC_CHAN_MAINLCD:
-                        priv->ch[j].enabled = 1 << 1;
-                        priv->ch[j].reg_offs = lcdc_offs_mainlcd;
+                        ch->enabled = 1 << 1;
+                        ch->reg_offs = lcdc_offs_mainlcd;
                         j++;
                         break;
                 case LCDC_CHAN_SUBLCD:
-                        priv->ch[j].enabled = 1 << 2;
-                        priv->ch[j].reg_offs = lcdc_offs_sublcd;
+                        ch->enabled = 1 << 2;
+                        ch->reg_offs = lcdc_offs_sublcd;
                         j++;
                         break;
                 }
@@ -1091,6 +1490,10 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                 goto err1;
         }
 
+        /* for dual channel LCDC (MAIN + SUB) force shared bpp setting */
+        if (j == 2)
+                priv->forced_bpp = pdata->ch[0].bpp;
+
         priv->base = ioremap_nocache(res->start, resource_size(res));
         if (!priv->base)
                 goto err1;
@@ -1101,71 +1504,112 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                 goto err1;
         }
 
+        priv->meram_dev = pdata->meram_dev;
+
         for (i = 0; i < j; i++) {
                 struct fb_var_screeninfo *var;
-                struct fb_videomode *lcd_cfg;
-                cfg = &priv->ch[i].cfg;
-
-                priv->ch[i].info = framebuffer_alloc(0, &pdev->dev);
-                if (!priv->ch[i].info) {
+                const struct fb_videomode *lcd_cfg, *max_cfg = NULL;
+                struct sh_mobile_lcdc_chan *ch = priv->ch + i;
+                struct sh_mobile_lcdc_chan_cfg *cfg = &ch->cfg;
+                const struct fb_videomode *mode = cfg->lcd_cfg;
+                unsigned long max_size = 0;
+                int k;
+                int num_cfg;
+
+                ch->info = framebuffer_alloc(0, &pdev->dev);
+                if (!ch->info) {
                         dev_err(&pdev->dev, "unable to allocate fb_info\n");
                         error = -ENOMEM;
                         break;
                 }
 
-                info = priv->ch[i].info;
+                info = ch->info;
                 var = &info->var;
-                lcd_cfg = &cfg->lcd_cfg;
                 info->fbops = &sh_mobile_lcdc_ops;
-                var->xres = var->xres_virtual = lcd_cfg->xres;
-                var->yres = lcd_cfg->yres;
-                /* Default Y virtual resolution is 2x panel size */
-                var->yres_virtual = var->yres * 2;
+                info->par = ch;
+
+                mutex_init(&ch->open_lock);
+
+                for (k = 0, lcd_cfg = mode;
+                     k < cfg->num_cfg && lcd_cfg;
+                     k++, lcd_cfg++) {
+                        unsigned long size = lcd_cfg->yres * lcd_cfg->xres;
+                        /* NV12 buffers must have even number of lines */
+                        if ((cfg->nonstd) && cfg->bpp == 12 &&
+                                        (lcd_cfg->yres & 0x1)) {
+                                dev_err(&pdev->dev, "yres must be multiple of 2"
+                                                " for YCbCr420 mode.\n");
+                                error = -EINVAL;
+                                goto err1;
+                        }
+
+                        if (size > max_size) {
+                                max_cfg = lcd_cfg;
+                                max_size = size;
+                        }
+                }
+
+                if (!mode)
+                        max_size = MAX_XRES * MAX_YRES;
+                else if (max_cfg)
+                        dev_dbg(&pdev->dev, "Found largest videomode %ux%u\n",
+                                max_cfg->xres, max_cfg->yres);
+
+                info->fix = sh_mobile_lcdc_fix;
+                info->fix.smem_len = max_size * 2 * cfg->bpp / 8;
+
+                 /* Only pan in 2 line steps for NV12 */
+                if (cfg->nonstd && cfg->bpp == 12)
+                        info->fix.ypanstep = 2;
+
+                if (!mode) {
+                        mode = &default_720p;
+                        num_cfg = 1;
+                } else {
+                        num_cfg = cfg->num_cfg;
+                }
+
+                fb_videomode_to_modelist(mode, num_cfg, &info->modelist);
+
+                fb_videomode_to_var(var, mode);
                 var->width = cfg->lcd_size_cfg.width;
                 var->height = cfg->lcd_size_cfg.height;
+                /* Default Y virtual resolution is 2x panel size */
+                var->yres_virtual = var->yres * 2;
                 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);
+
+                error = sh_mobile_lcdc_set_bpp(var, cfg->bpp, cfg->nonstd);
                 if (error)
                         break;
 
-                info->fix = sh_mobile_lcdc_fix;
-                info->fix.line_length = lcd_cfg->xres * (cfg->bpp / 8);
-                info->fix.smem_len = info->fix.line_length *
-                        var->yres_virtual;
-
                 buf = dma_alloc_coherent(&pdev->dev, info->fix.smem_len,
-                                         &priv->ch[i].dma_handle, GFP_KERNEL);
+                                         &ch->dma_handle, GFP_KERNEL);
                 if (!buf) {
                         dev_err(&pdev->dev, "unable to allocate buffer\n");
                         error = -ENOMEM;
                         break;
                 }
 
-                info->pseudo_palette = &priv->ch[i].pseudo_palette;
+                info->pseudo_palette = &ch->pseudo_palette;
                 info->flags = FBINFO_FLAG_DEFAULT;
 
                 error = fb_alloc_cmap(&info->cmap, PALETTE_NR, 0);
                 if (error < 0) {
                         dev_err(&pdev->dev, "unable to allocate cmap\n");
                         dma_free_coherent(&pdev->dev, info->fix.smem_len,
-                                          buf, priv->ch[i].dma_handle);
+                                          buf, ch->dma_handle);
                         break;
                 }
 
-                memset(buf, 0, info->fix.smem_len);
-                info->fix.smem_start = priv->ch[i].dma_handle;
+                info->fix.smem_start = ch->dma_handle;
+                if (var->nonstd)
+                        info->fix.line_length = var->xres;
+                else
+                        info->fix.line_length = var->xres * (cfg->bpp / 8);
+
                 info->screen_base = buf;
                 info->device = &pdev->dev;
-                info->par = &priv->ch[i];
+                ch->display_var = *var;
         }
 
         if (error)
@@ -1191,6 +1635,8 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                         }
                 }
 
+                info->bl_dev = ch->bl;
+
                 error = register_framebuffer(info);
                 if (error < 0)
                         goto err1;
@@ -1200,8 +1646,7 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                          pdev->name,
                          (ch->cfg.chan == LCDC_CHAN_MAINLCD) ?
                          "mainlcd" : "sublcd",
-                         (int) ch->cfg.lcd_cfg.xres,
-                         (int) ch->cfg.lcd_cfg.yres,
+                         info->var.xres, info->var.yres,
                          ch->cfg.bpp);
 
                 /* deferred io mode: disable clock to save power */
@@ -1243,12 +1688,19 @@ 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);
         }
 
+        for (i = 0; i < ARRAY_SIZE(priv->ch); i++) {
+                if (priv->ch[i].bl)
+                        sh_mobile_lcdc_bl_remove(priv->ch[i].bl);
+        }
+
         if (priv->dot_clk)
                 clk_put(priv->dot_clk);
 
diff --git a/drivers/video/sh_mobile_lcdcfb.h b/drivers/video/sh_mobile_lcdcfb.h
new file mode 100644
index 000000000000..aeed6687e6a7
--- /dev/null
+++ b/drivers/video/sh_mobile_lcdcfb.h
@@ -0,0 +1,45 @@
+#ifndef SH_MOBILE_LCDCFB_H
+#define SH_MOBILE_LCDCFB_H
+
+#include <linux/completion.h>
+#include <linux/fb.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+
+/* per-channel registers */
+enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
+       LDSM2R, LDSA1R, LDSA2R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR,
+       LDHAJR,
+       NR_CH_REGS };
+
+#define PALETTE_NR 16
+
+struct sh_mobile_lcdc_priv;
+struct fb_info;
+struct backlight_device;
+
+struct sh_mobile_lcdc_chan {
+        struct sh_mobile_lcdc_priv *lcdc;
+        unsigned long *reg_offs;
+        unsigned long ldmt1r_value;
+        unsigned long enabled; /* ME and SE in LDCNT2R */
+        struct sh_mobile_lcdc_chan_cfg cfg;
+        u32 pseudo_palette[PALETTE_NR];
+        unsigned long saved_ch_regs[NR_CH_REGS];
+        struct fb_info *info;
+        struct backlight_device *bl;
+        dma_addr_t dma_handle;
+        struct fb_deferred_io defio;
+        struct scatterlist *sglist;
+        unsigned long frame_end;
+        unsigned long pan_offset;
+        wait_queue_head_t frame_end_wait;
+        struct completion vsync_completion;
+        struct fb_var_screeninfo display_var;
+        int use_count;
+        int blank_status;
+        struct mutex open_lock;         /* protects the use counter */
+        int meram_enabled;
+};
+
+#endif
diff --git a/drivers/video/sh_mobile_meram.c b/drivers/video/sh_mobile_meram.c
new file mode 100644
index 000000000000..cc7d7329dc15
--- /dev/null
+++ b/drivers/video/sh_mobile_meram.c
@@ -0,0 +1,567 @@
+/*
+ * SuperH Mobile MERAM Driver for SuperH Mobile LCDC Driver
+ *
+ * Copyright (c) 2011   Damian Hobson-Garcia <dhobsong@igel.co.jp>
+ *                      Takanari Hayama <taki@igel.co.jp>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include "sh_mobile_meram.h"
+
+/* meram registers */
+#define MExxCTL 0x0
+#define MExxBSIZE 0x4
+#define MExxMNCF 0x8
+#define MExxSARA 0x10
+#define MExxSARB 0x14
+#define MExxSBSIZE 0x18
+
+#define MERAM_MExxCTL_VAL(ctl, next_icb, addr)  \
+        ((ctl) | (((next_icb) & 0x1f) << 11) | (((addr) & 0x7ff) << 16))
+#define MERAM_MExxBSIZE_VAL(a, b, c) \
+        (((a) << 28) | ((b) << 16) | (c))
+
+#define MEVCR1 0x4
+#define MEACTS 0x10
+#define MEQSEL1 0x40
+#define MEQSEL2 0x44
+
+/* settings */
+#define MERAM_SEC_LINE 15
+#define MERAM_LINE_WIDTH 2048
+
+/*
+ * MERAM/ICB access functions
+ */
+
+#define MERAM_ICB_OFFSET(base, idx, off)        \
+        ((base) + (0x400 + ((idx) * 0x20) + (off)))
+
+static inline void meram_write_icb(void __iomem *base, int idx, int off,
+        unsigned long val)
+{
+        iowrite32(val, MERAM_ICB_OFFSET(base, idx, off));
+}
+
+static inline unsigned long meram_read_icb(void __iomem *base, int idx, int off)
+{
+        return ioread32(MERAM_ICB_OFFSET(base, idx, off));
+}
+
+static inline void meram_write_reg(void __iomem *base, int off,
+                unsigned long val)
+{
+        iowrite32(val, base + off);
+}
+
+static inline unsigned long meram_read_reg(void __iomem *base, int off)
+{
+        return ioread32(base + off);
+}
+
+/*
+ * register ICB
+ */
+
+#define MERAM_CACHE_START(p)     ((p) >> 16)
+#define MERAM_CACHE_END(p)       ((p) & 0xffff)
+#define MERAM_CACHE_SET(o, s)    ((((o) & 0xffff) << 16) | \
+                                  (((o) + (s) - 1) & 0xffff))
+
+/*
+ * check if there's no overlaps in MERAM allocation.
+ */
+
+static inline int meram_check_overlap(struct sh_mobile_meram_priv *priv,
+                                      struct sh_mobile_meram_icb *new)
+{
+        int i;
+        int used_start, used_end, meram_start, meram_end;
+
+        /* valid ICB? */
+        if (new->marker_icb & ~0x1f || new->cache_icb & ~0x1f)
+                return 1;
+
+        if (test_bit(new->marker_icb, &priv->used_icb) ||
+                        test_bit(new->cache_icb,  &priv->used_icb))
+                return  1;
+
+        for (i = 0; i < priv->used_meram_cache_regions; i++) {
+                used_start = MERAM_CACHE_START(priv->used_meram_cache[i]);
+                used_end   = MERAM_CACHE_END(priv->used_meram_cache[i]);
+                meram_start = new->meram_offset;
+                meram_end   = new->meram_offset + new->meram_size;
+
+                if ((meram_start >= used_start && meram_start < used_end) ||
+                        (meram_end > used_start && meram_end < used_end))
+                        return 1;
+        }
+
+        return 0;
+}
+
+/*
+ * mark the specified ICB as used
+ */
+
+static inline void meram_mark(struct sh_mobile_meram_priv *priv,
+                              struct sh_mobile_meram_icb *new)
+{
+        int n;
+
+        if (new->marker_icb < 0 || new->cache_icb < 0)
+                return;
+
+        __set_bit(new->marker_icb, &priv->used_icb);
+        __set_bit(new->cache_icb, &priv->used_icb);
+
+        n = priv->used_meram_cache_regions;
+
+        priv->used_meram_cache[n] = MERAM_CACHE_SET(new->meram_offset,
+                                                    new->meram_size);
+
+        priv->used_meram_cache_regions++;
+}
+
+/*
+ * unmark the specified ICB as used
+ */
+
+static inline void meram_unmark(struct sh_mobile_meram_priv *priv,
+                                struct sh_mobile_meram_icb *icb)
+{
+        int i;
+        unsigned long pattern;
+
+        if (icb->marker_icb < 0 || icb->cache_icb < 0)
+                return;
+
+        __clear_bit(icb->marker_icb, &priv->used_icb);
+        __clear_bit(icb->cache_icb, &priv->used_icb);
+
+        pattern = MERAM_CACHE_SET(icb->meram_offset, icb->meram_size);
+        for (i = 0; i < priv->used_meram_cache_regions; i++) {
+                if (priv->used_meram_cache[i] == pattern) {
+                        while (i < priv->used_meram_cache_regions - 1) {
+                                priv->used_meram_cache[i] =
+                                        priv->used_meram_cache[i + 1] ;
+                                i++;
+                        }
+                        priv->used_meram_cache[i] = 0;
+                        priv->used_meram_cache_regions--;
+                        break;
+                }
+        }
+}
+
+/*
+ * is this a YCbCr(NV12, NV16 or NV24) colorspace
+ */
+static inline int is_nvcolor(int cspace)
+{
+        if (cspace == SH_MOBILE_MERAM_PF_NV ||
+                        cspace == SH_MOBILE_MERAM_PF_NV24)
+                return 1;
+        return 0;
+}
+
+/*
+ * set the next address to fetch
+ */
+static inline void meram_set_next_addr(struct sh_mobile_meram_priv *priv,
+                                       struct sh_mobile_meram_cfg *cfg,
+                                       unsigned long base_addr_y,
+                                       unsigned long base_addr_c)
+{
+        unsigned long target;
+
+        target = (cfg->current_reg) ? MExxSARA : MExxSARB;
+        cfg->current_reg ^= 1;
+
+        /* set the next address to fetch */
+        meram_write_icb(priv->base, cfg->icb[0].cache_icb,  target,
+                        base_addr_y);
+        meram_write_icb(priv->base, cfg->icb[0].marker_icb, target,
+                        base_addr_y + cfg->icb[0].cache_unit);
+
+        if (is_nvcolor(cfg->pixelformat)) {
+                meram_write_icb(priv->base, cfg->icb[1].cache_icb,  target,
+                                base_addr_c);
+                meram_write_icb(priv->base, cfg->icb[1].marker_icb, target,
+                                base_addr_c + cfg->icb[1].cache_unit);
+        }
+}
+
+/*
+ * get the next ICB address
+ */
+static inline void meram_get_next_icb_addr(struct sh_mobile_meram_info *pdata,
+                                           struct sh_mobile_meram_cfg *cfg,
+                                           unsigned long *icb_addr_y,
+                                           unsigned long *icb_addr_c)
+{
+        unsigned long icb_offset;
+
+        if (pdata->addr_mode == SH_MOBILE_MERAM_MODE0)
+                icb_offset = 0x80000000 | (cfg->current_reg << 29);
+        else
+                icb_offset = 0xc0000000 | (cfg->current_reg << 23);
+
+        *icb_addr_y = icb_offset | (cfg->icb[0].marker_icb << 24);
+        if (is_nvcolor(cfg->pixelformat))
+                *icb_addr_c = icb_offset | (cfg->icb[1].marker_icb << 24);
+}
+
+#define MERAM_CALC_BYTECOUNT(x, y) \
+        (((x) * (y) + (MERAM_LINE_WIDTH - 1)) & ~(MERAM_LINE_WIDTH - 1))
+
+/*
+ * initialize MERAM
+ */
+
+static int meram_init(struct sh_mobile_meram_priv *priv,
+                      struct sh_mobile_meram_icb *icb,
+                      int xres, int yres, int *out_pitch)
+{
+        unsigned long total_byte_count = MERAM_CALC_BYTECOUNT(xres, yres);
+        unsigned long bnm;
+        int lcdc_pitch, xpitch, line_cnt;
+        int save_lines;
+
+        /* adjust pitch to 1024, 2048, 4096 or 8192 */
+        lcdc_pitch = (xres - 1) | 1023;
+        lcdc_pitch = lcdc_pitch | (lcdc_pitch >> 1);
+        lcdc_pitch = lcdc_pitch | (lcdc_pitch >> 2);
+        lcdc_pitch += 1;
+
+        /* derive settings */
+        if (lcdc_pitch == 8192 && yres >= 1024) {
+                lcdc_pitch = xpitch = MERAM_LINE_WIDTH;
+                line_cnt = total_byte_count >> 11;
+                *out_pitch = xres;
+                save_lines = (icb->meram_size / 16 / MERAM_SEC_LINE);
+                save_lines *= MERAM_SEC_LINE;
+        } else {
+                xpitch = xres;
+                line_cnt = yres;
+                *out_pitch = lcdc_pitch;
+                save_lines = icb->meram_size / (lcdc_pitch >> 10) / 2;
+                save_lines &= 0xff;
+        }
+        bnm = (save_lines - 1) << 16;
+
+        /* TODO: we better to check if we have enough MERAM buffer size */
+
+        /* set up ICB */
+        meram_write_icb(priv->base, icb->cache_icb,  MExxBSIZE,
+                        MERAM_MExxBSIZE_VAL(0x0, line_cnt - 1, xpitch - 1));
+        meram_write_icb(priv->base, icb->marker_icb, MExxBSIZE,
+                        MERAM_MExxBSIZE_VAL(0xf, line_cnt - 1, xpitch - 1));
+
+        meram_write_icb(priv->base, icb->cache_icb,  MExxMNCF, bnm);
+        meram_write_icb(priv->base, icb->marker_icb, MExxMNCF, bnm);
+
+        meram_write_icb(priv->base, icb->cache_icb,  MExxSBSIZE, xpitch);
+        meram_write_icb(priv->base, icb->marker_icb, MExxSBSIZE, xpitch);
+
+        /* save a cache unit size */
+        icb->cache_unit = xres * save_lines;
+
+        /*
+         * Set MERAM for framebuffer
+         *
+         * 0x70f:  WD = 0x3, WS=0x1, CM=0x1, MD=FB mode
+         * we also chain the cache_icb and the marker_icb.
+         * we also split the allocated MERAM buffer between two ICBs.
+         */
+        meram_write_icb(priv->base, icb->cache_icb, MExxCTL,
+                        MERAM_MExxCTL_VAL(0x70f, icb->marker_icb,
+                                          icb->meram_offset));
+        meram_write_icb(priv->base, icb->marker_icb, MExxCTL,
+                        MERAM_MExxCTL_VAL(0x70f, icb->cache_icb,
+                                          icb->meram_offset +
+                                          icb->meram_size / 2));
+
+        return 0;
+}
+
+static void meram_deinit(struct sh_mobile_meram_priv *priv,
+                        struct sh_mobile_meram_icb *icb)
+{
+        /* disable ICB */
+        meram_write_icb(priv->base, icb->cache_icb,  MExxCTL, 0);
+        meram_write_icb(priv->base, icb->marker_icb, MExxCTL, 0);
+        icb->cache_unit = 0;
+}
+
+/*
+ * register the ICB
+ */
+
+static int sh_mobile_meram_register(struct sh_mobile_meram_info *pdata,
+                                    struct sh_mobile_meram_cfg *cfg,
+                                    int xres, int yres, int pixelformat,
+                                    unsigned long base_addr_y,
+                                    unsigned long base_addr_c,
+                                    unsigned long *icb_addr_y,
+                                    unsigned long *icb_addr_c,
+                                    int *pitch)
+{
+        struct platform_device *pdev;
+        struct sh_mobile_meram_priv *priv;
+        int n, out_pitch;
+        int error = 0;
+
+        if (!pdata || !pdata->priv || !pdata->pdev || !cfg)
+                return -EINVAL;
+
+        if (pixelformat != SH_MOBILE_MERAM_PF_NV &&
+            pixelformat != SH_MOBILE_MERAM_PF_NV24 &&
+            pixelformat != SH_MOBILE_MERAM_PF_RGB)
+                return -EINVAL;
+
+        priv = pdata->priv;
+        pdev = pdata->pdev;
+
+        dev_dbg(&pdev->dev, "registering %dx%d (%s) (y=%08lx, c=%08lx)",
+                xres, yres, (!pixelformat) ? "yuv" : "rgb",
+                base_addr_y, base_addr_c);
+
+        mutex_lock(&priv->lock);
+
+        /* we can't handle wider than 8192px */
+        if (xres > 8192) {
+                dev_err(&pdev->dev, "width exceeding the limit (> 8192).");
+                error = -EINVAL;
+                goto err;
+        }
+
+        if (priv->used_meram_cache_regions + 2 > SH_MOBILE_MERAM_ICB_NUM) {
+                dev_err(&pdev->dev, "no more ICB available.");
+                error = -EINVAL;
+                goto err;
+        }
+
+        /* do we have at least one ICB config? */
+        if (cfg->icb[0].marker_icb < 0 || cfg->icb[0].cache_icb < 0) {
+                dev_err(&pdev->dev, "at least one ICB is required.");
+                error = -EINVAL;
+                goto err;
+        }
+
+        /* make sure that there's no overlaps */
+        if (meram_check_overlap(priv, &cfg->icb[0])) {
+                dev_err(&pdev->dev, "conflicting config detected.");
+                error = -EINVAL;
+                goto err;
+        }
+        n = 1;
+
+        /* do the same if we have the second ICB set */
+        if (cfg->icb[1].marker_icb >= 0 && cfg->icb[1].cache_icb >= 0) {
+                if (meram_check_overlap(priv, &cfg->icb[1])) {
+                        dev_err(&pdev->dev, "conflicting config detected.");
+                        error = -EINVAL;
+                        goto err;
+                }
+                n = 2;
+        }
+
+        if (is_nvcolor(pixelformat) && n != 2) {
+                dev_err(&pdev->dev, "requires two ICB sets for planar Y/C.");
+                error =  -EINVAL;
+                goto err;
+        }
+
+        /* we now register the ICB */
+        cfg->pixelformat = pixelformat;
+        meram_mark(priv, &cfg->icb[0]);
+        if (is_nvcolor(pixelformat))
+                meram_mark(priv, &cfg->icb[1]);
+
+        /* initialize MERAM */
+        meram_init(priv, &cfg->icb[0], xres, yres, &out_pitch);
+        *pitch = out_pitch;
+        if (pixelformat == SH_MOBILE_MERAM_PF_NV)
+                meram_init(priv, &cfg->icb[1], xres, (yres + 1) / 2,
+                        &out_pitch);
+        else if (pixelformat == SH_MOBILE_MERAM_PF_NV24)
+                meram_init(priv, &cfg->icb[1], 2 * xres, (yres + 1) / 2,
+                        &out_pitch);
+
+        cfg->current_reg = 1;
+        meram_set_next_addr(priv, cfg, base_addr_y, base_addr_c);
+        meram_get_next_icb_addr(pdata, cfg, icb_addr_y, icb_addr_c);
+
+        dev_dbg(&pdev->dev, "registered - can access via y=%08lx, c=%08lx",
+                *icb_addr_y, *icb_addr_c);
+
+err:
+        mutex_unlock(&priv->lock);
+        return error;
+}
+
+static int sh_mobile_meram_unregister(struct sh_mobile_meram_info *pdata,
+                                      struct sh_mobile_meram_cfg *cfg)
+{
+        struct sh_mobile_meram_priv *priv;
+
+        if (!pdata || !pdata->priv || !cfg)
+                return -EINVAL;
+
+        priv = pdata->priv;
+
+        mutex_lock(&priv->lock);
+
+        /* deinit & unmark */
+        if (is_nvcolor(cfg->pixelformat)) {
+                meram_deinit(priv, &cfg->icb[1]);
+                meram_unmark(priv, &cfg->icb[1]);
+        }
+        meram_deinit(priv, &cfg->icb[0]);
+        meram_unmark(priv, &cfg->icb[0]);
+
+        mutex_unlock(&priv->lock);
+
+        return 0;
+}
+
+static int sh_mobile_meram_update(struct sh_mobile_meram_info *pdata,
+                                  struct sh_mobile_meram_cfg *cfg,
+                                  unsigned long base_addr_y,
+                                  unsigned long base_addr_c,
+                                  unsigned long *icb_addr_y,
+                                  unsigned long *icb_addr_c)
+{
+        struct sh_mobile_meram_priv *priv;
+
+        if (!pdata || !pdata->priv || !cfg)
+                return -EINVAL;
+
+        priv = pdata->priv;
+
+        mutex_lock(&priv->lock);
+
+        meram_set_next_addr(priv, cfg, base_addr_y, base_addr_c);
+        meram_get_next_icb_addr(pdata, cfg, icb_addr_y, icb_addr_c);
+
+        mutex_unlock(&priv->lock);
+
+        return 0;
+}
+
+static struct sh_mobile_meram_ops sh_mobile_meram_ops = {
+        .module                 = THIS_MODULE,
+        .meram_register         = sh_mobile_meram_register,
+        .meram_unregister       = sh_mobile_meram_unregister,
+        .meram_update           = sh_mobile_meram_update,
+};
+
+/*
+ * initialize MERAM
+ */
+
+static int sh_mobile_meram_remove(struct platform_device *pdev);
+
+static int __devinit sh_mobile_meram_probe(struct platform_device *pdev)
+{
+        struct sh_mobile_meram_priv *priv;
+        struct sh_mobile_meram_info *pdata = pdev->dev.platform_data;
+        struct resource *res;
+        int error;
+
+        if (!pdata) {
+                dev_err(&pdev->dev, "no platform data defined\n");
+                return -EINVAL;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "cannot get platform resources\n");
+                return -ENOENT;
+        }
+
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv) {
+                dev_err(&pdev->dev, "cannot allocate device data\n");
+                return -ENOMEM;
+        }
+
+        platform_set_drvdata(pdev, priv);
+
+        /* initialize private data */
+        mutex_init(&priv->lock);
+        priv->base = ioremap_nocache(res->start, resource_size(res));
+        if (!priv->base) {
+                dev_err(&pdev->dev, "ioremap failed\n");
+                error = -EFAULT;
+                goto err;
+        }
+        pdata->ops = &sh_mobile_meram_ops;
+        pdata->priv = priv;
+        pdata->pdev = pdev;
+
+        /* initialize ICB addressing mode */
+        if (pdata->addr_mode == SH_MOBILE_MERAM_MODE1)
+                meram_write_reg(priv->base, MEVCR1, 1 << 29);
+
+        dev_info(&pdev->dev, "sh_mobile_meram initialized.");
+
+        return 0;
+
+err:
+        sh_mobile_meram_remove(pdev);
+
+        return error;
+}
+
+
+static int sh_mobile_meram_remove(struct platform_device *pdev)
+{
+        struct sh_mobile_meram_priv *priv = platform_get_drvdata(pdev);
+
+        if (priv->base)
+                iounmap(priv->base);
+
+        mutex_destroy(&priv->lock);
+
+        kfree(priv);
+
+        return 0;
+}
+
+static struct platform_driver sh_mobile_meram_driver = {
+        .driver = {
+                .name           = "sh_mobile_meram",
+                .owner          = THIS_MODULE,
+        },
+        .probe          = sh_mobile_meram_probe,
+        .remove         = sh_mobile_meram_remove,
+};
+
+static int __init sh_mobile_meram_init(void)
+{
+        return platform_driver_register(&sh_mobile_meram_driver);
+}
+
+static void __exit sh_mobile_meram_exit(void)
+{
+        platform_driver_unregister(&sh_mobile_meram_driver);
+}
+
+module_init(sh_mobile_meram_init);
+module_exit(sh_mobile_meram_exit);
+
+MODULE_DESCRIPTION("SuperH Mobile MERAM driver");
+MODULE_AUTHOR("Damian Hobson-Garcia / Takanari Hayama");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sh_mobile_meram.h b/drivers/video/sh_mobile_meram.h
new file mode 100644
index 000000000000..82c54fbce8bd
--- /dev/null
+++ b/drivers/video/sh_mobile_meram.h
@@ -0,0 +1,41 @@
+#ifndef __sh_mobile_meram_h__
+#define __sh_mobile_meram_h__
+
+#include <linux/mutex.h>
+#include <video/sh_mobile_meram.h>
+
+/*
+ * MERAM private
+ */
+
+#define MERAM_ICB_Y 0x1
+#define MERAM_ICB_C 0x2
+
+/* MERAM cache size */
+#define SH_MOBILE_MERAM_ICB_NUM         32
+
+#define SH_MOBILE_MERAM_CACHE_OFFSET(p) ((p) >> 16)
+#define SH_MOBILE_MERAM_CACHE_SIZE(p)   ((p) & 0xffff)
+
+struct sh_mobile_meram_priv {
+        void __iomem    *base;
+        struct mutex    lock;
+        unsigned long   used_icb;
+        int             used_meram_cache_regions;
+        unsigned long   used_meram_cache[SH_MOBILE_MERAM_ICB_NUM];
+};
+
+int sh_mobile_meram_alloc_icb(const struct sh_mobile_meram_cfg *cfg,
+                   int xres,
+                   int yres,
+                   unsigned int base_addr,
+                   int yuv_mode,
+                   int *marker_icb,
+                   int *out_pitch);
+
+void sh_mobile_meram_free_icb(int marker_icb);
+
+#define SH_MOBILE_MERAM_START(ind, ab) \
+        (0xC0000000 | ((ab & 0x1) << 23) | ((ind & 0x1F) << 24))
+
+#endif /* !__sh_mobile_meram_h__ */
diff --git a/drivers/video/sis/init.c b/drivers/video/sis/init.c
index c311ad3c3687..66de832361cc 100644
--- a/drivers/video/sis/init.c
+++ b/drivers/video/sis/init.c
@@ -56,17 +56,13 @@
  * Used by permission.
  */
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #include "init.h"
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 #include "300vtbl.h"
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 #include "310vtbl.h"
 #endif
 
@@ -78,7 +74,7 @@
 /*         POINTER INITIALIZATION            */
 /*********************************************/
 
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
 static void
 InitCommonPointer(struct SiS_Private *SiS_Pr)
 {
@@ -160,7 +156,7 @@ InitCommonPointer(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static void
 InitTo300Pointer(struct SiS_Private *SiS_Pr)
 {
@@ -237,7 +233,7 @@ InitTo300Pointer(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void
 InitTo310Pointer(struct SiS_Private *SiS_Pr)
 {
@@ -321,13 +317,13 @@ bool
 SiSInitPtr(struct SiS_Private *SiS_Pr)
 {
    if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
       InitTo300Pointer(SiS_Pr);
 #else
       return false;
 #endif
    } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       InitTo310Pointer(SiS_Pr);
 #else
       return false;
@@ -340,9 +336,7 @@ SiSInitPtr(struct SiS_Private *SiS_Pr)
 /*            HELPER: Get ModeID             */
 /*********************************************/
 
-#ifndef SIS_XORG_XF86
 static
-#endif
 unsigned short
 SiS_GetModeID(int VGAEngine, unsigned int VBFlags, int HDisplay, int VDisplay,
                 int Depth, bool FSTN, int LCDwidth, int LCDheight)
@@ -882,59 +876,59 @@ SiS_GetModeID_VGA2(int VGAEngine, unsigned int VBFlags, int HDisplay, int VDispl
 /*********************************************/
 
 void
-SiS_SetReg(SISIOADDRESS port, unsigned short index, unsigned short data)
+SiS_SetReg(SISIOADDRESS port, u8 index, u8 data)
 {
-   OutPortByte(port, index);
-   OutPortByte(port + 1, data);
+        outb(index, port);
+        outb(data, port + 1);
 }
 
 void
-SiS_SetRegByte(SISIOADDRESS port, unsigned short data)
+SiS_SetRegByte(SISIOADDRESS port, u8 data)
 {
-   OutPortByte(port, data);
+        outb(data, port);
 }
 
 void
-SiS_SetRegShort(SISIOADDRESS port, unsigned short data)
+SiS_SetRegShort(SISIOADDRESS port, u16 data)
 {
-   OutPortWord(port, data);
+        outw(data, port);
 }
 
 void
-SiS_SetRegLong(SISIOADDRESS port, unsigned int data)
+SiS_SetRegLong(SISIOADDRESS port, u32 data)
 {
-   OutPortLong(port, data);
+        outl(data, port);
 }
 
-unsigned char
-SiS_GetReg(SISIOADDRESS port, unsigned short index)
+u8
+SiS_GetReg(SISIOADDRESS port, u8 index)
 {
-   OutPortByte(port, index);
-   return(InPortByte(port + 1));
+        outb(index, port);
+        return inb(port + 1);
 }
 
-unsigned char
+u8
 SiS_GetRegByte(SISIOADDRESS port)
 {
-   return(InPortByte(port));
+        return inb(port);
 }
 
-unsigned short
+u16
 SiS_GetRegShort(SISIOADDRESS port)
 {
-   return(InPortWord(port));
+        return inw(port);
 }
 
-unsigned int
+u32
 SiS_GetRegLong(SISIOADDRESS port)
 {
-   return(InPortLong(port));
+        return inl(port);
 }
 
 void
-SiS_SetRegANDOR(SISIOADDRESS Port, unsigned short Index, unsigned short DataAND, unsigned short DataOR)
+SiS_SetRegANDOR(SISIOADDRESS Port, u8 Index, u8 DataAND, u8 DataOR)
 {
-   unsigned short temp;
+   u8 temp;
 
    temp = SiS_GetReg(Port, Index);
    temp = (temp & (DataAND)) | DataOR;
@@ -942,9 +936,9 @@ SiS_SetRegANDOR(SISIOADDRESS Port, unsigned short Index, unsigned short DataAND,
 }
 
 void
-SiS_SetRegAND(SISIOADDRESS Port, unsigned short Index, unsigned short DataAND)
+SiS_SetRegAND(SISIOADDRESS Port, u8 Index, u8 DataAND)
 {
-   unsigned short temp;
+   u8 temp;
 
    temp = SiS_GetReg(Port, Index);
    temp &= DataAND;
@@ -952,9 +946,9 @@ SiS_SetRegAND(SISIOADDRESS Port, unsigned short Index, unsigned short DataAND)
 }
 
 void
-SiS_SetRegOR(SISIOADDRESS Port, unsigned short Index, unsigned short DataOR)
+SiS_SetRegOR(SISIOADDRESS Port, u8 Index, u8 DataOR)
 {
-   unsigned short temp;
+   u8 temp;
 
    temp = SiS_GetReg(Port, Index);
    temp |= DataOR;
@@ -1089,7 +1083,7 @@ static void
 SiSInitPCIetc(struct SiS_Private *SiS_Pr)
 {
    switch(SiS_Pr->ChipType) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    case SIS_300:
    case SIS_540:
    case SIS_630:
@@ -1108,7 +1102,7 @@ SiSInitPCIetc(struct SiS_Private *SiS_Pr)
       SiS_SetRegOR(SiS_Pr->SiS_P3c4,0x1E,0x5A);
       break;
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    case SIS_315H:
    case SIS_315:
    case SIS_315PRO:
@@ -1152,9 +1146,7 @@ SiSInitPCIetc(struct SiS_Private *SiS_Pr)
 /*             HELPER: SetLVDSetc            */
 /*********************************************/
 
-#ifdef SIS_LINUX_KERNEL
 static
-#endif
 void
 SiSSetLVDSetc(struct SiS_Private *SiS_Pr)
 {
@@ -1174,7 +1166,7 @@ SiSSetLVDSetc(struct SiS_Private *SiS_Pr)
    if((temp == 1) || (temp == 2)) return;
 
    switch(SiS_Pr->ChipType) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    case SIS_540:
    case SIS_630:
    case SIS_730:
@@ -1188,7 +1180,7 @@ SiSSetLVDSetc(struct SiS_Private *SiS_Pr)
         }
         break;
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    case SIS_550:
    case SIS_650:
    case SIS_740:
@@ -1420,9 +1412,7 @@ SiS_ResetSegmentRegisters(struct SiS_Private *SiS_Pr)
 /*             HELPER: GetVBType             */
 /*********************************************/
 
-#ifdef SIS_LINUX_KERNEL
 static
-#endif
 void
 SiS_GetVBType(struct SiS_Private *SiS_Pr)
 {
@@ -1487,7 +1477,6 @@ SiS_GetVBType(struct SiS_Private *SiS_Pr)
 /*           HELPER: Check RAM size          */
 /*********************************************/
 
-#ifdef SIS_LINUX_KERNEL
 static bool
 SiS_CheckMemorySize(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
                 unsigned short ModeIdIndex)
@@ -1501,13 +1490,12 @@ SiS_CheckMemorySize(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    if(AdapterMemSize < memorysize) return false;
    return true;
 }
-#endif
 
 /*********************************************/
 /*           HELPER: Get DRAM type           */
 /*********************************************/
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static unsigned char
 SiS_Get310DRAMType(struct SiS_Private *SiS_Pr)
 {
@@ -1574,7 +1562,6 @@ SiS_GetMCLK(struct SiS_Private *SiS_Pr)
 /*           HELPER: ClearBuffer             */
 /*********************************************/
 
-#ifdef SIS_LINUX_KERNEL
 static void
 SiS_ClearBuffer(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 {
@@ -1587,7 +1574,7 @@ SiS_ClearBuffer(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 
    if(SiS_Pr->SiS_ModeType >= ModeEGA) {
       if(ModeNo > 0x13) {
-         SiS_SetMemory(memaddr, memsize, 0);
+         memset_io(memaddr, 0, memsize);
       } else {
          pBuffer = (unsigned short SISIOMEMTYPE *)memaddr;
          for(i = 0; i < 0x4000; i++) writew(0x0000, &pBuffer[i]);
@@ -1596,10 +1583,9 @@ SiS_ClearBuffer(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
       pBuffer = (unsigned short SISIOMEMTYPE *)memaddr;
       for(i = 0; i < 0x4000; i++) writew(0x0720, &pBuffer[i]);
    } else {
-      SiS_SetMemory(memaddr, 0x8000, 0);
+      memset_io(memaddr, 0, 0x8000);
    }
 }
-#endif
 
 /*********************************************/
 /*           HELPER: SearchModeID            */
@@ -2132,7 +2118,7 @@ SiS_SetCRT1CRTC(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       SiS_SetReg(SiS_Pr->SiS_P3d4,0x14,0x4F);
    }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType == XGI_20) {
       SiS_SetReg(SiS_Pr->SiS_P3d4,0x04,crt1data[4] - 1);
       if(!(temp = crt1data[5] & 0x1f)) {
@@ -2215,7 +2201,7 @@ SiS_SetCRT1VCLK(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    SiS_SetReg(SiS_Pr->SiS_P3c4,0x2c,clkb);
 
    if(SiS_Pr->ChipType >= SIS_315H) {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       SiS_SetReg(SiS_Pr->SiS_P3c4,0x2D,0x01);
       if(SiS_Pr->ChipType == XGI_20) {
          unsigned short mf = SiS_GetModeFlag(SiS_Pr, ModeNo, ModeIdIndex);
@@ -2236,7 +2222,7 @@ SiS_SetCRT1VCLK(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 /*                  FIFO                     */
 /*********************************************/
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 void
 SiS_GetFIFOThresholdIndex300(struct SiS_Private *SiS_Pr, unsigned short *idx1,
                 unsigned short *idx2)
@@ -2506,11 +2492,7 @@ SiS_SetCRT1FIFO_630(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x09,0x80,data);
 
   /* Write foreground and background queue */
-#ifdef SIS_LINUX_KERNEL
    templ = sisfb_read_nbridge_pci_dword(SiS_Pr, 0x50);
-#else
-   templ = pciReadLong(0x00000000, 0x50);
-#endif
 
    if(SiS_Pr->ChipType == SIS_730) {
 
@@ -2530,13 +2512,8 @@ SiS_SetCRT1FIFO_630(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 
    }
 
-#ifdef SIS_LINUX_KERNEL
    sisfb_write_nbridge_pci_dword(SiS_Pr, 0x50, templ);
    templ = sisfb_read_nbridge_pci_dword(SiS_Pr, 0xA0);
-#else
-   pciWriteLong(0x00000000, 0x50, templ);
-   templ = pciReadLong(0x00000000, 0xA0);
-#endif
 
    /* GUI grant timer (PCI config 0xA3) */
    if(SiS_Pr->ChipType == SIS_730) {
@@ -2552,15 +2529,11 @@ SiS_SetCRT1FIFO_630(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 
    }
 
-#ifdef SIS_LINUX_KERNEL
    sisfb_write_nbridge_pci_dword(SiS_Pr, 0xA0, templ);
-#else
-   pciWriteLong(0x00000000, 0xA0, templ);
-#endif
 }
-#endif /* SIS300 */
+#endif /* CONFIG_FB_SIS_300 */
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void
 SiS_SetCRT1FIFO_310(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short ModeIdIndex)
 {
@@ -2612,7 +2585,7 @@ SiS_SetVCLKState(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    }
 
    if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
       if(VCLK > 150) data |= 0x80;
       SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x07,0x7B,data);
 
@@ -2621,7 +2594,7 @@ SiS_SetVCLKState(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x32,0xF7,data);
 #endif
    } else if(SiS_Pr->ChipType < XGI_20) {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       if(VCLK >= 166) data |= 0x0c;
       SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x32,0xf3,data);
 
@@ -2630,7 +2603,7 @@ SiS_SetVCLKState(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       }
 #endif
    } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       if(VCLK >= 200) data |= 0x0c;
       if(SiS_Pr->ChipType == XGI_20) data &= ~0x04;
       SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x32,0xf3,data);
@@ -2675,7 +2648,7 @@ SiS_SetCRT1ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
                 unsigned short ModeIdIndex, unsigned short RRTI)
 {
    unsigned short data, infoflag = 0, modeflag, resindex;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    unsigned char  *ROMAddr  = SiS_Pr->VirtualRomBase;
    unsigned short data2, data3;
 #endif
@@ -2736,7 +2709,7 @@ SiS_SetCRT1ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x0F,0xB7,data);
    }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType >= SIS_315H) {
       SiS_SetRegAND(SiS_Pr->SiS_P3c4,0x31,0xfb);
    }
@@ -2826,7 +2799,7 @@ SiS_SetCRT1ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 
    SiS_SetVCLKState(SiS_Pr, ModeNo, RRTI, ModeIdIndex);
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(((SiS_Pr->ChipType >= SIS_315H) && (SiS_Pr->ChipType < SIS_661)) ||
        (SiS_Pr->ChipType == XGI_40)) {
       if(SiS_GetReg(SiS_Pr->SiS_P3d4,0x31) & 0x40) {
@@ -2845,7 +2818,7 @@ SiS_SetCRT1ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 #endif
 }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void
 SiS_SetupDualChip(struct SiS_Private *SiS_Pr)
 {
@@ -2999,11 +2972,6 @@ SiS_SetCRT1Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sho
    SiS_Pr->SiS_SelectCRT2Rate = 0;
    SiS_Pr->SiS_SetFlag &= (~ProgrammingCRT2);
 
-#ifdef SIS_XORG_XF86
-   xf86DrvMsgVerb(0, X_PROBED, 4, "(init: VBType=0x%04x, VBInfo=0x%04x)\n",
-                    SiS_Pr->SiS_VBType, SiS_Pr->SiS_VBInfo);
-#endif
-
    if(SiS_Pr->SiS_VBInfo & SetSimuScanMode) {
       if(SiS_Pr->SiS_VBInfo & SetInSlaveMode) {
          SiS_Pr->SiS_SetFlag |= ProgrammingCRT2;
@@ -3028,7 +2996,7 @@ SiS_SetCRT1Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sho
    }
 
    switch(SiS_Pr->ChipType) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    case SIS_300:
       SiS_SetCRT1FIFO_300(SiS_Pr, ModeNo, RefreshRateTableIndex);
       break;
@@ -3039,7 +3007,7 @@ SiS_SetCRT1Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sho
       break;
 #endif
    default:
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       if(SiS_Pr->ChipType == XGI_20) {
          unsigned char sr2b = 0, sr2c = 0;
          switch(ModeNo) {
@@ -3062,7 +3030,7 @@ SiS_SetCRT1Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sho
 
    SiS_SetCRT1ModeRegs(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType == XGI_40) {
       SiS_SetupDualChip(SiS_Pr);
    }
@@ -3070,11 +3038,9 @@ SiS_SetCRT1Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sho
 
    SiS_LoadDAC(SiS_Pr, ModeNo, ModeIdIndex);
 
-#ifdef SIS_LINUX_KERNEL
    if(SiS_Pr->SiS_flag_clearbuffer) {
       SiS_ClearBuffer(SiS_Pr, ModeNo);
    }
-#endif
 
    if(!(SiS_Pr->SiS_VBInfo & (SetSimuScanMode | SwitchCRT2 | SetCRT2ToLCDA))) {
       SiS_WaitRetrace1(SiS_Pr);
@@ -3104,7 +3070,7 @@ SiS_InitVB(struct SiS_Private *SiS_Pr)
 static void
 SiS_ResetVB(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    unsigned char  *ROMAddr = SiS_Pr->VirtualRomBase;
    unsigned short temp;
 
@@ -3139,7 +3105,7 @@ SiS_StrangeStuff(struct SiS_Private *SiS_Pr)
     * which locks CRT2 in some way to CRT1 timing. Disable
     * this here.
     */
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if((IS_SIS651) || (IS_SISM650) ||
       SiS_Pr->ChipType == SIS_340 ||
       SiS_Pr->ChipType == XGI_40) {
@@ -3160,7 +3126,7 @@ SiS_StrangeStuff(struct SiS_Private *SiS_Pr)
 static void
 SiS_Handle760(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    unsigned int somebase;
    unsigned char temp1, temp2, temp3;
 
@@ -3170,11 +3136,7 @@ SiS_Handle760(struct SiS_Private *SiS_Pr)
        (!(SiS_Pr->SiS_SysFlags & SF_760UMA)) )
       return;
 
-#ifdef SIS_LINUX_KERNEL
    somebase = sisfb_read_mio_pci_word(SiS_Pr, 0x74);
-#else
-   somebase = pciReadWord(0x00001000, 0x74);
-#endif
    somebase &= 0xffff;
 
    if(somebase == 0) return;
@@ -3190,105 +3152,34 @@ SiS_Handle760(struct SiS_Private *SiS_Pr)
       temp2 = 0x0b;
    }
 
-#ifdef SIS_LINUX_KERNEL
    sisfb_write_nbridge_pci_byte(SiS_Pr, 0x7e, temp1);
    sisfb_write_nbridge_pci_byte(SiS_Pr, 0x8d, temp2);
-#else
-   pciWriteByte(0x00000000, 0x7e, temp1);
-   pciWriteByte(0x00000000, 0x8d, temp2);
-#endif
 
    SiS_SetRegByte((somebase + 0x85), temp3);
 #endif
 }
 
 /*********************************************/
-/*      X.org/XFree86: SET SCREEN PITCH      */
-/*********************************************/
-
-#ifdef SIS_XORG_XF86
-static void
-SiS_SetPitchCRT1(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn)
-{
-   SISPtr pSiS = SISPTR(pScrn);
-   unsigned short HDisplay = pSiS->scrnPitch >> 3;
-
-   SiS_SetReg(SiS_Pr->SiS_P3d4,0x13,(HDisplay & 0xFF));
-   SiS_SetRegANDOR(SiS_Pr->SiS_P3c4,0x0E,0xF0,(HDisplay >> 8));
-}
-
-static void
-SiS_SetPitchCRT2(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn)
-{
-   SISPtr pSiS = SISPTR(pScrn);
-   unsigned short HDisplay = pSiS->scrnPitch2 >> 3;
-
-    /* Unlock CRT2 */
-   if(pSiS->VGAEngine == SIS_315_VGA)
-      SiS_SetRegOR(SiS_Pr->SiS_Part1Port,0x2F, 0x01);
-   else
-      SiS_SetRegOR(SiS_Pr->SiS_Part1Port,0x24, 0x01);
-
-   SiS_SetReg(SiS_Pr->SiS_Part1Port,0x07,(HDisplay & 0xFF));
-   SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port,0x09,0xF0,(HDisplay >> 8));
-}
-
-static void
-SiS_SetPitch(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn)
-{
-   SISPtr pSiS = SISPTR(pScrn);
-   bool isslavemode = false;
-
-   if( (pSiS->VBFlags2 & VB2_VIDEOBRIDGE) &&
-       ( ((pSiS->VGAEngine == SIS_300_VGA) &&
-          (SiS_GetReg(SiS_Pr->SiS_Part1Port,0x00) & 0xa0) == 0x20) ||
-         ((pSiS->VGAEngine == SIS_315_VGA) &&
-          (SiS_GetReg(SiS_Pr->SiS_Part1Port,0x00) & 0x50) == 0x10) ) ) {
-      isslavemode = true;
-   }
-
-   /* We need to set pitch for CRT1 if bridge is in slave mode, too */
-   if((pSiS->VBFlags & DISPTYPE_DISP1) || (isslavemode)) {
-      SiS_SetPitchCRT1(SiS_Pr, pScrn);
-   }
-   /* We must not set the pitch for CRT2 if bridge is in slave mode */
-   if((pSiS->VBFlags & DISPTYPE_DISP2) && (!isslavemode)) {
-      SiS_SetPitchCRT2(SiS_Pr, pScrn);
-   }
-}
-#endif
-
-/*********************************************/
 /*                 SiSSetMode()              */
 /*********************************************/
 
-#ifdef SIS_XORG_XF86
-/* We need pScrn for setting the pitch correctly */
-bool
-SiSSetMode(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn, unsigned short ModeNo, bool dosetpitch)
-#else
 bool
 SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
-#endif
 {
    SISIOADDRESS BaseAddr = SiS_Pr->IOAddress;
    unsigned short RealModeNo, ModeIdIndex;
    unsigned char  backupreg = 0;
-#ifdef SIS_LINUX_KERNEL
    unsigned short KeepLockReg;
 
    SiS_Pr->UseCustomMode = false;
    SiS_Pr->CRT1UsesCustomMode = false;
-#endif
 
    SiS_Pr->SiS_flag_clearbuffer = 0;
 
    if(SiS_Pr->UseCustomMode) {
       ModeNo = 0xfe;
    } else {
-#ifdef SIS_LINUX_KERNEL
       if(!(ModeNo & 0x80)) SiS_Pr->SiS_flag_clearbuffer = 1;
-#endif
       ModeNo &= 0x7f;
    }
 
@@ -3301,13 +3192,8 @@ SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
    SiS_GetSysFlags(SiS_Pr);
 
    SiS_Pr->SiS_VGAINFO = 0x11;
-#if defined(SIS_XORG_XF86) && (defined(i386) || defined(__i386) || defined(__i386__) || defined(__AMD64__) || defined(__amd64__) || defined(__x86_64__))
-   if(pScrn) SiS_Pr->SiS_VGAINFO = SiS_GetSetBIOSScratch(pScrn, 0x489, 0xff);
-#endif
 
-#ifdef SIS_LINUX_KERNEL
    KeepLockReg = SiS_GetReg(SiS_Pr->SiS_P3c4,0x05);
-#endif
    SiS_SetReg(SiS_Pr->SiS_P3c4,0x05,0x86);
 
    SiSInitPCIetc(SiS_Pr);
@@ -3344,12 +3230,10 @@ SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
    SiS_GetLCDResInfo(SiS_Pr, ModeNo, ModeIdIndex);
    SiS_SetLowModeTest(SiS_Pr, ModeNo);
 
-#ifdef SIS_LINUX_KERNEL
    /* Check memory size (kernel framebuffer driver only) */
    if(!SiS_CheckMemorySize(SiS_Pr, ModeNo, ModeIdIndex)) {
       return false;
    }
-#endif
 
    SiS_OpenCRTC(SiS_Pr);
 
@@ -3384,7 +3268,7 @@ SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
    SiS_DisplayOn(SiS_Pr);
    SiS_SetRegByte(SiS_Pr->SiS_P3c6,0xFF);
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType >= SIS_315H) {
       if(SiS_Pr->SiS_IF_DEF_LVDS == 1) {
          if(!(SiS_IsDualEdge(SiS_Pr))) {
@@ -3396,7 +3280,7 @@ SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 
    if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
       if(SiS_Pr->ChipType >= SIS_315H) {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
          if(!SiS_Pr->SiS_ROMNew) {
             if(SiS_IsVAMode(SiS_Pr)) {
                SiS_SetRegOR(SiS_Pr->SiS_P3d4,0x35,0x01);
@@ -3424,424 +3308,16 @@ SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
       }
    }
 
-#ifdef SIS_XORG_XF86
-   if(pScrn) {
-      /* SetPitch: Adapt to virtual size & position */
-      if((ModeNo > 0x13) && (dosetpitch)) {
-         SiS_SetPitch(SiS_Pr, pScrn);
-      }
-
-      /* Backup/Set ModeNo in BIOS scratch area */
-      SiS_GetSetModeID(pScrn, ModeNo);
-   }
-#endif
-
    SiS_CloseCRTC(SiS_Pr);
 
    SiS_Handle760(SiS_Pr);
 
-#ifdef SIS_LINUX_KERNEL
    /* We never lock registers in XF86 */
    if(KeepLockReg != 0xA1) SiS_SetReg(SiS_Pr->SiS_P3c4,0x05,0x00);
-#endif
 
    return true;
 }
 
-/*********************************************/
-/*       X.org/XFree86: SiSBIOSSetMode()     */
-/*           for non-Dual-Head mode          */
-/*********************************************/
-
-#ifdef SIS_XORG_XF86
-bool
-SiSBIOSSetMode(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn,
-               DisplayModePtr mode, bool IsCustom)
-{
-   SISPtr pSiS = SISPTR(pScrn);
-   unsigned short ModeNo = 0;
-
-   SiS_Pr->UseCustomMode = false;
-
-   if((IsCustom) && (SiS_CheckBuildCustomMode(pScrn, mode, pSiS->VBFlags))) {
-
-      xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3, "Setting custom mode %dx%d\n",
-                SiS_Pr->CHDisplay,
-                (mode->Flags & V_INTERLACE ? SiS_Pr->CVDisplay * 2 :
-                   (mode->Flags & V_DBLSCAN ? SiS_Pr->CVDisplay / 2 :
-                      SiS_Pr->CVDisplay)));
-
-   } else {
-
-      /* Don't need vbflags here; checks done earlier */
-      ModeNo = SiS_GetModeNumber(pScrn, mode, pSiS->VBFlags);
-      if(!ModeNo) return false;
-
-      xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3, "Setting standard mode 0x%x\n", ModeNo);
-
-   }
-
-   return(SiSSetMode(SiS_Pr, pScrn, ModeNo, true));
-}
-
-/*********************************************/
-/*    X.org/XFree86: SiSBIOSSetModeCRT2()    */
-/*           for Dual-Head modes             */
-/*********************************************/
-
-bool
-SiSBIOSSetModeCRT2(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn,
-               DisplayModePtr mode, bool IsCustom)
-{
-   SISIOADDRESS BaseAddr = SiS_Pr->IOAddress;
-   SISPtr  pSiS = SISPTR(pScrn);
-#ifdef SISDUALHEAD
-   SISEntPtr pSiSEnt = pSiS->entityPrivate;
-#endif
-   unsigned short ModeIdIndex;
-   unsigned short ModeNo = 0;
-   unsigned char  backupreg = 0;
-
-   SiS_Pr->UseCustomMode = false;
-
-   /* Remember: Custom modes for CRT2 are ONLY supported
-    *     -) on the 30x/B/C, and
-    *     -) if CRT2 is LCD or VGA, or CRT1 is LCDA
-    */
-
-   if((IsCustom) && (SiS_CheckBuildCustomMode(pScrn, mode, pSiS->VBFlags))) {
-
-         ModeNo = 0xfe;
-
-   } else {
-
-         ModeNo = SiS_GetModeNumber(pScrn, mode, pSiS->VBFlags);
-         if(!ModeNo) return false;
-
-   }
-
-   SiSRegInit(SiS_Pr, BaseAddr);
-   SiSInitPtr(SiS_Pr);
-   SiS_GetSysFlags(SiS_Pr);
-#if defined(i386) || defined(__i386) || defined(__i386__) || defined(__AMD64__) || defined(__amd64__) || defined(__x86_64__)
-   SiS_Pr->SiS_VGAINFO = SiS_GetSetBIOSScratch(pScrn, 0x489, 0xff);
-#else
-   SiS_Pr->SiS_VGAINFO = 0x11;
-#endif
-
-   SiS_SetReg(SiS_Pr->SiS_P3c4,0x05,0x86);
-
-   SiSInitPCIetc(SiS_Pr);
-   SiSSetLVDSetc(SiS_Pr);
-   SiSDetermineROMUsage(SiS_Pr);
-
-   /* Save mode info so we can set it from within SetMode for CRT1 */
-#ifdef SISDUALHEAD
-   if(pSiS->DualHeadMode) {
-      pSiSEnt->CRT2ModeNo = ModeNo;
-      pSiSEnt->CRT2DMode = mode;
-      pSiSEnt->CRT2IsCustom = IsCustom;
-      pSiSEnt->CRT2CR30 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x30);
-      pSiSEnt->CRT2CR31 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x31);
-      pSiSEnt->CRT2CR35 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x35);
-      pSiSEnt->CRT2CR38 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x38);
-#if 0
-      /* We can't set CRT2 mode before CRT1 mode is set - says who...? */
-      if(pSiSEnt->CRT1ModeNo == -1) {
-         xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3,
-                "Setting CRT2 mode delayed until after setting CRT1 mode\n");
-         return true;
-      }
-#endif
-      pSiSEnt->CRT2ModeSet = true;
-   }
-#endif
-
-   if(SiS_Pr->UseCustomMode) {
-
-      unsigned short temptemp = SiS_Pr->CVDisplay;
-
-      if(SiS_Pr->CModeFlag & DoubleScanMode)     temptemp >>= 1;
-      else if(SiS_Pr->CInfoFlag & InterlaceMode) temptemp <<= 1;
-
-      xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3,
-          "Setting custom mode %dx%d on CRT2\n",
-          SiS_Pr->CHDisplay, temptemp);
-
-   } else {
-
-      xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3,
-          "Setting standard mode 0x%x on CRT2\n", ModeNo);
-
-   }
-
-   SiS_UnLockCRT2(SiS_Pr);
-
-   if(!SiS_Pr->UseCustomMode) {
-      if(!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex))) return false;
-   } else {
-      ModeIdIndex = 0;
-   }
-
-   SiS_GetVBType(SiS_Pr);
-
-   SiS_InitVB(SiS_Pr);
-   if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
-      if(SiS_Pr->ChipType >= SIS_315H) {
-         SiS_ResetVB(SiS_Pr);
-         SiS_SetRegOR(SiS_Pr->SiS_P3c4,0x32,0x10);
-         SiS_SetRegOR(SiS_Pr->SiS_Part2Port,0x00,0x0c);
-         backupreg = SiS_GetReg(SiS_Pr->SiS_P3d4,0x38);
-      } else {
-         backupreg = SiS_GetReg(SiS_Pr->SiS_P3d4,0x35);
-      }
-   }
-
-   /* Get VB information (connectors, connected devices) */
-   if(!SiS_Pr->UseCustomMode) {
-      SiS_GetVBInfo(SiS_Pr, ModeNo, ModeIdIndex, 1);
-   } else {
-      /* If this is a custom mode, we don't check the modeflag for CRT2Mode */
-      SiS_GetVBInfo(SiS_Pr, ModeNo, ModeIdIndex, 0);
-   }
-   SiS_SetYPbPr(SiS_Pr);
-   SiS_SetTVMode(SiS_Pr, ModeNo, ModeIdIndex);
-   SiS_GetLCDResInfo(SiS_Pr, ModeNo, ModeIdIndex);
-   SiS_SetLowModeTest(SiS_Pr, ModeNo);
-
-   SiS_ResetSegmentRegisters(SiS_Pr);
-
-   /* Set mode on CRT2 */
-   if( (SiS_Pr->SiS_VBType & VB_SISVB)    ||
-       (SiS_Pr->SiS_IF_DEF_LVDS     == 1) ||
-       (SiS_Pr->SiS_IF_DEF_CH70xx   != 0) ||
-       (SiS_Pr->SiS_IF_DEF_TRUMPION != 0) ) {
-      SiS_SetCRT2Group(SiS_Pr, ModeNo);
-   }
-
-   SiS_StrangeStuff(SiS_Pr);
-
-   SiS_DisplayOn(SiS_Pr);
-   SiS_SetRegByte(SiS_Pr->SiS_P3c6,0xFF);
-
-   if(SiS_Pr->ChipType >= SIS_315H) {
-      if(SiS_Pr->SiS_IF_DEF_LVDS == 1) {
-         if(!(SiS_IsDualEdge(SiS_Pr))) {
-            SiS_SetRegAND(SiS_Pr->SiS_Part1Port,0x13,0xfb);
-         }
-      }
-   }
-
-   if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
-      if(SiS_Pr->ChipType >= SIS_315H) {
-         if(!SiS_Pr->SiS_ROMNew) {
-            if(SiS_IsVAMode(SiS_Pr)) {
-               SiS_SetRegOR(SiS_Pr->SiS_P3d4,0x35,0x01);
-            } else {
-               SiS_SetRegAND(SiS_Pr->SiS_P3d4,0x35,0xFE);
-            }
-         }
-
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x38,backupreg);
-
-         if(SiS_GetReg(SiS_Pr->SiS_P3d4,0x30) & SetCRT2ToLCD) {
-            SiS_SetRegAND(SiS_Pr->SiS_P3d4,0x38,0xfc);
-         }
-      } else if((SiS_Pr->ChipType == SIS_630) ||
-                (SiS_Pr->ChipType == SIS_730)) {
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x35,backupreg);
-      }
-   }
-
-   /* SetPitch: Adapt to virtual size & position */
-   SiS_SetPitchCRT2(SiS_Pr, pScrn);
-
-   SiS_Handle760(SiS_Pr);
-
-   return true;
-}
-
-/*********************************************/
-/*    X.org/XFree86: SiSBIOSSetModeCRT1()    */
-/*           for Dual-Head modes             */
-/*********************************************/
-
-bool
-SiSBIOSSetModeCRT1(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn,
-                   DisplayModePtr mode, bool IsCustom)
-{
-   SISIOADDRESS BaseAddr = SiS_Pr->IOAddress;
-   SISPtr  pSiS = SISPTR(pScrn);
-   unsigned short ModeIdIndex, ModeNo = 0;
-   unsigned char  backupreg = 0;
-#ifdef SISDUALHEAD
-   SISEntPtr pSiSEnt = pSiS->entityPrivate;
-   unsigned char  backupcr30, backupcr31, backupcr38, backupcr35, backupp40d=0;
-   bool backupcustom;
-#endif
-
-   SiS_Pr->UseCustomMode = false;
-
-   if((IsCustom) && (SiS_CheckBuildCustomMode(pScrn, mode, pSiS->VBFlags))) {
-
-         unsigned short temptemp = SiS_Pr->CVDisplay;
-
-         if(SiS_Pr->CModeFlag & DoubleScanMode)     temptemp >>= 1;
-         else if(SiS_Pr->CInfoFlag & InterlaceMode) temptemp <<= 1;
-
-         xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3,
-                "Setting custom mode %dx%d on CRT1\n",
-                SiS_Pr->CHDisplay, temptemp);
-         ModeNo = 0xfe;
-
-   } else {
-
-         ModeNo = SiS_GetModeNumber(pScrn, mode, 0); /* don't give VBFlags */
-         if(!ModeNo) return false;
-
-         xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3,
-                "Setting standard mode 0x%x on CRT1\n", ModeNo);
-   }
-
-   SiSInitPtr(SiS_Pr);
-   SiSRegInit(SiS_Pr, BaseAddr);
-   SiS_GetSysFlags(SiS_Pr);
-#if defined(i386) || defined(__i386) || defined(__i386__) || defined(__AMD64__) || defined(__amd64__) || defined(__x86_64__)
-   SiS_Pr->SiS_VGAINFO = SiS_GetSetBIOSScratch(pScrn, 0x489, 0xff);
-#else
-   SiS_Pr->SiS_VGAINFO = 0x11;
-#endif
-
-   SiS_SetReg(SiS_Pr->SiS_P3c4,0x05,0x86);
-
-   SiSInitPCIetc(SiS_Pr);
-   SiSSetLVDSetc(SiS_Pr);
-   SiSDetermineROMUsage(SiS_Pr);
-
-   SiS_UnLockCRT2(SiS_Pr);
-
-   if(!SiS_Pr->UseCustomMode) {
-      if(!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex))) return false;
-   } else {
-      ModeIdIndex = 0;
-   }
-
-   /* Determine VBType */
-   SiS_GetVBType(SiS_Pr);
-
-   SiS_InitVB(SiS_Pr);
-   if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
-      if(SiS_Pr->ChipType >= SIS_315H) {
-         backupreg = SiS_GetReg(SiS_Pr->SiS_P3d4,0x38);
-      } else {
-         backupreg = SiS_GetReg(SiS_Pr->SiS_P3d4,0x35);
-      }
-   }
-
-   /* Get VB information (connectors, connected devices) */
-   /* (We don't care if the current mode is a CRT2 mode) */
-   SiS_GetVBInfo(SiS_Pr, ModeNo, ModeIdIndex, 0);
-   SiS_SetYPbPr(SiS_Pr);
-   SiS_SetTVMode(SiS_Pr, ModeNo, ModeIdIndex);
-   SiS_GetLCDResInfo(SiS_Pr, ModeNo, ModeIdIndex);
-   SiS_SetLowModeTest(SiS_Pr, ModeNo);
-
-   SiS_OpenCRTC(SiS_Pr);
-
-   /* Set mode on CRT1 */
-   SiS_SetCRT1Group(SiS_Pr, ModeNo, ModeIdIndex);
-   if(SiS_Pr->SiS_VBInfo & SetCRT2ToLCDA) {
-      SiS_SetCRT2Group(SiS_Pr, ModeNo);
-   }
-
-   /* SetPitch: Adapt to virtual size & position */
-   SiS_SetPitchCRT1(SiS_Pr, pScrn);
-
-   SiS_HandleCRT1(SiS_Pr);
-
-   SiS_StrangeStuff(SiS_Pr);
-
-   SiS_CloseCRTC(SiS_Pr);
-
-#ifdef SISDUALHEAD
-   if(pSiS->DualHeadMode) {
-      pSiSEnt->CRT1ModeNo = ModeNo;
-      pSiSEnt->CRT1DMode = mode;
-   }
-#endif
-
-   if(SiS_Pr->UseCustomMode) {
-      SiS_Pr->CRT1UsesCustomMode = true;
-      SiS_Pr->CSRClock_CRT1 = SiS_Pr->CSRClock;
-      SiS_Pr->CModeFlag_CRT1 = SiS_Pr->CModeFlag;
-   } else {
-      SiS_Pr->CRT1UsesCustomMode = false;
-   }
-
-   /* Reset CRT2 if changing mode on CRT1 */
-#ifdef SISDUALHEAD
-   if(pSiS->DualHeadMode) {
-      if(pSiSEnt->CRT2ModeNo != -1) {
-         xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, 3,
-                                "(Re-)Setting mode for CRT2\n");
-         backupcustom = SiS_Pr->UseCustomMode;
-         backupcr30 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x30);
-         backupcr31 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x31);
-         backupcr35 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x35);
-         backupcr38 = SiS_GetReg(SiS_Pr->SiS_P3d4,0x38);
-         if(SiS_Pr->SiS_VBType & VB_SISVB) {
-            /* Backup LUT-enable */
-            if(pSiSEnt->CRT2ModeSet) {
-               backupp40d = SiS_GetReg(SiS_Pr->SiS_Part4Port,0x0d) & 0x08;
-            }
-         }
-         if(SiS_Pr->SiS_VBInfo & SetCRT2ToLCDA) {
-            SiS_SetReg(SiS_Pr->SiS_P3d4,0x30,pSiSEnt->CRT2CR30);
-            SiS_SetReg(SiS_Pr->SiS_P3d4,0x31,pSiSEnt->CRT2CR31);
-            SiS_SetReg(SiS_Pr->SiS_P3d4,0x35,pSiSEnt->CRT2CR35);
-            SiS_SetReg(SiS_Pr->SiS_P3d4,0x38,pSiSEnt->CRT2CR38);
-         }
-
-         SiSBIOSSetModeCRT2(SiS_Pr, pSiSEnt->pScrn_1,
-                            pSiSEnt->CRT2DMode, pSiSEnt->CRT2IsCustom);
-
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x30,backupcr30);
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x31,backupcr31);
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x35,backupcr35);
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x38,backupcr38);
-         if(SiS_Pr->SiS_VBType & VB_SISVB) {
-            SiS_SetRegANDOR(SiS_Pr->SiS_Part4Port,0x0d, ~0x08, backupp40d);
-         }
-         SiS_Pr->UseCustomMode = backupcustom;
-      }
-   }
-#endif
-
-   /* Warning: From here, the custom mode entries in SiS_Pr are
-    * possibly overwritten
-    */
-
-   SiS_DisplayOn(SiS_Pr);
-   SiS_SetRegByte(SiS_Pr->SiS_P3c6,0xFF);
-
-   if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
-      if(SiS_Pr->ChipType >= SIS_315H) {
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x38,backupreg);
-      } else if((SiS_Pr->ChipType == SIS_630) ||
-                (SiS_Pr->ChipType == SIS_730)) {
-         SiS_SetReg(SiS_Pr->SiS_P3d4,0x35,backupreg);
-      }
-   }
-
-   SiS_Handle760(SiS_Pr);
-
-   /* Backup/Set ModeNo in BIOS scratch area */
-   SiS_GetSetModeID(pScrn,ModeNo);
-
-   return true;
-}
-#endif /* Linux_XF86 */
-
 #ifndef GETBITSTR
 #define BITMASK(h,l)            (((unsigned)(1U << ((h)-(l)+1))-1)<<(l))
 #define GENMASK(mask)           BITMASK(1?mask,0?mask)
@@ -3927,7 +3403,7 @@ SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    SiS_Pr->CVBlankStart = SiS_Pr->SiS_VGAVDE;
 
    if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
       tempbx = SiS_Pr->SiS_VGAHT;
       if(SiS_Pr->SiS_LCDInfo & DontExpandLCD) {
          tempbx = SiS_Pr->PanelHT;
@@ -3936,7 +3412,7 @@ SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       remaining = tempbx % 8;
 #endif
    } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       /* OK for LCDA, LVDS */
       tempbx = SiS_Pr->PanelHT - SiS_Pr->PanelXRes;
       tempax = SiS_Pr->SiS_VGAHDE;  /* not /2 ! */
@@ -3950,7 +3426,7 @@ SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    SiS_Pr->CHTotal = SiS_Pr->CHBlankEnd = tempbx;
 
    if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
       if(SiS_Pr->SiS_VGAHDE == SiS_Pr->PanelXRes) {
          SiS_Pr->CHSyncStart = SiS_Pr->SiS_VGAHDE + ((SiS_Pr->PanelHRS + 1) & ~1);
          SiS_Pr->CHSyncEnd = SiS_Pr->CHSyncStart + SiS_Pr->PanelHRE;
@@ -3982,7 +3458,7 @@ SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       }
 #endif
    } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       tempax = VGAHDE;
       if(SiS_Pr->SiS_LCDInfo & DontExpandLCD) {
          tempbx = SiS_Pr->PanelXRes;
@@ -4001,7 +3477,7 @@ SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    if(SiS_Pr->SiS_LCDInfo & DontExpandLCD) {
       tempax = SiS_Pr->PanelYRes;
    } else if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
       /* Stupid hack for 640x400/320x200 */
       if(SiS_Pr->SiS_LCDResInfo == Panel_1024x768) {
          if((tempax + tempbx) == 438) tempbx += 16;
@@ -4054,36 +3530,12 @@ SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
    if(modeflag & DoubleScanMode) tempax |= 0x80;
    SiS_SetRegANDOR(SiS_Pr->SiS_P3d4,0x09,0x5F,tempax);
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_INFO, "%d %d %d %d  %d %d %d %d  (%d %d %d %d)\n",
-        SiS_Pr->CHDisplay, SiS_Pr->CHSyncStart, SiS_Pr->CHSyncEnd, SiS_Pr->CHTotal,
-        SiS_Pr->CVDisplay, SiS_Pr->CVSyncStart, SiS_Pr->CVSyncEnd, SiS_Pr->CVTotal,
-        SiS_Pr->CHBlankStart, SiS_Pr->CHBlankEnd, SiS_Pr->CVBlankStart, SiS_Pr->CVBlankEnd);
-   xf86DrvMsg(0, X_INFO, " {{0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,\n",
-        SiS_Pr->CCRT1CRTC[0], SiS_Pr->CCRT1CRTC[1],
-        SiS_Pr->CCRT1CRTC[2], SiS_Pr->CCRT1CRTC[3],
-        SiS_Pr->CCRT1CRTC[4], SiS_Pr->CCRT1CRTC[5],
-        SiS_Pr->CCRT1CRTC[6], SiS_Pr->CCRT1CRTC[7]);
-   xf86DrvMsg(0, X_INFO, "   0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,\n",
-        SiS_Pr->CCRT1CRTC[8], SiS_Pr->CCRT1CRTC[9],
-        SiS_Pr->CCRT1CRTC[10], SiS_Pr->CCRT1CRTC[11],
-        SiS_Pr->CCRT1CRTC[12], SiS_Pr->CCRT1CRTC[13],
-        SiS_Pr->CCRT1CRTC[14], SiS_Pr->CCRT1CRTC[15]);
-   xf86DrvMsg(0, X_INFO, "   0x%02x}},\n", SiS_Pr->CCRT1CRTC[16]);
-#endif
-#endif
 }
 
 void
 SiS_Generic_ConvertCRData(struct SiS_Private *SiS_Pr, unsigned char *crdata,
                         int xres, int yres,
-#ifdef SIS_XORG_XF86
-                        DisplayModePtr current
-#endif
-#ifdef SIS_LINUX_KERNEL
                         struct fb_var_screeninfo *var, bool writeres
-#endif
 )
 {
    unsigned short HRE, HBE, HRS, HBS, HDE, HT;
@@ -4127,25 +3579,10 @@ SiS_Generic_ConvertCRData(struct SiS_Private *SiS_Pr, unsigned char *crdata,
 
    D = B - F - C;
 
-#ifdef SIS_XORG_XF86
-   current->HDisplay   = (E * 8);
-   current->HSyncStart = (E * 8) + (F * 8);
-   current->HSyncEnd   = (E * 8) + (F * 8) + (C * 8);
-   current->HTotal     = (E * 8) + (F * 8) + (C * 8) + (D * 8);
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_INFO,
-                "H: A %d B %d C %d D %d E %d F %d  HT %d HDE %d HRS %d HBS %d HBE %d HRE %d\n",
-                A, B, C, D, E, F, HT, HDE, HRS, HBS, HBE, HRE);
-#else
-   (void)VBS;  (void)HBS;  (void)A;
-#endif
-#endif
-#ifdef SIS_LINUX_KERNEL
    if(writeres) var->xres = xres = E * 8;
    var->left_margin = D * 8;
    var->right_margin = F * 8;
    var->hsync_len = C * 8;
-#endif
 
    /* Vertical */
    sr_data = crdata[13];
@@ -4192,30 +3629,10 @@ SiS_Generic_ConvertCRData(struct SiS_Private *SiS_Pr, unsigned char *crdata,
 
    D = B - F - C;
 
-#ifdef SIS_XORG_XF86
-   current->VDisplay   = VDE + 1;
-   current->VSyncStart = VRS + 1;
-   current->VSyncEnd   = ((VRS & ~0x1f) | VRE) + 1;
-   if(VRE <= (VRS & 0x1f)) current->VSyncEnd += 32;
-   current->VTotal     = E + D + C + F;
-#if 0
-   current->VDisplay   = E;
-   current->VSyncStart = E + D;
-   current->VSyncEnd   = E + D + C;
-   current->VTotal     = E + D + C + F;
-#endif
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_INFO,
-        "V: A %d B %d C %d D %d E %d F %d  VT %d VDE %d VRS %d VBS %d VBE %d VRE %d\n",
-        A, B, C, D, E, F, VT, VDE, VRS, VBS, VBE, VRE);
-#endif
-#endif
-#ifdef SIS_LINUX_KERNEL
    if(writeres) var->yres = yres = E;
    var->upper_margin = D;
    var->lower_margin = F;
    var->vsync_len = C;
-#endif
 
    if((xres == 320) && ((yres == 200) || (yres == 240))) {
         /* Terrible hack, but correct CRTC data for
@@ -4224,17 +3641,9 @@ SiS_Generic_ConvertCRData(struct SiS_Private *SiS_Pr, unsigned char *crdata,
          * a negative D. The CRT controller does not
          * seem to like correcting HRE to 50)
          */
-#ifdef SIS_XORG_XF86
-      current->HDisplay   = 320;
-      current->HSyncStart = 328;
-      current->HSyncEnd   = 376;
-      current->HTotal     = 400;
-#endif
-#ifdef SIS_LINUX_KERNEL
       var->left_margin = (400 - 376);
       var->right_margin = (328 - 320);
       var->hsync_len = (376 - 328);
-#endif
 
    }
 
diff --git a/drivers/video/sis/init.h b/drivers/video/sis/init.h
index b96005c39c67..aff73842d877 100644
--- a/drivers/video/sis/init.h
+++ b/drivers/video/sis/init.h
@@ -53,21 +53,8 @@
 #ifndef _INIT_H_
 #define _INIT_H_
 
-#include "osdef.h"
 #include "initdef.h"
 
-#ifdef SIS_XORG_XF86
-#include "sis.h"
-#define SIS_NEED_inSISREG
-#define SIS_NEED_inSISREGW
-#define SIS_NEED_inSISREGL
-#define SIS_NEED_outSISREG
-#define SIS_NEED_outSISREGW
-#define SIS_NEED_outSISREGL
-#include "sis_regs.h"
-#endif
-
-#ifdef SIS_LINUX_KERNEL
 #include "vgatypes.h"
 #include "vstruct.h"
 #ifdef SIS_CP
@@ -78,7 +65,6 @@
 #include <linux/fb.h>
 #include "sis.h"
 #include <video/sisfb.h>
-#endif
 
 /* Mode numbers */
 static const unsigned short ModeIndex_320x200[]      = {0x59, 0x41, 0x00, 0x4f};
@@ -286,7 +272,7 @@ static const struct SiS_ModeResInfo_S SiS_ModeResInfo[] =
         { 1280, 854, 8,16}    /* 0x22 */
 };
 
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
 static const struct SiS_StandTable_S SiS_StandTable[]=
 {
 /* 0x00: MD_0_200 */
@@ -1521,10 +1507,6 @@ static const struct SiS_LVDSCRT1Data SiS_LVDSCRT1640x480_1_H[] =
 };
 
 bool            SiSInitPtr(struct SiS_Private *SiS_Pr);
-#ifdef SIS_XORG_XF86
-unsigned short  SiS_GetModeID(int VGAEngine, unsigned int VBFlags, int HDisplay, int VDisplay,
-                                int Depth, bool FSTN, int LCDwith, int LCDheight);
-#endif
 unsigned short  SiS_GetModeID_LCD(int VGAEngine, unsigned int VBFlags, int HDisplay,
                                 int VDisplay, int Depth, bool FSTN,
                                 unsigned short CustomT, int LCDwith, int LCDheight,
@@ -1534,33 +1516,14 @@ unsigned short	SiS_GetModeID_TV(int VGAEngine, unsigned int VBFlags, int HDispla
 unsigned short  SiS_GetModeID_VGA2(int VGAEngine, unsigned int VBFlags, int HDisplay,
                                 int VDisplay, int Depth, unsigned int VBFlags2);
 
-void            SiS_SetReg(SISIOADDRESS port, unsigned short index, unsigned short data);
-void            SiS_SetRegByte(SISIOADDRESS port, unsigned short data);
-void            SiS_SetRegShort(SISIOADDRESS port, unsigned short data);
-void            SiS_SetRegLong(SISIOADDRESS port, unsigned int data);
-unsigned char   SiS_GetReg(SISIOADDRESS port, unsigned short index);
-unsigned char   SiS_GetRegByte(SISIOADDRESS port);
-unsigned short  SiS_GetRegShort(SISIOADDRESS port);
-unsigned int    SiS_GetRegLong(SISIOADDRESS port);
-void            SiS_SetRegANDOR(SISIOADDRESS Port, unsigned short Index, unsigned short DataAND,
-                                unsigned short DataOR);
-void            SiS_SetRegAND(SISIOADDRESS Port,unsigned short Index, unsigned short DataAND);
-void            SiS_SetRegOR(SISIOADDRESS Port,unsigned short Index, unsigned short DataOR);
-
 void            SiS_DisplayOn(struct SiS_Private *SiS_Pr);
 void            SiS_DisplayOff(struct SiS_Private *SiS_Pr);
 void            SiSRegInit(struct SiS_Private *SiS_Pr, SISIOADDRESS BaseAddr);
-#ifndef SIS_LINUX_KERNEL
-void            SiSSetLVDSetc(struct SiS_Private *SiS_Pr);
-#endif
 void            SiS_SetEnableDstn(struct SiS_Private *SiS_Pr, int enable);
 void            SiS_SetEnableFstn(struct SiS_Private *SiS_Pr, int enable);
 unsigned short  SiS_GetModeFlag(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
                                 unsigned short ModeIdIndex);
 bool            SiSDetermineROMLayout661(struct SiS_Private *SiS_Pr);
-#ifndef SIS_LINUX_KERNEL
-void            SiS_GetVBType(struct SiS_Private *SiS_Pr);
-#endif
 
 bool            SiS_SearchModeID(struct SiS_Private *SiS_Pr, unsigned short *ModeNo,
                                 unsigned short *ModeIdIndex);
@@ -1572,37 +1535,19 @@ unsigned short	SiS_GetColorDepth(struct SiS_Private *SiS_Pr, unsigned short Mode
                                 unsigned short ModeIdIndex);
 unsigned short  SiS_GetOffset(struct SiS_Private *SiS_Pr,unsigned short ModeNo,
                                 unsigned short ModeIdIndex, unsigned short RRTI);
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 void            SiS_GetFIFOThresholdIndex300(struct SiS_Private *SiS_Pr, unsigned short *idx1,
                                 unsigned short *idx2);
 unsigned short  SiS_GetFIFOThresholdB300(unsigned short idx1, unsigned short idx2);
 unsigned short  SiS_GetLatencyFactor630(struct SiS_Private *SiS_Pr, unsigned short index);
 #endif
 void            SiS_LoadDAC(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short ModeIdIndex);
-#ifdef SIS_XORG_XF86
-bool            SiSSetMode(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn, unsigned short ModeNo,
-                                bool dosetpitch);
-bool            SiSBIOSSetMode(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn,
-                                DisplayModePtr mode, bool IsCustom);
-bool            SiSBIOSSetModeCRT2(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn,
-                                DisplayModePtr mode, bool IsCustom);
-bool            SiSBIOSSetModeCRT1(struct SiS_Private *SiS_Pr, ScrnInfoPtr pScrn,
-                                DisplayModePtr mode, bool IsCustom);
-#endif
-#ifdef SIS_LINUX_KERNEL
 bool            SiSSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo);
-#endif
 void            SiS_CalcCRRegisters(struct SiS_Private *SiS_Pr, int depth);
 void            SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
                                 unsigned short ModeIdIndex);
-#ifdef SIS_XORG_XF86
-void            SiS_Generic_ConvertCRData(struct SiS_Private *SiS_Pr, unsigned char *crdata, int xres,
-                                int yres, DisplayModePtr current);
-#endif
-#ifdef SIS_LINUX_KERNEL
 void            SiS_Generic_ConvertCRData(struct SiS_Private *SiS_Pr, unsigned char *crdata, int xres,
                                 int yres, struct fb_var_screeninfo *var, bool writeres);
-#endif
 
 /* From init301.c: */
 extern void             SiS_GetVBInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
@@ -1626,29 +1571,16 @@ extern unsigned short	SiS_GetVCLK2Ptr(struct SiS_Private *SiS_Pr, unsigned short
 extern bool             SiS_IsVAMode(struct SiS_Private *);
 extern bool             SiS_IsDualEdge(struct SiS_Private *);
 
-#ifdef SIS_XORG_XF86
-/* From other modules: */
-extern unsigned short   SiS_CheckBuildCustomMode(ScrnInfoPtr pScrn, DisplayModePtr mode,
-                                unsigned int VBFlags);
-extern unsigned char    SiS_GetSetBIOSScratch(ScrnInfoPtr pScrn, unsigned short offset,
-                                unsigned char value);
-extern unsigned char    SiS_GetSetModeID(ScrnInfoPtr pScrn, unsigned char id);
-extern unsigned short   SiS_GetModeNumber(ScrnInfoPtr pScrn, DisplayModePtr mode,
-                                unsigned int VBFlags);
-#endif
-
-#ifdef SIS_LINUX_KERNEL
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 extern unsigned int     sisfb_read_nbridge_pci_dword(struct SiS_Private *SiS_Pr, int reg);
 extern void             sisfb_write_nbridge_pci_dword(struct SiS_Private *SiS_Pr, int reg,
                                 unsigned int val);
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 extern void             sisfb_write_nbridge_pci_byte(struct SiS_Private *SiS_Pr, int reg,
                                 unsigned char val);
 extern unsigned int     sisfb_read_mio_pci_word(struct SiS_Private *SiS_Pr, int reg);
 #endif
-#endif
 
 #endif
 
diff --git a/drivers/video/sis/init301.c b/drivers/video/sis/init301.c
index da33d801c22e..a89e3cafd5ad 100644
--- a/drivers/video/sis/init301.c
+++ b/drivers/video/sis/init301.c
@@ -57,10 +57,6 @@
  *
  */
 
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
 #if 1
 #define SET_EMI         /* 302LV/ELV: Set EMI values */
 #endif
@@ -75,11 +71,11 @@
 
 #include "init301.h"
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 #include "oem300.h"
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 #include "oem310.h"
 #endif
 
@@ -87,9 +83,7 @@
 #define SiS_I2CDELAYSHORT  150
 
 static unsigned short   SiS_GetBIOSLCDResInfo(struct SiS_Private *SiS_Pr);
-#ifdef SIS_LINUX_KERNEL
 static void             SiS_SetCH70xx(struct SiS_Private *SiS_Pr, unsigned short reg, unsigned char val);
-#endif
 
 /*********************************************/
 /*         HELPER: Lock/Unlock CRT2          */
@@ -106,9 +100,7 @@ SiS_UnLockCRT2(struct SiS_Private *SiS_Pr)
       SiS_SetRegOR(SiS_Pr->SiS_Part1Port,0x24,0x01);
 }
 
-#ifdef SIS_LINUX_KERNEL
 static
-#endif
 void
 SiS_LockCRT2(struct SiS_Private *SiS_Pr)
 {
@@ -138,7 +130,7 @@ SiS_SetRegSR11ANDOR(struct SiS_Private *SiS_Pr, unsigned short DataAND, unsigned
 /*    HELPER: Get Pointer to LCD structure   */
 /*********************************************/
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static unsigned char *
 GetLCDStructPtr661(struct SiS_Private *SiS_Pr)
 {
@@ -404,7 +396,7 @@ SiS_SaveCRT2Info(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 /*    HELPER: GET SOME DATA FROM BIOS ROM    */
 /*********************************************/
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static bool
 SiS_CR36BIOSWord23b(struct SiS_Private *SiS_Pr)
 {
@@ -449,7 +441,7 @@ SiS_DDC2Delay(struct SiS_Private *SiS_Pr, unsigned int delaytime)
       SiS_GetReg(SiS_Pr->SiS_P3c4, 0x05);
 }
 
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
 static void
 SiS_GenericDelay(struct SiS_Private *SiS_Pr, unsigned short delay)
 {
@@ -457,7 +449,7 @@ SiS_GenericDelay(struct SiS_Private *SiS_Pr, unsigned short delay)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void
 SiS_LongDelay(struct SiS_Private *SiS_Pr, unsigned short delay)
 {
@@ -467,7 +459,7 @@ SiS_LongDelay(struct SiS_Private *SiS_Pr, unsigned short delay)
 }
 #endif
 
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
 static void
 SiS_ShortDelay(struct SiS_Private *SiS_Pr, unsigned short delay)
 {
@@ -480,14 +472,14 @@ SiS_ShortDelay(struct SiS_Private *SiS_Pr, unsigned short delay)
 static void
 SiS_PanelDelay(struct SiS_Private *SiS_Pr, unsigned short DelayTime)
 {
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
    unsigned char  *ROMAddr = SiS_Pr->VirtualRomBase;
    unsigned short PanelID, DelayIndex, Delay=0;
 #endif
 
    if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 
       PanelID = SiS_GetReg(SiS_Pr->SiS_P3d4,0x36);
       if(SiS_Pr->SiS_VBType & VB_SISVB) {
@@ -513,11 +505,11 @@ SiS_PanelDelay(struct SiS_Private *SiS_Pr, unsigned short DelayTime)
       }
       SiS_ShortDelay(SiS_Pr, Delay);
 
-#endif  /* SIS300 */
+#endif  /* CONFIG_FB_SIS_300 */
 
    } else {
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 
       if((SiS_Pr->ChipType >= SIS_661)    ||
          (SiS_Pr->ChipType <= SIS_315PRO) ||
@@ -579,12 +571,12 @@ SiS_PanelDelay(struct SiS_Private *SiS_Pr, unsigned short DelayTime)
 
       }
 
-#endif /* SIS315H */
+#endif /* CONFIG_FB_SIS_315 */
 
    }
 }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void
 SiS_PanelDelayLoop(struct SiS_Private *SiS_Pr, unsigned short DelayTime, unsigned short DelayLoop)
 {
@@ -613,7 +605,7 @@ SiS_WaitRetrace1(struct SiS_Private *SiS_Pr)
    while((!(SiS_GetRegByte(SiS_Pr->SiS_P3da) & 0x08)) && --watchdog);
 }
 
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
 static void
 SiS_WaitRetrace2(struct SiS_Private *SiS_Pr, unsigned short reg)
 {
@@ -630,7 +622,7 @@ static void
 SiS_WaitVBRetrace(struct SiS_Private *SiS_Pr)
 {
    if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
       if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
          if(!(SiS_GetReg(SiS_Pr->SiS_Part1Port,0x00) & 0x20)) return;
       }
@@ -641,7 +633,7 @@ SiS_WaitVBRetrace(struct SiS_Private *SiS_Pr)
       }
 #endif
    } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       if(!(SiS_GetReg(SiS_Pr->SiS_Part1Port,0x00) & 0x40)) {
          SiS_WaitRetrace1(SiS_Pr);
       } else {
@@ -686,7 +678,7 @@ SiS_VBLongWait(struct SiS_Private *SiS_Pr)
 /*               HELPER: MISC                */
 /*********************************************/
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static bool
 SiS_Is301B(struct SiS_Private *SiS_Pr)
 {
@@ -708,7 +700,7 @@ SiS_CRT2IsLCD(struct SiS_Private *SiS_Pr)
 bool
 SiS_IsDualEdge(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType >= SIS_315H) {
       if((SiS_Pr->ChipType != SIS_650) || (SiS_GetReg(SiS_Pr->SiS_P3d4,0x5f) & 0xf0)) {
          if(SiS_GetReg(SiS_Pr->SiS_P3d4,0x38) & EnableDualEdge) return true;
@@ -721,7 +713,7 @@ SiS_IsDualEdge(struct SiS_Private *SiS_Pr)
 bool
 SiS_IsVAMode(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    unsigned short flag;
 
    if(SiS_Pr->ChipType >= SIS_315H) {
@@ -732,7 +724,7 @@ SiS_IsVAMode(struct SiS_Private *SiS_Pr)
    return false;
 }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_IsVAorLCD(struct SiS_Private *SiS_Pr)
 {
@@ -745,7 +737,7 @@ SiS_IsVAorLCD(struct SiS_Private *SiS_Pr)
 static bool
 SiS_IsDualLink(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType >= SIS_315H) {
       if((SiS_CRT2IsLCD(SiS_Pr)) ||
          (SiS_IsVAMode(SiS_Pr))) {
@@ -756,7 +748,7 @@ SiS_IsDualLink(struct SiS_Private *SiS_Pr)
    return false;
 }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_TVEnabled(struct SiS_Private *SiS_Pr)
 {
@@ -768,7 +760,7 @@ SiS_TVEnabled(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_LCDAEnabled(struct SiS_Private *SiS_Pr)
 {
@@ -777,7 +769,7 @@ SiS_LCDAEnabled(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_WeHaveBacklightCtrl(struct SiS_Private *SiS_Pr)
 {
@@ -788,7 +780,7 @@ SiS_WeHaveBacklightCtrl(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_IsNotM650orLater(struct SiS_Private *SiS_Pr)
 {
@@ -804,7 +796,7 @@ SiS_IsNotM650orLater(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_IsYPbPr(struct SiS_Private *SiS_Pr)
 {
@@ -816,7 +808,7 @@ SiS_IsYPbPr(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_IsChScart(struct SiS_Private *SiS_Pr)
 {
@@ -828,7 +820,7 @@ SiS_IsChScart(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_IsTVOrYPbPrOrScart(struct SiS_Private *SiS_Pr)
 {
@@ -848,7 +840,7 @@ SiS_IsTVOrYPbPrOrScart(struct SiS_Private *SiS_Pr)
 }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static bool
 SiS_IsLCDOrLCDA(struct SiS_Private *SiS_Pr)
 {
@@ -914,7 +906,7 @@ SiS_BridgeInSlavemode(struct SiS_Private *SiS_Pr)
 /*********************************************/
 
 /* Setup general purpose IO for Chrontel communication */
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 void
 SiS_SetChrontelGPIO(struct SiS_Private *SiS_Pr, unsigned short myvbinfo)
 {
@@ -923,11 +915,7 @@ SiS_SetChrontelGPIO(struct SiS_Private *SiS_Pr, unsigned short myvbinfo)
 
    if(!(SiS_Pr->SiS_ChSW)) return;
 
-#ifdef SIS_LINUX_KERNEL
    acpibase = sisfb_read_lpc_pci_dword(SiS_Pr, 0x74);
-#else
-   acpibase = pciReadLong(0x00000800, 0x74);
-#endif
    acpibase &= 0xFFFF;
    if(!acpibase) return;
    temp = SiS_GetRegShort((acpibase + 0x3c));   /* ACPI register 0x3c: GP Event 1 I/O mode select */
@@ -969,7 +957,7 @@ SiS_GetVBInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
         tempax &= (DriverMode | LoadDACFlag | SetNotSimuMode | SetPALTV);
         tempbx |= tempax;
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
         if(SiS_Pr->ChipType >= SIS_315H) {
            if(SiS_Pr->SiS_VBType & VB_SISLCDA) {
               if(ModeNo == 0x03) {
@@ -1019,7 +1007,7 @@ SiS_GetVBInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
            }
         }
 
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 
         if(!(SiS_Pr->SiS_VBType & VB_SISVGA2)) {
            tempbx &= ~(SetCRT2ToRAMDAC);
@@ -1154,24 +1142,16 @@ SiS_GetVBInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 
    SiS_Pr->SiS_VBInfo = tempbx;
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    if(SiS_Pr->ChipType == SIS_630) {
       SiS_SetChrontelGPIO(SiS_Pr, SiS_Pr->SiS_VBInfo);
    }
 #endif
 
-#ifdef SIS_LINUX_KERNEL
 #if 0
    printk(KERN_DEBUG "sisfb: (init301: VBInfo= 0x%04x, SetFlag=0x%04x)\n",
       SiS_Pr->SiS_VBInfo, SiS_Pr->SiS_SetFlag);
 #endif
-#endif
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_PROBED, "(init301: VBInfo=0x%04x, SetFlag=0x%04x)\n",
-      SiS_Pr->SiS_VBInfo, SiS_Pr->SiS_SetFlag);
-#endif
-#endif
 }
 
 /*********************************************/
@@ -1415,12 +1395,6 @@ SiS_SetTVMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
    }
 
    SiS_Pr->SiS_VBInfo &= ~SetPALTV;
-
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_INFO, "(init301: TVMode %x, VBInfo %x)\n", SiS_Pr->SiS_TVMode, SiS_Pr->SiS_VBInfo);
-#endif
-#endif
 }
 
 /*********************************************/
@@ -1443,22 +1417,10 @@ SiS_GetBIOSLCDResInfo(struct SiS_Private *SiS_Pr)
 static void
 SiS_GetLCDInfoBIOS(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    unsigned char  *ROMAddr;
    unsigned short temp;
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_INFO, "Paneldata driver: [%d %d] [H %d %d] [V %d %d] [C %d 0x%02x 0x%02x]\n",
-        SiS_Pr->PanelHT, SiS_Pr->PanelVT,
-        SiS_Pr->PanelHRS, SiS_Pr->PanelHRE,
-        SiS_Pr->PanelVRS, SiS_Pr->PanelVRE,
-        SiS_Pr->SiS_VBVCLKData[SiS_Pr->PanelVCLKIdx315].CLOCK,
-        SiS_Pr->SiS_VBVCLKData[SiS_Pr->PanelVCLKIdx315].Part4_A,
-        SiS_Pr->SiS_VBVCLKData[SiS_Pr->PanelVCLKIdx315].Part4_B);
-#endif
-#endif
-
    if((ROMAddr = GetLCDStructPtr661(SiS_Pr))) {
       if((temp = SISGETROMW(6)) != SiS_Pr->PanelHT) {
          SiS_Pr->SiS_NeedRomModeData = true;
@@ -1480,18 +1442,6 @@ SiS_GetLCDInfoBIOS(struct SiS_Private *SiS_Pr)
       SiS_Pr->SiS_VCLKData[VCLK_CUSTOM_315].SR2C =
          SiS_Pr->SiS_VBVCLKData[VCLK_CUSTOM_315].Part4_B = ROMAddr[20];
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-      xf86DrvMsg(0, X_INFO, "Paneldata BIOS:  [%d %d] [H %d %d] [V %d %d] [C %d 0x%02x 0x%02x]\n",
-        SiS_Pr->PanelHT, SiS_Pr->PanelVT,
-        SiS_Pr->PanelHRS, SiS_Pr->PanelHRE,
-        SiS_Pr->PanelVRS, SiS_Pr->PanelVRE,
-        SiS_Pr->SiS_VBVCLKData[SiS_Pr->PanelVCLKIdx315].CLOCK,
-        SiS_Pr->SiS_VBVCLKData[SiS_Pr->PanelVCLKIdx315].Part4_A,
-        SiS_Pr->SiS_VBVCLKData[SiS_Pr->PanelVCLKIdx315].Part4_B);
-#endif
-#endif
-
    }
 #endif
 }
@@ -1517,13 +1467,13 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
 {
   unsigned short temp,modeflag,resinfo=0,modexres=0,modeyres=0;
   bool panelcanscale = false;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
   unsigned char *ROMAddr = SiS_Pr->VirtualRomBase;
   static const unsigned char SiS300SeriesLCDRes[] =
           { 0,  1,  2,  3,  7,  4,  5,  8,
             0,  0, 10,  0,  0,  0,  0, 15 };
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned char   *myptr = NULL;
 #endif
 
@@ -1562,7 +1512,7 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
      SiS_Pr->SiS_LCDTypeInfo = (temp & 0x0F) - 1;
   }
   temp &= 0x0f;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
   if(SiS_Pr->ChipType < SIS_315H) {
      /* Very old BIOSes only know 7 sizes (NetVista 2179, 1.01g) */
      if(SiS_Pr->SiS_VBType & VB_SIS301) {
@@ -1574,7 +1524,7 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
 #endif
 
   /* Translate to our internal types */
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   if(SiS_Pr->ChipType == SIS_550) {
      if     (temp == Panel310_1152x768)  temp = Panel_320x240_2; /* Verified working */
      else if(temp == Panel310_320x240_2) temp = Panel_320x240_2;
@@ -1597,7 +1547,7 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
 
   SiS_Pr->SiS_LCDResInfo = temp;
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
   if(SiS_Pr->SiS_IF_DEF_LVDS == 1) {
      if(SiS_Pr->SiS_CustomT == CUT_BARCO1366) {
         SiS_Pr->SiS_LCDResInfo = Panel_Barco1366;
@@ -1639,7 +1589,7 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
   else if(SiS_Pr->UsePanelScaler == 1) SiS_Pr->SiS_LCDInfo |= DontExpandLCD;
 
   /* Dual link, Pass 1:1 BIOS default, etc. */
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   if(SiS_Pr->ChipType >= SIS_661) {
      if(SiS_Pr->SiS_LCDInfo & DontExpandLCD) {
         if(temp & 0x08) SiS_Pr->SiS_LCDInfo |= LCDPass11;
@@ -2076,7 +2026,7 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
      }
   }
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
   if(SiS_Pr->SiS_IF_DEF_LVDS == 1) {
      if(SiS_Pr->SiS_CustomT == CUT_PANEL848 || SiS_Pr->SiS_CustomT == CUT_PANEL856) {
         SiS_Pr->SiS_LCDInfo = 0x80 | 0x40 | 0x20;   /* neg h/v sync, RGB24(D0 = 0) */
@@ -2186,17 +2136,10 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
      SiS_Pr->SiS_SetFlag |= LCDVESATiming;
   }
 
-#ifdef SIS_LINUX_KERNEL
 #if 0
   printk(KERN_DEBUG "sisfb: (LCDInfo=0x%04x LCDResInfo=0x%02x LCDTypeInfo=0x%02x)\n",
         SiS_Pr->SiS_LCDInfo, SiS_Pr->SiS_LCDResInfo, SiS_Pr->SiS_LCDTypeInfo);
 #endif
-#endif
-#ifdef SIS_XORG_XF86
-  xf86DrvMsgVerb(0, X_PROBED, 4,
-        "(init301: LCDInfo=0x%04x LCDResInfo=0x%02x LCDTypeInfo=0x%02x SetFlag=0x%04x)\n",
-        SiS_Pr->SiS_LCDInfo, SiS_Pr->SiS_LCDResInfo, SiS_Pr->SiS_LCDTypeInfo, SiS_Pr->SiS_SetFlag);
-#endif
 }
 
 /*********************************************/
@@ -2359,7 +2302,7 @@ SiS_GetVCLK2Ptr(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned shor
               VCLKIndex = SiS_Pr->PanelVCLKIdx315;
            }
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
            /* Special Timing: Barco iQ Pro R series */
            if(SiS_Pr->SiS_CustomT == CUT_BARCO1366) VCLKIndex = 0x44;
 
@@ -2410,12 +2353,6 @@ SiS_GetVCLK2Ptr(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned shor
 
   }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-  xf86DrvMsg(0, X_INFO, "VCLKIndex %d (0x%x)\n", VCLKIndex, VCLKIndex);
-#endif
-#endif
-
   return VCLKIndex;
 }
 
@@ -2428,10 +2365,10 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 {
   unsigned short i, j, modeflag, tempah=0;
   short tempcl;
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
   unsigned short tempbl;
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned char  *ROMAddr = SiS_Pr->VirtualRomBase;
   unsigned short tempah2, tempbl2;
 #endif
@@ -2454,7 +2391,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
      if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300    /* ---- 300 series ---- */
+#ifdef CONFIG_FB_SIS_300    /* ---- 300 series ---- */
 
         /* For 301BDH: (with LCD via LVDS) */
         if(SiS_Pr->SiS_VBType & VB_NoLCD) {
@@ -2477,11 +2414,11 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
         if(SiS_Pr->SiS_VBInfo & SetInSlaveMode)  tempah ^= 0xA0;
 
-#endif  /* SIS300 */
+#endif  /* CONFIG_FB_SIS_300 */
 
      } else {
 
-#ifdef SIS315H    /* ------- 315/330 series ------ */
+#ifdef CONFIG_FB_SIS_315    /* ------- 315/330 series ------ */
 
         if(ModeNo > 0x13) {
            tempcl -= ModeVGA;
@@ -2494,7 +2431,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
         if(SiS_Pr->SiS_VBInfo & SetInSlaveMode) tempah ^= 0x50;
 
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 
      }
 
@@ -2503,7 +2440,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
      if(SiS_Pr->ChipType < SIS_315H) {
         SiS_SetReg(SiS_Pr->SiS_Part1Port,0x00,tempah);
      } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
         if(SiS_Pr->SiS_IF_DEF_LVDS == 1) {
            SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port,0x00,0xa0,tempah);
         } else if(SiS_Pr->SiS_VBType & VB_SISVB) {
@@ -2584,7 +2521,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
         if(SiS_Pr->ChipType >= SIS_315H) {
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
            /* LVDS can only be slave in 8bpp modes */
            tempah = 0x80;
            if((modeflag & CRT2Mode) && (SiS_Pr->SiS_ModeType > ModeVGA)) {
@@ -2604,7 +2541,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
         } else {
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
            tempah = 0;
            if( (!(SiS_Pr->SiS_VBInfo & SetInSlaveMode)) && (SiS_Pr->SiS_ModeType > ModeVGA) ) {
               tempah |= 0x02;
@@ -2626,7 +2563,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
      if(SiS_Pr->ChipType >= SIS_315H) {
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
         /* unsigned char bridgerev = SiS_GetReg(SiS_Pr->SiS_Part4Port,0x01); */
 
         /* The following is nearly unpreditable and varies from machine
@@ -2718,11 +2655,11 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
            SiS_SetRegANDOR(SiS_Pr->SiS_Part4Port,0x23,tempbl,tempah);
         }
 
-#endif /* SIS315H */
+#endif /* CONFIG_FB_SIS_315 */
 
      } else if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
         SiS_SetRegAND(SiS_Pr->SiS_Part4Port,0x21,0x3f);
 
         if((SiS_Pr->SiS_VBInfo & DisableCRT2Display) ||
@@ -2745,7 +2682,7 @@ SiS_SetCRT2ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
   } else {  /* LVDS */
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
      if(SiS_Pr->ChipType >= SIS_315H) {
 
         if(SiS_Pr->SiS_IF_DEF_CH70xx != 0) {
@@ -2931,7 +2868,7 @@ SiS_GetCRT2Ptr(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
            }
         }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
         if(SiS_Pr->SiS_CustomT == CUT_COMPAQ1280) {
            if(SiS_Pr->SiS_LCDResInfo == Panel_1280x1024) {
               if(!(SiS_Pr->SiS_LCDInfo & DontExpandLCD)) {
@@ -3036,7 +2973,7 @@ SiS_GetCRT2Ptr(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
         case Panel_1280x1024: tempbx = 24; break;
         case Panel_1400x1050: tempbx = 26; break;
         case Panel_1600x1200: tempbx = 28; break;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
         case Panel_Barco1366: tempbx = 80; break;
 #endif
         }
@@ -3053,7 +2990,7 @@ SiS_GetCRT2Ptr(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
         if(SiS_Pr->SiS_LCDInfo & LCDPass11) tempbx = 30;
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
         if(SiS_Pr->SiS_CustomT == CUT_BARCO1024) {
            tempbx = 82;
            if(SiS_Pr->SiS_LCDInfo & DontExpandLCD) tempbx++;
@@ -3189,7 +3126,7 @@ SiS_GetCRT2DataLVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
 
    if((SiS_Pr->SiS_VBType & VB_SISVB) && (SiS_Pr->SiS_VBInfo & SetCRT2ToLCDA)) {
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
       SiS_CalcPanelLinkTiming(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
       SiS_CalcLCDACRT1Timing(SiS_Pr, ModeNo, ModeIdIndex);
 #endif
@@ -3214,7 +3151,7 @@ SiS_GetCRT2DataLVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
          case 16: LVDSData = SiS_Pr->SiS_LVDS800x600Data_1;    break;
          case 18: LVDSData = SiS_Pr->SiS_LVDS1024x600Data_1;   break;
          case 20: LVDSData = SiS_Pr->SiS_LVDS1024x768Data_1;   break;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
          case 80: LVDSData = SiS_Pr->SiS_LVDSBARCO1366Data_1;  break;
          case 81: LVDSData = SiS_Pr->SiS_LVDSBARCO1366Data_2;  break;
          case 82: LVDSData = SiS_Pr->SiS_LVDSBARCO1024Data_1;  break;
@@ -3248,7 +3185,7 @@ SiS_GetCRT2DataLVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned
              (SiS_Pr->SiS_SetFlag & SetDOSMode) ) {
             SiS_Pr->SiS_HDE = SiS_Pr->PanelXRes;
             SiS_Pr->SiS_VDE = SiS_Pr->PanelYRes;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
             if(SiS_Pr->SiS_CustomT == CUT_BARCO1366) {
                if(ResIndex < 0x08) {
                   SiS_Pr->SiS_HDE = 1280;
@@ -3270,7 +3207,7 @@ SiS_GetCRT2Data301(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   unsigned short resinfo, CRT2Index, ResIndex;
   const struct SiS_LCDData *LCDPtr = NULL;
   const struct SiS_TVData  *TVPtr  = NULL;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   short resinfo661;
 #endif
 
@@ -3283,7 +3220,7 @@ SiS_GetCRT2Data301(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   } else {
      modeflag = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_ModeFlag;
      resinfo = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_RESINFO;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
      resinfo661 = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].ROMMODEIDX661;
      if( (SiS_Pr->SiS_VBInfo & SetCRT2ToLCD)   &&
          (SiS_Pr->SiS_SetFlag & LCDVESATiming) &&
@@ -3460,7 +3397,7 @@ SiS_GetCRT2Data301(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
 
         } else if( (!(SiS_Pr->SiS_LCDInfo & DontExpandLCD)) && (romptr) && (ROMAddr) ) {
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
            SiS_Pr->SiS_RVBHCMAX  = ROMAddr[romptr];
            SiS_Pr->SiS_RVBHCFACT = ROMAddr[romptr+1];
            SiS_Pr->SiS_VGAHT     = ROMAddr[romptr+2] | ((ROMAddr[romptr+3] & 0x0f) << 8);
@@ -3520,19 +3457,13 @@ SiS_GetCRT2Data301(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
               case Panel_1680x1050     :
               case Panel_1680x1050 + 32: LCDPtr = SiS_Pr->SiS_LCD1680x1050Data;     break;
               case 100                 : LCDPtr = SiS_Pr->SiS_NoScaleData;          break;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
               case 200                 : LCDPtr = SiS310_ExtCompaq1280x1024Data;    break;
               case 201                 : LCDPtr = SiS_Pr->SiS_St2LCD1280x1024Data;  break;
 #endif
               default                  : LCDPtr = SiS_Pr->SiS_ExtLCD1024x768Data;   break;
            }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-           xf86DrvMsg(0, X_INFO, "GetCRT2Data: Index %d ResIndex %d\n", CRT2Index, ResIndex);
-#endif
-#endif
-
            SiS_Pr->SiS_RVBHCMAX  = (LCDPtr+ResIndex)->RVBHCMAX;
            SiS_Pr->SiS_RVBHCFACT = (LCDPtr+ResIndex)->RVBHCFACT;
            SiS_Pr->SiS_VGAHT     = (LCDPtr+ResIndex)->VGAHT;
@@ -3624,7 +3555,7 @@ SiS_GetLVDSDesPtr(struct SiS_Private *SiS_Pr)
 {
    const struct SiS_LVDSDes *PanelDesPtr = NULL;
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    if(SiS_Pr->SiS_VBInfo & SetCRT2ToLCD) {
 
       if(SiS_Pr->ChipType < SIS_315H) {
@@ -3696,7 +3627,7 @@ SiS_GetLVDSDesData(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
 
   if((SiS_Pr->SiS_VBType & VB_SIS30xBLV) && (SiS_Pr->SiS_VBInfo & SetCRT2ToLCDA)) {
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
      if(SiS_Pr->SiS_LCDInfo & DontExpandLCD) {
         /* non-pass 1:1 only, see above */
         if(SiS_Pr->SiS_VGAHDE != SiS_Pr->PanelXRes) {
@@ -3771,7 +3702,7 @@ SiS_GetLVDSDesData(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
      } else {
 
         if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
            switch(SiS_Pr->SiS_LCDResInfo) {
            case Panel_800x600:
               if(SiS_Pr->SiS_VGAVDE == SiS_Pr->PanelYRes) {
@@ -3816,7 +3747,7 @@ SiS_GetLVDSDesData(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
            }
 #endif
         } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
            switch(SiS_Pr->SiS_LCDResInfo) {
            case Panel_1024x768:
            case Panel_1280x1024:
@@ -3844,7 +3775,7 @@ SiS_GetLVDSDesData(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
                  if(SiS_Pr->ChipType < SIS_315H) {
                     if(!(modeflag & HalfDCLK)) SiS_Pr->SiS_LCDHDES = 320;
                  } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
                     if(SiS_Pr->SiS_LCDResInfo == Panel_1024x768)  SiS_Pr->SiS_LCDHDES = 480;
                     if(SiS_Pr->SiS_LCDResInfo == Panel_1400x1050) SiS_Pr->SiS_LCDHDES = 804;
                     if(SiS_Pr->SiS_LCDResInfo == Panel_1600x1200) SiS_Pr->SiS_LCDHDES = 704;
@@ -3866,7 +3797,7 @@ SiS_GetLVDSDesData(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
 /*           DISABLE VIDEO BRIDGE            */
 /*********************************************/
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static int
 SiS_HandlePWD(struct SiS_Private *SiS_Pr)
 {
@@ -3891,11 +3822,6 @@ SiS_HandlePWD(struct SiS_Private *SiS_Pr)
          ret = 1;
       }
       SiS_SetRegANDOR(SiS_Pr->SiS_Part4Port,0x27,0x7f,temp);
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-      xf86DrvMsg(0, 0, "Setting PWD %x\n", temp);
-#endif
-#endif
    }
 #endif
    return ret;
@@ -3909,7 +3835,7 @@ SiS_HandlePWD(struct SiS_Private *SiS_Pr)
 void
 SiS_DisableBridge(struct SiS_Private *SiS_Pr)
 {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned short tempah, pushax=0, modenum;
 #endif
   unsigned short temp=0;
@@ -3920,7 +3846,7 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
 
         if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300      /* 300 series */
+#ifdef CONFIG_FB_SIS_300           /* 300 series */
 
            if(!(SiS_CR36BIOSWord23b(SiS_Pr))) {
               if(SiS_Pr->SiS_VBType & VB_SISLVDS) {
@@ -3953,11 +3879,11 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
               }
            }
 
-#endif  /* SIS300 */
+#endif  /* CONFIG_FB_SIS_300 */
 
         } else {
 
-#ifdef SIS315H     /* 315 series */
+#ifdef CONFIG_FB_SIS_315           /* 315 series */
 
            int didpwd = 0;
            bool custom1 = (SiS_Pr->SiS_CustomT == CUT_COMPAQ1280) ||
@@ -4081,14 +4007,14 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
 
            }
 
-#endif /* SIS315H */
+#endif /* CONFIG_FB_SIS_315 */
 
         }
 
      } else {     /* ============ For 301 ================ */
 
         if(SiS_Pr->ChipType < SIS_315H) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
            if(!(SiS_CR36BIOSWord23b(SiS_Pr))) {
               SiS_SetRegSR11ANDOR(SiS_Pr,0xF7,0x08);
               SiS_PanelDelay(SiS_Pr, 3);
@@ -4111,7 +4037,7 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
             SiS_SetRegOR(SiS_Pr->SiS_P3c4,0x1E,0x20);
             SiS_SetReg(SiS_Pr->SiS_Part1Port,0x00,temp);
         } else {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
             SiS_SetRegAND(SiS_Pr->SiS_P3c4,0x1E,0xDF);            /* disable CRT2 */
             if( (!(SiS_CRT2IsLCD(SiS_Pr))) ||
                 (!(SiS_CR36BIOSWord23d(SiS_Pr))) ) {
@@ -4127,7 +4053,7 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
 
     if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300   /* 300 series */
+#ifdef CONFIG_FB_SIS_300        /* 300 series */
 
         if(SiS_Pr->SiS_IF_DEF_CH70xx == 1) {
            SiS_SetCH700x(SiS_Pr,0x0E,0x09);
@@ -4171,11 +4097,11 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
            SiS_SetRegSR11ANDOR(SiS_Pr,0xFB,0x04);
         }
 
-#endif  /* SIS300 */
+#endif  /* CONFIG_FB_SIS_300 */
 
     } else {
 
-#ifdef SIS315H  /* 315 series */
+#ifdef CONFIG_FB_SIS_315        /* 315 series */
 
         if(!(SiS_IsNotM650orLater(SiS_Pr))) {
            /*if(SiS_Pr->ChipType < SIS_340) { */ /* XGI needs this */
@@ -4288,7 +4214,7 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
            }
         }
 
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 
     }  /* 315 series */
 
@@ -4304,14 +4230,12 @@ SiS_DisableBridge(struct SiS_Private *SiS_Pr)
  * from outside the context of a mode switch!
  * MUST call getVBType before calling this
  */
-#ifdef SIS_LINUX_KERNEL
 static
-#endif
 void
 SiS_EnableBridge(struct SiS_Private *SiS_Pr)
 {
   unsigned short temp=0, tempah;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned short temp1, pushax=0;
   bool delaylong = false;
 #endif
@@ -4322,7 +4246,7 @@ SiS_EnableBridge(struct SiS_Private *SiS_Pr)
 
       if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300     /* 300 series */
+#ifdef CONFIG_FB_SIS_300     /* 300 series */
 
          if(SiS_CRT2IsLCD(SiS_Pr)) {
             if(SiS_Pr->SiS_VBType & VB_SISLVDS) {
@@ -4385,11 +4309,11 @@ SiS_EnableBridge(struct SiS_Private *SiS_Pr)
          }
 
 
-#endif /* SIS300 */
+#endif /* CONFIG_FB_SIS_300 */
 
       } else {
 
-#ifdef SIS315H    /* 315 series */
+#ifdef CONFIG_FB_SIS_315    /* 315 series */
 
 #ifdef SET_EMI
          unsigned char   r30=0, r31=0, r32=0, r33=0, cr36=0;
@@ -4688,7 +4612,7 @@ SiS_EnableBridge(struct SiS_Private *SiS_Pr)
             SiS_SetRegAND(SiS_Pr->SiS_Part1Port,0x00,0x7f);
          }
 
-#endif /* SIS315H */
+#endif /* CONFIG_FB_SIS_315 */
 
       }
 
@@ -4739,7 +4663,7 @@ SiS_EnableBridge(struct SiS_Private *SiS_Pr)
 
     if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300    /* 300 series */
+#ifdef CONFIG_FB_SIS_300    /* 300 series */
 
        if(SiS_CRT2IsLCD(SiS_Pr)) {
           if(SiS_Pr->ChipType == SIS_730) {
@@ -4783,11 +4707,11 @@ SiS_EnableBridge(struct SiS_Private *SiS_Pr)
           }
        }
 
-#endif  /* SIS300 */
+#endif  /* CONFIG_FB_SIS_300 */
 
     } else {
 
-#ifdef SIS315H    /* 315 series */
+#ifdef CONFIG_FB_SIS_315    /* 315 series */
 
        if(!(SiS_IsNotM650orLater(SiS_Pr))) {
           /*if(SiS_Pr->ChipType < SIS_340) {*/  /* XGI needs this */
@@ -4881,7 +4805,7 @@ SiS_EnableBridge(struct SiS_Private *SiS_Pr)
           }
        }
 
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 
     } /* 310 series */
 
@@ -4971,7 +4895,7 @@ SiS_SetCRT2Sync(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned shor
 
       if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300  /* ---- 300 series --- */
+#ifdef CONFIG_FB_SIS_300  /* ---- 300 series --- */
 
          if(SiS_Pr->SiS_VBType & VB_SIS30xBLV) {                        /* 630 - 301B(-DH) */
 
@@ -5000,11 +4924,11 @@ SiS_SetCRT2Sync(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned shor
 
          }
 
-#endif /* SIS300 */
+#endif /* CONFIG_FB_SIS_300 */
 
       } else {
 
-#ifdef SIS315H  /* ------- 315 series ------ */
+#ifdef CONFIG_FB_SIS_315  /* ------- 315 series ------ */
 
          if(SiS_Pr->SiS_VBType & VB_SISLVDS) {                  /* 315 - LVDS */
 
@@ -5076,13 +5000,13 @@ SiS_SetCRT2Sync(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned shor
             }
 
          }
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
       }
    }
 }
 
 /* Set CRT2 FIFO on 300/540/630/730 */
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static void
 SiS_SetCRT2FIFO_300(struct SiS_Private *SiS_Pr,unsigned short ModeNo)
 {
@@ -5154,13 +5078,8 @@ SiS_SetCRT2FIFO_300(struct SiS_Private *SiS_Pr,unsigned short ModeNo)
 
      } else {
 
-#ifdef SIS_LINUX_KERNEL
         pci50 = sisfb_read_nbridge_pci_dword(SiS_Pr, 0x50);
         pciA0 = sisfb_read_nbridge_pci_dword(SiS_Pr, 0xa0);
-#else
-        pci50 = pciReadLong(0x00000000, 0x50);
-        pciA0 = pciReadLong(0x00000000, 0xA0);
-#endif
 
         if(SiS_Pr->ChipType == SIS_730) {
 
@@ -5262,7 +5181,7 @@ SiS_SetCRT2FIFO_300(struct SiS_Private *SiS_Pr,unsigned short ModeNo)
 #endif
 
 /* Set CRT2 FIFO on 315/330 series */
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void
 SiS_SetCRT2FIFO_310(struct SiS_Private *SiS_Pr)
 {
@@ -5420,27 +5339,6 @@ SiS_SetGroup1_301(struct SiS_Private *SiS_Pr, unsigned short ModeNo,unsigned sho
 
   temp = SiS_GetRegByte((SiS_Pr->SiS_P3ca+0x02));
   SiS_SetReg(SiS_Pr->SiS_Part1Port,0x1b,temp);                  /* ? */
-
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-   xf86DrvMsg(0, X_INFO, "%d %d %d %d  %d %d %d %d  (%d %d %d %d)\n",
-        SiS_Pr->CHDisplay, SiS_Pr->CHSyncStart, SiS_Pr->CHSyncEnd, SiS_Pr->CHTotal,
-        SiS_Pr->CVDisplay, SiS_Pr->CVSyncStart, SiS_Pr->CVSyncEnd, SiS_Pr->CVTotal,
-        SiS_Pr->CHBlankStart, SiS_Pr->CHBlankEnd, SiS_Pr->CVBlankStart, SiS_Pr->CVBlankEnd);
-
-   xf86DrvMsg(0, X_INFO, " {{0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,\n",
-        SiS_Pr->CCRT1CRTC[0], SiS_Pr->CCRT1CRTC[1],
-        SiS_Pr->CCRT1CRTC[2], SiS_Pr->CCRT1CRTC[3],
-        SiS_Pr->CCRT1CRTC[4], SiS_Pr->CCRT1CRTC[5],
-        SiS_Pr->CCRT1CRTC[6], SiS_Pr->CCRT1CRTC[7]);
-   xf86DrvMsg(0, X_INFO, "   0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,\n",
-        SiS_Pr->CCRT1CRTC[8], SiS_Pr->CCRT1CRTC[9],
-        SiS_Pr->CCRT1CRTC[10], SiS_Pr->CCRT1CRTC[11],
-        SiS_Pr->CCRT1CRTC[12], SiS_Pr->CCRT1CRTC[13],
-        SiS_Pr->CCRT1CRTC[14], SiS_Pr->CCRT1CRTC[15]);
-   xf86DrvMsg(0, X_INFO, "   0x%02x}},\n", SiS_Pr->CCRT1CRTC[16]);
-#endif
-#endif
 }
 
 /* Setup panel link
@@ -5455,17 +5353,17 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   unsigned short push2, tempax, tempbx, tempcx, temp;
   unsigned int   tempeax = 0, tempebx, tempecx, tempvcfact = 0;
   bool islvds = false, issis  = false, chkdclkfirst = false;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
   unsigned short crt2crtc = 0;
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned short pushcx;
 #endif
 
   if(ModeNo <= 0x13) {
      modeflag = SiS_Pr->SiS_SModeIDTable[ModeIdIndex].St_ModeFlag;
      resinfo = SiS_Pr->SiS_SModeIDTable[ModeIdIndex].St_ResInfo;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
      crt2crtc = SiS_Pr->SiS_SModeIDTable[ModeIdIndex].St_CRT2CRTC;
 #endif
   } else if(SiS_Pr->UseCustomMode) {
@@ -5473,7 +5371,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   } else {
      modeflag = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_ModeFlag;
      resinfo = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_RESINFO;
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
      crt2crtc = SiS_Pr->SiS_RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
 #endif
   }
@@ -5494,7 +5392,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
      }
   }
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   if((SiS_Pr->ChipType >= SIS_315H) && (SiS_Pr->SiS_VBInfo & SetCRT2ToLCDA)) {
      if(IS_SIS330) {
         SiS_SetRegOR(SiS_Pr->SiS_Part1Port,0x2D,0x10);
@@ -5744,7 +5642,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
 
   if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300      /* 300 series */
+#ifdef CONFIG_FB_SIS_300      /* 300 series */
      tempeax = SiS_Pr->SiS_VGAVDE << 6;
      temp = (tempeax % (unsigned int)SiS_Pr->SiS_VDE);
      tempeax = tempeax / (unsigned int)SiS_Pr->SiS_VDE;
@@ -5755,11 +5653,11 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
      temp = (unsigned short)(tempeax & 0x00FF);
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x1E,temp);       /* BPLVCFACT */
      tempvcfact = temp;
-#endif /* SIS300 */
+#endif /* CONFIG_FB_SIS_300 */
 
   } else {
 
-#ifdef SIS315H  /* 315 series */
+#ifdef CONFIG_FB_SIS_315  /* 315 series */
      tempeax = SiS_Pr->SiS_VGAVDE << 18;
      tempebx = SiS_Pr->SiS_VDE;
      temp = (tempeax % tempebx);
@@ -5845,7 +5743,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   temp = (unsigned short)(tempecx & 0x00FF);
   SiS_SetReg(SiS_Pr->SiS_Part1Port,0x23,temp);
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   if(SiS_Pr->ChipType >= SIS_315H) {
      if(SiS_Pr->SiS_VBInfo & SetCRT2ToLCDA) {
         if((islvds) || (SiS_Pr->SiS_VBInfo & VB_SISLVDS)) {
@@ -5863,7 +5761,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   }
 #endif
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
   if(SiS_Pr->SiS_IF_DEF_TRUMPION) {
      unsigned char *ROMAddr = SiS_Pr->VirtualRomBase;
      unsigned char *trumpdata;
@@ -5899,7 +5797,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
   }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   if(SiS_Pr->SiS_IF_DEF_FSTN || SiS_Pr->SiS_IF_DEF_DSTN) {
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x25,0x00);
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x26,0x00);
@@ -5954,7 +5852,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
      temp = tempax & 0x00FF;
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x43,temp);
      temp = ((tempax & 0xFF00) >> 8) << 3;
-     SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port,0x44,~0x0F8,temp);
+     SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port, 0x44, 0x07, temp);
 
      tempax = SiS_Pr->SiS_VDE;                                  /* BDxWadrst1 = BDxWadrst0 + BDxWadroff * VDE */
      if(SiS_Pr->SiS_LCDResInfo == Panel_320x240_1 ||
@@ -5968,7 +5866,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
      temp = ((tempeax & 0xFF0000) >> 16) | 0x10;
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x40,temp);
      temp = ((tempeax & 0x01000000) >> 24) << 7;
-     SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port,0x3C,~0x080,temp);
+     SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port, 0x3C, 0x7F, temp);
 
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x2F,0x03);
      SiS_SetReg(SiS_Pr->SiS_Part1Port,0x03,0x50);
@@ -5999,7 +5897,7 @@ SiS_SetGroup1_LVDS(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned s
         SiS_SetReg(SiS_Pr->SiS_Part1Port,0x45,0x0a);
      }
   }
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 }
 
 /* Set Part 1 */
@@ -6007,12 +5905,12 @@ static void
 SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short ModeIdIndex,
                 unsigned short RefreshRateTableIndex)
 {
-#if defined(SIS300) || defined(SIS315H)
+#if defined(CONFIG_FB_SIS_300) || defined(CONFIG_FB_SIS_315)
   unsigned char   *ROMAddr = SiS_Pr->VirtualRomBase;
 #endif
   unsigned short  temp=0, tempax=0, tempbx=0, tempcx=0, bridgeadd=0;
   unsigned short  pushbx=0, CRT1Index=0, modeflag, resinfo=0;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned short  tempbl=0;
 #endif
 
@@ -6038,11 +5936,11 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
          (SiS_Pr->SiS_VBInfo & SetInSlaveMode)) ) {
 
      if(SiS_Pr->ChipType < SIS_315H ) {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
         SiS_SetCRT2FIFO_300(SiS_Pr, ModeNo);
 #endif
      } else {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
         SiS_SetCRT2FIFO_310(SiS_Pr);
 #endif
      }
@@ -6051,7 +5949,7 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
      if(SiS_Pr->ChipType < SIS_315H ) {
 
-#ifdef SIS300   /* ------------- 300 series --------------*/
+#ifdef CONFIG_FB_SIS_300   /* ------------- 300 series --------------*/
 
         temp = (SiS_Pr->SiS_VGAHT - 1) & 0x0FF;                   /* BTVGA2HT 0x08,0x09 */
         SiS_SetReg(SiS_Pr->SiS_Part1Port,0x08,temp);              /* CRT2 Horizontal Total */
@@ -6070,11 +5968,11 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
         bridgeadd = 12;
 
-#endif /* SIS300 */
+#endif /* CONFIG_FB_SIS_300 */
 
      } else {
 
-#ifdef SIS315H  /* ------------------- 315/330 series --------------- */
+#ifdef CONFIG_FB_SIS_315  /* ------------------- 315/330 series --------------- */
 
         tempcx = SiS_Pr->SiS_VGAHT;                               /* BTVGA2HT 0x08,0x09 */
         if(modeflag & HalfDCLK) {
@@ -6125,7 +6023,7 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
            }
         }
 
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 
      }  /* 315/330 series */
 
@@ -6256,7 +6154,7 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
      if(SiS_Pr->ChipType < SIS_315H) {
 
-#ifdef SIS300  /* ---------- 300 series -------------- */
+#ifdef CONFIG_FB_SIS_300  /* ---------- 300 series -------------- */
 
         if(SiS_Pr->SiS_VBType & VB_SISVB) {
            temp = 0x20;
@@ -6310,11 +6208,11 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
         SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port,0x13,~0x3C,temp);   /* Panel Link Delay Compensation; (Software Command Reset; Power Saving) */
 
-#endif  /* SIS300 */
+#endif  /* CONFIG_FB_SIS_300 */
 
      } else {
 
-#ifdef SIS315H   /* --------------- 315/330 series ---------------*/
+#ifdef CONFIG_FB_SIS_315   /* --------------- 315/330 series ---------------*/
 
         if(SiS_Pr->ChipType < SIS_661) {
 
@@ -6349,7 +6247,7 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
         if(modeflag & HalfDCLK)       tempax |= 0x40;
         SiS_SetRegANDOR(SiS_Pr->SiS_Part1Port,0x2C,0x3f,tempax);
 
-#endif  /* SIS315H */
+#endif  /* CONFIG_FB_SIS_315 */
 
      }
 
@@ -6381,7 +6279,7 @@ SiS_SetGroup1(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 /*         SET PART 2 REGISTER GROUP         */
 /*********************************************/
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static unsigned char *
 SiS_GetGroup2CLVXPtr(struct SiS_Private *SiS_Pr, int tabletype)
 {
@@ -6478,7 +6376,7 @@ SiS_GetCRT2Part2Ptr(struct SiS_Private *SiS_Pr,unsigned short ModeNo,unsigned sh
 }
 #endif
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static void
 SiS_Group2LCDSpecial(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short crt2crtc)
 {
@@ -6690,7 +6588,7 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
   unsigned int   longtemp, PhaseIndex;
   bool           newtvphase;
   const unsigned char *TimingPoint;
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   unsigned short resindex, CRT2Index;
   const struct SiS_Part2PortTbl *CRT2Part2Ptr = NULL;
 
@@ -7069,7 +6967,7 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
   SiS_SetRegAND(SiS_Pr->SiS_Part2Port,0x17,0xFB);
   SiS_SetRegAND(SiS_Pr->SiS_Part2Port,0x18,0xDF);
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   if(SiS_GetCRT2Part2Ptr(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex,
                                                 &CRT2Index, &resindex)) {
       switch(CRT2Index) {
@@ -7130,12 +7028,6 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
     /* Non-expanding: lcdvdes = tempcx = VT-1; lcdvdee = tempbx = VDE-1 */
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "lcdvdes 0x%x lcdvdee 0x%x\n", tempcx, tempbx);
-#endif
-#endif
-
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x05,tempcx);      /* lcdvdes  */
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x06,tempbx);      /* lcdvdee  */
 
@@ -7184,12 +7076,6 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
        tempbx = SiS_Pr->CVSyncStart;
     }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "lcdvrs 0x%x\n", tempbx);
-#endif
-#endif
-
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x04,tempbx);          /* lcdvrs */
 
     temp = (tempbx >> 4) & 0xF0;
@@ -7201,15 +7087,9 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
        temp |= (SiS_Pr->CVSyncEnd & 0x0f);
     }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "lcdvre[3:0] 0x%x\n", (temp & 0x0f));
-#endif
-#endif
-
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x01,temp);
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
     SiS_Group2LCDSpecial(SiS_Pr, ModeNo, crt2crtc);
 #endif
 
@@ -7245,12 +7125,6 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
        tempax >>= 1;
     }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "lcdhdee 0x%x\n", tempbx);
-#endif
-#endif
-
     tempbx += bridgeoffset;
 
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x23,tempbx);          /* lcdhdee */
@@ -7276,12 +7150,6 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
        tempbx += bridgeoffset;
     }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "lcdhrs 0x%x\n", tempbx);
-#endif
-#endif
-
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x1C,tempbx);          /* lcdhrs */
     SiS_SetRegANDOR(SiS_Pr->SiS_Part2Port,0x1D,0x0F,((tempbx >> 4) & 0xf0));
 
@@ -7300,20 +7168,14 @@ SiS_SetGroup2(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
        tempbx += bridgeoffset;
     }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "lcdhre 0x%x\n", tempbx);
-#endif
-#endif
-
     SiS_SetReg(SiS_Pr->SiS_Part2Port,0x21,tempbx);          /* lcdhre */
 
     SiS_SetGroup2_Tail(SiS_Pr, ModeNo);
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
     SiS_Set300Part2Regs(SiS_Pr, ModeIdIndex, RefreshRateTableIndex, ModeNo);
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
   } /* CRT2-LCD from table */
 #endif
 }
@@ -7382,7 +7244,7 @@ SiS_SetGroup3(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 /*         SET PART 4 REGISTER GROUP         */
 /*********************************************/
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 #if 0
 static void
 SiS_ShiftXPos(struct SiS_Private *SiS_Pr, int shift)
@@ -8011,7 +7873,7 @@ SiS_SetCHTVReg(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
    if(SiS_Pr->SiS_IF_DEF_CH70xx == 1) {
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 
       /* Chrontel 7005 - I assume that it does not come with a 315 series chip */
 
@@ -8124,7 +7986,7 @@ SiS_SetCHTVReg(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
       /* Chrontel 7019 - assumed that it does not come with a 300 series chip */
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 
       unsigned short temp;
 
@@ -8175,7 +8037,7 @@ SiS_SetCHTVReg(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short
 
 }
 
-#ifdef SIS315H  /* ----------- 315 series only ---------- */
+#ifdef CONFIG_FB_SIS_315  /* ----------- 315 series only ---------- */
 
 void
 SiS_Chrontel701xBLOn(struct SiS_Private *SiS_Pr)
@@ -8657,7 +8519,7 @@ SiS_ChrontelDoSomething1(struct SiS_Private *SiS_Pr)
 bool
 SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 {
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    unsigned char  *ROMAddr  = SiS_Pr->VirtualRomBase;
 #endif
    unsigned short ModeIdIndex, RefreshRateTableIndex;
@@ -8703,16 +8565,6 @@ SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
       SiS_GetLVDSDesData(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
    }
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-  xf86DrvMsg(0, X_INFO, "(init301: LCDHDES 0x%03x LCDVDES 0x%03x)\n", SiS_Pr->SiS_LCDHDES, SiS_Pr->SiS_LCDVDES);
-  xf86DrvMsg(0, X_INFO, "(init301: HDE     0x%03x VDE     0x%03x)\n", SiS_Pr->SiS_HDE, SiS_Pr->SiS_VDE);
-  xf86DrvMsg(0, X_INFO, "(init301: VGAHDE  0x%03x VGAVDE  0x%03x)\n", SiS_Pr->SiS_VGAHDE, SiS_Pr->SiS_VGAVDE);
-  xf86DrvMsg(0, X_INFO, "(init301: HT      0x%03x VT      0x%03x)\n", SiS_Pr->SiS_HT, SiS_Pr->SiS_VT);
-  xf86DrvMsg(0, X_INFO, "(init301: VGAHT   0x%03x VGAVT   0x%03x)\n", SiS_Pr->SiS_VGAHT, SiS_Pr->SiS_VGAVT);
-#endif
-#endif
-
    if(SiS_Pr->SiS_SetFlag & LowModeTests) {
       SiS_SetGroup1(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
    }
@@ -8722,12 +8574,12 @@ SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
       if(SiS_Pr->SiS_SetFlag & LowModeTests) {
 
          SiS_SetGroup2(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
          SiS_SetGroup2_C_ELV(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
 #endif
          SiS_SetGroup3(SiS_Pr, ModeNo, ModeIdIndex);
          SiS_SetGroup4(SiS_Pr, ModeNo, ModeIdIndex, RefreshRateTableIndex);
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
          SiS_SetGroup4_C_ELV(SiS_Pr, ModeNo, ModeIdIndex);
 #endif
          SiS_SetGroup5(SiS_Pr, ModeNo, ModeIdIndex);
@@ -8758,7 +8610,7 @@ SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
          if(SiS_Pr->SiS_IF_DEF_CH70xx != 0) {
             if(SiS_Pr->SiS_VBInfo & (SetCRT2ToLCD | SetCRT2ToLCDA)) {
                if(SiS_Pr->SiS_IF_DEF_CH70xx == 2) {
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
                   SiS_SetCH701xForLCD(SiS_Pr);
 #endif
                }
@@ -8771,7 +8623,7 @@ SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 
    }
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
    if(SiS_Pr->ChipType < SIS_315H) {
       if(SiS_Pr->SiS_SetFlag & LowModeTests) {
          if(SiS_Pr->SiS_UseOEM) {
@@ -8794,7 +8646,7 @@ SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
    }
 #endif
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
    if(SiS_Pr->ChipType >= SIS_315H) {
       if(SiS_Pr->SiS_SetFlag & LowModeTests) {
          if(SiS_Pr->ChipType < SIS_661) {
@@ -8873,7 +8725,7 @@ SiS_SetupDDCN(struct SiS_Private *SiS_Pr)
   }
 }
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static unsigned char *
 SiS_SetTrumpBlockLoop(struct SiS_Private *SiS_Pr, unsigned char *dataptr)
 {
@@ -8923,11 +8775,6 @@ SiS_SetTrumpionBlock(struct SiS_Private *SiS_Pr, unsigned char *dataptr)
      dataptr = SiS_SetTrumpBlockLoop(SiS_Pr, dataptr);
      if(!dataptr) return false;
   }
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-  xf86DrvMsg(0, X_INFO, "Trumpion block success\n");
-#endif
-#endif
   return true;
 }
 #endif
@@ -9002,9 +8849,7 @@ SiS_SetCH701x(struct SiS_Private *SiS_Pr, unsigned short reg, unsigned char val)
   SiS_SetChReg(SiS_Pr, reg, val, 0);
 }
 
-#ifdef SIS_LINUX_KERNEL
 static
-#endif
 void
 SiS_SetCH70xx(struct SiS_Private *SiS_Pr, unsigned short reg, unsigned char val)
 {
@@ -9091,9 +8936,7 @@ SiS_GetCH701x(struct SiS_Private *SiS_Pr, unsigned short tempbx)
 
 /* Read from Chrontel 70xx */
 /* Parameter is [Register no (S7-S0)] */
-#ifdef SIS_LINUX_KERNEL
 static
-#endif
 unsigned short
 SiS_GetCH70xx(struct SiS_Private *SiS_Pr, unsigned short tempbx)
 {
@@ -9114,9 +8957,7 @@ SiS_SetCH70xxANDOR(struct SiS_Private *SiS_Pr, unsigned short reg,
 }
 
 /* Our own DDC functions */
-#ifndef SIS_XORG_XF86
 static
-#endif
 unsigned short
 SiS_InitDDCRegs(struct SiS_Private *SiS_Pr, unsigned int VBFlags, int VGAEngine,
                 unsigned short adaptnum, unsigned short DDCdatatype, bool checkcr32,
@@ -9224,12 +9065,6 @@ SiS_InitDDCRegs(struct SiS_Private *SiS_Pr, unsigned int VBFlags, int VGAEngine,
 
     SiS_SetupDDCN(SiS_Pr);
 
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-    xf86DrvMsg(0, X_INFO, "DDC Port %x Index %x Shift %d\n",
-                SiS_Pr->SiS_DDC_Port, SiS_Pr->SiS_DDC_Index, temp);
-#endif
-#endif
     return 0;
 }
 
@@ -9292,11 +9127,6 @@ SiS_DoProbeDDC(struct SiS_Private *SiS_Pr)
     SiS_SetSwitchDDC2(SiS_Pr);
     if(SiS_PrepareDDC(SiS_Pr)) {
          SiS_SetStop(SiS_Pr);
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-         xf86DrvMsg(0, X_INFO, "Probe: Prepare failed\n");
-#endif
-#endif
          return 0xFFFF;
     }
     mask = 0xf0;
@@ -9310,11 +9140,6 @@ SiS_DoProbeDDC(struct SiS_Private *SiS_Pr)
        } else {
            failed = true;
            ret = 0xFFFF;
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-           xf86DrvMsg(0, X_INFO, "Probe: Read 1 failed\n");
-#endif
-#endif
        }
     }
     if(!failed) {
@@ -9324,11 +9149,6 @@ SiS_DoProbeDDC(struct SiS_Private *SiS_Pr)
        if(temp == value) ret = 0;
        else {
           ret = 0xFFFF;
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-          xf86DrvMsg(0, X_INFO, "Probe: Read 2 failed\n");
-#endif
-#endif
           if(SiS_Pr->SiS_DDC_DeviceAddr == 0xa0) {
              if(temp == 0x30) ret = 0;
           }
@@ -9338,9 +9158,7 @@ SiS_DoProbeDDC(struct SiS_Private *SiS_Pr)
     return ret;
 }
 
-#ifndef SIS_XORG_XF86
 static
-#endif
 unsigned short
 SiS_ProbeDDC(struct SiS_Private *SiS_Pr)
 {
@@ -9357,9 +9175,7 @@ SiS_ProbeDDC(struct SiS_Private *SiS_Pr)
    return flag;
 }
 
-#ifndef SIS_XORG_XF86
 static
-#endif
 unsigned short
 SiS_ReadDDC(struct SiS_Private *SiS_Pr, unsigned short DDCdatatype, unsigned char *buffer)
 {
@@ -9606,11 +9422,6 @@ SiS_SetSCLKHigh(struct SiS_Private *SiS_Pr)
     temp = SiS_GetReg(SiS_Pr->SiS_DDC_Port,SiS_Pr->SiS_DDC_Index);
   } while((!(temp & SiS_Pr->SiS_DDC_Clk)) && --watchdog);
   if (!watchdog) {
-#ifdef SIS_XORG_XF86
-#ifdef TWDEBUG
-        xf86DrvMsg(0, X_INFO, "SetClkHigh failed\n");
-#endif
-#endif
         return 0xFFFF;
   }
   SiS_DDC2Delay(SiS_Pr,SiS_I2CDELAYSHORT);
@@ -9641,7 +9452,7 @@ SiS_CheckACK(struct SiS_Private *SiS_Pr)
 
 /* =============== SiS 315/330 O.E.M. ================= */
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 
 static unsigned short
 GetRAMDACromptr(struct SiS_Private *SiS_Pr)
@@ -10829,7 +10640,7 @@ SiS_FinalizeLCD(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned shor
 
 /*  =================  SiS 300 O.E.M. ================== */
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 
 static void
 SetOEMLCDData2(struct SiS_Private *SiS_Pr, unsigned short ModeNo,unsigned short ModeIdIndex,
diff --git a/drivers/video/sis/init301.h b/drivers/video/sis/init301.h
index 51d99222375d..2112d6d7feda 100644
--- a/drivers/video/sis/init301.h
+++ b/drivers/video/sis/init301.h
@@ -53,15 +53,8 @@
 #ifndef  _INIT301_H_
 #define  _INIT301_H_
 
-#include "osdef.h"
 #include "initdef.h"
 
-#ifdef SIS_XORG_XF86
-#include "sis.h"
-#include "sis_regs.h"
-#endif
-
-#ifdef SIS_LINUX_KERNEL
 #include "vgatypes.h"
 #include "vstruct.h"
 #ifdef SIS_CP
@@ -72,7 +65,6 @@
 #include <linux/fb.h>
 #include "sis.h"
 #include <video/sisfb.h>
-#endif
 
 static const unsigned char SiS_YPbPrTable[3][64] = {
   {
@@ -237,7 +229,7 @@ static const unsigned char SiS_Part2CLVX_6[] = {   /* 1080i */
     0xFF,0xFF,
 };
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 /* 661 et al LCD data structure (2.03.00) */
 static const unsigned char SiS_LCDStruct661[] = {
     /* 1024x768 */
@@ -279,7 +271,7 @@ static const unsigned char SiS_LCDStruct661[] = {
 };
 #endif
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static unsigned char SiS300_TrumpionData[14][80] = {
   { 0x02,0x0A,0x0A,0x01,0x04,0x01,0x00,0x03,0x0D,0x00,0x0D,0x10,0x7F,0x00,0x80,0x02,
     0x20,0x03,0x0B,0x00,0x90,0x01,0xC1,0x01,0x60,0x0C,0x30,0x10,0x00,0x00,0x04,0x23,
@@ -356,9 +348,6 @@ static unsigned char SiS300_TrumpionData[14][80] = {
 #endif
 
 void            SiS_UnLockCRT2(struct SiS_Private *SiS_Pr);
-#ifndef SIS_LINUX_KERNEL
-void            SiS_LockCRT2(struct SiS_Private *SiS_Pr);
-#endif
 void            SiS_EnableCRT2(struct SiS_Private *SiS_Pr);
 unsigned short  SiS_GetRatePtr(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned short ModeIdIndex);
 void            SiS_WaitRetrace1(struct SiS_Private *SiS_Pr);
@@ -375,9 +364,6 @@ unsigned short	SiS_GetVCLK2Ptr(struct SiS_Private *SiS_Pr, unsigned short ModeNo
                         unsigned short RefreshRateTableIndex);
 unsigned short  SiS_GetResInfo(struct SiS_Private *SiS_Pr,unsigned short ModeNo,unsigned short ModeIdIndex);
 void            SiS_DisableBridge(struct SiS_Private *SiS_Pr);
-#ifndef SIS_LINUX_KERNEL
-void            SiS_EnableBridge(struct SiS_Private *SiS_Pr);
-#endif
 bool            SiS_SetCRT2Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo);
 void            SiS_SiS30xBLOn(struct SiS_Private *SiS_Pr);
 void            SiS_SiS30xBLOff(struct SiS_Private *SiS_Pr);
@@ -386,13 +372,9 @@ void		SiS_SetCH700x(struct SiS_Private *SiS_Pr, unsigned short reg, unsigned cha
 unsigned short  SiS_GetCH700x(struct SiS_Private *SiS_Pr, unsigned short tempax);
 void            SiS_SetCH701x(struct SiS_Private *SiS_Pr, unsigned short reg, unsigned char val);
 unsigned short  SiS_GetCH701x(struct SiS_Private *SiS_Pr, unsigned short tempax);
-#ifndef SIS_LINUX_KERNEL
-void            SiS_SetCH70xx(struct SiS_Private *SiS_Pr, unsigned short reg, unsigned char val);
-unsigned short  SiS_GetCH70xx(struct SiS_Private *SiS_Pr, unsigned short tempax);
-#endif
 void            SiS_SetCH70xxANDOR(struct SiS_Private *SiS_Pr, unsigned short reg,
                         unsigned char orval,unsigned short andval);
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void     SiS_Chrontel701xOn(struct SiS_Private *SiS_Pr);
 static void     SiS_Chrontel701xOff(struct SiS_Private *SiS_Pr);
 static void     SiS_ChrontelInitTVVSync(struct SiS_Private *SiS_Pr);
@@ -401,7 +383,7 @@ void		SiS_Chrontel701xBLOn(struct SiS_Private *SiS_Pr);
 void            SiS_Chrontel701xBLOff(struct SiS_Private *SiS_Pr);
 #endif /* 315 */
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static  bool    SiS_SetTrumpionBlock(struct SiS_Private *SiS_Pr, unsigned char *dataptr);
 void            SiS_SetChrontelGPIO(struct SiS_Private *SiS_Pr, unsigned short myvbinfo);
 #endif
@@ -412,21 +394,12 @@ unsigned short	SiS_HandleDDC(struct SiS_Private *SiS_Pr, unsigned int VBFlags, i
                         unsigned short adaptnum, unsigned short DDCdatatype,
                         unsigned char *buffer, unsigned int VBFlags2);
 
-#ifdef SIS_XORG_XF86
-unsigned short          SiS_InitDDCRegs(struct SiS_Private *SiS_Pr, unsigned int VBFlags,
-                                int VGAEngine, unsigned short adaptnum, unsigned short DDCdatatype,
-                                bool checkcr32, unsigned int VBFlags2);
-unsigned short          SiS_ProbeDDC(struct SiS_Private *SiS_Pr);
-unsigned short          SiS_ReadDDC(struct SiS_Private *SiS_Pr, unsigned short DDCdatatype,
-                                unsigned char *buffer);
-#else
 static unsigned short   SiS_InitDDCRegs(struct SiS_Private *SiS_Pr, unsigned int VBFlags,
                                 int VGAEngine, unsigned short adaptnum, unsigned short DDCdatatype,
                                 bool checkcr32, unsigned int VBFlags2);
 static unsigned short   SiS_ProbeDDC(struct SiS_Private *SiS_Pr);
 static unsigned short   SiS_ReadDDC(struct SiS_Private *SiS_Pr, unsigned short DDCdatatype,
                                 unsigned char *buffer);
-#endif
 static void             SiS_SetSwitchDDC2(struct SiS_Private *SiS_Pr);
 static unsigned short   SiS_SetStart(struct SiS_Private *SiS_Pr);
 static unsigned short   SiS_SetStop(struct SiS_Private *SiS_Pr);
@@ -441,13 +414,13 @@ static unsigned short	SiS_PrepareDDC(struct SiS_Private *SiS_Pr);
 static void             SiS_SendACK(struct SiS_Private *SiS_Pr, unsigned short yesno);
 static unsigned short   SiS_DoProbeDDC(struct SiS_Private *SiS_Pr);
 
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 static void             SiS_OEM300Setting(struct SiS_Private *SiS_Pr,
                                 unsigned short ModeNo, unsigned short ModeIdIndex, unsigned short RefTabindex);
 static void             SetOEMLCDData2(struct SiS_Private *SiS_Pr,
                                 unsigned short ModeNo, unsigned short ModeIdIndex,unsigned short RefTableIndex);
 #endif
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
 static void             SiS_OEM310Setting(struct SiS_Private *SiS_Pr,
                                 unsigned short ModeNo,unsigned short ModeIdIndex, unsigned short RRTI);
 static void             SiS_OEM661Setting(struct SiS_Private *SiS_Pr,
@@ -455,17 +428,6 @@ static void		SiS_OEM661Setting(struct SiS_Private *SiS_Pr,
 static void             SiS_FinalizeLCD(struct SiS_Private *, unsigned short, unsigned short);
 #endif
 
-extern void             SiS_SetReg(SISIOADDRESS, unsigned short, unsigned short);
-extern void             SiS_SetRegByte(SISIOADDRESS, unsigned short);
-extern void             SiS_SetRegShort(SISIOADDRESS, unsigned short);
-extern void             SiS_SetRegLong(SISIOADDRESS, unsigned int);
-extern unsigned char    SiS_GetReg(SISIOADDRESS, unsigned short);
-extern unsigned char    SiS_GetRegByte(SISIOADDRESS);
-extern unsigned short   SiS_GetRegShort(SISIOADDRESS);
-extern unsigned int     SiS_GetRegLong(SISIOADDRESS);
-extern void             SiS_SetRegANDOR(SISIOADDRESS, unsigned short, unsigned short, unsigned short);
-extern void             SiS_SetRegOR(SISIOADDRESS, unsigned short, unsigned short);
-extern void             SiS_SetRegAND(SISIOADDRESS, unsigned short, unsigned short);
 extern void             SiS_DisplayOff(struct SiS_Private *SiS_Pr);
 extern void             SiS_DisplayOn(struct SiS_Private *SiS_Pr);
 extern bool             SiS_SearchModeID(struct SiS_Private *, unsigned short *, unsigned short *);
@@ -482,15 +444,13 @@ extern void		SiS_CalcLCDACRT1Timing(struct SiS_Private *SiS_Pr, unsigned short M
 extern void             SiS_CalcCRRegisters(struct SiS_Private *SiS_Pr, int depth);
 extern unsigned short   SiS_GetRefCRTVCLK(struct SiS_Private *SiS_Pr, unsigned short Index, int UseWide);
 extern unsigned short   SiS_GetRefCRT1CRTC(struct SiS_Private *SiS_Pr, unsigned short Index, int UseWide);
-#ifdef SIS300
+#ifdef CONFIG_FB_SIS_300
 extern void             SiS_GetFIFOThresholdIndex300(struct SiS_Private *SiS_Pr, unsigned short *tempbx,
                                 unsigned short *tempcl);
 extern unsigned short   SiS_GetFIFOThresholdB300(unsigned short tempbx, unsigned short tempcl);
 extern unsigned short   SiS_GetLatencyFactor630(struct SiS_Private *SiS_Pr, unsigned short index);
-#ifdef SIS_LINUX_KERNEL
 extern unsigned int     sisfb_read_nbridge_pci_dword(struct SiS_Private *SiS_Pr, int reg);
 extern unsigned int     sisfb_read_lpc_pci_dword(struct SiS_Private *SiS_Pr, int reg);
 #endif
-#endif
 
 #endif
diff --git a/drivers/video/sis/initextlfb.c b/drivers/video/sis/initextlfb.c
index 99c04a4855d1..9dec64da4015 100644
--- a/drivers/video/sis/initextlfb.c
+++ b/drivers/video/sis/initextlfb.c
@@ -25,7 +25,6 @@
  * Author:      Thomas Winischhofer <thomas@winischhofer.net>
  */
 
-#include "osdef.h"
 #include "initdef.h"
 #include "vgatypes.h"
 #include "vstruct.h"
@@ -59,7 +58,7 @@ sisfb_mode_rate_to_dclock(struct SiS_Private *SiS_Pr, unsigned char modeno,
 
     if(rateindex > 0) rateindex--;
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
     switch(ModeNo) {
     case 0x5a: ModeNo = 0x50; break;
     case 0x5b: ModeNo = 0x56;
@@ -103,7 +102,7 @@ sisfb_mode_rate_to_ddata(struct SiS_Private *SiS_Pr, unsigned char modeno,
 
     if(rateindex > 0) rateindex--;
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
     switch(ModeNo) {
        case 0x5a: ModeNo = 0x50; break;
        case 0x5b: ModeNo = 0x56;
@@ -187,7 +186,7 @@ sisfb_gettotalfrommode(struct SiS_Private *SiS_Pr, unsigned char modeno, int *ht
 
     if(rateindex > 0) rateindex--;
 
-#ifdef SIS315H
+#ifdef CONFIG_FB_SIS_315
     switch(ModeNo) {
        case 0x5a: ModeNo = 0x50; break;
        case 0x5b: ModeNo = 0x56;
diff --git a/drivers/video/sis/osdef.h b/drivers/video/sis/osdef.h
deleted file mode 100644
index 6ff8f988a1a7..000000000000
--- a/drivers/video/sis/osdef.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/* $XFree86$ */
-/* $XdotOrg$ */
-/*
- * OS depending defines
- *
- * Copyright (C) 2001-2005 by Thomas Winischhofer, Vienna, Austria
- *
- * If distributed as part of the Linux kernel, the following license terms
- * apply:
- *
- * * 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 named License,
- * * or any later version.
- * *
- * * This program is distributed in the hope that it will be useful,
- * * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * * GNU General Public License for more details.
- * *
- * * You should have received a copy of the GNU General Public License
- * * along with this program; if not, write to the Free Software
- * * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA
- *
- * Otherwise, the following license terms apply:
- *
- * * Redistribution and use in source and binary forms, with or without
- * * modification, are permitted provided that the following conditions
- * * are met:
- * * 1) Redistributions of source code must retain the above copyright
- * *    notice, this list of conditions and the following disclaimer.
- * * 2) Redistributions in binary form must reproduce the above copyright
- * *    notice, this list of conditions and the following disclaimer in the
- * *    documentation and/or other materials provided with the distribution.
- * * 3) The name of the author may not be used to endorse or promote products
- * *    derived from this software without specific prior written permission.
- * *
- * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
- * * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
- * * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
- * * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
- * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
- * * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
- * * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * Author:      Thomas Winischhofer <thomas@winischhofer.net>
- *              Silicon Integrated Systems, Inc. (used by permission)
- *
- */
-
-#ifndef _SIS_OSDEF_H_
-#define _SIS_OSDEF_H_
-
-/* The choices are: */
-#define SIS_LINUX_KERNEL                /* Linux kernel framebuffer */
-#undef  SIS_XORG_XF86                   /* XFree86/X.org */
-
-#ifdef OutPortByte
-#undef OutPortByte
-#endif
-
-#ifdef OutPortWord
-#undef OutPortWord
-#endif
-
-#ifdef OutPortLong
-#undef OutPortLong
-#endif
-
-#ifdef InPortByte
-#undef InPortByte
-#endif
-
-#ifdef InPortWord
-#undef InPortWord
-#endif
-
-#ifdef InPortLong
-#undef InPortLong
-#endif
-
-/**********************************************************************/
-/*  LINUX KERNEL                                                      */
-/**********************************************************************/
-
-#ifdef SIS_LINUX_KERNEL
-
-#ifdef CONFIG_FB_SIS_300
-#define SIS300
-#endif
-
-#ifdef CONFIG_FB_SIS_315
-#define SIS315H
-#endif
-
-#if !defined(SIS300) && !defined(SIS315H)
-#warning Neither CONFIG_FB_SIS_300 nor CONFIG_FB_SIS_315 is set
-#warning sisfb will not work!
-#endif
-
-#define OutPortByte(p,v) outb((u8)(v),(SISIOADDRESS)(p))
-#define OutPortWord(p,v) outw((u16)(v),(SISIOADDRESS)(p))
-#define OutPortLong(p,v) outl((u32)(v),(SISIOADDRESS)(p))
-#define InPortByte(p)    inb((SISIOADDRESS)(p))
-#define InPortWord(p)    inw((SISIOADDRESS)(p))
-#define InPortLong(p)    inl((SISIOADDRESS)(p))
-#define SiS_SetMemory(MemoryAddress,MemorySize,value) memset_io(MemoryAddress, value, MemorySize)
-
-#endif /* LINUX_KERNEL */
-
-/**********************************************************************/
-/*  XFree86/X.org                                                    */
-/**********************************************************************/
-
-#ifdef SIS_XORG_XF86
-
-#define SIS300
-#define SIS315H
-
-#define OutPortByte(p,v) outSISREG((IOADDRESS)(p),(CARD8)(v))
-#define OutPortWord(p,v) outSISREGW((IOADDRESS)(p),(CARD16)(v))
-#define OutPortLong(p,v) outSISREGL((IOADDRESS)(p),(CARD32)(v))
-#define InPortByte(p)    inSISREG((IOADDRESS)(p))
-#define InPortWord(p)    inSISREGW((IOADDRESS)(p))
-#define InPortLong(p)    inSISREGL((IOADDRESS)(p))
-#define SiS_SetMemory(MemoryAddress,MemorySize,value) memset(MemoryAddress, value, MemorySize)
-
-#endif /* XF86 */
-
-#endif  /* _OSDEF_H_ */
diff --git a/drivers/video/sis/sis.h b/drivers/video/sis/sis.h
index 7c5710e3fb56..1987f1b7212f 100644
--- a/drivers/video/sis/sis.h
+++ b/drivers/video/sis/sis.h
@@ -24,7 +24,6 @@
 #ifndef _SIS_H_
 #define _SIS_H_
 
-#include "osdef.h"
 #include <video/sisfb.h>
 
 #include "vgatypes.h"
@@ -308,58 +307,19 @@
 #define VB2_LCDOVER1600BRIDGE   (VB2_307T  | VB2_307LV)
 #define VB2_RAMDAC202MHZBRIDGE  (VB2_301C  | VB2_307T)
 
-/* I/O port access macros */
-#define inSISREG(base)          inb(base)
-
-#define outSISREG(base,val)     outb(val,base)
-
-#define orSISREG(base,val)                              \
-                do {                                    \
-                        u8 __Temp = inSISREG(base);     \
-                        outSISREG(base, __Temp | (val));\
-                } while (0)
-
-#define andSISREG(base,val)                             \
-                do {                                    \
-                        u8 __Temp = inSISREG(base);     \
-                        outSISREG(base, __Temp & (val));\
-                } while (0)
-
-#define inSISIDXREG(base,idx,var)                       \
-                do {                                    \
-                        outSISREG(base, idx);           \
-                        var = inSISREG((base)+1);       \
-                } while (0)
-
-#define outSISIDXREG(base,idx,val)                      \
-                do {                                    \
-                        outSISREG(base, idx);           \
-                        outSISREG((base)+1, val);       \
-                } while (0)
-
-#define orSISIDXREG(base,idx,val)                               \
-                do {                                            \
-                        u8 __Temp;                              \
-                        outSISREG(base, idx);                   \
-                        __Temp = inSISREG((base)+1) | (val);    \
-                        outSISREG((base)+1, __Temp);            \
-                } while (0)
-
-#define andSISIDXREG(base,idx,and)                              \
-                do {                                            \
-                        u8 __Temp;                              \
-                        outSISREG(base, idx);                   \
-                        __Temp = inSISREG((base)+1) & (and);    \
-                        outSISREG((base)+1, __Temp);            \
-                } while (0)
-
-#define setSISIDXREG(base,idx,and,or)                                   \
-                do {                                                    \
-                        u8 __Temp;                                      \
-                        outSISREG(base, idx);                           \
-                        __Temp = (inSISREG((base)+1) & (and)) | (or);   \
-                        outSISREG((base)+1, __Temp);                    \
-                } while (0)
+/* I/O port access functions */
+
+void SiS_SetReg(SISIOADDRESS, u8, u8);
+void SiS_SetRegByte(SISIOADDRESS, u8);
+void SiS_SetRegShort(SISIOADDRESS, u16);
+void SiS_SetRegLong(SISIOADDRESS, u32);
+void SiS_SetRegANDOR(SISIOADDRESS, u8, u8, u8);
+void SiS_SetRegAND(SISIOADDRESS, u8, u8);
+void SiS_SetRegOR(SISIOADDRESS, u8, u8);
+u8 SiS_GetReg(SISIOADDRESS, u8);
+u8 SiS_GetRegByte(SISIOADDRESS);
+u16 SiS_GetRegShort(SISIOADDRESS);
+u32 SiS_GetRegLong(SISIOADDRESS);
 
 /* MMIO access macros */
 #define MMIO_IN8(base, offset)  readb((base+offset))
@@ -535,6 +495,7 @@ struct sis_video_info {
         unsigned int    refresh_rate;
 
         unsigned int    chip;
+        unsigned int    chip_real_id;
         u8              revision_id;
         int             sisvga_enabled;         /* PCI device was enabled */
 
diff --git a/drivers/video/sis/sis_main.c b/drivers/video/sis/sis_main.c
index b52f8e4ef1fd..75259845933d 100644
--- a/drivers/video/sis/sis_main.c
+++ b/drivers/video/sis/sis_main.c
@@ -60,6 +60,11 @@
 #include "sis.h"
 #include "sis_main.h"
 
+#if !defined(CONFIG_FB_SIS_300) && !defined(CONFIG_FB_SIS_315)
+#warning Neither CONFIG_FB_SIS_300 nor CONFIG_FB_SIS_315 is set
+#warning sisfb will not work!
+#endif
+
 static void sisfb_handle_command(struct sis_video_info *ivideo,
                                  struct sisfb_cmd *sisfb_command);
 
@@ -732,7 +737,7 @@ sisfb_bridgeisslave(struct sis_video_info *ivideo)
         if(!(ivideo->vbflags2 & VB2_VIDEOBRIDGE))
                 return false;
 
-        inSISIDXREG(SISPART1,0x00,P1_00);
+        P1_00 = SiS_GetReg(SISPART1, 0x00);
         if( ((ivideo->sisvga_engine == SIS_300_VGA) && (P1_00 & 0xa0) == 0x20) ||
             ((ivideo->sisvga_engine == SIS_315_VGA) && (P1_00 & 0x50) == 0x10) ) {
                 return true;
@@ -746,11 +751,11 @@ sisfballowretracecrt1(struct sis_video_info *ivideo)
 {
         u8 temp;
 
-        inSISIDXREG(SISCR,0x17,temp);
+        temp = SiS_GetReg(SISCR, 0x17);
         if(!(temp & 0x80))
                 return false;
 
-        inSISIDXREG(SISSR,0x1f,temp);
+        temp = SiS_GetReg(SISSR, 0x1f);
         if(temp & 0xc0)
                 return false;
 
@@ -763,7 +768,7 @@ sisfbcheckvretracecrt1(struct sis_video_info *ivideo)
         if(!sisfballowretracecrt1(ivideo))
                 return false;
 
-        if(inSISREG(SISINPSTAT) & 0x08)
+        if (SiS_GetRegByte(SISINPSTAT) & 0x08)
                 return true;
         else
                 return false;
@@ -778,9 +783,9 @@ sisfbwaitretracecrt1(struct sis_video_info *ivideo)
                 return;
 
         watchdog = 65536;
-        while((!(inSISREG(SISINPSTAT) & 0x08)) && --watchdog);
+        while ((!(SiS_GetRegByte(SISINPSTAT) & 0x08)) && --watchdog);
         watchdog = 65536;
-        while((inSISREG(SISINPSTAT) & 0x08) && --watchdog);
+        while ((SiS_GetRegByte(SISINPSTAT) & 0x08) && --watchdog);
 }
 
 static bool
@@ -794,7 +799,7 @@ sisfbcheckvretracecrt2(struct sis_video_info *ivideo)
         default:          return false;
         }
 
-        inSISIDXREG(SISPART1, reg, temp);
+        temp = SiS_GetReg(SISPART1, reg);
         if(temp & 0x02)
                 return true;
         else
@@ -832,10 +837,10 @@ sisfb_setupvbblankflags(struct sis_video_info *ivideo, u32 *vcount, u32 *hcount)
                         default:
                         case SIS_315_VGA: idx = 0x30; break;
                 }
-                inSISIDXREG(SISPART1,(idx+0),reg1); /* 30 */
-                inSISIDXREG(SISPART1,(idx+1),reg2); /* 31 */
-                inSISIDXREG(SISPART1,(idx+2),reg3); /* 32 */
-                inSISIDXREG(SISPART1,(idx+3),reg4); /* 33 */
+                reg1 = SiS_GetReg(SISPART1, (idx+0)); /* 30 */
+                reg2 = SiS_GetReg(SISPART1, (idx+1)); /* 31 */
+                reg3 = SiS_GetReg(SISPART1, (idx+2)); /* 32 */
+                reg4 = SiS_GetReg(SISPART1, (idx+3)); /* 33 */
                 if(reg1 & 0x01) ret |= FB_VBLANK_VBLANKING;
                 if(reg1 & 0x02) ret |= FB_VBLANK_VSYNCING;
                 if(reg4 & 0x80) ret |= FB_VBLANK_HBLANKING;
@@ -848,13 +853,13 @@ sisfb_setupvbblankflags(struct sis_video_info *ivideo, u32 *vcount, u32 *hcount)
                         FB_VBLANK_HAVE_VBLANK |
                         FB_VBLANK_HAVE_VCOUNT |
                         FB_VBLANK_HAVE_HCOUNT);
-                reg1 = inSISREG(SISINPSTAT);
+                reg1 = SiS_GetRegByte(SISINPSTAT);
                 if(reg1 & 0x08) ret |= FB_VBLANK_VSYNCING;
                 if(reg1 & 0x01) ret |= FB_VBLANK_VBLANKING;
-                inSISIDXREG(SISCR,0x20,reg1);
-                inSISIDXREG(SISCR,0x1b,reg1);
-                inSISIDXREG(SISCR,0x1c,reg2);
-                inSISIDXREG(SISCR,0x1d,reg3);
+                reg1 = SiS_GetReg(SISCR, 0x20);
+                reg1 = SiS_GetReg(SISCR, 0x1b);
+                reg2 = SiS_GetReg(SISCR, 0x1c);
+                reg3 = SiS_GetReg(SISCR, 0x1d);
                 (*vcount) = reg2 | ((reg3 & 0x07) << 8);
                 (*hcount) = (reg1 | ((reg3 & 0x10) << 4)) << 3;
         }
@@ -925,12 +930,12 @@ sisfb_myblank(struct sis_video_info *ivideo, int blank)
                      (ivideo->sisfb_thismonitor.feature & 0xe0))) {
 
                         if(ivideo->sisvga_engine == SIS_315_VGA) {
-                                setSISIDXREG(SISCR, ivideo->SiS_Pr.SiS_MyCR63, 0xbf, cr63);
+                                SiS_SetRegANDOR(SISCR, ivideo->SiS_Pr.SiS_MyCR63, 0xbf, cr63);
                         }
 
                         if(!(sisfb_bridgeisslave(ivideo))) {
-                                setSISIDXREG(SISSR, 0x01, ~0x20, sr01);
-                                setSISIDXREG(SISSR, 0x1f, 0x3f, sr1f);
+                                SiS_SetRegANDOR(SISSR, 0x01, ~0x20, sr01);
+                                SiS_SetRegANDOR(SISSR, 0x1f, 0x3f, sr1f);
                         }
                 }
 
@@ -960,25 +965,25 @@ sisfb_myblank(struct sis_video_info *ivideo, int blank)
                     (ivideo->vbflags2 & (VB2_301|VB2_30xBDH|VB2_LVDS))) ||
                    ((ivideo->sisvga_engine == SIS_315_VGA) &&
                     ((ivideo->vbflags2 & (VB2_LVDS | VB2_CHRONTEL)) == VB2_LVDS))) {
-                        setSISIDXREG(SISSR, 0x11, ~0x0c, sr11);
+                        SiS_SetRegANDOR(SISSR, 0x11, ~0x0c, sr11);
                 }
 
                 if(ivideo->sisvga_engine == SIS_300_VGA) {
                         if((ivideo->vbflags2 & VB2_30xB) &&
                            (!(ivideo->vbflags2 & VB2_30xBDH))) {
-                                setSISIDXREG(SISPART1, 0x13, 0x3f, p1_13);
+                                SiS_SetRegANDOR(SISPART1, 0x13, 0x3f, p1_13);
                         }
                 } else if(ivideo->sisvga_engine == SIS_315_VGA) {
                         if((ivideo->vbflags2 & VB2_30xB) &&
                            (!(ivideo->vbflags2 & VB2_30xBDH))) {
-                                setSISIDXREG(SISPART2, 0x00, 0x1f, p2_0);
+                                SiS_SetRegANDOR(SISPART2, 0x00, 0x1f, p2_0);
                         }
                 }
 
         } else if(ivideo->currentvbflags & CRT2_VGA) {
 
                 if(ivideo->vbflags2 & VB2_30xB) {
-                        setSISIDXREG(SISPART2, 0x00, 0x1f, p2_0);
+                        SiS_SetRegANDOR(SISPART2, 0x00, 0x1f, p2_0);
                 }
 
         }
@@ -1109,15 +1114,15 @@ sisfb_set_pitch(struct sis_video_info *ivideo)
 
         /* We need to set pitch for CRT1 if bridge is in slave mode, too */
         if((ivideo->currentvbflags & VB_DISPTYPE_DISP1) || (isslavemode)) {
-                outSISIDXREG(SISCR,0x13,(HDisplay1 & 0xFF));
-                setSISIDXREG(SISSR,0x0E,0xF0,(HDisplay1 >> 8));
+                SiS_SetReg(SISCR, 0x13, (HDisplay1 & 0xFF));
+                SiS_SetRegANDOR(SISSR, 0x0E, 0xF0, (HDisplay1 >> 8));
         }
 
         /* We must not set the pitch for CRT2 if bridge is in slave mode */
         if((ivideo->currentvbflags & VB_DISPTYPE_DISP2) && (!isslavemode)) {
-                orSISIDXREG(SISPART1,ivideo->CRT2_write_enable,0x01);
-                outSISIDXREG(SISPART1,0x07,(HDisplay2 & 0xFF));
-                setSISIDXREG(SISPART1,0x09,0xF0,(HDisplay2 >> 8));
+                SiS_SetRegOR(SISPART1, ivideo->CRT2_write_enable, 0x01);
+                SiS_SetReg(SISPART1, 0x07, (HDisplay2 & 0xFF));
+                SiS_SetRegANDOR(SISPART1, 0x09, 0xF0, (HDisplay2 >> 8));
         }
 }
 
@@ -1162,7 +1167,7 @@ sisfb_set_mode(struct sis_video_info *ivideo, int clrscrn)
         /* >=2.6.12's fbcon clears the screen anyway */
         modeno |= 0x80;
 
-        outSISIDXREG(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
+        SiS_SetReg(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
 
         sisfb_pre_setmode(ivideo);
 
@@ -1171,7 +1176,7 @@ sisfb_set_mode(struct sis_video_info *ivideo, int clrscrn)
                 return -EINVAL;
         }
 
-        outSISIDXREG(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
+        SiS_SetReg(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
 
         sisfb_post_setmode(ivideo);
 
@@ -1303,13 +1308,13 @@ sisfb_do_set_var(struct fb_var_screeninfo *var, int isactive, struct fb_info *in
 static void
 sisfb_set_base_CRT1(struct sis_video_info *ivideo, unsigned int base)
 {
-        outSISIDXREG(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
+        SiS_SetReg(SISSR, IND_SIS_PASSWORD, SIS_PASSWORD);
 
-        outSISIDXREG(SISCR, 0x0D, base & 0xFF);
-        outSISIDXREG(SISCR, 0x0C, (base >> 8) & 0xFF);
-        outSISIDXREG(SISSR, 0x0D, (base >> 16) & 0xFF);
+        SiS_SetReg(SISCR, 0x0D, base & 0xFF);
+        SiS_SetReg(SISCR, 0x0C, (base >> 8) & 0xFF);
+        SiS_SetReg(SISSR, 0x0D, (base >> 16) & 0xFF);
         if(ivideo->sisvga_engine == SIS_315_VGA) {
-                setSISIDXREG(SISSR, 0x37, 0xFE, (base >> 24) & 0x01);
+                SiS_SetRegANDOR(SISSR, 0x37, 0xFE, (base >> 24) & 0x01);
         }
 }
 
@@ -1317,12 +1322,12 @@ static void
 sisfb_set_base_CRT2(struct sis_video_info *ivideo, unsigned int base)
 {
         if(ivideo->currentvbflags & VB_DISPTYPE_DISP2) {
-                orSISIDXREG(SISPART1, ivideo->CRT2_write_enable, 0x01);
-                outSISIDXREG(SISPART1, 0x06, (base & 0xFF));
-                outSISIDXREG(SISPART1, 0x05, ((base >> 8) & 0xFF));
-                outSISIDXREG(SISPART1, 0x04, ((base >> 16) & 0xFF));
+                SiS_SetRegOR(SISPART1, ivideo->CRT2_write_enable, 0x01);
+                SiS_SetReg(SISPART1, 0x06, (base & 0xFF));
+                SiS_SetReg(SISPART1, 0x05, ((base >> 8) & 0xFF));
+                SiS_SetReg(SISPART1, 0x04, ((base >> 16) & 0xFF));
                 if(ivideo->sisvga_engine == SIS_315_VGA) {
-                        setSISIDXREG(SISPART1, 0x02, 0x7F, ((base >> 24) & 0x01) << 7);
+                        SiS_SetRegANDOR(SISPART1, 0x02, 0x7F, ((base >> 24) & 0x01) << 7);
                 }
         }
 }
@@ -1383,15 +1388,15 @@ sisfb_setcolreg(unsigned regno, unsigned red, unsigned green, unsigned blue,
 
         switch(info->var.bits_per_pixel) {
         case 8:
-                outSISREG(SISDACA, regno);
-                outSISREG(SISDACD, (red >> 10));
-                outSISREG(SISDACD, (green >> 10));
-                outSISREG(SISDACD, (blue >> 10));
+                SiS_SetRegByte(SISDACA, regno);
+                SiS_SetRegByte(SISDACD, (red >> 10));
+                SiS_SetRegByte(SISDACD, (green >> 10));
+                SiS_SetRegByte(SISDACD, (blue >> 10));
                 if(ivideo->currentvbflags & VB_DISPTYPE_DISP2) {
-                        outSISREG(SISDAC2A, regno);
-                        outSISREG(SISDAC2D, (red >> 8));
-                        outSISREG(SISDAC2D, (green >> 8));
-                        outSISREG(SISDAC2D, (blue >> 8));
+                        SiS_SetRegByte(SISDAC2A, regno);
+                        SiS_SetRegByte(SISDAC2D, (red >> 8));
+                        SiS_SetRegByte(SISDAC2D, (green >> 8));
+                        SiS_SetRegByte(SISDAC2D, (blue >> 8));
                 }
                 break;
         case 16:
@@ -1956,7 +1961,7 @@ sisfb_get_dram_size(struct sis_video_info *ivideo)
         switch(ivideo->chip) {
 #ifdef CONFIG_FB_SIS_300
         case SIS_300:
-                inSISIDXREG(SISSR, 0x14, reg);
+                reg = SiS_GetReg(SISSR, 0x14);
                 ivideo->video_size = ((reg & 0x3F) + 1) << 20;
                 break;
         case SIS_540:
@@ -1972,7 +1977,7 @@ sisfb_get_dram_size(struct sis_video_info *ivideo)
         case SIS_315H:
         case SIS_315PRO:
         case SIS_315:
-                inSISIDXREG(SISSR, 0x14, reg);
+                reg = SiS_GetReg(SISSR, 0x14);
                 ivideo->video_size = (1 << ((reg & 0xf0) >> 4)) << 20;
                 switch((reg >> 2) & 0x03) {
                 case 0x01:
@@ -1984,31 +1989,31 @@ sisfb_get_dram_size(struct sis_video_info *ivideo)
                 }
                 break;
         case SIS_330:
-                inSISIDXREG(SISSR, 0x14, reg);
+                reg = SiS_GetReg(SISSR, 0x14);
                 ivideo->video_size = (1 << ((reg & 0xf0) >> 4)) << 20;
                 if(reg & 0x0c) ivideo->video_size <<= 1;
                 break;
         case SIS_550:
         case SIS_650:
         case SIS_740:
-                inSISIDXREG(SISSR, 0x14, reg);
+                reg = SiS_GetReg(SISSR, 0x14);
                 ivideo->video_size = (((reg & 0x3f) + 1) << 2) << 20;
                 break;
         case SIS_661:
         case SIS_741:
-                inSISIDXREG(SISCR, 0x79, reg);
+                reg = SiS_GetReg(SISCR, 0x79);
                 ivideo->video_size = (1 << ((reg & 0xf0) >> 4)) << 20;
                 break;
         case SIS_660:
         case SIS_760:
         case SIS_761:
-                inSISIDXREG(SISCR, 0x79, reg);
+                reg = SiS_GetReg(SISCR, 0x79);
                 reg = (reg & 0xf0) >> 4;
                 if(reg) {
                         ivideo->video_size = (1 << reg) << 20;
                         ivideo->UMAsize = ivideo->video_size;
                 }
-                inSISIDXREG(SISCR, 0x78, reg);
+                reg = SiS_GetReg(SISCR, 0x78);
                 reg &= 0x30;
                 if(reg) {
                         if(reg == 0x10) {
@@ -2022,7 +2027,7 @@ sisfb_get_dram_size(struct sis_video_info *ivideo)
         case SIS_340:
         case XGI_20:
         case XGI_40:
-                inSISIDXREG(SISSR, 0x14, reg);
+                reg = SiS_GetReg(SISSR, 0x14);
                 ivideo->video_size = (1 << ((reg & 0xf0) >> 4)) << 20;
                 if(ivideo->chip != XGI_20) {
                         reg = (reg & 0x0c) >> 2;
@@ -2056,11 +2061,11 @@ sisfb_detect_VB_connect(struct sis_video_info *ivideo)
 
 #ifdef CONFIG_FB_SIS_300
         if(ivideo->sisvga_engine == SIS_300_VGA) {
-                inSISIDXREG(SISSR, 0x17, temp);
+                temp = SiS_GetReg(SISSR, 0x17);
                 if((temp & 0x0F) && (ivideo->chip != SIS_300)) {
                         /* PAL/NTSC is stored on SR16 on such machines */
                         if(!(ivideo->vbflags & (TV_PAL | TV_NTSC | TV_PALM | TV_PALN))) {
-                                inSISIDXREG(SISSR, 0x16, temp);
+                                temp = SiS_GetReg(SISSR, 0x16);
                                 if(temp & 0x20)
                                         ivideo->vbflags |= TV_PAL;
                                 else
@@ -2070,7 +2075,7 @@ sisfb_detect_VB_connect(struct sis_video_info *ivideo)
         }
 #endif
 
-        inSISIDXREG(SISCR, 0x32, cr32);
+        cr32 = SiS_GetReg(SISCR, 0x32);
 
         if(cr32 & SIS_CRT1) {
                 ivideo->sisfb_crt1off = 0;
@@ -2146,15 +2151,15 @@ sisfb_detect_VB_connect(struct sis_video_info *ivideo)
             }
             if(!(ivideo->vbflags & (TV_PAL | TV_NTSC | TV_PALM | TV_PALN | TV_NTSCJ))) {
                 if(ivideo->sisvga_engine == SIS_300_VGA) {
-                        inSISIDXREG(SISSR, 0x38, temp);
+                        temp = SiS_GetReg(SISSR, 0x38);
                         if(temp & 0x01) ivideo->vbflags |= TV_PAL;
                         else            ivideo->vbflags |= TV_NTSC;
                 } else if((ivideo->chip <= SIS_315PRO) || (ivideo->chip >= SIS_330)) {
-                        inSISIDXREG(SISSR, 0x38, temp);
+                        temp = SiS_GetReg(SISSR, 0x38);
                         if(temp & 0x01) ivideo->vbflags |= TV_PAL;
                         else            ivideo->vbflags |= TV_NTSC;
                 } else {
-                        inSISIDXREG(SISCR, 0x79, temp);
+                        temp = SiS_GetReg(SISCR, 0x79);
                         if(temp & 0x20) ivideo->vbflags |= TV_PAL;
                         else            ivideo->vbflags |= TV_NTSC;
                 }
@@ -2193,26 +2198,26 @@ sisfb_sense_crt1(struct sis_video_info *ivideo)
     u16 temp = 0xffff;
     int i;
 
-    inSISIDXREG(SISSR,0x1F,sr1F);
-    orSISIDXREG(SISSR,0x1F,0x04);
-    andSISIDXREG(SISSR,0x1F,0x3F);
+    sr1F = SiS_GetReg(SISSR, 0x1F);
+    SiS_SetRegOR(SISSR, 0x1F, 0x04);
+    SiS_SetRegAND(SISSR, 0x1F, 0x3F);
     if(sr1F & 0xc0) mustwait = true;
 
 #ifdef CONFIG_FB_SIS_315
     if(ivideo->sisvga_engine == SIS_315_VGA) {
-       inSISIDXREG(SISCR,ivideo->SiS_Pr.SiS_MyCR63,cr63);
+       cr63 = SiS_GetReg(SISCR, ivideo->SiS_Pr.SiS_MyCR63);
        cr63 &= 0x40;
-       andSISIDXREG(SISCR,ivideo->SiS_Pr.SiS_MyCR63,0xBF);
+       SiS_SetRegAND(SISCR, ivideo->SiS_Pr.SiS_MyCR63, 0xBF);
     }
 #endif
 
-    inSISIDXREG(SISCR,0x17,cr17);
+    cr17 = SiS_GetReg(SISCR, 0x17);
     cr17 &= 0x80;
     if(!cr17) {
-       orSISIDXREG(SISCR,0x17,0x80);
+       SiS_SetRegOR(SISCR, 0x17, 0x80);
        mustwait = true;
-       outSISIDXREG(SISSR, 0x00, 0x01);
-       outSISIDXREG(SISSR, 0x00, 0x03);
+       SiS_SetReg(SISSR, 0x00, 0x01);
+       SiS_SetReg(SISSR, 0x00, 0x03);
     }
 
     if(mustwait) {
@@ -2221,18 +2226,18 @@ sisfb_sense_crt1(struct sis_video_info *ivideo)
 
 #ifdef CONFIG_FB_SIS_315
     if(ivideo->chip >= SIS_330) {
-       andSISIDXREG(SISCR,0x32,~0x20);
+       SiS_SetRegAND(SISCR, 0x32, ~0x20);
        if(ivideo->chip >= SIS_340) {
-          outSISIDXREG(SISCR, 0x57, 0x4a);
+           SiS_SetReg(SISCR, 0x57, 0x4a);
        } else {
-          outSISIDXREG(SISCR, 0x57, 0x5f);
+           SiS_SetReg(SISCR, 0x57, 0x5f);
        }
-       orSISIDXREG(SISCR, 0x53, 0x02);
-       while((inSISREG(SISINPSTAT)) & 0x01)    break;
-       while(!((inSISREG(SISINPSTAT)) & 0x01)) break;
-       if((inSISREG(SISMISCW)) & 0x10) temp = 1;
-       andSISIDXREG(SISCR, 0x53, 0xfd);
-       andSISIDXREG(SISCR, 0x57, 0x00);
+        SiS_SetRegOR(SISCR, 0x53, 0x02);
+        while ((SiS_GetRegByte(SISINPSTAT)) & 0x01)    break;
+        while (!((SiS_GetRegByte(SISINPSTAT)) & 0x01)) break;
+        if ((SiS_GetRegByte(SISMISCW)) & 0x10) temp = 1;
+        SiS_SetRegAND(SISCR, 0x53, 0xfd);
+        SiS_SetRegAND(SISCR, 0x57, 0x00);
     }
 #endif
 
@@ -2249,18 +2254,18 @@ sisfb_sense_crt1(struct sis_video_info *ivideo)
     }
 
     if((temp) && (temp != 0xffff)) {
-       orSISIDXREG(SISCR,0x32,0x20);
+       SiS_SetRegOR(SISCR, 0x32, 0x20);
     }
 
 #ifdef CONFIG_FB_SIS_315
     if(ivideo->sisvga_engine == SIS_315_VGA) {
-       setSISIDXREG(SISCR,ivideo->SiS_Pr.SiS_MyCR63,0xBF,cr63);
+        SiS_SetRegANDOR(SISCR, ivideo->SiS_Pr.SiS_MyCR63, 0xBF, cr63);
     }
 #endif
 
-    setSISIDXREG(SISCR,0x17,0x7F,cr17);
+    SiS_SetRegANDOR(SISCR, 0x17, 0x7F, cr17);
 
-    outSISIDXREG(SISSR,0x1F,sr1F);
+    SiS_SetReg(SISSR, 0x1F, sr1F);
 }
 
 /* Determine and detect attached devices on SiS30x */
@@ -2281,7 +2286,7 @@ SiS_SenseLCD(struct sis_video_info *ivideo)
                 return;
 
         /* If LCD already set up by BIOS, skip it */
-        inSISIDXREG(SISCR, 0x32, reg);
+        reg = SiS_GetReg(SISCR, 0x32);
         if(reg & 0x08)
                 return;
 
@@ -2344,10 +2349,10 @@ SiS_SenseLCD(struct sis_video_info *ivideo)
         else
                 cr37 |= 0xc0;
 
-        outSISIDXREG(SISCR, 0x36, paneltype);
+        SiS_SetReg(SISCR, 0x36, paneltype);
         cr37 &= 0xf1;
-        setSISIDXREG(SISCR, 0x37, 0x0c, cr37);
-        orSISIDXREG(SISCR, 0x32, 0x08);
+        SiS_SetRegANDOR(SISCR, 0x37, 0x0c, cr37);
+        SiS_SetRegOR(SISCR, 0x32, 0x08);
 
         ivideo->SiS_Pr.PanelSelfDetected = true;
 }
@@ -2361,19 +2366,19 @@ SISDoSense(struct sis_video_info *ivideo, u16 type, u16 test)
        result = 0;
        for(i = 0; i < 3; i++) {
           mytest = test;
-          outSISIDXREG(SISPART4,0x11,(type & 0x00ff));
+           SiS_SetReg(SISPART4, 0x11, (type & 0x00ff));
           temp = (type >> 8) | (mytest & 0x00ff);
-          setSISIDXREG(SISPART4,0x10,0xe0,temp);
+          SiS_SetRegANDOR(SISPART4, 0x10, 0xe0, temp);
           SiS_DDC2Delay(&ivideo->SiS_Pr, 0x1500);
           mytest >>= 8;
           mytest &= 0x7f;
-          inSISIDXREG(SISPART4,0x03,temp);
+           temp = SiS_GetReg(SISPART4, 0x03);
           temp ^= 0x0e;
           temp &= mytest;
           if(temp == mytest) result++;
 #if 1
-          outSISIDXREG(SISPART4,0x11,0x00);
-          andSISIDXREG(SISPART4,0x10,0xe0);
+          SiS_SetReg(SISPART4, 0x11, 0x00);
+          SiS_SetRegAND(SISPART4, 0x10, 0xe0);
           SiS_DDC2Delay(&ivideo->SiS_Pr, 0x1000);
 #endif
        }
@@ -2395,7 +2400,7 @@ SiS_Sense30x(struct sis_video_info *ivideo)
 
     if(ivideo->vbflags2 & VB2_301) {
        svhs = 0x00b9; cvbs = 0x00b3; vga2 = 0x00d1;
-       inSISIDXREG(SISPART4,0x01,myflag);
+       myflag = SiS_GetReg(SISPART4, 0x01);
        if(myflag & 0x04) {
           svhs = 0x00dd; cvbs = 0x00ee; vga2 = 0x00fd;
        }
@@ -2425,7 +2430,7 @@ SiS_Sense30x(struct sis_video_info *ivideo)
     }
 
     if(ivideo->chip == SIS_300) {
-       inSISIDXREG(SISSR,0x3b,myflag);
+       myflag = SiS_GetReg(SISSR, 0x3b);
        if(!(myflag & 0x01)) vga2 = vga2_c = 0;
     }
 
@@ -2433,93 +2438,93 @@ SiS_Sense30x(struct sis_video_info *ivideo)
        vga2 = vga2_c = 0;
     }
 
-    inSISIDXREG(SISSR,0x1e,backupSR_1e);
-    orSISIDXREG(SISSR,0x1e,0x20);
+    backupSR_1e = SiS_GetReg(SISSR, 0x1e);
+    SiS_SetRegOR(SISSR, 0x1e, 0x20);
 
-    inSISIDXREG(SISPART4,0x0d,backupP4_0d);
+    backupP4_0d = SiS_GetReg(SISPART4, 0x0d);
     if(ivideo->vbflags2 & VB2_30xC) {
-       setSISIDXREG(SISPART4,0x0d,~0x07,0x01);
+        SiS_SetRegANDOR(SISPART4, 0x0d, ~0x07, 0x01);
     } else {
-       orSISIDXREG(SISPART4,0x0d,0x04);
+       SiS_SetRegOR(SISPART4, 0x0d, 0x04);
     }
     SiS_DDC2Delay(&ivideo->SiS_Pr, 0x2000);
 
-    inSISIDXREG(SISPART2,0x00,backupP2_00);
-    outSISIDXREG(SISPART2,0x00,((backupP2_00 | 0x1c) & 0xfc));
+    backupP2_00 = SiS_GetReg(SISPART2, 0x00);
+    SiS_SetReg(SISPART2, 0x00, ((backupP2_00 | 0x1c) & 0xfc));
 
-    inSISIDXREG(SISPART2,0x4d,backupP2_4d);
+    backupP2_4d = SiS_GetReg(SISPART2, 0x4d);
     if(ivideo->vbflags2 & VB2_SISYPBPRBRIDGE) {
-       outSISIDXREG(SISPART2,0x4d,(backupP2_4d & ~0x10));
+        SiS_SetReg(SISPART2, 0x4d, (backupP2_4d & ~0x10));
     }
 
     if(!(ivideo->vbflags2 & VB2_30xCLV)) {
        SISDoSense(ivideo, 0, 0);
     }
 
-    andSISIDXREG(SISCR, 0x32, ~0x14);
+    SiS_SetRegAND(SISCR, 0x32, ~0x14);
 
     if(vga2_c || vga2) {
        if(SISDoSense(ivideo, vga2, vga2_c)) {
           if(biosflag & 0x01) {
              printk(KERN_INFO "%s %s SCART output\n", stdstr, tvstr);
-             orSISIDXREG(SISCR, 0x32, 0x04);
+             SiS_SetRegOR(SISCR, 0x32, 0x04);
           } else {
              printk(KERN_INFO "%s secondary VGA connection\n", stdstr);
-             orSISIDXREG(SISCR, 0x32, 0x10);
+             SiS_SetRegOR(SISCR, 0x32, 0x10);
           }
        }
     }
 
-    andSISIDXREG(SISCR, 0x32, 0x3f);
+    SiS_SetRegAND(SISCR, 0x32, 0x3f);
 
     if(ivideo->vbflags2 & VB2_30xCLV) {
-       orSISIDXREG(SISPART4,0x0d,0x04);
+       SiS_SetRegOR(SISPART4, 0x0d, 0x04);
     }
 
     if((ivideo->sisvga_engine == SIS_315_VGA) && (ivideo->vbflags2 & VB2_SISYPBPRBRIDGE)) {
-       outSISIDXREG(SISPART2,0x4d,(backupP2_4d | 0x10));
+       SiS_SetReg(SISPART2, 0x4d, (backupP2_4d | 0x10));
        SiS_DDC2Delay(&ivideo->SiS_Pr, 0x2000);
        if((result = SISDoSense(ivideo, svhs, 0x0604))) {
           if((result = SISDoSense(ivideo, cvbs, 0x0804))) {
              printk(KERN_INFO "%s %s YPbPr component output\n", stdstr, tvstr);
-             orSISIDXREG(SISCR,0x32,0x80);
+             SiS_SetRegOR(SISCR, 0x32, 0x80);
           }
        }
-       outSISIDXREG(SISPART2,0x4d,backupP2_4d);
+       SiS_SetReg(SISPART2, 0x4d, backupP2_4d);
     }
 
-    andSISIDXREG(SISCR, 0x32, ~0x03);
+    SiS_SetRegAND(SISCR, 0x32, ~0x03);
 
     if(!(ivideo->vbflags & TV_YPBPR)) {
        if((result = SISDoSense(ivideo, svhs, svhs_c))) {
           printk(KERN_INFO "%s %s SVIDEO output\n", stdstr, tvstr);
-          orSISIDXREG(SISCR, 0x32, 0x02);
+           SiS_SetRegOR(SISCR, 0x32, 0x02);
        }
        if((biosflag & 0x02) || (!result)) {
           if(SISDoSense(ivideo, cvbs, cvbs_c)) {
              printk(KERN_INFO "%s %s COMPOSITE output\n", stdstr, tvstr);
-             orSISIDXREG(SISCR, 0x32, 0x01);
+             SiS_SetRegOR(SISCR, 0x32, 0x01);
           }
        }
     }
 
     SISDoSense(ivideo, 0, 0);
 
-    outSISIDXREG(SISPART2,0x00,backupP2_00);
-    outSISIDXREG(SISPART4,0x0d,backupP4_0d);
-    outSISIDXREG(SISSR,0x1e,backupSR_1e);
+    SiS_SetReg(SISPART2, 0x00, backupP2_00);
+    SiS_SetReg(SISPART4, 0x0d, backupP4_0d);
+    SiS_SetReg(SISSR, 0x1e, backupSR_1e);
 
     if(ivideo->vbflags2 & VB2_30xCLV) {
-       inSISIDXREG(SISPART2,0x00,biosflag);
+        biosflag = SiS_GetReg(SISPART2, 0x00);
        if(biosflag & 0x20) {
           for(myflag = 2; myflag > 0; myflag--) {
              biosflag ^= 0x20;
-             outSISIDXREG(SISPART2,0x00,biosflag);
+             SiS_SetReg(SISPART2, 0x00, biosflag);
           }
        }
     }
 
-    outSISIDXREG(SISPART2,0x00,backupP2_00);
+    SiS_SetReg(SISPART2, 0x00, backupP2_00);
 }
 
 /* Determine and detect attached TV's on Chrontel */
@@ -2583,20 +2588,20 @@ SiS_SenseCh(struct sis_video_info *ivideo)
            if(temp1 == 0x02) {
                 printk(KERN_INFO "%s SVIDEO output\n", stdstr);
                 ivideo->vbflags |= TV_SVIDEO;
-                orSISIDXREG(SISCR, 0x32, 0x02);
-                andSISIDXREG(SISCR, 0x32, ~0x05);
+                SiS_SetRegOR(SISCR, 0x32, 0x02);
+                SiS_SetRegAND(SISCR, 0x32, ~0x05);
            } else if (temp1 == 0x01) {
                 printk(KERN_INFO "%s CVBS output\n", stdstr);
                 ivideo->vbflags |= TV_AVIDEO;
-                orSISIDXREG(SISCR, 0x32, 0x01);
-                andSISIDXREG(SISCR, 0x32, ~0x06);
+                SiS_SetRegOR(SISCR, 0x32, 0x01);
+                SiS_SetRegAND(SISCR, 0x32, ~0x06);
            } else {
                 SiS_SetCH70xxANDOR(&ivideo->SiS_Pr, 0x0e, 0x01, 0xF8);
-                andSISIDXREG(SISCR, 0x32, ~0x07);
+                SiS_SetRegAND(SISCR, 0x32, ~0x07);
            }
        } else if(temp1 == 0) {
           SiS_SetCH70xxANDOR(&ivideo->SiS_Pr, 0x0e, 0x01, 0xF8);
-          andSISIDXREG(SISCR, 0x32, ~0x07);
+          SiS_SetRegAND(SISCR, 0x32, ~0x07);
        }
        /* Set general purpose IO for Chrontel communication */
        SiS_SetChrontelGPIO(&ivideo->SiS_Pr, 0x00);
@@ -2627,22 +2632,22 @@ SiS_SenseCh(struct sis_video_info *ivideo)
         case 0x01:
              printk(KERN_INFO "%s CVBS output\n", stdstr);
              ivideo->vbflags |= TV_AVIDEO;
-             orSISIDXREG(SISCR, 0x32, 0x01);
-             andSISIDXREG(SISCR, 0x32, ~0x06);
+             SiS_SetRegOR(SISCR, 0x32, 0x01);
+             SiS_SetRegAND(SISCR, 0x32, ~0x06);
              break;
         case 0x02:
              printk(KERN_INFO "%s SVIDEO output\n", stdstr);
              ivideo->vbflags |= TV_SVIDEO;
-             orSISIDXREG(SISCR, 0x32, 0x02);
-             andSISIDXREG(SISCR, 0x32, ~0x05);
+             SiS_SetRegOR(SISCR, 0x32, 0x02);
+             SiS_SetRegAND(SISCR, 0x32, ~0x05);
              break;
         case 0x04:
              printk(KERN_INFO "%s SCART output\n", stdstr);
-             orSISIDXREG(SISCR, 0x32, 0x04);
-             andSISIDXREG(SISCR, 0x32, ~0x03);
+             SiS_SetRegOR(SISCR, 0x32, 0x04);
+             SiS_SetRegAND(SISCR, 0x32, ~0x03);
              break;
         default:
-             andSISIDXREG(SISCR, 0x32, ~0x07);
+             SiS_SetRegAND(SISCR, 0x32, ~0x07);
         }
 #endif
     }
@@ -2660,10 +2665,10 @@ sisfb_get_VB_type(struct sis_video_info *ivideo)
         if(ivideo->chip == XGI_20)
                 return;
 
-        inSISIDXREG(SISPART4, 0x00, vb_chipid);
+        vb_chipid = SiS_GetReg(SISPART4, 0x00);
         switch(vb_chipid) {
         case 0x01:
-                inSISIDXREG(SISPART4, 0x01, reg);
+                reg = SiS_GetReg(SISPART4, 0x01);
                 if(reg < 0xb0) {
                         ivideo->vbflags |= VB_301;      /* Deprecated */
                         ivideo->vbflags2 |= VB2_301;
@@ -2671,7 +2676,7 @@ sisfb_get_VB_type(struct sis_video_info *ivideo)
                 } else if(reg < 0xc0) {
                         ivideo->vbflags |= VB_301B;     /* Deprecated */
                         ivideo->vbflags2 |= VB2_301B;
-                        inSISIDXREG(SISPART4,0x23,reg);
+                        reg = SiS_GetReg(SISPART4, 0x23);
                         if(!(reg & 0x02)) {
                            ivideo->vbflags |= VB_30xBDH;        /* Deprecated */
                            ivideo->vbflags2 |= VB2_30xBDH;
@@ -2688,7 +2693,7 @@ sisfb_get_VB_type(struct sis_video_info *ivideo)
                         ivideo->vbflags2 |= VB2_301LV;
                         printk(KERN_INFO "%s SiS301LV %s\n", stdstr, bridgestr);
                 } else if(reg <= 0xe1) {
-                        inSISIDXREG(SISPART4,0x39,reg);
+                        reg = SiS_GetReg(SISPART4, 0x39);
                         if(reg == 0xff) {
                            ivideo->vbflags |= VB_302LV; /* Deprecated */
                            ivideo->vbflags2 |= VB2_302LV;
@@ -2713,7 +2718,7 @@ sisfb_get_VB_type(struct sis_video_info *ivideo)
         }
 
         if((!(ivideo->vbflags2 & VB2_VIDEOBRIDGE)) && (ivideo->chip != SIS_300)) {
-                inSISIDXREG(SISCR, 0x37, reg);
+                reg = SiS_GetReg(SISCR, 0x37);
                 reg &= SIS_EXTERNAL_CHIP_MASK;
                 reg >>= 1;
                 if(ivideo->sisvga_engine == SIS_300_VGA) {
@@ -2754,7 +2759,7 @@ sisfb_get_VB_type(struct sis_video_info *ivideo)
 #endif
                 } else if(ivideo->chip >= SIS_661) {
 #ifdef CONFIG_FB_SIS_315
-                        inSISIDXREG(SISCR, 0x38, reg);
+                        reg = SiS_GetReg(SISCR, 0x38);
                         reg >>= 5;
                         switch(reg) {
                            case 0x02:
@@ -2817,13 +2822,13 @@ sisfb_engine_init(struct sis_video_info *ivideo)
 
                 tqueue_pos = (ivideo->video_size - ivideo->cmdQueueSize) / (64 * 1024);
 
-                inSISIDXREG(SISSR, IND_SIS_TURBOQUEUE_SET, tq_state);
+                tq_state = SiS_GetReg(SISSR, IND_SIS_TURBOQUEUE_SET);
                 tq_state |= 0xf0;
                 tq_state &= 0xfc;
                 tq_state |= (u8)(tqueue_pos >> 8);
-                outSISIDXREG(SISSR, IND_SIS_TURBOQUEUE_SET, tq_state);
+                SiS_SetReg(SISSR, IND_SIS_TURBOQUEUE_SET, tq_state);
 
-                outSISIDXREG(SISSR, IND_SIS_TURBOQUEUE_ADR, (u8)(tqueue_pos & 0xff));
+                SiS_SetReg(SISSR, IND_SIS_TURBOQUEUE_ADR, (u8)(tqueue_pos & 0xff));
 
                 ivideo->caps |= TURBO_QUEUE_CAP;
         }
@@ -2860,8 +2865,8 @@ sisfb_engine_init(struct sis_video_info *ivideo)
                         }
                 }
 
-                outSISIDXREG(SISSR, IND_SIS_CMDQUEUE_THRESHOLD, COMMAND_QUEUE_THRESHOLD);
-                outSISIDXREG(SISSR, IND_SIS_CMDQUEUE_SET, SIS_CMD_QUEUE_RESET);
+                SiS_SetReg(SISSR, IND_SIS_CMDQUEUE_THRESHOLD, COMMAND_QUEUE_THRESHOLD);
+                SiS_SetReg(SISSR, IND_SIS_CMDQUEUE_SET, SIS_CMD_QUEUE_RESET);
 
                 if((ivideo->chip >= XGI_40) && ivideo->modechanged) {
                         /* Must disable dual pipe on XGI_40. Can't do
@@ -2873,7 +2878,7 @@ sisfb_engine_init(struct sis_video_info *ivideo)
 
                                 MMIO_OUT32(ivideo->mmio_vbase, Q_WRITE_PTR, 0);
 
-                                outSISIDXREG(SISSR, IND_SIS_CMDQUEUE_SET, (temp | SIS_VRAM_CMDQUEUE_ENABLE));
+                                SiS_SetReg(SISSR, IND_SIS_CMDQUEUE_SET, (temp | SIS_VRAM_CMDQUEUE_ENABLE));
 
                                 tempq = MMIO_IN32(ivideo->mmio_vbase, Q_READ_PTR);
                                 MMIO_OUT32(ivideo->mmio_vbase, Q_WRITE_PTR, tempq);
@@ -2890,7 +2895,7 @@ sisfb_engine_init(struct sis_video_info *ivideo)
 
                                 sisfb_syncaccel(ivideo);
 
-                                outSISIDXREG(SISSR, IND_SIS_CMDQUEUE_SET, SIS_CMD_QUEUE_RESET);
+                                SiS_SetReg(SISSR, IND_SIS_CMDQUEUE_SET, SIS_CMD_QUEUE_RESET);
 
                         }
                 }
@@ -2899,7 +2904,7 @@ sisfb_engine_init(struct sis_video_info *ivideo)
                 MMIO_OUT32(ivideo->mmio_vbase, MMIO_QUEUE_WRITEPORT, tempq);
 
                 temp |= (SIS_MMIO_CMD_ENABLE | SIS_CMD_AUTO_CORR);
-                outSISIDXREG(SISSR, IND_SIS_CMDQUEUE_SET, temp);
+                SiS_SetReg(SISSR, IND_SIS_CMDQUEUE_SET, temp);
 
                 tempq = (u32)(ivideo->video_size - ivideo->cmdQueueSize);
                 MMIO_OUT32(ivideo->mmio_vbase, MMIO_QUEUE_PHYBASE, tempq);
@@ -2917,7 +2922,7 @@ sisfb_detect_lcd_type(struct sis_video_info *ivideo)
         u8 reg;
         int i;
 
-        inSISIDXREG(SISCR, 0x36, reg);
+        reg = SiS_GetReg(SISCR, 0x36);
         reg &= 0x0f;
         if(ivideo->sisvga_engine == SIS_300_VGA) {
                 ivideo->CRT2LCDType = sis300paneltype[reg];
@@ -2936,8 +2941,8 @@ sisfb_detect_lcd_type(struct sis_video_info *ivideo)
         if(ivideo->CRT2LCDType == LCD_UNKNOWN) {
                 /* For broken BIOSes: Assume 1024x768, RGB18 */
                 ivideo->CRT2LCDType = LCD_1024x768;
-                setSISIDXREG(SISCR,0x36,0xf0,0x02);
-                setSISIDXREG(SISCR,0x37,0xee,0x01);
+                SiS_SetRegANDOR(SISCR, 0x36, 0xf0, 0x02);
+                SiS_SetRegANDOR(SISCR, 0x37, 0xee, 0x01);
                 printk(KERN_DEBUG "sisfb: Invalid panel ID (%02x), assuming 1024x768, RGB18\n", reg);
         }
 
@@ -2975,10 +2980,10 @@ sisfb_save_pdc_emi(struct sis_video_info *ivideo)
         if(ivideo->sisvga_engine == SIS_300_VGA) {
                 if(ivideo->vbflags2 & (VB2_LVDS | VB2_30xBDH)) {
                         int tmp;
-                        inSISIDXREG(SISCR,0x30,tmp);
+                        tmp = SiS_GetReg(SISCR, 0x30);
                         if(tmp & 0x20) {
                                 /* Currently on LCD? If yes, read current pdc */
-                                inSISIDXREG(SISPART1,0x13,ivideo->detectedpdc);
+                                ivideo->detectedpdc = SiS_GetReg(SISPART1, 0x13);
                                 ivideo->detectedpdc &= 0x3c;
                                 if(ivideo->SiS_Pr.PDC == -1) {
                                         /* Let option override detection */
@@ -3002,7 +3007,7 @@ sisfb_save_pdc_emi(struct sis_video_info *ivideo)
                 /* Try to find about LCDA */
                 if(ivideo->vbflags2 & VB2_SISLCDABRIDGE) {
                         int tmp;
-                        inSISIDXREG(SISPART1,0x13,tmp);
+                        tmp = SiS_GetReg(SISPART1, 0x13);
                         if(tmp & 0x04) {
                                 ivideo->SiS_Pr.SiS_UseLCDA = true;
                                 ivideo->detectedlcda = 0x03;
@@ -3012,16 +3017,16 @@ sisfb_save_pdc_emi(struct sis_video_info *ivideo)
                 /* Save PDC */
                 if(ivideo->vbflags2 & VB2_SISLVDSBRIDGE) {
                         int tmp;
-                        inSISIDXREG(SISCR,0x30,tmp);
+                        tmp = SiS_GetReg(SISCR, 0x30);
                         if((tmp & 0x20) || (ivideo->detectedlcda != 0xff)) {
                                 /* Currently on LCD? If yes, read current pdc */
                                 u8 pdc;
-                                inSISIDXREG(SISPART1,0x2D,pdc);
+                                pdc = SiS_GetReg(SISPART1, 0x2D);
                                 ivideo->detectedpdc  = (pdc & 0x0f) << 1;
                                 ivideo->detectedpdca = (pdc & 0xf0) >> 3;
-                                inSISIDXREG(SISPART1,0x35,pdc);
+                                pdc = SiS_GetReg(SISPART1, 0x35);
                                 ivideo->detectedpdc |= ((pdc >> 7) & 0x01);
-                                inSISIDXREG(SISPART1,0x20,pdc);
+                                pdc = SiS_GetReg(SISPART1, 0x20);
                                 ivideo->detectedpdca |= ((pdc >> 6) & 0x01);
                                 if(ivideo->newrom) {
                                         /* New ROM invalidates other PDC resp. */
@@ -3055,10 +3060,10 @@ sisfb_save_pdc_emi(struct sis_video_info *ivideo)
 
                         /* Save EMI */
                         if(ivideo->vbflags2 & VB2_SISEMIBRIDGE) {
-                                inSISIDXREG(SISPART4,0x30,ivideo->SiS_Pr.EMI_30);
-                                inSISIDXREG(SISPART4,0x31,ivideo->SiS_Pr.EMI_31);
-                                inSISIDXREG(SISPART4,0x32,ivideo->SiS_Pr.EMI_32);
-                                inSISIDXREG(SISPART4,0x33,ivideo->SiS_Pr.EMI_33);
+                                ivideo->SiS_Pr.EMI_30 = SiS_GetReg(SISPART4, 0x30);
+                                ivideo->SiS_Pr.EMI_31 = SiS_GetReg(SISPART4, 0x31);
+                                ivideo->SiS_Pr.EMI_32 = SiS_GetReg(SISPART4, 0x32);
+                                ivideo->SiS_Pr.EMI_33 = SiS_GetReg(SISPART4, 0x33);
                                 ivideo->SiS_Pr.HaveEMI = true;
                                 if((tmp & 0x20) || (ivideo->detectedlcda != 0xff)) {
                                         ivideo->SiS_Pr.HaveEMILCD = true;
@@ -3483,8 +3488,8 @@ sisfb_check_engine_and_sync(struct sis_video_info *ivideo)
          * ivideo->accel here, as this might have
          * been changed before this is called.
          */
-        inSISIDXREG(SISSR, IND_SIS_PCI_ADDRESS_SET, cr30);
-        inSISIDXREG(SISSR, IND_SIS_MODULE_ENABLE, cr31);
+        cr30 = SiS_GetReg(SISSR, IND_SIS_PCI_ADDRESS_SET);
+        cr31 = SiS_GetReg(SISSR, IND_SIS_MODULE_ENABLE);
         /* MMIO and 2D/3D engine enabled? */
         if((cr30 & SIS_MEM_MAP_IO_ENABLE) && (cr31 & 0x42)) {
 #ifdef CONFIG_FB_SIS_300
@@ -3502,7 +3507,7 @@ sisfb_check_engine_and_sync(struct sis_video_info *ivideo)
                          * enabled, and that the queue
                          * is not in the state of "reset"
                          */
-                        inSISIDXREG(SISSR, 0x26, cr30);
+                        cr30 = SiS_GetReg(SISSR, 0x26);
                         if((cr30 & 0xe0) && (!(cr30 & 0x01))) {
                                 sisfb_syncaccel(ivideo);
                         }
@@ -3519,9 +3524,9 @@ sisfb_pre_setmode(struct sis_video_info *ivideo)
 
         ivideo->currentvbflags &= (VB_VIDEOBRIDGE | VB_DISPTYPE_DISP2);
 
-        outSISIDXREG(SISSR, 0x05, 0x86);
+        SiS_SetReg(SISSR, 0x05, 0x86);
 
-        inSISIDXREG(SISCR, 0x31, cr31);
+        cr31 = SiS_GetReg(SISCR, 0x31);
         cr31 &= ~0x60;
         cr31 |= 0x04;
 
@@ -3530,11 +3535,11 @@ sisfb_pre_setmode(struct sis_video_info *ivideo)
 #ifdef CONFIG_FB_SIS_315
         if(ivideo->sisvga_engine == SIS_315_VGA) {
            if(ivideo->chip >= SIS_661) {
-              inSISIDXREG(SISCR, 0x38, cr38);
+              cr38 = SiS_GetReg(SISCR, 0x38);
               cr38 &= ~0x07;  /* Clear LCDA/DualEdge and YPbPr bits */
            } else {
               tvregnum = 0x38;
-              inSISIDXREG(SISCR, tvregnum, cr38);
+              cr38 = SiS_GetReg(SISCR, tvregnum);
               cr38 &= ~0x3b;  /* Clear LCDA/DualEdge and YPbPr bits */
            }
         }
@@ -3542,7 +3547,7 @@ sisfb_pre_setmode(struct sis_video_info *ivideo)
 #ifdef CONFIG_FB_SIS_300
         if(ivideo->sisvga_engine == SIS_300_VGA) {
            tvregnum = 0x35;
-           inSISIDXREG(SISCR, tvregnum, cr38);
+           cr38 = SiS_GetReg(SISCR, tvregnum);
         }
 #endif
 
@@ -3649,20 +3654,20 @@ sisfb_pre_setmode(struct sis_video_info *ivideo)
               cr31 |= (SIS_DRIVER_MODE | SIS_VB_OUTPUT_DISABLE);
         }
 
-        outSISIDXREG(SISCR, 0x30, cr30);
-        outSISIDXREG(SISCR, 0x33, cr33);
+        SiS_SetReg(SISCR, 0x30, cr30);
+        SiS_SetReg(SISCR, 0x33, cr33);
 
         if(ivideo->chip >= SIS_661) {
 #ifdef CONFIG_FB_SIS_315
            cr31 &= ~0x01;                          /* Clear PAL flag (now in CR35) */
-           setSISIDXREG(SISCR, 0x35, ~0x10, cr35); /* Leave overscan bit alone */
+           SiS_SetRegANDOR(SISCR, 0x35, ~0x10, cr35); /* Leave overscan bit alone */
            cr38 &= 0x07;                           /* Use only LCDA and HiVision/YPbPr bits */
-           setSISIDXREG(SISCR, 0x38, 0xf8, cr38);
+           SiS_SetRegANDOR(SISCR, 0x38, 0xf8, cr38);
 #endif
         } else if(ivideo->chip != SIS_300) {
-           outSISIDXREG(SISCR, tvregnum, cr38);
+           SiS_SetReg(SISCR, tvregnum, cr38);
         }
-        outSISIDXREG(SISCR, 0x31, cr31);
+        SiS_SetReg(SISCR, 0x31, cr31);
 
         ivideo->SiS_Pr.SiS_UseOEM = ivideo->sisfb_useoem;
 
@@ -3677,15 +3682,15 @@ sisfb_fixup_SR11(struct sis_video_info *ivideo)
         u8  tmpreg;
 
         if(ivideo->chip >= SIS_661) {
-                inSISIDXREG(SISSR,0x11,tmpreg);
+                tmpreg = SiS_GetReg(SISSR, 0x11);
                 if(tmpreg & 0x20) {
-                        inSISIDXREG(SISSR,0x3e,tmpreg);
+                        tmpreg = SiS_GetReg(SISSR, 0x3e);
                         tmpreg = (tmpreg + 1) & 0xff;
-                        outSISIDXREG(SISSR,0x3e,tmpreg);
-                        inSISIDXREG(SISSR,0x11,tmpreg);
+                        SiS_SetReg(SISSR, 0x3e, tmpreg);
+                        tmpreg = SiS_GetReg(SISSR, 0x11);
                 }
                 if(tmpreg & 0xf0) {
-                        andSISIDXREG(SISSR,0x11,0x0f);
+                        SiS_SetRegAND(SISSR, 0x11, 0x0f);
                 }
         }
 }
@@ -3711,7 +3716,7 @@ sisfb_set_TVxposoffset(struct sis_video_info *ivideo, int val)
                         case 1:
                                 x += val;
                                 if(x < 0) x = 0;
-                                outSISIDXREG(SISSR,0x05,0x86);
+                                SiS_SetReg(SISSR, 0x05, 0x86);
                                 SiS_SetCH700x(&ivideo->SiS_Pr, 0x0a, (x & 0xff));
                                 SiS_SetCH70xxANDOR(&ivideo->SiS_Pr, 0x08, ((x & 0x0100) >> 7), 0xFD);
                                 break;
@@ -3740,11 +3745,11 @@ sisfb_set_TVxposoffset(struct sis_video_info *ivideo, int val)
                         temp += (val * 2);
                         p2_43 = temp & 0xff;
                         p2_42 = (temp & 0xf00) >> 4;
-                        outSISIDXREG(SISPART2,0x1f,p2_1f);
-                        setSISIDXREG(SISPART2,0x20,0x0F,p2_20);
-                        setSISIDXREG(SISPART2,0x2b,0xF0,p2_2b);
-                        setSISIDXREG(SISPART2,0x42,0x0F,p2_42);
-                        outSISIDXREG(SISPART2,0x43,p2_43);
+                        SiS_SetReg(SISPART2, 0x1f, p2_1f);
+                        SiS_SetRegANDOR(SISPART2, 0x20, 0x0F, p2_20);
+                        SiS_SetRegANDOR(SISPART2, 0x2b, 0xF0, p2_2b);
+                        SiS_SetRegANDOR(SISPART2, 0x42, 0x0F, p2_42);
+                        SiS_SetReg(SISPART2, 0x43, p2_43);
                 }
         }
 }
@@ -3769,7 +3774,7 @@ sisfb_set_TVyposoffset(struct sis_video_info *ivideo, int val)
                         case 1:
                                 y -= val;
                                 if(y < 0) y = 0;
-                                outSISIDXREG(SISSR,0x05,0x86);
+                                SiS_SetReg(SISSR, 0x05, 0x86);
                                 SiS_SetCH700x(&ivideo->SiS_Pr, 0x0b, (y & 0xff));
                                 SiS_SetCH70xxANDOR(&ivideo->SiS_Pr, 0x08, ((y & 0x0100) >> 8), 0xFE);
                                 break;
@@ -3793,8 +3798,8 @@ sisfb_set_TVyposoffset(struct sis_video_info *ivideo, int val)
                                         p2_02 += 2;
                                 }
                         }
-                        outSISIDXREG(SISPART2,0x01,p2_01);
-                        outSISIDXREG(SISPART2,0x02,p2_02);
+                        SiS_SetReg(SISPART2, 0x01, p2_01);
+                        SiS_SetReg(SISPART2, 0x02, p2_02);
                 }
         }
 }
@@ -3811,7 +3816,7 @@ sisfb_post_setmode(struct sis_video_info *ivideo)
         u8 reg1;
 #endif
 
-        outSISIDXREG(SISSR, 0x05, 0x86);
+        SiS_SetReg(SISSR, 0x05, 0x86);
 
 #ifdef CONFIG_FB_SIS_315
         sisfb_fixup_SR11(ivideo);
@@ -3835,7 +3840,7 @@ sisfb_post_setmode(struct sis_video_info *ivideo)
                         crt1isoff = false;
                         reg = 0x80;
                 }
-                setSISIDXREG(SISCR, 0x17, 0x7f, reg);
+                SiS_SetRegANDOR(SISCR, 0x17, 0x7f, reg);
         }
 #endif
 #ifdef CONFIG_FB_SIS_315
@@ -3849,8 +3854,8 @@ sisfb_post_setmode(struct sis_video_info *ivideo)
                         reg  = 0x00;
                         reg1 = 0x00;
                 }
-                setSISIDXREG(SISCR, ivideo->SiS_Pr.SiS_MyCR63, ~0x40, reg);
-                setSISIDXREG(SISSR, 0x1f, ~0xc0, reg1);
+                SiS_SetRegANDOR(SISCR, ivideo->SiS_Pr.SiS_MyCR63, ~0x40, reg);
+                SiS_SetRegANDOR(SISSR, 0x1f, 0x3f, reg1);
         }
 #endif
 
@@ -3866,17 +3871,17 @@ sisfb_post_setmode(struct sis_video_info *ivideo)
                 }
         }
 
-        andSISIDXREG(SISSR, IND_SIS_RAMDAC_CONTROL, ~0x04);
+        SiS_SetRegAND(SISSR, IND_SIS_RAMDAC_CONTROL, ~0x04);
 
         if(ivideo->currentvbflags & CRT2_TV) {
                 if(ivideo->vbflags2 & VB2_SISBRIDGE) {
-                        inSISIDXREG(SISPART2,0x1f,ivideo->p2_1f);
-                        inSISIDXREG(SISPART2,0x20,ivideo->p2_20);
-                        inSISIDXREG(SISPART2,0x2b,ivideo->p2_2b);
-                        inSISIDXREG(SISPART2,0x42,ivideo->p2_42);
-                        inSISIDXREG(SISPART2,0x43,ivideo->p2_43);
-                        inSISIDXREG(SISPART2,0x01,ivideo->p2_01);
-                        inSISIDXREG(SISPART2,0x02,ivideo->p2_02);
+                        ivideo->p2_1f = SiS_GetReg(SISPART2, 0x1f);
+                        ivideo->p2_20 = SiS_GetReg(SISPART2, 0x20);
+                        ivideo->p2_2b = SiS_GetReg(SISPART2, 0x2b);
+                        ivideo->p2_42 = SiS_GetReg(SISPART2, 0x42);
+                        ivideo->p2_43 = SiS_GetReg(SISPART2, 0x43);
+                        ivideo->p2_01 = SiS_GetReg(SISPART2, 0x01);
+                        ivideo->p2_02 = SiS_GetReg(SISPART2, 0x02);
                 } else if(ivideo->vbflags2 & VB2_CHRONTEL) {
                         if(ivideo->chronteltype == 1) {
                                 ivideo->tvx = SiS_GetCH700x(&ivideo->SiS_Pr, 0x0a);
@@ -4100,7 +4105,6 @@ sisfb_find_rom(struct pci_dev *pdev)
         struct sis_video_info *ivideo = pci_get_drvdata(pdev);
         void __iomem *rom_base;
         unsigned char *myrombase = NULL;
-        u32 temp;
         size_t romsize;
 
         /* First, try the official pci ROM functions (except
@@ -4114,14 +4118,6 @@ sisfb_find_rom(struct pci_dev *pdev)
                         if(sisfb_check_rom(rom_base, ivideo)) {
 
                                 if((myrombase = vmalloc(65536))) {
-
-                                        /* Work around bug in pci/rom.c: Folks forgot to check
-                                         * whether the size retrieved from the BIOS image eventually
-                                         * is larger than the mapped size
-                                         */
-                                        if(pci_resource_len(pdev, PCI_ROM_RESOURCE) < romsize)
-                                                romsize = pci_resource_len(pdev, PCI_ROM_RESOURCE);
-
                                         memcpy_fromio(myrombase, rom_base,
                                                         (romsize > 65536) ? 65536 : romsize);
                                 }
@@ -4135,43 +4131,29 @@ sisfb_find_rom(struct pci_dev *pdev)
         /* Otherwise do it the conventional way. */
 
 #if defined(__i386__) || defined(__x86_64__)
+        {
+                u32 temp;
 
-        for(temp = 0x000c0000; temp < 0x000f0000; temp += 0x00001000) {
+                for (temp = 0x000c0000; temp < 0x000f0000; temp += 0x00001000) {
 
-                rom_base = ioremap(temp, 65536);
-                if(!rom_base)
-                        continue;
-
-                if(!sisfb_check_rom(rom_base, ivideo)) {
-                        iounmap(rom_base);
-                        continue;
-                }
+                        rom_base = ioremap(temp, 65536);
+                        if (!rom_base)
+                                continue;
 
-                if((myrombase = vmalloc(65536)))
-                        memcpy_fromio(myrombase, rom_base, 65536);
-
-                iounmap(rom_base);
-                break;
-
-        }
+                        if (!sisfb_check_rom(rom_base, ivideo)) {
+                                iounmap(rom_base);
+                                continue;
+                        }
 
-#else
+                        if ((myrombase = vmalloc(65536)))
+                                memcpy_fromio(myrombase, rom_base, 65536);
 
-        pci_read_config_dword(pdev, PCI_ROM_ADDRESS, &temp);
-        pci_write_config_dword(pdev, PCI_ROM_ADDRESS,
-                        (ivideo->video_base & PCI_ROM_ADDRESS_MASK) | PCI_ROM_ADDRESS_ENABLE);
+                        iounmap(rom_base);
+                        break;
 
-        rom_base = ioremap(ivideo->video_base, 65536);
-        if(rom_base) {
-                if(sisfb_check_rom(rom_base, ivideo)) {
-                        if((myrombase = vmalloc(65536)))
-                                memcpy_fromio(myrombase, rom_base, 65536);
                 }
-                iounmap(rom_base);
-        }
-
-        pci_write_config_dword(pdev, PCI_ROM_ADDRESS, temp);
 
+        }
 #endif
 
         return myrombase;
@@ -4181,6 +4163,9 @@ static void __devinit
 sisfb_post_map_vram(struct sis_video_info *ivideo, unsigned int *mapsize,
                         unsigned int min)
 {
+        if (*mapsize < (min << 20))
+                return;
+
         ivideo->video_vbase = ioremap(ivideo->video_base, (*mapsize));
 
         if(!ivideo->video_vbase) {
@@ -4209,10 +4194,10 @@ sisfb_post_300_buswidth(struct sis_video_info *ivideo)
         unsigned char reg;
         int i, j;
 
-        andSISIDXREG(SISSR, 0x15, 0xFB);
-        orSISIDXREG(SISSR, 0x15, 0x04);
-        outSISIDXREG(SISSR, 0x13, 0x00);
-        outSISIDXREG(SISSR, 0x14, 0xBF);
+        SiS_SetRegAND(SISSR, 0x15, 0xFB);
+        SiS_SetRegOR(SISSR, 0x15, 0x04);
+        SiS_SetReg(SISSR, 0x13, 0x00);
+        SiS_SetReg(SISSR, 0x14, 0xBF);
 
         for(i = 0; i < 2; i++) {
                 temp = 0x1234;
@@ -4220,12 +4205,12 @@ sisfb_post_300_buswidth(struct sis_video_info *ivideo)
                         writew(temp, FBAddress);
                         if(readw(FBAddress) == temp)
                                 break;
-                        orSISIDXREG(SISSR, 0x3c, 0x01);
-                        inSISIDXREG(SISSR, 0x05, reg);
-                        inSISIDXREG(SISSR, 0x05, reg);
-                        andSISIDXREG(SISSR, 0x3c, 0xfe);
-                        inSISIDXREG(SISSR, 0x05, reg);
-                        inSISIDXREG(SISSR, 0x05, reg);
+                        SiS_SetRegOR(SISSR, 0x3c, 0x01);
+                        reg = SiS_GetReg(SISSR, 0x05);
+                        reg = SiS_GetReg(SISSR, 0x05);
+                        SiS_SetRegAND(SISSR, 0x3c, 0xfe);
+                        reg = SiS_GetReg(SISSR, 0x05);
+                        reg = SiS_GetReg(SISSR, 0x05);
                         temp++;
                 }
         }
@@ -4235,7 +4220,7 @@ sisfb_post_300_buswidth(struct sis_video_info *ivideo)
         writel(0x89ABCDEFL, (FBAddress + 8));
         writel(0xCDEF0123L, (FBAddress + 12));
 
-        inSISIDXREG(SISSR, 0x3b, reg);
+        reg = SiS_GetReg(SISSR, 0x3b);
         if(reg & 0x01) {
                 if(readl((FBAddress + 12)) == 0xCDEF0123L)
                         return 4;       /* Channel A 128bit */
@@ -4298,13 +4283,13 @@ sisfb_post_300_rwtest(struct sis_video_info *ivideo, int iteration, int buswidth
                 PhysicalAdrHalfPage = (PageCapacity / 2 + PhysicalAdrHigh) % PageCapacity;
                 PhysicalAdrOtherPage = PageCapacity * SiS_DRAMType[k][2] + PhysicalAdrHigh;
 
-                andSISIDXREG(SISSR, 0x15, 0xFB); /* Test */
-                orSISIDXREG(SISSR, 0x15, 0x04);  /* Test */
+                SiS_SetRegAND(SISSR, 0x15, 0xFB); /* Test */
+                SiS_SetRegOR(SISSR, 0x15, 0x04);  /* Test */
                 sr14 = (SiS_DRAMType[k][3] * buswidth) - 1;
                 if(buswidth == 4)      sr14 |= 0x80;
                 else if(buswidth == 2) sr14 |= 0x40;
-                outSISIDXREG(SISSR, 0x13, SiS_DRAMType[k][4]);
-                outSISIDXREG(SISSR, 0x14, sr14);
+                SiS_SetReg(SISSR, 0x13, SiS_DRAMType[k][4]);
+                SiS_SetReg(SISSR, 0x14, sr14);
 
                 BankNumHigh <<= 16;
                 BankNumMid <<= 16;
@@ -4371,13 +4356,13 @@ sisfb_post_sis300(struct pci_dev *pdev)
         if(!ivideo->SiS_Pr.UseROM)
                 bios = NULL;
 
-        outSISIDXREG(SISSR, 0x05, 0x86);
+        SiS_SetReg(SISSR, 0x05, 0x86);
 
         if(bios) {
                 if(bios[0x52] & 0x80) {
                         memtype = bios[0x52];
                 } else {
-                        inSISIDXREG(SISSR, 0x3a, memtype);
+                        memtype = SiS_GetReg(SISSR, 0x3a);
                 }
                 memtype &= 0x07;
         }
@@ -4401,19 +4386,19 @@ sisfb_post_sis300(struct pci_dev *pdev)
                         v6 = bios[rindex++];
                 }
         }
-        outSISIDXREG(SISSR, 0x28, v1);
-        outSISIDXREG(SISSR, 0x29, v2);
-        outSISIDXREG(SISSR, 0x2a, v3);
-        outSISIDXREG(SISSR, 0x2e, v4);
-        outSISIDXREG(SISSR, 0x2f, v5);
-        outSISIDXREG(SISSR, 0x30, v6);
+        SiS_SetReg(SISSR, 0x28, v1);
+        SiS_SetReg(SISSR, 0x29, v2);
+        SiS_SetReg(SISSR, 0x2a, v3);
+        SiS_SetReg(SISSR, 0x2e, v4);
+        SiS_SetReg(SISSR, 0x2f, v5);
+        SiS_SetReg(SISSR, 0x30, v6);
 
         v1 = 0x10;
         if(bios)
                 v1 = bios[0xa4];
-        outSISIDXREG(SISSR, 0x07, v1);       /* DAC speed */
+        SiS_SetReg(SISSR, 0x07, v1);       /* DAC speed */
 
-        outSISIDXREG(SISSR, 0x11, 0x0f);     /* DDC, power save */
+        SiS_SetReg(SISSR, 0x11, 0x0f);     /* DDC, power save */
 
         v1 = 0x01; v2 = 0x43; v3 = 0x1e; v4 = 0x2a;
         v5 = 0x06; v6 = 0x00; v7 = 0x00; v8 = 0x00;
@@ -4430,91 +4415,91 @@ sisfb_post_sis300(struct pci_dev *pdev)
         }
         if(ivideo->revision_id >= 0x80)
                 v3 &= 0xfd;
-        outSISIDXREG(SISSR, 0x15, v1);       /* Ram type (assuming 0, BIOS 0xa5 step 8) */
-        outSISIDXREG(SISSR, 0x16, v2);
-        outSISIDXREG(SISSR, 0x17, v3);
-        outSISIDXREG(SISSR, 0x18, v4);
-        outSISIDXREG(SISSR, 0x19, v5);
-        outSISIDXREG(SISSR, 0x1a, v6);
-        outSISIDXREG(SISSR, 0x1b, v7);
-        outSISIDXREG(SISSR, 0x1c, v8);     /* ---- */
-        andSISIDXREG(SISSR, 0x15 ,0xfb);
-        orSISIDXREG(SISSR, 0x15, 0x04);
+        SiS_SetReg(SISSR, 0x15, v1);       /* Ram type (assuming 0, BIOS 0xa5 step 8) */
+        SiS_SetReg(SISSR, 0x16, v2);
+        SiS_SetReg(SISSR, 0x17, v3);
+        SiS_SetReg(SISSR, 0x18, v4);
+        SiS_SetReg(SISSR, 0x19, v5);
+        SiS_SetReg(SISSR, 0x1a, v6);
+        SiS_SetReg(SISSR, 0x1b, v7);
+        SiS_SetReg(SISSR, 0x1c, v8);       /* ---- */
+        SiS_SetRegAND(SISSR, 0x15, 0xfb);
+        SiS_SetRegOR(SISSR, 0x15, 0x04);
         if(bios) {
                 if(bios[0x53] & 0x02) {
-                        orSISIDXREG(SISSR, 0x19, 0x20);
+                        SiS_SetRegOR(SISSR, 0x19, 0x20);
                 }
         }
         v1 = 0x04;                         /* DAC pedestal (BIOS 0xe5) */
         if(ivideo->revision_id >= 0x80)
                 v1 |= 0x01;
-        outSISIDXREG(SISSR, 0x1f, v1);
-        outSISIDXREG(SISSR, 0x20, 0xa4);     /* linear & relocated io & disable a0000 */
+        SiS_SetReg(SISSR, 0x1f, v1);
+        SiS_SetReg(SISSR, 0x20, 0xa4);     /* linear & relocated io & disable a0000 */
         v1 = 0xf6; v2 = 0x0d; v3 = 0x00;
         if(bios) {
                 v1 = bios[0xe8];
                 v2 = bios[0xe9];
                 v3 = bios[0xea];
         }
-        outSISIDXREG(SISSR, 0x23, v1);
-        outSISIDXREG(SISSR, 0x24, v2);
-        outSISIDXREG(SISSR, 0x25, v3);
-        outSISIDXREG(SISSR, 0x21, 0x84);
-        outSISIDXREG(SISSR, 0x22, 0x00);
-        outSISIDXREG(SISCR, 0x37, 0x00);
-        orSISIDXREG(SISPART1, 0x24, 0x01);   /* unlock crt2 */
-        outSISIDXREG(SISPART1, 0x00, 0x00);
+        SiS_SetReg(SISSR, 0x23, v1);
+        SiS_SetReg(SISSR, 0x24, v2);
+        SiS_SetReg(SISSR, 0x25, v3);
+        SiS_SetReg(SISSR, 0x21, 0x84);
+        SiS_SetReg(SISSR, 0x22, 0x00);
+        SiS_SetReg(SISCR, 0x37, 0x00);
+        SiS_SetRegOR(SISPART1, 0x24, 0x01);   /* unlock crt2 */
+        SiS_SetReg(SISPART1, 0x00, 0x00);
         v1 = 0x40; v2 = 0x11;
         if(bios) {
                 v1 = bios[0xec];
                 v2 = bios[0xeb];
         }
-        outSISIDXREG(SISPART1, 0x02, v1);
+        SiS_SetReg(SISPART1, 0x02, v1);
 
         if(ivideo->revision_id >= 0x80)
                 v2 &= ~0x01;
 
-        inSISIDXREG(SISPART4, 0x00, reg);
+        reg = SiS_GetReg(SISPART4, 0x00);
         if((reg == 1) || (reg == 2)) {
-                outSISIDXREG(SISCR, 0x37, 0x02);
-                outSISIDXREG(SISPART2, 0x00, 0x1c);
+                SiS_SetReg(SISCR, 0x37, 0x02);
+                SiS_SetReg(SISPART2, 0x00, 0x1c);
                 v4 = 0x00; v5 = 0x00; v6 = 0x10;
                 if(ivideo->SiS_Pr.UseROM) {
                         v4 = bios[0xf5];
                         v5 = bios[0xf6];
                         v6 = bios[0xf7];
                 }
-                outSISIDXREG(SISPART4, 0x0d, v4);
-                outSISIDXREG(SISPART4, 0x0e, v5);
-                outSISIDXREG(SISPART4, 0x10, v6);
-                outSISIDXREG(SISPART4, 0x0f, 0x3f);
-                inSISIDXREG(SISPART4, 0x01, reg);
+                SiS_SetReg(SISPART4, 0x0d, v4);
+                SiS_SetReg(SISPART4, 0x0e, v5);
+                SiS_SetReg(SISPART4, 0x10, v6);
+                SiS_SetReg(SISPART4, 0x0f, 0x3f);
+                reg = SiS_GetReg(SISPART4, 0x01);
                 if(reg >= 0xb0) {
-                        inSISIDXREG(SISPART4, 0x23, reg);
+                        reg = SiS_GetReg(SISPART4, 0x23);
                         reg &= 0x20;
                         reg <<= 1;
-                        outSISIDXREG(SISPART4, 0x23, reg);
+                        SiS_SetReg(SISPART4, 0x23, reg);
                 }
         } else {
                 v2 &= ~0x10;
         }
-        outSISIDXREG(SISSR, 0x32, v2);
+        SiS_SetReg(SISSR, 0x32, v2);
 
-        andSISIDXREG(SISPART1, 0x24, 0xfe);  /* Lock CRT2 */
+        SiS_SetRegAND(SISPART1, 0x24, 0xfe);  /* Lock CRT2 */
 
-        inSISIDXREG(SISSR, 0x16, reg);
+        reg = SiS_GetReg(SISSR, 0x16);
         reg &= 0xc3;
-        outSISIDXREG(SISCR, 0x35, reg);
-        outSISIDXREG(SISCR, 0x83, 0x00);
+        SiS_SetReg(SISCR, 0x35, reg);
+        SiS_SetReg(SISCR, 0x83, 0x00);
 #if !defined(__i386__) && !defined(__x86_64__)
         if(sisfb_videoram) {
-                outSISIDXREG(SISSR, 0x13, 0x28);  /* ? */
+                SiS_SetReg(SISSR, 0x13, 0x28);  /* ? */
                 reg = ((sisfb_videoram >> 10) - 1) | 0x40;
-                outSISIDXREG(SISSR, 0x14, reg);
+                SiS_SetReg(SISSR, 0x14, reg);
         } else {
 #endif
                 /* Need to map max FB size for finding out about RAM size */
-                mapsize = 64 << 20;
+                mapsize = ivideo->video_size;
                 sisfb_post_map_vram(ivideo, &mapsize, 4);
 
                 if(ivideo->video_vbase) {
@@ -4523,8 +4508,8 @@ sisfb_post_sis300(struct pci_dev *pdev)
                 } else {
                         printk(KERN_DEBUG
                                 "sisfb: Failed to map memory for size detection, assuming 8MB\n");
-                        outSISIDXREG(SISSR, 0x13, 0x28);  /* ? */
-                        outSISIDXREG(SISSR, 0x14, 0x47);  /* 8MB, 64bit default */
+                        SiS_SetReg(SISSR, 0x13, 0x28);  /* ? */
+                        SiS_SetReg(SISSR, 0x14, 0x47);  /* 8MB, 64bit default */
                 }
 #if !defined(__i386__) && !defined(__x86_64__)
         }
@@ -4533,7 +4518,7 @@ sisfb_post_sis300(struct pci_dev *pdev)
                 v1 = bios[0xe6];
                 v2 = bios[0xe7];
         } else {
-                inSISIDXREG(SISSR, 0x3a, reg);
+                reg = SiS_GetReg(SISSR, 0x3a);
                 if((reg & 0x30) == 0x30) {
                         v1 = 0x04; /* PCI */
                         v2 = 0x92;
@@ -4542,8 +4527,8 @@ sisfb_post_sis300(struct pci_dev *pdev)
                         v2 = 0xb2;
                 }
         }
-        outSISIDXREG(SISSR, 0x21, v1);
-        outSISIDXREG(SISSR, 0x22, v2);
+        SiS_SetReg(SISSR, 0x21, v1);
+        SiS_SetReg(SISSR, 0x22, v2);
 
         /* Sense CRT1 */
         sisfb_sense_crt1(ivideo);
@@ -4556,13 +4541,13 @@ sisfb_post_sis300(struct pci_dev *pdev)
         ivideo->SiS_Pr.VideoMemorySize = 8 << 20;
         SiSSetMode(&ivideo->SiS_Pr, 0x2e | 0x80);
 
-        outSISIDXREG(SISSR, 0x05, 0x86);
+        SiS_SetReg(SISSR, 0x05, 0x86);
 
         /* Display off */
-        orSISIDXREG(SISSR, 0x01, 0x20);
+        SiS_SetRegOR(SISSR, 0x01, 0x20);
 
         /* Save mode number in CR34 */
-        outSISIDXREG(SISCR, 0x34, 0x2e);
+        SiS_SetReg(SISCR, 0x34, 0x2e);
 
         /* Let everyone know what the current mode is */
         ivideo->modeprechange = 0x2e;
@@ -4578,6 +4563,11 @@ sisfb_post_sis315330(struct pci_dev *pdev)
 }
 #endif
 
+static inline int sisfb_xgi_is21(struct sis_video_info *ivideo)
+{
+        return ivideo->chip_real_id == XGI_21;
+}
+
 static void __devinit
 sisfb_post_xgi_delay(struct sis_video_info *ivideo, int delay)
 {
@@ -4585,7 +4575,7 @@ sisfb_post_xgi_delay(struct sis_video_info *ivideo, int delay)
         u8 reg;
 
         for(i = 0; i <= (delay * 10 * 36); i++) {
-                inSISIDXREG(SISSR, 0x05, reg);
+                reg = SiS_GetReg(SISSR, 0x05);
                 reg++;
         }
 }
@@ -4642,11 +4632,11 @@ sisfb_post_xgi_rwtest(struct sis_video_info *ivideo, int starta,
         return 1;
 }
 
-static void __devinit
+static int __devinit
 sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
 {
         unsigned int buswidth, ranksize, channelab, mapsize;
-        int i, j, k, l;
+        int i, j, k, l, status;
         u8 reg, sr14;
         static const u8 dramsr13[12 * 5] = {
                 0x02, 0x0e, 0x0b, 0x80, 0x5d,
@@ -4677,84 +4667,84 @@ sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
          * - if running on non-x86, there usually is no VGA window
          *   at a0000.
          */
-        orSISIDXREG(SISSR, 0x20, (0x80 | 0x04));
+        SiS_SetRegOR(SISSR, 0x20, (0x80 | 0x04));
 
         /* Need to map max FB size for finding out about RAM size */
-        mapsize = 256 << 20;
+        mapsize = ivideo->video_size;
         sisfb_post_map_vram(ivideo, &mapsize, 32);
 
         if(!ivideo->video_vbase) {
                 printk(KERN_ERR "sisfb: Unable to detect RAM size. Setting default.\n");
-                outSISIDXREG(SISSR, 0x13, 0x35);
-                outSISIDXREG(SISSR, 0x14, 0x41);
+                SiS_SetReg(SISSR, 0x13, 0x35);
+                SiS_SetReg(SISSR, 0x14, 0x41);
                 /* TODO */
-                return;
+                return -ENOMEM;
         }
 
         /* Non-interleaving */
-        outSISIDXREG(SISSR, 0x15, 0x00);
+        SiS_SetReg(SISSR, 0x15, 0x00);
         /* No tiling */
-        outSISIDXREG(SISSR, 0x1c, 0x00);
+        SiS_SetReg(SISSR, 0x1c, 0x00);
 
         if(ivideo->chip == XGI_20) {
 
                 channelab = 1;
-                inSISIDXREG(SISCR, 0x97, reg);
+                reg = SiS_GetReg(SISCR, 0x97);
                 if(!(reg & 0x01)) {     /* Single 32/16 */
                         buswidth = 32;
-                        outSISIDXREG(SISSR, 0x13, 0xb1);
-                        outSISIDXREG(SISSR, 0x14, 0x52);
+                        SiS_SetReg(SISSR, 0x13, 0xb1);
+                        SiS_SetReg(SISSR, 0x14, 0x52);
                         sisfb_post_xgi_delay(ivideo, 1);
                         sr14 = 0x02;
                         if(sisfb_post_xgi_rwtest(ivideo, 23, 24, mapsize))
                                 goto bail_out;
 
-                        outSISIDXREG(SISSR, 0x13, 0x31);
-                        outSISIDXREG(SISSR, 0x14, 0x42);
+                        SiS_SetReg(SISSR, 0x13, 0x31);
+                        SiS_SetReg(SISSR, 0x14, 0x42);
                         sisfb_post_xgi_delay(ivideo, 1);
                         if(sisfb_post_xgi_rwtest(ivideo, 23, 23, mapsize))
                                 goto bail_out;
 
                         buswidth = 16;
-                        outSISIDXREG(SISSR, 0x13, 0xb1);
-                        outSISIDXREG(SISSR, 0x14, 0x41);
+                        SiS_SetReg(SISSR, 0x13, 0xb1);
+                        SiS_SetReg(SISSR, 0x14, 0x41);
                         sisfb_post_xgi_delay(ivideo, 1);
                         sr14 = 0x01;
                         if(sisfb_post_xgi_rwtest(ivideo, 22, 23, mapsize))
                                 goto bail_out;
                         else
-                                outSISIDXREG(SISSR, 0x13, 0x31);
+                                SiS_SetReg(SISSR, 0x13, 0x31);
                 } else {                /* Dual 16/8 */
                         buswidth = 16;
-                        outSISIDXREG(SISSR, 0x13, 0xb1);
-                        outSISIDXREG(SISSR, 0x14, 0x41);
+                        SiS_SetReg(SISSR, 0x13, 0xb1);
+                        SiS_SetReg(SISSR, 0x14, 0x41);
                         sisfb_post_xgi_delay(ivideo, 1);
                         sr14 = 0x01;
                         if(sisfb_post_xgi_rwtest(ivideo, 22, 23, mapsize))
                                 goto bail_out;
 
-                        outSISIDXREG(SISSR, 0x13, 0x31);
-                        outSISIDXREG(SISSR, 0x14, 0x31);
+                        SiS_SetReg(SISSR, 0x13, 0x31);
+                        SiS_SetReg(SISSR, 0x14, 0x31);
                         sisfb_post_xgi_delay(ivideo, 1);
                         if(sisfb_post_xgi_rwtest(ivideo, 22, 22, mapsize))
                                 goto bail_out;
 
                         buswidth = 8;
-                        outSISIDXREG(SISSR, 0x13, 0xb1);
-                        outSISIDXREG(SISSR, 0x14, 0x30);
+                        SiS_SetReg(SISSR, 0x13, 0xb1);
+                        SiS_SetReg(SISSR, 0x14, 0x30);
                         sisfb_post_xgi_delay(ivideo, 1);
                         sr14 = 0x00;
                         if(sisfb_post_xgi_rwtest(ivideo, 21, 22, mapsize))
                                 goto bail_out;
                         else
-                                outSISIDXREG(SISSR, 0x13, 0x31);
+                                SiS_SetReg(SISSR, 0x13, 0x31);
                 }
 
         } else {        /* XGI_40 */
 
-                inSISIDXREG(SISCR, 0x97, reg);
+                reg = SiS_GetReg(SISCR, 0x97);
                 if(!(reg & 0x10)) {
-                        inSISIDXREG(SISSR, 0x39, reg);
+                        reg = SiS_GetReg(SISSR, 0x39);
                         reg >>= 1;
                 }
 
@@ -4762,52 +4752,52 @@ sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
                         buswidth = 32;
                         if(ivideo->revision_id == 2) {
                                 channelab = 2;
-                                outSISIDXREG(SISSR, 0x13, 0xa1);
-                                outSISIDXREG(SISSR, 0x14, 0x44);
+                                SiS_SetReg(SISSR, 0x13, 0xa1);
+                                SiS_SetReg(SISSR, 0x14, 0x44);
                                 sr14 = 0x04;
                                 sisfb_post_xgi_delay(ivideo, 1);
                                 if(sisfb_post_xgi_rwtest(ivideo, 23, 24, mapsize))
                                         goto bail_out;
 
-                                outSISIDXREG(SISSR, 0x13, 0x21);
-                                outSISIDXREG(SISSR, 0x14, 0x34);
+                                SiS_SetReg(SISSR, 0x13, 0x21);
+                                SiS_SetReg(SISSR, 0x14, 0x34);
                                 if(sisfb_post_xgi_rwtest(ivideo, 22, 23, mapsize))
                                         goto bail_out;
 
                                 channelab = 1;
-                                outSISIDXREG(SISSR, 0x13, 0xa1);
-                                outSISIDXREG(SISSR, 0x14, 0x40);
+                                SiS_SetReg(SISSR, 0x13, 0xa1);
+                                SiS_SetReg(SISSR, 0x14, 0x40);
                                 sr14 = 0x00;
                                 if(sisfb_post_xgi_rwtest(ivideo, 22, 23, mapsize))
                                         goto bail_out;
 
-                                outSISIDXREG(SISSR, 0x13, 0x21);
-                                outSISIDXREG(SISSR, 0x14, 0x30);
+                                SiS_SetReg(SISSR, 0x13, 0x21);
+                                SiS_SetReg(SISSR, 0x14, 0x30);
                         } else {
                                 channelab = 3;
-                                outSISIDXREG(SISSR, 0x13, 0xa1);
-                                outSISIDXREG(SISSR, 0x14, 0x4c);
+                                SiS_SetReg(SISSR, 0x13, 0xa1);
+                                SiS_SetReg(SISSR, 0x14, 0x4c);
                                 sr14 = 0x0c;
                                 sisfb_post_xgi_delay(ivideo, 1);
                                 if(sisfb_post_xgi_rwtest(ivideo, 23, 25, mapsize))
                                         goto bail_out;
 
                                 channelab = 2;
-                                outSISIDXREG(SISSR, 0x14, 0x48);
+                                SiS_SetReg(SISSR, 0x14, 0x48);
                                 sisfb_post_xgi_delay(ivideo, 1);
                                 sr14 = 0x08;
                                 if(sisfb_post_xgi_rwtest(ivideo, 23, 24, mapsize))
                                         goto bail_out;
 
-                                outSISIDXREG(SISSR, 0x13, 0x21);
-                                outSISIDXREG(SISSR, 0x14, 0x3c);
+                                SiS_SetReg(SISSR, 0x13, 0x21);
+                                SiS_SetReg(SISSR, 0x14, 0x3c);
                                 sr14 = 0x0c;
 
                                 if(sisfb_post_xgi_rwtest(ivideo, 23, 24, mapsize)) {
                                         channelab = 3;
                                 } else {
                                         channelab = 2;
-                                        outSISIDXREG(SISSR, 0x14, 0x38);
+                                        SiS_SetReg(SISSR, 0x14, 0x38);
                                         sr14 = 0x08;
                                 }
                         }
@@ -4818,26 +4808,26 @@ sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
                         buswidth = 64;
                         if(ivideo->revision_id == 2) {
                                 channelab = 1;
-                                outSISIDXREG(SISSR, 0x13, 0xa1);
-                                outSISIDXREG(SISSR, 0x14, 0x52);
+                                SiS_SetReg(SISSR, 0x13, 0xa1);
+                                SiS_SetReg(SISSR, 0x14, 0x52);
                                 sisfb_post_xgi_delay(ivideo, 1);
                                 sr14 = 0x02;
                                 if(sisfb_post_xgi_rwtest(ivideo, 23, 24, mapsize))
                                         goto bail_out;
 
-                                outSISIDXREG(SISSR, 0x13, 0x21);
-                                outSISIDXREG(SISSR, 0x14, 0x42);
+                                SiS_SetReg(SISSR, 0x13, 0x21);
+                                SiS_SetReg(SISSR, 0x14, 0x42);
                         } else {
                                 channelab = 2;
-                                outSISIDXREG(SISSR, 0x13, 0xa1);
-                                outSISIDXREG(SISSR, 0x14, 0x5a);
+                                SiS_SetReg(SISSR, 0x13, 0xa1);
+                                SiS_SetReg(SISSR, 0x14, 0x5a);
                                 sisfb_post_xgi_delay(ivideo, 1);
                                 sr14 = 0x0a;
                                 if(sisfb_post_xgi_rwtest(ivideo, 24, 25, mapsize))
                                         goto bail_out;
 
-                                outSISIDXREG(SISSR, 0x13, 0x21);
-                                outSISIDXREG(SISSR, 0x14, 0x4a);
+                                SiS_SetReg(SISSR, 0x13, 0x21);
+                                SiS_SetReg(SISSR, 0x14, 0x4a);
                         }
                         sisfb_post_xgi_delay(ivideo, 1);
 
@@ -4845,23 +4835,24 @@ sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
         }
 
 bail_out:
-        setSISIDXREG(SISSR, 0x14, 0xf0, sr14);
+        SiS_SetRegANDOR(SISSR, 0x14, 0xf0, sr14);
         sisfb_post_xgi_delay(ivideo, 1);
 
         j = (ivideo->chip == XGI_20) ? 5 : 9;
         k = (ivideo->chip == XGI_20) ? 12 : 4;
+        status = -EIO;
 
         for(i = 0; i < k; i++) {
 
                 reg = (ivideo->chip == XGI_20) ?
                                 dramsr13[(i * 5) + 4] : dramsr13_4[(i * 5) + 4];
-                setSISIDXREG(SISSR, 0x13, 0x80, reg);
+                SiS_SetRegANDOR(SISSR, 0x13, 0x80, reg);
                 sisfb_post_xgi_delay(ivideo, 50);
 
                 ranksize = (ivideo->chip == XGI_20) ?
                                 dramsr13[(i * 5) + 3] : dramsr13_4[(i * 5) + 3];
 
-                inSISIDXREG(SISSR, 0x13, reg);
+                reg = SiS_GetReg(SISSR, 0x13);
                 if(reg & 0x80) ranksize <<= 1;
 
                 if(ivideo->chip == XGI_20) {
@@ -4880,14 +4871,18 @@ bail_out:
 
                 if(!reg) continue;
 
-                setSISIDXREG(SISSR, 0x14, 0x0f, (reg & 0xf0));
+                SiS_SetRegANDOR(SISSR, 0x14, 0x0f, (reg & 0xf0));
                 sisfb_post_xgi_delay(ivideo, 1);
 
-                if(sisfb_post_xgi_rwtest(ivideo, j, ((reg >> 4) + channelab - 2 + 20), mapsize))
+                if (sisfb_post_xgi_rwtest(ivideo, j, ((reg >> 4) + channelab - 2 + 20), mapsize)) {
+                        status = 0;
                         break;
+                }
         }
 
         iounmap(ivideo->video_vbase);
+
+        return status;
 }
 
 static void __devinit
@@ -4925,9 +4920,9 @@ sisfb_post_xgi_setclocks(struct sis_video_info *ivideo, u8 regb)
                 v2 = ivideo->bios_abase[0x90 + index + 1];
                 v3 = ivideo->bios_abase[0x90 + index + 2];
         }
-        outSISIDXREG(SISSR, 0x28, v1);
-        outSISIDXREG(SISSR, 0x29, v2);
-        outSISIDXREG(SISSR, 0x2a, v3);
+        SiS_SetReg(SISSR, 0x28, v1);
+        SiS_SetReg(SISSR, 0x29, v2);
+        SiS_SetReg(SISSR, 0x2a, v3);
         sisfb_post_xgi_delay(ivideo, 0x43);
         sisfb_post_xgi_delay(ivideo, 0x43);
         sisfb_post_xgi_delay(ivideo, 0x43);
@@ -4938,14 +4933,183 @@ sisfb_post_xgi_setclocks(struct sis_video_info *ivideo, u8 regb)
                 v2 = ivideo->bios_abase[0xb8 + index + 1];
                 v3 = ivideo->bios_abase[0xb8 + index + 2];
         }
-        outSISIDXREG(SISSR, 0x2e, v1);
-        outSISIDXREG(SISSR, 0x2f, v2);
-        outSISIDXREG(SISSR, 0x30, v3);
+        SiS_SetReg(SISSR, 0x2e, v1);
+        SiS_SetReg(SISSR, 0x2f, v2);
+        SiS_SetReg(SISSR, 0x30, v3);
         sisfb_post_xgi_delay(ivideo, 0x43);
         sisfb_post_xgi_delay(ivideo, 0x43);
         sisfb_post_xgi_delay(ivideo, 0x43);
 }
 
+static void __devinit
+sisfb_post_xgi_ddr2_mrs_default(struct sis_video_info *ivideo, u8 regb)
+{
+        unsigned char *bios = ivideo->bios_abase;
+        u8 v1;
+
+        SiS_SetReg(SISSR, 0x28, 0x64);
+        SiS_SetReg(SISSR, 0x29, 0x63);
+        sisfb_post_xgi_delay(ivideo, 15);
+        SiS_SetReg(SISSR, 0x18, 0x00);
+        SiS_SetReg(SISSR, 0x19, 0x20);
+        SiS_SetReg(SISSR, 0x16, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x80);
+        SiS_SetReg(SISSR, 0x18, 0xc5);
+        SiS_SetReg(SISSR, 0x19, 0x23);
+        SiS_SetReg(SISSR, 0x16, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x80);
+        sisfb_post_xgi_delay(ivideo, 1);
+        SiS_SetReg(SISCR, 0x97, 0x11);
+        sisfb_post_xgi_setclocks(ivideo, regb);
+        sisfb_post_xgi_delay(ivideo, 0x46);
+        SiS_SetReg(SISSR, 0x18, 0xc5);
+        SiS_SetReg(SISSR, 0x19, 0x23);
+        SiS_SetReg(SISSR, 0x16, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x80);
+        sisfb_post_xgi_delay(ivideo, 1);
+        SiS_SetReg(SISSR, 0x1b, 0x04);
+        sisfb_post_xgi_delay(ivideo, 1);
+        SiS_SetReg(SISSR, 0x1b, 0x00);
+        sisfb_post_xgi_delay(ivideo, 1);
+        v1 = 0x31;
+        if (ivideo->haveXGIROM) {
+                v1 = bios[0xf0];
+        }
+        SiS_SetReg(SISSR, 0x18, v1);
+        SiS_SetReg(SISSR, 0x19, 0x06);
+        SiS_SetReg(SISSR, 0x16, 0x04);
+        SiS_SetReg(SISSR, 0x16, 0x84);
+        sisfb_post_xgi_delay(ivideo, 1);
+}
+
+static void __devinit
+sisfb_post_xgi_ddr2_mrs_xg21(struct sis_video_info *ivideo)
+{
+        sisfb_post_xgi_setclocks(ivideo, 1);
+
+        SiS_SetReg(SISCR, 0x97, 0x11);
+        sisfb_post_xgi_delay(ivideo, 0x46);
+
+        SiS_SetReg(SISSR, 0x18, 0x00);  /* EMRS2 */
+        SiS_SetReg(SISSR, 0x19, 0x80);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+
+        SiS_SetReg(SISSR, 0x18, 0x00);  /* EMRS3 */
+        SiS_SetReg(SISSR, 0x19, 0xc0);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+
+        SiS_SetReg(SISSR, 0x18, 0x00);  /* EMRS1 */
+        SiS_SetReg(SISSR, 0x19, 0x40);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+
+        SiS_SetReg(SISSR, 0x18, 0x42);  /* MRS1 */
+        SiS_SetReg(SISSR, 0x19, 0x02);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+        sisfb_post_xgi_delay(ivideo, 1);
+
+        SiS_SetReg(SISSR, 0x1b, 0x04);
+        sisfb_post_xgi_delay(ivideo, 1);
+
+        SiS_SetReg(SISSR, 0x1b, 0x00);
+        sisfb_post_xgi_delay(ivideo, 1);
+
+        SiS_SetReg(SISSR, 0x18, 0x42);  /* MRS1 */
+        SiS_SetReg(SISSR, 0x19, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+        sisfb_post_xgi_delay(ivideo, 1);
+}
+
+static void __devinit
+sisfb_post_xgi_ddr2(struct sis_video_info *ivideo, u8 regb)
+{
+        unsigned char *bios = ivideo->bios_abase;
+        static const u8 cs158[8] = {
+                0x88, 0xaa, 0x48, 0x00, 0x00, 0x00, 0x00, 0x00
+        };
+        static const u8 cs160[8] = {
+                0x44, 0x77, 0x77, 0x00, 0x00, 0x00, 0x00, 0x00
+        };
+        static const u8 cs168[8] = {
+                0x48, 0x78, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00
+        };
+        u8 reg;
+        u8 v1;
+        u8 v2;
+        u8 v3;
+
+        SiS_SetReg(SISCR, 0xb0, 0x80); /* DDR2 dual frequency mode */
+        SiS_SetReg(SISCR, 0x82, 0x77);
+        SiS_SetReg(SISCR, 0x86, 0x00);
+        reg = SiS_GetReg(SISCR, 0x86);
+        SiS_SetReg(SISCR, 0x86, 0x88);
+        reg = SiS_GetReg(SISCR, 0x86);
+        v1 = cs168[regb]; v2 = cs160[regb]; v3 = cs158[regb];
+        if (ivideo->haveXGIROM) {
+                v1 = bios[regb + 0x168];
+                v2 = bios[regb + 0x160];
+                v3 = bios[regb + 0x158];
+        }
+        SiS_SetReg(SISCR, 0x86, v1);
+        SiS_SetReg(SISCR, 0x82, 0x77);
+        SiS_SetReg(SISCR, 0x85, 0x00);
+        reg = SiS_GetReg(SISCR, 0x85);
+        SiS_SetReg(SISCR, 0x85, 0x88);
+        reg = SiS_GetReg(SISCR, 0x85);
+        SiS_SetReg(SISCR, 0x85, v2);
+        SiS_SetReg(SISCR, 0x82, v3);
+        SiS_SetReg(SISCR, 0x98, 0x01);
+        SiS_SetReg(SISCR, 0x9a, 0x02);
+        if (sisfb_xgi_is21(ivideo))
+                sisfb_post_xgi_ddr2_mrs_xg21(ivideo);
+        else
+                sisfb_post_xgi_ddr2_mrs_default(ivideo, regb);
+}
+
+static u8 __devinit
+sisfb_post_xgi_ramtype(struct sis_video_info *ivideo)
+{
+        unsigned char *bios = ivideo->bios_abase;
+        u8 ramtype;
+        u8 reg;
+        u8 v1;
+
+        ramtype = 0x00; v1 = 0x10;
+        if (ivideo->haveXGIROM) {
+                ramtype = bios[0x62];
+                v1 = bios[0x1d2];
+        }
+        if (!(ramtype & 0x80)) {
+                if (sisfb_xgi_is21(ivideo)) {
+                        SiS_SetRegAND(SISCR, 0xb4, 0xfd); /* GPIO control */
+                        SiS_SetRegOR(SISCR, 0x4a, 0x80);  /* GPIOH EN */
+                        reg = SiS_GetReg(SISCR, 0x48);
+                        SiS_SetRegOR(SISCR, 0xb4, 0x02);
+                        ramtype = reg & 0x01;             /* GPIOH */
+                } else if (ivideo->chip == XGI_20) {
+                        SiS_SetReg(SISCR, 0x97, v1);
+                        reg = SiS_GetReg(SISCR, 0x97);
+                        if (reg & 0x10) {
+                                ramtype = (reg & 0x01) << 1;
+                        }
+                } else {
+                        reg = SiS_GetReg(SISSR, 0x39);
+                        ramtype = reg & 0x02;
+                        if (!(ramtype)) {
+                                reg = SiS_GetReg(SISSR, 0x3a);
+                                ramtype = (reg >> 1) & 0x01;
+                        }
+                }
+        }
+        ramtype &= 0x07;
+
+        return ramtype;
+}
+
 static int __devinit
 sisfb_post_xgi(struct pci_dev *pdev)
 {
@@ -5013,29 +5177,29 @@ sisfb_post_xgi(struct pci_dev *pdev)
         };
 
         /* VGA enable */
-        reg = inSISREG(SISVGAENABLE) | 0x01;
-        outSISREG(SISVGAENABLE, reg);
+        reg = SiS_GetRegByte(SISVGAENABLE) | 0x01;
+        SiS_SetRegByte(SISVGAENABLE, reg);
 
         /* Misc */
-        reg = inSISREG(SISMISCR) | 0x01;
-        outSISREG(SISMISCW, reg);
+        reg = SiS_GetRegByte(SISMISCR) | 0x01;
+        SiS_SetRegByte(SISMISCW, reg);
 
         /* Unlock SR */
-        outSISIDXREG(SISSR, 0x05, 0x86);
-        inSISIDXREG(SISSR, 0x05, reg);
+        SiS_SetReg(SISSR, 0x05, 0x86);
+        reg = SiS_GetReg(SISSR, 0x05);
         if(reg != 0xa1)
                 return 0;
 
         /* Clear some regs */
         for(i = 0; i < 0x22; i++) {
                 if(0x06 + i == 0x20) continue;
-                outSISIDXREG(SISSR, 0x06 + i, 0x00);
+                SiS_SetReg(SISSR, 0x06 + i, 0x00);
         }
         for(i = 0; i < 0x0b; i++) {
-                outSISIDXREG(SISSR, 0x31 + i, 0x00);
+                SiS_SetReg(SISSR, 0x31 + i, 0x00);
         }
         for(i = 0; i < 0x10; i++) {
-                outSISIDXREG(SISCR, 0x30 + i, 0x00);
+                SiS_SetReg(SISCR, 0x30 + i, 0x00);
         }
 
         ptr = cs78;
@@ -5043,7 +5207,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 ptr = (const u8 *)&bios[0x78];
         }
         for(i = 0; i < 3; i++) {
-                outSISIDXREG(SISSR, 0x23 + i, ptr[i]);
+                SiS_SetReg(SISSR, 0x23 + i, ptr[i]);
         }
 
         ptr = cs76;
@@ -5051,7 +5215,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 ptr = (const u8 *)&bios[0x76];
         }
         for(i = 0; i < 2; i++) {
-                outSISIDXREG(SISSR, 0x21 + i, ptr[i]);
+                SiS_SetReg(SISSR, 0x21 + i, ptr[i]);
         }
 
         v1 = 0x18; v2 = 0x00;
@@ -5059,83 +5223,83 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 v1 = bios[0x74];
                 v2 = bios[0x75];
         }
-        outSISIDXREG(SISSR, 0x07, v1);
-        outSISIDXREG(SISSR, 0x11, 0x0f);
-        outSISIDXREG(SISSR, 0x1f, v2);
+        SiS_SetReg(SISSR, 0x07, v1);
+        SiS_SetReg(SISSR, 0x11, 0x0f);
+        SiS_SetReg(SISSR, 0x1f, v2);
         /* PCI linear mode, RelIO enabled, A0000 decoding disabled */
-        outSISIDXREG(SISSR, 0x20, 0x80 | 0x20 | 0x04);
-        outSISIDXREG(SISSR, 0x27, 0x74);
+        SiS_SetReg(SISSR, 0x20, 0x80 | 0x20 | 0x04);
+        SiS_SetReg(SISSR, 0x27, 0x74);
 
         ptr = cs7b;
         if(ivideo->haveXGIROM) {
                 ptr = (const u8 *)&bios[0x7b];
         }
         for(i = 0; i < 3; i++) {
-                outSISIDXREG(SISSR, 0x31 + i, ptr[i]);
+                SiS_SetReg(SISSR, 0x31 + i, ptr[i]);
         }
 
         if(ivideo->chip == XGI_40) {
                 if(ivideo->revision_id == 2) {
-                        setSISIDXREG(SISSR, 0x3b, 0x3f, 0xc0);
+                        SiS_SetRegANDOR(SISSR, 0x3b, 0x3f, 0xc0);
                 }
-                outSISIDXREG(SISCR, 0x7d, 0xfe);
-                outSISIDXREG(SISCR, 0x7e, 0x0f);
+                SiS_SetReg(SISCR, 0x7d, 0xfe);
+                SiS_SetReg(SISCR, 0x7e, 0x0f);
         }
         if(ivideo->revision_id == 0) {  /* 40 *and* 20? */
-                andSISIDXREG(SISCR, 0x58, 0xd7);
-                inSISIDXREG(SISCR, 0xcb, reg);
+                SiS_SetRegAND(SISCR, 0x58, 0xd7);
+                reg = SiS_GetReg(SISCR, 0xcb);
                 if(reg & 0x20) {
-                        setSISIDXREG(SISCR, 0x58, 0xd7, (reg & 0x10) ? 0x08 : 0x20); /* =0x28 Z7 ? */
+                        SiS_SetRegANDOR(SISCR, 0x58, 0xd7, (reg & 0x10) ? 0x08 : 0x20); /* =0x28 Z7 ? */
                 }
         }
 
         reg = (ivideo->chip == XGI_40) ? 0x20 : 0x00;
-        setSISIDXREG(SISCR, 0x38, 0x1f, reg);
+        SiS_SetRegANDOR(SISCR, 0x38, 0x1f, reg);
 
         if(ivideo->chip == XGI_20) {
-                outSISIDXREG(SISSR, 0x36, 0x70);
+                SiS_SetReg(SISSR, 0x36, 0x70);
         } else {
-                outSISIDXREG(SISVID, 0x00, 0x86);
-                outSISIDXREG(SISVID, 0x32, 0x00);
-                outSISIDXREG(SISVID, 0x30, 0x00);
-                outSISIDXREG(SISVID, 0x32, 0x01);
-                outSISIDXREG(SISVID, 0x30, 0x00);
-                andSISIDXREG(SISVID, 0x2f, 0xdf);
-                andSISIDXREG(SISCAP, 0x00, 0x3f);
-
-                outSISIDXREG(SISPART1, 0x2f, 0x01);
-                outSISIDXREG(SISPART1, 0x00, 0x00);
-                outSISIDXREG(SISPART1, 0x02, bios[0x7e]);
-                outSISIDXREG(SISPART1, 0x2e, 0x08);
-                andSISIDXREG(SISPART1, 0x35, 0x7f);
-                andSISIDXREG(SISPART1, 0x50, 0xfe);
-
-                inSISIDXREG(SISPART4, 0x00, reg);
+                SiS_SetReg(SISVID, 0x00, 0x86);
+                SiS_SetReg(SISVID, 0x32, 0x00);
+                SiS_SetReg(SISVID, 0x30, 0x00);
+                SiS_SetReg(SISVID, 0x32, 0x01);
+                SiS_SetReg(SISVID, 0x30, 0x00);
+                SiS_SetRegAND(SISVID, 0x2f, 0xdf);
+                SiS_SetRegAND(SISCAP, 0x00, 0x3f);
+
+                SiS_SetReg(SISPART1, 0x2f, 0x01);
+                SiS_SetReg(SISPART1, 0x00, 0x00);
+                SiS_SetReg(SISPART1, 0x02, bios[0x7e]);
+                SiS_SetReg(SISPART1, 0x2e, 0x08);
+                SiS_SetRegAND(SISPART1, 0x35, 0x7f);
+                SiS_SetRegAND(SISPART1, 0x50, 0xfe);
+
+                reg = SiS_GetReg(SISPART4, 0x00);
                 if(reg == 1 || reg == 2) {
-                        outSISIDXREG(SISPART2, 0x00, 0x1c);
-                        outSISIDXREG(SISPART4, 0x0d, bios[0x7f]);
-                        outSISIDXREG(SISPART4, 0x0e, bios[0x80]);
-                        outSISIDXREG(SISPART4, 0x10, bios[0x81]);
-                        andSISIDXREG(SISPART4, 0x0f, 0x3f);
+                        SiS_SetReg(SISPART2, 0x00, 0x1c);
+                        SiS_SetReg(SISPART4, 0x0d, bios[0x7f]);
+                        SiS_SetReg(SISPART4, 0x0e, bios[0x80]);
+                        SiS_SetReg(SISPART4, 0x10, bios[0x81]);
+                        SiS_SetRegAND(SISPART4, 0x0f, 0x3f);
 
-                        inSISIDXREG(SISPART4, 0x01, reg);
+                        reg = SiS_GetReg(SISPART4, 0x01);
                         if((reg & 0xf0) >= 0xb0) {
-                                inSISIDXREG(SISPART4, 0x23, reg);
+                                reg = SiS_GetReg(SISPART4, 0x23);
                                 if(reg & 0x20) reg |= 0x40;
-                                outSISIDXREG(SISPART4, 0x23, reg);
+                                SiS_SetReg(SISPART4, 0x23, reg);
                                 reg = (reg & 0x20) ? 0x02 : 0x00;
-                                setSISIDXREG(SISPART1, 0x1e, 0xfd, reg);
+                                SiS_SetRegANDOR(SISPART1, 0x1e, 0xfd, reg);
                         }
                 }
 
                 v1 = bios[0x77];
 
-                inSISIDXREG(SISSR, 0x3b, reg);
+                reg = SiS_GetReg(SISSR, 0x3b);
                 if(reg & 0x02) {
-                        inSISIDXREG(SISSR, 0x3a, reg);
+                        reg = SiS_GetReg(SISSR, 0x3a);
                         v2 = (reg & 0x30) >> 3;
                         if(!(v2 & 0x04)) v2 ^= 0x02;
-                        inSISIDXREG(SISSR, 0x39, reg);
+                        reg = SiS_GetReg(SISSR, 0x39);
                         if(reg & 0x80) v2 |= 0x80;
                         v2 |= 0x01;
 
@@ -5168,36 +5332,36 @@ sisfb_post_xgi(struct pci_dev *pdev)
                                         v2 |= 0x08;
                                 }
                         }
-                        setSISIDXREG(SISCR, 0x5f, 0xf0, v2);
+                        SiS_SetRegANDOR(SISCR, 0x5f, 0xf0, v2);
                 }
-                outSISIDXREG(SISSR, 0x22, v1);
+                SiS_SetReg(SISSR, 0x22, v1);
 
                 if(ivideo->revision_id == 2) {
-                        inSISIDXREG(SISSR, 0x3b, v1);
-                        inSISIDXREG(SISSR, 0x3a, v2);
+                        v1 = SiS_GetReg(SISSR, 0x3b);
+                        v2 = SiS_GetReg(SISSR, 0x3a);
                         regd = bios[0x90 + 3] | (bios[0x90 + 4] << 8);
                         if( (!(v1 & 0x02)) && (v2 & 0x30) && (regd < 0xcf) )
-                                setSISIDXREG(SISCR, 0x5f, 0xf1, 0x01);
+                                SiS_SetRegANDOR(SISCR, 0x5f, 0xf1, 0x01);
 
                         if((mypdev = pci_get_device(0x10de, 0x01e0, NULL))) {
                                 /* TODO: set CR5f &0xf1 | 0x01 for version 6570
                                  * of nforce 2 ROM
                                  */
                                 if(0)
-                                        setSISIDXREG(SISCR, 0x5f, 0xf1, 0x01);
+                                        SiS_SetRegANDOR(SISCR, 0x5f, 0xf1, 0x01);
                                 pci_dev_put(mypdev);
                         }
                 }
 
                 v1 = 0x30;
-                inSISIDXREG(SISSR, 0x3b, reg);
-                inSISIDXREG(SISCR, 0x5f, v2);
+                reg = SiS_GetReg(SISSR, 0x3b);
+                v2 = SiS_GetReg(SISCR, 0x5f);
                 if((!(reg & 0x02)) && (v2 & 0x0e))
                         v1 |= 0x08;
-                outSISIDXREG(SISSR, 0x27, v1);
+                SiS_SetReg(SISSR, 0x27, v1);
 
                 if(bios[0x64] & 0x01) {
-                        setSISIDXREG(SISCR, 0x5f, 0xf0, bios[0x64]);
+                        SiS_SetRegANDOR(SISCR, 0x5f, 0xf0, bios[0x64]);
                 }
 
                 v1 = bios[0x4f7];
@@ -5205,45 +5369,59 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 regd = (regd >> 20) & 0x0f;
                 if(regd == 1) {
                         v1 &= 0xfc;
-                        orSISIDXREG(SISCR, 0x5f, 0x08);
-                }
-                outSISIDXREG(SISCR, 0x48, v1);
-
-                setSISIDXREG(SISCR, 0x47, 0x04, bios[0x4f6] & 0xfb);
-                setSISIDXREG(SISCR, 0x49, 0xf0, bios[0x4f8] & 0x0f);
-                setSISIDXREG(SISCR, 0x4a, 0x60, bios[0x4f9] & 0x9f);
-                setSISIDXREG(SISCR, 0x4b, 0x08, bios[0x4fa] & 0xf7);
-                setSISIDXREG(SISCR, 0x4c, 0x80, bios[0x4fb] & 0x7f);
-                outSISIDXREG(SISCR, 0x70, bios[0x4fc]);
-                setSISIDXREG(SISCR, 0x71, 0xf0, bios[0x4fd] & 0x0f);
-                outSISIDXREG(SISCR, 0x74, 0xd0);
-                setSISIDXREG(SISCR, 0x74, 0xcf, bios[0x4fe] & 0x30);
-                setSISIDXREG(SISCR, 0x75, 0xe0, bios[0x4ff] & 0x1f);
-                setSISIDXREG(SISCR, 0x76, 0xe0, bios[0x500] & 0x1f);
+                        SiS_SetRegOR(SISCR, 0x5f, 0x08);
+                }
+                SiS_SetReg(SISCR, 0x48, v1);
+
+                SiS_SetRegANDOR(SISCR, 0x47, 0x04, bios[0x4f6] & 0xfb);
+                SiS_SetRegANDOR(SISCR, 0x49, 0xf0, bios[0x4f8] & 0x0f);
+                SiS_SetRegANDOR(SISCR, 0x4a, 0x60, bios[0x4f9] & 0x9f);
+                SiS_SetRegANDOR(SISCR, 0x4b, 0x08, bios[0x4fa] & 0xf7);
+                SiS_SetRegANDOR(SISCR, 0x4c, 0x80, bios[0x4fb] & 0x7f);
+                SiS_SetReg(SISCR, 0x70, bios[0x4fc]);
+                SiS_SetRegANDOR(SISCR, 0x71, 0xf0, bios[0x4fd] & 0x0f);
+                SiS_SetReg(SISCR, 0x74, 0xd0);
+                SiS_SetRegANDOR(SISCR, 0x74, 0xcf, bios[0x4fe] & 0x30);
+                SiS_SetRegANDOR(SISCR, 0x75, 0xe0, bios[0x4ff] & 0x1f);
+                SiS_SetRegANDOR(SISCR, 0x76, 0xe0, bios[0x500] & 0x1f);
                 v1 = bios[0x501];
                 if((mypdev = pci_get_device(0x8086, 0x2530, NULL))) {
                         v1 = 0xf0;
                         pci_dev_put(mypdev);
                 }
-                outSISIDXREG(SISCR, 0x77, v1);
+                SiS_SetReg(SISCR, 0x77, v1);
         }
 
-        /* RAM type */
-
-        regb = 0;       /* ! */
+        /* RAM type:
+         *
+         * 0 == DDR1, 1 == DDR2, 2..7 == reserved?
+         *
+         * The code seems to written so that regb should equal ramtype,
+         * however, so far it has been hardcoded to 0. Enable other values only
+         * on XGI Z9, as it passes the POST, and add a warning for others.
+         */
+        ramtype = sisfb_post_xgi_ramtype(ivideo);
+        if (!sisfb_xgi_is21(ivideo) && ramtype) {
+                dev_warn(&pdev->dev,
+                         "RAM type something else than expected: %d\n",
+                         ramtype);
+                regb = 0;
+        } else {
+                regb = ramtype;
+        }
 
         v1 = 0xff;
         if(ivideo->haveXGIROM) {
                 v1 = bios[0x140 + regb];
         }
-        outSISIDXREG(SISCR, 0x6d, v1);
+        SiS_SetReg(SISCR, 0x6d, v1);
 
         ptr = cs128;
         if(ivideo->haveXGIROM) {
                 ptr = (const u8 *)&bios[0x128];
         }
         for(i = 0, j = 0; i < 3; i++, j += 8) {
-                outSISIDXREG(SISCR, 0x68 + i, ptr[j + regb]);
+                SiS_SetReg(SISCR, 0x68 + i, ptr[j + regb]);
         }
 
         ptr  = cs31a;
@@ -5267,14 +5445,14 @@ sisfb_post_xgi(struct pci_dev *pdev)
                         if(regd & 0x01) reg |= 0x04;
                         if(regd & 0x02) reg |= 0x08;
                         regd >>= 2;
-                        outSISIDXREG(SISCR, rega, reg);
-                        inSISIDXREG(SISCR, rega, reg);
-                        inSISIDXREG(SISCR, rega, reg);
+                        SiS_SetReg(SISCR, rega, reg);
+                        reg = SiS_GetReg(SISCR, rega);
+                        reg = SiS_GetReg(SISCR, rega);
                         reg += 0x10;
                 }
         }
 
-        andSISIDXREG(SISCR, 0x6e, 0xfc);
+        SiS_SetRegAND(SISCR, 0x6e, 0xfc);
 
         ptr  = NULL;
         if(ivideo->haveXGIROM) {
@@ -5282,7 +5460,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 ptr  = (const u8 *)&bios[index];
         }
         for(i = 0; i < 4; i++) {
-                setSISIDXREG(SISCR, 0x6e, 0xfc, i);
+                SiS_SetRegANDOR(SISCR, 0x6e, 0xfc, i);
                 reg = 0x00;
                 for(j = 0; j < 2; j++) {
                         regd = 0;
@@ -5296,9 +5474,9 @@ sisfb_post_xgi(struct pci_dev *pdev)
                                 if(regd & 0x01) reg |= 0x01;
                                 if(regd & 0x02) reg |= 0x02;
                                 regd >>= 2;
-                                outSISIDXREG(SISCR, 0x6f, reg);
-                                inSISIDXREG(SISCR, 0x6f, reg);
-                                inSISIDXREG(SISCR, 0x6f, reg);
+                                SiS_SetReg(SISCR, 0x6f, reg);
+                                reg = SiS_GetReg(SISCR, 0x6f);
+                                reg = SiS_GetReg(SISCR, 0x6f);
                                 reg += 0x08;
                         }
                 }
@@ -5309,10 +5487,10 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 ptr  = (const u8 *)&bios[0x148];
         }
         for(i = 0, j = 0; i < 2; i++, j += 8) {
-                outSISIDXREG(SISCR, 0x80 + i, ptr[j + regb]);
+                SiS_SetReg(SISCR, 0x80 + i, ptr[j + regb]);
         }
 
-        andSISIDXREG(SISCR, 0x89, 0x8f);
+        SiS_SetRegAND(SISCR, 0x89, 0x8f);
 
         ptr  = cs45a;
         if(ivideo->haveXGIROM) {
@@ -5326,9 +5504,9 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 if(regd & 0x01) reg |= 0x01;
                 if(regd & 0x02) reg |= 0x02;
                 regd >>= 2;
-                outSISIDXREG(SISCR, 0x89, reg);
-                inSISIDXREG(SISCR, 0x89, reg);
-                inSISIDXREG(SISCR, 0x89, reg);
+                SiS_SetReg(SISCR, 0x89, reg);
+                reg = SiS_GetReg(SISCR, 0x89);
+                reg = SiS_GetReg(SISCR, 0x89);
                 reg += 0x10;
         }
 
@@ -5339,27 +5517,27 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 v3 = bios[0x120 + regb];
                 v4 = bios[0x1ca];
         }
-        outSISIDXREG(SISCR, 0x45, v1 & 0x0f);
-        outSISIDXREG(SISCR, 0x99, (v1 >> 4) & 0x07);
-        orSISIDXREG(SISCR, 0x40, v1 & 0x80);
-        outSISIDXREG(SISCR, 0x41, v2);
+        SiS_SetReg(SISCR, 0x45, v1 & 0x0f);
+        SiS_SetReg(SISCR, 0x99, (v1 >> 4) & 0x07);
+        SiS_SetRegOR(SISCR, 0x40, v1 & 0x80);
+        SiS_SetReg(SISCR, 0x41, v2);
 
         ptr  = cs170;
         if(ivideo->haveXGIROM) {
                 ptr  = (const u8 *)&bios[0x170];
         }
         for(i = 0, j = 0; i < 7; i++, j += 8) {
-                outSISIDXREG(SISCR, 0x90 + i, ptr[j + regb]);
+                SiS_SetReg(SISCR, 0x90 + i, ptr[j + regb]);
         }
 
-        outSISIDXREG(SISCR, 0x59, v3);
+        SiS_SetReg(SISCR, 0x59, v3);
 
         ptr  = cs1a8;
         if(ivideo->haveXGIROM) {
                 ptr  = (const u8 *)&bios[0x1a8];
         }
         for(i = 0, j = 0; i < 3; i++, j += 8) {
-                outSISIDXREG(SISCR, 0xc3 + i, ptr[j + regb]);
+                SiS_SetReg(SISCR, 0xc3 + i, ptr[j + regb]);
         }
 
         ptr  = cs100;
@@ -5367,53 +5545,31 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 ptr  = (const u8 *)&bios[0x100];
         }
         for(i = 0, j = 0; i < 2; i++, j += 8) {
-                outSISIDXREG(SISCR, 0x8a + i, ptr[j + regb]);
+                SiS_SetReg(SISCR, 0x8a + i, ptr[j + regb]);
         }
 
-        outSISIDXREG(SISCR, 0xcf, v4);
+        SiS_SetReg(SISCR, 0xcf, v4);
 
-        outSISIDXREG(SISCR, 0x83, 0x09);
-        outSISIDXREG(SISCR, 0x87, 0x00);
+        SiS_SetReg(SISCR, 0x83, 0x09);
+        SiS_SetReg(SISCR, 0x87, 0x00);
 
         if(ivideo->chip == XGI_40) {
                 if( (ivideo->revision_id == 1) ||
                     (ivideo->revision_id == 2) ) {
-                        outSISIDXREG(SISCR, 0x8c, 0x87);
+                        SiS_SetReg(SISCR, 0x8c, 0x87);
                 }
         }
 
-        outSISIDXREG(SISSR, 0x17, 0x00);
-        outSISIDXREG(SISSR, 0x1a, 0x87);
+        if (regb == 1)
+                SiS_SetReg(SISSR, 0x17, 0x80);          /* DDR2 */
+        else
+                SiS_SetReg(SISSR, 0x17, 0x00);          /* DDR1 */
+        SiS_SetReg(SISSR, 0x1a, 0x87);
 
         if(ivideo->chip == XGI_20) {
-                outSISIDXREG(SISSR, 0x15, 0x00);
-                outSISIDXREG(SISSR, 0x1c, 0x00);
-        }
-
-        ramtype = 0x00; v1 = 0x10;
-        if(ivideo->haveXGIROM) {
-                ramtype = bios[0x62];
-                v1 = bios[0x1d2];
+                SiS_SetReg(SISSR, 0x15, 0x00);
+                SiS_SetReg(SISSR, 0x1c, 0x00);
         }
-        if(!(ramtype & 0x80)) {
-                if(ivideo->chip == XGI_20) {
-                        outSISIDXREG(SISCR, 0x97, v1);
-                        inSISIDXREG(SISCR, 0x97, reg);
-                        if(reg & 0x10) {
-                                ramtype = (reg & 0x01) << 1;
-                        }
-                } else {
-                        inSISIDXREG(SISSR, 0x39, reg);
-                        ramtype = reg & 0x02;
-                        if(!(ramtype)) {
-                                inSISIDXREG(SISSR, 0x3a, reg);
-                                ramtype = (reg >> 1) & 0x01;
-                        }
-                }
-        }
-        ramtype &= 0x07;
-
-        regb = 0;       /* ! */
 
         switch(ramtype) {
         case 0:
@@ -5427,55 +5583,55 @@ sisfb_post_xgi(struct pci_dev *pdev)
                                 v2 = bios[regb + 0x160];
                                 v3 = bios[regb + 0x168];
                         }
-                        outSISIDXREG(SISCR, 0x82, v1);
-                        outSISIDXREG(SISCR, 0x85, v2);
-                        outSISIDXREG(SISCR, 0x86, v3);
+                        SiS_SetReg(SISCR, 0x82, v1);
+                        SiS_SetReg(SISCR, 0x85, v2);
+                        SiS_SetReg(SISCR, 0x86, v3);
                 } else {
-                        outSISIDXREG(SISCR, 0x82, 0x88);
-                        outSISIDXREG(SISCR, 0x86, 0x00);
-                        inSISIDXREG(SISCR, 0x86, reg);
-                        outSISIDXREG(SISCR, 0x86, 0x88);
-                        inSISIDXREG(SISCR, 0x86, reg);
-                        outSISIDXREG(SISCR, 0x86, bios[regb + 0x168]);
-                        outSISIDXREG(SISCR, 0x82, 0x77);
-                        outSISIDXREG(SISCR, 0x85, 0x00);
-                        inSISIDXREG(SISCR, 0x85, reg);
-                        outSISIDXREG(SISCR, 0x85, 0x88);
-                        inSISIDXREG(SISCR, 0x85, reg);
-                        outSISIDXREG(SISCR, 0x85, bios[regb + 0x160]);
-                        outSISIDXREG(SISCR, 0x82, bios[regb + 0x158]);
+                        SiS_SetReg(SISCR, 0x82, 0x88);
+                        SiS_SetReg(SISCR, 0x86, 0x00);
+                        reg = SiS_GetReg(SISCR, 0x86);
+                        SiS_SetReg(SISCR, 0x86, 0x88);
+                        reg = SiS_GetReg(SISCR, 0x86);
+                        SiS_SetReg(SISCR, 0x86, bios[regb + 0x168]);
+                        SiS_SetReg(SISCR, 0x82, 0x77);
+                        SiS_SetReg(SISCR, 0x85, 0x00);
+                        reg = SiS_GetReg(SISCR, 0x85);
+                        SiS_SetReg(SISCR, 0x85, 0x88);
+                        reg = SiS_GetReg(SISCR, 0x85);
+                        SiS_SetReg(SISCR, 0x85, bios[regb + 0x160]);
+                        SiS_SetReg(SISCR, 0x82, bios[regb + 0x158]);
                 }
                 if(ivideo->chip == XGI_40) {
-                        outSISIDXREG(SISCR, 0x97, 0x00);
+                        SiS_SetReg(SISCR, 0x97, 0x00);
                 }
-                outSISIDXREG(SISCR, 0x98, 0x01);
-                outSISIDXREG(SISCR, 0x9a, 0x02);
+                SiS_SetReg(SISCR, 0x98, 0x01);
+                SiS_SetReg(SISCR, 0x9a, 0x02);
 
-                outSISIDXREG(SISSR, 0x18, 0x01);
+                SiS_SetReg(SISSR, 0x18, 0x01);
                 if((ivideo->chip == XGI_20) ||
                    (ivideo->revision_id == 2)) {
-                        outSISIDXREG(SISSR, 0x19, 0x40);
+                        SiS_SetReg(SISSR, 0x19, 0x40);
                 } else {
-                        outSISIDXREG(SISSR, 0x19, 0x20);
+                        SiS_SetReg(SISSR, 0x19, 0x20);
                 }
-                outSISIDXREG(SISSR, 0x16, 0x00);
-                outSISIDXREG(SISSR, 0x16, 0x80);
+                SiS_SetReg(SISSR, 0x16, 0x00);
+                SiS_SetReg(SISSR, 0x16, 0x80);
                 if((ivideo->chip == XGI_20) || (bios[0x1cb] != 0x0c)) {
                         sisfb_post_xgi_delay(ivideo, 0x43);
                         sisfb_post_xgi_delay(ivideo, 0x43);
                         sisfb_post_xgi_delay(ivideo, 0x43);
-                        outSISIDXREG(SISSR, 0x18, 0x00);
+                        SiS_SetReg(SISSR, 0x18, 0x00);
                         if((ivideo->chip == XGI_20) ||
                            (ivideo->revision_id == 2)) {
-                                outSISIDXREG(SISSR, 0x19, 0x40);
+                                SiS_SetReg(SISSR, 0x19, 0x40);
                         } else {
-                                outSISIDXREG(SISSR, 0x19, 0x20);
+                                SiS_SetReg(SISSR, 0x19, 0x20);
                         }
                 } else if((ivideo->chip == XGI_40) && (bios[0x1cb] == 0x0c)) {
-                        /* outSISIDXREG(SISSR, 0x16, 0x0c); */ /* ? */
+                        /* SiS_SetReg(SISSR, 0x16, 0x0c); */ /* ? */
                 }
-                outSISIDXREG(SISSR, 0x16, 0x00);
-                outSISIDXREG(SISSR, 0x16, 0x80);
+                SiS_SetReg(SISSR, 0x16, 0x00);
+                SiS_SetReg(SISSR, 0x16, 0x80);
                 sisfb_post_xgi_delay(ivideo, 4);
                 v1 = 0x31; v2 = 0x03; v3 = 0x83; v4 = 0x03; v5 = 0x83;
                 if(ivideo->haveXGIROM) {
@@ -5486,160 +5642,106 @@ sisfb_post_xgi(struct pci_dev *pdev)
                         v4 = bios[index + 2];
                         v5 = bios[index + 3];
                 }
-                outSISIDXREG(SISSR, 0x18, v1);
-                outSISIDXREG(SISSR, 0x19, ((ivideo->chip == XGI_20) ? 0x02 : 0x01));
-                outSISIDXREG(SISSR, 0x16, v2);
-                outSISIDXREG(SISSR, 0x16, v3);
+                SiS_SetReg(SISSR, 0x18, v1);
+                SiS_SetReg(SISSR, 0x19, ((ivideo->chip == XGI_20) ? 0x02 : 0x01));
+                SiS_SetReg(SISSR, 0x16, v2);
+                SiS_SetReg(SISSR, 0x16, v3);
                 sisfb_post_xgi_delay(ivideo, 0x43);
-                outSISIDXREG(SISSR, 0x1b, 0x03);
+                SiS_SetReg(SISSR, 0x1b, 0x03);
                 sisfb_post_xgi_delay(ivideo, 0x22);
-                outSISIDXREG(SISSR, 0x18, v1);
-                outSISIDXREG(SISSR, 0x19, 0x00);
-                outSISIDXREG(SISSR, 0x16, v4);
-                outSISIDXREG(SISSR, 0x16, v5);
-                outSISIDXREG(SISSR, 0x1b, 0x00);
+                SiS_SetReg(SISSR, 0x18, v1);
+                SiS_SetReg(SISSR, 0x19, 0x00);
+                SiS_SetReg(SISSR, 0x16, v4);
+                SiS_SetReg(SISSR, 0x16, v5);
+                SiS_SetReg(SISSR, 0x1b, 0x00);
                 break;
         case 1:
-                outSISIDXREG(SISCR, 0x82, 0x77);
-                outSISIDXREG(SISCR, 0x86, 0x00);
-                inSISIDXREG(SISCR, 0x86, reg);
-                outSISIDXREG(SISCR, 0x86, 0x88);
-                inSISIDXREG(SISCR, 0x86, reg);
-                v1 = cs168[regb]; v2 = cs160[regb]; v3 = cs158[regb];
-                if(ivideo->haveXGIROM) {
-                        v1 = bios[regb + 0x168];
-                        v2 = bios[regb + 0x160];
-                        v3 = bios[regb + 0x158];
-                }
-                outSISIDXREG(SISCR, 0x86, v1);
-                outSISIDXREG(SISCR, 0x82, 0x77);
-                outSISIDXREG(SISCR, 0x85, 0x00);
-                inSISIDXREG(SISCR, 0x85, reg);
-                outSISIDXREG(SISCR, 0x85, 0x88);
-                inSISIDXREG(SISCR, 0x85, reg);
-                outSISIDXREG(SISCR, 0x85, v2);
-                outSISIDXREG(SISCR, 0x82, v3);
-                outSISIDXREG(SISCR, 0x98, 0x01);
-                outSISIDXREG(SISCR, 0x9a, 0x02);
-
-                outSISIDXREG(SISSR, 0x28, 0x64);
-                outSISIDXREG(SISSR, 0x29, 0x63);
-                sisfb_post_xgi_delay(ivideo, 15);
-                outSISIDXREG(SISSR, 0x18, 0x00);
-                outSISIDXREG(SISSR, 0x19, 0x20);
-                outSISIDXREG(SISSR, 0x16, 0x00);
-                outSISIDXREG(SISSR, 0x16, 0x80);
-                outSISIDXREG(SISSR, 0x18, 0xc5);
-                outSISIDXREG(SISSR, 0x19, 0x23);
-                outSISIDXREG(SISSR, 0x16, 0x00);
-                outSISIDXREG(SISSR, 0x16, 0x80);
-                sisfb_post_xgi_delay(ivideo, 1);
-                outSISIDXREG(SISCR, 0x97,0x11);
-                sisfb_post_xgi_setclocks(ivideo, regb);
-                sisfb_post_xgi_delay(ivideo, 0x46);
-                outSISIDXREG(SISSR, 0x18, 0xc5);
-                outSISIDXREG(SISSR, 0x19, 0x23);
-                outSISIDXREG(SISSR, 0x16, 0x00);
-                outSISIDXREG(SISSR, 0x16, 0x80);
-                sisfb_post_xgi_delay(ivideo, 1);
-                outSISIDXREG(SISSR, 0x1b, 0x04);
-                sisfb_post_xgi_delay(ivideo, 1);
-                outSISIDXREG(SISSR, 0x1b, 0x00);
-                sisfb_post_xgi_delay(ivideo, 1);
-                v1 = 0x31;
-                if(ivideo->haveXGIROM) {
-                        v1 = bios[0xf0];
-                }
-                outSISIDXREG(SISSR, 0x18, v1);
-                outSISIDXREG(SISSR, 0x19, 0x06);
-                outSISIDXREG(SISSR, 0x16, 0x04);
-                outSISIDXREG(SISSR, 0x16, 0x84);
-                sisfb_post_xgi_delay(ivideo, 1);
+                sisfb_post_xgi_ddr2(ivideo, regb);
                 break;
         default:
                 sisfb_post_xgi_setclocks(ivideo, regb);
                 if((ivideo->chip == XGI_40) &&
                    ((ivideo->revision_id == 1) ||
                     (ivideo->revision_id == 2))) {
-                        outSISIDXREG(SISCR, 0x82, bios[regb + 0x158]);
-                        outSISIDXREG(SISCR, 0x85, bios[regb + 0x160]);
-                        outSISIDXREG(SISCR, 0x86, bios[regb + 0x168]);
+                        SiS_SetReg(SISCR, 0x82, bios[regb + 0x158]);
+                        SiS_SetReg(SISCR, 0x85, bios[regb + 0x160]);
+                        SiS_SetReg(SISCR, 0x86, bios[regb + 0x168]);
                 } else {
-                        outSISIDXREG(SISCR, 0x82, 0x88);
-                        outSISIDXREG(SISCR, 0x86, 0x00);
-                        inSISIDXREG(SISCR, 0x86, reg);
-                        outSISIDXREG(SISCR, 0x86, 0x88);
-                        outSISIDXREG(SISCR, 0x82, 0x77);
-                        outSISIDXREG(SISCR, 0x85, 0x00);
-                        inSISIDXREG(SISCR, 0x85, reg);
-                        outSISIDXREG(SISCR, 0x85, 0x88);
-                        inSISIDXREG(SISCR, 0x85, reg);
+                        SiS_SetReg(SISCR, 0x82, 0x88);
+                        SiS_SetReg(SISCR, 0x86, 0x00);
+                        reg = SiS_GetReg(SISCR, 0x86);
+                        SiS_SetReg(SISCR, 0x86, 0x88);
+                        SiS_SetReg(SISCR, 0x82, 0x77);
+                        SiS_SetReg(SISCR, 0x85, 0x00);
+                        reg = SiS_GetReg(SISCR, 0x85);
+                        SiS_SetReg(SISCR, 0x85, 0x88);
+                        reg = SiS_GetReg(SISCR, 0x85);
                         v1 = cs160[regb]; v2 = cs158[regb];
                         if(ivideo->haveXGIROM) {
                                 v1 = bios[regb + 0x160];
                                 v2 = bios[regb + 0x158];
                         }
-                        outSISIDXREG(SISCR, 0x85, v1);
-                        outSISIDXREG(SISCR, 0x82, v2);
+                        SiS_SetReg(SISCR, 0x85, v1);
+                        SiS_SetReg(SISCR, 0x82, v2);
                 }
                 if(ivideo->chip == XGI_40) {
-                        outSISIDXREG(SISCR, 0x97, 0x11);
+                        SiS_SetReg(SISCR, 0x97, 0x11);
                 }
                 if((ivideo->chip == XGI_40) && (ivideo->revision_id == 2)) {
-                        outSISIDXREG(SISCR, 0x98, 0x01);
+                        SiS_SetReg(SISCR, 0x98, 0x01);
                 } else {
-                        outSISIDXREG(SISCR, 0x98, 0x03);
+                        SiS_SetReg(SISCR, 0x98, 0x03);
                 }
-                outSISIDXREG(SISCR, 0x9a, 0x02);
+                SiS_SetReg(SISCR, 0x9a, 0x02);
 
                 if(ivideo->chip == XGI_40) {
-                        outSISIDXREG(SISSR, 0x18, 0x01);
+                        SiS_SetReg(SISSR, 0x18, 0x01);
                 } else {
-                        outSISIDXREG(SISSR, 0x18, 0x00);
+                        SiS_SetReg(SISSR, 0x18, 0x00);
                 }
-                outSISIDXREG(SISSR, 0x19, 0x40);
-                outSISIDXREG(SISSR, 0x16, 0x00);
-                outSISIDXREG(SISSR, 0x16, 0x80);
+                SiS_SetReg(SISSR, 0x19, 0x40);
+                SiS_SetReg(SISSR, 0x16, 0x00);
+                SiS_SetReg(SISSR, 0x16, 0x80);
                 if((ivideo->chip == XGI_40) && (bios[0x1cb] != 0x0c)) {
                         sisfb_post_xgi_delay(ivideo, 0x43);
                         sisfb_post_xgi_delay(ivideo, 0x43);
                         sisfb_post_xgi_delay(ivideo, 0x43);
-                        outSISIDXREG(SISSR, 0x18, 0x00);
-                        outSISIDXREG(SISSR, 0x19, 0x40);
-                        outSISIDXREG(SISSR, 0x16, 0x00);
-                        outSISIDXREG(SISSR, 0x16, 0x80);
+                        SiS_SetReg(SISSR, 0x18, 0x00);
+                        SiS_SetReg(SISSR, 0x19, 0x40);
+                        SiS_SetReg(SISSR, 0x16, 0x00);
+                        SiS_SetReg(SISSR, 0x16, 0x80);
                 }
                 sisfb_post_xgi_delay(ivideo, 4);
                 v1 = 0x31;
                 if(ivideo->haveXGIROM) {
                         v1 = bios[0xf0];
                 }
-                outSISIDXREG(SISSR, 0x18, v1);
-                outSISIDXREG(SISSR, 0x19, 0x01);
+                SiS_SetReg(SISSR, 0x18, v1);
+                SiS_SetReg(SISSR, 0x19, 0x01);
                 if(ivideo->chip == XGI_40) {
-                        outSISIDXREG(SISSR, 0x16, bios[0x53e]);
-                        outSISIDXREG(SISSR, 0x16, bios[0x53f]);
+                        SiS_SetReg(SISSR, 0x16, bios[0x53e]);
+                        SiS_SetReg(SISSR, 0x16, bios[0x53f]);
                 } else {
-                        outSISIDXREG(SISSR, 0x16, 0x05);
-                        outSISIDXREG(SISSR, 0x16, 0x85);
+                        SiS_SetReg(SISSR, 0x16, 0x05);
+                        SiS_SetReg(SISSR, 0x16, 0x85);
                 }
                 sisfb_post_xgi_delay(ivideo, 0x43);
                 if(ivideo->chip == XGI_40) {
-                        outSISIDXREG(SISSR, 0x1b, 0x01);
+                        SiS_SetReg(SISSR, 0x1b, 0x01);
                 } else {
-                        outSISIDXREG(SISSR, 0x1b, 0x03);
+                        SiS_SetReg(SISSR, 0x1b, 0x03);
                 }
                 sisfb_post_xgi_delay(ivideo, 0x22);
-                outSISIDXREG(SISSR, 0x18, v1);
-                outSISIDXREG(SISSR, 0x19, 0x00);
+                SiS_SetReg(SISSR, 0x18, v1);
+                SiS_SetReg(SISSR, 0x19, 0x00);
                 if(ivideo->chip == XGI_40) {
-                        outSISIDXREG(SISSR, 0x16, bios[0x540]);
-                        outSISIDXREG(SISSR, 0x16, bios[0x541]);
+                        SiS_SetReg(SISSR, 0x16, bios[0x540]);
+                        SiS_SetReg(SISSR, 0x16, bios[0x541]);
                 } else {
-                        outSISIDXREG(SISSR, 0x16, 0x05);
-                        outSISIDXREG(SISSR, 0x16, 0x85);
+                        SiS_SetReg(SISSR, 0x16, 0x05);
+                        SiS_SetReg(SISSR, 0x16, 0x85);
                 }
-                outSISIDXREG(SISSR, 0x1b, 0x00);
+                SiS_SetReg(SISSR, 0x1b, 0x00);
         }
 
         regb = 0;       /* ! */
@@ -5647,7 +5749,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
         if(ivideo->haveXGIROM) {
                 v1 = bios[0x110 + regb];
         }
-        outSISIDXREG(SISSR, 0x1b, v1);
+        SiS_SetReg(SISSR, 0x1b, v1);
 
         /* RAM size */
         v1 = 0x00; v2 = 0x00;
@@ -5659,10 +5761,11 @@ sisfb_post_xgi(struct pci_dev *pdev)
         regd = 1 << regb;
         if((v1 & 0x40) && (v2 & regd) && ivideo->haveXGIROM) {
 
-                outSISIDXREG(SISSR, 0x13, bios[regb + 0xe0]);
-                outSISIDXREG(SISSR, 0x14, bios[regb + 0xe0 + 8]);
+                SiS_SetReg(SISSR, 0x13, bios[regb + 0xe0]);
+                SiS_SetReg(SISSR, 0x14, bios[regb + 0xe0 + 8]);
 
         } else {
+                int err;
 
                 /* Set default mode, don't clear screen */
                 ivideo->SiS_Pr.SiS_UseOEM = false;
@@ -5672,24 +5775,30 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 ivideo->SiS_Pr.VideoMemorySize = 8 << 20;
                 SiSSetMode(&ivideo->SiS_Pr, 0x2e | 0x80);
 
-                outSISIDXREG(SISSR, 0x05, 0x86);
+                SiS_SetReg(SISSR, 0x05, 0x86);
 
                 /* Disable read-cache */
-                andSISIDXREG(SISSR, 0x21, 0xdf);
-                sisfb_post_xgi_ramsize(ivideo);
+                SiS_SetRegAND(SISSR, 0x21, 0xdf);
+                err = sisfb_post_xgi_ramsize(ivideo);
                 /* Enable read-cache */
-                orSISIDXREG(SISSR, 0x21, 0x20);
+                SiS_SetRegOR(SISSR, 0x21, 0x20);
 
+                if (err) {
+                        dev_err(&pdev->dev,
+                                "%s: RAM size detection failed: %d\n",
+                                __func__, err);
+                        return 0;
+                }
         }
 
 #if 0
         printk(KERN_DEBUG "-----------------\n");
         for(i = 0; i < 0xff; i++) {
-                inSISIDXREG(SISCR, i, reg);
+                reg = SiS_GetReg(SISCR, i);
                 printk(KERN_DEBUG "CR%02x(%x) = 0x%02x\n", i, SISCR, reg);
         }
         for(i = 0; i < 0x40; i++) {
-                inSISIDXREG(SISSR, i, reg);
+                reg = SiS_GetReg(SISSR, i);
                 printk(KERN_DEBUG "SR%02x(%x) = 0x%02x\n", i, SISSR, reg);
         }
         printk(KERN_DEBUG "-----------------\n");
@@ -5697,13 +5806,13 @@ sisfb_post_xgi(struct pci_dev *pdev)
 
         /* Sense CRT1 */
         if(ivideo->chip == XGI_20) {
-                orSISIDXREG(SISCR, 0x32, 0x20);
+                SiS_SetRegOR(SISCR, 0x32, 0x20);
         } else {
-                inSISIDXREG(SISPART4, 0x00, reg);
+                reg = SiS_GetReg(SISPART4, 0x00);
                 if((reg == 1) || (reg == 2)) {
                         sisfb_sense_crt1(ivideo);
                 } else {
-                        orSISIDXREG(SISCR, 0x32, 0x20);
+                        SiS_SetRegOR(SISCR, 0x32, 0x20);
                 }
         }
 
@@ -5714,20 +5823,20 @@ sisfb_post_xgi(struct pci_dev *pdev)
         ivideo->curFSTN = ivideo->curDSTN = 0;
         SiSSetMode(&ivideo->SiS_Pr, 0x2e | 0x80);
 
-        outSISIDXREG(SISSR, 0x05, 0x86);
+        SiS_SetReg(SISSR, 0x05, 0x86);
 
         /* Display off */
-        orSISIDXREG(SISSR, 0x01, 0x20);
+        SiS_SetRegOR(SISSR, 0x01, 0x20);
 
         /* Save mode number in CR34 */
-        outSISIDXREG(SISCR, 0x34, 0x2e);
+        SiS_SetReg(SISCR, 0x34, 0x2e);
 
         /* Let everyone know what the current mode is */
         ivideo->modeprechange = 0x2e;
 
         if(ivideo->chip == XGI_40) {
-                inSISIDXREG(SISCR, 0xca, reg);
-                inSISIDXREG(SISCR, 0xcc, v1);
+                reg = SiS_GetReg(SISCR, 0xca);
+                v1 = SiS_GetReg(SISCR, 0xcc);
                 if((reg & 0x10) && (!(v1 & 0x04))) {
                         printk(KERN_ERR
                                 "sisfb: Please connect power to the card.\n");
@@ -5792,6 +5901,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 #endif
 
         ivideo->chip = chipinfo->chip;
+        ivideo->chip_real_id = chipinfo->chip;
         ivideo->sisvga_engine = chipinfo->vgaengine;
         ivideo->hwcursor_size = chipinfo->hwcursor_size;
         ivideo->CRT2_write_enable = chipinfo->CRT2_write_enable;
@@ -5936,6 +6046,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         }
 
         ivideo->video_base = pci_resource_start(pdev, 0);
+        ivideo->video_size = pci_resource_len(pdev, 0);
         ivideo->mmio_base  = pci_resource_start(pdev, 1);
         ivideo->mmio_size  = pci_resource_len(pdev, 1);
         ivideo->SiS_Pr.RelIO = pci_resource_start(pdev, 2) + 0x30;
@@ -5969,7 +6080,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         }
 #endif
 
-        outSISIDXREG(SISSR, 0x05, 0x86);
+        SiS_SetReg(SISSR, 0x05, 0x86);
 
         if( (!ivideo->sisvga_enabled)
 #if !defined(__i386__) && !defined(__x86_64__)
@@ -5977,13 +6088,13 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 #endif
                                                    ) {
                 for(i = 0x30; i <= 0x3f; i++) {
-                        outSISIDXREG(SISCR, i, 0x00);
+                        SiS_SetReg(SISCR, i, 0x00);
                 }
         }
 
         /* Find out about current video mode */
         ivideo->modeprechange = 0x03;
-        inSISIDXREG(SISCR, 0x34, reg);
+        reg = SiS_GetReg(SISCR, 0x34);
         if(reg & 0x7f) {
                 ivideo->modeprechange = reg & 0x7f;
         } else if(ivideo->sisvga_enabled) {
@@ -6024,6 +6135,18 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 sisfb_detect_custom_timing(ivideo);
         }
 
+#ifdef CONFIG_FB_SIS_315
+        if (ivideo->chip == XGI_20) {
+                /* Check if our Z7 chip is actually Z9 */
+                SiS_SetRegOR(SISCR, 0x4a, 0x40);        /* GPIOG EN */
+                reg = SiS_GetReg(SISCR, 0x48);
+                if (reg & 0x02) {                       /* GPIOG */
+                        ivideo->chip_real_id = XGI_21;
+                        dev_info(&pdev->dev, "Z9 detected\n");
+                }
+        }
+#endif
+
         /* POST card in case this has not been done by the BIOS */
         if( (!ivideo->sisvga_enabled)
 #if !defined(__i386__) && !defined(__x86_64__)
@@ -6080,9 +6203,9 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         if((ivideo->sisfb_mode_idx < 0) ||
            ((sisbios_mode[ivideo->sisfb_mode_idx].mode_no[ivideo->mni]) != 0xFF)) {
                 /* Enable PCI_LINEAR_ADDRESSING and MMIO_ENABLE  */
-                orSISIDXREG(SISSR, IND_SIS_PCI_ADDRESS_SET, (SIS_PCI_ADDR_ENABLE | SIS_MEM_MAP_IO_ENABLE));
+                SiS_SetRegOR(SISSR, IND_SIS_PCI_ADDRESS_SET, (SIS_PCI_ADDR_ENABLE | SIS_MEM_MAP_IO_ENABLE));
                 /* Enable 2D accelerator engine */
-                orSISIDXREG(SISSR, IND_SIS_MODULE_ENABLE, SIS_ENABLE_2D);
+                SiS_SetRegOR(SISSR, IND_SIS_MODULE_ENABLE, SIS_ENABLE_2D);
         }
 
         if(sisfb_pdc != 0xff) {
diff --git a/drivers/video/sis/vgatypes.h b/drivers/video/sis/vgatypes.h
index 81a22eaabfde..e3f9976cfef0 100644
--- a/drivers/video/sis/vgatypes.h
+++ b/drivers/video/sis/vgatypes.h
@@ -55,21 +55,10 @@
 
 #define SISIOMEMTYPE
 
-#ifdef SIS_LINUX_KERNEL
 typedef unsigned long SISIOADDRESS;
 #include <linux/types.h>  /* Need __iomem */
 #undef SISIOMEMTYPE
 #define SISIOMEMTYPE __iomem
-#endif
-
-#ifdef SIS_XORG_XF86
-#if XF86_VERSION_CURRENT < XF86_VERSION_NUMERIC(4,2,0,0,0)
-typedef unsigned long IOADDRESS;
-typedef unsigned long SISIOADDRESS;
-#else
-typedef IOADDRESS SISIOADDRESS;
-#endif
-#endif
 
 typedef enum _SIS_CHIP_TYPE {
     SIS_VGALegacy = 0,
@@ -98,6 +87,7 @@ typedef enum _SIS_CHIP_TYPE {
     SIS_341,
     SIS_342,
     XGI_20  = 75,
+    XGI_21,
     XGI_40,
     MAX_SIS_CHIP
 } SIS_CHIP_TYPE;
diff --git a/drivers/video/sis/vstruct.h b/drivers/video/sis/vstruct.h
index bef4aae388d0..ea94d214dcff 100644
--- a/drivers/video/sis/vstruct.h
+++ b/drivers/video/sis/vstruct.h
@@ -233,24 +233,15 @@ struct SiS_Private
 {
         unsigned char                   ChipType;
         unsigned char                   ChipRevision;
-#ifdef SIS_XORG_XF86
-        PCITAG                          PciTag;
-#endif
-#ifdef SIS_LINUX_KERNEL
         void                            *ivideo;
-#endif
         unsigned char                   *VirtualRomBase;
         bool                            UseROM;
-#ifdef SIS_LINUX_KERNEL
         unsigned char SISIOMEMTYPE      *VideoMemoryAddress;
         unsigned int                    VideoMemorySize;
-#endif
         SISIOADDRESS                    IOAddress;
         SISIOADDRESS                    IOAddress2;  /* For dual chip XGI volari */
 
-#ifdef SIS_LINUX_KERNEL
         SISIOADDRESS                    RelIO;
-#endif
         SISIOADDRESS                    SiS_P3c4;
         SISIOADDRESS                    SiS_P3d4;
         SISIOADDRESS                    SiS_P3c0;
@@ -280,9 +271,6 @@ struct SiS_Private
         unsigned short                  SiS_IF_DEF_FSTN;
         unsigned short                  SiS_SysFlags;
         unsigned char                   SiS_VGAINFO;
-#ifdef SIS_XORG_XF86
-        unsigned short                  SiS_CP1, SiS_CP2, SiS_CP3, SiS_CP4;
-#endif
         bool                            SiS_UseROM;
         bool                            SiS_ROMNew;
         bool                            SiS_XGIROM;
diff --git a/drivers/video/sm501fb.c b/drivers/video/sm501fb.c
index b7dc1800efa9..6294dca95500 100644
--- a/drivers/video/sm501fb.c
+++ b/drivers/video/sm501fb.c
@@ -41,6 +41,26 @@
 #include <linux/sm501.h>
 #include <linux/sm501-regs.h>
 
+#include "edid.h"
+
+static char *fb_mode = "640x480-16@60";
+static unsigned long default_bpp = 16;
+
+static struct fb_videomode __devinitdata sm501_default_mode = {
+        .refresh        = 60,
+        .xres           = 640,
+        .yres           = 480,
+        .pixclock       = 20833,
+        .left_margin    = 142,
+        .right_margin   = 13,
+        .upper_margin   = 21,
+        .lower_margin   = 1,
+        .hsync_len      = 69,
+        .vsync_len      = 3,
+        .sync           = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+        .vmode          = FB_VMODE_NONINTERLACED
+};
+
 #define NR_PALETTE      256
 
 enum sm501_controller {
@@ -77,6 +97,7 @@ struct sm501fb_info {
         void __iomem            *regs2d;        /* 2d remapped registers */
         void __iomem            *fbmem;         /* remapped framebuffer */
         size_t                   fbmem_len;     /* length of remapped region */
+        u8 *edid_data;
 };
 
 /* per-framebuffer private data */
@@ -117,7 +138,7 @@ static inline int v_total(struct fb_var_screeninfo *var)
 
 static inline void sm501fb_sync_regs(struct sm501fb_info *info)
 {
-        readl(info->regs);
+        smc501_readl(info->regs);
 }
 
 /* sm501_alloc_mem
@@ -244,7 +265,7 @@ static unsigned long sm501fb_ps_to_hz(unsigned long psvalue)
         return (unsigned long)numerator;
 }
 
-/* sm501fb_hz_to_ps is identical to the oposite transform */
+/* sm501fb_hz_to_ps is identical to the opposite transform */
 
 #define sm501fb_hz_to_ps(x) sm501fb_ps_to_hz(x)
 
@@ -262,7 +283,7 @@ static void sm501fb_setup_gamma(struct sm501fb_info *fbi,
 
         /* set gamma values */
         for (offset = 0; offset < 256 * 4; offset += 4) {
-                writel(value, fbi->regs + palette + offset);
+                smc501_writel(value, fbi->regs + palette + offset);
                 value += 0x010101;      /* Advance RGB by 1,1,1.*/
         }
 }
@@ -476,7 +497,8 @@ static int sm501fb_set_par_common(struct fb_info *info,
 
         /* set start of framebuffer to the screen */
 
-        writel(par->screen.sm_addr | SM501_ADDR_FLIP, fbi->regs + head_addr);
+        smc501_writel(par->screen.sm_addr | SM501_ADDR_FLIP,
+                        fbi->regs + head_addr);
 
         /* program CRT clock  */
 
@@ -519,7 +541,7 @@ static void sm501fb_set_par_geometry(struct fb_info *info,
         reg = info->fix.line_length;
         reg |= ((var->xres * var->bits_per_pixel)/8) << 16;
 
-        writel(reg, fbi->regs + (par->head == HEAD_CRT ?
+        smc501_writel(reg, fbi->regs + (par->head == HEAD_CRT ?
                     SM501_DC_CRT_FB_OFFSET :  SM501_DC_PANEL_FB_OFFSET));
 
         /* program horizontal total */
@@ -527,27 +549,27 @@ static void sm501fb_set_par_geometry(struct fb_info *info,
         reg  = (h_total(var) - 1) << 16;
         reg |= (var->xres - 1);
 
-        writel(reg, base + SM501_OFF_DC_H_TOT);
+        smc501_writel(reg, base + SM501_OFF_DC_H_TOT);
 
         /* program horizontal sync */
 
         reg  = var->hsync_len << 16;
         reg |= var->xres + var->right_margin - 1;
 
-        writel(reg, base + SM501_OFF_DC_H_SYNC);
+        smc501_writel(reg, base + SM501_OFF_DC_H_SYNC);
 
         /* program vertical total */
 
         reg  = (v_total(var) - 1) << 16;
         reg |= (var->yres - 1);
 
-        writel(reg, base + SM501_OFF_DC_V_TOT);
+        smc501_writel(reg, base + SM501_OFF_DC_V_TOT);
 
         /* program vertical sync */
         reg  = var->vsync_len << 16;
         reg |= var->yres + var->lower_margin - 1;
 
-        writel(reg, base + SM501_OFF_DC_V_SYNC);
+        smc501_writel(reg, base + SM501_OFF_DC_V_SYNC);
 }
 
 /* sm501fb_pan_crt
@@ -566,15 +588,15 @@ static int sm501fb_pan_crt(struct fb_var_screeninfo *var,
 
         xoffs = var->xoffset * bytes_pixel;
 
-        reg = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+        reg = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
         reg &= ~SM501_DC_CRT_CONTROL_PIXEL_MASK;
         reg |= ((xoffs & 15) / bytes_pixel) << 4;
-        writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
 
         reg = (par->screen.sm_addr + xoffs +
                var->yoffset * info->fix.line_length);
-        writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
+        smc501_writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
 
         sm501fb_sync_regs(fbi);
         return 0;
@@ -593,10 +615,10 @@ static int sm501fb_pan_pnl(struct fb_var_screeninfo *var,
         unsigned long reg;
 
         reg = var->xoffset | (var->xres_virtual << 16);
-        writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
+        smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
 
         reg = var->yoffset | (var->yres_virtual << 16);
-        writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
+        smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
 
         sm501fb_sync_regs(fbi);
         return 0;
@@ -622,7 +644,7 @@ static int sm501fb_set_par_crt(struct fb_info *info)
         /* enable CRT DAC - note 0 is on!*/
         sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
 
-        control = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+        control = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
         control &= (SM501_DC_CRT_CONTROL_PIXEL_MASK |
                     SM501_DC_CRT_CONTROL_GAMMA |
@@ -684,7 +706,7 @@ static int sm501fb_set_par_crt(struct fb_info *info)
  out_update:
         dev_dbg(fbi->dev, "new control is %08lx\n", control);
 
-        writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
         sm501fb_sync_regs(fbi);
 
         return 0;
@@ -696,18 +718,18 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
         void __iomem *ctrl_reg = fbi->regs + SM501_DC_PANEL_CONTROL;
         struct sm501_platdata_fbsub *pd = fbi->pdata->fb_pnl;
 
-        control = readl(ctrl_reg);
+        control = smc501_readl(ctrl_reg);
 
         if (to && (control & SM501_DC_PANEL_CONTROL_VDD) == 0) {
                 /* enable panel power */
 
                 control |= SM501_DC_PANEL_CONTROL_VDD;  /* FPVDDEN */
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
 
                 control |= SM501_DC_PANEL_CONTROL_DATA; /* DATA */
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
 
@@ -719,7 +741,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control |= SM501_DC_PANEL_CONTROL_BIAS;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
@@ -730,7 +752,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control |= SM501_DC_PANEL_CONTROL_FPEN;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
@@ -742,7 +764,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control &= ~SM501_DC_PANEL_CONTROL_FPEN;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
@@ -753,18 +775,18 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control &= ~SM501_DC_PANEL_CONTROL_BIAS;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
 
                 control &= ~SM501_DC_PANEL_CONTROL_DATA;
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
 
                 control &= ~SM501_DC_PANEL_CONTROL_VDD;
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
         }
@@ -799,7 +821,7 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
 
         /* update control register */
 
-        control = readl(fbi->regs + SM501_DC_PANEL_CONTROL);
+        control = smc501_readl(fbi->regs + SM501_DC_PANEL_CONTROL);
         control &= (SM501_DC_PANEL_CONTROL_GAMMA |
                     SM501_DC_PANEL_CONTROL_VDD  |
                     SM501_DC_PANEL_CONTROL_DATA |
@@ -833,16 +855,16 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
                 BUG();
         }
 
-        writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
+        smc501_writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
 
         /* panel plane top left and bottom right location */
 
-        writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
+        smc501_writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
 
         reg  = var->xres - 1;
         reg |= (var->yres - 1) << 16;
 
-        writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
+        smc501_writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
 
         /* program panel control register */
 
@@ -855,7 +877,7 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
         if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
                 control |= SM501_DC_PANEL_CONTROL_VSP;
 
-        writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
+        smc501_writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
         sm501fb_sync_regs(fbi);
 
         /* ensure the panel interface is not tristated at this point */
@@ -924,7 +946,7 @@ static int sm501fb_setcolreg(unsigned regno,
                         val |= (green >> 8) << 8;
                         val |= blue >> 8;
 
-                        writel(val, base + (regno * 4));
+                        smc501_writel(val, base + (regno * 4));
                 }
 
                 break;
@@ -980,7 +1002,7 @@ static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 
         dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
 
-        ctrl = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+        ctrl = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
         switch (blank_mode) {
         case FB_BLANK_POWERDOWN:
@@ -1004,7 +1026,7 @@ static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 
         }
 
-        writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
         sm501fb_sync_regs(fbi);
 
         return 0;
@@ -1041,12 +1063,14 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
         if (cursor->image.depth > 1)
                 return -EINVAL;
 
-        hwc_addr = readl(base + SM501_OFF_HWC_ADDR);
+        hwc_addr = smc501_readl(base + SM501_OFF_HWC_ADDR);
 
         if (cursor->enable)
-                writel(hwc_addr | SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
+                smc501_writel(hwc_addr | SM501_HWC_EN,
+                                base + SM501_OFF_HWC_ADDR);
         else
-                writel(hwc_addr & ~SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
+                smc501_writel(hwc_addr & ~SM501_HWC_EN,
+                                base + SM501_OFF_HWC_ADDR);
 
         /* set data */
         if (cursor->set & FB_CUR_SETPOS) {
@@ -1060,7 +1084,7 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
                 //y += cursor->image.height;
 
-                writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
+                smc501_writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
         }
 
         if (cursor->set & FB_CUR_SETCMAP) {
@@ -1080,8 +1104,8 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
                 dev_dbg(fbi->dev, "fgcol %08lx, bgcol %08lx\n", fg, bg);
 
-                writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
-                writel(fg, base + SM501_OFF_HWC_COLOR_3);
+                smc501_writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
+                smc501_writel(fg, base + SM501_OFF_HWC_COLOR_3);
         }
 
         if (cursor->set & FB_CUR_SETSIZE ||
@@ -1102,7 +1126,7 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
                         __func__, cursor->image.width, cursor->image.height);
 
                 for (op = 0; op < (64*64*2)/8; op+=4)
-                        writel(0x0, dst + op);
+                        smc501_writel(0x0, dst + op);
 
                 for (y = 0; y < cursor->image.height; y++) {
                         for (x = 0; x < cursor->image.width; x++) {
@@ -1141,7 +1165,7 @@ static ssize_t sm501fb_crtsrc_show(struct device *dev,
         struct sm501fb_info *info = dev_get_drvdata(dev);
         unsigned long ctrl;
 
-        ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
         ctrl &= SM501_DC_CRT_CONTROL_SEL;
 
         return snprintf(buf, PAGE_SIZE, "%s\n", ctrl ? "crt" : "panel");
@@ -1172,7 +1196,7 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
 
         dev_info(dev, "setting crt source to head %d\n", head);
 
-        ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 
         if (head == HEAD_CRT) {
                 ctrl |= SM501_DC_CRT_CONTROL_SEL;
@@ -1184,7 +1208,7 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
                 ctrl &= ~SM501_DC_CRT_CONTROL_TE;
         }
 
-        writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
         sm501fb_sync_regs(info);
 
         return len;
@@ -1205,7 +1229,8 @@ static int sm501fb_show_regs(struct sm501fb_info *info, char *ptr,
         unsigned int reg;
 
         for (reg = start; reg < (len + start); reg += 4)
-                ptr += sprintf(ptr, "%08x = %08x\n", reg, readl(mem + reg));
+                ptr += sprintf(ptr, "%08x = %08x\n", reg,
+                                smc501_readl(mem + reg));
 
         return ptr - buf;
 }
@@ -1257,7 +1282,7 @@ static int sm501fb_sync(struct fb_info *info)
 
         /* wait for the 2d engine to be ready */
         while ((count > 0) &&
-               (readl(fbi->regs + SM501_SYSTEM_CONTROL) &
+               (smc501_readl(fbi->regs + SM501_SYSTEM_CONTROL) &
                 SM501_SYSCTRL_2D_ENGINE_STATUS) != 0)
                 count--;
 
@@ -1312,45 +1337,46 @@ static void sm501fb_copyarea(struct fb_info *info, const struct fb_copyarea *are
                 return;
 
         /* set the base addresses */
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
+        smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
+        smc501_writel(par->screen.sm_addr,
+                        fbi->regs2d + SM501_2D_DESTINATION_BASE);
 
         /* set the window width */
-        writel((info->var.xres << 16) | info->var.xres,
+        smc501_writel((info->var.xres << 16) | info->var.xres,
                fbi->regs2d + SM501_2D_WINDOW_WIDTH);
 
         /* set window stride */
-        writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
+        smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
                fbi->regs2d + SM501_2D_PITCH);
 
         /* set data format */
         switch (info->var.bits_per_pixel) {
         case 8:
-                writel(0, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 16:
-                writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 32:
-                writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         }
 
         /* 2d compare mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
 
         /* 2d mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
 
         /* source and destination x y */
-        writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
-        writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
+        smc501_writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
+        smc501_writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
 
         /* w/h */
-        writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
+        smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
 
         /* do area move */
-        writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
+        smc501_writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
 }
 
 static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
@@ -1372,47 +1398,49 @@ static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rec
                 return;
 
         /* set the base addresses */
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
+        smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
+        smc501_writel(par->screen.sm_addr,
+                        fbi->regs2d + SM501_2D_DESTINATION_BASE);
 
         /* set the window width */
-        writel((info->var.xres << 16) | info->var.xres,
+        smc501_writel((info->var.xres << 16) | info->var.xres,
                fbi->regs2d + SM501_2D_WINDOW_WIDTH);
 
         /* set window stride */
-        writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
+        smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
                fbi->regs2d + SM501_2D_PITCH);
 
         /* set data format */
         switch (info->var.bits_per_pixel) {
         case 8:
-                writel(0, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 16:
-                writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 32:
-                writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         }
 
         /* 2d compare mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
 
         /* 2d mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
 
         /* colour */
-        writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
+        smc501_writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
 
         /* x y */
-        writel((rect->dx << 16) | rect->dy, fbi->regs2d + SM501_2D_DESTINATION);
+        smc501_writel((rect->dx << 16) | rect->dy,
+                        fbi->regs2d + SM501_2D_DESTINATION);
 
         /* w/h */
-        writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
+        smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
 
         /* do rectangle fill */
-        writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
+        smc501_writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
 }
 
 
@@ -1470,11 +1498,12 @@ static int sm501_init_cursor(struct fb_info *fbi, unsigned int reg_base)
 
         /* initialise the colour registers */
 
-        writel(par->cursor.sm_addr, par->cursor_regs + SM501_OFF_HWC_ADDR);
+        smc501_writel(par->cursor.sm_addr,
+                        par->cursor_regs + SM501_OFF_HWC_ADDR);
 
-        writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
-        writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
-        writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
+        smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
+        smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
+        smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
         sm501fb_sync_regs(info);
 
         return 0;
@@ -1581,7 +1610,7 @@ static int sm501fb_start(struct sm501fb_info *info,
 
         /* clear palette ram - undefined at power on */
         for (k = 0; k < (256 * 3); k++)
-                writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
+                smc501_writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
 
         /* enable display controller */
         sm501_unit_power(dev->parent, SM501_GATE_DISPLAY, 1);
@@ -1596,22 +1625,22 @@ static int sm501fb_start(struct sm501fb_info *info,
         return 0; /* everything is setup */
 
  err_mem_res:
-        release_resource(info->fbmem_res);
-        kfree(info->fbmem_res);
+        release_mem_region(info->fbmem_res->start,
+                           resource_size(info->fbmem_res));
 
  err_regs2d_map:
         iounmap(info->regs2d);
 
  err_regs2d_res:
-        release_resource(info->regs2d_res);
-        kfree(info->regs2d_res);
+        release_mem_region(info->regs2d_res->start,
+                           resource_size(info->regs2d_res));
 
  err_regs_map:
         iounmap(info->regs);
 
  err_regs_res:
-        release_resource(info->regs_res);
-        kfree(info->regs_res);
+        release_mem_region(info->regs_res->start,
+                           resource_size(info->regs_res));
 
  err_release:
         return ret;
@@ -1623,19 +1652,19 @@ static void sm501fb_stop(struct sm501fb_info *info)
         sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 0);
 
         iounmap(info->fbmem);
-        release_resource(info->fbmem_res);
-        kfree(info->fbmem_res);
+        release_mem_region(info->fbmem_res->start,
+                           resource_size(info->fbmem_res));
 
         iounmap(info->regs2d);
-        release_resource(info->regs2d_res);
-        kfree(info->regs2d_res);
+        release_mem_region(info->regs2d_res->start,
+                           resource_size(info->regs2d_res));
 
         iounmap(info->regs);
-        release_resource(info->regs_res);
-        kfree(info->regs_res);
+        release_mem_region(info->regs_res->start,
+                           resource_size(info->regs_res));
 }
 
-static int sm501fb_init_fb(struct fb_info *fb,
+static int __devinit sm501fb_init_fb(struct fb_info *fb,
                            enum sm501_controller head,
                            const char *fbname)
 {
@@ -1649,20 +1678,20 @@ static int sm501fb_init_fb(struct fb_info *fb,
         switch (head) {
         case HEAD_CRT:
                 pd = info->pdata->fb_crt;
-                ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+                ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
                 enable = (ctrl & SM501_DC_CRT_CONTROL_ENABLE) ? 1 : 0;
 
                 /* ensure we set the correct source register */
                 if (info->pdata->fb_route != SM501_FB_CRT_PANEL) {
                         ctrl |= SM501_DC_CRT_CONTROL_SEL;
-                        writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+                        smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
                 }
 
                 break;
 
         case HEAD_PANEL:
                 pd = info->pdata->fb_pnl;
-                ctrl = readl(info->regs + SM501_DC_PANEL_CONTROL);
+                ctrl = smc501_readl(info->regs + SM501_DC_PANEL_CONTROL);
                 enable = (ctrl & SM501_DC_PANEL_CONTROL_EN) ? 1 : 0;
                 break;
 
@@ -1680,7 +1709,7 @@ static int sm501fb_init_fb(struct fb_info *fb,
 
         if (head == HEAD_CRT && info->pdata->fb_route == SM501_FB_CRT_PANEL) {
                 ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
-                writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+                smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
                 enable = 0;
         }
 
@@ -1690,7 +1719,7 @@ static int sm501fb_init_fb(struct fb_info *fb,
                (head == HEAD_CRT) ? &sm501fb_ops_crt : &sm501fb_ops_pnl,
                sizeof(struct fb_ops));
 
-        /* update ops dependant on what we've been passed */
+        /* update ops dependent on what we've been passed */
 
         if ((pd->flags & SM501FB_FLAG_USE_HWCURSOR) == 0)
                 par->ops.fb_cursor = NULL;
@@ -1700,6 +1729,15 @@ static int sm501fb_init_fb(struct fb_info *fb,
                 FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT |
                 FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
 
+#if defined(CONFIG_OF)
+#ifdef __BIG_ENDIAN
+        if (of_get_property(info->dev->parent->of_node, "little-endian", NULL))
+                fb->flags |= FBINFO_FOREIGN_ENDIAN;
+#else
+        if (of_get_property(info->dev->parent->of_node, "big-endian", NULL))
+                fb->flags |= FBINFO_FOREIGN_ENDIAN;
+#endif
+#endif
         /* fixed data */
 
         fb->fix.type            = FB_TYPE_PACKED_PIXELS;
@@ -1717,9 +1755,16 @@ static int sm501fb_init_fb(struct fb_info *fb,
         fb->var.vmode           = FB_VMODE_NONINTERLACED;
         fb->var.bits_per_pixel  = 16;
 
+        if (info->edid_data) {
+                        /* Now build modedb from EDID */
+                        fb_edid_to_monspecs(info->edid_data, &fb->monspecs);
+                        fb_videomode_to_modelist(fb->monspecs.modedb,
+                                                 fb->monspecs.modedb_len,
+                                                 &fb->modelist);
+        }
+
         if (enable && (pd->flags & SM501FB_FLAG_USE_INIT_MODE) && 0) {
                 /* TODO read the mode from the current display */
-
         } else {
                 if (pd->def_mode) {
                         dev_info(info->dev, "using supplied mode\n");
@@ -1729,12 +1774,37 @@ static int sm501fb_init_fb(struct fb_info *fb,
                         fb->var.xres_virtual = fb->var.xres;
                         fb->var.yres_virtual = fb->var.yres;
                 } else {
-                        ret = fb_find_mode(&fb->var, fb,
+                        if (info->edid_data) {
+                                ret = fb_find_mode(&fb->var, fb, fb_mode,
+                                        fb->monspecs.modedb,
+                                        fb->monspecs.modedb_len,
+                                        &sm501_default_mode, default_bpp);
+                                /* edid_data is no longer needed, free it */
+                                kfree(info->edid_data);
+                        } else {
+                                ret = fb_find_mode(&fb->var, fb,
                                            NULL, NULL, 0, NULL, 8);
+                        }
 
-                        if (ret == 0 || ret == 4) {
-                                dev_err(info->dev,
-                                        "failed to get initial mode\n");
+                        switch (ret) {
+                        case 1:
+                                dev_info(info->dev, "using mode specified in "
+                                                "@mode\n");
+                                break;
+                        case 2:
+                                dev_info(info->dev, "using mode specified in "
+                                        "@mode with ignored refresh rate\n");
+                                break;
+                        case 3:
+                                dev_info(info->dev, "using mode default "
+                                        "mode\n");
+                                break;
+                        case 4:
+                                dev_info(info->dev, "using mode from list\n");
+                                break;
+                        default:
+                                dev_info(info->dev, "ret = %d\n", ret);
+                                dev_info(info->dev, "failed to find mode\n");
                                 return -EINVAL;
                         }
                 }
@@ -1875,8 +1945,32 @@ static int __devinit sm501fb_probe(struct platform_device *pdev)
         }
 
         if (info->pdata == NULL) {
-                dev_info(dev, "using default configuration data\n");
+                int found = 0;
+#if defined(CONFIG_OF)
+                struct device_node *np = pdev->dev.parent->of_node;
+                const u8 *prop;
+                const char *cp;
+                int len;
+
                 info->pdata = &sm501fb_def_pdata;
+                if (np) {
+                        /* Get EDID */
+                        cp = of_get_property(np, "mode", &len);
+                        if (cp)
+                                strcpy(fb_mode, cp);
+                        prop = of_get_property(np, "edid", &len);
+                        if (prop && len == EDID_LENGTH) {
+                                info->edid_data = kmemdup(prop, EDID_LENGTH,
+                                                          GFP_KERNEL);
+                                if (info->edid_data)
+                                        found = 1;
+                        }
+                }
+#endif
+                if (!found) {
+                        dev_info(dev, "using default configuration data\n");
+                        info->pdata = &sm501fb_def_pdata;
+                }
         }
 
         /* probe for the presence of each panel */
@@ -2010,9 +2104,9 @@ static int sm501fb_suspend_fb(struct sm501fb_info *info,
 
         /* tell console/fb driver we are suspending */
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(fbi, 1);
-        release_console_sem();
+        console_unlock();
 
         /* backup copies in case chip is powered down over suspend */
 
@@ -2069,9 +2163,9 @@ static void sm501fb_resume_fb(struct sm501fb_info *info,
                 memcpy_toio(par->cursor.k_addr, par->store_cursor,
                             par->cursor.size);
 
-        acquire_console_sem();
+        console_lock();
         fb_set_suspend(fbi, 0);
-        release_console_sem();
+        console_unlock();
 
         vfree(par->store_fb);
         vfree(par->store_cursor);
@@ -2085,7 +2179,7 @@ static int sm501fb_suspend(struct platform_device *pdev, pm_message_t state)
         struct sm501fb_info *info = platform_get_drvdata(pdev);
 
         /* store crt control to resume with */
-        info->pm_crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        info->pm_crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 
         sm501fb_suspend_fb(info, HEAD_CRT);
         sm501fb_suspend_fb(info, HEAD_PANEL);
@@ -2109,10 +2203,10 @@ static int sm501fb_resume(struct platform_device *pdev)
 
         /* restore the items we want to be saved for crt control */
 
-        crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
         crt_ctrl &= ~SM501_CRT_CTRL_SAVE;
         crt_ctrl |= info->pm_crt_ctrl & SM501_CRT_CTRL_SAVE;
-        writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
 
         sm501fb_resume_fb(info, HEAD_CRT);
         sm501fb_resume_fb(info, HEAD_PANEL);
@@ -2149,6 +2243,11 @@ static void __exit sm501fb_cleanup(void)
 module_init(sm501fb_init);
 module_exit(sm501fb_cleanup);
 
+module_param_named(mode, fb_mode, charp, 0);
+MODULE_PARM_DESC(mode,
+        "Specify resolution as \"<xres>x<yres>[-<bpp>][@<refresh>]\" ");
+module_param_named(bpp, default_bpp, ulong, 0);
+MODULE_PARM_DESC(bpp, "Specify bit-per-pixel if not specified mode");
 MODULE_AUTHOR("Ben Dooks, Vincent Sanders");
 MODULE_DESCRIPTION("SM501 Framebuffer driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/sstfb.c b/drivers/video/sstfb.c
index dee64c3b1e67..2301c275d63a 100644
--- a/drivers/video/sstfb.c
+++ b/drivers/video/sstfb.c
@@ -221,7 +221,7 @@ static int __sst_wait_idle(u8 __iomem *vbase)
         while(1) {
                 if (__sst_read(vbase, STATUS) & STATUS_FBI_BUSY) {
                         f_dddprintk("status: busy\n");
-/* FIXME basicaly, this is a busy wait. maybe not that good. oh well;
+/* FIXME basically, this is a busy wait. maybe not that good. oh well;
  * this is a small loop after all.
  * Or maybe we should use mdelay() or udelay() here instead ? */
                         count = 0;
@@ -501,7 +501,7 @@ static int sstfb_set_par(struct fb_info *info)
         }
 
         if (IS_VOODOO2(par)) {
-                /* voodoo2 has 32 pixel wide tiles , BUT stange things
+                /* voodoo2 has 32 pixel wide tiles , BUT strange things
                    happen with odd number of tiles */
                 par->tiles_in_X = (info->var.xres + 63 ) / 64 * 2;
         } else {
@@ -536,7 +536,7 @@ static int sstfb_set_par(struct fb_info *info)
         fbiinit2 = sst_read(FBIINIT2);
         fbiinit3 = sst_read(FBIINIT3);
 
-        /* everything is reset. we enable fbiinit2/3 remap : dac acces ok */
+        /* everything is reset. we enable fbiinit2/3 remap : dac access ok */
         pci_write_config_dword(sst_dev, PCI_INIT_ENABLE,
                                PCI_EN_INIT_WR | PCI_REMAP_DAC );
 
@@ -920,11 +920,11 @@ static int __devinit sst_detect_ti(struct fb_info *info)
  * we get the 1st byte (M value) of preset f1,f7 and fB
  * why those 3 ? mmmh... for now, i'll do it the glide way...
  * and ask questions later. anyway, it seems that all the freq registers are
- * realy at their default state (cf specs) so i ask again, why those 3 regs ?
+ * really at their default state (cf specs) so i ask again, why those 3 regs ?
  * mmmmh.. it seems that's much more ugly than i thought. we use f0 and fA for
  * pll programming, so in fact, we *hope* that the f1, f7 & fB won't be
  * touched...
- * is it realy safe ? how can i reset this ramdac ? geee...
+ * is it really safe ? how can i reset this ramdac ? geee...
  */
 static int __devinit sst_detect_ics(struct fb_info *info)
 {
diff --git a/drivers/video/sticore.h b/drivers/video/sticore.h
index 7fe5be4bc70e..addf7b615ef8 100644
--- a/drivers/video/sticore.h
+++ b/drivers/video/sticore.h
@@ -79,7 +79,7 @@ struct sti_glob_cfg_ext {
          u8 curr_mon;                   /* current monitor configured */
          u8 friendly_boot;              /* in friendly boot mode */
         s16 power;                      /* power calculation (in Watts) */
-        s32 freq_ref;                   /* frequency refrence */
+        s32 freq_ref;                   /* frequency reference */
         u32 sti_mem_addr;               /* pointer to global sti memory (size=sti_mem_request) */
         u32 future_ptr;                 /* pointer to future data */
 };
diff --git a/drivers/video/sunxvr1000.c b/drivers/video/sunxvr1000.c
index 5dbe06af2226..b7f27acaf817 100644
--- a/drivers/video/sunxvr1000.c
+++ b/drivers/video/sunxvr1000.c
@@ -111,8 +111,7 @@ static int __devinit gfb_set_fbinfo(struct gfb_info *gp)
         return 0;
 }
 
-static int __devinit gfb_probe(struct platform_device *op,
-                               const struct of_device_id *match)
+static int __devinit gfb_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -198,7 +197,7 @@ static const struct of_device_id gfb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ffb_match);
 
-static struct of_platform_driver gfb_driver = {
+static struct platform_driver gfb_driver = {
         .probe          = gfb_probe,
         .remove         = __devexit_p(gfb_remove),
         .driver = {
@@ -213,12 +212,12 @@ static int __init gfb_init(void)
         if (fb_get_options("gfb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&gfb_driver);
+        return platform_driver_register(&gfb_driver);
 }
 
 static void __exit gfb_exit(void)
 {
-        of_unregister_platform_driver(&gfb_driver);
+        platform_driver_unregister(&gfb_driver);
 }
 
 module_init(gfb_init);
diff --git a/drivers/video/svgalib.c b/drivers/video/svgalib.c
index fdb45674e2f6..33df9ec91795 100644
--- a/drivers/video/svgalib.c
+++ b/drivers/video/svgalib.c
@@ -20,12 +20,12 @@
 
 
 /* Write a CRT register value spread across multiple registers */
-void svga_wcrt_multi(const struct vga_regset *regset, u32 value) {
-
+void svga_wcrt_multi(void __iomem *regbase, const struct vga_regset *regset, u32 value)
+{
         u8 regval, bitval, bitnum;
 
         while (regset->regnum != VGA_REGSET_END_VAL) {
-                regval = vga_rcrt(NULL, regset->regnum);
+                regval = vga_rcrt(regbase, regset->regnum);
                 bitnum = regset->lowbit;
                 while (bitnum <= regset->highbit) {
                         bitval = 1 << bitnum;
@@ -34,18 +34,18 @@ void svga_wcrt_multi(const struct vga_regset *regset, u32 value) {
                         bitnum ++;
                         value = value >> 1;
                 }
-                vga_wcrt(NULL, regset->regnum, regval);
+                vga_wcrt(regbase, regset->regnum, regval);
                 regset ++;
         }
 }
 
 /* Write a sequencer register value spread across multiple registers */
-void svga_wseq_multi(const struct vga_regset *regset, u32 value) {
-
+void svga_wseq_multi(void __iomem *regbase, const struct vga_regset *regset, u32 value)
+{
         u8 regval, bitval, bitnum;
 
         while (regset->regnum != VGA_REGSET_END_VAL) {
-                regval = vga_rseq(NULL, regset->regnum);
+                regval = vga_rseq(regbase, regset->regnum);
                 bitnum = regset->lowbit;
                 while (bitnum <= regset->highbit) {
                         bitval = 1 << bitnum;
@@ -54,7 +54,7 @@ void svga_wseq_multi(const struct vga_regset *regset, u32 value) {
                         bitnum ++;
                         value = value >> 1;
                 }
-                vga_wseq(NULL, regset->regnum, regval);
+                vga_wseq(regbase, regset->regnum, regval);
                 regset ++;
         }
 }
@@ -75,95 +75,95 @@ static unsigned int svga_regset_size(const struct vga_regset *regset)
 
 
 /* Set graphics controller registers to sane values */
-void svga_set_default_gfx_regs(void)
+void svga_set_default_gfx_regs(void __iomem *regbase)
 {
         /* All standard GFX registers (GR00 - GR08) */
-        vga_wgfx(NULL, VGA_GFX_SR_VALUE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_SR_ENABLE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_COMPARE_VALUE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_DATA_ROTATE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_PLANE_READ, 0x00);
-        vga_wgfx(NULL, VGA_GFX_MODE, 0x00);
-/*      vga_wgfx(NULL, VGA_GFX_MODE, 0x20); */
-/*      vga_wgfx(NULL, VGA_GFX_MODE, 0x40); */
-        vga_wgfx(NULL, VGA_GFX_MISC, 0x05);
-/*      vga_wgfx(NULL, VGA_GFX_MISC, 0x01); */
-        vga_wgfx(NULL, VGA_GFX_COMPARE_MASK, 0x0F);
-        vga_wgfx(NULL, VGA_GFX_BIT_MASK, 0xFF);
+        vga_wgfx(regbase, VGA_GFX_SR_VALUE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_SR_ENABLE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_COMPARE_VALUE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_DATA_ROTATE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_PLANE_READ, 0x00);
+        vga_wgfx(regbase, VGA_GFX_MODE, 0x00);
+/*      vga_wgfx(regbase, VGA_GFX_MODE, 0x20); */
+/*      vga_wgfx(regbase, VGA_GFX_MODE, 0x40); */
+        vga_wgfx(regbase, VGA_GFX_MISC, 0x05);
+/*      vga_wgfx(regbase, VGA_GFX_MISC, 0x01); */
+        vga_wgfx(regbase, VGA_GFX_COMPARE_MASK, 0x0F);
+        vga_wgfx(regbase, VGA_GFX_BIT_MASK, 0xFF);
 }
 
 /* Set attribute controller registers to sane values */
-void svga_set_default_atc_regs(void)
+void svga_set_default_atc_regs(void __iomem *regbase)
 {
         u8 count;
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x00);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x00);
 
         /* All standard ATC registers (AR00 - AR14) */
         for (count = 0; count <= 0xF; count ++)
-                svga_wattr(count, count);
+                svga_wattr(regbase, count, count);
 
-        svga_wattr(VGA_ATC_MODE, 0x01);
-/*      svga_wattr(VGA_ATC_MODE, 0x41); */
-        svga_wattr(VGA_ATC_OVERSCAN, 0x00);
-        svga_wattr(VGA_ATC_PLANE_ENABLE, 0x0F);
-        svga_wattr(VGA_ATC_PEL, 0x00);
-        svga_wattr(VGA_ATC_COLOR_PAGE, 0x00);
+        svga_wattr(regbase, VGA_ATC_MODE, 0x01);
+/*      svga_wattr(regbase, VGA_ATC_MODE, 0x41); */
+        svga_wattr(regbase, VGA_ATC_OVERSCAN, 0x00);
+        svga_wattr(regbase, VGA_ATC_PLANE_ENABLE, 0x0F);
+        svga_wattr(regbase, VGA_ATC_PEL, 0x00);
+        svga_wattr(regbase, VGA_ATC_COLOR_PAGE, 0x00);
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x20);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x20);
 }
 
 /* Set sequencer registers to sane values */
-void svga_set_default_seq_regs(void)
+void svga_set_default_seq_regs(void __iomem *regbase)
 {
         /* Standard sequencer registers (SR01 - SR04), SR00 is not set */
-        vga_wseq(NULL, VGA_SEQ_CLOCK_MODE, VGA_SR01_CHAR_CLK_8DOTS);
-        vga_wseq(NULL, VGA_SEQ_PLANE_WRITE, VGA_SR02_ALL_PLANES);
-        vga_wseq(NULL, VGA_SEQ_CHARACTER_MAP, 0x00);
-/*      vga_wseq(NULL, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE | VGA_SR04_CHN_4M); */
-        vga_wseq(NULL, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE);
+        vga_wseq(regbase, VGA_SEQ_CLOCK_MODE, VGA_SR01_CHAR_CLK_8DOTS);
+        vga_wseq(regbase, VGA_SEQ_PLANE_WRITE, VGA_SR02_ALL_PLANES);
+        vga_wseq(regbase, VGA_SEQ_CHARACTER_MAP, 0x00);
+/*      vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE | VGA_SR04_CHN_4M); */
+        vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE);
 }
 
 /* Set CRTC registers to sane values */
-void svga_set_default_crt_regs(void)
+void svga_set_default_crt_regs(void __iomem *regbase)
 {
         /* Standard CRT registers CR03 CR08 CR09 CR14 CR17 */
-        svga_wcrt_mask(0x03, 0x80, 0x80);       /* Enable vertical retrace EVRA */
-        vga_wcrt(NULL, VGA_CRTC_PRESET_ROW, 0);
-        svga_wcrt_mask(VGA_CRTC_MAX_SCAN, 0, 0x1F);
-        vga_wcrt(NULL, VGA_CRTC_UNDERLINE, 0);
-        vga_wcrt(NULL, VGA_CRTC_MODE, 0xE3);
+        svga_wcrt_mask(regbase, 0x03, 0x80, 0x80);      /* Enable vertical retrace EVRA */
+        vga_wcrt(regbase, VGA_CRTC_PRESET_ROW, 0);
+        svga_wcrt_mask(regbase, VGA_CRTC_MAX_SCAN, 0, 0x1F);
+        vga_wcrt(regbase, VGA_CRTC_UNDERLINE, 0);
+        vga_wcrt(regbase, VGA_CRTC_MODE, 0xE3);
 }
 
-void svga_set_textmode_vga_regs(void)
+void svga_set_textmode_vga_regs(void __iomem *regbase)
 {
-        /* svga_wseq_mask(0x1, 0x00, 0x01); */   /* Switch 8/9 pixel per char */
-        vga_wseq(NULL, VGA_SEQ_MEMORY_MODE,     VGA_SR04_EXT_MEM);
-        vga_wseq(NULL, VGA_SEQ_PLANE_WRITE,     0x03);
+        /* svga_wseq_mask(regbase, 0x1, 0x00, 0x01); */   /* Switch 8/9 pixel per char */
+        vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM);
+        vga_wseq(regbase, VGA_SEQ_PLANE_WRITE, 0x03);
 
-        vga_wcrt(NULL, VGA_CRTC_MAX_SCAN,       0x0f); /* 0x4f */
-        vga_wcrt(NULL, VGA_CRTC_UNDERLINE,      0x1f);
-        svga_wcrt_mask(VGA_CRTC_MODE,           0x23, 0x7f);
+        vga_wcrt(regbase, VGA_CRTC_MAX_SCAN, 0x0f); /* 0x4f */
+        vga_wcrt(regbase, VGA_CRTC_UNDERLINE, 0x1f);
+        svga_wcrt_mask(regbase, VGA_CRTC_MODE, 0x23, 0x7f);
 
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_START,   0x0d);
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_END,     0x0e);
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_HI,      0x00);
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_LO,      0x00);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_START, 0x0d);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_END, 0x0e);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_HI, 0x00);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_LO, 0x00);
 
-        vga_wgfx(NULL, VGA_GFX_MODE,            0x10); /* Odd/even memory mode */
-        vga_wgfx(NULL, VGA_GFX_MISC,            0x0E); /* Misc graphics register - text mode enable */
-        vga_wgfx(NULL, VGA_GFX_COMPARE_MASK,    0x00);
+        vga_wgfx(regbase, VGA_GFX_MODE, 0x10); /* Odd/even memory mode */
+        vga_wgfx(regbase, VGA_GFX_MISC, 0x0E); /* Misc graphics register - text mode enable */
+        vga_wgfx(regbase, VGA_GFX_COMPARE_MASK, 0x00);
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x00);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x00);
 
-        svga_wattr(0x10, 0x0C);                 /* Attribute Mode Control Register - text mode, blinking and line graphics */
-        svga_wattr(0x13, 0x08);                 /* Horizontal Pixel Panning Register  */
+        svga_wattr(regbase, 0x10, 0x0C);                        /* Attribute Mode Control Register - text mode, blinking and line graphics */
+        svga_wattr(regbase, 0x13, 0x08);                        /* Horizontal Pixel Panning Register  */
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x20);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x20);
 }
 
 #if 0
@@ -299,7 +299,7 @@ void svga_tileblit(struct fb_info *info, struct fb_tileblit *blit)
 }
 
 /* Set cursor in text (tileblit) mode */
-void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+void svga_tilecursor(void __iomem *regbase, struct fb_info *info, struct fb_tilecursor *cursor)
 {
         u8 cs = 0x0d;
         u8 ce = 0x0e;
@@ -310,7 +310,7 @@ void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
         if (! cursor -> mode)
                 return;
 
-        svga_wcrt_mask(0x0A, 0x20, 0x20); /* disable cursor */
+        svga_wcrt_mask(regbase, 0x0A, 0x20, 0x20); /* disable cursor */
 
         if (cursor -> shape == FB_TILE_CURSOR_NONE)
                 return;
@@ -334,11 +334,11 @@ void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
         }
 
         /* set cursor position */
-        vga_wcrt(NULL, 0x0E, pos >> 8);
-        vga_wcrt(NULL, 0x0F, pos & 0xFF);
+        vga_wcrt(regbase, 0x0E, pos >> 8);
+        vga_wcrt(regbase, 0x0F, pos & 0xFF);
 
-        vga_wcrt(NULL, 0x0B, ce); /* set cursor end */
-        vga_wcrt(NULL, 0x0A, cs); /* set cursor start and enable it */
+        vga_wcrt(regbase, 0x0B, ce); /* set cursor end */
+        vga_wcrt(regbase, 0x0A, cs); /* set cursor start and enable it */
 }
 
 int svga_get_tilemax(struct fb_info *info)
@@ -507,8 +507,9 @@ int svga_check_timings(const struct svga_timing_regs *tm, struct fb_var_screenin
 }
 
 /* Set CRT timing registers */
-void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninfo *var,
-                        u32 hmul, u32 hdiv, u32 vmul, u32 vdiv, u32 hborder, int node)
+void svga_set_timings(void __iomem *regbase, const struct svga_timing_regs *tm,
+                      struct fb_var_screeninfo *var,
+                      u32 hmul, u32 hdiv, u32 vmul, u32 vdiv, u32 hborder, int node)
 {
         u8 regval;
         u32 value;
@@ -516,66 +517,66 @@ void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninf
         value = var->xres + var->left_margin + var->right_margin + var->hsync_len;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal total      : %d\n", node, value);
-        svga_wcrt_multi(tm->h_total_regs, (value / 8) - 5);
+        svga_wcrt_multi(regbase, tm->h_total_regs, (value / 8) - 5);
 
         value = var->xres;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal display    : %d\n", node, value);
-        svga_wcrt_multi(tm->h_display_regs, (value / 8) - 1);
+        svga_wcrt_multi(regbase, tm->h_display_regs, (value / 8) - 1);
 
         value = var->xres;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal blank start: %d\n", node, value);
-        svga_wcrt_multi(tm->h_blank_start_regs, (value / 8) - 1 + hborder);
+        svga_wcrt_multi(regbase, tm->h_blank_start_regs, (value / 8) - 1 + hborder);
 
         value = var->xres + var->left_margin + var->right_margin + var->hsync_len;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal blank end  : %d\n", node, value);
-        svga_wcrt_multi(tm->h_blank_end_regs, (value / 8) - 1 - hborder);
+        svga_wcrt_multi(regbase, tm->h_blank_end_regs, (value / 8) - 1 - hborder);
 
         value = var->xres + var->right_margin;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal sync start : %d\n", node, value);
-        svga_wcrt_multi(tm->h_sync_start_regs, (value / 8));
+        svga_wcrt_multi(regbase, tm->h_sync_start_regs, (value / 8));
 
         value = var->xres + var->right_margin + var->hsync_len;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal sync end   : %d\n", node, value);
-        svga_wcrt_multi(tm->h_sync_end_regs, (value / 8));
+        svga_wcrt_multi(regbase, tm->h_sync_end_regs, (value / 8));
 
         value = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical total        : %d\n", node, value);
-        svga_wcrt_multi(tm->v_total_regs, value - 2);
+        svga_wcrt_multi(regbase, tm->v_total_regs, value - 2);
 
         value = var->yres;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical display      : %d\n", node, value);
-        svga_wcrt_multi(tm->v_display_regs, value - 1);
+        svga_wcrt_multi(regbase, tm->v_display_regs, value - 1);
 
         value = var->yres;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical blank start  : %d\n", node, value);
-        svga_wcrt_multi(tm->v_blank_start_regs, value);
+        svga_wcrt_multi(regbase, tm->v_blank_start_regs, value);
 
         value = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical blank end    : %d\n", node, value);
-        svga_wcrt_multi(tm->v_blank_end_regs, value - 2);
+        svga_wcrt_multi(regbase, tm->v_blank_end_regs, value - 2);
 
         value = var->yres + var->lower_margin;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical sync start   : %d\n", node, value);
-        svga_wcrt_multi(tm->v_sync_start_regs, value);
+        svga_wcrt_multi(regbase, tm->v_sync_start_regs, value);
 
         value = var->yres + var->lower_margin + var->vsync_len;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical sync end     : %d\n", node, value);
-        svga_wcrt_multi(tm->v_sync_end_regs, value);
+        svga_wcrt_multi(regbase, tm->v_sync_end_regs, value);
 
         /* Set horizontal and vertical sync pulse polarity in misc register */
 
-        regval = vga_r(NULL, VGA_MIS_R);
+        regval = vga_r(regbase, VGA_MIS_R);
         if (var->sync & FB_SYNC_HOR_HIGH_ACT) {
                 pr_debug("fb%d: positive horizontal sync\n", node);
                 regval = regval & ~0x80;
@@ -590,7 +591,7 @@ void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninf
                 pr_debug("fb%d: negative vertical sync\n\n", node);
                 regval = regval | 0x40;
         }
-        vga_w(NULL, VGA_MIS_W, regval);
+        vga_w(regbase, VGA_MIS_W, regval);
 }
 
 
diff --git a/drivers/video/tcx.c b/drivers/video/tcx.c
index 77ad27955cf0..07c66e946634 100644
--- a/drivers/video/tcx.c
+++ b/drivers/video/tcx.c
@@ -362,8 +362,7 @@ static void tcx_unmap_regs(struct platform_device *op, struct fb_info *info,
                            info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit tcx_probe(struct platform_device *op,
-                               const struct of_device_id *match)
+static int __devinit tcx_probe(struct platform_device *op)
 {
         struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
@@ -481,6 +480,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
         tcx_unmap_regs(op, info, par);
+        framebuffer_release(info);
 
 out_err:
         return err;
@@ -511,7 +511,7 @@ static const struct of_device_id tcx_match[] = {
 };
 MODULE_DEVICE_TABLE(of, tcx_match);
 
-static struct of_platform_driver tcx_driver = {
+static struct platform_driver tcx_driver = {
         .driver = {
                 .name = "tcx",
                 .owner = THIS_MODULE,
@@ -526,12 +526,12 @@ static int __init tcx_init(void)
         if (fb_get_options("tcxfb", NULL))
                 return -ENODEV;
 
-        return of_register_platform_driver(&tcx_driver);
+        return platform_driver_register(&tcx_driver);
 }
 
 static void __exit tcx_exit(void)
 {
-        of_unregister_platform_driver(&tcx_driver);
+        platform_driver_unregister(&tcx_driver);
 }
 
 module_init(tcx_init);
diff --git a/drivers/video/tdfxfb.c b/drivers/video/tdfxfb.c
index 3ee5e63cfa4f..a99b994c9b6b 100644
--- a/drivers/video/tdfxfb.c
+++ b/drivers/video/tdfxfb.c
@@ -877,12 +877,12 @@ static void tdfxfb_fillrect(struct fb_info *info,
         else
                 tdfx_rop = TDFX_ROP_XOR;
 
-        /* asume always rect->height < 4096 */
+        /* assume always rect->height < 4096 */
         if (dy + rect->height > 4095) {
                 dstbase = stride * dy;
                 dy = 0;
         }
-        /* asume always rect->width < 4096 */
+        /* assume always rect->width < 4096 */
         if (dx + rect->width > 4095) {
                 dstbase += dx * bpp >> 3;
                 dx = 0;
@@ -915,22 +915,22 @@ static void tdfxfb_copyarea(struct fb_info *info,
         u32 dstbase = 0;
         u32 srcbase = 0;
 
-        /* asume always area->height < 4096 */
+        /* assume always area->height < 4096 */
         if (sy + area->height > 4095) {
                 srcbase = stride * sy;
                 sy = 0;
         }
-        /* asume always area->width < 4096 */
+        /* assume always area->width < 4096 */
         if (sx + area->width > 4095) {
                 srcbase += sx * bpp >> 3;
                 sx = 0;
         }
-        /* asume always area->height < 4096 */
+        /* assume always area->height < 4096 */
         if (dy + area->height > 4095) {
                 dstbase = stride * dy;
                 dy = 0;
         }
-        /* asume always area->width < 4096 */
+        /* assume always area->width < 4096 */
         if (dx + area->width > 4095) {
                 dstbase += dx * bpp >> 3;
                 dx = 0;
@@ -1003,12 +1003,12 @@ static void tdfxfb_imageblit(struct fb_info *info, const struct fb_image *image)
 #else
         srcfmt = 0x400000;
 #endif
-        /* asume always image->height < 4096 */
+        /* assume always image->height < 4096 */
         if (dy + image->height > 4095) {
                 dstbase = stride * dy;
                 dy = 0;
         }
-        /* asume always image->width < 4096 */
+        /* assume always image->width < 4096 */
         if (dx + image->width > 4095) {
                 dstbase += dx * bpp >> 3;
                 dx = 0;
@@ -1124,7 +1124,7 @@ static int tdfxfb_cursor(struct fb_info *info, struct fb_cursor *cursor)
                  * lower half (least significant 64 bits) of a 128 bit word
                  * and pattern 1 the upper half. If you examine the data of
                  * the cursor image the graphics card uses then from the
-                 * begining you see line one of pattern 0, line one of
+                 * beginning you see line one of pattern 0, line one of
                  * pattern 1, line two of pattern 0, line two of pattern 1,
                  * etc etc. The linear stride for the cursor is always 16 bytes
                  * (128 bits) which is the maximum cursor width times two for
diff --git a/drivers/video/tmiofb.c b/drivers/video/tmiofb.c
index 6913fe168c25..cd1c4dcef8fd 100644
--- a/drivers/video/tmiofb.c
+++ b/drivers/video/tmiofb.c
@@ -25,7 +25,7 @@
 #include <linux/fb.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
-/* Why should fb driver call console functions? because acquire_console_sem() */
+/* Why should fb driver call console functions? because console_lock() */
 #include <linux/console.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/tmio.h>
@@ -250,8 +250,7 @@ static irqreturn_t tmiofb_irq(int irq, void *__info)
  */
 static int tmiofb_hw_stop(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data = dev->dev.platform_data;
         struct fb_info *info = platform_get_drvdata(dev);
         struct tmiofb_par *par = info->par;
 
@@ -268,7 +267,7 @@ static int tmiofb_hw_stop(struct platform_device *dev)
  */
 static int tmiofb_hw_init(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct fb_info *info = platform_get_drvdata(dev);
         struct tmiofb_par *par = info->par;
         const struct resource *nlcr = &cell->resources[0];
@@ -312,8 +311,7 @@ static int tmiofb_hw_init(struct platform_device *dev)
  */
 static void tmiofb_hw_mode(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data = dev->dev.platform_data;
         struct fb_info *info = platform_get_drvdata(dev);
         struct fb_videomode *mode = info->mode;
         struct tmiofb_par *par = info->par;
@@ -361,7 +359,7 @@ tmiofb_acc_wait(struct fb_info *info, unsigned int ccs)
 {
         struct tmiofb_par *par = info->par;
         /*
-         * This code can be called whith interrupts disabled.
+         * This code can be called with interrupts disabled.
          * So instead of relaying on irq to trigger the event,
          * poll the state till the necessary command is executed.
          */
@@ -559,9 +557,7 @@ static int tmiofb_ioctl(struct fb_info *fbi,
 static struct fb_videomode *
 tmiofb_find_mode(struct fb_info *info, struct fb_var_screeninfo *var)
 {
-        struct mfd_cell *cell =
-                info->device->platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data = info->device->platform_data;
         struct fb_videomode *best = NULL;
         int i;
 
@@ -581,9 +577,7 @@ static int tmiofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 
         struct fb_videomode *mode;
-        struct mfd_cell *cell =
-                info->device->platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data = info->device->platform_data;
 
         mode = tmiofb_find_mode(info, var);
         if (!mode || var->bits_per_pixel > 16)
@@ -683,8 +677,8 @@ static struct fb_ops tmiofb_ops = {
 
 static int __devinit tmiofb_probe(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
+        struct tmio_fb_data *data = dev->dev.platform_data;
         struct resource *ccr = platform_get_resource(dev, IORESOURCE_MEM, 1);
         struct resource *lcr = platform_get_resource(dev, IORESOURCE_MEM, 0);
         struct resource *vram = platform_get_resource(dev, IORESOURCE_MEM, 2);
@@ -811,7 +805,7 @@ err_ioremap_ccr:
 
 static int __devexit tmiofb_remove(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct fb_info *info = platform_get_drvdata(dev);
         int irq = platform_get_irq(dev, 0);
         struct tmiofb_par *par;
@@ -941,10 +935,10 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 #ifdef CONFIG_FB_TMIO_ACCELL
         struct tmiofb_par *par = info->par;
 #endif
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         int retval = 0;
 
-        acquire_console_sem();
+        console_lock();
 
         fb_set_suspend(info, 1);
 
@@ -965,7 +959,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
         if (cell->suspend)
                 retval = cell->suspend(dev);
 
-        release_console_sem();
+        console_unlock();
 
         return retval;
 }
@@ -973,10 +967,10 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 static int tmiofb_resume(struct platform_device *dev)
 {
         struct fb_info *info = platform_get_drvdata(dev);
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         int retval = 0;
 
-        acquire_console_sem();
+        console_lock();
 
         if (cell->resume) {
                 retval = cell->resume(dev);
@@ -992,7 +986,7 @@ static int tmiofb_resume(struct platform_device *dev)
 
         fb_set_suspend(info, 0);
 out:
-        release_console_sem();
+        console_unlock();
         return retval;
 }
 #else
diff --git a/drivers/video/udlfb.c b/drivers/video/udlfb.c
new file mode 100644
index 000000000000..816a4fda04f5
--- /dev/null
+++ b/drivers/video/udlfb.c
@@ -0,0 +1,1897 @@
+/*
+ * udlfb.c -- Framebuffer driver for DisplayLink USB controller
+ *
+ * Copyright (C) 2009 Roberto De Ioris <roberto@unbit.it>
+ * Copyright (C) 2009 Jaya Kumar <jayakumar.lkml@gmail.com>
+ * Copyright (C) 2009 Bernie Thompson <bernie@plugable.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License v2. See the file COPYING in the main directory of this archive for
+ * more details.
+ *
+ * Layout is based on skeletonfb by James Simmons and Geert Uytterhoeven,
+ * usb-skeleton by GregKH.
+ *
+ * Device-specific portions based on information from Displaylink, with work
+ * from Florian Echtler, Henrik Bjerregaard Pedersen, and others.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <linux/uaccess.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
+#include <linux/prefetch.h>
+#include <linux/delay.h>
+#include <linux/prefetch.h>
+#include <video/udlfb.h>
+#include "edid.h"
+
+static struct fb_fix_screeninfo dlfb_fix = {
+        .id =           "udlfb",
+        .type =         FB_TYPE_PACKED_PIXELS,
+        .visual =       FB_VISUAL_TRUECOLOR,
+        .xpanstep =     0,
+        .ypanstep =     0,
+        .ywrapstep =    0,
+        .accel =        FB_ACCEL_NONE,
+};
+
+static const u32 udlfb_info_flags = FBINFO_DEFAULT | FBINFO_READS_FAST |
+                FBINFO_VIRTFB |
+                FBINFO_HWACCEL_IMAGEBLIT | FBINFO_HWACCEL_FILLRECT |
+                FBINFO_HWACCEL_COPYAREA | FBINFO_MISC_ALWAYS_SETPAR;
+
+/*
+ * There are many DisplayLink-based products, all with unique PIDs. We are able
+ * to support all volume ones (circa 2009) with a single driver, so we match
+ * globally on VID. TODO: Probe() needs to detect when we might be running
+ * "future" chips, and bail on those, so a compatible driver can match.
+ */
+static struct usb_device_id id_table[] = {
+        {.idVendor = 0x17e9, .match_flags = USB_DEVICE_ID_MATCH_VENDOR,},
+        {},
+};
+MODULE_DEVICE_TABLE(usb, id_table);
+
+/* module options */
+static int console;   /* Optionally allow fbcon to consume first framebuffer */
+static int fb_defio;  /* Optionally enable experimental fb_defio mmap support */
+
+/* dlfb keeps a list of urbs for efficient bulk transfers */
+static void dlfb_urb_completion(struct urb *urb);
+static struct urb *dlfb_get_urb(struct dlfb_data *dev);
+static int dlfb_submit_urb(struct dlfb_data *dev, struct urb * urb, size_t len);
+static int dlfb_alloc_urb_list(struct dlfb_data *dev, int count, size_t size);
+static void dlfb_free_urb_list(struct dlfb_data *dev);
+
+/*
+ * All DisplayLink bulk operations start with 0xAF, followed by specific code
+ * All operations are written to buffers which then later get sent to device
+ */
+static char *dlfb_set_register(char *buf, u8 reg, u8 val)
+{
+        *buf++ = 0xAF;
+        *buf++ = 0x20;
+        *buf++ = reg;
+        *buf++ = val;
+        return buf;
+}
+
+static char *dlfb_vidreg_lock(char *buf)
+{
+        return dlfb_set_register(buf, 0xFF, 0x00);
+}
+
+static char *dlfb_vidreg_unlock(char *buf)
+{
+        return dlfb_set_register(buf, 0xFF, 0xFF);
+}
+
+/*
+ * On/Off for driving the DisplayLink framebuffer to the display
+ *  0x00 H and V sync on
+ *  0x01 H and V sync off (screen blank but powered)
+ *  0x07 DPMS powerdown (requires modeset to come back)
+ */
+static char *dlfb_enable_hvsync(char *buf, bool enable)
+{
+        if (enable)
+                return dlfb_set_register(buf, 0x1F, 0x00);
+        else
+                return dlfb_set_register(buf, 0x1F, 0x07);
+}
+
+static char *dlfb_set_color_depth(char *buf, u8 selection)
+{
+        return dlfb_set_register(buf, 0x00, selection);
+}
+
+static char *dlfb_set_base16bpp(char *wrptr, u32 base)
+{
+        /* the base pointer is 16 bits wide, 0x20 is hi byte. */
+        wrptr = dlfb_set_register(wrptr, 0x20, base >> 16);
+        wrptr = dlfb_set_register(wrptr, 0x21, base >> 8);
+        return dlfb_set_register(wrptr, 0x22, base);
+}
+
+/*
+ * DisplayLink HW has separate 16bpp and 8bpp framebuffers.
+ * In 24bpp modes, the low 323 RGB bits go in the 8bpp framebuffer
+ */
+static char *dlfb_set_base8bpp(char *wrptr, u32 base)
+{
+        wrptr = dlfb_set_register(wrptr, 0x26, base >> 16);
+        wrptr = dlfb_set_register(wrptr, 0x27, base >> 8);
+        return dlfb_set_register(wrptr, 0x28, base);
+}
+
+static char *dlfb_set_register_16(char *wrptr, u8 reg, u16 value)
+{
+        wrptr = dlfb_set_register(wrptr, reg, value >> 8);
+        return dlfb_set_register(wrptr, reg+1, value);
+}
+
+/*
+ * This is kind of weird because the controller takes some
+ * register values in a different byte order than other registers.
+ */
+static char *dlfb_set_register_16be(char *wrptr, u8 reg, u16 value)
+{
+        wrptr = dlfb_set_register(wrptr, reg, value);
+        return dlfb_set_register(wrptr, reg+1, value >> 8);
+}
+
+/*
+ * LFSR is linear feedback shift register. The reason we have this is
+ * because the display controller needs to minimize the clock depth of
+ * various counters used in the display path. So this code reverses the
+ * provided value into the lfsr16 value by counting backwards to get
+ * the value that needs to be set in the hardware comparator to get the
+ * same actual count. This makes sense once you read above a couple of
+ * times and think about it from a hardware perspective.
+ */
+static u16 dlfb_lfsr16(u16 actual_count)
+{
+        u32 lv = 0xFFFF; /* This is the lfsr value that the hw starts with */
+
+        while (actual_count--) {
+                lv =     ((lv << 1) |
+                        (((lv >> 15) ^ (lv >> 4) ^ (lv >> 2) ^ (lv >> 1)) & 1))
+                        & 0xFFFF;
+        }
+
+        return (u16) lv;
+}
+
+/*
+ * This does LFSR conversion on the value that is to be written.
+ * See LFSR explanation above for more detail.
+ */
+static char *dlfb_set_register_lfsr16(char *wrptr, u8 reg, u16 value)
+{
+        return dlfb_set_register_16(wrptr, reg, dlfb_lfsr16(value));
+}
+
+/*
+ * This takes a standard fbdev screeninfo struct and all of its monitor mode
+ * details and converts them into the DisplayLink equivalent register commands.
+ */
+static char *dlfb_set_vid_cmds(char *wrptr, struct fb_var_screeninfo *var)
+{
+        u16 xds, yds;
+        u16 xde, yde;
+        u16 yec;
+
+        /* x display start */
+        xds = var->left_margin + var->hsync_len;
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x01, xds);
+        /* x display end */
+        xde = xds + var->xres;
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x03, xde);
+
+        /* y display start */
+        yds = var->upper_margin + var->vsync_len;
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x05, yds);
+        /* y display end */
+        yde = yds + var->yres;
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x07, yde);
+
+        /* x end count is active + blanking - 1 */
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x09,
+                        xde + var->right_margin - 1);
+
+        /* libdlo hardcodes hsync start to 1 */
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x0B, 1);
+
+        /* hsync end is width of sync pulse + 1 */
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x0D, var->hsync_len + 1);
+
+        /* hpixels is active pixels */
+        wrptr = dlfb_set_register_16(wrptr, 0x0F, var->xres);
+
+        /* yendcount is vertical active + vertical blanking */
+        yec = var->yres + var->upper_margin + var->lower_margin +
+                        var->vsync_len;
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x11, yec);
+
+        /* libdlo hardcodes vsync start to 0 */
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x13, 0);
+
+        /* vsync end is width of vsync pulse */
+        wrptr = dlfb_set_register_lfsr16(wrptr, 0x15, var->vsync_len);
+
+        /* vpixels is active pixels */
+        wrptr = dlfb_set_register_16(wrptr, 0x17, var->yres);
+
+        /* convert picoseconds to 5kHz multiple for pclk5k = x * 1E12/5k */
+        wrptr = dlfb_set_register_16be(wrptr, 0x1B,
+                        200*1000*1000/var->pixclock);
+
+        return wrptr;
+}
+
+/*
+ * This takes a standard fbdev screeninfo struct that was fetched or prepared
+ * and then generates the appropriate command sequence that then drives the
+ * display controller.
+ */
+static int dlfb_set_video_mode(struct dlfb_data *dev,
+                                struct fb_var_screeninfo *var)
+{
+        char *buf;
+        char *wrptr;
+        int retval = 0;
+        int writesize;
+        struct urb *urb;
+
+        if (!atomic_read(&dev->usb_active))
+                return -EPERM;
+
+        urb = dlfb_get_urb(dev);
+        if (!urb)
+                return -ENOMEM;
+
+        buf = (char *) urb->transfer_buffer;
+
+        /*
+        * This first section has to do with setting the base address on the
+        * controller * associated with the display. There are 2 base
+        * pointers, currently, we only * use the 16 bpp segment.
+        */
+        wrptr = dlfb_vidreg_lock(buf);
+        wrptr = dlfb_set_color_depth(wrptr, 0x00);
+        /* set base for 16bpp segment to 0 */
+        wrptr = dlfb_set_base16bpp(wrptr, 0);
+        /* set base for 8bpp segment to end of fb */
+        wrptr = dlfb_set_base8bpp(wrptr, dev->info->fix.smem_len);
+
+        wrptr = dlfb_set_vid_cmds(wrptr, var);
+        wrptr = dlfb_enable_hvsync(wrptr, true);
+        wrptr = dlfb_vidreg_unlock(wrptr);
+
+        writesize = wrptr - buf;
+
+        retval = dlfb_submit_urb(dev, urb, writesize);
+
+        return retval;
+}
+
+static int dlfb_ops_mmap(struct fb_info *info, struct vm_area_struct *vma)
+{
+        unsigned long start = vma->vm_start;
+        unsigned long size = vma->vm_end - vma->vm_start;
+        unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+        unsigned long page, pos;
+
+        if (offset + size > info->fix.smem_len)
+                return -EINVAL;
+
+        pos = (unsigned long)info->fix.smem_start + offset;
+
+        pr_notice("mmap() framebuffer addr:%lu size:%lu\n",
+                  pos, size);
+
+        while (size > 0) {
+                page = vmalloc_to_pfn((void *)pos);
+                if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED))
+                        return -EAGAIN;
+
+                start += PAGE_SIZE;
+                pos += PAGE_SIZE;
+                if (size > PAGE_SIZE)
+                        size -= PAGE_SIZE;
+                else
+                        size = 0;
+        }
+
+        vma->vm_flags |= VM_RESERVED;   /* avoid to swap out this VMA */
+        return 0;
+}
+
+/*
+ * Trims identical data from front and back of line
+ * Sets new front buffer address and width
+ * And returns byte count of identical pixels
+ * Assumes CPU natural alignment (unsigned long)
+ * for back and front buffer ptrs and width
+ */
+static int dlfb_trim_hline(const u8 *bback, const u8 **bfront, int *width_bytes)
+{
+        int j, k;
+        const unsigned long *back = (const unsigned long *) bback;
+        const unsigned long *front = (const unsigned long *) *bfront;
+        const int width = *width_bytes / sizeof(unsigned long);
+        int identical = width;
+        int start = width;
+        int end = width;
+
+        prefetch((void *) front);
+        prefetch((void *) back);
+
+        for (j = 0; j < width; j++) {
+                if (back[j] != front[j]) {
+                        start = j;
+                        break;
+                }
+        }
+
+        for (k = width - 1; k > j; k--) {
+                if (back[k] != front[k]) {
+                        end = k+1;
+                        break;
+                }
+        }
+
+        identical = start + (width - end);
+        *bfront = (u8 *) &front[start];
+        *width_bytes = (end - start) * sizeof(unsigned long);
+
+        return identical * sizeof(unsigned long);
+}
+
+/*
+ * Render a command stream for an encoded horizontal line segment of pixels.
+ *
+ * A command buffer holds several commands.
+ * It always begins with a fresh command header
+ * (the protocol doesn't require this, but we enforce it to allow
+ * multiple buffers to be potentially encoded and sent in parallel).
+ * A single command encodes one contiguous horizontal line of pixels
+ *
+ * The function relies on the client to do all allocation, so that
+ * rendering can be done directly to output buffers (e.g. USB URBs).
+ * The function fills the supplied command buffer, providing information
+ * on where it left off, so the client may call in again with additional
+ * buffers if the line will take several buffers to complete.
+ *
+ * A single command can transmit a maximum of 256 pixels,
+ * regardless of the compression ratio (protocol design limit).
+ * To the hardware, 0 for a size byte means 256
+ *
+ * Rather than 256 pixel commands which are either rl or raw encoded,
+ * the rlx command simply assumes alternating raw and rl spans within one cmd.
+ * This has a slightly larger header overhead, but produces more even results.
+ * It also processes all data (read and write) in a single pass.
+ * Performance benchmarks of common cases show it having just slightly better
+ * compression than 256 pixel raw or rle commands, with similar CPU consumpion.
+ * But for very rl friendly data, will compress not quite as well.
+ */
+static void dlfb_compress_hline(
+        const uint16_t **pixel_start_ptr,
+        const uint16_t *const pixel_end,
+        uint32_t *device_address_ptr,
+        uint8_t **command_buffer_ptr,
+        const uint8_t *const cmd_buffer_end)
+{
+        const uint16_t *pixel = *pixel_start_ptr;
+        uint32_t dev_addr  = *device_address_ptr;
+        uint8_t *cmd = *command_buffer_ptr;
+        const int bpp = 2;
+
+        while ((pixel_end > pixel) &&
+               (cmd_buffer_end - MIN_RLX_CMD_BYTES > cmd)) {
+                uint8_t *raw_pixels_count_byte = 0;
+                uint8_t *cmd_pixels_count_byte = 0;
+                const uint16_t *raw_pixel_start = 0;
+                const uint16_t *cmd_pixel_start, *cmd_pixel_end = 0;
+
+                prefetchw((void *) cmd); /* pull in one cache line at least */
+
+                *cmd++ = 0xAF;
+                *cmd++ = 0x6B;
+                *cmd++ = (uint8_t) ((dev_addr >> 16) & 0xFF);
+                *cmd++ = (uint8_t) ((dev_addr >> 8) & 0xFF);
+                *cmd++ = (uint8_t) ((dev_addr) & 0xFF);
+
+                cmd_pixels_count_byte = cmd++; /*  we'll know this later */
+                cmd_pixel_start = pixel;
+
+                raw_pixels_count_byte = cmd++; /*  we'll know this later */
+                raw_pixel_start = pixel;
+
+                cmd_pixel_end = pixel + min(MAX_CMD_PIXELS + 1,
+                        min((int)(pixel_end - pixel),
+                            (int)(cmd_buffer_end - cmd) / bpp));
+
+                prefetch_range((void *) pixel, (cmd_pixel_end - pixel) * bpp);
+
+                while (pixel < cmd_pixel_end) {
+                        const uint16_t * const repeating_pixel = pixel;
+
+                        *(uint16_t *)cmd = cpu_to_be16p(pixel);
+                        cmd += 2;
+                        pixel++;
+
+                        if (unlikely((pixel < cmd_pixel_end) &&
+                                     (*pixel == *repeating_pixel))) {
+                                /* go back and fill in raw pixel count */
+                                *raw_pixels_count_byte = ((repeating_pixel -
+                                                raw_pixel_start) + 1) & 0xFF;
+
+                                while ((pixel < cmd_pixel_end)
+                                       && (*pixel == *repeating_pixel)) {
+                                        pixel++;
+                                }
+
+                                /* immediately after raw data is repeat byte */
+                                *cmd++ = ((pixel - repeating_pixel) - 1) & 0xFF;
+
+                                /* Then start another raw pixel span */
+                                raw_pixel_start = pixel;
+                                raw_pixels_count_byte = cmd++;
+                        }
+                }
+
+                if (pixel > raw_pixel_start) {
+                        /* finalize last RAW span */
+                        *raw_pixels_count_byte = (pixel-raw_pixel_start) & 0xFF;
+                }
+
+                *cmd_pixels_count_byte = (pixel - cmd_pixel_start) & 0xFF;
+                dev_addr += (pixel - cmd_pixel_start) * bpp;
+        }
+
+        if (cmd_buffer_end <= MIN_RLX_CMD_BYTES + cmd) {
+                /* Fill leftover bytes with no-ops */
+                if (cmd_buffer_end > cmd)
+                        memset(cmd, 0xAF, cmd_buffer_end - cmd);
+                cmd = (uint8_t *) cmd_buffer_end;
+        }
+
+        *command_buffer_ptr = cmd;
+        *pixel_start_ptr = pixel;
+        *device_address_ptr = dev_addr;
+
+        return;
+}
+
+/*
+ * There are 3 copies of every pixel: The front buffer that the fbdev
+ * client renders to, the actual framebuffer across the USB bus in hardware
+ * (that we can only write to, slowly, and can never read), and (optionally)
+ * our shadow copy that tracks what's been sent to that hardware buffer.
+ */
+static int dlfb_render_hline(struct dlfb_data *dev, struct urb **urb_ptr,
+                              const char *front, char **urb_buf_ptr,
+                              u32 byte_offset, u32 byte_width,
+                              int *ident_ptr, int *sent_ptr)
+{
+        const u8 *line_start, *line_end, *next_pixel;
+        u32 dev_addr = dev->base16 + byte_offset;
+        struct urb *urb = *urb_ptr;
+        u8 *cmd = *urb_buf_ptr;
+        u8 *cmd_end = (u8 *) urb->transfer_buffer + urb->transfer_buffer_length;
+
+        line_start = (u8 *) (front + byte_offset);
+        next_pixel = line_start;
+        line_end = next_pixel + byte_width;
+
+        if (dev->backing_buffer) {
+                int offset;
+                const u8 *back_start = (u8 *) (dev->backing_buffer
+                                                + byte_offset);
+
+                *ident_ptr += dlfb_trim_hline(back_start, &next_pixel,
+                        &byte_width);
+
+                offset = next_pixel - line_start;
+                line_end = next_pixel + byte_width;
+                dev_addr += offset;
+                back_start += offset;
+                line_start += offset;
+
+                memcpy((char *)back_start, (char *) line_start,
+                       byte_width);
+        }
+
+        while (next_pixel < line_end) {
+
+                dlfb_compress_hline((const uint16_t **) &next_pixel,
+                             (const uint16_t *) line_end, &dev_addr,
+                        (u8 **) &cmd, (u8 *) cmd_end);
+
+                if (cmd >= cmd_end) {
+                        int len = cmd - (u8 *) urb->transfer_buffer;
+                        if (dlfb_submit_urb(dev, urb, len))
+                                return 1; /* lost pixels is set */
+                        *sent_ptr += len;
+                        urb = dlfb_get_urb(dev);
+                        if (!urb)
+                                return 1; /* lost_pixels is set */
+                        *urb_ptr = urb;
+                        cmd = urb->transfer_buffer;
+                        cmd_end = &cmd[urb->transfer_buffer_length];
+                }
+        }
+
+        *urb_buf_ptr = cmd;
+
+        return 0;
+}
+
+int dlfb_handle_damage(struct dlfb_data *dev, int x, int y,
+               int width, int height, char *data)
+{
+        int i, ret;
+        char *cmd;
+        cycles_t start_cycles, end_cycles;
+        int bytes_sent = 0;
+        int bytes_identical = 0;
+        struct urb *urb;
+        int aligned_x;
+
+        start_cycles = get_cycles();
+
+        aligned_x = DL_ALIGN_DOWN(x, sizeof(unsigned long));
+        width = DL_ALIGN_UP(width + (x-aligned_x), sizeof(unsigned long));
+        x = aligned_x;
+
+        if ((width <= 0) ||
+            (x + width > dev->info->var.xres) ||
+            (y + height > dev->info->var.yres))
+                return -EINVAL;
+
+        if (!atomic_read(&dev->usb_active))
+                return 0;
+
+        urb = dlfb_get_urb(dev);
+        if (!urb)
+                return 0;
+        cmd = urb->transfer_buffer;
+
+        for (i = y; i < y + height ; i++) {
+                const int line_offset = dev->info->fix.line_length * i;
+                const int byte_offset = line_offset + (x * BPP);
+
+                if (dlfb_render_hline(dev, &urb,
+                                      (char *) dev->info->fix.smem_start,
+                                      &cmd, byte_offset, width * BPP,
+                                      &bytes_identical, &bytes_sent))
+                        goto error;
+        }
+
+        if (cmd > (char *) urb->transfer_buffer) {
+                /* Send partial buffer remaining before exiting */
+                int len = cmd - (char *) urb->transfer_buffer;
+                ret = dlfb_submit_urb(dev, urb, len);
+                bytes_sent += len;
+        } else
+                dlfb_urb_completion(urb);
+
+error:
+        atomic_add(bytes_sent, &dev->bytes_sent);
+        atomic_add(bytes_identical, &dev->bytes_identical);
+        atomic_add(width*height*2, &dev->bytes_rendered);
+        end_cycles = get_cycles();
+        atomic_add(((unsigned int) ((end_cycles - start_cycles)
+                    >> 10)), /* Kcycles */
+                   &dev->cpu_kcycles_used);
+
+        return 0;
+}
+
+/*
+ * Path triggered by usermode clients who write to filesystem
+ * e.g. cat filename > /dev/fb1
+ * Not used by X Windows or text-mode console. But useful for testing.
+ * Slow because of extra copy and we must assume all pixels dirty.
+ */
+static ssize_t dlfb_ops_write(struct fb_info *info, const char __user *buf,
+                          size_t count, loff_t *ppos)
+{
+        ssize_t result;
+        struct dlfb_data *dev = info->par;
+        u32 offset = (u32) *ppos;
+
+        result = fb_sys_write(info, buf, count, ppos);
+
+        if (result > 0) {
+                int start = max((int)(offset / info->fix.line_length) - 1, 0);
+                int lines = min((u32)((result / info->fix.line_length) + 1),
+                                (u32)info->var.yres);
+
+                dlfb_handle_damage(dev, 0, start, info->var.xres,
+                        lines, info->screen_base);
+        }
+
+        return result;
+}
+
+/* hardware has native COPY command (see libdlo), but not worth it for fbcon */
+static void dlfb_ops_copyarea(struct fb_info *info,
+                                const struct fb_copyarea *area)
+{
+
+        struct dlfb_data *dev = info->par;
+
+        sys_copyarea(info, area);
+
+        dlfb_handle_damage(dev, area->dx, area->dy,
+                        area->width, area->height, info->screen_base);
+}
+
+static void dlfb_ops_imageblit(struct fb_info *info,
+                                const struct fb_image *image)
+{
+        struct dlfb_data *dev = info->par;
+
+        sys_imageblit(info, image);
+
+        dlfb_handle_damage(dev, image->dx, image->dy,
+                        image->width, image->height, info->screen_base);
+}
+
+static void dlfb_ops_fillrect(struct fb_info *info,
+                          const struct fb_fillrect *rect)
+{
+        struct dlfb_data *dev = info->par;
+
+        sys_fillrect(info, rect);
+
+        dlfb_handle_damage(dev, rect->dx, rect->dy, rect->width,
+                              rect->height, info->screen_base);
+}
+
+/*
+ * NOTE: fb_defio.c is holding info->fbdefio.mutex
+ *   Touching ANY framebuffer memory that triggers a page fault
+ *   in fb_defio will cause a deadlock, when it also tries to
+ *   grab the same mutex.
+ */
+static void dlfb_dpy_deferred_io(struct fb_info *info,
+                                struct list_head *pagelist)
+{
+        struct page *cur;
+        struct fb_deferred_io *fbdefio = info->fbdefio;
+        struct dlfb_data *dev = info->par;
+        struct urb *urb;
+        char *cmd;
+        cycles_t start_cycles, end_cycles;
+        int bytes_sent = 0;
+        int bytes_identical = 0;
+        int bytes_rendered = 0;
+
+        if (!fb_defio)
+                return;
+
+        if (!atomic_read(&dev->usb_active))
+                return;
+
+        start_cycles = get_cycles();
+
+        urb = dlfb_get_urb(dev);
+        if (!urb)
+                return;
+
+        cmd = urb->transfer_buffer;
+
+        /* walk the written page list and render each to device */
+        list_for_each_entry(cur, &fbdefio->pagelist, lru) {
+
+                if (dlfb_render_hline(dev, &urb, (char *) info->fix.smem_start,
+                                  &cmd, cur->index << PAGE_SHIFT,
+                                  PAGE_SIZE, &bytes_identical, &bytes_sent))
+                        goto error;
+                bytes_rendered += PAGE_SIZE;
+        }
+
+        if (cmd > (char *) urb->transfer_buffer) {
+                /* Send partial buffer remaining before exiting */
+                int len = cmd - (char *) urb->transfer_buffer;
+                dlfb_submit_urb(dev, urb, len);
+                bytes_sent += len;
+        } else
+                dlfb_urb_completion(urb);
+
+error:
+        atomic_add(bytes_sent, &dev->bytes_sent);
+        atomic_add(bytes_identical, &dev->bytes_identical);
+        atomic_add(bytes_rendered, &dev->bytes_rendered);
+        end_cycles = get_cycles();
+        atomic_add(((unsigned int) ((end_cycles - start_cycles)
+                    >> 10)), /* Kcycles */
+                   &dev->cpu_kcycles_used);
+}
+
+static int dlfb_get_edid(struct dlfb_data *dev, char *edid, int len)
+{
+        int i;
+        int ret;
+        char *rbuf;
+
+        rbuf = kmalloc(2, GFP_KERNEL);
+        if (!rbuf)
+                return 0;
+
+        for (i = 0; i < len; i++) {
+                ret = usb_control_msg(dev->udev,
+                                    usb_rcvctrlpipe(dev->udev, 0), (0x02),
+                                    (0x80 | (0x02 << 5)), i << 8, 0xA1, rbuf, 2,
+                                    HZ);
+                if (ret < 1) {
+                        pr_err("Read EDID byte %d failed err %x\n", i, ret);
+                        i--;
+                        break;
+                }
+                edid[i] = rbuf[1];
+        }
+
+        kfree(rbuf);
+
+        return i;
+}
+
+static int dlfb_ops_ioctl(struct fb_info *info, unsigned int cmd,
+                                unsigned long arg)
+{
+
+        struct dlfb_data *dev = info->par;
+        struct dloarea *area = NULL;
+
+        if (!atomic_read(&dev->usb_active))
+                return 0;
+
+        /* TODO: Update X server to get this from sysfs instead */
+        if (cmd == DLFB_IOCTL_RETURN_EDID) {
+                char *edid = (char *)arg;
+                if (copy_to_user(edid, dev->edid, dev->edid_size))
+                        return -EFAULT;
+                return 0;
+        }
+
+        /* TODO: Help propose a standard fb.h ioctl to report mmap damage */
+        if (cmd == DLFB_IOCTL_REPORT_DAMAGE) {
+
+                /*
+                 * If we have a damage-aware client, turn fb_defio "off"
+                 * To avoid perf imact of unnecessary page fault handling.
+                 * Done by resetting the delay for this fb_info to a very
+                 * long period. Pages will become writable and stay that way.
+                 * Reset to normal value when all clients have closed this fb.
+                 */
+                if (info->fbdefio)
+                        info->fbdefio->delay = DL_DEFIO_WRITE_DISABLE;
+
+                area = (struct dloarea *)arg;
+
+                if (area->x < 0)
+                        area->x = 0;
+
+                if (area->x > info->var.xres)
+                        area->x = info->var.xres;
+
+                if (area->y < 0)
+                        area->y = 0;
+
+                if (area->y > info->var.yres)
+                        area->y = info->var.yres;
+
+                dlfb_handle_damage(dev, area->x, area->y, area->w, area->h,
+                           info->screen_base);
+        }
+
+        return 0;
+}
+
+/* taken from vesafb */
+static int
+dlfb_ops_setcolreg(unsigned regno, unsigned red, unsigned green,
+               unsigned blue, unsigned transp, struct fb_info *info)
+{
+        int err = 0;
+
+        if (regno >= info->cmap.len)
+                return 1;
+
+        if (regno < 16) {
+                if (info->var.red.offset == 10) {
+                        /* 1:5:5:5 */
+                        ((u32 *) (info->pseudo_palette))[regno] =
+                            ((red & 0xf800) >> 1) |
+                            ((green & 0xf800) >> 6) | ((blue & 0xf800) >> 11);
+                } else {
+                        /* 0:5:6:5 */
+                        ((u32 *) (info->pseudo_palette))[regno] =
+                            ((red & 0xf800)) |
+                            ((green & 0xfc00) >> 5) | ((blue & 0xf800) >> 11);
+                }
+        }
+
+        return err;
+}
+
+/*
+ * It's common for several clients to have framebuffer open simultaneously.
+ * e.g. both fbcon and X. Makes things interesting.
+ * Assumes caller is holding info->lock (for open and release at least)
+ */
+static int dlfb_ops_open(struct fb_info *info, int user)
+{
+        struct dlfb_data *dev = info->par;
+
+        /*
+         * fbcon aggressively connects to first framebuffer it finds,
+         * preventing other clients (X) from working properly. Usually
+         * not what the user wants. Fail by default with option to enable.
+         */
+        if ((user == 0) & (!console))
+                return -EBUSY;
+
+        /* If the USB device is gone, we don't accept new opens */
+        if (dev->virtualized)
+                return -ENODEV;
+
+        dev->fb_count++;
+
+        kref_get(&dev->kref);
+
+        if (fb_defio && (info->fbdefio == NULL)) {
+                /* enable defio at last moment if not disabled by client */
+
+                struct fb_deferred_io *fbdefio;
+
+                fbdefio = kmalloc(sizeof(struct fb_deferred_io), GFP_KERNEL);
+
+                if (fbdefio) {
+                        fbdefio->delay = DL_DEFIO_WRITE_DELAY;
+                        fbdefio->deferred_io = dlfb_dpy_deferred_io;
+                }
+
+                info->fbdefio = fbdefio;
+                fb_deferred_io_init(info);
+        }
+
+        pr_notice("open /dev/fb%d user=%d fb_info=%p count=%d\n",
+            info->node, user, info, dev->fb_count);
+
+        return 0;
+}
+
+/*
+ * Called when all client interfaces to start transactions have been disabled,
+ * and all references to our device instance (dlfb_data) are released.
+ * Every transaction must have a reference, so we know are fully spun down
+ */
+static void dlfb_free(struct kref *kref)
+{
+        struct dlfb_data *dev = container_of(kref, struct dlfb_data, kref);
+
+        /* this function will wait for all in-flight urbs to complete */
+        if (dev->urbs.count > 0)
+                dlfb_free_urb_list(dev);
+
+        if (dev->backing_buffer)
+                vfree(dev->backing_buffer);
+
+        kfree(dev->edid);
+
+        pr_warn("freeing dlfb_data %p\n", dev);
+
+        kfree(dev);
+}
+
+static void dlfb_release_urb_work(struct work_struct *work)
+{
+        struct urb_node *unode = container_of(work, struct urb_node,
+                                              release_urb_work.work);
+
+        up(&unode->dev->urbs.limit_sem);
+}
+
+static void dlfb_free_framebuffer_work(struct work_struct *work)
+{
+        struct dlfb_data *dev = container_of(work, struct dlfb_data,
+                                             free_framebuffer_work.work);
+        struct fb_info *info = dev->info;
+        int node = info->node;
+
+        unregister_framebuffer(info);
+
+        if (info->cmap.len != 0)
+                fb_dealloc_cmap(&info->cmap);
+        if (info->monspecs.modedb)
+                fb_destroy_modedb(info->monspecs.modedb);
+        if (info->screen_base)
+                vfree(info->screen_base);
+
+        fb_destroy_modelist(&info->modelist);
+
+        dev->info = 0;
+
+        /* Assume info structure is freed after this point */
+        framebuffer_release(info);
+
+        pr_warn("fb_info for /dev/fb%d has been freed\n", node);
+
+        /* ref taken in probe() as part of registering framebfufer */
+        kref_put(&dev->kref, dlfb_free);
+}
+
+/*
+ * Assumes caller is holding info->lock mutex (for open and release at least)
+ */
+static int dlfb_ops_release(struct fb_info *info, int user)
+{
+        struct dlfb_data *dev = info->par;
+
+        dev->fb_count--;
+
+        /* We can't free fb_info here - fbmem will touch it when we return */
+        if (dev->virtualized && (dev->fb_count == 0))
+                schedule_delayed_work(&dev->free_framebuffer_work, HZ);
+
+        if ((dev->fb_count == 0) && (info->fbdefio)) {
+                fb_deferred_io_cleanup(info);
+                kfree(info->fbdefio);
+                info->fbdefio = NULL;
+                info->fbops->fb_mmap = dlfb_ops_mmap;
+        }
+
+        pr_warn("released /dev/fb%d user=%d count=%d\n",
+                  info->node, user, dev->fb_count);
+
+        kref_put(&dev->kref, dlfb_free);
+
+        return 0;
+}
+
+/*
+ * Check whether a video mode is supported by the DisplayLink chip
+ * We start from monitor's modes, so don't need to filter that here
+ */
+static int dlfb_is_valid_mode(struct fb_videomode *mode,
+                struct fb_info *info)
+{
+        struct dlfb_data *dev = info->par;
+
+        if (mode->xres * mode->yres > dev->sku_pixel_limit) {
+                pr_warn("%dx%d beyond chip capabilities\n",
+                       mode->xres, mode->yres);
+                return 0;
+        }
+
+        pr_info("%dx%d valid mode\n", mode->xres, mode->yres);
+
+        return 1;
+}
+
+static void dlfb_var_color_format(struct fb_var_screeninfo *var)
+{
+        const struct fb_bitfield red = { 11, 5, 0 };
+        const struct fb_bitfield green = { 5, 6, 0 };
+        const struct fb_bitfield blue = { 0, 5, 0 };
+
+        var->bits_per_pixel = 16;
+        var->red = red;
+        var->green = green;
+        var->blue = blue;
+}
+
+static int dlfb_ops_check_var(struct fb_var_screeninfo *var,
+                                struct fb_info *info)
+{
+        struct fb_videomode mode;
+
+        /* TODO: support dynamically changing framebuffer size */
+        if ((var->xres * var->yres * 2) > info->fix.smem_len)
+                return -EINVAL;
+
+        /* set device-specific elements of var unrelated to mode */
+        dlfb_var_color_format(var);
+
+        fb_var_to_videomode(&mode, var);
+
+        if (!dlfb_is_valid_mode(&mode, info))
+                return -EINVAL;
+
+        return 0;
+}
+
+static int dlfb_ops_set_par(struct fb_info *info)
+{
+        struct dlfb_data *dev = info->par;
+        int result;
+        u16 *pix_framebuffer;
+        int i;
+
+        pr_notice("set_par mode %dx%d\n", info->var.xres, info->var.yres);
+
+        result = dlfb_set_video_mode(dev, &info->var);
+
+        if ((result == 0) && (dev->fb_count == 0)) {
+
+                /* paint greenscreen */
+
+                pix_framebuffer = (u16 *) info->screen_base;
+                for (i = 0; i < info->fix.smem_len / 2; i++)
+                        pix_framebuffer[i] = 0x37e6;
+
+                dlfb_handle_damage(dev, 0, 0, info->var.xres, info->var.yres,
+                                   info->screen_base);
+        }
+
+        return result;
+}
+
+/*
+ * In order to come back from full DPMS off, we need to set the mode again
+ */
+static int dlfb_ops_blank(int blank_mode, struct fb_info *info)
+{
+        struct dlfb_data *dev = info->par;
+
+        if (blank_mode != FB_BLANK_UNBLANK) {
+                char *bufptr;
+                struct urb *urb;
+
+                urb = dlfb_get_urb(dev);
+                if (!urb)
+                        return 0;
+
+                bufptr = (char *) urb->transfer_buffer;
+                bufptr = dlfb_vidreg_lock(bufptr);
+                bufptr = dlfb_enable_hvsync(bufptr, false);
+                bufptr = dlfb_vidreg_unlock(bufptr);
+
+                dlfb_submit_urb(dev, urb, bufptr -
+                                (char *) urb->transfer_buffer);
+        } else {
+                dlfb_set_video_mode(dev, &info->var);
+        }
+
+        return 0;
+}
+
+static struct fb_ops dlfb_ops = {
+        .owner = THIS_MODULE,
+        .fb_read = fb_sys_read,
+        .fb_write = dlfb_ops_write,
+        .fb_setcolreg = dlfb_ops_setcolreg,
+        .fb_fillrect = dlfb_ops_fillrect,
+        .fb_copyarea = dlfb_ops_copyarea,
+        .fb_imageblit = dlfb_ops_imageblit,
+        .fb_mmap = dlfb_ops_mmap,
+        .fb_ioctl = dlfb_ops_ioctl,
+        .fb_open = dlfb_ops_open,
+        .fb_release = dlfb_ops_release,
+        .fb_blank = dlfb_ops_blank,
+        .fb_check_var = dlfb_ops_check_var,
+        .fb_set_par = dlfb_ops_set_par,
+};
+
+
+/*
+ * Assumes &info->lock held by caller
+ * Assumes no active clients have framebuffer open
+ */
+static int dlfb_realloc_framebuffer(struct dlfb_data *dev, struct fb_info *info)
+{
+        int retval = -ENOMEM;
+        int old_len = info->fix.smem_len;
+        int new_len;
+        unsigned char *old_fb = info->screen_base;
+        unsigned char *new_fb;
+        unsigned char *new_back;
+
+        pr_warn("Reallocating framebuffer. Addresses will change!\n");
+
+        new_len = info->fix.line_length * info->var.yres;
+
+        if (PAGE_ALIGN(new_len) > old_len) {
+                /*
+                 * Alloc system memory for virtual framebuffer
+                 */
+                new_fb = vmalloc(new_len);
+                if (!new_fb) {
+                        pr_err("Virtual framebuffer alloc failed\n");
+                        goto error;
+                }
+
+                if (info->screen_base) {
+                        memcpy(new_fb, old_fb, old_len);
+                        vfree(info->screen_base);
+                }
+
+                info->screen_base = new_fb;
+                info->fix.smem_len = PAGE_ALIGN(new_len);
+                info->fix.smem_start = (unsigned long) new_fb;
+                info->flags = udlfb_info_flags;
+
+                /*
+                 * Second framebuffer copy to mirror the framebuffer state
+                 * on the physical USB device. We can function without this.
+                 * But with imperfect damage info we may send pixels over USB
+                 * that were, in fact, unchanged - wasting limited USB bandwidth
+                 */
+                new_back = vzalloc(new_len);
+                if (!new_back)
+                        pr_info("No shadow/backing buffer allocated\n");
+                else {
+                        if (dev->backing_buffer)
+                                vfree(dev->backing_buffer);
+                        dev->backing_buffer = new_back;
+                }
+        }
+
+        retval = 0;
+
+error:
+        return retval;
+}
+
+/*
+ * 1) Get EDID from hw, or use sw default
+ * 2) Parse into various fb_info structs
+ * 3) Allocate virtual framebuffer memory to back highest res mode
+ *
+ * Parses EDID into three places used by various parts of fbdev:
+ * fb_var_screeninfo contains the timing of the monitor's preferred mode
+ * fb_info.monspecs is full parsed EDID info, including monspecs.modedb
+ * fb_info.modelist is a linked list of all monitor & VESA modes which work
+ *
+ * If EDID is not readable/valid, then modelist is all VESA modes,
+ * monspecs is NULL, and fb_var_screeninfo is set to safe VESA mode
+ * Returns 0 if successful
+ */
+static int dlfb_setup_modes(struct dlfb_data *dev,
+                           struct fb_info *info,
+                           char *default_edid, size_t default_edid_size)
+{
+        int i;
+        const struct fb_videomode *default_vmode = NULL;
+        int result = 0;
+        char *edid;
+        int tries = 3;
+
+        if (info->dev) /* only use mutex if info has been registered */
+                mutex_lock(&info->lock);
+
+        edid = kmalloc(EDID_LENGTH, GFP_KERNEL);
+        if (!edid) {
+                result = -ENOMEM;
+                goto error;
+        }
+
+        fb_destroy_modelist(&info->modelist);
+        memset(&info->monspecs, 0, sizeof(info->monspecs));
+
+        /*
+         * Try to (re)read EDID from hardware first
+         * EDID data may return, but not parse as valid
+         * Try again a few times, in case of e.g. analog cable noise
+         */
+        while (tries--) {
+
+                i = dlfb_get_edid(dev, edid, EDID_LENGTH);
+
+                if (i >= EDID_LENGTH)
+                        fb_edid_to_monspecs(edid, &info->monspecs);
+
+                if (info->monspecs.modedb_len > 0) {
+                        dev->edid = edid;
+                        dev->edid_size = i;
+                        break;
+                }
+        }
+
+        /* If that fails, use a previously returned EDID if available */
+        if (info->monspecs.modedb_len == 0) {
+
+                pr_err("Unable to get valid EDID from device/display\n");
+
+                if (dev->edid) {
+                        fb_edid_to_monspecs(dev->edid, &info->monspecs);
+                        if (info->monspecs.modedb_len > 0)
+                                pr_err("Using previously queried EDID\n");
+                }
+        }
+
+        /* If that fails, use the default EDID we were handed */
+        if (info->monspecs.modedb_len == 0) {
+                if (default_edid_size >= EDID_LENGTH) {
+                        fb_edid_to_monspecs(default_edid, &info->monspecs);
+                        if (info->monspecs.modedb_len > 0) {
+                                memcpy(edid, default_edid, default_edid_size);
+                                dev->edid = edid;
+                                dev->edid_size = default_edid_size;
+                                pr_err("Using default/backup EDID\n");
+                        }
+                }
+        }
+
+        /* If we've got modes, let's pick a best default mode */
+        if (info->monspecs.modedb_len > 0) {
+
+                for (i = 0; i < info->monspecs.modedb_len; i++) {
+                        if (dlfb_is_valid_mode(&info->monspecs.modedb[i], info))
+                                fb_add_videomode(&info->monspecs.modedb[i],
+                                        &info->modelist);
+                        else {
+                                if (i == 0)
+                                        /* if we've removed top/best mode */
+                                        info->monspecs.misc
+                                                &= ~FB_MISC_1ST_DETAIL;
+                        }
+                }
+
+                default_vmode = fb_find_best_display(&info->monspecs,
+                                                     &info->modelist);
+        }
+
+        /* If everything else has failed, fall back to safe default mode */
+        if (default_vmode == NULL) {
+
+                struct fb_videomode fb_vmode = {0};
+
+                /*
+                 * Add the standard VESA modes to our modelist
+                 * Since we don't have EDID, there may be modes that
+                 * overspec monitor and/or are incorrect aspect ratio, etc.
+                 * But at least the user has a chance to choose
+                 */
+                for (i = 0; i < VESA_MODEDB_SIZE; i++) {
+                        if (dlfb_is_valid_mode((struct fb_videomode *)
+                                                &vesa_modes[i], info))
+                                fb_add_videomode(&vesa_modes[i],
+                                                 &info->modelist);
+                }
+
+                /*
+                 * default to resolution safe for projectors
+                 * (since they are most common case without EDID)
+                 */
+                fb_vmode.xres = 800;
+                fb_vmode.yres = 600;
+                fb_vmode.refresh = 60;
+                default_vmode = fb_find_nearest_mode(&fb_vmode,
+                                                     &info->modelist);
+        }
+
+        /* If we have good mode and no active clients*/
+        if ((default_vmode != NULL) && (dev->fb_count == 0)) {
+
+                fb_videomode_to_var(&info->var, default_vmode);
+                dlfb_var_color_format(&info->var);
+
+                /*
+                 * with mode size info, we can now alloc our framebuffer.
+                 */
+                memcpy(&info->fix, &dlfb_fix, sizeof(dlfb_fix));
+                info->fix.line_length = info->var.xres *
+                        (info->var.bits_per_pixel / 8);
+
+                result = dlfb_realloc_framebuffer(dev, info);
+
+        } else
+                result = -EINVAL;
+
+error:
+        if (edid && (dev->edid != edid))
+                kfree(edid);
+
+        if (info->dev)
+                mutex_unlock(&info->lock);
+
+        return result;
+}
+
+static ssize_t metrics_bytes_rendered_show(struct device *fbdev,
+                                   struct device_attribute *a, char *buf) {
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+        return snprintf(buf, PAGE_SIZE, "%u\n",
+                        atomic_read(&dev->bytes_rendered));
+}
+
+static ssize_t metrics_bytes_identical_show(struct device *fbdev,
+                                   struct device_attribute *a, char *buf) {
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+        return snprintf(buf, PAGE_SIZE, "%u\n",
+                        atomic_read(&dev->bytes_identical));
+}
+
+static ssize_t metrics_bytes_sent_show(struct device *fbdev,
+                                   struct device_attribute *a, char *buf) {
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+        return snprintf(buf, PAGE_SIZE, "%u\n",
+                        atomic_read(&dev->bytes_sent));
+}
+
+static ssize_t metrics_cpu_kcycles_used_show(struct device *fbdev,
+                                   struct device_attribute *a, char *buf) {
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+        return snprintf(buf, PAGE_SIZE, "%u\n",
+                        atomic_read(&dev->cpu_kcycles_used));
+}
+
+static ssize_t edid_show(
+                        struct file *filp,
+                        struct kobject *kobj, struct bin_attribute *a,
+                         char *buf, loff_t off, size_t count) {
+        struct device *fbdev = container_of(kobj, struct device, kobj);
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+
+        if (dev->edid == NULL)
+                return 0;
+
+        if ((off >= dev->edid_size) || (count > dev->edid_size))
+                return 0;
+
+        if (off + count > dev->edid_size)
+                count = dev->edid_size - off;
+
+        pr_info("sysfs edid copy %p to %p, %d bytes\n",
+                dev->edid, buf, (int) count);
+
+        memcpy(buf, dev->edid, count);
+
+        return count;
+}
+
+static ssize_t edid_store(
+                        struct file *filp,
+                        struct kobject *kobj, struct bin_attribute *a,
+                        char *src, loff_t src_off, size_t src_size) {
+        struct device *fbdev = container_of(kobj, struct device, kobj);
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+
+        /* We only support write of entire EDID at once, no offset*/
+        if ((src_size != EDID_LENGTH) || (src_off != 0))
+                return 0;
+
+        dlfb_setup_modes(dev, fb_info, src, src_size);
+
+        if (dev->edid && (memcmp(src, dev->edid, src_size) == 0)) {
+                pr_info("sysfs written EDID is new default\n");
+                dlfb_ops_set_par(fb_info);
+                return src_size;
+        } else
+                return 0;
+}
+
+static ssize_t metrics_reset_store(struct device *fbdev,
+                           struct device_attribute *attr,
+                           const char *buf, size_t count)
+{
+        struct fb_info *fb_info = dev_get_drvdata(fbdev);
+        struct dlfb_data *dev = fb_info->par;
+
+        atomic_set(&dev->bytes_rendered, 0);
+        atomic_set(&dev->bytes_identical, 0);
+        atomic_set(&dev->bytes_sent, 0);
+        atomic_set(&dev->cpu_kcycles_used, 0);
+
+        return count;
+}
+
+static struct bin_attribute edid_attr = {
+        .attr.name = "edid",
+        .attr.mode = 0666,
+        .size = EDID_LENGTH,
+        .read = edid_show,
+        .write = edid_store
+};
+
+static struct device_attribute fb_device_attrs[] = {
+        __ATTR_RO(metrics_bytes_rendered),
+        __ATTR_RO(metrics_bytes_identical),
+        __ATTR_RO(metrics_bytes_sent),
+        __ATTR_RO(metrics_cpu_kcycles_used),
+        __ATTR(metrics_reset, S_IWUSR, NULL, metrics_reset_store),
+};
+
+/*
+ * This is necessary before we can communicate with the display controller.
+ */
+static int dlfb_select_std_channel(struct dlfb_data *dev)
+{
+        int ret;
+        u8 set_def_chn[] = {       0x57, 0xCD, 0xDC, 0xA7,
+                                0x1C, 0x88, 0x5E, 0x15,
+                                0x60, 0xFE, 0xC6, 0x97,
+                                0x16, 0x3D, 0x47, 0xF2  };
+
+        ret = usb_control_msg(dev->udev, usb_sndctrlpipe(dev->udev, 0),
+                        NR_USB_REQUEST_CHANNEL,
+                        (USB_DIR_OUT | USB_TYPE_VENDOR), 0, 0,
+                        set_def_chn, sizeof(set_def_chn), USB_CTRL_SET_TIMEOUT);
+        return ret;
+}
+
+static int dlfb_parse_vendor_descriptor(struct dlfb_data *dev,
+                                        struct usb_device *usbdev)
+{
+        char *desc;
+        char *buf;
+        char *desc_end;
+
+        u8 total_len = 0;
+
+        buf = kzalloc(MAX_VENDOR_DESCRIPTOR_SIZE, GFP_KERNEL);
+        if (!buf)
+                return false;
+        desc = buf;
+
+        total_len = usb_get_descriptor(usbdev, 0x5f, /* vendor specific */
+                                    0, desc, MAX_VENDOR_DESCRIPTOR_SIZE);
+        if (total_len > 5) {
+                pr_info("vendor descriptor length:%x data:%02x %02x %02x %02x" \
+                        "%02x %02x %02x %02x %02x %02x %02x\n",
+                        total_len, desc[0],
+                        desc[1], desc[2], desc[3], desc[4], desc[5], desc[6],
+                        desc[7], desc[8], desc[9], desc[10]);
+
+                if ((desc[0] != total_len) || /* descriptor length */
+                    (desc[1] != 0x5f) ||   /* vendor descriptor type */
+                    (desc[2] != 0x01) ||   /* version (2 bytes) */
+                    (desc[3] != 0x00) ||
+                    (desc[4] != total_len - 2)) /* length after type */
+                        goto unrecognized;
+
+                desc_end = desc + total_len;
+                desc += 5; /* the fixed header we've already parsed */
+
+                while (desc < desc_end) {
+                        u8 length;
+                        u16 key;
+
+                        key = *((u16 *) desc);
+                        desc += sizeof(u16);
+                        length = *desc;
+                        desc++;
+
+                        switch (key) {
+                        case 0x0200: { /* max_area */
+                                u32 max_area;
+                                max_area = le32_to_cpu(*((u32 *)desc));
+                                pr_warn("DL chip limited to %d pixel modes\n",
+                                        max_area);
+                                dev->sku_pixel_limit = max_area;
+                                break;
+                        }
+                        default:
+                                break;
+                        }
+                        desc += length;
+                }
+        }
+
+        goto success;
+
+unrecognized:
+        /* allow udlfb to load for now even if firmware unrecognized */
+        pr_err("Unrecognized vendor firmware descriptor\n");
+
+success:
+        kfree(buf);
+        return true;
+}
+static int dlfb_usb_probe(struct usb_interface *interface,
+                        const struct usb_device_id *id)
+{
+        struct usb_device *usbdev;
+        struct dlfb_data *dev = 0;
+        struct fb_info *info = 0;
+        int retval = -ENOMEM;
+        int i;
+
+        /* usb initialization */
+
+        usbdev = interface_to_usbdev(interface);
+
+        dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+        if (dev == NULL) {
+                err("dlfb_usb_probe: failed alloc of dev struct\n");
+                goto error;
+        }
+
+        /* we need to wait for both usb and fbdev to spin down on disconnect */
+        kref_init(&dev->kref); /* matching kref_put in usb .disconnect fn */
+        kref_get(&dev->kref); /* matching kref_put in free_framebuffer_work */
+
+        dev->udev = usbdev;
+        dev->gdev = &usbdev->dev; /* our generic struct device * */
+        usb_set_intfdata(interface, dev);
+
+        pr_info("%s %s - serial #%s\n",
+                usbdev->manufacturer, usbdev->product, usbdev->serial);
+        pr_info("vid_%04x&pid_%04x&rev_%04x driver's dlfb_data struct at %p\n",
+                usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
+                usbdev->descriptor.bcdDevice, dev);
+        pr_info("console enable=%d\n", console);
+        pr_info("fb_defio enable=%d\n", fb_defio);
+
+        dev->sku_pixel_limit = 2048 * 1152; /* default to maximum */
+
+        if (!dlfb_parse_vendor_descriptor(dev, usbdev)) {
+                pr_err("firmware not recognized. Assume incompatible device\n");
+                goto error;
+        }
+
+        if (!dlfb_alloc_urb_list(dev, WRITES_IN_FLIGHT, MAX_TRANSFER)) {
+                retval = -ENOMEM;
+                pr_err("dlfb_alloc_urb_list failed\n");
+                goto error;
+        }
+
+        /* We don't register a new USB class. Our client interface is fbdev */
+
+        /* allocates framebuffer driver structure, not framebuffer memory */
+        info = framebuffer_alloc(0, &usbdev->dev);
+        if (!info) {
+                retval = -ENOMEM;
+                pr_err("framebuffer_alloc failed\n");
+                goto error;
+        }
+
+        dev->info = info;
+        info->par = dev;
+        info->pseudo_palette = dev->pseudo_palette;
+        info->fbops = &dlfb_ops;
+
+        retval = fb_alloc_cmap(&info->cmap, 256, 0);
+        if (retval < 0) {
+                pr_err("fb_alloc_cmap failed %x\n", retval);
+                goto error;
+        }
+
+        INIT_DELAYED_WORK(&dev->free_framebuffer_work,
+                          dlfb_free_framebuffer_work);
+
+        INIT_LIST_HEAD(&info->modelist);
+
+        retval = dlfb_setup_modes(dev, info, NULL, 0);
+        if (retval != 0) {
+                pr_err("unable to find common mode for display and adapter\n");
+                goto error;
+        }
+
+        /* ready to begin using device */
+
+        atomic_set(&dev->usb_active, 1);
+        dlfb_select_std_channel(dev);
+
+        dlfb_ops_check_var(&info->var, info);
+        dlfb_ops_set_par(info);
+
+        retval = register_framebuffer(info);
+        if (retval < 0) {
+                pr_err("register_framebuffer failed %d\n", retval);
+                goto error;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(fb_device_attrs); i++) {
+                retval = device_create_file(info->dev, &fb_device_attrs[i]);
+                if (retval) {
+                        pr_err("device_create_file failed %d\n", retval);
+                        goto err_del_attrs;
+                }
+        }
+
+        retval = device_create_bin_file(info->dev, &edid_attr);
+        if (retval) {
+                pr_err("device_create_bin_file failed %d\n", retval);
+                goto err_del_attrs;
+        }
+
+        pr_info("DisplayLink USB device /dev/fb%d attached. %dx%d resolution."
+                        " Using %dK framebuffer memory\n", info->node,
+                        info->var.xres, info->var.yres,
+                        ((dev->backing_buffer) ?
+                        info->fix.smem_len * 2 : info->fix.smem_len) >> 10);
+        return 0;
+
+err_del_attrs:
+        for (i -= 1; i >= 0; i--)
+                device_remove_file(info->dev, &fb_device_attrs[i]);
+
+error:
+        if (dev) {
+
+                if (info) {
+                        if (info->cmap.len != 0)
+                                fb_dealloc_cmap(&info->cmap);
+                        if (info->monspecs.modedb)
+                                fb_destroy_modedb(info->monspecs.modedb);
+                        if (info->screen_base)
+                                vfree(info->screen_base);
+
+                        fb_destroy_modelist(&info->modelist);
+
+                        framebuffer_release(info);
+                }
+
+                if (dev->backing_buffer)
+                        vfree(dev->backing_buffer);
+
+                kref_put(&dev->kref, dlfb_free); /* ref for framebuffer */
+                kref_put(&dev->kref, dlfb_free); /* last ref from kref_init */
+
+                /* dev has been deallocated. Do not dereference */
+        }
+
+        return retval;
+}
+
+static void dlfb_usb_disconnect(struct usb_interface *interface)
+{
+        struct dlfb_data *dev;
+        struct fb_info *info;
+        int i;
+
+        dev = usb_get_intfdata(interface);
+        info = dev->info;
+
+        pr_info("USB disconnect starting\n");
+
+        /* we virtualize until all fb clients release. Then we free */
+        dev->virtualized = true;
+
+        /* When non-active we'll update virtual framebuffer, but no new urbs */
+        atomic_set(&dev->usb_active, 0);
+
+        /* remove udlfb's sysfs interfaces */
+        for (i = 0; i < ARRAY_SIZE(fb_device_attrs); i++)
+                device_remove_file(info->dev, &fb_device_attrs[i]);
+        device_remove_bin_file(info->dev, &edid_attr);
+
+        usb_set_intfdata(interface, NULL);
+
+        /* if clients still have us open, will be freed on last close */
+        if (dev->fb_count == 0)
+                schedule_delayed_work(&dev->free_framebuffer_work, 0);
+
+        /* release reference taken by kref_init in probe() */
+        kref_put(&dev->kref, dlfb_free);
+
+        /* consider dlfb_data freed */
+
+        return;
+}
+
+static struct usb_driver dlfb_driver = {
+        .name = "udlfb",
+        .probe = dlfb_usb_probe,
+        .disconnect = dlfb_usb_disconnect,
+        .id_table = id_table,
+};
+
+static int __init dlfb_module_init(void)
+{
+        int res;
+
+        res = usb_register(&dlfb_driver);
+        if (res)
+                err("usb_register failed. Error number %d", res);
+
+        return res;
+}
+
+static void __exit dlfb_module_exit(void)
+{
+        usb_deregister(&dlfb_driver);
+}
+
+module_init(dlfb_module_init);
+module_exit(dlfb_module_exit);
+
+static void dlfb_urb_completion(struct urb *urb)
+{
+        struct urb_node *unode = urb->context;
+        struct dlfb_data *dev = unode->dev;
+        unsigned long flags;
+
+        /* sync/async unlink faults aren't errors */
+        if (urb->status) {
+                if (!(urb->status == -ENOENT ||
+                    urb->status == -ECONNRESET ||
+                    urb->status == -ESHUTDOWN)) {
+                        pr_err("%s - nonzero write bulk status received: %d\n",
+                                __func__, urb->status);
+                        atomic_set(&dev->lost_pixels, 1);
+                }
+        }
+
+        urb->transfer_buffer_length = dev->urbs.size; /* reset to actual */
+
+        spin_lock_irqsave(&dev->urbs.lock, flags);
+        list_add_tail(&unode->entry, &dev->urbs.list);
+        dev->urbs.available++;
+        spin_unlock_irqrestore(&dev->urbs.lock, flags);
+
+        /*
+         * When using fb_defio, we deadlock if up() is called
+         * while another is waiting. So queue to another process.
+         */
+        if (fb_defio)
+                schedule_delayed_work(&unode->release_urb_work, 0);
+        else
+                up(&dev->urbs.limit_sem);
+}
+
+static void dlfb_free_urb_list(struct dlfb_data *dev)
+{
+        int count = dev->urbs.count;
+        struct list_head *node;
+        struct urb_node *unode;
+        struct urb *urb;
+        int ret;
+        unsigned long flags;
+
+        pr_notice("Waiting for completes and freeing all render urbs\n");
+
+        /* keep waiting and freeing, until we've got 'em all */
+        while (count--) {
+
+                /* Getting interrupted means a leak, but ok at shutdown*/
+                ret = down_interruptible(&dev->urbs.limit_sem);
+                if (ret)
+                        break;
+
+                spin_lock_irqsave(&dev->urbs.lock, flags);
+
+                node = dev->urbs.list.next; /* have reserved one with sem */
+                list_del_init(node);
+
+                spin_unlock_irqrestore(&dev->urbs.lock, flags);
+
+                unode = list_entry(node, struct urb_node, entry);
+                urb = unode->urb;
+
+                /* Free each separately allocated piece */
+                usb_free_coherent(urb->dev, dev->urbs.size,
+                                  urb->transfer_buffer, urb->transfer_dma);
+                usb_free_urb(urb);
+                kfree(node);
+        }
+
+}
+
+static int dlfb_alloc_urb_list(struct dlfb_data *dev, int count, size_t size)
+{
+        int i = 0;
+        struct urb *urb;
+        struct urb_node *unode;
+        char *buf;
+
+        spin_lock_init(&dev->urbs.lock);
+
+        dev->urbs.size = size;
+        INIT_LIST_HEAD(&dev->urbs.list);
+
+        while (i < count) {
+                unode = kzalloc(sizeof(struct urb_node), GFP_KERNEL);
+                if (!unode)
+                        break;
+                unode->dev = dev;
+
+                INIT_DELAYED_WORK(&unode->release_urb_work,
+                          dlfb_release_urb_work);
+
+                urb = usb_alloc_urb(0, GFP_KERNEL);
+                if (!urb) {
+                        kfree(unode);
+                        break;
+                }
+                unode->urb = urb;
+
+                buf = usb_alloc_coherent(dev->udev, MAX_TRANSFER, GFP_KERNEL,
+                                         &urb->transfer_dma);
+                if (!buf) {
+                        kfree(unode);
+                        usb_free_urb(urb);
+                        break;
+                }
+
+                /* urb->transfer_buffer_length set to actual before submit */
+                usb_fill_bulk_urb(urb, dev->udev, usb_sndbulkpipe(dev->udev, 1),
+                        buf, size, dlfb_urb_completion, unode);
+                urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+                list_add_tail(&unode->entry, &dev->urbs.list);
+
+                i++;
+        }
+
+        sema_init(&dev->urbs.limit_sem, i);
+        dev->urbs.count = i;
+        dev->urbs.available = i;
+
+        pr_notice("allocated %d %d byte urbs\n", i, (int) size);
+
+        return i;
+}
+
+static struct urb *dlfb_get_urb(struct dlfb_data *dev)
+{
+        int ret = 0;
+        struct list_head *entry;
+        struct urb_node *unode;
+        struct urb *urb = NULL;
+        unsigned long flags;
+
+        /* Wait for an in-flight buffer to complete and get re-queued */
+        ret = down_timeout(&dev->urbs.limit_sem, GET_URB_TIMEOUT);
+        if (ret) {
+                atomic_set(&dev->lost_pixels, 1);
+                pr_warn("wait for urb interrupted: %x available: %d\n",
+                       ret, dev->urbs.available);
+                goto error;
+        }
+
+        spin_lock_irqsave(&dev->urbs.lock, flags);
+
+        BUG_ON(list_empty(&dev->urbs.list)); /* reserved one with limit_sem */
+        entry = dev->urbs.list.next;
+        list_del_init(entry);
+        dev->urbs.available--;
+
+        spin_unlock_irqrestore(&dev->urbs.lock, flags);
+
+        unode = list_entry(entry, struct urb_node, entry);
+        urb = unode->urb;
+
+error:
+        return urb;
+}
+
+static int dlfb_submit_urb(struct dlfb_data *dev, struct urb *urb, size_t len)
+{
+        int ret;
+
+        BUG_ON(len > dev->urbs.size);
+
+        urb->transfer_buffer_length = len; /* set to actual payload len */
+        ret = usb_submit_urb(urb, GFP_KERNEL);
+        if (ret) {
+                dlfb_urb_completion(urb); /* because no one else will */
+                atomic_set(&dev->lost_pixels, 1);
+                pr_err("usb_submit_urb error %x\n", ret);
+        }
+        return ret;
+}
+
+module_param(console, bool, S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP);
+MODULE_PARM_DESC(console, "Allow fbcon to consume first framebuffer found");
+
+module_param(fb_defio, bool, S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP);
+MODULE_PARM_DESC(fb_defio, "Enable fb_defio mmap support. *Experimental*");
+
+MODULE_AUTHOR("Roberto De Ioris <roberto@unbit.it>, "
+              "Jaya Kumar <jayakumar.lkml@gmail.com>, "
+              "Bernie Thompson <bernie@plugable.com>");
+MODULE_DESCRIPTION("DisplayLink kernel framebuffer driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index 52ec0959d462..7f8472cc993b 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -73,7 +73,7 @@ static void uvesafb_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *ns
         struct uvesafb_task *utask;
         struct uvesafb_ktask *task;
 
-        if (!cap_raised(nsp->eff_cap, CAP_SYS_ADMIN))
+        if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
                 return;
 
         if (msg->seq >= UVESAFB_TASKS_MAX)
@@ -1552,8 +1552,7 @@ static void __devinit uvesafb_init_mtrr(struct fb_info *info)
                         int rc;
 
                         /* Find the largest power-of-two */
-                        while (temp_size & (temp_size - 1))
-                                temp_size &= (temp_size - 1);
+                        temp_size = roundup_pow_of_two(temp_size);
 
                         /* Try and find a power of two to add */
                         do {
@@ -1566,6 +1565,28 @@ static void __devinit uvesafb_init_mtrr(struct fb_info *info)
 #endif /* CONFIG_MTRR */
 }
 
+static void __devinit uvesafb_ioremap(struct fb_info *info)
+{
+#ifdef CONFIG_X86
+        switch (mtrr) {
+        case 1: /* uncachable */
+                info->screen_base = ioremap_nocache(info->fix.smem_start, info->fix.smem_len);
+                break;
+        case 2: /* write-back */
+                info->screen_base = ioremap_cache(info->fix.smem_start, info->fix.smem_len);
+                break;
+        case 3: /* write-combining */
+                info->screen_base = ioremap_wc(info->fix.smem_start, info->fix.smem_len);
+                break;
+        case 4: /* write-through */
+        default:
+                info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+                break;
+        }
+#else
+        info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+#endif /* CONFIG_X86 */
+}
 
 static ssize_t uvesafb_show_vbe_ver(struct device *dev,
                 struct device_attribute *attr, char *buf)
@@ -1736,15 +1757,22 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 
         uvesafb_init_info(info, mode);
 
+        if (!request_region(0x3c0, 32, "uvesafb")) {
+                printk(KERN_ERR "uvesafb: request region 0x3c0-0x3e0 failed\n");
+                err = -EIO;
+                goto out_mode;
+        }
+
         if (!request_mem_region(info->fix.smem_start, info->fix.smem_len,
                                 "uvesafb")) {
                 printk(KERN_ERR "uvesafb: cannot reserve video memory at "
                                 "0x%lx\n", info->fix.smem_start);
                 err = -EIO;
-                goto out_mode;
+                goto out_reg;
         }
 
-        info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+        uvesafb_init_mtrr(info);
+        uvesafb_ioremap(info);
 
         if (!info->screen_base) {
                 printk(KERN_ERR
@@ -1755,20 +1783,13 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
                 goto out_mem;
         }
 
-        if (!request_region(0x3c0, 32, "uvesafb")) {
-                printk(KERN_ERR "uvesafb: request region 0x3c0-0x3e0 failed\n");
-                err = -EIO;
-                goto out_unmap;
-        }
-
-        uvesafb_init_mtrr(info);
         platform_set_drvdata(dev, info);
 
         if (register_framebuffer(info) < 0) {
                 printk(KERN_ERR
                         "uvesafb: failed to register framebuffer device\n");
                 err = -EINVAL;
-                goto out_reg;
+                goto out_unmap;
         }
 
         printk(KERN_INFO "uvesafb: framebuffer at 0x%lx, mapped to 0x%p, "
@@ -1785,12 +1806,12 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 
         return 0;
 
-out_reg:
-        release_region(0x3c0, 32);
 out_unmap:
         iounmap(info->screen_base);
 out_mem:
         release_mem_region(info->fix.smem_start, info->fix.smem_len);
+out_reg:
+        release_region(0x3c0, 32);
 out_mode:
         if (!list_empty(&info->modelist))
                 fb_destroy_modelist(&info->modelist);
diff --git a/drivers/video/vermilion/vermilion.c b/drivers/video/vermilion/vermilion.c
index 931a567f9aff..970e43d13f52 100644
--- a/drivers/video/vermilion/vermilion.c
+++ b/drivers/video/vermilion/vermilion.c
@@ -891,8 +891,7 @@ static int vmlfb_set_par(struct fb_info *info)
         int ret;
 
         mutex_lock(&vml_mutex);
-        list_del(&vinfo->head);
-        list_add(&vinfo->head, (subsys) ? &global_has_mode : &global_no_mode);
+        list_move(&vinfo->head, (subsys) ? &global_has_mode : &global_no_mode);
         ret = vmlfb_set_par_locked(vinfo);
 
         mutex_unlock(&vml_mutex);
diff --git a/drivers/video/vesafb.c b/drivers/video/vesafb.c
index 090aa1a9be6e..501b3406c6d5 100644
--- a/drivers/video/vesafb.c
+++ b/drivers/video/vesafb.c
@@ -175,6 +175,7 @@ static int vesafb_setcolreg(unsigned regno, unsigned red, unsigned green,
 
 static void vesafb_destroy(struct fb_info *info)
 {
+        fb_dealloc_cmap(&info->cmap);
         if (info->screen_base)
                 iounmap(info->screen_base);
         release_mem_region(info->apertures->ranges[0].base, info->apertures->ranges[0].size);
@@ -253,7 +254,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)
@@ -303,19 +304,6 @@ static int __init vesafb_probe(struct platform_device *dev)
         info->apertures->ranges[0].base = screen_info.lfb_base;
         info->apertures->ranges[0].size = size_total;
 
-        info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
-        if (!info->screen_base) {
-                printk(KERN_ERR
-                       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
-                        vesafb_fix.smem_len, vesafb_fix.smem_start);
-                err = -EIO;
-                goto err;
-        }
-
-        printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, "
-               "using %dk, total %dk\n",
-               vesafb_fix.smem_start, info->screen_base,
-               size_remap/1024, size_total/1024);
         printk(KERN_INFO "vesafb: mode is %dx%dx%d, linelength=%d, pages=%d\n",
                vesafb_defined.xres, vesafb_defined.yres, vesafb_defined.bits_per_pixel, vesafb_fix.line_length, screen_info.pages);
 
@@ -438,8 +426,7 @@ static int __init vesafb_probe(struct platform_device *dev)
                         int rc;
 
                         /* Find the largest power-of-two */
-                        while (temp_size & (temp_size - 1))
-                                temp_size &= (temp_size - 1);
+                        temp_size = roundup_pow_of_two(temp_size);
 
                         /* Try and find a power of two to add */
                         do {
@@ -451,6 +438,34 @@ static int __init vesafb_probe(struct platform_device *dev)
         }
 #endif
         
+        switch (mtrr) {
+        case 1: /* uncachable */
+                info->screen_base = ioremap_nocache(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        case 2: /* write-back */
+                info->screen_base = ioremap_cache(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        case 3: /* write-combining */
+                info->screen_base = ioremap_wc(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        case 4: /* write-through */
+        default:
+                info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        }
+        if (!info->screen_base) {
+                printk(KERN_ERR
+                       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
+                        vesafb_fix.smem_len, vesafb_fix.smem_start);
+                err = -EIO;
+                goto err;
+        }
+
+        printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, "
+               "using %dk, total %dk\n",
+               vesafb_fix.smem_start, info->screen_base,
+               size_remap/1024, size_total/1024);
+
         info->fbops = &vesafb_ops;
         info->var = vesafb_defined;
         info->fix = vesafb_fix;
diff --git a/drivers/video/vga16fb.c b/drivers/video/vga16fb.c
index 28ccab44a391..305c975b1787 100644
--- a/drivers/video/vga16fb.c
+++ b/drivers/video/vga16fb.c
@@ -152,7 +152,7 @@ static inline int setop(int op)
 }
 
 /* Set the Enable Set/Reset Register and return its old value.  
-   The code here always uses value 0xf for thsi register. */
+   The code here always uses value 0xf for this register. */
 static inline int setsr(int sr)
 {
         int oldsr;
@@ -1265,9 +1265,11 @@ static void vga16fb_imageblit(struct fb_info *info, const struct fb_image *image
 
 static void vga16fb_destroy(struct fb_info *info)
 {
+        struct platform_device *dev = container_of(info->device, struct platform_device, dev);
         iounmap(info->screen_base);
         fb_dealloc_cmap(&info->cmap);
         /* XXX unshare VGA regions */
+        platform_set_drvdata(dev, NULL);
         framebuffer_release(info);
 }
 
diff --git a/drivers/video/via/Makefile b/drivers/video/via/Makefile
index d496adb0f832..5108136e8776 100644
--- a/drivers/video/via/Makefile
+++ b/drivers/video/via/Makefile
@@ -5,5 +5,5 @@
 obj-$(CONFIG_FB_VIA) += viafb.o
 
 viafb-y :=viafbdev.o hw.o via_i2c.o dvi.o lcd.o ioctl.o accel.o \
-        via_utility.o vt1636.o global.o tblDPASetting.o viamode.o tbl1636.o \
-        via-core.o via-gpio.o via_modesetting.o
+        via_utility.o vt1636.o global.o tblDPASetting.o viamode.o \
+        via-core.o via-gpio.o via_modesetting.o via_clock.o
diff --git a/drivers/video/via/accel.c b/drivers/video/via/accel.c
index e44893ea590d..4b67b8e6030a 100644
--- a/drivers/video/via/accel.c
+++ b/drivers/video/via/accel.c
@@ -283,11 +283,12 @@ static int hw_bitblt_2(void __iomem *engine, u8 op, u32 width, u32 height,
                 writel(tmp, engine + 0x1C);
         }
 
-        if (op != VIA_BITBLT_COLOR)
+        if (op == VIA_BITBLT_FILL) {
+                writel(fg_color, engine + 0x58);
+        } else if (op == VIA_BITBLT_MONO) {
                 writel(fg_color, engine + 0x4C);
-
-        if (op == VIA_BITBLT_MONO)
                 writel(bg_color, engine + 0x50);
+        }
 
         if (op == VIA_BITBLT_FILL)
                 ge_cmd |= fill_rop << 24 | 0x00002000 | 0x00000001;
@@ -314,13 +315,11 @@ static int hw_bitblt_2(void __iomem *engine, u8 op, u32 width, u32 height,
         return 0;
 }
 
-int viafb_init_engine(struct fb_info *info)
+int viafb_setup_engine(struct fb_info *info)
 {
         struct viafb_par *viapar = info->par;
         void __iomem *engine;
-        int highest_reg, i;
-        u32 vq_start_addr, vq_end_addr, vq_start_low, vq_end_low, vq_high,
-                vq_len, chip_name = viapar->shared->chip_info.gfx_chip_name;
+        u32 chip_name = viapar->shared->chip_info.gfx_chip_name;
 
         engine = viapar->shared->vdev->engine_mmio;
         if (!engine) {
@@ -329,18 +328,6 @@ int viafb_init_engine(struct fb_info *info)
                 return -ENOMEM;
         }
 
-        /* Initialize registers to reset the 2D engine */
-        switch (viapar->shared->chip_info.twod_engine) {
-        case VIA_2D_ENG_M1:
-                highest_reg = 0x5c;
-                break;
-        default:
-                highest_reg = 0x40;
-                break;
-        }
-        for (i = 0; i <= highest_reg; i += 4)
-                writel(0x0, engine + i);
-
         switch (chip_name) {
         case UNICHROME_CLE266:
         case UNICHROME_K400:
@@ -356,6 +343,7 @@ int viafb_init_engine(struct fb_info *info)
                 break;
         case UNICHROME_VX800:
         case UNICHROME_VX855:
+        case UNICHROME_VX900:
                 viapar->shared->hw_bitblt = hw_bitblt_2;
                 break;
         default:
@@ -370,7 +358,7 @@ int viafb_init_engine(struct fb_info *info)
         viapar->shared->vq_vram_addr = viapar->fbmem_free;
         viapar->fbmem_used += VQ_SIZE;
 
-#if defined(CONFIG_FB_VIA_CAMERA) || defined(CONFIG_FB_VIA_CAMERA_MODULE)
+#if defined(CONFIG_VIDEO_VIA_CAMERA) || defined(CONFIG_VIDEO_VIA_CAMERA_MODULE)
         /*
          * Set aside a chunk of framebuffer memory for the camera
          * driver.  Someday this driver probably needs a proper allocator
@@ -386,12 +374,36 @@ int viafb_init_engine(struct fb_info *info)
         viapar->shared->vdev->camera_fbmem_offset = viapar->fbmem_free;
 #endif
 
+        viafb_reset_engine(viapar);
+        return 0;
+}
+
+void viafb_reset_engine(struct viafb_par *viapar)
+{
+        void __iomem *engine = viapar->shared->vdev->engine_mmio;
+        int highest_reg, i;
+        u32 vq_start_addr, vq_end_addr, vq_start_low, vq_end_low, vq_high,
+                vq_len, chip_name = viapar->shared->chip_info.gfx_chip_name;
+
+        /* Initialize registers to reset the 2D engine */
+        switch (viapar->shared->chip_info.twod_engine) {
+        case VIA_2D_ENG_M1:
+                highest_reg = 0x5c;
+                break;
+        default:
+                highest_reg = 0x40;
+                break;
+        }
+        for (i = 0; i <= highest_reg; i += 4)
+                writel(0x0, engine + i);
+
         /* Init AGP and VQ regs */
         switch (chip_name) {
         case UNICHROME_K8M890:
         case UNICHROME_P4M900:
         case UNICHROME_VX800:
         case UNICHROME_VX855:
+        case UNICHROME_VX900:
                 writel(0x00100000, engine + VIA_REG_CR_TRANSET);
                 writel(0x680A0000, engine + VIA_REG_CR_TRANSPACE);
                 writel(0x02000000, engine + VIA_REG_CR_TRANSPACE);
@@ -428,6 +440,7 @@ int viafb_init_engine(struct fb_info *info)
         case UNICHROME_P4M900:
         case UNICHROME_VX800:
         case UNICHROME_VX855:
+        case UNICHROME_VX900:
                 vq_start_low |= 0x20000000;
                 vq_end_low |= 0x20000000;
                 vq_high |= 0x20000000;
@@ -473,7 +486,7 @@ int viafb_init_engine(struct fb_info *info)
         writel(0x0, engine + VIA_REG_CURSOR_ORG);
         writel(0x0, engine + VIA_REG_CURSOR_BG);
         writel(0x0, engine + VIA_REG_CURSOR_FG);
-        return 0;
+        return;
 }
 
 void viafb_show_hw_cursor(struct fb_info *info, int Status)
diff --git a/drivers/video/via/accel.h b/drivers/video/via/accel.h
index 2c122d292365..79d5e10cc835 100644
--- a/drivers/video/via/accel.h
+++ b/drivers/video/via/accel.h
@@ -203,7 +203,8 @@
 #define VIA_BITBLT_MONO         2
 #define VIA_BITBLT_FILL         3
 
-int viafb_init_engine(struct fb_info *info);
+int viafb_setup_engine(struct fb_info *info);
+void viafb_reset_engine(struct viafb_par *viapar);
 void viafb_show_hw_cursor(struct fb_info *info, int Status);
 void viafb_wait_engine_idle(struct fb_info *info);
 
diff --git a/drivers/video/via/chip.h b/drivers/video/via/chip.h
index ef1f3de2e052..3ebf20c06eef 100644
--- a/drivers/video/via/chip.h
+++ b/drivers/video/via/chip.h
@@ -71,6 +71,9 @@
 #define     UNICHROME_VX855         12
 #define     UNICHROME_VX855_DID     0x5122
 
+#define     UNICHROME_VX900         13
+#define     UNICHROME_VX900_DID     0x7122
+
 /**************************************************/
 /* Definition TMDS Trasmitter Information         */
 /**************************************************/
@@ -107,16 +110,13 @@
 struct tmds_chip_information {
         int tmds_chip_name;
         int tmds_chip_slave_addr;
-        int data_mode;
         int output_interface;
         int i2c_port;
-        int device_type;
 };
 
 struct lvds_chip_information {
         int lvds_chip_name;
         int lvds_chip_slave_addr;
-        int data_mode;
         int output_interface;
         int i2c_port;
 };
@@ -137,21 +137,11 @@ struct chip_information {
         struct lvds_chip_information lvds_chip_info2;
 };
 
-struct crt_setting_information {
-        int iga_path;
-        int h_active;
-        int v_active;
-        int bpp;
-        int refresh_rate;
-};
-
 struct tmds_setting_information {
         int iga_path;
         int h_active;
         int v_active;
         int max_pixel_clock;
-        int max_hres;
-        int max_vres;
 };
 
 struct lvds_setting_information {
@@ -159,8 +149,6 @@ struct lvds_setting_information {
         int h_active;
         int v_active;
         int bpp;
-        int refresh_rate;
-        int lcd_panel_id;
         int lcd_panel_hres;
         int lcd_panel_vres;
         int display_method;
@@ -185,7 +173,6 @@ struct GFX_DPA_SETTING {
 };
 
 struct VT1636_DPA_SETTING {
-        int PanelSizeID;
         u8 CLK_SEL_ST1;
         u8 CLK_SEL_ST2;
 };
diff --git a/drivers/video/via/dvi.c b/drivers/video/via/dvi.c
index 39b040bb3817..b1f364745ca0 100644
--- a/drivers/video/via/dvi.c
+++ b/drivers/video/via/dvi.c
@@ -25,42 +25,29 @@
 static void tmds_register_write(int index, u8 data);
 static int tmds_register_read(int index);
 static int tmds_register_read_bytes(int index, u8 *buff, int buff_len);
-static void dvi_get_panel_size_from_DDCv1(struct tmds_chip_information
-        *tmds_chip, struct tmds_setting_information *tmds_setting);
-static void dvi_get_panel_size_from_DDCv2(struct tmds_chip_information
-        *tmds_chip, struct tmds_setting_information *tmds_setting);
+static void __devinit dvi_get_panel_size_from_DDCv1(
+        struct tmds_chip_information *tmds_chip,
+        struct tmds_setting_information *tmds_setting);
 static int viafb_dvi_query_EDID(void);
 
-static int check_tmds_chip(int device_id_subaddr, int device_id)
+static inline bool check_tmds_chip(int device_id_subaddr, int device_id)
 {
-        if (tmds_register_read(device_id_subaddr) == device_id)
-                return OK;
-        else
-                return FAIL;
+        return tmds_register_read(device_id_subaddr) == device_id;
 }
 
-void viafb_init_dvi_size(struct tmds_chip_information *tmds_chip,
+void __devinit viafb_init_dvi_size(struct tmds_chip_information *tmds_chip,
         struct tmds_setting_information *tmds_setting)
 {
         DEBUG_MSG(KERN_INFO "viafb_init_dvi_size()\n");
 
         viafb_dvi_sense();
-        switch (viafb_dvi_query_EDID()) {
-        case 1:
+        if (viafb_dvi_query_EDID() == 1)
                 dvi_get_panel_size_from_DDCv1(tmds_chip, tmds_setting);
-                break;
-        case 2:
-                dvi_get_panel_size_from_DDCv2(tmds_chip, tmds_setting);
-                break;
-        default:
-                printk(KERN_WARNING "viafb_init_dvi_size: DVI panel size undetected!\n");
-                break;
-        }
 
         return;
 }
 
-int viafb_tmds_trasmitter_identify(void)
+bool __devinit viafb_tmds_trasmitter_identify(void)
 {
         unsigned char sr2a = 0, sr1e = 0, sr3e = 0;
 
@@ -99,7 +86,7 @@ int viafb_tmds_trasmitter_identify(void)
         viaparinfo->chip_info->
                 tmds_chip_info.tmds_chip_slave_addr = VT1632_TMDS_I2C_ADDR;
         viaparinfo->chip_info->tmds_chip_info.i2c_port = VIA_PORT_31;
-        if (check_tmds_chip(VT1632_DEVICE_ID_REG, VT1632_DEVICE_ID) != FAIL) {
+        if (check_tmds_chip(VT1632_DEVICE_ID_REG, VT1632_DEVICE_ID)) {
                 /*
                  * Currently only support 12bits,dual edge,add 24bits mode later
                  */
@@ -110,11 +97,10 @@ int viafb_tmds_trasmitter_identify(void)
                           viaparinfo->chip_info->tmds_chip_info.tmds_chip_name);
                 DEBUG_MSG(KERN_INFO "\n %2d",
                           viaparinfo->chip_info->tmds_chip_info.i2c_port);
-                return OK;
+                return true;
         } else {
                 viaparinfo->chip_info->tmds_chip_info.i2c_port = VIA_PORT_2C;
-                if (check_tmds_chip(VT1632_DEVICE_ID_REG, VT1632_DEVICE_ID)
-                    != FAIL) {
+                if (check_tmds_chip(VT1632_DEVICE_ID_REG, VT1632_DEVICE_ID)) {
                         tmds_register_write(0x08, 0x3b);
                         DEBUG_MSG(KERN_INFO "\n VT1632 TMDS ! \n");
                         DEBUG_MSG(KERN_INFO "\n %2d",
@@ -123,7 +109,7 @@ int viafb_tmds_trasmitter_identify(void)
                         DEBUG_MSG(KERN_INFO "\n %2d",
                                   viaparinfo->chip_info->
                                   tmds_chip_info.i2c_port);
-                        return OK;
+                        return true;
                 }
         }
 
@@ -133,7 +119,7 @@ int viafb_tmds_trasmitter_identify(void)
             ((viafb_display_hardware_layout == HW_LAYOUT_DVI_ONLY) ||
              (viafb_display_hardware_layout == HW_LAYOUT_LCD_DVI))) {
                 DEBUG_MSG(KERN_INFO "\n Integrated TMDS ! \n");
-                return OK;
+                return true;
         }
 
         switch (viaparinfo->chip_info->gfx_chip_name) {
@@ -157,7 +143,7 @@ int viafb_tmds_trasmitter_identify(void)
                 tmds_chip_info.tmds_chip_name = NON_TMDS_TRANSMITTER;
         viaparinfo->chip_info->tmds_chip_info.
                 tmds_chip_slave_addr = VT1632_TMDS_I2C_ADDR;
-        return FAIL;
+        return false;
 }
 
 static void tmds_register_write(int index, u8 data)
@@ -193,7 +179,9 @@ void viafb_dvi_set_mode(struct VideoModeTable *mode, int mode_bpp,
         struct crt_mode_table *pDviTiming;
         unsigned long desirePixelClock, maxPixelClock;
         pDviTiming = mode->crtc;
-        desirePixelClock = pDviTiming->clk / 1000000;
+        desirePixelClock = pDviTiming->refresh_rate
+                * pDviTiming->crtc.hor_total * pDviTiming->crtc.ver_total
+                / 1000000;
         maxPixelClock = (unsigned long)viaparinfo->
                 tmds_setting_info->max_pixel_clock;
 
@@ -208,8 +196,6 @@ void viafb_dvi_set_mode(struct VideoModeTable *mode, int mode_bpp,
                 }
         }
         viafb_fill_crtc_timing(pDviTiming, mode, mode_bpp / 8, set_iga);
-        viafb_set_output_path(DEVICE_DVI, set_iga,
-                        viaparinfo->chip_info->tmds_chip_info.output_interface);
 }
 
 /* Sense DVI Connector */
@@ -304,62 +290,23 @@ static int viafb_dvi_query_EDID(void)
                 return EDID_VERSION_1;  /* Found EDID1 Table */
         }
 
-        data0 = (u8) tmds_register_read(0x00);
-        viaparinfo->chip_info->tmds_chip_info.tmds_chip_slave_addr = restore;
-        if (data0 == 0x20)
-                return EDID_VERSION_2;  /* Found EDID2 Table */
-        else
-                return false;
+        return false;
 }
 
 /* Get Panel Size Using EDID1 Table */
-static void dvi_get_panel_size_from_DDCv1(struct tmds_chip_information
-        *tmds_chip, struct tmds_setting_information *tmds_setting)
+static void __devinit dvi_get_panel_size_from_DDCv1(
+        struct tmds_chip_information *tmds_chip,
+        struct tmds_setting_information *tmds_setting)
 {
-        int i, max_h = 0, tmp, restore;
-        unsigned char rData;
+        int i, restore;
         unsigned char EDID_DATA[18];
 
         DEBUG_MSG(KERN_INFO "\n dvi_get_panel_size_from_DDCv1 \n");
 
         restore = tmds_chip->tmds_chip_slave_addr;
         tmds_chip->tmds_chip_slave_addr = 0xA0;
-
-        rData = tmds_register_read(0x23);
-        if (rData & 0x3C)
-                max_h = 640;
-        if (rData & 0xC0)
-                max_h = 720;
-        if (rData & 0x03)
-                max_h = 800;
-
-        rData = tmds_register_read(0x24);
-        if (rData & 0xC0)
-                max_h = 800;
-        if (rData & 0x1E)
-                max_h = 1024;
-        if (rData & 0x01)
-                max_h = 1280;
-
         for (i = 0x25; i < 0x6D; i++) {
                 switch (i) {
-                case 0x26:
-                case 0x28:
-                case 0x2A:
-                case 0x2C:
-                case 0x2E:
-                case 0x30:
-                case 0x32:
-                case 0x34:
-                        rData = tmds_register_read(i);
-                        if (rData == 1)
-                                break;
-                        /* data = (data + 31) * 8 */
-                        tmp = (rData + 31) << 3;
-                        if (tmp > max_h)
-                                max_h = tmp;
-                        break;
-
                 case 0x36:
                 case 0x48:
                 case 0x5A:
@@ -380,118 +327,85 @@ static void dvi_get_panel_size_from_DDCv1(struct tmds_chip_information
                 }
         }
 
-        tmds_setting->max_hres = max_h;
-        switch (max_h) {
-        case 640:
-                tmds_setting->max_vres = 480;
-                break;
-        case 800:
-                tmds_setting->max_vres = 600;
-                break;
-        case 1024:
-                tmds_setting->max_vres = 768;
-                break;
-        case 1280:
-                tmds_setting->max_vres = 1024;
-                break;
-        case 1400:
-                tmds_setting->max_vres = 1050;
-                break;
-        case 1440:
-                tmds_setting->max_vres = 1050;
-                break;
-        case 1600:
-                tmds_setting->max_vres = 1200;
-                break;
-        case 1920:
-                tmds_setting->max_vres = 1080;
-                break;
-        default:
-                DEBUG_MSG(KERN_INFO "Unknown panel size max resolution = %d ! "
-                                         "set default panel size.\n", max_h);
-                break;
-        }
-
         DEBUG_MSG(KERN_INFO "DVI max pixelclock = %d\n",
                 tmds_setting->max_pixel_clock);
         tmds_chip->tmds_chip_slave_addr = restore;
 }
 
-/* Get Panel Size Using EDID2 Table */
-static void dvi_get_panel_size_from_DDCv2(struct tmds_chip_information
-        *tmds_chip, struct tmds_setting_information *tmds_setting)
+/* If Disable DVI, turn off pad */
+void viafb_dvi_disable(void)
 {
-        int restore;
-        unsigned char R_Buffer[2];
+        if (viaparinfo->chip_info->
+                tmds_chip_info.output_interface == INTERFACE_TMDS)
+                /* Turn off TMDS power. */
+                viafb_write_reg(CRD2, VIACR,
+                viafb_read_reg(VIACR, CRD2) | 0x08);
+}
 
-        DEBUG_MSG(KERN_INFO "\n dvi_get_panel_size_from_DDCv2 \n");
+static void dvi_patch_skew_dvp0(void)
+{
+        /* Reset data driving first: */
+        viafb_write_reg_mask(SR1B, VIASR, 0, BIT1);
+        viafb_write_reg_mask(SR2A, VIASR, 0, BIT4);
 
-        restore = tmds_chip->tmds_chip_slave_addr;
-        tmds_chip->tmds_chip_slave_addr = 0xA2;
+        switch (viaparinfo->chip_info->gfx_chip_name) {
+        case UNICHROME_P4M890:
+                {
+                        if ((viaparinfo->tmds_setting_info->h_active == 1600) &&
+                                (viaparinfo->tmds_setting_info->v_active ==
+                                1200))
+                                viafb_write_reg_mask(CR96, VIACR, 0x03,
+                                               BIT0 + BIT1 + BIT2);
+                        else
+                                viafb_write_reg_mask(CR96, VIACR, 0x07,
+                                               BIT0 + BIT1 + BIT2);
+                        break;
+                }
 
-        /* Horizontal: 0x76, 0x77 */
-        tmds_register_read_bytes(0x76, R_Buffer, 2);
-        tmds_setting->max_hres = R_Buffer[0] + (R_Buffer[1] << 8);
+        case UNICHROME_P4M900:
+                {
+                        viafb_write_reg_mask(CR96, VIACR, 0x07,
+                                       BIT0 + BIT1 + BIT2 + BIT3);
+                        viafb_write_reg_mask(SR1B, VIASR, 0x02, BIT1);
+                        viafb_write_reg_mask(SR2A, VIASR, 0x10, BIT4);
+                        break;
+                }
 
-        switch (tmds_setting->max_hres) {
-        case 640:
-                tmds_setting->max_vres = 480;
-                break;
-        case 800:
-                tmds_setting->max_vres = 600;
-                break;
-        case 1024:
-                tmds_setting->max_vres = 768;
-                break;
-        case 1280:
-                tmds_setting->max_vres = 1024;
-                break;
-        case 1400:
-                tmds_setting->max_vres = 1050;
-                break;
-        case 1440:
-                tmds_setting->max_vres = 1050;
-                break;
-        case 1600:
-                tmds_setting->max_vres = 1200;
-                break;
         default:
-                DEBUG_MSG(KERN_INFO "Unknown panel size max resolution = %d! "
-                        "set default panel size.\n", tmds_setting->max_hres);
-                break;
+                {
+                        break;
+                }
         }
-
-        tmds_chip->tmds_chip_slave_addr = restore;
 }
 
-/* If Disable DVI, turn off pad */
-void viafb_dvi_disable(void)
+static void dvi_patch_skew_dvp_low(void)
 {
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DVP0)
-                viafb_write_reg(SR1E, VIASR,
-                viafb_read_reg(VIASR, SR1E) & (~0xC0));
-
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DVP1)
-                viafb_write_reg(SR1E, VIASR,
-                viafb_read_reg(VIASR, SR1E) & (~0x30));
+        switch (viaparinfo->chip_info->gfx_chip_name) {
+        case UNICHROME_K8M890:
+                {
+                        viafb_write_reg_mask(CR99, VIACR, 0x03, BIT0 + BIT1);
+                        break;
+                }
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DFP_HIGH)
-                viafb_write_reg(SR2A, VIASR,
-                viafb_read_reg(VIASR, SR2A) & (~0x0C));
+        case UNICHROME_P4M900:
+                {
+                        viafb_write_reg_mask(CR99, VIACR, 0x08,
+                                       BIT0 + BIT1 + BIT2 + BIT3);
+                        break;
+                }
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DFP_LOW)
-                viafb_write_reg(SR2A, VIASR,
-                viafb_read_reg(VIASR, SR2A) & (~0x03));
+        case UNICHROME_P4M890:
+                {
+                        viafb_write_reg_mask(CR99, VIACR, 0x0F,
+                                       BIT0 + BIT1 + BIT2 + BIT3);
+                        break;
+                }
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_TMDS)
-                /* Turn off TMDS power. */
-                viafb_write_reg(CRD2, VIACR,
-                viafb_read_reg(VIACR, CRD2) | 0x08);
+        default:
+                {
+                        break;
+                }
+        }
 }
 
 /* If Enable DVI, turn off pad */
@@ -499,33 +413,30 @@ void viafb_dvi_enable(void)
 {
         u8 data;
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DVP0) {
-                viafb_write_reg(SR1E, VIASR,
-                        viafb_read_reg(VIASR, SR1E) | 0xC0);
+        switch (viaparinfo->chip_info->tmds_chip_info.output_interface) {
+        case INTERFACE_DVP0:
+                viafb_write_reg_mask(CR6B, VIACR, 0x01, BIT0);
+                viafb_write_reg_mask(CR6C, VIACR, 0x21, BIT0 + BIT5);
+                dvi_patch_skew_dvp0();
                 if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
                         tmds_register_write(0x88, 0x3b);
                 else
                         /*clear CR91[5] to direct on display period
                            in the secondary diplay path */
-                        viafb_write_reg(CR91, VIACR,
-                        viafb_read_reg(VIACR, CR91) & 0xDF);
-        }
+                        via_write_reg_mask(VIACR, 0x91, 0x00, 0x20);
+                break;
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DVP1) {
-                viafb_write_reg(SR1E, VIASR,
-                        viafb_read_reg(VIASR, SR1E) | 0x30);
+        case INTERFACE_DVP1:
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
+                        viafb_write_reg_mask(CR93, VIACR, 0x21, BIT0 + BIT5);
 
                 /*fix dvi cann't be enabled with MB VT5718C4 - Al Zhang */
-                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266) {
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
                         tmds_register_write(0x88, 0x3b);
-                } else {
+                else
                         /*clear CR91[5] to direct on display period
                           in the secondary diplay path */
-                        viafb_write_reg(CR91, VIACR,
-                        viafb_read_reg(VIACR, CR91) & 0xDF);
-                }
+                        via_write_reg_mask(VIACR, 0x91, 0x00, 0x20);
 
                 /*fix DVI cannot enable on EPIA-M board */
                 if (viafb_platform_epia_dvi == 1) {
@@ -537,36 +448,40 @@ void viafb_dvi_enable(void)
                                 else
                                         data = 0x37;
                                 viafb_i2c_writebyte(viaparinfo->chip_info->
-                                                       tmds_chip_info.i2c_port,
-                                                    viaparinfo->chip_info->
-                                                       tmds_chip_info.tmds_chip_slave_addr,
-                                                    0x08, data);
+                                        tmds_chip_info.i2c_port,
+                                        viaparinfo->chip_info->
+                                        tmds_chip_info.tmds_chip_slave_addr,
+                                        0x08, data);
                         }
                 }
-        }
+                break;
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DFP_HIGH) {
-                viafb_write_reg(SR2A, VIASR,
-                        viafb_read_reg(VIASR, SR2A) | 0x0C);
-                viafb_write_reg(CR91, VIACR,
-                        viafb_read_reg(VIACR, CR91) & 0xDF);
-        }
+        case INTERFACE_DFP_HIGH:
+                if (viaparinfo->chip_info->gfx_chip_name != UNICHROME_CLE266)
+                        via_write_reg_mask(VIACR, CR97, 0x03, 0x03);
 
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_DFP_LOW) {
-                viafb_write_reg(SR2A, VIASR,
-                        viafb_read_reg(VIASR, SR2A) | 0x03);
-                viafb_write_reg(CR91, VIACR,
-                        viafb_read_reg(VIACR, CR91) & 0xDF);
-        }
-        if (viaparinfo->chip_info->
-                tmds_chip_info.output_interface == INTERFACE_TMDS) {
+                via_write_reg_mask(VIACR, 0x91, 0x00, 0x20);
+                break;
+
+        case INTERFACE_DFP_LOW:
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
+                        break;
+
+                dvi_patch_skew_dvp_low();
+                via_write_reg_mask(VIACR, 0x91, 0x00, 0x20);
+                break;
+
+        case INTERFACE_TMDS:
                 /* Turn on Display period in the panel path. */
                 viafb_write_reg_mask(CR91, VIACR, 0, BIT7);
 
                 /* Turn on TMDS power. */
                 viafb_write_reg_mask(CRD2, VIACR, 0, BIT3);
+                break;
         }
-}
 
+        if (viaparinfo->tmds_setting_info->iga_path == IGA2) {
+                /* Disable LCD Scaling */
+                viafb_write_reg_mask(CR79, VIACR, 0x00, BIT0);
+        }
+}
diff --git a/drivers/video/via/dvi.h b/drivers/video/via/dvi.h
index 0dffcfd395f3..f473dd010977 100644
--- a/drivers/video/via/dvi.h
+++ b/drivers/video/via/dvi.h
@@ -56,8 +56,8 @@
 int viafb_dvi_sense(void);
 void viafb_dvi_disable(void);
 void viafb_dvi_enable(void);
-int viafb_tmds_trasmitter_identify(void);
-void viafb_init_dvi_size(struct tmds_chip_information *tmds_chip,
+bool __devinit viafb_tmds_trasmitter_identify(void);
+void __devinit viafb_init_dvi_size(struct tmds_chip_information *tmds_chip,
         struct tmds_setting_information *tmds_setting);
 void viafb_dvi_set_mode(struct VideoModeTable *videoMode, int mode_bpp,
         int set_iga);
diff --git a/drivers/video/via/global.c b/drivers/video/via/global.c
index 1ee511b73307..e10d8249534c 100644
--- a/drivers/video/via/global.c
+++ b/drivers/video/via/global.c
@@ -40,10 +40,6 @@ int viafb_hotplug_Yres = 480;
 int viafb_hotplug_bpp = 32;
 int viafb_hotplug_refresh = 60;
 int viafb_primary_dev = None_Device;
-unsigned int viafb_second_xres = 640;
-unsigned int viafb_second_yres = 480;
-unsigned int viafb_second_virtual_xres;
-unsigned int viafb_second_virtual_yres;
 int viafb_lcd_panel_id = LCD_PANEL_ID_MAXIMUM + 1;
 struct fb_info *viafbinfo;
 struct fb_info *viafbinfo1;
diff --git a/drivers/video/via/global.h b/drivers/video/via/global.h
index 28221a062dda..ff969dc34593 100644
--- a/drivers/video/via/global.h
+++ b/drivers/video/via/global.h
@@ -48,7 +48,6 @@
 #include "via_utility.h"
 #include "vt1636.h"
 #include "tblDPASetting.h"
-#include "tbl1636.h"
 
 /* External struct*/
 
@@ -74,8 +73,6 @@ extern int viafb_hotplug_bpp;
 extern int viafb_hotplug_refresh;
 extern int viafb_primary_dev;
 
-extern unsigned int viafb_second_xres;
-extern unsigned int viafb_second_yres;
 extern int viafb_lcd_panel_id;
 
 #endif /* __GLOBAL_H__ */
diff --git a/drivers/video/via/hw.c b/drivers/video/via/hw.c
index 7dcb4d5bb9c3..47b13535ed2b 100644
--- a/drivers/video/via/hw.c
+++ b/drivers/video/via/hw.c
@@ -20,344 +20,138 @@
  */
 
 #include <linux/via-core.h>
+#include <asm/olpc.h>
 #include "global.h"
+#include "via_clock.h"
+
+static struct pll_limit cle266_pll_limits[] = {
+        {19, 19, 4, 0},
+        {26, 102, 5, 0},
+        {53, 112, 6, 0},
+        {41, 100, 7, 0},
+        {83, 108, 8, 0},
+        {87, 118, 9, 0},
+        {95, 115, 12, 0},
+        {108, 108, 13, 0},
+        {83, 83, 17, 0},
+        {67, 98, 20, 0},
+        {121, 121, 24, 0},
+        {99, 99, 29, 0},
+        {33, 33, 3, 1},
+        {15, 23, 4, 1},
+        {37, 121, 5, 1},
+        {82, 82, 6, 1},
+        {31, 84, 7, 1},
+        {83, 83, 8, 1},
+        {76, 127, 9, 1},
+        {33, 121, 4, 2},
+        {91, 118, 5, 2},
+        {83, 109, 6, 2},
+        {90, 90, 7, 2},
+        {93, 93, 2, 3},
+        {53, 53, 3, 3},
+        {73, 117, 4, 3},
+        {101, 127, 5, 3},
+        {99, 99, 7, 3}
+};
+
+static struct pll_limit k800_pll_limits[] = {
+        {22, 22, 2, 0},
+        {28, 28, 3, 0},
+        {81, 112, 3, 1},
+        {86, 166, 4, 1},
+        {109, 153, 5, 1},
+        {66, 116, 3, 2},
+        {93, 137, 4, 2},
+        {117, 208, 5, 2},
+        {30, 30, 2, 3},
+        {69, 125, 3, 3},
+        {89, 161, 4, 3},
+        {121, 208, 5, 3},
+        {66, 66, 2, 4},
+        {85, 85, 3, 4},
+        {141, 161, 4, 4},
+        {177, 177, 5, 4}
+};
 
-static struct pll_map pll_value[] = {
-        {25175000,
-                {99, 7, 3},
-                {85, 3, 4},     /* ignoring bit difference: 0x00008000 */
-                {141, 5, 4},
-                {141, 5, 4} },
-        {29581000,
-                {33, 4, 2},
-                {66, 2, 4},     /* ignoring bit difference: 0x00808000 */
-                {166, 5, 4},    /* ignoring bit difference: 0x00008000 */
-                {165, 5, 4} },
-        {26880000,
-                {15, 4, 1},
-                {30, 2, 3},     /* ignoring bit difference: 0x00808000 */
-                {150, 5, 4},
-                {150, 5, 4} },
-        {31500000,
-                {53, 3, 3},     /* ignoring bit difference: 0x00008000 */
-                {141, 4, 4},    /* ignoring bit difference: 0x00008000 */
-                {176, 5, 4},
-                {176, 5, 4} },
-        {31728000,
-                {31, 7, 1},
-                {177, 5, 4},    /* ignoring bit difference: 0x00008000 */
-                {177, 5, 4},
-                {142, 4, 4} },
-        {32688000,
-                {73, 4, 3},
-                {146, 4, 4},    /* ignoring bit difference: 0x00008000 */
-                {183, 5, 4},
-                {146, 4, 4} },
-        {36000000,
-                {101, 5, 3},    /* ignoring bit difference: 0x00008000 */
-                {161, 4, 4},    /* ignoring bit difference: 0x00008000 */
-                {202, 5, 4},
-                {161, 4, 4} },
-        {40000000,
-                {89, 4, 3},
-                {89, 4, 3},     /* ignoring bit difference: 0x00008000 */
-                {112, 5, 3},
-                {112, 5, 3} },
-        {41291000,
-                {23, 4, 1},
-                {69, 3, 3},     /* ignoring bit difference: 0x00008000 */
-                {115, 5, 3},
-                {115, 5, 3} },
-        {43163000,
-                {121, 5, 3},
-                {121, 5, 3},    /* ignoring bit difference: 0x00008000 */
-                {121, 5, 3},
-                {121, 5, 3} },
-        {45250000,
-                {127, 5, 3},
-                {127, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {127, 5, 3},
-                {127, 5, 3} },
-        {46000000,
-                {90, 7, 2},
-                {103, 4, 3},    /* ignoring bit difference: 0x00008000 */
-                {129, 5, 3},
-                {103, 4, 3} },
-        {46996000,
-                {105, 4, 3},    /* ignoring bit difference: 0x00008000 */
-                {131, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {131, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {105, 4, 3} },
-        {48000000,
-                {67, 20, 0},
-                {134, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {134, 5, 3},
-                {134, 5, 3} },
-        {48875000,
-                {99, 29, 0},
-                {82, 3, 3},     /* ignoring bit difference: 0x00808000 */
-                {82, 3, 3},     /* ignoring bit difference: 0x00808000 */
-                {137, 5, 3} },
-        {49500000,
-                {83, 6, 2},
-                {83, 3, 3},     /* ignoring bit difference: 0x00008000 */
-                {138, 5, 3},
-                {83, 3, 3} },
-        {52406000,
-                {117, 4, 3},
-                {117, 4, 3},    /* ignoring bit difference: 0x00008000 */
-                {117, 4, 3},
-                {88, 3, 3} },
-        {52977000,
-                {37, 5, 1},
-                {148, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {148, 5, 3},
-                {148, 5, 3} },
-        {56250000,
-                {55, 7, 1},     /* ignoring bit difference: 0x00008000 */
-                {126, 4, 3},    /* ignoring bit difference: 0x00008000 */
-                {157, 5, 3},
-                {157, 5, 3} },
-        {57275000,
-                {0, 0, 0},
-                {2, 2, 0},
-                {2, 2, 0},
-                {157, 5, 3} },  /* ignoring bit difference: 0x00808000 */
-        {60466000,
-                {76, 9, 1},
-                {169, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {169, 5, 3},    /* FIXED: old = {72, 2, 3} */
-                {169, 5, 3} },
-        {61500000,
-                {86, 20, 0},
-                {172, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {172, 5, 3},
-                {172, 5, 3} },
-        {65000000,
-                {109, 6, 2},    /* ignoring bit difference: 0x00008000 */
-                {109, 3, 3},    /* ignoring bit difference: 0x00008000 */
-                {109, 3, 3},
-                {109, 3, 3} },
-        {65178000,
-                {91, 5, 2},
-                {182, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {109, 3, 3},
-                {182, 5, 3} },
-        {66750000,
-                {75, 4, 2},
-                {150, 4, 3},    /* ignoring bit difference: 0x00808000 */
-                {150, 4, 3},
-                {112, 3, 3} },
-        {68179000,
-                {19, 4, 0},
-                {114, 3, 3},    /* ignoring bit difference: 0x00008000 */
-                {190, 5, 3},
-                {191, 5, 3} },
-        {69924000,
-                {83, 17, 0},
-                {195, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {195, 5, 3},
-                {195, 5, 3} },
-        {70159000,
-                {98, 20, 0},
-                {196, 5, 3},    /* ignoring bit difference: 0x00808000 */
-                {196, 5, 3},
-                {195, 5, 3} },
-        {72000000,
-                {121, 24, 0},
-                {161, 4, 3},    /* ignoring bit difference: 0x00808000 */
-                {161, 4, 3},
-                {161, 4, 3} },
-        {78750000,
-                {33, 3, 1},
-                {66, 3, 2},     /* ignoring bit difference: 0x00008000 */
-                {110, 5, 2},
-                {110, 5, 2} },
-        {80136000,
-                {28, 5, 0},
-                {68, 3, 2},     /* ignoring bit difference: 0x00008000 */
-                {112, 5, 2},
-                {112, 5, 2} },
-        {83375000,
-                {93, 2, 3},
-                {93, 4, 2},     /* ignoring bit difference: 0x00800000 */
-                {93, 4, 2},     /* ignoring bit difference: 0x00800000 */
-                {117, 5, 2} },
-        {83950000,
-                {41, 7, 0},
-                {117, 5, 2},    /* ignoring bit difference: 0x00008000 */
-                {117, 5, 2},
-                {117, 5, 2} },
-        {84750000,
-                {118, 5, 2},
-                {118, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {118, 5, 2},
-                {118, 5, 2} },
-        {85860000,
-                {84, 7, 1},
-                {120, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {120, 5, 2},
-                {118, 5, 2} },
-        {88750000,
-                {31, 5, 0},
-                {124, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {174, 7, 2},    /* ignoring bit difference: 0x00808000 */
-                {124, 5, 2} },
-        {94500000,
-                {33, 5, 0},
-                {132, 5, 2},    /* ignoring bit difference: 0x00008000 */
-                {132, 5, 2},
-                {132, 5, 2} },
-        {97750000,
-                {82, 6, 1},
-                {137, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {137, 5, 2},
-                {137, 5, 2} },
-        {101000000,
-                {127, 9, 1},
-                {141, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {141, 5, 2},
-                {141, 5, 2} },
-        {106500000,
-                {119, 4, 2},
-                {119, 4, 2},    /* ignoring bit difference: 0x00808000 */
-                {119, 4, 2},
-                {149, 5, 2} },
-        {108000000,
-                {121, 4, 2},
-                {121, 4, 2},    /* ignoring bit difference: 0x00808000 */
-                {151, 5, 2},
-                {151, 5, 2} },
-        {113309000,
-                {95, 12, 0},
-                {95, 3, 2},     /* ignoring bit difference: 0x00808000 */
-                {95, 3, 2},
-                {159, 5, 2} },
-        {118840000,
-                {83, 5, 1},
-                {166, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {166, 5, 2},
-                {166, 5, 2} },
-        {119000000,
-                {108, 13, 0},
-                {133, 4, 2},    /* ignoring bit difference: 0x00808000 */
-                {133, 4, 2},
-                {167, 5, 2} },
-        {121750000,
-                {85, 5, 1},
-                {170, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {68, 2, 2},
-                {0, 0, 0} },
-        {125104000,
-                {53, 6, 0},     /* ignoring bit difference: 0x00008000 */
-                {106, 3, 2},    /* ignoring bit difference: 0x00008000 */
-                {175, 5, 2},
-                {0, 0, 0} },
-        {135000000,
-                {94, 5, 1},
-                {28, 3, 0},     /* ignoring bit difference: 0x00804000 */
-                {151, 4, 2},
-                {189, 5, 2} },
-        {136700000,
-                {115, 12, 0},
-                {191, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {191, 5, 2},
-                {191, 5, 2} },
-        {138400000,
-                {87, 9, 0},
-                {116, 3, 2},    /* ignoring bit difference: 0x00808000 */
-                {116, 3, 2},
-                {194, 5, 2} },
-        {146760000,
-                {103, 5, 1},
-                {206, 5, 2},    /* ignoring bit difference: 0x00808000 */
-                {206, 5, 2},
-                {206, 5, 2} },
-        {153920000,
-                {86, 8, 0},
-                {86, 4, 1},     /* ignoring bit difference: 0x00808000 */
-                {86, 4, 1},
-                {86, 4, 1} },   /* FIXED: old = {84, 2, 1} */
-        {156000000,
-                {109, 5, 1},
-                {109, 5, 1},    /* ignoring bit difference: 0x00808000 */
-                {109, 5, 1},
-                {108, 5, 1} },
-        {157500000,
-                {55, 5, 0},     /* ignoring bit difference: 0x00008000 */
-                {22, 2, 0},     /* ignoring bit difference: 0x00802000 */
-                {110, 5, 1},
-                {110, 5, 1} },
-        {162000000,
-                {113, 5, 1},
-                {113, 5, 1},    /* ignoring bit difference: 0x00808000 */
-                {113, 5, 1},
-                {113, 5, 1} },
-        {187000000,
-                {118, 9, 0},
-                {131, 5, 1},    /* ignoring bit difference: 0x00808000 */
-                {131, 5, 1},
-                {131, 5, 1} },
-        {193295000,
-                {108, 8, 0},
-                {81, 3, 1},     /* ignoring bit difference: 0x00808000 */
-                {135, 5, 1},
-                {135, 5, 1} },
-        {202500000,
-                {99, 7, 0},
-                {85, 3, 1},     /* ignoring bit difference: 0x00808000 */
-                {142, 5, 1},
-                {142, 5, 1} },
-        {204000000,
-                {100, 7, 0},
-                {143, 5, 1},    /* ignoring bit difference: 0x00808000 */
-                {143, 5, 1},
-                {143, 5, 1} },
-        {218500000,
-                {92, 6, 0},
-                {153, 5, 1},    /* ignoring bit difference: 0x00808000 */
-                {153, 5, 1},
-                {153, 5, 1} },
-        {234000000,
-                {98, 6, 0},
-                {98, 3, 1},     /* ignoring bit difference: 0x00008000 */
-                {98, 3, 1},
-                {164, 5, 1} },
-        {267250000,
-                {112, 6, 0},
-                {112, 3, 1},    /* ignoring bit difference: 0x00808000 */
-                {187, 5, 1},
-                {187, 5, 1} },
-        {297500000,
-                {102, 5, 0},    /* ignoring bit difference: 0x00008000 */
-                {166, 4, 1},    /* ignoring bit difference: 0x00008000 */
-                {208, 5, 1},
-                {208, 5, 1} },
-        {74481000,
-                {26, 5, 0},
-                {125, 3, 3},    /* ignoring bit difference: 0x00808000 */
-                {208, 5, 3},
-                {209, 5, 3} },
-        {172798000,
-                {121, 5, 1},
-                {121, 5, 1},    /* ignoring bit difference: 0x00808000 */
-                {121, 5, 1},
-                {121, 5, 1} },
-        {122614000,
-                {60, 7, 0},
-                {137, 4, 2},    /* ignoring bit difference: 0x00808000 */
-                {137, 4, 2},
-                {172, 5, 2} },
-        {74270000,
-                {83, 8, 1},
-                {208, 5, 3},
-                {208, 5, 3},
-                {0, 0, 0} },
-        {148500000,
-                {83, 8, 0},
-                {208, 5, 2},
-                {166, 4, 2},
-                {208, 5, 2} }
+static struct pll_limit cx700_pll_limits[] = {
+        {98, 98, 3, 1},
+        {86, 86, 4, 1},
+        {109, 208, 5, 1},
+        {68, 68, 2, 2},
+        {95, 116, 3, 2},
+        {93, 166, 4, 2},
+        {110, 206, 5, 2},
+        {174, 174, 7, 2},
+        {82, 109, 3, 3},
+        {117, 161, 4, 3},
+        {112, 208, 5, 3},
+        {141, 202, 5, 4}
+};
+
+static struct pll_limit vx855_pll_limits[] = {
+        {86, 86, 4, 1},
+        {108, 208, 5, 1},
+        {110, 208, 5, 2},
+        {83, 112, 3, 3},
+        {103, 161, 4, 3},
+        {112, 209, 5, 3},
+        {142, 161, 4, 4},
+        {141, 176, 5, 4}
+};
+
+/* according to VIA Technologies these values are based on experiment */
+static struct io_reg scaling_parameters[] = {
+        {VIACR, CR7A, 0xFF, 0x01},      /* LCD Scaling Parameter 1 */
+        {VIACR, CR7B, 0xFF, 0x02},      /* LCD Scaling Parameter 2 */
+        {VIACR, CR7C, 0xFF, 0x03},      /* LCD Scaling Parameter 3 */
+        {VIACR, CR7D, 0xFF, 0x04},      /* LCD Scaling Parameter 4 */
+        {VIACR, CR7E, 0xFF, 0x07},      /* LCD Scaling Parameter 5 */
+        {VIACR, CR7F, 0xFF, 0x0A},      /* LCD Scaling Parameter 6 */
+        {VIACR, CR80, 0xFF, 0x0D},      /* LCD Scaling Parameter 7 */
+        {VIACR, CR81, 0xFF, 0x13},      /* LCD Scaling Parameter 8 */
+        {VIACR, CR82, 0xFF, 0x16},      /* LCD Scaling Parameter 9 */
+        {VIACR, CR83, 0xFF, 0x19},      /* LCD Scaling Parameter 10 */
+        {VIACR, CR84, 0xFF, 0x1C},      /* LCD Scaling Parameter 11 */
+        {VIACR, CR85, 0xFF, 0x1D},      /* LCD Scaling Parameter 12 */
+        {VIACR, CR86, 0xFF, 0x1E},      /* LCD Scaling Parameter 13 */
+        {VIACR, CR87, 0xFF, 0x1F},      /* LCD Scaling Parameter 14 */
+};
+
+static struct io_reg common_vga[] = {
+        {VIACR, CR07, 0x10, 0x10}, /* [0] vertical total (bit 8)
+                                        [1] vertical display end (bit 8)
+                                        [2] vertical retrace start (bit 8)
+                                        [3] start vertical blanking (bit 8)
+                                        [4] line compare (bit 8)
+                                        [5] vertical total (bit 9)
+                                        [6] vertical display end (bit 9)
+                                        [7] vertical retrace start (bit 9) */
+        {VIACR, CR08, 0xFF, 0x00}, /* [0-4] preset row scan
+                                        [5-6] byte panning */
+        {VIACR, CR09, 0xDF, 0x40}, /* [0-4] max scan line
+                                        [5] start vertical blanking (bit 9)
+                                        [6] line compare (bit 9)
+                                        [7] scan doubling */
+        {VIACR, CR0A, 0xFF, 0x1E}, /* [0-4] cursor start
+                                        [5] cursor disable */
+        {VIACR, CR0B, 0xFF, 0x00}, /* [0-4] cursor end
+                                        [5-6] cursor skew */
+        {VIACR, CR0E, 0xFF, 0x00}, /* [0-7] cursor location (high) */
+        {VIACR, CR0F, 0xFF, 0x00}, /* [0-7] cursor location (low) */
+        {VIACR, CR11, 0xF0, 0x80}, /* [0-3] vertical retrace end
+                                        [6] memory refresh bandwidth
+                                        [7] CRTC register protect enable */
+        {VIACR, CR14, 0xFF, 0x00}, /* [0-4] underline location
+                                        [5] divide memory address clock by 4
+                                        [6] double word addressing */
+        {VIACR, CR17, 0xFF, 0x63}, /* [0-1] mapping of display address 13-14
+                                        [2] divide scan line clock by 2
+                                        [3] divide memory address clock by 2
+                                        [5] address wrap
+                                        [6] byte mode select
+                                        [7] sync enable */
+        {VIACR, CR18, 0xFF, 0xFF}, /* [0-7] line compare */
 };
 
 static struct fifo_depth_select display_fifo_depth_reg = {
@@ -718,16 +512,23 @@ static struct rgbLUT palLUT_table[] = {
                                                                      0x00}
 };
 
-static void set_crt_output_path(int set_iga);
-static void dvi_patch_skew_dvp0(void);
-static void dvi_patch_skew_dvp1(void);
-static void dvi_patch_skew_dvp_low(void);
-static void set_dvi_output_path(int set_iga, int output_interface);
-static void set_lcd_output_path(int set_iga, int output_interface);
+static struct via_device_mapping device_mapping[] = {
+        {VIA_LDVP0, "LDVP0"},
+        {VIA_LDVP1, "LDVP1"},
+        {VIA_DVP0, "DVP0"},
+        {VIA_CRT, "CRT"},
+        {VIA_DVP1, "DVP1"},
+        {VIA_LVDS1, "LVDS1"},
+        {VIA_LVDS2, "LVDS2"}
+};
+
+/* structure with function pointers to support clock control */
+static struct via_clock clock;
+
 static void load_fix_bit_crtc_reg(void);
-static void init_gfx_chip_info(int chip_type);
-static void init_tmds_chip_info(void);
-static void init_lvds_chip_info(void);
+static void __devinit init_gfx_chip_info(int chip_type);
+static void __devinit init_tmds_chip_info(void);
+static void __devinit init_lvds_chip_info(void);
 static void device_screen_off(void);
 static void device_screen_on(void);
 static void set_display_channel(void);
@@ -747,7 +548,7 @@ void viafb_unlock_crt(void)
         viafb_write_reg_mask(CR47, VIACR, 0, BIT0);
 }
 
-void write_dac_reg(u8 index, u8 r, u8 g, u8 b)
+static void write_dac_reg(u8 index, u8 r, u8 g, u8 b)
 {
         outb(index, LUT_INDEX_WRITE);
         outb(r, LUT_DATA);
@@ -755,16 +556,77 @@ void write_dac_reg(u8 index, u8 r, u8 g, u8 b)
         outb(b, LUT_DATA);
 }
 
+static u32 get_dvi_devices(int output_interface)
+{
+        switch (output_interface) {
+        case INTERFACE_DVP0:
+                return VIA_DVP0 | VIA_LDVP0;
+
+        case INTERFACE_DVP1:
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
+                        return VIA_LDVP1;
+                else
+                        return VIA_DVP1;
+
+        case INTERFACE_DFP_HIGH:
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
+                        return 0;
+                else
+                        return VIA_LVDS2 | VIA_DVP0;
+
+        case INTERFACE_DFP_LOW:
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
+                        return 0;
+                else
+                        return VIA_DVP1 | VIA_LVDS1;
+
+        case INTERFACE_TMDS:
+                return VIA_LVDS1;
+        }
+
+        return 0;
+}
+
+static u32 get_lcd_devices(int output_interface)
+{
+        switch (output_interface) {
+        case INTERFACE_DVP0:
+                return VIA_DVP0;
+
+        case INTERFACE_DVP1:
+                return VIA_DVP1;
+
+        case INTERFACE_DFP_HIGH:
+                return VIA_LVDS2 | VIA_DVP0;
+
+        case INTERFACE_DFP_LOW:
+                return VIA_LVDS1 | VIA_DVP1;
+
+        case INTERFACE_DFP:
+                return VIA_LVDS1 | VIA_LVDS2;
+
+        case INTERFACE_LVDS0:
+        case INTERFACE_LVDS0LVDS1:
+                return VIA_LVDS1;
+
+        case INTERFACE_LVDS1:
+                return VIA_LVDS2;
+        }
+
+        return 0;
+}
+
 /*Set IGA path for each device*/
 void viafb_set_iga_path(void)
 {
+        int crt_iga_path = 0;
 
         if (viafb_SAMM_ON == 1) {
                 if (viafb_CRT_ON) {
                         if (viafb_primary_dev == CRT_Device)
-                                viaparinfo->crt_setting_info->iga_path = IGA1;
+                                crt_iga_path = IGA1;
                         else
-                                viaparinfo->crt_setting_info->iga_path = IGA2;
+                                crt_iga_path = IGA2;
                 }
 
                 if (viafb_DVI_ON) {
@@ -781,8 +643,7 @@ void viafb_set_iga_path(void)
                                         UNICHROME_CLE266)) {
                                         viaparinfo->
                                         lvds_setting_info->iga_path = IGA2;
-                                        viaparinfo->
-                                        crt_setting_info->iga_path = IGA1;
+                                        crt_iga_path = IGA1;
                                         viaparinfo->
                                         tmds_setting_info->iga_path = IGA1;
                                 } else
@@ -802,10 +663,10 @@ void viafb_set_iga_path(void)
                 viafb_SAMM_ON = 0;
 
                 if (viafb_CRT_ON && viafb_LCD_ON) {
-                        viaparinfo->crt_setting_info->iga_path = IGA1;
+                        crt_iga_path = IGA1;
                         viaparinfo->lvds_setting_info->iga_path = IGA2;
                 } else if (viafb_CRT_ON && viafb_DVI_ON) {
-                        viaparinfo->crt_setting_info->iga_path = IGA1;
+                        crt_iga_path = IGA1;
                         viaparinfo->tmds_setting_info->iga_path = IGA2;
                 } else if (viafb_LCD_ON && viafb_DVI_ON) {
                         viaparinfo->tmds_setting_info->iga_path = IGA1;
@@ -814,13 +675,59 @@ void viafb_set_iga_path(void)
                         viaparinfo->lvds_setting_info->iga_path = IGA2;
                         viaparinfo->lvds_setting_info2->iga_path = IGA2;
                 } else if (viafb_CRT_ON) {
-                        viaparinfo->crt_setting_info->iga_path = IGA1;
+                        crt_iga_path = IGA1;
                 } else if (viafb_LCD_ON) {
                         viaparinfo->lvds_setting_info->iga_path = IGA2;
                 } else if (viafb_DVI_ON) {
                         viaparinfo->tmds_setting_info->iga_path = IGA1;
                 }
         }
+
+        viaparinfo->shared->iga1_devices = 0;
+        viaparinfo->shared->iga2_devices = 0;
+        if (viafb_CRT_ON) {
+                if (crt_iga_path == IGA1)
+                        viaparinfo->shared->iga1_devices |= VIA_CRT;
+                else
+                        viaparinfo->shared->iga2_devices |= VIA_CRT;
+        }
+
+        if (viafb_DVI_ON) {
+                if (viaparinfo->tmds_setting_info->iga_path == IGA1)
+                        viaparinfo->shared->iga1_devices |= get_dvi_devices(
+                                viaparinfo->chip_info->
+                                tmds_chip_info.output_interface);
+                else
+                        viaparinfo->shared->iga2_devices |= get_dvi_devices(
+                                viaparinfo->chip_info->
+                                tmds_chip_info.output_interface);
+        }
+
+        if (viafb_LCD_ON) {
+                if (viaparinfo->lvds_setting_info->iga_path == IGA1)
+                        viaparinfo->shared->iga1_devices |= get_lcd_devices(
+                                viaparinfo->chip_info->
+                                lvds_chip_info.output_interface);
+                else
+                        viaparinfo->shared->iga2_devices |= get_lcd_devices(
+                                viaparinfo->chip_info->
+                                lvds_chip_info.output_interface);
+        }
+
+        if (viafb_LCD2_ON) {
+                if (viaparinfo->lvds_setting_info2->iga_path == IGA1)
+                        viaparinfo->shared->iga1_devices |= get_lcd_devices(
+                                viaparinfo->chip_info->
+                                lvds_chip_info2.output_interface);
+                else
+                        viaparinfo->shared->iga2_devices |= get_lcd_devices(
+                                viaparinfo->chip_info->
+                                lvds_chip_info2.output_interface);
+        }
+
+        /* looks like the OLPC has its display wired to DVP1 and LVDS2 */
+        if (machine_is_olpc())
+                viaparinfo->shared->iga2_devices = VIA_DVP1 | VIA_LVDS2;
 }
 
 static void set_color_register(u8 index, u8 red, u8 green, u8 blue)
@@ -844,318 +751,281 @@ void viafb_set_secondary_color_register(u8 index, u8 red, u8 green, u8 blue)
         set_color_register(index, red, green, blue);
 }
 
-void viafb_set_output_path(int device, int set_iga, int output_interface)
+static void set_source_common(u8 index, u8 offset, u8 iga)
 {
-        switch (device) {
-        case DEVICE_CRT:
-                set_crt_output_path(set_iga);
-                break;
-        case DEVICE_DVI:
-                set_dvi_output_path(set_iga, output_interface);
+        u8 value, mask = 1 << offset;
+
+        switch (iga) {
+        case IGA1:
+                value = 0x00;
                 break;
-        case DEVICE_LCD:
-                set_lcd_output_path(set_iga, output_interface);
+        case IGA2:
+                value = mask;
                 break;
+        default:
+                printk(KERN_WARNING "viafb: Unsupported source: %d\n", iga);
+                return;
         }
+
+        via_write_reg_mask(VIACR, index, value, mask);
 }
 
-static void set_crt_output_path(int set_iga)
+static void set_crt_source(u8 iga)
 {
-        viafb_write_reg_mask(CR36, VIACR, 0x00, BIT4 + BIT5);
+        u8 value;
 
-        switch (set_iga) {
+        switch (iga) {
         case IGA1:
-                viafb_write_reg_mask(SR16, VIASR, 0x00, BIT6);
+                value = 0x00;
                 break;
         case IGA2:
-                viafb_write_reg_mask(CR6A, VIACR, 0xC0, BIT6 + BIT7);
-                viafb_write_reg_mask(SR16, VIASR, 0x40, BIT6);
+                value = 0x40;
                 break;
+        default:
+                printk(KERN_WARNING "viafb: Unsupported source: %d\n", iga);
+                return;
         }
+
+        via_write_reg_mask(VIASR, 0x16, value, 0x40);
 }
 
-static void dvi_patch_skew_dvp0(void)
+static inline void set_ldvp0_source(u8 iga)
 {
-        /* Reset data driving first: */
-        viafb_write_reg_mask(SR1B, VIASR, 0, BIT1);
-        viafb_write_reg_mask(SR2A, VIASR, 0, BIT4);
-
-        switch (viaparinfo->chip_info->gfx_chip_name) {
-        case UNICHROME_P4M890:
-                {
-                        if ((viaparinfo->tmds_setting_info->h_active == 1600) &&
-                                (viaparinfo->tmds_setting_info->v_active ==
-                                1200))
-                                viafb_write_reg_mask(CR96, VIACR, 0x03,
-                                               BIT0 + BIT1 + BIT2);
-                        else
-                                viafb_write_reg_mask(CR96, VIACR, 0x07,
-                                               BIT0 + BIT1 + BIT2);
-                        break;
-                }
+        set_source_common(0x6C, 7, iga);
+}
 
-        case UNICHROME_P4M900:
-                {
-                        viafb_write_reg_mask(CR96, VIACR, 0x07,
-                                       BIT0 + BIT1 + BIT2 + BIT3);
-                        viafb_write_reg_mask(SR1B, VIASR, 0x02, BIT1);
-                        viafb_write_reg_mask(SR2A, VIASR, 0x10, BIT4);
-                        break;
-                }
+static inline void set_ldvp1_source(u8 iga)
+{
+        set_source_common(0x93, 7, iga);
+}
 
-        default:
-                {
-                        break;
-                }
-        }
+static inline void set_dvp0_source(u8 iga)
+{
+        set_source_common(0x96, 4, iga);
 }
 
-static void dvi_patch_skew_dvp1(void)
+static inline void set_dvp1_source(u8 iga)
 {
-        switch (viaparinfo->chip_info->gfx_chip_name) {
-        case UNICHROME_CX700:
-                {
-                        break;
-                }
+        set_source_common(0x9B, 4, iga);
+}
 
-        default:
-                {
-                        break;
-                }
-        }
+static inline void set_lvds1_source(u8 iga)
+{
+        set_source_common(0x99, 4, iga);
 }
 
-static void dvi_patch_skew_dvp_low(void)
+static inline void set_lvds2_source(u8 iga)
 {
-        switch (viaparinfo->chip_info->gfx_chip_name) {
-        case UNICHROME_K8M890:
-                {
-                        viafb_write_reg_mask(CR99, VIACR, 0x03, BIT0 + BIT1);
-                        break;
-                }
+        set_source_common(0x97, 4, iga);
+}
 
-        case UNICHROME_P4M900:
-                {
-                        viafb_write_reg_mask(CR99, VIACR, 0x08,
-                                       BIT0 + BIT1 + BIT2 + BIT3);
-                        break;
-                }
+void via_set_source(u32 devices, u8 iga)
+{
+        if (devices & VIA_LDVP0)
+                set_ldvp0_source(iga);
+        if (devices & VIA_LDVP1)
+                set_ldvp1_source(iga);
+        if (devices & VIA_DVP0)
+                set_dvp0_source(iga);
+        if (devices & VIA_CRT)
+                set_crt_source(iga);
+        if (devices & VIA_DVP1)
+                set_dvp1_source(iga);
+        if (devices & VIA_LVDS1)
+                set_lvds1_source(iga);
+        if (devices & VIA_LVDS2)
+                set_lvds2_source(iga);
+}
 
-        case UNICHROME_P4M890:
-                {
-                        viafb_write_reg_mask(CR99, VIACR, 0x0F,
-                                       BIT0 + BIT1 + BIT2 + BIT3);
-                        break;
-                }
+static void set_crt_state(u8 state)
+{
+        u8 value;
 
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x00;
+                break;
+        case VIA_STATE_STANDBY:
+                value = 0x10;
+                break;
+        case VIA_STATE_SUSPEND:
+                value = 0x20;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x30;
+                break;
         default:
-                {
-                        break;
-                }
+                return;
         }
+
+        via_write_reg_mask(VIACR, 0x36, value, 0x30);
 }
 
-static void set_dvi_output_path(int set_iga, int output_interface)
+static void set_dvp0_state(u8 state)
 {
-        switch (output_interface) {
-        case INTERFACE_DVP0:
-                viafb_write_reg_mask(CR6B, VIACR, 0x01, BIT0);
-
-                if (set_iga == IGA1) {
-                        viafb_write_reg_mask(CR96, VIACR, 0x00, BIT4);
-                        viafb_write_reg_mask(CR6C, VIACR, 0x21, BIT0 +
-                                BIT5 + BIT7);
-                } else {
-                        viafb_write_reg_mask(CR96, VIACR, 0x10, BIT4);
-                        viafb_write_reg_mask(CR6C, VIACR, 0xA1, BIT0 +
-                                BIT5 + BIT7);
-                }
-
-                viafb_write_reg_mask(SR1E, VIASR, 0xC0, BIT7 + BIT6);
+        u8 value;
 
-                dvi_patch_skew_dvp0();
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0xC0;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x00;
                 break;
+        default:
+                return;
+        }
 
-        case INTERFACE_DVP1:
-                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266) {
-                        if (set_iga == IGA1)
-                                viafb_write_reg_mask(CR93, VIACR, 0x21,
-                                               BIT0 + BIT5 + BIT7);
-                        else
-                                viafb_write_reg_mask(CR93, VIACR, 0xA1,
-                                               BIT0 + BIT5 + BIT7);
-                } else {
-                        if (set_iga == IGA1)
-                                viafb_write_reg_mask(CR9B, VIACR, 0x00, BIT4);
-                        else
-                                viafb_write_reg_mask(CR9B, VIACR, 0x10, BIT4);
-                }
+        via_write_reg_mask(VIASR, 0x1E, value, 0xC0);
+}
 
-                viafb_write_reg_mask(SR1E, VIASR, 0x30, BIT4 + BIT5);
-                dvi_patch_skew_dvp1();
+static void set_dvp1_state(u8 state)
+{
+        u8 value;
+
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x30;
                 break;
-        case INTERFACE_DFP_HIGH:
-                if (viaparinfo->chip_info->gfx_chip_name != UNICHROME_CLE266) {
-                        if (set_iga == IGA1) {
-                                viafb_write_reg_mask(CR96, VIACR, 0x00, BIT4);
-                                viafb_write_reg_mask(CR97, VIACR, 0x03,
-                                               BIT0 + BIT1 + BIT4);
-                        } else {
-                                viafb_write_reg_mask(CR96, VIACR, 0x10, BIT4);
-                                viafb_write_reg_mask(CR97, VIACR, 0x13,
-                                               BIT0 + BIT1 + BIT4);
-                        }
-                }
-                viafb_write_reg_mask(SR2A, VIASR, 0x0C, BIT2 + BIT3);
+        case VIA_STATE_OFF:
+                value = 0x00;
                 break;
+        default:
+                return;
+        }
 
-        case INTERFACE_DFP_LOW:
-                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266)
-                        break;
+        via_write_reg_mask(VIASR, 0x1E, value, 0x30);
+}
 
-                if (set_iga == IGA1) {
-                        viafb_write_reg_mask(CR99, VIACR, 0x00, BIT4);
-                        viafb_write_reg_mask(CR9B, VIACR, 0x00, BIT4);
-                } else {
-                        viafb_write_reg_mask(CR99, VIACR, 0x10, BIT4);
-                        viafb_write_reg_mask(CR9B, VIACR, 0x10, BIT4);
-                }
+static void set_lvds1_state(u8 state)
+{
+        u8 value;
 
-                viafb_write_reg_mask(SR2A, VIASR, 0x03, BIT0 + BIT1);
-                dvi_patch_skew_dvp_low();
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x03;
                 break;
-
-        case INTERFACE_TMDS:
-                if (set_iga == IGA1)
-                        viafb_write_reg_mask(CR99, VIACR, 0x00, BIT4);
-                else
-                        viafb_write_reg_mask(CR99, VIACR, 0x10, BIT4);
+        case VIA_STATE_OFF:
+                value = 0x00;
                 break;
+        default:
+                return;
         }
 
-        if (set_iga == IGA2) {
-                enable_second_display_channel();
-                /* Disable LCD Scaling */
-                viafb_write_reg_mask(CR79, VIACR, 0x00, BIT0);
-        }
+        via_write_reg_mask(VIASR, 0x2A, value, 0x03);
 }
 
-static void set_lcd_output_path(int set_iga, int output_interface)
+static void set_lvds2_state(u8 state)
 {
-        DEBUG_MSG(KERN_INFO
-                  "set_lcd_output_path, iga:%d,out_interface:%d\n",
-                  set_iga, output_interface);
-        switch (set_iga) {
-        case IGA1:
-                viafb_write_reg_mask(CR6B, VIACR, 0x00, BIT3);
-                viafb_write_reg_mask(CR6A, VIACR, 0x08, BIT3);
+        u8 value;
 
-                disable_second_display_channel();
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x0C;
                 break;
-
-        case IGA2:
-                viafb_write_reg_mask(CR6B, VIACR, 0x00, BIT3);
-                viafb_write_reg_mask(CR6A, VIACR, 0x08, BIT3);
-
-                enable_second_display_channel();
+        case VIA_STATE_OFF:
+                value = 0x00;
                 break;
+        default:
+                return;
         }
 
-        switch (output_interface) {
-        case INTERFACE_DVP0:
-                if (set_iga == IGA1) {
-                        viafb_write_reg_mask(CR96, VIACR, 0x00, BIT4);
-                } else {
-                        viafb_write_reg(CR91, VIACR, 0x00);
-                        viafb_write_reg_mask(CR96, VIACR, 0x10, BIT4);
-                }
-                break;
-
-        case INTERFACE_DVP1:
-                if (set_iga == IGA1)
-                        viafb_write_reg_mask(CR9B, VIACR, 0x00, BIT4);
-                else {
-                        viafb_write_reg(CR91, VIACR, 0x00);
-                        viafb_write_reg_mask(CR9B, VIACR, 0x10, BIT4);
-                }
-                break;
+        via_write_reg_mask(VIASR, 0x2A, value, 0x0C);
+}
 
-        case INTERFACE_DFP_HIGH:
-                if (set_iga == IGA1)
-                        viafb_write_reg_mask(CR97, VIACR, 0x00, BIT4);
-                else {
-                        viafb_write_reg(CR91, VIACR, 0x00);
-                        viafb_write_reg_mask(CR97, VIACR, 0x10, BIT4);
-                        viafb_write_reg_mask(CR96, VIACR, 0x10, BIT4);
-                }
-                break;
+void via_set_state(u32 devices, u8 state)
+{
+        /*
+        TODO: Can we enable/disable these devices? How?
+        if (devices & VIA_LDVP0)
+        if (devices & VIA_LDVP1)
+        */
+        if (devices & VIA_DVP0)
+                set_dvp0_state(state);
+        if (devices & VIA_CRT)
+                set_crt_state(state);
+        if (devices & VIA_DVP1)
+                set_dvp1_state(state);
+        if (devices & VIA_LVDS1)
+                set_lvds1_state(state);
+        if (devices & VIA_LVDS2)
+                set_lvds2_state(state);
+}
 
-        case INTERFACE_DFP_LOW:
-                if (set_iga == IGA1)
-                        viafb_write_reg_mask(CR99, VIACR, 0x00, BIT4);
-                else {
-                        viafb_write_reg(CR91, VIACR, 0x00);
-                        viafb_write_reg_mask(CR99, VIACR, 0x10, BIT4);
-                        viafb_write_reg_mask(CR9B, VIACR, 0x10, BIT4);
-                }
+void via_set_sync_polarity(u32 devices, u8 polarity)
+{
+        if (polarity & ~(VIA_HSYNC_NEGATIVE | VIA_VSYNC_NEGATIVE)) {
+                printk(KERN_WARNING "viafb: Unsupported polarity: %d\n",
+                        polarity);
+                return;
+        }
 
-                break;
+        if (devices & VIA_CRT)
+                via_write_misc_reg_mask(polarity << 6, 0xC0);
+        if (devices & VIA_DVP1)
+                via_write_reg_mask(VIACR, 0x9B, polarity << 5, 0x60);
+        if (devices & VIA_LVDS1)
+                via_write_reg_mask(VIACR, 0x99, polarity << 5, 0x60);
+        if (devices & VIA_LVDS2)
+                via_write_reg_mask(VIACR, 0x97, polarity << 5, 0x60);
+}
 
-        case INTERFACE_DFP:
-                if ((UNICHROME_K8M890 == viaparinfo->chip_info->gfx_chip_name)
-                    || (UNICHROME_P4M890 ==
-                    viaparinfo->chip_info->gfx_chip_name))
-                        viafb_write_reg_mask(CR97, VIACR, 0x84,
-                                       BIT7 + BIT2 + BIT1 + BIT0);
-                if (set_iga == IGA1) {
-                        viafb_write_reg_mask(CR97, VIACR, 0x00, BIT4);
-                        viafb_write_reg_mask(CR99, VIACR, 0x00, BIT4);
-                } else {
-                        viafb_write_reg(CR91, VIACR, 0x00);
-                        viafb_write_reg_mask(CR97, VIACR, 0x10, BIT4);
-                        viafb_write_reg_mask(CR99, VIACR, 0x10, BIT4);
+u32 via_parse_odev(char *input, char **end)
+{
+        char *ptr = input;
+        u32 odev = 0;
+        bool next = true;
+        int i, len;
+
+        while (next) {
+                next = false;
+                for (i = 0; i < ARRAY_SIZE(device_mapping); i++) {
+                        len = strlen(device_mapping[i].name);
+                        if (!strncmp(ptr, device_mapping[i].name, len)) {
+                                odev |= device_mapping[i].device;
+                                ptr += len;
+                                if (*ptr == ',') {
+                                        ptr++;
+                                        next = true;
+                                }
+                        }
                 }
-                break;
+        }
 
-        case INTERFACE_LVDS0:
-        case INTERFACE_LVDS0LVDS1:
-                if (set_iga == IGA1)
-                        viafb_write_reg_mask(CR99, VIACR, 0x00, BIT4);
-                else
-                        viafb_write_reg_mask(CR99, VIACR, 0x10, BIT4);
+        *end = ptr;
+        return odev;
+}
 
-                break;
+void via_odev_to_seq(struct seq_file *m, u32 odev)
+{
+        int i, count = 0;
 
-        case INTERFACE_LVDS1:
-                if (set_iga == IGA1)
-                        viafb_write_reg_mask(CR97, VIACR, 0x00, BIT4);
-                else
-                        viafb_write_reg_mask(CR97, VIACR, 0x10, BIT4);
-                break;
+        for (i = 0; i < ARRAY_SIZE(device_mapping); i++) {
+                if (odev & device_mapping[i].device) {
+                        if (count > 0)
+                                seq_putc(m, ',');
+
+                        seq_puts(m, device_mapping[i].name);
+                        count++;
+                }
         }
+
+        seq_putc(m, '\n');
 }
 
 static void load_fix_bit_crtc_reg(void)
 {
+        viafb_unlock_crt();
+
         /* always set to 1 */
         viafb_write_reg_mask(CR03, VIACR, 0x80, BIT7);
         /* line compare should set all bits = 1 (extend modes) */
-        viafb_write_reg(CR18, VIACR, 0xff);
-        /* line compare should set all bits = 1 (extend modes) */
-        viafb_write_reg_mask(CR07, VIACR, 0x10, BIT4);
-        /* line compare should set all bits = 1 (extend modes) */
-        viafb_write_reg_mask(CR09, VIACR, 0x40, BIT6);
-        /* line compare should set all bits = 1 (extend modes) */
         viafb_write_reg_mask(CR35, VIACR, 0x10, BIT4);
         /* line compare should set all bits = 1 (extend modes) */
         viafb_write_reg_mask(CR33, VIACR, 0x06, BIT0 + BIT1 + BIT2);
         /*viafb_write_reg_mask(CR32, VIACR, 0x01, BIT0); */
-        /* extend mode always set to e3h */
-        viafb_write_reg(CR17, VIACR, 0xe3);
-        /* extend mode always set to 0h */
-        viafb_write_reg(CR08, VIACR, 0x00);
-        /* extend mode always set to 0h */
-        viafb_write_reg(CR14, VIACR, 0x00);
+
+        viafb_lock_crt();
 
         /* If K8M800, enable Prefetch Mode. */
         if ((viaparinfo->chip_info->gfx_chip_name == UNICHROME_K800)
@@ -1352,6 +1222,15 @@ void viafb_load_FIFO_reg(int set_iga, int hor_active, int ver_active)
                             VX855_IGA1_DISPLAY_QUEUE_EXPIRE_NUM;
                 }
 
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_VX900) {
+                        iga1_fifo_max_depth = VX900_IGA1_FIFO_MAX_DEPTH;
+                        iga1_fifo_threshold = VX900_IGA1_FIFO_THRESHOLD;
+                        iga1_fifo_high_threshold =
+                            VX900_IGA1_FIFO_HIGH_THRESHOLD;
+                        iga1_display_queue_expire_num =
+                            VX900_IGA1_DISPLAY_QUEUE_EXPIRE_NUM;
+                }
+
                 /* Set Display FIFO Depath Select */
                 reg_value = IGA1_FIFO_DEPTH_SELECT_FORMULA(iga1_fifo_max_depth);
                 viafb_load_reg_num =
@@ -1492,6 +1371,15 @@ void viafb_load_FIFO_reg(int set_iga, int hor_active, int ver_active)
                             VX855_IGA2_DISPLAY_QUEUE_EXPIRE_NUM;
                 }
 
+                if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_VX900) {
+                        iga2_fifo_max_depth = VX900_IGA2_FIFO_MAX_DEPTH;
+                        iga2_fifo_threshold = VX900_IGA2_FIFO_THRESHOLD;
+                        iga2_fifo_high_threshold =
+                            VX900_IGA2_FIFO_HIGH_THRESHOLD;
+                        iga2_display_queue_expire_num =
+                            VX900_IGA2_DISPLAY_QUEUE_EXPIRE_NUM;
+                }
+
                 if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_K800) {
                         /* Set Display FIFO Depath Select */
                         reg_value =
@@ -1558,139 +1446,83 @@ void viafb_load_FIFO_reg(int set_iga, int hor_active, int ver_active)
 
 }
 
-static u32 cle266_encode_pll(struct pll_config pll)
+static struct via_pll_config get_pll_config(struct pll_limit *limits, int size,
+        int clk)
 {
-        return (pll.multiplier << 8)
-                | (pll.rshift << 6)
-                | pll.divisor;
-}
-
-static u32 k800_encode_pll(struct pll_config pll)
-{
-        return ((pll.divisor - 2) << 16)
-                | (pll.rshift << 10)
-                | (pll.multiplier - 2);
-}
+        struct via_pll_config cur, up, down, best = {0, 1, 0};
+        const u32 f0 = 14318180; /* X1 frequency */
+        int i, f;
+
+        for (i = 0; i < size; i++) {
+                cur.rshift = limits[i].rshift;
+                cur.divisor = limits[i].divisor;
+                cur.multiplier = clk / ((f0 / cur.divisor)>>cur.rshift);
+                f = abs(get_pll_output_frequency(f0, cur) - clk);
+                up = down = cur;
+                up.multiplier++;
+                down.multiplier--;
+                if (abs(get_pll_output_frequency(f0, up) - clk) < f)
+                        cur = up;
+                else if (abs(get_pll_output_frequency(f0, down) - clk) < f)
+                        cur = down;
+
+                if (cur.multiplier < limits[i].multiplier_min)
+                        cur.multiplier = limits[i].multiplier_min;
+                else if (cur.multiplier > limits[i].multiplier_max)
+                        cur.multiplier = limits[i].multiplier_max;
+
+                f = abs(get_pll_output_frequency(f0, cur) - clk);
+                if (f < abs(get_pll_output_frequency(f0, best) - clk))
+                        best = cur;
+        }
 
-static u32 vx855_encode_pll(struct pll_config pll)
-{
-        return (pll.divisor << 16)
-                | (pll.rshift << 10)
-                | pll.multiplier;
+        return best;
 }
 
-u32 viafb_get_clk_value(int clk)
+static struct via_pll_config get_best_pll_config(int clk)
 {
-        u32 value = 0;
-        int i = 0;
-
-        while (i < NUM_TOTAL_PLL_TABLE && clk != pll_value[i].clk)
-                i++;
-
-        if (i == NUM_TOTAL_PLL_TABLE) {
-                printk(KERN_WARNING "viafb_get_clk_value: PLL lookup failed!");
-        } else {
-                switch (viaparinfo->chip_info->gfx_chip_name) {
-                case UNICHROME_CLE266:
-                case UNICHROME_K400:
-                        value = cle266_encode_pll(pll_value[i].cle266_pll);
-                        break;
+        struct via_pll_config config;
 
-                case UNICHROME_K800:
-                case UNICHROME_PM800:
-                case UNICHROME_CN700:
-                        value = k800_encode_pll(pll_value[i].k800_pll);
-                        break;
-
-                case UNICHROME_CX700:
-                case UNICHROME_CN750:
-                case UNICHROME_K8M890:
-                case UNICHROME_P4M890:
-                case UNICHROME_P4M900:
-                case UNICHROME_VX800:
-                        value = k800_encode_pll(pll_value[i].cx700_pll);
-                        break;
-
-                case UNICHROME_VX855:
-                        value = vx855_encode_pll(pll_value[i].vx855_pll);
-                        break;
-                }
+        switch (viaparinfo->chip_info->gfx_chip_name) {
+        case UNICHROME_CLE266:
+        case UNICHROME_K400:
+                config = get_pll_config(cle266_pll_limits,
+                        ARRAY_SIZE(cle266_pll_limits), clk);
+                break;
+        case UNICHROME_K800:
+        case UNICHROME_PM800:
+        case UNICHROME_CN700:
+                config = get_pll_config(k800_pll_limits,
+                        ARRAY_SIZE(k800_pll_limits), clk);
+                break;
+        case UNICHROME_CX700:
+        case UNICHROME_CN750:
+        case UNICHROME_K8M890:
+        case UNICHROME_P4M890:
+        case UNICHROME_P4M900:
+        case UNICHROME_VX800:
+                config = get_pll_config(cx700_pll_limits,
+                        ARRAY_SIZE(cx700_pll_limits), clk);
+                break;
+        case UNICHROME_VX855:
+        case UNICHROME_VX900:
+                config = get_pll_config(vx855_pll_limits,
+                        ARRAY_SIZE(vx855_pll_limits), clk);
+                break;
         }
 
-        return value;
+        return config;
 }
 
 /* Set VCLK*/
 void viafb_set_vclock(u32 clk, int set_iga)
 {
-        /* H.W. Reset : ON */
-        viafb_write_reg_mask(CR17, VIACR, 0x00, BIT7);
-
-        if (set_iga == IGA1) {
-                /* Change D,N FOR VCLK */
-                switch (viaparinfo->chip_info->gfx_chip_name) {
-                case UNICHROME_CLE266:
-                case UNICHROME_K400:
-                        via_write_reg(VIASR, SR46, (clk & 0x00FF));
-                        via_write_reg(VIASR, SR47, (clk & 0xFF00) >> 8);
-                        break;
-
-                case UNICHROME_K800:
-                case UNICHROME_PM800:
-                case UNICHROME_CN700:
-                case UNICHROME_CX700:
-                case UNICHROME_CN750:
-                case UNICHROME_K8M890:
-                case UNICHROME_P4M890:
-                case UNICHROME_P4M900:
-                case UNICHROME_VX800:
-                case UNICHROME_VX855:
-                        via_write_reg(VIASR, SR44, (clk & 0x0000FF));
-                        via_write_reg(VIASR, SR45, (clk & 0x00FF00) >> 8);
-                        via_write_reg(VIASR, SR46, (clk & 0xFF0000) >> 16);
-                        break;
-                }
-        }
-
-        if (set_iga == IGA2) {
-                /* Change D,N FOR LCK */
-                switch (viaparinfo->chip_info->gfx_chip_name) {
-                case UNICHROME_CLE266:
-                case UNICHROME_K400:
-                        via_write_reg(VIASR, SR44, (clk & 0x00FF));
-                        via_write_reg(VIASR, SR45, (clk & 0xFF00) >> 8);
-                        break;
-
-                case UNICHROME_K800:
-                case UNICHROME_PM800:
-                case UNICHROME_CN700:
-                case UNICHROME_CX700:
-                case UNICHROME_CN750:
-                case UNICHROME_K8M890:
-                case UNICHROME_P4M890:
-                case UNICHROME_P4M900:
-                case UNICHROME_VX800:
-                case UNICHROME_VX855:
-                        via_write_reg(VIASR, SR4A, (clk & 0x0000FF));
-                        via_write_reg(VIASR, SR4B, (clk & 0x00FF00) >> 8);
-                        via_write_reg(VIASR, SR4C, (clk & 0xFF0000) >> 16);
-                        break;
-                }
-        }
+        struct via_pll_config config = get_best_pll_config(clk);
 
-        /* H.W. Reset : OFF */
-        viafb_write_reg_mask(CR17, VIACR, 0x80, BIT7);
-
-        /* Reset PLL */
-        if (set_iga == IGA1) {
-                viafb_write_reg_mask(SR40, VIASR, 0x02, BIT1);
-                viafb_write_reg_mask(SR40, VIASR, 0x00, BIT1);
-        }
-
-        if (set_iga == IGA2) {
-                viafb_write_reg_mask(SR40, VIASR, 0x01, BIT0);
-                viafb_write_reg_mask(SR40, VIASR, 0x00, BIT0);
-        }
+        if (set_iga == IGA1)
+                clock.set_primary_pll(config);
+        if (set_iga == IGA2)
+                clock.set_secondary_pll(config);
 
         /* Fire! */
         via_write_misc_reg_mask(0x0C, 0x0C); /* select external clock */
@@ -1936,14 +1768,15 @@ void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
         int i;
         int index = 0;
         int h_addr, v_addr;
-        u32 pll_D_N;
-        u8 polarity = 0;
+        u32 clock, refresh = viafb_refresh;
+
+        if (viafb_SAMM_ON && set_iga == IGA2)
+                refresh = viafb_refresh1;
 
         for (i = 0; i < video_mode->mode_array; i++) {
                 index = i;
 
-                if (crt_table[i].refresh_rate == viaparinfo->
-                        crt_setting_info->refresh_rate)
+                if (crt_table[i].refresh_rate == refresh)
                         break;
         }
 
@@ -1954,7 +1787,7 @@ void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
         if ((viafb_LCD_ON | viafb_DVI_ON)
             && video_mode->crtc[0].crtc.hor_addr == 640
             && video_mode->crtc[0].crtc.ver_addr == 480
-            && viaparinfo->crt_setting_info->refresh_rate == 60) {
+            && refresh == 60) {
                 /* The border is 8 pixels. */
                 crt_reg.hor_blank_start = crt_reg.hor_blank_start - 8;
 
@@ -1964,18 +1797,8 @@ void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
 
         h_addr = crt_reg.hor_addr;
         v_addr = crt_reg.ver_addr;
-
-        /* update polarity for CRT timing */
-        if (crt_table[index].h_sync_polarity == NEGATIVE)
-                polarity |= BIT6;
-        if (crt_table[index].v_sync_polarity == NEGATIVE)
-                polarity |= BIT7;
-        via_write_misc_reg_mask(polarity, BIT6 | BIT7);
-
         if (set_iga == IGA1) {
                 viafb_unlock_crt();
-                viafb_write_reg(CR09, VIACR, 0x00);     /*initial CR09=0 */
-                viafb_write_reg_mask(CR11, VIACR, 0x00, BIT4 + BIT5 + BIT6);
                 viafb_write_reg_mask(CR17, VIACR, 0x00, BIT7);
         }
 
@@ -1988,7 +1811,6 @@ void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
                 break;
         }
 
-        load_fix_bit_crtc_reg();
         viafb_lock_crt();
         viafb_write_reg_mask(CR17, VIACR, 0x80, BIT7);
         viafb_load_fetch_count_reg(h_addr, bpp_byte, set_iga);
@@ -1998,21 +1820,19 @@ void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
             && (viaparinfo->chip_info->gfx_chip_name != UNICHROME_K400))
                 viafb_load_FIFO_reg(set_iga, h_addr, v_addr);
 
-        pll_D_N = viafb_get_clk_value(crt_table[index].clk);
-        DEBUG_MSG(KERN_INFO "PLL=%x", pll_D_N);
-        viafb_set_vclock(pll_D_N, set_iga);
+        clock = crt_reg.hor_total * crt_reg.ver_total
+                * crt_table[index].refresh_rate;
+        viafb_set_vclock(clock, set_iga);
 
 }
 
-void viafb_init_chip_info(int chip_type)
+void __devinit viafb_init_chip_info(int chip_type)
 {
+        via_clock_init(&clock, chip_type);
         init_gfx_chip_info(chip_type);
         init_tmds_chip_info();
         init_lvds_chip_info();
 
-        viaparinfo->crt_setting_info->iga_path = IGA1;
-        viaparinfo->crt_setting_info->refresh_rate = viafb_refresh;
-
         /*Set IGA path for each device */
         viafb_set_iga_path();
 
@@ -2024,29 +1844,18 @@ void viafb_init_chip_info(int chip_type)
                 viaparinfo->lvds_setting_info->lcd_mode;
 }
 
-void viafb_update_device_setting(int hres, int vres,
-        int bpp, int vmode_refresh, int flag)
+void viafb_update_device_setting(int hres, int vres, int bpp, int flag)
 {
         if (flag == 0) {
-                viaparinfo->crt_setting_info->h_active = hres;
-                viaparinfo->crt_setting_info->v_active = vres;
-                viaparinfo->crt_setting_info->bpp = bpp;
-                viaparinfo->crt_setting_info->refresh_rate =
-                        vmode_refresh;
-
                 viaparinfo->tmds_setting_info->h_active = hres;
                 viaparinfo->tmds_setting_info->v_active = vres;
 
                 viaparinfo->lvds_setting_info->h_active = hres;
                 viaparinfo->lvds_setting_info->v_active = vres;
                 viaparinfo->lvds_setting_info->bpp = bpp;
-                viaparinfo->lvds_setting_info->refresh_rate =
-                        vmode_refresh;
                 viaparinfo->lvds_setting_info2->h_active = hres;
                 viaparinfo->lvds_setting_info2->v_active = vres;
                 viaparinfo->lvds_setting_info2->bpp = bpp;
-                viaparinfo->lvds_setting_info2->refresh_rate =
-                        vmode_refresh;
         } else {
 
                 if (viaparinfo->tmds_setting_info->iga_path == IGA2) {
@@ -2058,20 +1867,16 @@ void viafb_update_device_setting(int hres, int vres,
                         viaparinfo->lvds_setting_info->h_active = hres;
                         viaparinfo->lvds_setting_info->v_active = vres;
                         viaparinfo->lvds_setting_info->bpp = bpp;
-                        viaparinfo->lvds_setting_info->refresh_rate =
-                                vmode_refresh;
                 }
                 if (IGA2 == viaparinfo->lvds_setting_info2->iga_path) {
                         viaparinfo->lvds_setting_info2->h_active = hres;
                         viaparinfo->lvds_setting_info2->v_active = vres;
                         viaparinfo->lvds_setting_info2->bpp = bpp;
-                        viaparinfo->lvds_setting_info2->refresh_rate =
-                                vmode_refresh;
                 }
         }
 }
 
-static void init_gfx_chip_info(int chip_type)
+static void __devinit init_gfx_chip_info(int chip_type)
 {
         u8 tmp;
 
@@ -2111,6 +1916,7 @@ static void init_gfx_chip_info(int chip_type)
         switch (viaparinfo->chip_info->gfx_chip_name) {
         case UNICHROME_VX800:
         case UNICHROME_VX855:
+        case UNICHROME_VX900:
                 viaparinfo->chip_info->twod_engine = VIA_2D_ENG_M1;
                 break;
         case UNICHROME_K8M890:
@@ -2123,7 +1929,7 @@ static void init_gfx_chip_info(int chip_type)
         }
 }
 
-static void init_tmds_chip_info(void)
+static void __devinit init_tmds_chip_info(void)
 {
         viafb_tmds_trasmitter_identify();
 
@@ -2168,7 +1974,7 @@ static void init_tmds_chip_info(void)
                 &viaparinfo->shared->tmds_setting_info);
 }
 
-static void init_lvds_chip_info(void)
+static void __devinit init_lvds_chip_info(void)
 {
         viafb_lvds_trasmitter_identify();
         viafb_init_lcd_size();
@@ -2202,7 +2008,7 @@ static void init_lvds_chip_info(void)
                   viaparinfo->chip_info->lvds_chip_info.output_interface);
 }
 
-void viafb_init_dac(int set_iga)
+void __devinit viafb_init_dac(int set_iga)
 {
         int i;
         u8 tmp;
@@ -2275,11 +2081,24 @@ static void set_display_channel(void)
         }
 }
 
+static u8 get_sync(struct fb_info *info)
+{
+        u8 polarity = 0;
+
+        if (!(info->var.sync & FB_SYNC_HOR_HIGH_ACT))
+                polarity |= VIA_HSYNC_NEGATIVE;
+        if (!(info->var.sync & FB_SYNC_VERT_HIGH_ACT))
+                polarity |= VIA_VSYNC_NEGATIVE;
+        return polarity;
+}
+
 int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
         struct VideoModeTable *vmode_tbl1, int video_bpp1)
 {
         int i, j;
         int port;
+        u32 devices = viaparinfo->shared->iga1_devices
+                | viaparinfo->shared->iga2_devices;
         u8 value, index, mask;
         struct crt_mode_table *crt_timing;
         struct crt_mode_table *crt_timing1 = NULL;
@@ -2295,6 +2114,7 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
         outb(0x00, VIAAR);
 
         /* Write Common Setting for Video Mode */
+        viafb_write_regx(common_vga, ARRAY_SIZE(common_vga));
         switch (viaparinfo->chip_info->gfx_chip_name) {
         case UNICHROME_CLE266:
                 viafb_write_regx(CLE266_ModeXregs, NUM_TOTAL_CLE266_ModeXregs);
@@ -2322,11 +2142,14 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
                 break;
 
         case UNICHROME_VX855:
+        case UNICHROME_VX900:
                 viafb_write_regx(VX855_ModeXregs, NUM_TOTAL_VX855_ModeXregs);
                 break;
         }
 
+        viafb_write_regx(scaling_parameters, ARRAY_SIZE(scaling_parameters));
         device_off();
+        via_set_state(devices, VIA_STATE_OFF);
 
         /* Fill VPIT Parameters */
         /* Write Misc Register */
@@ -2337,10 +2160,6 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
                 via_write_reg(VIASR, i, VPIT.SR[i - 1]);
 
         viafb_write_reg_mask(0x15, VIASR, 0xA2, 0xA2);
-        viafb_set_iga_path();
-
-        /* Write CRTC */
-        viafb_fill_crtc_timing(crt_timing, vmode_tbl, video_bpp / 8, IGA1);
 
         /* Write Graphic Controller */
         for (i = 0; i < StdGR; i++)
@@ -2371,31 +2190,37 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
                 }
         }
 
+        load_fix_bit_crtc_reg();
         via_set_primary_pitch(viafbinfo->fix.line_length);
         via_set_secondary_pitch(viafb_dual_fb ? viafbinfo1->fix.line_length
                 : viafbinfo->fix.line_length);
         via_set_primary_color_depth(viaparinfo->depth);
         via_set_secondary_color_depth(viafb_dual_fb ? viaparinfo1->depth
                 : viaparinfo->depth);
+        via_set_source(viaparinfo->shared->iga1_devices, IGA1);
+        via_set_source(viaparinfo->shared->iga2_devices, IGA2);
+        if (viaparinfo->shared->iga2_devices)
+                enable_second_display_channel();
+        else
+                disable_second_display_channel();
+
         /* Update Refresh Rate Setting */
 
         /* Clear On Screen */
 
         /* CRT set mode */
         if (viafb_CRT_ON) {
-                if (viafb_SAMM_ON && (viaparinfo->crt_setting_info->iga_path ==
-                        IGA2)) {
+                if (viafb_SAMM_ON &&
+                        viaparinfo->shared->iga2_devices & VIA_CRT) {
                         viafb_fill_crtc_timing(crt_timing1, vmode_tbl1,
-                                video_bpp1 / 8,
-                                viaparinfo->crt_setting_info->iga_path);
+                                video_bpp1 / 8, IGA2);
                 } else {
                         viafb_fill_crtc_timing(crt_timing, vmode_tbl,
                                 video_bpp / 8,
-                                viaparinfo->crt_setting_info->iga_path);
+                                (viaparinfo->shared->iga1_devices & VIA_CRT)
+                                ? IGA1 : IGA2);
                 }
 
-                set_crt_output_path(viaparinfo->crt_setting_info->iga_path);
-
                 /* Patch if set_hres is not 8 alignment (1366) to viafb_setmode
                 to 8 alignment (1368),there is several pixels (2 pixels)
                 on right side of screen. */
@@ -2482,10 +2307,43 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
                         viafb_DeviceStatus = CRT_Device;
         }
         device_on();
+        if (!viafb_dual_fb)
+                via_set_sync_polarity(devices, get_sync(viafbinfo));
+        else {
+                via_set_sync_polarity(viaparinfo->shared->iga1_devices,
+                        get_sync(viafbinfo));
+                via_set_sync_polarity(viaparinfo->shared->iga2_devices,
+                        get_sync(viafbinfo1));
+        }
 
-        if (viafb_SAMM_ON == 1)
-                viafb_write_reg_mask(CR6A, VIACR, 0xC0, BIT6 + BIT7);
+        clock.set_engine_pll_state(VIA_STATE_ON);
+        clock.set_primary_clock_source(VIA_CLKSRC_X1, true);
+        clock.set_secondary_clock_source(VIA_CLKSRC_X1, true);
+
+#ifdef CONFIG_FB_VIA_X_COMPATIBILITY
+        clock.set_primary_pll_state(VIA_STATE_ON);
+        clock.set_primary_clock_state(VIA_STATE_ON);
+        clock.set_secondary_pll_state(VIA_STATE_ON);
+        clock.set_secondary_clock_state(VIA_STATE_ON);
+#else
+        if (viaparinfo->shared->iga1_devices) {
+                clock.set_primary_pll_state(VIA_STATE_ON);
+                clock.set_primary_clock_state(VIA_STATE_ON);
+        } else {
+                clock.set_primary_pll_state(VIA_STATE_OFF);
+                clock.set_primary_clock_state(VIA_STATE_OFF);
+        }
+
+        if (viaparinfo->shared->iga2_devices) {
+                clock.set_secondary_pll_state(VIA_STATE_ON);
+                clock.set_secondary_clock_state(VIA_STATE_ON);
+        } else {
+                clock.set_secondary_pll_state(VIA_STATE_OFF);
+                clock.set_secondary_clock_state(VIA_STATE_OFF);
+        }
+#endif /*CONFIG_FB_VIA_X_COMPATIBILITY*/
 
+        via_set_state(devices, VIA_STATE_ON);
         device_screen_on();
         return 1;
 }
@@ -2493,64 +2351,63 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
 int viafb_get_pixclock(int hres, int vres, int vmode_refresh)
 {
         int i;
+        struct crt_mode_table *best;
+        struct VideoModeTable *vmode = viafb_get_mode(hres, vres);
+
+        if (!vmode)
+                return RES_640X480_60HZ_PIXCLOCK;
 
-        for (i = 0; i < NUM_TOTAL_RES_MAP_REFRESH; i++) {
-                if ((hres == res_map_refresh_tbl[i].hres)
-                    && (vres == res_map_refresh_tbl[i].vres)
-                    && (vmode_refresh == res_map_refresh_tbl[i].vmode_refresh))
-                        return res_map_refresh_tbl[i].pixclock;
+        best = &vmode->crtc[0];
+        for (i = 1; i < vmode->mode_array; i++) {
+                if (abs(vmode->crtc[i].refresh_rate - vmode_refresh)
+                        < abs(best->refresh_rate - vmode_refresh))
+                        best = &vmode->crtc[i];
         }
-        return RES_640X480_60HZ_PIXCLOCK;
 
+        return 1000000000 / (best->crtc.hor_total * best->crtc.ver_total)
+                * 1000 / best->refresh_rate;
 }
 
 int viafb_get_refresh(int hres, int vres, u32 long_refresh)
 {
-#define REFRESH_TOLERANCE 3
-        int i, nearest = -1, diff = REFRESH_TOLERANCE;
-        for (i = 0; i < NUM_TOTAL_RES_MAP_REFRESH; i++) {
-                if ((hres == res_map_refresh_tbl[i].hres)
-                    && (vres == res_map_refresh_tbl[i].vres)
-                    && (diff > (abs(long_refresh -
-                    res_map_refresh_tbl[i].vmode_refresh)))) {
-                        diff = abs(long_refresh - res_map_refresh_tbl[i].
-                                vmode_refresh);
-                        nearest = i;
-                }
+        int i;
+        struct crt_mode_table *best;
+        struct VideoModeTable *vmode = viafb_get_mode(hres, vres);
+
+        if (!vmode)
+                return 60;
+
+        best = &vmode->crtc[0];
+        for (i = 1; i < vmode->mode_array; i++) {
+                if (abs(vmode->crtc[i].refresh_rate - long_refresh)
+                        < abs(best->refresh_rate - long_refresh))
+                        best = &vmode->crtc[i];
         }
-#undef REFRESH_TOLERANCE
-        if (nearest > 0)
-                return res_map_refresh_tbl[nearest].vmode_refresh;
-        return 60;
+
+        if (abs(best->refresh_rate - long_refresh) > 3) {
+                if (hres == 1200 && vres == 900)
+                        return 49; /* OLPC DCON only supports 50 Hz */
+                else
+                        return 60;
+        }
+
+        return best->refresh_rate;
 }
 
 static void device_off(void)
 {
-        viafb_crt_disable();
         viafb_dvi_disable();
         viafb_lcd_disable();
 }
 
 static void device_on(void)
 {
-        if (viafb_CRT_ON == 1)
-                viafb_crt_enable();
         if (viafb_DVI_ON == 1)
                 viafb_dvi_enable();
         if (viafb_LCD_ON == 1)
                 viafb_lcd_enable();
 }
 
-void viafb_crt_disable(void)
-{
-        viafb_write_reg_mask(CR36, VIACR, BIT5 + BIT4, BIT5 + BIT4);
-}
-
-void viafb_crt_enable(void)
-{
-        viafb_write_reg_mask(CR36, VIACR, 0x0, BIT5 + BIT4);
-}
-
 static void enable_second_display_channel(void)
 {
         /* to enable second display channel. */
@@ -2567,7 +2424,6 @@ static void disable_second_display_channel(void)
         viafb_write_reg_mask(CR6A, VIACR, BIT6, BIT6);
 }
 
-
 void viafb_set_dpa_gfx(int output_interface, struct GFX_DPA_SETTING\
                                         *p_gfx_dpa_setting)
 {
@@ -2652,4 +2508,9 @@ void viafb_fill_var_timing_info(struct fb_var_screeninfo *var, int refresh,
             crt_reg.ver_total - (crt_reg.ver_sync_start + crt_reg.ver_sync_end);
         var->lower_margin = crt_reg.ver_sync_start - crt_reg.ver_addr;
         var->vsync_len = crt_reg.ver_sync_end;
+        var->sync = 0;
+        if (crt_timing[index].h_sync_polarity == POSITIVE)
+                var->sync |= FB_SYNC_HOR_HIGH_ACT;
+        if (crt_timing[index].v_sync_polarity == POSITIVE)
+                var->sync |= FB_SYNC_VERT_HIGH_ACT;
 }
diff --git a/drivers/video/via/hw.h b/drivers/video/via/hw.h
index c44399895294..c7239eb83bae 100644
--- a/drivers/video/via/hw.h
+++ b/drivers/video/via/hw.h
@@ -22,6 +22,8 @@
 #ifndef __HW_H__
 #define __HW_H__
 
+#include <linux/seq_file.h>
+
 #include "viamode.h"
 #include "global.h"
 #include "via_modesetting.h"
@@ -30,6 +32,25 @@
 #define viafb_write_reg(i, p, d)                via_write_reg(p, i, d)
 #define viafb_write_reg_mask(i, p, d, m)        via_write_reg_mask(p, i, d, m)
 
+/* VIA output devices */
+#define VIA_LDVP0       0x00000001
+#define VIA_LDVP1       0x00000002
+#define VIA_DVP0        0x00000004
+#define VIA_CRT         0x00000010
+#define VIA_DVP1        0x00000020
+#define VIA_LVDS1       0x00000040
+#define VIA_LVDS2       0x00000080
+
+/* VIA output device power states */
+#define VIA_STATE_ON            0
+#define VIA_STATE_STANDBY       1
+#define VIA_STATE_SUSPEND       2
+#define VIA_STATE_OFF           3
+
+/* VIA output device sync polarity */
+#define VIA_HSYNC_NEGATIVE      0x01
+#define VIA_VSYNC_NEGATIVE      0x02
+
 /***************************************************
 * Definition IGA1 Design Method of CRTC Registers *
 ****************************************************/
@@ -341,6 +362,17 @@ is reserved, so it may have problem to set 1600x1200 on IGA2. */
 #define VX855_IGA2_FIFO_HIGH_THRESHOLD          160
 #define VX855_IGA2_DISPLAY_QUEUE_EXPIRE_NUM     320
 
+/* For VT3410 */
+#define VX900_IGA1_FIFO_MAX_DEPTH               400
+#define VX900_IGA1_FIFO_THRESHOLD               320
+#define VX900_IGA1_FIFO_HIGH_THRESHOLD          320
+#define VX900_IGA1_DISPLAY_QUEUE_EXPIRE_NUM     160
+
+#define VX900_IGA2_FIFO_MAX_DEPTH               192
+#define VX900_IGA2_FIFO_THRESHOLD               160
+#define VX900_IGA2_FIFO_HIGH_THRESHOLD          160
+#define VX900_IGA2_DISPLAY_QUEUE_EXPIRE_NUM     320
+
 #define IGA1_FIFO_DEPTH_SELECT_REG_NUM          1
 #define IGA1_FIFO_THRESHOLD_REG_NUM             2
 #define IGA1_FIFO_HIGH_THRESHOLD_REG_NUM        2
@@ -700,20 +732,13 @@ struct _lcd_scaling_factor {
         struct _lcd_ver_scaling_factor lcd_ver_scaling_factor;
 };
 
-struct pll_config {
-        u16 multiplier;
+struct pll_limit {
+        u16 multiplier_min;
+        u16 multiplier_max;
         u8 divisor;
         u8 rshift;
 };
 
-struct pll_map {
-        u32 clk;
-        struct pll_config cle266_pll;
-        struct pll_config k800_pll;
-        struct pll_config cx700_pll;
-        struct pll_config vx855_pll;
-};
-
 struct rgbLUT {
         u8 red;
         u8 green;
@@ -858,8 +883,8 @@ struct iga2_crtc_timing {
 #define VX800_FUNCTION3     0x3353
 /* VT3409 chipset*/
 #define VX855_FUNCTION3     0x3409
-
-#define NUM_TOTAL_PLL_TABLE ARRAY_SIZE(pll_value)
+/* VT3410 chipset*/
+#define VX900_FUNCTION3     0x3410
 
 struct IODATA {
         u8 Index;
@@ -873,7 +898,11 @@ struct pci_device_id_info {
         u32 chip_index;
 };
 
-extern unsigned int viafb_second_virtual_xres;
+struct via_device_mapping {
+        u32 device;
+        const char *name;
+};
+
 extern int viafb_SAMM_ON;
 extern int viafb_dual_fb;
 extern int viafb_LCD2_ON;
@@ -881,9 +910,6 @@ extern int viafb_LCD_ON;
 extern int viafb_DVI_ON;
 extern int viafb_hotplug;
 
-void viafb_set_output_path(int device, int set_iga,
-        int output_interface);
-
 void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
         struct VideoModeTable *video_mode, int bpp_byte, int set_iga);
 
@@ -891,15 +917,17 @@ void viafb_set_vclock(u32 CLK, int set_iga);
 void viafb_load_reg(int timing_value, int viafb_load_reg_num,
         struct io_register *reg,
               int io_type);
-void viafb_crt_disable(void);
-void viafb_crt_enable(void);
+void via_set_source(u32 devices, u8 iga);
+void via_set_state(u32 devices, u8 state);
+void via_set_sync_polarity(u32 devices, u8 polarity);
+u32 via_parse_odev(char *input, char **end);
+void via_odev_to_seq(struct seq_file *m, u32 odev);
 void init_ad9389(void);
 /* Access I/O Function */
 void viafb_lock_crt(void);
 void viafb_unlock_crt(void);
 void viafb_load_fetch_count_reg(int h_addr, int bpp_byte, int set_iga);
 void viafb_write_regx(struct io_reg RegTable[], int ItemNum);
-u32 viafb_get_clk_value(int clk);
 void viafb_load_FIFO_reg(int set_iga, int hor_active, int ver_active);
 void viafb_set_dpa_gfx(int output_interface, struct GFX_DPA_SETTING\
                                         *p_gfx_dpa_setting);
@@ -908,12 +936,11 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
         struct VideoModeTable *vmode_tbl1, int video_bpp1);
 void viafb_fill_var_timing_info(struct fb_var_screeninfo *var, int refresh,
         struct VideoModeTable *vmode_tbl);
-void viafb_init_chip_info(int chip_type);
-void viafb_init_dac(int set_iga);
+void __devinit viafb_init_chip_info(int chip_type);
+void __devinit viafb_init_dac(int set_iga);
 int viafb_get_pixclock(int hres, int vres, int vmode_refresh);
 int viafb_get_refresh(int hres, int vres, u32 float_refresh);
-void viafb_update_device_setting(int hres, int vres, int bpp,
-                           int vmode_refresh, int flag);
+void viafb_update_device_setting(int hres, int vres, int bpp, int flag);
 
 void viafb_set_iga_path(void);
 void viafb_set_primary_color_register(u8 index, u8 red, u8 green, u8 blue);
diff --git a/drivers/video/via/ioctl.c b/drivers/video/via/ioctl.c
index 4d553d0b8d7a..ea1c51428823 100644
--- a/drivers/video/via/ioctl.c
+++ b/drivers/video/via/ioctl.c
@@ -94,6 +94,7 @@ int viafb_ioctl_hotplug(int hres, int vres, int bpp)
                                 viafb_CRT_ON = 0;
                                 viafb_LCD_ON = 0;
                                 viafb_DeviceStatus = DVI_Device;
+                                viafb_set_iga_path();
                                 return viafb_DeviceStatus;
                         }
                         status = 1;
@@ -107,6 +108,7 @@ int viafb_ioctl_hotplug(int hres, int vres, int bpp)
                 viafb_LCD_ON = 0;
 
                 viafb_DeviceStatus = CRT_Device;
+                viafb_set_iga_path();
                 return viafb_DeviceStatus;
         }
 
diff --git a/drivers/video/via/lcd.c b/drivers/video/via/lcd.c
index fc25ae30c5f6..6e06981d638b 100644
--- a/drivers/video/via/lcd.c
+++ b/drivers/video/via/lcd.c
@@ -21,10 +21,18 @@
 #include <linux/via-core.h>
 #include <linux/via_i2c.h>
 #include "global.h"
-#include "lcdtbl.h"
 
 #define viafb_compact_res(x, y) (((x)<<16)|(y))
 
+/* CLE266 Software Power Sequence */
+/* {Mask}, {Data}, {Delay} */
+static const int PowerSequenceOn[3][3] = {
+        {0x10, 0x08, 0x06}, {0x10, 0x08, 0x06}, {0x19, 0x1FE, 0x01}
+};
+static const int PowerSequenceOff[3][3] = {
+        {0x06, 0x08, 0x10}, {0x00, 0x00, 0x00}, {0xD2, 0x19, 0x01}
+};
+
 static struct _lcd_scaling_factor lcd_scaling_factor = {
         /* LCD Horizontal Scaling Factor Register */
         {LCD_HOR_SCALING_FACTOR_REG_NUM,
@@ -40,9 +48,8 @@ static struct _lcd_scaling_factor lcd_scaling_factor_CLE = {
         {LCD_VER_SCALING_FACTOR_REG_NUM_CLE, {{CR78, 0, 7}, {CR79, 6, 7} } }
 };
 
-static int check_lvds_chip(int device_id_subaddr, int device_id);
 static bool lvds_identify_integratedlvds(void);
-static void fp_id_to_vindex(int panel_id);
+static void __devinit fp_id_to_vindex(int panel_id);
 static int lvds_register_read(int index);
 static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
                       int panel_vres);
@@ -76,21 +83,16 @@ static struct display_timing lcd_centering_timging(struct display_timing
                                             mode_crt_reg,
                                            struct display_timing panel_crt_reg);
 
-static int check_lvds_chip(int device_id_subaddr, int device_id)
+static inline bool check_lvds_chip(int device_id_subaddr, int device_id)
 {
-        if (lvds_register_read(device_id_subaddr) == device_id)
-                return OK;
-        else
-                return FAIL;
+        return lvds_register_read(device_id_subaddr) == device_id;
 }
 
-void viafb_init_lcd_size(void)
+void __devinit viafb_init_lcd_size(void)
 {
         DEBUG_MSG(KERN_INFO "viafb_init_lcd_size()\n");
 
         fp_id_to_vindex(viafb_lcd_panel_id);
-        viaparinfo->lvds_setting_info2->lcd_panel_id =
-                viaparinfo->lvds_setting_info->lcd_panel_id;
         viaparinfo->lvds_setting_info2->lcd_panel_hres =
                 viaparinfo->lvds_setting_info->lcd_panel_hres;
         viaparinfo->lvds_setting_info2->lcd_panel_vres =
@@ -144,7 +146,7 @@ static bool lvds_identify_integratedlvds(void)
         return true;
 }
 
-int viafb_lvds_trasmitter_identify(void)
+bool __devinit viafb_lvds_trasmitter_identify(void)
 {
         if (viafb_lvds_identify_vt1636(VIA_PORT_31)) {
                 viaparinfo->chip_info->lvds_chip_info.i2c_port = VIA_PORT_31;
@@ -169,23 +171,23 @@ int viafb_lvds_trasmitter_identify(void)
         viaparinfo->chip_info->lvds_chip_info.lvds_chip_slave_addr =
                 VT1631_LVDS_I2C_ADDR;
 
-        if (check_lvds_chip(VT1631_DEVICE_ID_REG, VT1631_DEVICE_ID) != FAIL) {
+        if (check_lvds_chip(VT1631_DEVICE_ID_REG, VT1631_DEVICE_ID)) {
                 DEBUG_MSG(KERN_INFO "\n VT1631 LVDS ! \n");
                 DEBUG_MSG(KERN_INFO "\n %2d",
                           viaparinfo->chip_info->lvds_chip_info.lvds_chip_name);
                 DEBUG_MSG(KERN_INFO "\n %2d",
                           viaparinfo->chip_info->lvds_chip_info.lvds_chip_name);
-                return OK;
+                return true;
         }
 
         viaparinfo->chip_info->lvds_chip_info.lvds_chip_name =
                 NON_LVDS_TRANSMITTER;
         viaparinfo->chip_info->lvds_chip_info.lvds_chip_slave_addr =
                 VT1631_LVDS_I2C_ADDR;
-        return FAIL;
+        return false;
 }
 
-static void fp_id_to_vindex(int panel_id)
+static void __devinit fp_id_to_vindex(int panel_id)
 {
         DEBUG_MSG(KERN_INFO "fp_get_panel_id()\n");
 
@@ -197,176 +199,132 @@ static void fp_id_to_vindex(int panel_id)
         case 0x0:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 640;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 480;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID0_640X480;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x1:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 800;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 600;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID1_800X600;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x2:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1024;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID2_1024X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x3:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID3_1280X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x4:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 1024;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID4_1280X1024;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x5:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1400;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 1050;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID5_1400X1050;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x6:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1600;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 1200;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID6_1600X1200;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x8:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 800;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 480;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_IDA_800X480;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x9:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1024;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID2_1024X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0xA:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1024;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID2_1024X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0xB:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1024;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID2_1024X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0xC:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID3_1280X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0xD:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 1024;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID4_1280X1024;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0xE:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1400;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 1050;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID5_1400X1050;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0xF:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1600;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 1200;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID6_1600X1200;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0x10:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1366;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID7_1366X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0x11:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1024;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 600;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID8_1024X600;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x12:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID3_1280X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x13:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 800;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID9_1280X800;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
         case 0x14:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1360;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_IDB_1360X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0x15:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1280;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 768;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID3_1280X768;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 1;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         case 0x16:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 480;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 640;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_IDC_480X640;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
                 break;
@@ -374,16 +332,12 @@ static void fp_id_to_vindex(int panel_id)
                 /* OLPC XO-1.5 panel */
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 1200;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 900;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_IDD_1200X900;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 0;
                 break;
         default:
                 viaparinfo->lvds_setting_info->lcd_panel_hres = 800;
                 viaparinfo->lvds_setting_info->lcd_panel_vres = 600;
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID1_800X600;
                 viaparinfo->lvds_setting_info->device_lcd_dualedge = 0;
                 viaparinfo->lvds_setting_info->LCDDithering = 1;
         }
@@ -436,6 +390,7 @@ static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
                 case UNICHROME_CN750:
                 case UNICHROME_VX800:
                 case UNICHROME_VX855:
+                case UNICHROME_VX900:
                         reg_value =
                             K800_LCD_HOR_SCF_FORMULA(set_hres, panel_hres);
                         /* Horizontal scaling enabled */
@@ -479,6 +434,7 @@ static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
                 case UNICHROME_CN750:
                 case UNICHROME_VX800:
                 case UNICHROME_VX855:
+                case UNICHROME_VX900:
                         reg_value =
                             K800_LCD_VER_SCF_FORMULA(set_vres, panel_vres);
                         /* Vertical scaling enabled */
@@ -602,7 +558,7 @@ void viafb_lcd_set_mode(struct crt_mode_table *mode_crt_table,
         int set_vres = plvds_setting_info->v_active;
         int panel_hres = plvds_setting_info->lcd_panel_hres;
         int panel_vres = plvds_setting_info->lcd_panel_vres;
-        u32 pll_D_N;
+        u32 clock;
         struct display_timing mode_crt_reg, panel_crt_reg;
         struct crt_mode_table *panel_crt_table = NULL;
         struct VideoModeTable *vmode_tbl = viafb_get_mode(panel_hres,
@@ -617,7 +573,9 @@ void viafb_lcd_set_mode(struct crt_mode_table *mode_crt_table,
         DEBUG_MSG(KERN_INFO "bellow viafb_lcd_set_mode!!\n");
         if (VT1636_LVDS == plvds_chip_info->lvds_chip_name)
                 viafb_init_lvds_vt1636(plvds_setting_info, plvds_chip_info);
-        plvds_setting_info->vclk = panel_crt_table->clk;
+        clock = panel_crt_reg.hor_total * panel_crt_reg.ver_total
+                * panel_crt_table->refresh_rate;
+        plvds_setting_info->vclk = clock;
         if (set_iga == IGA1) {
                 /* IGA1 doesn't have LCD scaling, so set it as centering. */
                 viafb_load_crtc_timing(lcd_centering_timging
@@ -651,13 +609,7 @@ void viafb_lcd_set_mode(struct crt_mode_table *mode_crt_table,
                 viafb_load_FIFO_reg(set_iga, set_hres, set_vres);
 
         fill_lcd_format();
-
-        pll_D_N = viafb_get_clk_value(panel_crt_table[0].clk);
-        DEBUG_MSG(KERN_INFO "PLL=0x%x", pll_D_N);
-        viafb_set_vclock(pll_D_N, set_iga);
-
-        viafb_set_output_path(DEVICE_LCD, set_iga,
-                plvds_chip_info->output_interface);
+        viafb_set_vclock(clock, set_iga);
         lcd_patch_skew(plvds_setting_info, plvds_chip_info);
 
         /* If K8M800, enable LCD Prefetch Mode. */
@@ -700,9 +652,6 @@ static void integrated_lvds_disable(struct lvds_setting_information
                 viafb_write_reg_mask(CR91, VIACR, 0xC0, BIT6 + BIT7);
         }
 
-        /* Turn DFP High/Low Pad off. */
-        viafb_write_reg_mask(SR2A, VIASR, 0, BIT0 + BIT1 + BIT2 + BIT3);
-
         /* Power off LVDS channel. */
         switch (plvds_chip_info->output_interface) {
         case INTERFACE_LVDS0:
@@ -758,9 +707,6 @@ static void integrated_lvds_enable(struct lvds_setting_information
                 break;
         }
 
-        /* Turn DFP High/Low pad on. */
-        viafb_write_reg_mask(SR2A, VIASR, 0x0F, BIT0 + BIT1 + BIT2 + BIT3);
-
         /* Power on LVDS channel. */
         switch (plvds_chip_info->output_interface) {
         case INTERFACE_LVDS0:
@@ -809,29 +755,48 @@ void viafb_lcd_disable(void)
                 viafb_disable_lvds_vt1636(viaparinfo->lvds_setting_info,
                                     &viaparinfo->chip_info->lvds_chip_info);
         } else {
-                /* DFP-HL pad off          */
-                viafb_write_reg_mask(SR2A, VIASR, 0x00, 0x0F);
                 /* Backlight off           */
                 viafb_write_reg_mask(SR3D, VIASR, 0x00, 0x20);
                 /* 24 bit DI data paht off */
                 viafb_write_reg_mask(CR91, VIACR, 0x80, 0x80);
-                /* Simultaneout disabled   */
-                viafb_write_reg_mask(CR6B, VIACR, 0x00, 0x08);
         }
 
         /* Disable expansion bit   */
         viafb_write_reg_mask(CR79, VIACR, 0x00, 0x01);
-        /* CRT path set to IGA1    */
-        viafb_write_reg_mask(SR16, VIASR, 0x00, 0x40);
         /* Simultaneout disabled   */
         viafb_write_reg_mask(CR6B, VIACR, 0x00, 0x08);
-        /* IGA2 path disabled      */
-        viafb_write_reg_mask(CR6A, VIACR, 0x00, 0x80);
+}
 
+static void set_lcd_output_path(int set_iga, int output_interface)
+{
+        switch (output_interface) {
+        case INTERFACE_DFP:
+                if ((UNICHROME_K8M890 == viaparinfo->chip_info->gfx_chip_name)
+                    || (UNICHROME_P4M890 ==
+                    viaparinfo->chip_info->gfx_chip_name))
+                        viafb_write_reg_mask(CR97, VIACR, 0x84,
+                                       BIT7 + BIT2 + BIT1 + BIT0);
+        case INTERFACE_DVP0:
+        case INTERFACE_DVP1:
+        case INTERFACE_DFP_HIGH:
+        case INTERFACE_DFP_LOW:
+                if (set_iga == IGA2)
+                        viafb_write_reg(CR91, VIACR, 0x00);
+                break;
+        }
 }
 
 void viafb_lcd_enable(void)
 {
+        viafb_write_reg_mask(CR6B, VIACR, 0x00, BIT3);
+        viafb_write_reg_mask(CR6A, VIACR, 0x08, BIT3);
+        set_lcd_output_path(viaparinfo->lvds_setting_info->iga_path,
+                viaparinfo->chip_info->lvds_chip_info.output_interface);
+        if (viafb_LCD2_ON)
+                set_lcd_output_path(viaparinfo->lvds_setting_info2->iga_path,
+                        viaparinfo->chip_info->
+                        lvds_chip_info2.output_interface);
+
         if (viaparinfo->chip_info->gfx_chip_name == UNICHROME_CLE266) {
                 /* DI1 pad on */
                 viafb_write_reg_mask(SR1E, VIASR, 0x30, 0x30);
@@ -855,39 +820,13 @@ void viafb_lcd_enable(void)
                 viafb_enable_lvds_vt1636(viaparinfo->lvds_setting_info,
                                    &viaparinfo->chip_info->lvds_chip_info);
         } else {
-                /* DFP-HL pad on           */
-                viafb_write_reg_mask(SR2A, VIASR, 0x0F, 0x0F);
                 /* Backlight on            */
                 viafb_write_reg_mask(SR3D, VIASR, 0x20, 0x20);
                 /* 24 bit DI data paht on  */
                 viafb_write_reg_mask(CR91, VIACR, 0x00, 0x80);
-
-                /* Set data source selection bit by iga path */
-                if (viaparinfo->lvds_setting_info->iga_path == IGA1) {
-                        /* DFP-H set to IGA1       */
-                        viafb_write_reg_mask(CR97, VIACR, 0x00, 0x10);
-                        /* DFP-L set to IGA1       */
-                        viafb_write_reg_mask(CR99, VIACR, 0x00, 0x10);
-                } else {
-                        /* DFP-H set to IGA2       */
-                        viafb_write_reg_mask(CR97, VIACR, 0x10, 0x10);
-                        /* DFP-L set to IGA2       */
-                        viafb_write_reg_mask(CR99, VIACR, 0x10, 0x10);
-                }
                 /* LCD enabled             */
                 viafb_write_reg_mask(CR6A, VIACR, 0x48, 0x48);
         }
-
-        if (viaparinfo->lvds_setting_info->iga_path == IGA1) {
-                /* CRT path set to IGA2    */
-                viafb_write_reg_mask(SR16, VIASR, 0x40, 0x40);
-                /* IGA2 path disabled      */
-                viafb_write_reg_mask(CR6A, VIACR, 0x00, 0x80);
-                /* IGA2 path enabled       */
-        } else {                /* IGA2 */
-                viafb_write_reg_mask(CR6A, VIACR, 0x80, 0x80);
-        }
-
 }
 
 static void lcd_powersequence_off(void)
@@ -993,7 +932,7 @@ static void check_diport_of_integrated_lvds(
                   plvds_chip_info->output_interface);
 }
 
-void viafb_init_lvds_output_interface(struct lvds_chip_information
+void __devinit viafb_init_lvds_output_interface(struct lvds_chip_information
                                 *plvds_chip_info,
                                 struct lvds_setting_information
                                 *plvds_setting_info)
@@ -1072,34 +1011,33 @@ static struct display_timing lcd_centering_timging(struct display_timing
 
 bool viafb_lcd_get_mobile_state(bool *mobile)
 {
-        unsigned char *romptr, *tableptr;
+        unsigned char __iomem *romptr, *tableptr, *biosptr;
         u8 core_base;
-        unsigned char *biosptr;
         /* Rom address */
-        u32 romaddr = 0x000C0000;
-        u16 start_pattern = 0;
+        const u32 romaddr = 0x000C0000;
+        u16 start_pattern;
 
         biosptr = ioremap(romaddr, 0x10000);
+        start_pattern = readw(biosptr);
 
-        memcpy(&start_pattern, biosptr, 2);
         /* Compare pattern */
         if (start_pattern == 0xAA55) {
                 /* Get the start of Table */
                 /* 0x1B means BIOS offset position */
                 romptr = biosptr + 0x1B;
-                tableptr = biosptr + *((u16 *) romptr);
+                tableptr = biosptr + readw(romptr);
 
                 /* Get the start of biosver structure */
                 /* 18 means BIOS version position. */
                 romptr = tableptr + 18;
-                romptr = biosptr + *((u16 *) romptr);
+                romptr = biosptr + readw(romptr);
 
                 /* The offset should be 44, but the
                    actual image is less three char. */
                 /* pRom += 44; */
                 romptr += 41;
 
-                core_base = *romptr++;
+                core_base = readb(romptr);
 
                 if (core_base & 0x8)
                         *mobile = false;
diff --git a/drivers/video/via/lcd.h b/drivers/video/via/lcd.h
index b348efc360b8..75f60a655b0e 100644
--- a/drivers/video/via/lcd.h
+++ b/drivers/video/via/lcd.h
@@ -71,15 +71,15 @@ void viafb_enable_lvds_vt1636(struct lvds_setting_information
                         struct lvds_chip_information *plvds_chip_info);
 void viafb_lcd_disable(void);
 void viafb_lcd_enable(void);
-void viafb_init_lcd_size(void);
-void viafb_init_lvds_output_interface(struct lvds_chip_information
+void __devinit viafb_init_lcd_size(void);
+void __devinit viafb_init_lvds_output_interface(struct lvds_chip_information
                                 *plvds_chip_info,
                                 struct lvds_setting_information
                                 *plvds_setting_info);
 void viafb_lcd_set_mode(struct crt_mode_table *mode_crt_table,
                   struct lvds_setting_information *plvds_setting_info,
                   struct lvds_chip_information *plvds_chip_info);
-int viafb_lvds_trasmitter_identify(void);
+bool __devinit viafb_lvds_trasmitter_identify(void);
 void viafb_init_lvds_output_interface(struct lvds_chip_information
                                 *plvds_chip_info,
                                 struct lvds_setting_information
diff --git a/drivers/video/via/lcdtbl.h b/drivers/video/via/lcdtbl.h
deleted file mode 100644
index 6f3dd800be59..000000000000
--- a/drivers/video/via/lcdtbl.h
+++ /dev/null
@@ -1,591 +0,0 @@
-/*
- * Copyright 1998-2008 VIA Technologies, Inc. All Rights Reserved.
- * Copyright 2001-2008 S3 Graphics, Inc. All Rights Reserved.
-
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public
- * License as published by the Free Software Foundation;
- * either version 2, or (at your option) any later version.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTIES OR REPRESENTATIONS; without even
- * the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE.See the GNU General Public License
- * for more details.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-#ifndef __LCDTBL_H__
-#define __LCDTBL_H__
-
-#include "share.h"
-
-/* CLE266 Software Power Sequence */
-/* {Mask}, {Data}, {Delay} */
-int PowerSequenceOn[3][3] =
-    { {0x10, 0x08, 0x06}, {0x10, 0x08, 0x06}, {0x19, 0x1FE, 0x01} };
-int PowerSequenceOff[3][3] =
-    { {0x06, 0x08, 0x10}, {0x00, 0x00, 0x00}, {0xD2, 0x19, 0x01} };
-
-/* ++++++ P880 ++++++ */
-/*   Panel 1600x1200   */
-struct io_reg P880_LCD_RES_6X4_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x73}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x73}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x5A}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x5E},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xD6}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR44, 0xFF, 0x7D}, {VIASR, SR45, 0xFF, 0x8C},
-        {VIASR, SR46, 0xFF, 0x02}
-
-};
-
-#define NUM_TOTAL_P880_LCD_RES_6X4_16X12 ARRAY_SIZE(P880_LCD_RES_6X4_16X12)
-
-struct io_reg P880_LCD_RES_7X4_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x67}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x67}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x74}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x78},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xF5}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR44, 0xFF, 0x78}, {VIASR, SR45, 0xFF, 0x8C},
-        {VIASR, SR46, 0xFF, 0x01}
-
-};
-
-#define NUM_TOTAL_P880_LCD_RES_7X4_16X12 ARRAY_SIZE(P880_LCD_RES_7X4_16X12)
-
-struct io_reg P880_LCD_RES_8X6_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x65}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x65}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x7F}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x83},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xE1}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR44, 0xFF, 0x6D}, {VIASR, SR45, 0xFF, 0x88},
-        {VIASR, SR46, 0xFF, 0x03}
-
-};
-
-#define NUM_TOTAL_P880_LCD_RES_8X6_16X12 ARRAY_SIZE(P880_LCD_RES_8X6_16X12)
-
-struct io_reg P880_LCD_RES_10X7_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x65}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x65}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0xAB}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xAF},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xF0}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR44, 0xFF, 0x92}, {VIASR, SR45, 0xFF, 0x88},
-        {VIASR, SR46, 0xFF, 0x03}
-
-};
-
-#define NUM_TOTAL_P880_LCD_RES_10X7_16X12 ARRAY_SIZE(P880_LCD_RES_10X7_16X12)
-
-struct io_reg P880_LCD_RES_12X10_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x7D}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x7D}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0xD0}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xD4},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xFA}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR44, 0xFF, 0xF6}, {VIASR, SR45, 0xFF, 0x88},
-        {VIASR, SR46, 0xFF, 0x05}
-
-};
-
-#define NUM_TOTAL_P880_LCD_RES_12X10_16X12 ARRAY_SIZE(P880_LCD_RES_12X10_16X12)
-
-/*   Panel 1400x1050   */
-struct io_reg P880_LCD_RES_6X4_14X10[] = {
-        /* 640x480                          */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x5F}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x63},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xB4}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR44, 0xFF, 0xC6}, {VIASR, SR45, 0xFF, 0x8C},
-        {VIASR, SR46, 0xFF, 0x05}
-};
-
-#define NUM_TOTAL_P880_LCD_RES_6X4_14X10 ARRAY_SIZE(P880_LCD_RES_6X4_14X10)
-
-struct io_reg P880_LCD_RES_8X6_14X10[] = {
-        /* 800x600                          */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x7F}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x83},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xBE}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR44, 0xFF, 0x06}, {VIASR, SR45, 0xFF, 0x8D},
-        {VIASR, SR46, 0xFF, 0x05}
-};
-
-#define NUM_TOTAL_P880_LCD_RES_8X6_14X10 ARRAY_SIZE(P880_LCD_RES_8X6_14X10)
-
-/* ++++++ K400 ++++++ */
-/*   Panel 1600x1200   */
-struct io_reg K400_LCD_RES_6X4_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x73}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x73}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x5A}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x5E},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xDA}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0xC4}, {VIASR, SR47, 0xFF, 0x7F}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_6X4_16X12 ARRAY_SIZE(K400_LCD_RES_6X4_16X12)
-
-struct io_reg K400_LCD_RES_7X4_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x67}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x67}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x74}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x78},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xF5}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x46}, {VIASR, SR47, 0xFF, 0x3D}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_7X4_16X12 ARRAY_SIZE(K400_LCD_RES_7X4_16X12)
-
-struct io_reg K400_LCD_RES_8X6_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x65}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x65}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x7F}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x83},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xE1}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x85}, {VIASR, SR47, 0xFF, 0x6F}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_8X6_16X12 ARRAY_SIZE(K400_LCD_RES_8X6_16X12)
-
-struct io_reg K400_LCD_RES_10X7_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x65}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x65}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0xAB}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xAF},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xF0}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x45}, {VIASR, SR47, 0xFF, 0x4A}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_10X7_16X12 ARRAY_SIZE(K400_LCD_RES_10X7_16X12)
-
-struct io_reg K400_LCD_RES_12X10_16X12[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x7D}, {VIACR, CR55, 0x0F, 0x08},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x7D}, {VIACR, CR54, 0x38, 0x00},
-        {VIACR, CR5D, 0x40, 0x40},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0xD0}, {VIACR, CR71, 0x08, 0x00},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xD4},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xFA}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x47}, {VIASR, SR47, 0xFF, 0x7C}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_12X10_16X12 ARRAY_SIZE(K400_LCD_RES_12X10_16X12)
-
-/*   Panel 1400x1050   */
-struct io_reg K400_LCD_RES_6X4_14X10[] = {
-        /* 640x400                          */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x5F}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x63},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xB4}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x07}, {VIASR, SR47, 0xFF, 0x19}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_6X4_14X10 ARRAY_SIZE(K400_LCD_RES_6X4_14X10)
-
-struct io_reg K400_LCD_RES_8X6_14X10[] = {
-        /* 800x600                          */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x7F}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x83},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xBE}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x07}, {VIASR, SR47, 0xFF, 0x21}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_8X6_14X10 ARRAY_SIZE(K400_LCD_RES_8X6_14X10)
-
-struct io_reg K400_LCD_RES_10X7_14X10[] = {
-        /* 1024x768                         */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0xA3}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xA7},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xC3}, {VIACR, CR67, 0x03, 0x04},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x05}, {VIASR, SR47, 0xFF, 0x1E}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_10X7_14X10 ARRAY_SIZE(K400_LCD_RES_10X7_14X10)
-
-struct io_reg K400_LCD_RES_12X10_14X10[] = {
-        /* 1280x768, 1280x960, 1280x1024    */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x97}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x97}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0xCE}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xD2},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xC9}, {VIACR, CR67, 0x03, 0x04},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x84}, {VIASR, SR47, 0xFF, 0x79}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_12X10_14X10 ARRAY_SIZE(K400_LCD_RES_12X10_14X10)
-
-/* ++++++ K400 ++++++ */
-/*   Panel 1366x768   */
-struct io_reg K400_LCD_RES_6X4_1366X7[] = {
-        /* 640x400                          */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x47}, {VIACR, CR55, 0x0F, 0x35},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x47}, {VIACR, CR54, 0x38, 0x2B},
-        {VIACR, CR5D, 0x40, 0x13},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x60}, {VIACR, CR71, 0x08, 0x23},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x64},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0x8C}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x87}, {VIASR, SR47, 0xFF, 0x4C}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_6X4_1366X7 ARRAY_SIZE(K400_LCD_RES_6X4_1366X7)
-
-struct io_reg K400_LCD_RES_7X4_1366X7[] = {
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x3B}, {VIACR, CR55, 0x0F, 0x35},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x3B}, {VIACR, CR54, 0x38, 0x2B},
-        {VIACR, CR5D, 0x40, 0x13},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x71}, {VIACR, CR71, 0x08, 0x23},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x75},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0x96}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x05}, {VIASR, SR47, 0xFF, 0x10}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_7X4_1366X7 ARRAY_SIZE(K400_LCD_RES_7X4_1366X7)
-
-struct io_reg K400_LCD_RES_8X6_1366X7[] = {
-        /* 800x600                          */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x37}, {VIACR, CR55, 0x0F, 0x35},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x37}, {VIACR, CR54, 0x38, 0x2B},
-        {VIACR, CR5D, 0x40, 0x13},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0x7E}, {VIACR, CR71, 0x08, 0x23},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x82},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0x8C}, {VIACR, CR67, 0x03, 0x00},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x84}, {VIASR, SR47, 0xFF, 0xB9}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_8X6_1366X7 ARRAY_SIZE(K400_LCD_RES_8X6_1366X7)
-
-struct io_reg K400_LCD_RES_10X7_1366X7[] = {
-        /* 1024x768                         */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0xA3}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xA7},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xC3}, {VIACR, CR67, 0x03, 0x04},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x05}, {VIASR, SR47, 0xFF, 0x1E}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_10X7_1366X7 ARRAY_SIZE(K400_LCD_RES_10X7_1366X7)
-
-struct io_reg K400_LCD_RES_12X10_1366X7[] = {
-        /* 1280x768, 1280x960, 1280x1024    */
-        /* IGA2 Horizontal Total            */
-        {VIACR, CR50, 0xFF, 0x97}, {VIACR, CR55, 0x0F, 0x56},
-        /* IGA2 Horizontal Blank End        */
-        {VIACR, CR53, 0xFF, 0x97}, {VIACR, CR54, 0x38, 0x75},
-        {VIACR, CR5D, 0x40, 0x24},
-        /* IGA2 Horizontal Total Shadow     */
-        {VIACR, CR6D, 0xFF, 0xCE}, {VIACR, CR71, 0x08, 0x44},
-        /* IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xD2},
-        /* IGA2 Offset                      */
-        {VIACR, CR66, 0xFF, 0xC9}, {VIACR, CR67, 0x03, 0x04},
-        /* VCLK                             */
-        {VIASR, SR46, 0xFF, 0x84}, {VIASR, SR47, 0xFF, 0x79}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_12X10_1366X7\
-                        ARRAY_SIZE(K400_LCD_RES_12X10_1366X7)
-
-/* ++++++ K400 ++++++ */
-/*   Panel 1280x1024   */
-struct io_reg K400_LCD_RES_6X4_12X10[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x46},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x74},
-        {VIACR, CR5D, 0x40, 0x1C},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x5F}, {VIACR, CR71, 0x08, 0x34},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x63},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xAA}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x07}, {VIASR, SR47, 0xFF, 0x19}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_6X4_12X10 ARRAY_SIZE(K400_LCD_RES_6X4_12X10)
-
-struct io_reg K400_LCD_RES_7X4_12X10[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x46},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x74},
-        {VIACR, CR5D, 0x40, 0x1C},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x68}, {VIACR, CR71, 0x08, 0x34},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x6C},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xA8}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x87}, {VIASR, SR47, 0xFF, 0xED}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_7X4_12X10 ARRAY_SIZE(K400_LCD_RES_7X4_12X10)
-
-struct io_reg K400_LCD_RES_8X6_12X10[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x46},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x74},
-        {VIACR, CR5D, 0x40, 0x1C},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x7F}, {VIACR, CR71, 0x08, 0x34},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x83},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xBE}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x07}, {VIASR, SR47, 0xFF, 0x21}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_8X6_12X10 ARRAY_SIZE(K400_LCD_RES_8X6_12X10)
-
-struct io_reg K400_LCD_RES_10X7_12X10[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x9D}, {VIACR, CR55, 0x0F, 0x46},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x9D}, {VIACR, CR54, 0x38, 0x74},
-        {VIACR, CR5D, 0x40, 0x1C},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0xA3}, {VIACR, CR71, 0x08, 0x34},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0xA7},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0xBE}, {VIACR, CR67, 0x03, 0x04},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x05}, {VIASR, SR47, 0xFF, 0x1E}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_10X7_12X10 ARRAY_SIZE(K400_LCD_RES_10X7_12X10)
-
-/* ++++++ K400 ++++++ */
-/*   Panel 1024x768    */
-struct io_reg K400_LCD_RES_6X4_10X7[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x47}, {VIACR, CR55, 0x0F, 0x35},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x47}, {VIACR, CR54, 0x38, 0x2B},
-        {VIACR, CR5D, 0x40, 0x13},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x60}, {VIACR, CR71, 0x08, 0x23},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x64},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0x8C}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x87}, {VIASR, SR47, 0xFF, 0x4C}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_6X4_10X7 ARRAY_SIZE(K400_LCD_RES_6X4_10X7)
-
-struct io_reg K400_LCD_RES_7X4_10X7[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x3B}, {VIACR, CR55, 0x0F, 0x35},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x3B}, {VIACR, CR54, 0x38, 0x2B},
-        {VIACR, CR5D, 0x40, 0x13},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x71}, {VIACR, CR71, 0x08, 0x23},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x75},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0x96}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x05}, {VIASR, SR47, 0xFF, 0x10}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_7X4_10X7 ARRAY_SIZE(K400_LCD_RES_7X4_10X7)
-
-struct io_reg K400_LCD_RES_8X6_10X7[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x37}, {VIACR, CR55, 0x0F, 0x35},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x37}, {VIACR, CR54, 0x38, 0x2B},
-        {VIACR, CR5D, 0x40, 0x13},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x7E}, {VIACR, CR71, 0x08, 0x23},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x82},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0x8C}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x84}, {VIASR, SR47, 0xFF, 0xB9}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_8X6_10X7 ARRAY_SIZE(K400_LCD_RES_8X6_10X7)
-
-/* ++++++ K400 ++++++ */
-/*   Panel 800x600     */
-struct io_reg K400_LCD_RES_6X4_8X6[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x1A}, {VIACR, CR55, 0x0F, 0x34},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x1A}, {VIACR, CR54, 0x38, 0xE3},
-        {VIACR, CR5D, 0x40, 0x12},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x5F}, {VIACR, CR71, 0x08, 0x22},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x63},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0x6E}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0x86}, {VIASR, SR47, 0xFF, 0xB3}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_6X4_8X6 ARRAY_SIZE(K400_LCD_RES_6X4_8X6)
-
-struct io_reg K400_LCD_RES_7X4_8X6[] = {
-        /*IGA2 Horizontal Total */
-        {VIACR, CR50, 0xFF, 0x1F}, {VIACR, CR55, 0x0F, 0x34},
-        /*IGA2 Horizontal Blank End */
-        {VIACR, CR53, 0xFF, 0x1F}, {VIACR, CR54, 0x38, 0xE3},
-        {VIACR, CR5D, 0x40, 0x12},
-        /*IGA2 Horizontal Total Shadow */
-        {VIACR, CR6D, 0xFF, 0x7F}, {VIACR, CR71, 0x08, 0x22},
-        /*IGA2 Horizontal Blank End Shadow */
-        {VIACR, CR6E, 0xFF, 0x83},
-        /*IGA2 Offset */
-        {VIACR, CR66, 0xFF, 0x78}, {VIACR, CR67, 0x03, 0x00},
-         /*VCLK*/ {VIASR, SR46, 0xFF, 0xC4}, {VIASR, SR47, 0xFF, 0x59}
-};
-
-#define NUM_TOTAL_K400_LCD_RES_7X4_8X6 ARRAY_SIZE(K400_LCD_RES_7X4_8X6)
-
-#endif /* __LCDTBL_H__ */
diff --git a/drivers/video/via/share.h b/drivers/video/via/share.h
index 2cbe1031b421..61b0bd596b85 100644
--- a/drivers/video/via/share.h
+++ b/drivers/video/via/share.h
@@ -22,14 +22,6 @@
 #ifndef __SHARE_H__
 #define __SHARE_H__
 
-/* Define Return Value */
-#define FAIL        -1
-#define OK          1
-
-#ifndef NULL
-#define NULL 0
-#endif
-
 /* Define Bit Field */
 #define BIT0    0x01
 #define BIT1    0x02
@@ -290,6 +282,7 @@
 #define HW_LAYOUT_LCD_EXTERNAL_LCD2 0x10
 
 /* Definition Refresh Rate */
+#define REFRESH_49      49
 #define REFRESH_50      50
 #define REFRESH_60      60
 #define REFRESH_75      75
@@ -575,10 +568,6 @@
 #define M1280X720_R50_HSP       NEGATIVE
 #define M1280X720_R50_VSP       POSITIVE
 
-/* 1280x720@60 Sync Polarity  (CEA Mode) */
-#define M1280X720_CEA_R60_HSP       POSITIVE
-#define M1280X720_CEA_R60_VSP       POSITIVE
-
 /* 1440x900@60 Sync Polarity (CVT Mode) */
 #define M1440X900_R60_HSP       NEGATIVE
 #define M1440X900_R60_VSP       POSITIVE
@@ -619,85 +608,10 @@
 #define M1920X1200_RB_R60_HSP  POSITIVE
 #define M1920X1200_RB_R60_VSP  NEGATIVE
 
-/* 1920x1080@60 Sync Polarity  (CEA Mode) */
-#define M1920X1080_CEA_R60_HSP       POSITIVE
-#define M1920X1080_CEA_R60_VSP       POSITIVE
-
 /* 2048x1536@60 Sync Polarity (CVT Mode) */
 #define M2048x1536_R60_HSP      NEGATIVE
 #define M2048x1536_R60_VSP      POSITIVE
 
-/* define PLL index: */
-#define CLK_25_175M     25175000
-#define CLK_26_880M     26880000
-#define CLK_29_581M     29581000
-#define CLK_31_500M     31500000
-#define CLK_31_728M     31728000
-#define CLK_32_668M     32688000
-#define CLK_36_000M     36000000
-#define CLK_40_000M     40000000
-#define CLK_41_291M     41291000
-#define CLK_43_163M     43163000
-#define CLK_45_250M     45250000        /* 45.46MHz */
-#define CLK_46_000M     46000000
-#define CLK_46_996M     46996000
-#define CLK_48_000M     48000000
-#define CLK_48_875M     48875000
-#define CLK_49_500M     49500000
-#define CLK_52_406M     52406000
-#define CLK_52_977M     52977000
-#define CLK_56_250M     56250000
-#define CLK_57_275M     57275000
-#define CLK_60_466M     60466000
-#define CLK_61_500M     61500000
-#define CLK_65_000M     65000000
-#define CLK_65_178M     65178000
-#define CLK_66_750M     66750000        /* 67.116MHz */
-#define CLK_68_179M     68179000
-#define CLK_69_924M     69924000
-#define CLK_70_159M     70159000
-#define CLK_72_000M     72000000
-#define CLK_74_270M     74270000
-#define CLK_78_750M     78750000
-#define CLK_80_136M     80136000
-#define CLK_83_375M     83375000
-#define CLK_83_950M     83950000
-#define CLK_84_750M     84750000        /* 84.537Mhz */
-#define CLK_85_860M     85860000
-#define CLK_88_750M     88750000
-#define CLK_94_500M     94500000
-#define CLK_97_750M     97750000
-#define CLK_101_000M    101000000
-#define CLK_106_500M    106500000
-#define CLK_108_000M    108000000
-#define CLK_113_309M    113309000
-#define CLK_118_840M    118840000
-#define CLK_119_000M    119000000
-#define CLK_121_750M    121750000       /* 121.704MHz */
-#define CLK_125_104M    125104000
-#define CLK_135_000M    135000000
-#define CLK_136_700M    136700000
-#define CLK_138_400M    138400000
-#define CLK_146_760M    146760000
-#define CLK_148_500M    148500000
-
-#define CLK_153_920M    153920000
-#define CLK_156_000M    156000000
-#define CLK_157_500M    157500000
-#define CLK_162_000M    162000000
-#define CLK_187_000M    187000000
-#define CLK_193_295M    193295000
-#define CLK_202_500M    202500000
-#define CLK_204_000M    204000000
-#define CLK_218_500M    218500000
-#define CLK_234_000M    234000000
-#define CLK_267_250M    267250000
-#define CLK_297_500M    297500000
-#define CLK_74_481M     74481000
-#define CLK_172_798M    172798000
-#define CLK_122_614M    122614000
-
-
 /* Definition CRTC Timing Index */
 #define H_TOTAL_INDEX               0
 #define H_ADDR_INDEX                1
@@ -722,76 +636,7 @@
 
 /* Definition Video Mode Pixel Clock (picoseconds)
 */
-#define RES_480X640_60HZ_PIXCLOCK    39722
 #define RES_640X480_60HZ_PIXCLOCK    39722
-#define RES_640X480_75HZ_PIXCLOCK    31747
-#define RES_640X480_85HZ_PIXCLOCK    27777
-#define RES_640X480_100HZ_PIXCLOCK   23168
-#define RES_640X480_120HZ_PIXCLOCK   19081
-#define RES_720X480_60HZ_PIXCLOCK    37020
-#define RES_720X576_60HZ_PIXCLOCK    30611
-#define RES_800X600_60HZ_PIXCLOCK    25000
-#define RES_800X600_75HZ_PIXCLOCK    20203
-#define RES_800X600_85HZ_PIXCLOCK    17777
-#define RES_800X600_100HZ_PIXCLOCK   14667
-#define RES_800X600_120HZ_PIXCLOCK   11912
-#define RES_800X480_60HZ_PIXCLOCK    33805
-#define RES_848X480_60HZ_PIXCLOCK    31756
-#define RES_856X480_60HZ_PIXCLOCK    31518
-#define RES_1024X512_60HZ_PIXCLOCK   24218
-#define RES_1024X600_60HZ_PIXCLOCK   20460
-#define RES_1024X768_60HZ_PIXCLOCK   15385
-#define RES_1024X768_75HZ_PIXCLOCK   12699
-#define RES_1024X768_85HZ_PIXCLOCK   10582
-#define RES_1024X768_100HZ_PIXCLOCK  8825
-#define RES_1152X864_75HZ_PIXCLOCK   9259
-#define RES_1280X768_60HZ_PIXCLOCK   12480
-#define RES_1280X800_60HZ_PIXCLOCK   11994
-#define RES_1280X960_60HZ_PIXCLOCK   9259
-#define RES_1280X1024_60HZ_PIXCLOCK  9260
-#define RES_1280X1024_75HZ_PIXCLOCK  7408
-#define RES_1280X768_85HZ_PIXCLOCK   6349
-#define RES_1440X1050_60HZ_PIXCLOCK  7993
-#define RES_1600X1200_60HZ_PIXCLOCK  6172
-#define RES_1600X1200_75HZ_PIXCLOCK  4938
-#define RES_1280X720_60HZ_PIXCLOCK   13426
-#define RES_1200X900_60HZ_PIXCLOCK   17459
-#define RES_1920X1080_60HZ_PIXCLOCK  5787
-#define RES_1400X1050_60HZ_PIXCLOCK  8214
-#define RES_1400X1050_75HZ_PIXCLOCK  6410
-#define RES_1368X768_60HZ_PIXCLOCK   11647
-#define RES_960X600_60HZ_PIXCLOCK      22099
-#define RES_1000X600_60HZ_PIXCLOCK    20834
-#define RES_1024X576_60HZ_PIXCLOCK    21278
-#define RES_1088X612_60HZ_PIXCLOCK    18877
-#define RES_1152X720_60HZ_PIXCLOCK    14981
-#define RES_1200X720_60HZ_PIXCLOCK    14253
-#define RES_1280X600_60HZ_PIXCLOCK    16260
-#define RES_1280X720_50HZ_PIXCLOCK    16538
-#define RES_1280X768_50HZ_PIXCLOCK    15342
-#define RES_1366X768_50HZ_PIXCLOCK    14301
-#define RES_1366X768_60HZ_PIXCLOCK    11646
-#define RES_1360X768_60HZ_PIXCLOCK    11799
-#define RES_1440X900_60HZ_PIXCLOCK    9390
-#define RES_1440X900_75HZ_PIXCLOCK    7315
-#define RES_1600X900_60HZ_PIXCLOCK    8415
-#define RES_1600X1024_60HZ_PIXCLOCK   7315
-#define RES_1680X1050_60HZ_PIXCLOCK   6814
-#define RES_1680X1050_75HZ_PIXCLOCK   5348
-#define RES_1792X1344_60HZ_PIXCLOCK   4902
-#define RES_1856X1392_60HZ_PIXCLOCK   4577
-#define RES_1920X1200_60HZ_PIXCLOCK   5173
-#define RES_1920X1440_60HZ_PIXCLOCK   4274
-#define RES_1920X1440_75HZ_PIXCLOCK   3367
-#define RES_2048X1536_60HZ_PIXCLOCK   3742
-
-#define RES_1360X768_RB_60HZ_PIXCLOCK 13889
-#define RES_1400X1050_RB_60HZ_PIXCLOCK 9901
-#define RES_1440X900_RB_60HZ_PIXCLOCK   11268
-#define RES_1600X900_RB_60HZ_PIXCLOCK   10230
-#define RES_1680X1050_RB_60HZ_PIXCLOCK 8403
-#define RES_1920X1080_RB_60HZ_PIXCLOCK 7225
-#define RES_1920X1200_RB_60HZ_PIXCLOCK 6497
 
 /* LCD display method
 */
@@ -822,7 +667,6 @@ struct display_timing {
 
 struct crt_mode_table {
         int refresh_rate;
-        unsigned long clk;
         int h_sync_polarity;
         int v_sync_polarity;
         struct display_timing crtc;
diff --git a/drivers/video/via/tbl1636.c b/drivers/video/via/tbl1636.c
deleted file mode 100644
index 2d8453429d4a..000000000000
--- a/drivers/video/via/tbl1636.c
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * Copyright 1998-2008 VIA Technologies, Inc. All Rights Reserved.
- * Copyright 2001-2008 S3 Graphics, Inc. All Rights Reserved.
-
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public
- * License as published by the Free Software Foundation;
- * either version 2, or (at your option) any later version.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTIES OR REPRESENTATIONS; without even
- * the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE.See the GNU General Public License
- * for more details.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#include "global.h"
-struct IODATA COMMON_INIT_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        /* Set panel power sequence timing */
-        {0x10, 0xC0, 0x00},
-        /* T1: VDD on - Data on. Each increment is 1 ms. (50ms = 031h) */
-        {0x0B, 0xFF, 0x40},
-        /* T2: Data on - Backlight on. Each increment is 2 ms. (210ms = 068h) */
-        {0x0C, 0xFF, 0x31},
-        /* T3: Backlight off -Data off. Each increment is 2 ms. (210ms = 068h)*/
-        {0x0D, 0xFF, 0x31},
-        /* T4: Data off - VDD off. Each increment is 1 ms. (50ms = 031h) */
-        {0x0E, 0xFF, 0x68},
-        /* T5: VDD off - VDD on. Each increment is 100 ms. (500ms = 04h) */
-        {0x0F, 0xFF, 0x68},
-        /* LVDS output power up */
-        {0x09, 0xA0, 0xA0},
-        /* turn on back light */
-        {0x10, 0x33, 0x13}
-};
-
-struct IODATA DUAL_CHANNEL_ENABLE_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        {0x08, 0xF0, 0xE0}      /* Input Data Mode Select */
-};
-
-struct IODATA SINGLE_CHANNEL_ENABLE_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        {0x08, 0xF0, 0x00}      /* Input Data Mode Select */
-};
-
-struct IODATA DITHERING_ENABLE_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        {0x0A, 0x70, 0x50}
-};
-
-struct IODATA DITHERING_DISABLE_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        {0x0A, 0x70, 0x00}
-};
-
-struct IODATA VDD_ON_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        {0x10, 0x20, 0x20}
-};
-
-struct IODATA VDD_OFF_TBL_VT1636[] = {
-/*  Index, Mask, Value */
-        {0x10, 0x20, 0x00}
-};
diff --git a/drivers/video/via/tbl1636.h b/drivers/video/via/tbl1636.h
deleted file mode 100644
index d906055f1511..000000000000
--- a/drivers/video/via/tbl1636.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Copyright 1998-2008 VIA Technologies, Inc. All Rights Reserved.
- * Copyright 2001-2008 S3 Graphics, Inc. All Rights Reserved.
-
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public
- * License as published by the Free Software Foundation;
- * either version 2, or (at your option) any later version.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTIES OR REPRESENTATIONS; without even
- * the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE.See the GNU General Public License
- * for more details.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#ifndef _TBL1636_H_
-#define _TBL1636_H_
-#include "hw.h"
-
-extern struct IODATA COMMON_INIT_TBL_VT1636[8];
-extern struct IODATA DUAL_CHANNEL_ENABLE_TBL_VT1636[1];
-extern struct IODATA SINGLE_CHANNEL_ENABLE_TBL_VT1636[1];
-extern struct IODATA DITHERING_ENABLE_TBL_VT1636[1];
-extern struct IODATA DITHERING_DISABLE_TBL_VT1636[1];
-extern struct IODATA VDD_ON_TBL_VT1636[1];
-extern struct IODATA VDD_OFF_TBL_VT1636[1];
-
-#endif /* _VIA_TBL1636_H_ */
diff --git a/drivers/video/via/tblDPASetting.c b/drivers/video/via/tblDPASetting.c
index 0c4c8cc712f4..73bb554e7c1e 100644
--- a/drivers/video/via/tblDPASetting.c
+++ b/drivers/video/via/tblDPASetting.c
@@ -20,17 +20,6 @@
  */
 
 #include "global.h"
-/* For VT3324: */
-struct VT1636_DPA_SETTING VT1636_DPA_SETTING_TBL_VT3324[] = {
-        /* Panel ID,       CLK_SEL_ST1[09],    CLK_SEL_ST2[08] */
-        {LCD_PANEL_ID0_640X480, 0x00, 0x00},    /* For 640x480   */
-        {LCD_PANEL_ID1_800X600, 0x00, 0x00},    /* For 800x600   */
-        {LCD_PANEL_ID2_1024X768, 0x00, 0x00},   /* For 1024x768  */
-        {LCD_PANEL_ID3_1280X768, 0x00, 0x00},   /* For 1280x768  */
-        {LCD_PANEL_ID4_1280X1024, 0x00, 0x00},  /* For 1280x1024 */
-        {LCD_PANEL_ID5_1400X1050, 0x00, 0x00},  /* For 1400x1050 */
-        {LCD_PANEL_ID6_1600X1200, 0x0B, 0x03}   /* For 1600x1200 */
-};
 
 struct GFX_DPA_SETTING GFX_DPA_SETTING_TBL_VT3324[] = {
 /*  ClkRange, DVP0, DVP0DataDriving,  DVP0ClockDriving, DVP1,
@@ -57,18 +46,6 @@ struct GFX_DPA_SETTING GFX_DPA_SETTING_TBL_VT3324[] = {
          0x00},
 };
 
-/* For VT3327: */
-struct VT1636_DPA_SETTING VT1636_DPA_SETTING_TBL_VT3327[] = {
-        /* Panel ID,       CLK_SEL_ST1[09],    CLK_SEL_ST2[08] */
-        {LCD_PANEL_ID0_640X480, 0x00, 0x00},    /* For 640x480   */
-        {LCD_PANEL_ID1_800X600, 0x00, 0x00},    /* For 800x600   */
-        {LCD_PANEL_ID2_1024X768, 0x00, 0x00},   /* For 1024x768  */
-        {LCD_PANEL_ID3_1280X768, 0x00, 0x00},   /* For 1280x768  */
-        {LCD_PANEL_ID4_1280X1024, 0x00, 0x00},  /* For 1280x1024 */
-        {LCD_PANEL_ID5_1400X1050, 0x00, 0x00},  /* For 1400x1050 */
-        {LCD_PANEL_ID6_1600X1200, 0x00, 0x00}   /* For 1600x1200 */
-};
-
 struct GFX_DPA_SETTING GFX_DPA_SETTING_TBL_VT3327[] = {
 /*  ClkRange,DVP0, DVP0DataDriving,  DVP0ClockDriving, DVP1,
                                         DVP1Driving, DFPHigh,   DFPLow */
diff --git a/drivers/video/via/tblDPASetting.h b/drivers/video/via/tblDPASetting.h
index b065a83481d3..6db61519cb5d 100644
--- a/drivers/video/via/tblDPASetting.h
+++ b/drivers/video/via/tblDPASetting.h
@@ -38,9 +38,7 @@ enum DPA_RANGE {
         DPA_CLK_RANGE_150M
 };
 
-extern struct VT1636_DPA_SETTING VT1636_DPA_SETTING_TBL_VT3324[7];
 extern struct GFX_DPA_SETTING GFX_DPA_SETTING_TBL_VT3324[6];
-extern struct VT1636_DPA_SETTING VT1636_DPA_SETTING_TBL_VT3327[7];
 extern struct GFX_DPA_SETTING GFX_DPA_SETTING_TBL_VT3327[];
 extern struct GFX_DPA_SETTING GFX_DPA_SETTING_TBL_VT3364[6];
 
diff --git a/drivers/video/via/via-core.c b/drivers/video/via/via-core.c
index 66f403033111..eb112b621735 100644
--- a/drivers/video/via/via-core.c
+++ b/drivers/video/via/via-core.c
@@ -15,12 +15,15 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/list.h>
+#include <linux/pm.h>
+#include <asm/olpc.h>
 
 /*
  * The default port config.
  */
 static struct via_port_cfg adap_configs[] = {
-        [VIA_PORT_26]   = { VIA_PORT_I2C,  VIA_MODE_OFF, VIASR, 0x26 },
+        [VIA_PORT_26]   = { VIA_PORT_I2C,  VIA_MODE_I2C, VIASR, 0x26 },
         [VIA_PORT_31]   = { VIA_PORT_I2C,  VIA_MODE_I2C, VIASR, 0x31 },
         [VIA_PORT_25]   = { VIA_PORT_GPIO, VIA_MODE_GPIO, VIASR, 0x25 },
         [VIA_PORT_2C]   = { VIA_PORT_GPIO, VIA_MODE_I2C, VIASR, 0x2c },
@@ -29,6 +32,19 @@ static struct via_port_cfg adap_configs[] = {
 };
 
 /*
+ * The OLPC XO-1.5 puts the camera power and reset lines onto
+ * GPIO 2C.
+ */
+static const struct via_port_cfg olpc_adap_configs[] = {
+        [VIA_PORT_26]   = { VIA_PORT_I2C,  VIA_MODE_I2C, VIASR, 0x26 },
+        [VIA_PORT_31]   = { VIA_PORT_I2C,  VIA_MODE_I2C, VIASR, 0x31 },
+        [VIA_PORT_25]   = { VIA_PORT_GPIO, VIA_MODE_GPIO, VIASR, 0x25 },
+        [VIA_PORT_2C]   = { VIA_PORT_GPIO, VIA_MODE_GPIO, VIASR, 0x2c },
+        [VIA_PORT_3D]   = { VIA_PORT_GPIO, VIA_MODE_GPIO, VIASR, 0x3d },
+        { 0, 0, 0, 0 }
+};
+
+/*
  * We currently only support one viafb device (will there ever be
  * more than one?), so just declare it globally here.
  */
@@ -95,6 +111,13 @@ EXPORT_SYMBOL_GPL(viafb_irq_disable);
 
 /* ---------------------------------------------------------------------- */
 /*
+ * Currently, the camera driver is the only user of the DMA code, so we
+ * only compile it in if the camera driver is being built.  Chances are,
+ * most viafb systems will not need to have this extra code for a while.
+ * As soon as another user comes long, the ifdef can be removed.
+ */
+#if defined(CONFIG_VIDEO_VIA_CAMERA) || defined(CONFIG_VIDEO_VIA_CAMERA_MODULE)
+/*
  * Access to the DMA engine.  This currently provides what the camera
  * driver needs (i.e. outgoing only) but is easily expandable if need
  * be.
@@ -322,7 +345,7 @@ int viafb_dma_copy_out_sg(unsigned int offset, struct scatterlist *sg, int nsg)
         return 0;
 }
 EXPORT_SYMBOL_GPL(viafb_dma_copy_out_sg);
-
+#endif /* CONFIG_VIDEO_VIA_CAMERA */
 
 /* ---------------------------------------------------------------------- */
 /*
@@ -333,7 +356,7 @@ EXPORT_SYMBOL_GPL(viafb_dma_copy_out_sg);
 static u16 via_function3[] = {
         CLE266_FUNCTION3, KM400_FUNCTION3, CN400_FUNCTION3, CN700_FUNCTION3,
         CX700_FUNCTION3, KM800_FUNCTION3, KM890_FUNCTION3, P4M890_FUNCTION3,
-        P4M900_FUNCTION3, VX800_FUNCTION3, VX855_FUNCTION3,
+        P4M900_FUNCTION3, VX800_FUNCTION3, VX855_FUNCTION3, VX900_FUNCTION3,
 };
 
 /* Get the BIOS-configured framebuffer size from PCI configuration space
@@ -370,6 +393,7 @@ static int viafb_get_fb_size_from_pci(int chip_type)
                 case P4M900_FUNCTION3:
                 case VX800_FUNCTION3:
                 case VX855_FUNCTION3:
+                case VX900_FUNCTION3:
                 /*case CN750_FUNCTION3: */
                         offset = 0xA0;
                         break;
@@ -474,11 +498,21 @@ static int __devinit via_pci_setup_mmio(struct viafb_dev *vdev)
          * Eventually we want to move away from mapping this
          * entire region.
          */
-        vdev->fbmem_start = pci_resource_start(vdev->pdev, 0);
+        if (vdev->chip_type == UNICHROME_VX900)
+                vdev->fbmem_start = pci_resource_start(vdev->pdev, 2);
+        else
+                vdev->fbmem_start = pci_resource_start(vdev->pdev, 0);
         ret = vdev->fbmem_len = viafb_get_fb_size_from_pci(vdev->chip_type);
         if (ret < 0)
                 goto out_unmap;
-        vdev->fbmem = ioremap_nocache(vdev->fbmem_start, vdev->fbmem_len);
+
+        /* try to map less memory on failure, 8 MB should be still enough */
+        for (; vdev->fbmem_len >= 8 << 20; vdev->fbmem_len /= 2) {
+                vdev->fbmem = ioremap_wc(vdev->fbmem_start, vdev->fbmem_len);
+                if (vdev->fbmem)
+                        break;
+        }
+
         if (vdev->fbmem == NULL) {
                 ret = -ENOMEM;
                 goto out_unmap;
@@ -507,7 +541,12 @@ static struct viafb_subdev_info {
         },
         {
                 .name = "viafb-i2c",
-        }
+        },
+#if defined(CONFIG_VIDEO_VIA_CAMERA) || defined(CONFIG_VIDEO_VIA_CAMERA_MODULE)
+        {
+                .name = "viafb-camera",
+        },
+#endif
 };
 #define N_SUBDEVS ARRAY_SIZE(viafb_subdevs)
 
@@ -559,6 +598,78 @@ static void via_teardown_subdevs(void)
                 }
 }
 
+/*
+ * Power management functions
+ */
+#ifdef CONFIG_PM
+static LIST_HEAD(viafb_pm_hooks);
+static DEFINE_MUTEX(viafb_pm_hooks_lock);
+
+void viafb_pm_register(struct viafb_pm_hooks *hooks)
+{
+        INIT_LIST_HEAD(&hooks->list);
+
+        mutex_lock(&viafb_pm_hooks_lock);
+        list_add_tail(&hooks->list, &viafb_pm_hooks);
+        mutex_unlock(&viafb_pm_hooks_lock);
+}
+EXPORT_SYMBOL_GPL(viafb_pm_register);
+
+void viafb_pm_unregister(struct viafb_pm_hooks *hooks)
+{
+        mutex_lock(&viafb_pm_hooks_lock);
+        list_del(&hooks->list);
+        mutex_unlock(&viafb_pm_hooks_lock);
+}
+EXPORT_SYMBOL_GPL(viafb_pm_unregister);
+
+static int via_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+        struct viafb_pm_hooks *hooks;
+
+        if (state.event != PM_EVENT_SUSPEND)
+                return 0;
+        /*
+         * "I've occasionally hit a few drivers that caused suspend
+         * failures, and each and every time it was a driver bug, and
+         * the right thing to do was to just ignore the error and suspend
+         * anyway - returning an error code and trying to undo the suspend
+         * is not what anybody ever really wants, even if our model
+         *_allows_ for it."
+         * -- Linus Torvalds, Dec. 7, 2009
+         */
+        mutex_lock(&viafb_pm_hooks_lock);
+        list_for_each_entry_reverse(hooks, &viafb_pm_hooks, list)
+                hooks->suspend(hooks->private);
+        mutex_unlock(&viafb_pm_hooks_lock);
+
+        pci_save_state(pdev);
+        pci_disable_device(pdev);
+        pci_set_power_state(pdev, pci_choose_state(pdev, state));
+        return 0;
+}
+
+static int via_resume(struct pci_dev *pdev)
+{
+        struct viafb_pm_hooks *hooks;
+
+        /* Get the bus side powered up */
+        pci_set_power_state(pdev, PCI_D0);
+        pci_restore_state(pdev);
+        if (pci_enable_device(pdev))
+                return 0;
+
+        pci_set_master(pdev);
+
+        /* Now bring back any subdevs */
+        mutex_lock(&viafb_pm_hooks_lock);
+        list_for_each_entry(hooks, &viafb_pm_hooks, list)
+                hooks->resume(hooks->private);
+        mutex_unlock(&viafb_pm_hooks_lock);
+
+        return 0;
+}
+#endif /* CONFIG_PM */
 
 static int __devinit via_pci_probe(struct pci_dev *pdev,
                 const struct pci_device_id *ent)
@@ -568,6 +679,7 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
         ret = pci_enable_device(pdev);
         if (ret)
                 return ret;
+
         /*
          * Global device initialization.
          */
@@ -575,6 +687,9 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
         global_dev.pdev = pdev;
         global_dev.chip_type = ent->driver_data;
         global_dev.port_cfg = adap_configs;
+        if (machine_is_olpc())
+                global_dev.port_cfg = olpc_adap_configs;
+
         spin_lock_init(&global_dev.reg_lock);
         ret = via_pci_setup_mmio(&global_dev);
         if (ret)
@@ -635,6 +750,8 @@ static struct pci_device_id via_pci_table[] __devinitdata = {
           .driver_data = UNICHROME_VX800 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_VX855_DID),
           .driver_data = UNICHROME_VX855 },
+        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_VX900_DID),
+          .driver_data = UNICHROME_VX900 },
         { }
 };
 MODULE_DEVICE_TABLE(pci, via_pci_table);
@@ -644,6 +761,10 @@ static struct pci_driver via_driver = {
         .id_table       = via_pci_table,
         .probe          = via_pci_probe,
         .remove         = __devexit_p(via_pci_remove),
+#ifdef CONFIG_PM
+        .suspend        = via_suspend,
+        .resume         = via_resume,
+#endif
 };
 
 static int __init via_core_init(void)
diff --git a/drivers/video/via/via-gpio.c b/drivers/video/via/via-gpio.c
index 39acb37e7a1d..ab5341814c74 100644
--- a/drivers/video/via/via-gpio.c
+++ b/drivers/video/via/via-gpio.c
@@ -145,7 +145,7 @@ static int via_gpio_get(struct gpio_chip *chip, unsigned int nr)
 }
 
 
-static struct viafb_gpio_cfg gpio_config = {
+static struct viafb_gpio_cfg viafb_gpio_config = {
         .gpio_chip = {
                 .label = "VIAFB onboard GPIO",
                 .owner = THIS_MODULE,
@@ -172,6 +172,28 @@ static void viafb_gpio_disable(struct viafb_gpio *gpio)
         via_write_reg_mask(VIASR, gpio->vg_port_index, 0, 0x02);
 }
 
+#ifdef CONFIG_PM
+
+static int viafb_gpio_suspend(void *private)
+{
+        return 0;
+}
+
+static int viafb_gpio_resume(void *private)
+{
+        int i;
+
+        for (i = 0; i < viafb_gpio_config.gpio_chip.ngpio; i += 2)
+                viafb_gpio_enable(viafb_gpio_config.active_gpios[i]);
+        return 0;
+}
+
+static struct viafb_pm_hooks viafb_gpio_pm_hooks = {
+        .suspend = viafb_gpio_suspend,
+        .resume = viafb_gpio_resume
+};
+#endif /* CONFIG_PM */
+
 /*
  * Look up a specific gpio and return the number it was assigned.
  */
@@ -179,9 +201,9 @@ int viafb_gpio_lookup(const char *name)
 {
         int i;
 
-        for (i = 0; i < gpio_config.gpio_chip.ngpio; i++)
-                if (!strcmp(name, gpio_config.active_gpios[i]->vg_name))
-                        return gpio_config.gpio_chip.base + i;
+        for (i = 0; i < viafb_gpio_config.gpio_chip.ngpio; i++)
+                if (!strcmp(name, viafb_gpio_config.active_gpios[i]->vg_name))
+                        return viafb_gpio_config.gpio_chip.base + i;
         return -1;
 }
 EXPORT_SYMBOL_GPL(viafb_gpio_lookup);
@@ -207,14 +229,15 @@ static __devinit int viafb_gpio_probe(struct platform_device *platdev)
                 for (gpio = viafb_all_gpios;
                      gpio < viafb_all_gpios + VIAFB_NUM_GPIOS; gpio++)
                         if (gpio->vg_port_index == port_cfg[i].ioport_index) {
-                                gpio_config.active_gpios[ngpio] = gpio;
-                                gpio_config.gpio_names[ngpio] = gpio->vg_name;
+                                viafb_gpio_config.active_gpios[ngpio] = gpio;
+                                viafb_gpio_config.gpio_names[ngpio] =
+                                        gpio->vg_name;
                                 ngpio++;
                         }
         }
-        gpio_config.gpio_chip.ngpio = ngpio;
-        gpio_config.gpio_chip.names = gpio_config.gpio_names;
-        gpio_config.vdev = vdev;
+        viafb_gpio_config.gpio_chip.ngpio = ngpio;
+        viafb_gpio_config.gpio_chip.names = viafb_gpio_config.gpio_names;
+        viafb_gpio_config.vdev = vdev;
         if (ngpio == 0) {
                 printk(KERN_INFO "viafb: no GPIOs configured\n");
                 return 0;
@@ -223,19 +246,22 @@ static __devinit int viafb_gpio_probe(struct platform_device *platdev)
          * Enable the ports.  They come in pairs, with a single
          * enable bit for both.
          */
-        spin_lock_irqsave(&gpio_config.vdev->reg_lock, flags);
+        spin_lock_irqsave(&viafb_gpio_config.vdev->reg_lock, flags);
         for (i = 0; i < ngpio; i += 2)
-                viafb_gpio_enable(gpio_config.active_gpios[i]);
-        spin_unlock_irqrestore(&gpio_config.vdev->reg_lock, flags);
+                viafb_gpio_enable(viafb_gpio_config.active_gpios[i]);
+        spin_unlock_irqrestore(&viafb_gpio_config.vdev->reg_lock, flags);
         /*
          * Get registered.
          */
-        gpio_config.gpio_chip.base = -1;  /* Dynamic */
-        ret = gpiochip_add(&gpio_config.gpio_chip);
+        viafb_gpio_config.gpio_chip.base = -1;  /* Dynamic */
+        ret = gpiochip_add(&viafb_gpio_config.gpio_chip);
         if (ret) {
                 printk(KERN_ERR "viafb: failed to add gpios (%d)\n", ret);
-                gpio_config.gpio_chip.ngpio = 0;
+                viafb_gpio_config.gpio_chip.ngpio = 0;
         }
+#ifdef CONFIG_PM
+        viafb_pm_register(&viafb_gpio_pm_hooks);
+#endif
         return ret;
 }
 
@@ -245,11 +271,15 @@ static int viafb_gpio_remove(struct platform_device *platdev)
         unsigned long flags;
         int ret = 0, i;
 
+#ifdef CONFIG_PM
+        viafb_pm_unregister(&viafb_gpio_pm_hooks);
+#endif
+
         /*
          * Get unregistered.
          */
-        if (gpio_config.gpio_chip.ngpio > 0) {
-                ret = gpiochip_remove(&gpio_config.gpio_chip);
+        if (viafb_gpio_config.gpio_chip.ngpio > 0) {
+                ret = gpiochip_remove(&viafb_gpio_config.gpio_chip);
                 if (ret) { /* Somebody still using it? */
                         printk(KERN_ERR "Viafb: GPIO remove failed\n");
                         return ret;
@@ -258,11 +288,11 @@ static int viafb_gpio_remove(struct platform_device *platdev)
         /*
          * Disable the ports.
          */
-        spin_lock_irqsave(&gpio_config.vdev->reg_lock, flags);
-        for (i = 0; i < gpio_config.gpio_chip.ngpio; i += 2)
-                viafb_gpio_disable(gpio_config.active_gpios[i]);
-        gpio_config.gpio_chip.ngpio = 0;
-        spin_unlock_irqrestore(&gpio_config.vdev->reg_lock, flags);
+        spin_lock_irqsave(&viafb_gpio_config.vdev->reg_lock, flags);
+        for (i = 0; i < viafb_gpio_config.gpio_chip.ngpio; i += 2)
+                viafb_gpio_disable(viafb_gpio_config.active_gpios[i]);
+        viafb_gpio_config.gpio_chip.ngpio = 0;
+        spin_unlock_irqrestore(&viafb_gpio_config.vdev->reg_lock, flags);
         return ret;
 }
 
diff --git a/drivers/video/via/via_clock.c b/drivers/video/via/via_clock.c
new file mode 100644
index 000000000000..af8f26b643c1
--- /dev/null
+++ b/drivers/video/via/via_clock.c
@@ -0,0 +1,349 @@
+/*
+ * Copyright 1998-2008 VIA Technologies, Inc. All Rights Reserved.
+ * Copyright 2001-2008 S3 Graphics, Inc. All Rights Reserved.
+ * Copyright 2011 Florian Tobias Schandinat <FlorianSchandinat@gmx.de>
+ *
+ * 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, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTIES OR REPRESENTATIONS; without even
+ * the implied warranty of MERCHANTABILITY or FITNESS FOR
+ * A PARTICULAR PURPOSE.See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+/*
+ * clock and PLL management functions
+ */
+
+#include <linux/kernel.h>
+#include <linux/via-core.h>
+#include "via_clock.h"
+#include "global.h"
+#include "debug.h"
+
+const char *via_slap = "Please slap VIA Technologies to motivate them "
+        "releasing full documentation for your platform!\n";
+
+static inline u32 cle266_encode_pll(struct via_pll_config pll)
+{
+        return (pll.multiplier << 8)
+                | (pll.rshift << 6)
+                | pll.divisor;
+}
+
+static inline u32 k800_encode_pll(struct via_pll_config pll)
+{
+        return ((pll.divisor - 2) << 16)
+                | (pll.rshift << 10)
+                | (pll.multiplier - 2);
+}
+
+static inline u32 vx855_encode_pll(struct via_pll_config pll)
+{
+        return (pll.divisor << 16)
+                | (pll.rshift << 10)
+                | pll.multiplier;
+}
+
+static inline void cle266_set_primary_pll_encoded(u32 data)
+{
+        via_write_reg_mask(VIASR, 0x40, 0x02, 0x02); /* enable reset */
+        via_write_reg(VIASR, 0x46, data & 0xFF);
+        via_write_reg(VIASR, 0x47, (data >> 8) & 0xFF);
+        via_write_reg_mask(VIASR, 0x40, 0x00, 0x02); /* disable reset */
+}
+
+static inline void k800_set_primary_pll_encoded(u32 data)
+{
+        via_write_reg_mask(VIASR, 0x40, 0x02, 0x02); /* enable reset */
+        via_write_reg(VIASR, 0x44, data & 0xFF);
+        via_write_reg(VIASR, 0x45, (data >> 8) & 0xFF);
+        via_write_reg(VIASR, 0x46, (data >> 16) & 0xFF);
+        via_write_reg_mask(VIASR, 0x40, 0x00, 0x02); /* disable reset */
+}
+
+static inline void cle266_set_secondary_pll_encoded(u32 data)
+{
+        via_write_reg_mask(VIASR, 0x40, 0x04, 0x04); /* enable reset */
+        via_write_reg(VIASR, 0x44, data & 0xFF);
+        via_write_reg(VIASR, 0x45, (data >> 8) & 0xFF);
+        via_write_reg_mask(VIASR, 0x40, 0x00, 0x04); /* disable reset */
+}
+
+static inline void k800_set_secondary_pll_encoded(u32 data)
+{
+        via_write_reg_mask(VIASR, 0x40, 0x04, 0x04); /* enable reset */
+        via_write_reg(VIASR, 0x4A, data & 0xFF);
+        via_write_reg(VIASR, 0x4B, (data >> 8) & 0xFF);
+        via_write_reg(VIASR, 0x4C, (data >> 16) & 0xFF);
+        via_write_reg_mask(VIASR, 0x40, 0x00, 0x04); /* disable reset */
+}
+
+static inline void set_engine_pll_encoded(u32 data)
+{
+        via_write_reg_mask(VIASR, 0x40, 0x01, 0x01); /* enable reset */
+        via_write_reg(VIASR, 0x47, data & 0xFF);
+        via_write_reg(VIASR, 0x48, (data >> 8) & 0xFF);
+        via_write_reg(VIASR, 0x49, (data >> 16) & 0xFF);
+        via_write_reg_mask(VIASR, 0x40, 0x00, 0x01); /* disable reset */
+}
+
+static void cle266_set_primary_pll(struct via_pll_config config)
+{
+        cle266_set_primary_pll_encoded(cle266_encode_pll(config));
+}
+
+static void k800_set_primary_pll(struct via_pll_config config)
+{
+        k800_set_primary_pll_encoded(k800_encode_pll(config));
+}
+
+static void vx855_set_primary_pll(struct via_pll_config config)
+{
+        k800_set_primary_pll_encoded(vx855_encode_pll(config));
+}
+
+static void cle266_set_secondary_pll(struct via_pll_config config)
+{
+        cle266_set_secondary_pll_encoded(cle266_encode_pll(config));
+}
+
+static void k800_set_secondary_pll(struct via_pll_config config)
+{
+        k800_set_secondary_pll_encoded(k800_encode_pll(config));
+}
+
+static void vx855_set_secondary_pll(struct via_pll_config config)
+{
+        k800_set_secondary_pll_encoded(vx855_encode_pll(config));
+}
+
+static void k800_set_engine_pll(struct via_pll_config config)
+{
+        set_engine_pll_encoded(k800_encode_pll(config));
+}
+
+static void vx855_set_engine_pll(struct via_pll_config config)
+{
+        set_engine_pll_encoded(vx855_encode_pll(config));
+}
+
+static void set_primary_pll_state(u8 state)
+{
+        u8 value;
+
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x20;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x00;
+                break;
+        default:
+                return;
+        }
+
+        via_write_reg_mask(VIASR, 0x2D, value, 0x30);
+}
+
+static void set_secondary_pll_state(u8 state)
+{
+        u8 value;
+
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x08;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x00;
+                break;
+        default:
+                return;
+        }
+
+        via_write_reg_mask(VIASR, 0x2D, value, 0x0C);
+}
+
+static void set_engine_pll_state(u8 state)
+{
+        u8 value;
+
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x02;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x00;
+                break;
+        default:
+                return;
+        }
+
+        via_write_reg_mask(VIASR, 0x2D, value, 0x03);
+}
+
+static void set_primary_clock_state(u8 state)
+{
+        u8 value;
+
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x20;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x00;
+                break;
+        default:
+                return;
+        }
+
+        via_write_reg_mask(VIASR, 0x1B, value, 0x30);
+}
+
+static void set_secondary_clock_state(u8 state)
+{
+        u8 value;
+
+        switch (state) {
+        case VIA_STATE_ON:
+                value = 0x80;
+                break;
+        case VIA_STATE_OFF:
+                value = 0x00;
+                break;
+        default:
+                return;
+        }
+
+        via_write_reg_mask(VIASR, 0x1B, value, 0xC0);
+}
+
+static inline u8 set_clock_source_common(enum via_clksrc source, bool use_pll)
+{
+        u8 data = 0;
+
+        switch (source) {
+        case VIA_CLKSRC_X1:
+                data = 0x00;
+                break;
+        case VIA_CLKSRC_TVX1:
+                data = 0x02;
+                break;
+        case VIA_CLKSRC_TVPLL:
+                data = 0x04; /* 0x06 should be the same */
+                break;
+        case VIA_CLKSRC_DVP1TVCLKR:
+                data = 0x0A;
+                break;
+        case VIA_CLKSRC_CAP0:
+                data = 0xC;
+                break;
+        case VIA_CLKSRC_CAP1:
+                data = 0x0E;
+                break;
+        }
+
+        if (!use_pll)
+                data |= 1;
+
+        return data;
+}
+
+static void set_primary_clock_source(enum via_clksrc source, bool use_pll)
+{
+        u8 data = set_clock_source_common(source, use_pll) << 4;
+        via_write_reg_mask(VIACR, 0x6C, data, 0xF0);
+}
+
+static void set_secondary_clock_source(enum via_clksrc source, bool use_pll)
+{
+        u8 data = set_clock_source_common(source, use_pll);
+        via_write_reg_mask(VIACR, 0x6C, data, 0x0F);
+}
+
+static void dummy_set_clock_state(u8 state)
+{
+        printk(KERN_INFO "Using undocumented set clock state.\n%s", via_slap);
+}
+
+static void dummy_set_clock_source(enum via_clksrc source, bool use_pll)
+{
+        printk(KERN_INFO "Using undocumented set clock source.\n%s", via_slap);
+}
+
+static void dummy_set_pll_state(u8 state)
+{
+        printk(KERN_INFO "Using undocumented set PLL state.\n%s", via_slap);
+}
+
+static void dummy_set_pll(struct via_pll_config config)
+{
+        printk(KERN_INFO "Using undocumented set PLL.\n%s", via_slap);
+}
+
+void via_clock_init(struct via_clock *clock, int gfx_chip)
+{
+        switch (gfx_chip) {
+        case UNICHROME_CLE266:
+        case UNICHROME_K400:
+                clock->set_primary_clock_state = dummy_set_clock_state;
+                clock->set_primary_clock_source = dummy_set_clock_source;
+                clock->set_primary_pll_state = dummy_set_pll_state;
+                clock->set_primary_pll = cle266_set_primary_pll;
+
+                clock->set_secondary_clock_state = dummy_set_clock_state;
+                clock->set_secondary_clock_source = dummy_set_clock_source;
+                clock->set_secondary_pll_state = dummy_set_pll_state;
+                clock->set_secondary_pll = cle266_set_secondary_pll;
+
+                clock->set_engine_pll_state = dummy_set_pll_state;
+                clock->set_engine_pll = dummy_set_pll;
+                break;
+        case UNICHROME_K800:
+        case UNICHROME_PM800:
+        case UNICHROME_CN700:
+        case UNICHROME_CX700:
+        case UNICHROME_CN750:
+        case UNICHROME_K8M890:
+        case UNICHROME_P4M890:
+        case UNICHROME_P4M900:
+        case UNICHROME_VX800:
+                clock->set_primary_clock_state = set_primary_clock_state;
+                clock->set_primary_clock_source = set_primary_clock_source;
+                clock->set_primary_pll_state = set_primary_pll_state;
+                clock->set_primary_pll = k800_set_primary_pll;
+
+                clock->set_secondary_clock_state = set_secondary_clock_state;
+                clock->set_secondary_clock_source = set_secondary_clock_source;
+                clock->set_secondary_pll_state = set_secondary_pll_state;
+                clock->set_secondary_pll = k800_set_secondary_pll;
+
+                clock->set_engine_pll_state = set_engine_pll_state;
+                clock->set_engine_pll = k800_set_engine_pll;
+                break;
+        case UNICHROME_VX855:
+        case UNICHROME_VX900:
+                clock->set_primary_clock_state = set_primary_clock_state;
+                clock->set_primary_clock_source = set_primary_clock_source;
+                clock->set_primary_pll_state = set_primary_pll_state;
+                clock->set_primary_pll = vx855_set_primary_pll;
+
+                clock->set_secondary_clock_state = set_secondary_clock_state;
+                clock->set_secondary_clock_source = set_secondary_clock_source;
+                clock->set_secondary_pll_state = set_secondary_pll_state;
+                clock->set_secondary_pll = vx855_set_secondary_pll;
+
+                clock->set_engine_pll_state = set_engine_pll_state;
+                clock->set_engine_pll = vx855_set_engine_pll;
+                break;
+
+        }
+}
diff --git a/drivers/video/via/via_clock.h b/drivers/video/via/via_clock.h
new file mode 100644
index 000000000000..88714ae0d157
--- /dev/null
+++ b/drivers/video/via/via_clock.h
@@ -0,0 +1,76 @@
+/*
+ * Copyright 1998-2008 VIA Technologies, Inc. All Rights Reserved.
+ * Copyright 2001-2008 S3 Graphics, Inc. All Rights Reserved.
+ * Copyright 2011 Florian Tobias Schandinat <FlorianSchandinat@gmx.de>
+ *
+ * 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, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTIES OR REPRESENTATIONS; without even
+ * the implied warranty of MERCHANTABILITY or FITNESS FOR
+ * A PARTICULAR PURPOSE.See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+/*
+ * clock and PLL management functions
+ */
+
+#ifndef __VIA_CLOCK_H__
+#define __VIA_CLOCK_H__
+
+#include <linux/types.h>
+
+enum via_clksrc {
+        VIA_CLKSRC_X1 = 0,
+        VIA_CLKSRC_TVX1,
+        VIA_CLKSRC_TVPLL,
+        VIA_CLKSRC_DVP1TVCLKR,
+        VIA_CLKSRC_CAP0,
+        VIA_CLKSRC_CAP1,
+};
+
+struct via_pll_config {
+        u16 multiplier;
+        u8 divisor;
+        u8 rshift;
+};
+
+struct via_clock {
+        void (*set_primary_clock_state)(u8 state);
+        void (*set_primary_clock_source)(enum via_clksrc src, bool use_pll);
+        void (*set_primary_pll_state)(u8 state);
+        void (*set_primary_pll)(struct via_pll_config config);
+
+        void (*set_secondary_clock_state)(u8 state);
+        void (*set_secondary_clock_source)(enum via_clksrc src, bool use_pll);
+        void (*set_secondary_pll_state)(u8 state);
+        void (*set_secondary_pll)(struct via_pll_config config);
+
+        void (*set_engine_pll_state)(u8 state);
+        void (*set_engine_pll)(struct via_pll_config config);
+};
+
+
+static inline u32 get_pll_internal_frequency(u32 ref_freq,
+        struct via_pll_config pll)
+{
+        return ref_freq / pll.divisor * pll.multiplier;
+}
+
+static inline u32 get_pll_output_frequency(u32 ref_freq,
+        struct via_pll_config pll)
+{
+        return get_pll_internal_frequency(ref_freq, pll) >> pll.rshift;
+}
+
+void via_clock_init(struct via_clock *clock, int gfx_chip);
+
+#endif /* __VIA_CLOCK_H__ */
diff --git a/drivers/video/via/via_i2c.c b/drivers/video/via/via_i2c.c
index da9e4ca94b17..78f1405dbab7 100644
--- a/drivers/video/via/via_i2c.c
+++ b/drivers/video/via/via_i2c.c
@@ -32,7 +32,7 @@
  */
 #define VIAFB_NUM_I2C           5
 static struct via_i2c_stuff via_i2c_par[VIAFB_NUM_I2C];
-struct viafb_dev *i2c_vdev;  /* Passed in from core */
+static struct viafb_dev *i2c_vdev;  /* Passed in from core */
 
 static void via_i2c_setscl(void *data, int state)
 {
@@ -114,6 +114,7 @@ static void via_i2c_setsda(void *data, int state)
 
 int viafb_i2c_readbyte(u8 adap, u8 slave_addr, u8 index, u8 *pdata)
 {
+        int ret;
         u8 mm1[] = {0x00};
         struct i2c_msg msgs[2];
 
@@ -126,11 +127,18 @@ int viafb_i2c_readbyte(u8 adap, u8 slave_addr, u8 index, u8 *pdata)
         mm1[0] = index;
         msgs[0].len = 1; msgs[1].len = 1;
         msgs[0].buf = mm1; msgs[1].buf = pdata;
-        return i2c_transfer(&via_i2c_par[adap].adapter, msgs, 2);
+        ret = i2c_transfer(&via_i2c_par[adap].adapter, msgs, 2);
+        if (ret == 2)
+                ret = 0;
+        else if (ret >= 0)
+                ret = -EIO;
+
+        return ret;
 }
 
 int viafb_i2c_writebyte(u8 adap, u8 slave_addr, u8 index, u8 data)
 {
+        int ret;
         u8 msg[2] = { index, data };
         struct i2c_msg msgs;
 
@@ -140,11 +148,18 @@ int viafb_i2c_writebyte(u8 adap, u8 slave_addr, u8 index, u8 data)
         msgs.addr = slave_addr / 2;
         msgs.len = 2;
         msgs.buf = msg;
-        return i2c_transfer(&via_i2c_par[adap].adapter, &msgs, 1);
+        ret = i2c_transfer(&via_i2c_par[adap].adapter, &msgs, 1);
+        if (ret == 1)
+                ret = 0;
+        else if (ret >= 0)
+                ret = -EIO;
+
+        return ret;
 }
 
 int viafb_i2c_readbytes(u8 adap, u8 slave_addr, u8 index, u8 *buff, int buff_len)
 {
+        int ret;
         u8 mm1[] = {0x00};
         struct i2c_msg msgs[2];
 
@@ -156,7 +171,13 @@ int viafb_i2c_readbytes(u8 adap, u8 slave_addr, u8 index, u8 *buff, int buff_len
         mm1[0] = index;
         msgs[0].len = 1; msgs[1].len = buff_len;
         msgs[0].buf = mm1; msgs[1].buf = buff;
-        return i2c_transfer(&via_i2c_par[adap].adapter, msgs, 2);
+        ret = i2c_transfer(&via_i2c_par[adap].adapter, msgs, 2);
+        if (ret == 2)
+                ret = 0;
+        else if (ret >= 0)
+                ret = -EIO;
+
+        return ret;
 }
 
 /*
@@ -181,14 +202,13 @@ static int create_i2c_bus(struct i2c_adapter *adapter,
         algo->setscl = via_i2c_setscl;
         algo->getsda = via_i2c_getsda;
         algo->getscl = via_i2c_getscl;
-        algo->udelay = 40;
-        algo->timeout = 20;
+        algo->udelay = 10;
+        algo->timeout = 2;
         algo->data = adap_cfg;
 
         sprintf(adapter->name, "viafb i2c io_port idx 0x%02x",
                 adap_cfg->ioport_index);
         adapter->owner = THIS_MODULE;
-        adapter->id = 0x01FFFF;
         adapter->class = I2C_CLASS_DDC;
         adapter->algo_data = algo;
         if (pdev)
diff --git a/drivers/video/via/via_utility.c b/drivers/video/via/via_utility.c
index d05ccb62b55f..35458a5eadc8 100644
--- a/drivers/video/via/via_utility.c
+++ b/drivers/video/via/via_utility.c
@@ -174,7 +174,7 @@ void viafb_set_gamma_table(int bpp, unsigned int *gamma_table)
         }
 
         /* If adjust Gamma value in SAMM, fill IGA1,
-           IGA2 Gamma table simultanous. */
+           IGA2 Gamma table simultaneous. */
         /* Switch to IGA2 Gamma Table */
         if ((active_device_amount > 1) &&
                 !((viaparinfo->chip_info->gfx_chip_name ==
diff --git a/drivers/video/via/via_utility.h b/drivers/video/via/via_utility.h
index 1670ba82143f..f23be1708c14 100644
--- a/drivers/video/via/via_utility.h
+++ b/drivers/video/via/via_utility.h
@@ -21,7 +21,7 @@
 #ifndef __VIAUTILITY_H__
 #define __VIAUTILITY_H__
 
-/* These functions are used to get infomation about device's state */
+/* These functions are used to get information about device's state */
 void viafb_get_device_support_state(u32 *support_state);
 void viafb_get_device_connect_state(u32 *connect_state);
 bool viafb_lcd_get_support_expand_state(u32 xres, u32 yres);
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index bdd0e4130f4e..cf43c80d27f6 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -24,6 +24,7 @@
 #include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/via-core.h>
+#include <asm/olpc.h>
 
 #define _MASTER_FILE
 #include "global.h"
@@ -37,17 +38,19 @@ static char *viafb_mode1;
 static int viafb_bpp = 32;
 static int viafb_bpp1 = 32;
 
+static unsigned int viafb_second_xres = 640;
+static unsigned int viafb_second_yres = 480;
 static unsigned int viafb_second_offset;
 static int viafb_second_size;
 
 static int viafb_accel = 1;
 
 /* Added for specifying active devices.*/
-char *viafb_active_dev;
+static char *viafb_active_dev;
 
 /*Added for specify lcd output port*/
-char *viafb_lcd_port = "";
-char *viafb_dvi_port = "";
+static char *viafb_lcd_port = "";
+static char *viafb_dvi_port = "";
 
 static void retrieve_device_setting(struct viafb_ioctl_setting
         *setting_info);
@@ -56,6 +59,32 @@ static int viafb_pan_display(struct fb_var_screeninfo *var,
 
 static struct fb_ops viafb_ops;
 
+/* supported output devices on each IGP
+ * only CX700, VX800, VX855, VX900 were documented
+ * VIA_CRT should be everywhere
+ * VIA_6C can be onle pre-CX700 (probably only on CLE266) as 6C is used for PLL
+ * source selection on CX700 and later
+ * K400 seems to support VIA_96, VIA_DVP1, VIA_LVDS{1,2} as in viamode.c
+ */
+static const u32 supported_odev_map[] = {
+        [UNICHROME_CLE266]      = VIA_CRT | VIA_LDVP0 | VIA_LDVP1,
+        [UNICHROME_K400]        = VIA_CRT | VIA_DVP0 | VIA_DVP1 | VIA_LVDS1
+                                | VIA_LVDS2,
+        [UNICHROME_K800]        = VIA_CRT | VIA_DVP0 | VIA_DVP1 | VIA_LVDS1
+                                | VIA_LVDS2,
+        [UNICHROME_PM800]       = VIA_CRT | VIA_DVP0 | VIA_DVP1 | VIA_LVDS1
+                                | VIA_LVDS2,
+        [UNICHROME_CN700]       = VIA_CRT | VIA_DVP0 | VIA_DVP1 | VIA_LVDS1
+                                | VIA_LVDS2,
+        [UNICHROME_CX700]       = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_CN750]       = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_K8M890]      = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_P4M890]      = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_P4M900]      = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_VX800]       = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_VX855]       = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+        [UNICHROME_VX900]       = VIA_CRT | VIA_DVP1 | VIA_LVDS1 | VIA_LVDS2,
+};
 
 static void viafb_fill_var_color_info(struct fb_var_screeninfo *var, u8 depth)
 {
@@ -156,13 +185,24 @@ static int viafb_release(struct fb_info *info, int user)
         return 0;
 }
 
+static inline int get_var_refresh(struct fb_var_screeninfo *var)
+{
+        u32 htotal, vtotal;
+
+        htotal = var->left_margin + var->xres + var->right_margin
+                + var->hsync_len;
+        vtotal = var->upper_margin + var->yres + var->lower_margin
+                + var->vsync_len;
+        return PICOS2KHZ(var->pixclock) * 1000 / (htotal * vtotal);
+}
+
 static int viafb_check_var(struct fb_var_screeninfo *var,
         struct fb_info *info)
 {
-        int htotal, vtotal, depth;
+        int depth, refresh;
         struct VideoModeTable *vmode_entry;
         struct viafb_par *ppar = info->par;
-        u32 long_refresh, line;
+        u32 line;
 
         DEBUG_MSG(KERN_INFO "viafb_check_var!\n");
         /* Sanity check */
@@ -205,17 +245,11 @@ static int viafb_check_var(struct fb_var_screeninfo *var,
         /* Based on var passed in to calculate the refresh,
          * because our driver use some modes special.
          */
-        htotal = var->xres + var->left_margin +
-        var->right_margin + var->hsync_len;
-        vtotal = var->yres + var->upper_margin +
-                var->lower_margin + var->vsync_len;
-        long_refresh = 1000000000UL / var->pixclock * 1000;
-        long_refresh /= (htotal * vtotal);
-
-        viafb_refresh = viafb_get_refresh(var->xres, var->yres, long_refresh);
+        refresh = viafb_get_refresh(var->xres, var->yres,
+                get_var_refresh(var));
 
         /* Adjust var according to our driver's own table */
-        viafb_fill_var_timing_info(var, viafb_refresh, vmode_entry);
+        viafb_fill_var_timing_info(var, refresh, vmode_entry);
         if (var->accel_flags & FB_ACCELF_TEXT &&
                 !ppar->shared->vdev->engine_mmio)
                 var->accel_flags = 0;
@@ -227,12 +261,13 @@ static int viafb_set_par(struct fb_info *info)
 {
         struct viafb_par *viapar = info->par;
         struct VideoModeTable *vmode_entry, *vmode_entry1 = NULL;
+        int refresh;
         DEBUG_MSG(KERN_INFO "viafb_set_par!\n");
 
         viafb_update_fix(info);
         viapar->depth = fb_get_color_depth(&info->var, &info->fix);
         viafb_update_device_setting(viafbinfo->var.xres, viafbinfo->var.yres,
-                viafbinfo->var.bits_per_pixel, viafb_refresh, 0);
+                viafbinfo->var.bits_per_pixel, 0);
 
         vmode_entry = viafb_get_mode(viafbinfo->var.xres, viafbinfo->var.yres);
         if (viafb_dual_fb) {
@@ -240,7 +275,7 @@ static int viafb_set_par(struct fb_info *info)
                         viafbinfo1->var.yres);
                 viafb_update_device_setting(viafbinfo1->var.xres,
                         viafbinfo1->var.yres, viafbinfo1->var.bits_per_pixel,
-                        viafb_refresh1, 1);
+                        1);
         } else if (viafb_SAMM_ON == 1) {
                 DEBUG_MSG(KERN_INFO
                 "viafb_second_xres = %d, viafb_second_yres = %d, bpp = %d\n",
@@ -249,14 +284,19 @@ static int viafb_set_par(struct fb_info *info)
                         viafb_second_yres);
 
                 viafb_update_device_setting(viafb_second_xres,
-                        viafb_second_yres, viafb_bpp1, viafb_refresh1, 1);
+                        viafb_second_yres, viafb_bpp1, 1);
         }
 
+        refresh = viafb_get_refresh(info->var.xres, info->var.yres,
+                get_var_refresh(&info->var));
         if (vmode_entry) {
-                if (viafb_dual_fb && viapar->iga_path == IGA2)
+                if (viafb_dual_fb && viapar->iga_path == IGA2) {
                         viafb_bpp1 = info->var.bits_per_pixel;
-                else
+                        viafb_refresh1 = refresh;
+                } else {
                         viafb_bpp = info->var.bits_per_pixel;
+                        viafb_refresh = refresh;
+                }
 
                 if (info->var.accel_flags & FB_ACCELF_TEXT)
                         info->flags &= ~FBINFO_HWACCEL_DISABLED;
@@ -332,22 +372,22 @@ static int viafb_blank(int blank_mode, struct fb_info *info)
         case FB_BLANK_UNBLANK:
                 /* Screen: On, HSync: On, VSync: On */
                 /* control CRT monitor power management */
-                viafb_write_reg_mask(CR36, VIACR, 0x00, BIT4 + BIT5);
+                via_set_state(VIA_CRT, VIA_STATE_ON);
                 break;
         case FB_BLANK_HSYNC_SUSPEND:
                 /* Screen: Off, HSync: Off, VSync: On */
                 /* control CRT monitor power management */
-                viafb_write_reg_mask(CR36, VIACR, 0x10, BIT4 + BIT5);
+                via_set_state(VIA_CRT, VIA_STATE_STANDBY);
                 break;
         case FB_BLANK_VSYNC_SUSPEND:
                 /* Screen: Off, HSync: On, VSync: Off */
                 /* control CRT monitor power management */
-                viafb_write_reg_mask(CR36, VIACR, 0x20, BIT4 + BIT5);
+                via_set_state(VIA_CRT, VIA_STATE_SUSPEND);
                 break;
         case FB_BLANK_POWERDOWN:
                 /* Screen: Off, HSync: Off, VSync: Off */
                 /* control CRT monitor power management */
-                viafb_write_reg_mask(CR36, VIACR, 0x30, BIT4 + BIT5);
+                via_set_state(VIA_CRT, VIA_STATE_OFF);
                 break;
         }
 
@@ -403,8 +443,8 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
                 if (viafb_SAMM_ON == 1) {
                         u.viamode.xres_sec = viafb_second_xres;
                         u.viamode.yres_sec = viafb_second_yres;
-                        u.viamode.virtual_xres_sec = viafb_second_virtual_xres;
-                        u.viamode.virtual_yres_sec = viafb_second_virtual_yres;
+                        u.viamode.virtual_xres_sec = viafb_dual_fb ? viafbinfo1->var.xres_virtual : viafbinfo->var.xres_virtual;
+                        u.viamode.virtual_yres_sec = viafb_dual_fb ? viafbinfo1->var.yres_virtual : viafbinfo->var.yres_virtual;
                         u.viamode.refresh_sec = viafb_refresh1;
                         u.viamode.bpp_sec = viafb_bpp1;
                 } else {
@@ -457,7 +497,7 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
                 if (copy_from_user(&gpu32, argp, sizeof(gpu32)))
                         return -EFAULT;
                 if (gpu32 & CRT_Device)
-                        viafb_crt_enable();
+                        via_set_state(VIA_CRT, VIA_STATE_ON);
                 if (gpu32 & DVI_Device)
                         viafb_dvi_enable();
                 if (gpu32 & LCD_Device)
@@ -467,7 +507,7 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
                 if (copy_from_user(&gpu32, argp, sizeof(gpu32)))
                         return -EFAULT;
                 if (gpu32 & CRT_Device)
-                        viafb_crt_disable();
+                        via_set_state(VIA_CRT, VIA_STATE_OFF);
                 if (gpu32 & DVI_Device)
                         viafb_dvi_disable();
                 if (gpu32 & LCD_Device)
@@ -787,7 +827,8 @@ static int viafb_cursor(struct fb_info *info, struct fb_cursor *cursor)
                 bg_color = cursor->image.bg_color;
                 if (chip_name == UNICHROME_CX700 ||
                         chip_name == UNICHROME_VX800 ||
-                        chip_name == UNICHROME_VX855) {
+                        chip_name == UNICHROME_VX855 ||
+                        chip_name == UNICHROME_VX900) {
                         fg_color =
                                 ((info->cmap.red[fg_color] & 0xFFC0) << 14) |
                                 ((info->cmap.green[fg_color] & 0xFFC0) << 4) |
@@ -892,10 +933,8 @@ static int get_primary_device(void)
         /* Rule: device on iga1 path are the primary device. */
         if (viafb_SAMM_ON) {
                 if (viafb_CRT_ON) {
-                        if (viaparinfo->crt_setting_info->iga_path == IGA1) {
-                                DEBUG_MSG(KERN_INFO "CRT IGA Path:%d\n",
-                                        viaparinfo->
-                                        crt_setting_info->iga_path);
+                        if (viaparinfo->shared->iga1_devices & VIA_CRT) {
+                                DEBUG_MSG(KERN_INFO "CRT IGA Path:%d\n", IGA1);
                                 primary_device = CRT_Device;
                         }
                 }
@@ -961,7 +1000,7 @@ static void retrieve_device_setting(struct viafb_ioctl_setting
         setting_info->lcd_attributes.lcd_mode = viafb_lcd_mode;
 }
 
-static int parse_active_dev(void)
+static int __init parse_active_dev(void)
 {
         viafb_CRT_ON = STATE_OFF;
         viafb_DVI_ON = STATE_OFF;
@@ -973,8 +1012,13 @@ static int parse_active_dev(void)
         /*    Note: The previous of active_dev is primary device,
            and the following is secondary device. */
         if (!viafb_active_dev) {
-                viafb_CRT_ON = STATE_ON;
-                viafb_SAMM_ON = STATE_OFF;
+                if (machine_is_olpc()) { /* LCD only */
+                        viafb_LCD_ON = STATE_ON;
+                        viafb_SAMM_ON = STATE_OFF;
+                } else {
+                        viafb_CRT_ON = STATE_ON;
+                        viafb_SAMM_ON = STATE_OFF;
+                }
         } else if (!strcmp(viafb_active_dev, "CRT+DVI")) {
                 /* CRT+DVI */
                 viafb_CRT_ON = STATE_ON;
@@ -1031,7 +1075,7 @@ static int parse_active_dev(void)
         return 0;
 }
 
-static int parse_port(char *opt_str, int *output_interface)
+static int __devinit parse_port(char *opt_str, int *output_interface)
 {
         if (!strncmp(opt_str, "DVP0", 4))
                 *output_interface = INTERFACE_DVP0;
@@ -1048,7 +1092,7 @@ static int parse_port(char *opt_str, int *output_interface)
         return 0;
 }
 
-static void parse_lcd_port(void)
+static void __devinit parse_lcd_port(void)
 {
         parse_port(viafb_lcd_port, &viaparinfo->chip_info->lvds_chip_info.
                 output_interface);
@@ -1061,7 +1105,7 @@ static void parse_lcd_port(void)
                   output_interface);
 }
 
-static void parse_dvi_port(void)
+static void __devinit parse_dvi_port(void)
 {
         parse_port(viafb_dvi_port, &viaparinfo->chip_info->tmds_chip_info.
                 output_interface);
@@ -1431,46 +1475,209 @@ static const struct file_operations viafb_vt1636_proc_fops = {
         .write          = viafb_vt1636_proc_write,
 };
 
-static void viafb_init_proc(struct proc_dir_entry **viafb_entry)
+#endif /* CONFIG_FB_VIA_DIRECT_PROCFS */
+
+static int viafb_sup_odev_proc_show(struct seq_file *m, void *v)
 {
-        *viafb_entry = proc_mkdir("viafb", NULL);
-        if (*viafb_entry) {
-                proc_create("dvp0", 0, *viafb_entry, &viafb_dvp0_proc_fops);
-                proc_create("dvp1", 0, *viafb_entry, &viafb_dvp1_proc_fops);
-                proc_create("dfph", 0, *viafb_entry, &viafb_dfph_proc_fops);
-                proc_create("dfpl", 0, *viafb_entry, &viafb_dfpl_proc_fops);
-                if (VT1636_LVDS == viaparinfo->chip_info->lvds_chip_info.
-                        lvds_chip_name || VT1636_LVDS ==
-                    viaparinfo->chip_info->lvds_chip_info2.lvds_chip_name) {
-                        proc_create("vt1636", 0, *viafb_entry, &viafb_vt1636_proc_fops);
-                }
+        via_odev_to_seq(m, supported_odev_map[
+                viaparinfo->shared->chip_info.gfx_chip_name]);
+        return 0;
+}
+
+static int viafb_sup_odev_proc_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, viafb_sup_odev_proc_show, NULL);
+}
+
+static const struct file_operations viafb_sup_odev_proc_fops = {
+        .owner          = THIS_MODULE,
+        .open           = viafb_sup_odev_proc_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
+
+static ssize_t odev_update(const char __user *buffer, size_t count, u32 *odev)
+{
+        char buf[64], *ptr = buf;
+        u32 devices;
+        bool add, sub;
+
+        if (count < 1 || count > 63)
+                return -EINVAL;
+        if (copy_from_user(&buf[0], buffer, count))
+                return -EFAULT;
+        buf[count] = '\0';
+        add = buf[0] == '+';
+        sub = buf[0] == '-';
+        if (add || sub)
+                ptr++;
+        devices = via_parse_odev(ptr, &ptr);
+        if (*ptr == '\n')
+                ptr++;
+        if (*ptr != 0)
+                return -EINVAL;
+        if (add)
+                *odev |= devices;
+        else if (sub)
+                *odev &= ~devices;
+        else
+                *odev = devices;
+        return count;
+}
+
+static int viafb_iga1_odev_proc_show(struct seq_file *m, void *v)
+{
+        via_odev_to_seq(m, viaparinfo->shared->iga1_devices);
+        return 0;
+}
+
+static int viafb_iga1_odev_proc_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, viafb_iga1_odev_proc_show, NULL);
+}
+
+static ssize_t viafb_iga1_odev_proc_write(struct file *file,
+        const char __user *buffer, size_t count, loff_t *pos)
+{
+        u32 dev_on, dev_off, dev_old, dev_new;
+        ssize_t res;
+
+        dev_old = dev_new = viaparinfo->shared->iga1_devices;
+        res = odev_update(buffer, count, &dev_new);
+        if (res != count)
+                return res;
+        dev_off = dev_old & ~dev_new;
+        dev_on = dev_new & ~dev_old;
+        viaparinfo->shared->iga1_devices = dev_new;
+        viaparinfo->shared->iga2_devices &= ~dev_new;
+        via_set_state(dev_off, VIA_STATE_OFF);
+        via_set_source(dev_new, IGA1);
+        via_set_state(dev_on, VIA_STATE_ON);
+        return res;
+}
+
+static const struct file_operations viafb_iga1_odev_proc_fops = {
+        .owner          = THIS_MODULE,
+        .open           = viafb_iga1_odev_proc_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+        .write          = viafb_iga1_odev_proc_write,
+};
+
+static int viafb_iga2_odev_proc_show(struct seq_file *m, void *v)
+{
+        via_odev_to_seq(m, viaparinfo->shared->iga2_devices);
+        return 0;
+}
+
+static int viafb_iga2_odev_proc_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, viafb_iga2_odev_proc_show, NULL);
+}
+
+static ssize_t viafb_iga2_odev_proc_write(struct file *file,
+        const char __user *buffer, size_t count, loff_t *pos)
+{
+        u32 dev_on, dev_off, dev_old, dev_new;
+        ssize_t res;
+
+        dev_old = dev_new = viaparinfo->shared->iga2_devices;
+        res = odev_update(buffer, count, &dev_new);
+        if (res != count)
+                return res;
+        dev_off = dev_old & ~dev_new;
+        dev_on = dev_new & ~dev_old;
+        viaparinfo->shared->iga2_devices = dev_new;
+        viaparinfo->shared->iga1_devices &= ~dev_new;
+        via_set_state(dev_off, VIA_STATE_OFF);
+        via_set_source(dev_new, IGA2);
+        via_set_state(dev_on, VIA_STATE_ON);
+        return res;
+}
 
+static const struct file_operations viafb_iga2_odev_proc_fops = {
+        .owner          = THIS_MODULE,
+        .open           = viafb_iga2_odev_proc_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+        .write          = viafb_iga2_odev_proc_write,
+};
+
+#define IS_VT1636(lvds_chip)    ((lvds_chip).lvds_chip_name == VT1636_LVDS)
+static void viafb_init_proc(struct viafb_shared *shared)
+{
+        struct proc_dir_entry *iga1_entry, *iga2_entry,
+                *viafb_entry = proc_mkdir("viafb", NULL);
+
+        shared->proc_entry = viafb_entry;
+        if (viafb_entry) {
+#ifdef CONFIG_FB_VIA_DIRECT_PROCFS
+                proc_create("dvp0", 0, viafb_entry, &viafb_dvp0_proc_fops);
+                proc_create("dvp1", 0, viafb_entry, &viafb_dvp1_proc_fops);
+                proc_create("dfph", 0, viafb_entry, &viafb_dfph_proc_fops);
+                proc_create("dfpl", 0, viafb_entry, &viafb_dfpl_proc_fops);
+                if (IS_VT1636(shared->chip_info.lvds_chip_info)
+                        || IS_VT1636(shared->chip_info.lvds_chip_info2))
+                        proc_create("vt1636", 0, viafb_entry,
+                                &viafb_vt1636_proc_fops);
+#endif /* CONFIG_FB_VIA_DIRECT_PROCFS */
+
+                proc_create("supported_output_devices", 0, viafb_entry,
+                        &viafb_sup_odev_proc_fops);
+                iga1_entry = proc_mkdir("iga1", viafb_entry);
+                shared->iga1_proc_entry = iga1_entry;
+                proc_create("output_devices", 0, iga1_entry,
+                        &viafb_iga1_odev_proc_fops);
+                iga2_entry = proc_mkdir("iga2", viafb_entry);
+                shared->iga2_proc_entry = iga2_entry;
+                proc_create("output_devices", 0, iga2_entry,
+                        &viafb_iga2_odev_proc_fops);
         }
 }
-static void viafb_remove_proc(struct proc_dir_entry *viafb_entry)
+static void viafb_remove_proc(struct viafb_shared *shared)
 {
-        struct chip_information *chip_info = &viaparinfo->shared->chip_info;
+        struct proc_dir_entry *viafb_entry = shared->proc_entry,
+                *iga1_entry = shared->iga1_proc_entry,
+                *iga2_entry = shared->iga2_proc_entry;
+
+        if (!viafb_entry)
+                return;
+
+        remove_proc_entry("output_devices", iga2_entry);
+        remove_proc_entry("iga2", viafb_entry);
+        remove_proc_entry("output_devices", iga1_entry);
+        remove_proc_entry("iga1", viafb_entry);
+        remove_proc_entry("supported_output_devices", viafb_entry);
 
+#ifdef CONFIG_FB_VIA_DIRECT_PROCFS
         remove_proc_entry("dvp0", viafb_entry);/* parent dir */
         remove_proc_entry("dvp1", viafb_entry);
         remove_proc_entry("dfph", viafb_entry);
         remove_proc_entry("dfpl", viafb_entry);
-        if (chip_info->lvds_chip_info.lvds_chip_name == VT1636_LVDS
-                || chip_info->lvds_chip_info2.lvds_chip_name == VT1636_LVDS)
+        if (IS_VT1636(shared->chip_info.lvds_chip_info)
+                || IS_VT1636(shared->chip_info.lvds_chip_info2))
                 remove_proc_entry("vt1636", viafb_entry);
+#endif /* CONFIG_FB_VIA_DIRECT_PROCFS */
 
         remove_proc_entry("viafb", NULL);
 }
-
-#endif /* CONFIG_FB_VIA_DIRECT_PROCFS */
+#undef IS_VT1636
 
 static int parse_mode(const char *str, u32 *xres, u32 *yres)
 {
         char *ptr;
 
         if (!str) {
-                *xres = 640;
-                *yres = 480;
+                if (machine_is_olpc()) {
+                        *xres = 1200;
+                        *yres = 900;
+                } else {
+                        *xres = 640;
+                        *yres = 480;
+                }
                 return 0;
         }
 
@@ -1486,6 +1693,39 @@ static int parse_mode(const char *str, u32 *xres, u32 *yres)
 }
 
 
+#ifdef CONFIG_PM
+static int viafb_suspend(void *unused)
+{
+        console_lock();
+        fb_set_suspend(viafbinfo, 1);
+        viafb_sync(viafbinfo);
+        console_unlock();
+
+        return 0;
+}
+
+static int viafb_resume(void *unused)
+{
+        console_lock();
+        if (viaparinfo->shared->vdev->engine_mmio)
+                viafb_reset_engine(viaparinfo);
+        viafb_set_par(viafbinfo);
+        if (viafb_dual_fb)
+                viafb_set_par(viafbinfo1);
+        fb_set_suspend(viafbinfo, 0);
+
+        console_unlock();
+        return 0;
+}
+
+static struct viafb_pm_hooks viafb_fb_pm_hooks = {
+        .suspend = viafb_suspend,
+        .resume = viafb_resume
+};
+
+#endif
+
+
 int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
 {
         u32 default_xres, default_yres;
@@ -1517,7 +1757,6 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
         viaparinfo->lvds_setting_info = &viaparinfo->shared->lvds_setting_info;
         viaparinfo->lvds_setting_info2 =
                 &viaparinfo->shared->lvds_setting_info2;
-        viaparinfo->crt_setting_info = &viaparinfo->shared->crt_setting_info;
         viaparinfo->chip_info = &viaparinfo->shared->chip_info;
 
         if (viafb_dual_fb)
@@ -1544,7 +1783,7 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
         viafbinfo->flags = FBINFO_DEFAULT | FBINFO_HWACCEL_YPAN;
 
         viafbinfo->pseudo_palette = pseudo_pal;
-        if (viafb_accel && !viafb_init_engine(viafbinfo)) {
+        if (viafb_accel && !viafb_setup_engine(viafbinfo)) {
                 viafbinfo->flags |= FBINFO_HWACCEL_COPYAREA |
                         FBINFO_HWACCEL_FILLRECT |  FBINFO_HWACCEL_IMAGEBLIT;
                 default_var.accel_flags = FB_ACCELF_TEXT;
@@ -1564,27 +1803,18 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
 
         parse_mode(viafb_mode, &default_xres, &default_yres);
         vmode_entry = viafb_get_mode(default_xres, default_yres);
-        if (viafb_SAMM_ON == 1) {
+        if (viafb_SAMM_ON == 1)
                 parse_mode(viafb_mode1, &viafb_second_xres,
                         &viafb_second_yres);
 
-                viafb_second_virtual_xres = viafb_second_xres;
-                viafb_second_virtual_yres = viafb_second_yres;
-        }
-
         default_var.xres = default_xres;
         default_var.yres = default_yres;
         default_var.xres_virtual = default_xres;
         default_var.yres_virtual = default_yres;
         default_var.bits_per_pixel = viafb_bpp;
-        default_var.pixclock =
-            viafb_get_pixclock(default_xres, default_yres, viafb_refresh);
-        default_var.left_margin = (default_xres >> 3) & 0xf8;
-        default_var.right_margin = 32;
-        default_var.upper_margin = 16;
-        default_var.lower_margin = 4;
-        default_var.hsync_len = default_var.left_margin;
-        default_var.vsync_len = 4;
+        viafb_fill_var_timing_info(&default_var, viafb_get_refresh(
+                default_var.xres, default_var.yres, viafb_refresh),
+                viafb_get_mode(default_var.xres, default_var.yres));
         viafb_setup_fixinfo(&viafbinfo->fix, viaparinfo);
         viafbinfo->var = default_var;
 
@@ -1620,18 +1850,12 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
 
                 default_var.xres = viafb_second_xres;
                 default_var.yres = viafb_second_yres;
-                default_var.xres_virtual = viafb_second_virtual_xres;
-                default_var.yres_virtual = viafb_second_virtual_yres;
+                default_var.xres_virtual = viafb_second_xres;
+                default_var.yres_virtual = viafb_second_yres;
                 default_var.bits_per_pixel = viafb_bpp1;
-                default_var.pixclock =
-                    viafb_get_pixclock(viafb_second_xres, viafb_second_yres,
-                    viafb_refresh);
-                default_var.left_margin = (viafb_second_xres >> 3) & 0xf8;
-                default_var.right_margin = 32;
-                default_var.upper_margin = 16;
-                default_var.lower_margin = 4;
-                default_var.hsync_len = default_var.left_margin;
-                default_var.vsync_len = 4;
+                viafb_fill_var_timing_info(&default_var, viafb_get_refresh(
+                        default_var.xres, default_var.yres, viafb_refresh1),
+                        viafb_get_mode(default_var.xres, default_var.yres));
 
                 viafb_setup_fixinfo(&viafbinfo1->fix, viaparinfo1);
                 viafb_check_var(&default_var, viafbinfo1);
@@ -1671,10 +1895,12 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
                   viafbinfo->node, viafbinfo->fix.id, default_var.xres,
                   default_var.yres, default_var.bits_per_pixel);
 
-#ifdef CONFIG_FB_VIA_DIRECT_PROCFS
-        viafb_init_proc(&viaparinfo->shared->proc_entry);
-#endif
+        viafb_init_proc(viaparinfo->shared);
         viafb_init_dac(IGA2);
+
+#ifdef CONFIG_PM
+        viafb_pm_register(&viafb_fb_pm_hooks);
+#endif
         return 0;
 
 out_fb_unreg:
@@ -1700,20 +1926,23 @@ void __devexit via_fb_pci_remove(struct pci_dev *pdev)
         unregister_framebuffer(viafbinfo);
         if (viafb_dual_fb)
                 unregister_framebuffer(viafbinfo1);
-#ifdef CONFIG_FB_VIA_DIRECT_PROCFS
-        viafb_remove_proc(viaparinfo->shared->proc_entry);
-#endif
+        viafb_remove_proc(viaparinfo->shared);
         framebuffer_release(viafbinfo);
         if (viafb_dual_fb)
                 framebuffer_release(viafbinfo1);
 }
 
 #ifndef MODULE
-static int __init viafb_setup(char *options)
+static int __init viafb_setup(void)
 {
         char *this_opt;
+        char *options;
+
         DEBUG_MSG(KERN_INFO "viafb_setup!\n");
 
+        if (fb_get_options("viafb", &options))
+                return -ENODEV;
+
         if (!options || !*options)
                 return 0;
 
@@ -1786,22 +2015,29 @@ static int __init viafb_setup(char *options)
  */
 int __init viafb_init(void)
 {
-        u32 dummy;
+        u32 dummy_x, dummy_y;
+        int r;
+
+        if (machine_is_olpc())
+                /* Apply XO-1.5-specific configuration. */
+                viafb_lcd_panel_id = 23;
+
 #ifndef MODULE
-        char *option = NULL;
-        if (fb_get_options("viafb", &option))
-                return -ENODEV;
-        viafb_setup(option);
+        r = viafb_setup();
+        if (r < 0)
+                return r;
 #endif
-        if (parse_mode(viafb_mode, &dummy, &dummy)
-                || parse_mode(viafb_mode1, &dummy, &dummy)
+        if (parse_mode(viafb_mode, &dummy_x, &dummy_y)
+                || !viafb_get_mode(dummy_x, dummy_y)
+                || parse_mode(viafb_mode1, &dummy_x, &dummy_y)
+                || !viafb_get_mode(dummy_x, dummy_y)
                 || viafb_bpp < 0 || viafb_bpp > 32
                 || viafb_bpp1 < 0 || viafb_bpp1 > 32
                 || parse_active_dev())
                 return -EINVAL;
 
         printk(KERN_INFO
-       "VIA Graphics Intergration Chipset framebuffer %d.%d initializing\n",
+       "VIA Graphics Integration Chipset framebuffer %d.%d initializing\n",
                VERSION_MAJOR, VERSION_MINOR);
         return 0;
 }
diff --git a/drivers/video/via/viafbdev.h b/drivers/video/via/viafbdev.h
index 52a35fabba91..d9440635d1d4 100644
--- a/drivers/video/via/viafbdev.h
+++ b/drivers/video/via/viafbdev.h
@@ -40,12 +40,16 @@
 #define VIAFB_NUM_I2C           5
 
 struct viafb_shared {
+        u32 iga1_devices;
+        u32 iga2_devices;
+
         struct proc_dir_entry *proc_entry;      /*viafb proc entry */
+        struct proc_dir_entry *iga1_proc_entry;
+        struct proc_dir_entry *iga2_proc_entry;
         struct viafb_dev *vdev;                 /* Global dev info */
 
         /* All the information will be needed to set engine */
         struct tmds_setting_information tmds_setting_info;
-        struct crt_setting_information crt_setting_info;
         struct lvds_setting_information lvds_setting_info;
         struct lvds_setting_information lvds_setting_info2;
         struct chip_information chip_info;
@@ -74,14 +78,11 @@ struct viafb_par {
         /* All the information will be needed to set engine */
         /* depreciated, use the ones in shared directly */
         struct tmds_setting_information *tmds_setting_info;
-        struct crt_setting_information *crt_setting_info;
         struct lvds_setting_information *lvds_setting_info;
         struct lvds_setting_information *lvds_setting_info2;
         struct chip_information *chip_info;
 };
 
-extern unsigned int viafb_second_virtual_yres;
-extern unsigned int viafb_second_virtual_xres;
 extern int viafb_SAMM_ON;
 extern int viafb_dual_fb;
 extern int viafb_LCD2_ON;
@@ -89,9 +90,6 @@ extern int viafb_LCD_ON;
 extern int viafb_DVI_ON;
 extern int viafb_hotplug;
 
-extern int strict_strtoul(const char *cp, unsigned int base,
-        unsigned long *res);
-
 u8 viafb_gpio_i2c_read_lvds(struct lvds_setting_information
         *plvds_setting_info, struct lvds_chip_information
         *plvds_chip_info, u8 index);
diff --git a/drivers/video/via/viamode.c b/drivers/video/via/viamode.c
index 2dbad3c0f679..58df74e1417e 100644
--- a/drivers/video/via/viamode.c
+++ b/drivers/video/via/viamode.c
@@ -21,72 +21,6 @@
 
 #include <linux/via-core.h>
 #include "global.h"
-struct res_map_refresh res_map_refresh_tbl[] = {
-/*hres, vres, vclock, vmode_refresh*/
-        {480, 640, RES_480X640_60HZ_PIXCLOCK, 60},
-        {640, 480, RES_640X480_60HZ_PIXCLOCK, 60},
-        {640, 480, RES_640X480_75HZ_PIXCLOCK, 75},
-        {640, 480, RES_640X480_85HZ_PIXCLOCK, 85},
-        {640, 480, RES_640X480_100HZ_PIXCLOCK, 100},
-        {640, 480, RES_640X480_120HZ_PIXCLOCK, 120},
-        {720, 480, RES_720X480_60HZ_PIXCLOCK, 60},
-        {720, 576, RES_720X576_60HZ_PIXCLOCK, 60},
-        {800, 480, RES_800X480_60HZ_PIXCLOCK, 60},
-        {800, 600, RES_800X600_60HZ_PIXCLOCK, 60},
-        {800, 600, RES_800X600_75HZ_PIXCLOCK, 75},
-        {800, 600, RES_800X600_85HZ_PIXCLOCK, 85},
-        {800, 600, RES_800X600_100HZ_PIXCLOCK, 100},
-        {800, 600, RES_800X600_120HZ_PIXCLOCK, 120},
-        {848, 480, RES_848X480_60HZ_PIXCLOCK, 60},
-        {856, 480, RES_856X480_60HZ_PIXCLOCK, 60},
-        {1024, 512, RES_1024X512_60HZ_PIXCLOCK, 60},
-        {1024, 600, RES_1024X600_60HZ_PIXCLOCK, 60},
-        {1024, 768, RES_1024X768_60HZ_PIXCLOCK, 60},
-        {1024, 768, RES_1024X768_75HZ_PIXCLOCK, 75},
-        {1024, 768, RES_1024X768_85HZ_PIXCLOCK, 85},
-        {1024, 768, RES_1024X768_100HZ_PIXCLOCK, 100},
-/*  {1152,864, RES_1152X864_70HZ_PIXCLOCK,  70},*/
-        {1152, 864, RES_1152X864_75HZ_PIXCLOCK, 75},
-        {1280, 768, RES_1280X768_60HZ_PIXCLOCK, 60},
-        {1280, 800, RES_1280X800_60HZ_PIXCLOCK, 60},
-        {1280, 960, RES_1280X960_60HZ_PIXCLOCK, 60},
-        {1280, 1024, RES_1280X1024_60HZ_PIXCLOCK, 60},
-        {1280, 1024, RES_1280X1024_75HZ_PIXCLOCK, 75},
-        {1280, 1024, RES_1280X768_85HZ_PIXCLOCK, 85},
-        {1440, 1050, RES_1440X1050_60HZ_PIXCLOCK, 60},
-        {1600, 1200, RES_1600X1200_60HZ_PIXCLOCK, 60},
-        {1600, 1200, RES_1600X1200_75HZ_PIXCLOCK, 75},
-        {1280, 720, RES_1280X720_60HZ_PIXCLOCK, 60},
-        {1920, 1080, RES_1920X1080_60HZ_PIXCLOCK, 60},
-        {1400, 1050, RES_1400X1050_60HZ_PIXCLOCK, 60},
-        {1400, 1050, RES_1400X1050_75HZ_PIXCLOCK, 75},
-        {1368, 768, RES_1368X768_60HZ_PIXCLOCK, 60},
-        {960, 600, RES_960X600_60HZ_PIXCLOCK, 60},
-        {1000, 600, RES_1000X600_60HZ_PIXCLOCK, 60},
-        {1024, 576, RES_1024X576_60HZ_PIXCLOCK, 60},
-        {1088, 612, RES_1088X612_60HZ_PIXCLOCK, 60},
-        {1152, 720, RES_1152X720_60HZ_PIXCLOCK, 60},
-        {1200, 720, RES_1200X720_60HZ_PIXCLOCK, 60},
-        {1200, 900, RES_1200X900_60HZ_PIXCLOCK, 60},
-        {1280, 600, RES_1280X600_60HZ_PIXCLOCK, 60},
-        {1280, 720, RES_1280X720_50HZ_PIXCLOCK, 50},
-        {1280, 768, RES_1280X768_50HZ_PIXCLOCK, 50},
-        {1360, 768, RES_1360X768_60HZ_PIXCLOCK, 60},
-        {1366, 768, RES_1366X768_50HZ_PIXCLOCK, 50},
-        {1366, 768, RES_1366X768_60HZ_PIXCLOCK, 60},
-        {1440, 900, RES_1440X900_60HZ_PIXCLOCK, 60},
-        {1440, 900, RES_1440X900_75HZ_PIXCLOCK, 75},
-        {1600, 900, RES_1600X900_60HZ_PIXCLOCK, 60},
-        {1600, 1024, RES_1600X1024_60HZ_PIXCLOCK, 60},
-        {1680, 1050, RES_1680X1050_60HZ_PIXCLOCK, 60},
-        {1680, 1050, RES_1680X1050_75HZ_PIXCLOCK, 75},
-        {1792, 1344, RES_1792X1344_60HZ_PIXCLOCK, 60},
-        {1856, 1392, RES_1856X1392_60HZ_PIXCLOCK, 60},
-        {1920, 1200, RES_1920X1200_60HZ_PIXCLOCK, 60},
-        {1920, 1440, RES_1920X1440_60HZ_PIXCLOCK, 60},
-        {1920, 1440, RES_1920X1440_75HZ_PIXCLOCK, 75},
-        {2048, 1536, RES_2048X1536_60HZ_PIXCLOCK, 60}
-};
 
 struct io_reg CN400_ModeXregs[] = { {VIASR, SR10, 0xFF, 0x01},
 {VIASR, SR15, 0x02, 0x02},
@@ -96,10 +30,6 @@ struct io_reg CN400_ModeXregs[] = { {VIASR, SR10, 0xFF, 0x01},
 {VIASR, SR1A, 0xFB, 0x08},
 {VIASR, SR1E, 0x0F, 0x01},
 {VIASR, SR2A, 0xFF, 0x00},
-{VIACR, CR0A, 0xFF, 0x1E},      /* Cursor Start                        */
-{VIACR, CR0B, 0xFF, 0x00},      /* Cursor End                          */
-{VIACR, CR0E, 0xFF, 0x00},      /* Cursor Location High                */
-{VIACR, CR0F, 0xFF, 0x00},      /* Cursor Localtion Low                */
 {VIACR, CR32, 0xFF, 0x00},
 {VIACR, CR33, 0xFF, 0x00},
 {VIACR, CR35, 0xFF, 0x00},
@@ -107,21 +37,6 @@ struct io_reg CN400_ModeXregs[] = { {VIASR, SR10, 0xFF, 0x01},
 {VIACR, CR69, 0xFF, 0x00},
 {VIACR, CR6A, 0xFF, 0x40},
 {VIACR, CR6B, 0xFF, 0x00},
-{VIACR, CR6C, 0xFF, 0x00},
-{VIACR, CR7A, 0xFF, 0x01},      /* LCD Scaling Parameter 1             */
-{VIACR, CR7B, 0xFF, 0x02},      /* LCD Scaling Parameter 2             */
-{VIACR, CR7C, 0xFF, 0x03},      /* LCD Scaling Parameter 3             */
-{VIACR, CR7D, 0xFF, 0x04},      /* LCD Scaling Parameter 4             */
-{VIACR, CR7E, 0xFF, 0x07},      /* LCD Scaling Parameter 5             */
-{VIACR, CR7F, 0xFF, 0x0A},      /* LCD Scaling Parameter 6             */
-{VIACR, CR80, 0xFF, 0x0D},      /* LCD Scaling Parameter 7             */
-{VIACR, CR81, 0xFF, 0x13},      /* LCD Scaling Parameter 8             */
-{VIACR, CR82, 0xFF, 0x16},      /* LCD Scaling Parameter 9             */
-{VIACR, CR83, 0xFF, 0x19},      /* LCD Scaling Parameter 10            */
-{VIACR, CR84, 0xFF, 0x1C},      /* LCD Scaling Parameter 11            */
-{VIACR, CR85, 0xFF, 0x1D},      /* LCD Scaling Parameter 12            */
-{VIACR, CR86, 0xFF, 0x1E},      /* LCD Scaling Parameter 13            */
-{VIACR, CR87, 0xFF, 0x1F},      /* LCD Scaling Parameter 14            */
 {VIACR, CR88, 0xFF, 0x40},      /* LCD Panel Type                      */
 {VIACR, CR89, 0xFF, 0x00},      /* LCD Timing Control 0                */
 {VIACR, CR8A, 0xFF, 0x88},      /* LCD Timing Control 1                */
@@ -167,25 +82,10 @@ struct io_reg CN700_ModeXregs[] = { {VIASR, SR10, 0xFF, 0x01},
 {VIACR, CR69, 0xFF, 0x00},
 {VIACR, CR6A, 0xFD, 0x40},
 {VIACR, CR6B, 0xFF, 0x00},
-{VIACR, CR6C, 0xFF, 0x00},
 {VIACR, CR77, 0xFF, 0x00},      /* LCD scaling Factor */
 {VIACR, CR78, 0xFF, 0x00},      /* LCD scaling Factor */
 {VIACR, CR79, 0xFF, 0x00},      /* LCD scaling Factor */
 {VIACR, CR9F, 0x03, 0x00},      /* LCD scaling Factor */
-{VIACR, CR7A, 0xFF, 0x01},      /* LCD Scaling Parameter 1 */
-{VIACR, CR7B, 0xFF, 0x02},      /* LCD Scaling Parameter 2 */
-{VIACR, CR7C, 0xFF, 0x03},      /* LCD Scaling Parameter 3 */
-{VIACR, CR7D, 0xFF, 0x04},      /* LCD Scaling Parameter 4 */
-{VIACR, CR7E, 0xFF, 0x07},      /* LCD Scaling Parameter 5 */
-{VIACR, CR7F, 0xFF, 0x0A},      /* LCD Scaling Parameter 6 */
-{VIACR, CR80, 0xFF, 0x0D},      /* LCD Scaling Parameter 7 */
-{VIACR, CR81, 0xFF, 0x13},      /* LCD Scaling Parameter 8 */
-{VIACR, CR82, 0xFF, 0x16},      /* LCD Scaling Parameter 9 */
-{VIACR, CR83, 0xFF, 0x19},      /* LCD Scaling Parameter 10 */
-{VIACR, CR84, 0xFF, 0x1C},      /* LCD Scaling Parameter 11 */
-{VIACR, CR85, 0xFF, 0x1D},      /* LCD Scaling Parameter 12 */
-{VIACR, CR86, 0xFF, 0x1E},      /* LCD Scaling Parameter 13 */
-{VIACR, CR87, 0xFF, 0x1F},      /* LCD Scaling Parameter 14 */
 {VIACR, CR88, 0xFF, 0x40},      /* LCD Panel Type */
 {VIACR, CR89, 0xFF, 0x00},      /* LCD Timing Control 0 */
 {VIACR, CR8A, 0xFF, 0x88},      /* LCD Timing Control 1 */
@@ -219,30 +119,12 @@ struct io_reg KM400_ModeXregs[] = {
         {VIASR, SR2A, 0xFF, 0x00},      /* Power Management Control 5      */
         {VIASR, SR2D, 0xFF, 0xFF},      /* Power Management Control 1      */
         {VIASR, SR2E, 0xFF, 0xFF},      /* Power Management Control 2      */
-        {VIACR, CR0A, 0xFF, 0x1E},      /* Cursor Start                    */
-        {VIACR, CR0B, 0xFF, 0x00},      /* Cursor End                      */
-        {VIACR, CR0E, 0xFF, 0x00},      /* Cursor Location High            */
-        {VIACR, CR0F, 0xFF, 0x00},      /* Cursor Localtion Low            */
         {VIACR, CR33, 0xFF, 0x00},
         {VIACR, CR55, 0x80, 0x00},
         {VIACR, CR5D, 0x80, 0x00},
         {VIACR, CR36, 0xFF, 0x01},      /* Power Mangement 3                  */
         {VIACR, CR68, 0xFF, 0x67},      /* Default FIFO For IGA2              */
         {VIACR, CR6A, 0x20, 0x20},      /* Extended FIFO On                   */
-        {VIACR, CR7A, 0xFF, 0x01},      /* LCD Scaling Parameter 1            */
-        {VIACR, CR7B, 0xFF, 0x02},      /* LCD Scaling Parameter 2            */
-        {VIACR, CR7C, 0xFF, 0x03},      /* LCD Scaling Parameter 3            */
-        {VIACR, CR7D, 0xFF, 0x04},      /* LCD Scaling Parameter 4            */
-        {VIACR, CR7E, 0xFF, 0x07},      /* LCD Scaling Parameter 5            */
-        {VIACR, CR7F, 0xFF, 0x0A},      /* LCD Scaling Parameter 6            */
-        {VIACR, CR80, 0xFF, 0x0D},      /* LCD Scaling Parameter 7            */
-        {VIACR, CR81, 0xFF, 0x13},      /* LCD Scaling Parameter 8            */
-        {VIACR, CR82, 0xFF, 0x16},      /* LCD Scaling Parameter 9            */
-        {VIACR, CR83, 0xFF, 0x19},      /* LCD Scaling Parameter 10           */
-        {VIACR, CR84, 0xFF, 0x1C},      /* LCD Scaling Parameter 11           */
-        {VIACR, CR85, 0xFF, 0x1D},      /* LCD Scaling Parameter 12           */
-        {VIACR, CR86, 0xFF, 0x1E},      /* LCD Scaling Parameter 13           */
-        {VIACR, CR87, 0xFF, 0x1F},      /* LCD Scaling Parameter 14           */
         {VIACR, CR88, 0xFF, 0x40},      /* LCD Panel Type                     */
         {VIACR, CR89, 0xFF, 0x00},      /* LCD Timing Control 0               */
         {VIACR, CR8A, 0xFF, 0x88},      /* LCD Timing Control 1               */
@@ -269,11 +151,7 @@ struct io_reg CX700_ModeXregs[] = { {VIASR, SR10, 0xFF, 0x01},
 {VIASR, SR1B, 0xFF, 0xF0},
 {VIASR, SR1E, 0xFF, 0x01},
 {VIASR, SR2A, 0xFF, 0x00},
-{VIASR, SR2D, 0xFF, 0xFF},      /* VCK and LCK PLL power on.           */
-{VIACR, CR0A, 0xFF, 0x1E},      /* Cursor Start                        */
-{VIACR, CR0B, 0xFF, 0x00},      /* Cursor End                          */
-{VIACR, CR0E, 0xFF, 0x00},      /* Cursor Location High                */
-{VIACR, CR0F, 0xFF, 0x00},      /* Cursor Localtion Low                */
+{VIASR, SR2D, 0xC0, 0xC0},      /* delayed E3_ECK */
 {VIACR, CR32, 0xFF, 0x00},
 {VIACR, CR33, 0xFF, 0x00},
 {VIACR, CR35, 0xFF, 0x00},
@@ -282,21 +160,6 @@ struct io_reg CX700_ModeXregs[] = { {VIASR, SR10, 0xFF, 0x01},
 {VIACR, CR69, 0xFF, 0x00},
 {VIACR, CR6A, 0xFF, 0x40},
 {VIACR, CR6B, 0xFF, 0x00},
-{VIACR, CR6C, 0xFF, 0x00},
-{VIACR, CR7A, 0xFF, 0x01},      /* LCD Scaling Parameter 1             */
-{VIACR, CR7B, 0xFF, 0x02},      /* LCD Scaling Parameter 2             */
-{VIACR, CR7C, 0xFF, 0x03},      /* LCD Scaling Parameter 3             */
-{VIACR, CR7D, 0xFF, 0x04},      /* LCD Scaling Parameter 4             */
-{VIACR, CR7E, 0xFF, 0x07},      /* LCD Scaling Parameter 5             */
-{VIACR, CR7F, 0xFF, 0x0A},      /* LCD Scaling Parameter 6             */
-{VIACR, CR80, 0xFF, 0x0D},      /* LCD Scaling Parameter 7             */
-{VIACR, CR81, 0xFF, 0x13},      /* LCD Scaling Parameter 8             */
-{VIACR, CR82, 0xFF, 0x16},      /* LCD Scaling Parameter 9             */
-{VIACR, CR83, 0xFF, 0x19},      /* LCD Scaling Parameter 10            */
-{VIACR, CR84, 0xFF, 0x1C},      /* LCD Scaling Parameter 11            */
-{VIACR, CR85, 0xFF, 0x1D},      /* LCD Scaling Parameter 12            */
-{VIACR, CR86, 0xFF, 0x1E},      /* LCD Scaling Parameter 13            */
-{VIACR, CR87, 0xFF, 0x1F},      /* LCD Scaling Parameter 14            */
 {VIACR, CR88, 0xFF, 0x40},      /* LCD Panel Type                      */
 {VIACR, CR89, 0xFF, 0x00},      /* LCD Timing Control 0                */
 {VIACR, CR8A, 0xFF, 0x88},      /* LCD Timing Control 1                */
@@ -326,14 +189,7 @@ struct io_reg VX855_ModeXregs[] = {
 {VIASR, SR2A, 0xF0, 0x00},
 {VIASR, SR58, 0xFF, 0x00},
 {VIASR, SR59, 0xFF, 0x00},
-{VIASR, SR2D, 0xFF, 0xFF},      /* VCK and LCK PLL power on.           */
-{VIACR, CR09, 0xFF, 0x00},      /* Initial CR09=0*/
-{VIACR, CR11, 0x8F, 0x00},      /* IGA1 initial  Vertical end       */
-{VIACR, CR17, 0x7F, 0x00},      /* IGA1 CRT Mode control init   */
-{VIACR, CR0A, 0xFF, 0x1E},      /* Cursor Start                        */
-{VIACR, CR0B, 0xFF, 0x00},      /* Cursor End                          */
-{VIACR, CR0E, 0xFF, 0x00},      /* Cursor Location High                */
-{VIACR, CR0F, 0xFF, 0x00},      /* Cursor Localtion Low                */
+{VIASR, SR2D, 0xC0, 0xC0},      /* delayed E3_ECK */
 {VIACR, CR32, 0xFF, 0x00},
 {VIACR, CR33, 0x7F, 0x00},
 {VIACR, CR35, 0xFF, 0x00},
@@ -341,21 +197,6 @@ struct io_reg VX855_ModeXregs[] = {
 {VIACR, CR69, 0xFF, 0x00},
 {VIACR, CR6A, 0xFD, 0x60},
 {VIACR, CR6B, 0xFF, 0x00},
-{VIACR, CR6C, 0xFF, 0x00},
-{VIACR, CR7A, 0xFF, 0x01},          /* LCD Scaling Parameter 1             */
-{VIACR, CR7B, 0xFF, 0x02},          /* LCD Scaling Parameter 2             */
-{VIACR, CR7C, 0xFF, 0x03},          /* LCD Scaling Parameter 3             */
-{VIACR, CR7D, 0xFF, 0x04},          /* LCD Scaling Parameter 4             */
-{VIACR, CR7E, 0xFF, 0x07},          /* LCD Scaling Parameter 5             */
-{VIACR, CR7F, 0xFF, 0x0A},          /* LCD Scaling Parameter 6             */
-{VIACR, CR80, 0xFF, 0x0D},          /* LCD Scaling Parameter 7             */
-{VIACR, CR81, 0xFF, 0x13},          /* LCD Scaling Parameter 8             */
-{VIACR, CR82, 0xFF, 0x16},          /* LCD Scaling Parameter 9             */
-{VIACR, CR83, 0xFF, 0x19},          /* LCD Scaling Parameter 10            */
-{VIACR, CR84, 0xFF, 0x1C},          /* LCD Scaling Parameter 11            */
-{VIACR, CR85, 0xFF, 0x1D},          /* LCD Scaling Parameter 12            */
-{VIACR, CR86, 0xFF, 0x1E},          /* LCD Scaling Parameter 13            */
-{VIACR, CR87, 0xFF, 0x1F},          /* LCD Scaling Parameter 14            */
 {VIACR, CR88, 0xFF, 0x40},          /* LCD Panel Type                      */
 {VIACR, CR89, 0xFF, 0x00},          /* LCD Timing Control 0                */
 {VIACR, CR8A, 0xFF, 0x88},          /* LCD Timing Control 1                */
@@ -390,21 +231,6 @@ struct io_reg CLE266_ModeXregs[] = { {VIASR, SR1E, 0xF0, 0x00},
 {VIAGR, GR20, 0xFF, 0x00},
 {VIAGR, GR21, 0xFF, 0x00},
 {VIAGR, GR22, 0xFF, 0x00},
-        /* LCD Parameters */
-{VIACR, CR7A, 0xFF, 0x01},      /* LCD Parameter 1 */
-{VIACR, CR7B, 0xFF, 0x02},      /* LCD Parameter 2 */
-{VIACR, CR7C, 0xFF, 0x03},      /* LCD Parameter 3 */
-{VIACR, CR7D, 0xFF, 0x04},      /* LCD Parameter 4 */
-{VIACR, CR7E, 0xFF, 0x07},      /* LCD Parameter 5 */
-{VIACR, CR7F, 0xFF, 0x0A},      /* LCD Parameter 6 */
-{VIACR, CR80, 0xFF, 0x0D},      /* LCD Parameter 7 */
-{VIACR, CR81, 0xFF, 0x13},      /* LCD Parameter 8 */
-{VIACR, CR82, 0xFF, 0x16},      /* LCD Parameter 9 */
-{VIACR, CR83, 0xFF, 0x19},      /* LCD Parameter 10 */
-{VIACR, CR84, 0xFF, 0x1C},      /* LCD Parameter 11 */
-{VIACR, CR85, 0xFF, 0x1D},      /* LCD Parameter 12 */
-{VIACR, CR86, 0xFF, 0x1E},      /* LCD Parameter 13 */
-{VIACR, CR87, 0xFF, 0x1F},      /* LCD Parameter 14 */
 
 };
 
@@ -443,328 +269,321 @@ struct VPITTable VPIT = {
 /********************/
 
 /* 480x640 */
-struct crt_mode_table CRTM480x640[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM480x640[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_25_175M, M480X640_R60_HSP, M480X640_R60_VSP,
+        {REFRESH_60, M480X640_R60_HSP, M480X640_R60_VSP,
          {624, 480, 480, 144, 504, 48, 663, 640, 640, 23, 641, 3} }     /* GTF*/
 };
 
 /* 640x480*/
-struct crt_mode_table CRTM640x480[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM640x480[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_25_175M, M640X480_R60_HSP, M640X480_R60_VSP,
+        {REFRESH_60, M640X480_R60_HSP, M640X480_R60_VSP,
          {800, 640, 648, 144, 656, 96, 525, 480, 480, 45, 490, 2} },
-        {REFRESH_75, CLK_31_500M, M640X480_R75_HSP, M640X480_R75_VSP,
+        {REFRESH_75, M640X480_R75_HSP, M640X480_R75_VSP,
          {840, 640, 640, 200, 656, 64, 500, 480, 480, 20, 481, 3} },
-        {REFRESH_85, CLK_36_000M, M640X480_R85_HSP, M640X480_R85_VSP,
+        {REFRESH_85, M640X480_R85_HSP, M640X480_R85_VSP,
          {832, 640, 640, 192, 696, 56, 509, 480, 480, 29, 481, 3} },
-        {REFRESH_100, CLK_43_163M, M640X480_R100_HSP, M640X480_R100_VSP,
+        {REFRESH_100, M640X480_R100_HSP, M640X480_R100_VSP,
          {848, 640, 640, 208, 680, 64, 509, 480, 480, 29, 481, 3} }, /*GTF*/
-            {REFRESH_120, CLK_52_406M, M640X480_R120_HSP,
-             M640X480_R120_VSP,
-             {848, 640, 640, 208, 680, 64, 515, 480, 480, 35, 481,
-              3} } /*GTF*/
+        {REFRESH_120, M640X480_R120_HSP, M640X480_R120_VSP,
+         {848, 640, 640, 208, 680, 64, 515, 480, 480, 35, 481, 3} } /*GTF*/
 };
 
 /*720x480 (GTF)*/
-struct crt_mode_table CRTM720x480[] = {
-        /*r_rate,vclk,hsp,vsp      */
+static struct crt_mode_table CRTM720x480[] = {
+        /*r_rate,hsp,vsp      */
         /*HT, HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_26_880M, M720X480_R60_HSP, M720X480_R60_VSP,
+        {REFRESH_60, M720X480_R60_HSP, M720X480_R60_VSP,
          {896, 720, 720, 176, 736, 72, 497, 480, 480, 17, 481, 3} }
 
 };
 
 /*720x576 (GTF)*/
-struct crt_mode_table CRTM720x576[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM720x576[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_32_668M, M720X576_R60_HSP, M720X576_R60_VSP,
+        {REFRESH_60, M720X576_R60_HSP, M720X576_R60_VSP,
          {912, 720, 720, 192, 744, 72, 597, 576, 576, 21, 577, 3} }
 };
 
 /* 800x480 (CVT) */
-struct crt_mode_table CRTM800x480[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM800x480[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_29_581M, M800X480_R60_HSP, M800X480_R60_VSP,
+        {REFRESH_60, M800X480_R60_HSP, M800X480_R60_VSP,
          {992, 800, 800, 192, 824, 72, 500, 480, 480, 20, 483, 7} }
 };
 
 /* 800x600*/
-struct crt_mode_table CRTM800x600[] = {
-        /*r_rate,vclk,hsp,vsp     */
+static struct crt_mode_table CRTM800x600[] = {
+        /*r_rate,hsp,vsp     */
         /*HT,   HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_40_000M, M800X600_R60_HSP, M800X600_R60_VSP,
+        {REFRESH_60, M800X600_R60_HSP, M800X600_R60_VSP,
          {1056, 800, 800, 256, 840, 128, 628, 600, 600, 28, 601, 4} },
-        {REFRESH_75, CLK_49_500M, M800X600_R75_HSP, M800X600_R75_VSP,
+        {REFRESH_75, M800X600_R75_HSP, M800X600_R75_VSP,
          {1056, 800, 800, 256, 816, 80, 625, 600, 600, 25, 601, 3} },
-        {REFRESH_85, CLK_56_250M, M800X600_R85_HSP, M800X600_R85_VSP,
+        {REFRESH_85, M800X600_R85_HSP, M800X600_R85_VSP,
          {1048, 800, 800, 248, 832, 64, 631, 600, 600, 31, 601, 3} },
-        {REFRESH_100, CLK_68_179M, M800X600_R100_HSP, M800X600_R100_VSP,
+        {REFRESH_100, M800X600_R100_HSP, M800X600_R100_VSP,
          {1072, 800, 800, 272, 848, 88, 636, 600, 600, 36, 601, 3} },
-        {REFRESH_120, CLK_83_950M, M800X600_R120_HSP,
-                  M800X600_R120_VSP,
-                  {1088, 800, 800, 288, 856, 88, 643, 600, 600, 43, 601,
-                   3} }
+        {REFRESH_120, M800X600_R120_HSP, M800X600_R120_VSP,
+         {1088, 800, 800, 288, 856, 88, 643, 600, 600, 43, 601, 3} }
 };
 
 /* 848x480 (CVT) */
-struct crt_mode_table CRTM848x480[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM848x480[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_31_500M, M848X480_R60_HSP, M848X480_R60_VSP,
+        {REFRESH_60, M848X480_R60_HSP, M848X480_R60_VSP,
          {1056, 848, 848, 208, 872, 80, 500, 480, 480, 20, 483, 5} }
 };
 
 /*856x480 (GTF) convert to 852x480*/
-struct crt_mode_table CRTM852x480[] = {
-        /*r_rate,vclk,hsp,vsp     */
+static struct crt_mode_table CRTM852x480[] = {
+        /*r_rate,hsp,vsp     */
         /*HT,   HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_31_728M, M852X480_R60_HSP, M852X480_R60_VSP,
+        {REFRESH_60, M852X480_R60_HSP, M852X480_R60_VSP,
         {1064, 856, 856, 208, 872, 88, 497, 480, 480, 17, 481, 3} }
 };
 
 /*1024x512 (GTF)*/
-struct crt_mode_table CRTM1024x512[] = {
-        /*r_rate,vclk,hsp,vsp     */
+static struct crt_mode_table CRTM1024x512[] = {
+        /*r_rate,hsp,vsp     */
         /*HT,   HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_41_291M, M1024X512_R60_HSP, M1024X512_R60_VSP,
+        {REFRESH_60, M1024X512_R60_HSP, M1024X512_R60_VSP,
          {1296, 1024, 1024, 272, 1056, 104, 531, 512, 512, 19, 513, 3} }
 
 };
 
 /* 1024x600*/
-struct crt_mode_table CRTM1024x600[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1024x600[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_48_875M, M1024X600_R60_HSP, M1024X600_R60_VSP,
+        {REFRESH_60, M1024X600_R60_HSP, M1024X600_R60_VSP,
          {1312, 1024, 1024, 288, 1064, 104, 622, 600, 600, 22, 601, 3} },
 };
 
 /* 1024x768*/
-struct crt_mode_table CRTM1024x768[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1024x768[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_65_000M, M1024X768_R60_HSP, M1024X768_R60_VSP,
+        {REFRESH_60, M1024X768_R60_HSP, M1024X768_R60_VSP,
         {1344, 1024, 1024, 320, 1048, 136, 806, 768, 768, 38, 771, 6} },
-        {REFRESH_75, CLK_78_750M, M1024X768_R75_HSP, M1024X768_R75_VSP,
+        {REFRESH_75, M1024X768_R75_HSP, M1024X768_R75_VSP,
         {1312, 1024, 1024, 288, 1040, 96, 800, 768, 768, 32, 769, 3} },
-        {REFRESH_85, CLK_94_500M, M1024X768_R85_HSP, M1024X768_R85_VSP,
+        {REFRESH_85, M1024X768_R85_HSP, M1024X768_R85_VSP,
         {1376, 1024, 1024, 352, 1072, 96, 808, 768, 768, 40, 769, 3} },
-        {REFRESH_100, CLK_113_309M, M1024X768_R100_HSP, M1024X768_R100_VSP,
+        {REFRESH_100, M1024X768_R100_HSP, M1024X768_R100_VSP,
         {1392, 1024, 1024, 368, 1096, 112, 814, 768, 768, 46, 769, 3} }
 };
 
 /* 1152x864*/
-struct crt_mode_table CRTM1152x864[] = {
-        /*r_rate,vclk,hsp,vsp      */
+static struct crt_mode_table CRTM1152x864[] = {
+        /*r_rate,hsp,vsp      */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_75, CLK_108_000M, M1152X864_R75_HSP, M1152X864_R75_VSP,
+        {REFRESH_75, M1152X864_R75_HSP, M1152X864_R75_VSP,
          {1600, 1152, 1152, 448, 1216, 128, 900, 864, 864, 36, 865, 3} }
 
 };
 
 /* 1280x720 (HDMI 720P)*/
-struct crt_mode_table CRTM1280x720[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1280x720[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE      */
-        {REFRESH_60, CLK_74_481M, M1280X720_R60_HSP, M1280X720_R60_VSP,
+        {REFRESH_60, M1280X720_R60_HSP, M1280X720_R60_VSP,
          {1648, 1280, 1280, 368, 1392, 40, 750, 720, 720, 30, 725, 5} },
-        {REFRESH_50, CLK_60_466M, M1280X720_R50_HSP, M1280X720_R50_VSP,
+        {REFRESH_50, M1280X720_R50_HSP, M1280X720_R50_VSP,
          {1632, 1280, 1280, 352, 1328, 128, 741, 720, 720, 21, 721, 3} }
 };
 
 /*1280x768 (GTF)*/
-struct crt_mode_table CRTM1280x768[] = {
-        /*r_rate,vclk,hsp,vsp     */
+static struct crt_mode_table CRTM1280x768[] = {
+        /*r_rate,hsp,vsp     */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_80_136M, M1280X768_R60_HSP, M1280X768_R60_VSP,
+        {REFRESH_60, M1280X768_R60_HSP, M1280X768_R60_VSP,
          {1680, 1280, 1280, 400, 1344, 136, 795, 768, 768, 27, 769, 3} },
-        {REFRESH_50, CLK_65_178M, M1280X768_R50_HSP, M1280X768_R50_VSP,
+        {REFRESH_50, M1280X768_R50_HSP, M1280X768_R50_VSP,
          {1648, 1280, 1280, 368, 1336, 128, 791, 768, 768, 23, 769, 3} }
 };
 
 /* 1280x800 (CVT) */
-struct crt_mode_table CRTM1280x800[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1280x800[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_83_375M, M1280X800_R60_HSP, M1280X800_R60_VSP,
+        {REFRESH_60, M1280X800_R60_HSP, M1280X800_R60_VSP,
          {1680, 1280, 1280, 400, 1352, 128, 831, 800, 800, 31, 803, 6} }
 };
 
 /*1280x960*/
-struct crt_mode_table CRTM1280x960[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1280x960[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_108_000M, M1280X960_R60_HSP, M1280X960_R60_VSP,
+        {REFRESH_60, M1280X960_R60_HSP, M1280X960_R60_VSP,
          {1800, 1280, 1280, 520, 1376, 112, 1000, 960, 960, 40, 961, 3} }
 };
 
 /* 1280x1024*/
-struct crt_mode_table CRTM1280x1024[] = {
-        /*r_rate,vclk,,hsp,vsp */
+static struct crt_mode_table CRTM1280x1024[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_108_000M, M1280X1024_R60_HSP, M1280X1024_R60_VSP,
+        {REFRESH_60, M1280X1024_R60_HSP, M1280X1024_R60_VSP,
          {1688, 1280, 1280, 408, 1328, 112, 1066, 1024, 1024, 42, 1025,
           3} },
-        {REFRESH_75, CLK_135_000M, M1280X1024_R75_HSP, M1280X1024_R75_VSP,
+        {REFRESH_75, M1280X1024_R75_HSP, M1280X1024_R75_VSP,
          {1688, 1280, 1280, 408, 1296, 144, 1066, 1024, 1024, 42, 1025,
           3} },
-        {REFRESH_85, CLK_157_500M, M1280X1024_R85_HSP, M1280X1024_R85_VSP,
+        {REFRESH_85, M1280X1024_R85_HSP, M1280X1024_R85_VSP,
          {1728, 1280, 1280, 448, 1344, 160, 1072, 1024, 1024, 48, 1025, 3} }
 };
 
 /* 1368x768 (GTF) */
-struct crt_mode_table CRTM1368x768[] = {
-        /* r_rate,  vclk, hsp, vsp */
+static struct crt_mode_table CRTM1368x768[] = {
+        /* r_rate,  hsp, vsp */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_85_860M, M1368X768_R60_HSP, M1368X768_R60_VSP,
+        {REFRESH_60, M1368X768_R60_HSP, M1368X768_R60_VSP,
          {1800, 1368, 1368, 432, 1440, 144, 795, 768, 768, 27, 769, 3} }
 };
 
 /*1440x1050 (GTF)*/
-struct crt_mode_table CRTM1440x1050[] = {
-        /*r_rate,vclk,hsp,vsp      */
+static struct crt_mode_table CRTM1440x1050[] = {
+        /*r_rate,hsp,vsp      */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_125_104M, M1440X1050_R60_HSP, M1440X1050_R60_VSP,
+        {REFRESH_60, M1440X1050_R60_HSP, M1440X1050_R60_VSP,
          {1936, 1440, 1440, 496, 1536, 152, 1077, 1040, 1040, 37, 1041, 3} }
 };
 
 /* 1600x1200*/
-struct crt_mode_table CRTM1600x1200[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1600x1200[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_162_000M, M1600X1200_R60_HSP, M1600X1200_R60_VSP,
+        {REFRESH_60, M1600X1200_R60_HSP, M1600X1200_R60_VSP,
          {2160, 1600, 1600, 560, 1664, 192, 1250, 1200, 1200, 50, 1201,
           3} },
-        {REFRESH_75, CLK_202_500M, M1600X1200_R75_HSP, M1600X1200_R75_VSP,
+        {REFRESH_75, M1600X1200_R75_HSP, M1600X1200_R75_VSP,
          {2160, 1600, 1600, 560, 1664, 192, 1250, 1200, 1200, 50, 1201, 3} }
 
 };
 
 /* 1680x1050 (CVT) */
-struct crt_mode_table CRTM1680x1050[] = {
-        /* r_rate,          vclk,              hsp,             vsp  */
+static struct crt_mode_table CRTM1680x1050[] = {
+        /* r_rate,              hsp,             vsp  */
         /* HT,  HA,  HBS, HBE, HSS, HSE,    VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_146_760M, M1680x1050_R60_HSP, M1680x1050_R60_VSP,
+        {REFRESH_60, M1680x1050_R60_HSP, M1680x1050_R60_VSP,
          {2240, 1680, 1680, 560, 1784, 176, 1089, 1050, 1050, 39, 1053,
           6} },
-        {REFRESH_75, CLK_187_000M, M1680x1050_R75_HSP, M1680x1050_R75_VSP,
+        {REFRESH_75, M1680x1050_R75_HSP, M1680x1050_R75_VSP,
          {2272, 1680, 1680, 592, 1800, 176, 1099, 1050, 1050, 49, 1053, 6} }
 };
 
 /* 1680x1050 (CVT Reduce Blanking) */
-struct crt_mode_table CRTM1680x1050_RB[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1680x1050_RB[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE,    VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_119_000M, M1680x1050_RB_R60_HSP,
-         M1680x1050_RB_R60_VSP,
+        {REFRESH_60, M1680x1050_RB_R60_HSP, M1680x1050_RB_R60_VSP,
          {1840, 1680, 1680, 160, 1728, 32, 1080, 1050, 1050, 30, 1053, 6} }
 };
 
 /* 1920x1080 (CVT)*/
-struct crt_mode_table CRTM1920x1080[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1920x1080[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_172_798M, M1920X1080_R60_HSP, M1920X1080_R60_VSP,
+        {REFRESH_60, M1920X1080_R60_HSP, M1920X1080_R60_VSP,
          {2576, 1920, 1920, 656, 2048, 200, 1120, 1080, 1080, 40, 1083, 5} }
 };
 
 /* 1920x1080 (CVT with Reduce Blanking) */
-struct crt_mode_table CRTM1920x1080_RB[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1920x1080_RB[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_138_400M, M1920X1080_RB_R60_HSP,
-         M1920X1080_RB_R60_VSP,
+        {REFRESH_60, M1920X1080_RB_R60_HSP, M1920X1080_RB_R60_VSP,
          {2080, 1920, 1920, 160, 1968, 32, 1111, 1080, 1080, 31, 1083, 5} }
 };
 
 /* 1920x1440*/
-struct crt_mode_table CRTM1920x1440[] = {
-        /*r_rate,vclk,hsp,vsp */
+static struct crt_mode_table CRTM1920x1440[] = {
+        /*r_rate,hsp,vsp */
         /*HT,  HA,   HBS,  HBE, HSS,  HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_234_000M, M1920X1440_R60_HSP, M1920X1440_R60_VSP,
+        {REFRESH_60, M1920X1440_R60_HSP, M1920X1440_R60_VSP,
          {2600, 1920, 1920, 680, 2048, 208, 1500, 1440, 1440, 60, 1441,
           3} },
-        {REFRESH_75, CLK_297_500M, M1920X1440_R75_HSP, M1920X1440_R75_VSP,
+        {REFRESH_75, M1920X1440_R75_HSP, M1920X1440_R75_VSP,
          {2640, 1920, 1920, 720, 2064, 224, 1500, 1440, 1440, 60, 1441, 3} }
 };
 
 /* 1400x1050 (CVT) */
-struct crt_mode_table CRTM1400x1050[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1400x1050[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE,   VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_121_750M, M1400X1050_R60_HSP, M1400X1050_R60_VSP,
+        {REFRESH_60, M1400X1050_R60_HSP, M1400X1050_R60_VSP,
          {1864, 1400, 1400, 464, 1488, 144, 1089, 1050, 1050, 39, 1053,
           4} },
-        {REFRESH_75, CLK_156_000M, M1400X1050_R75_HSP, M1400X1050_R75_VSP,
+        {REFRESH_75, M1400X1050_R75_HSP, M1400X1050_R75_VSP,
          {1896, 1400, 1400, 496, 1504, 144, 1099, 1050, 1050, 49, 1053, 4} }
 };
 
 /* 1400x1050 (CVT Reduce Blanking) */
-struct crt_mode_table CRTM1400x1050_RB[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1400x1050_RB[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE,   VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_101_000M, M1400X1050_RB_R60_HSP,
-         M1400X1050_RB_R60_VSP,
+        {REFRESH_60, M1400X1050_RB_R60_HSP, M1400X1050_RB_R60_VSP,
          {1560, 1400, 1400, 160, 1448, 32, 1080, 1050, 1050, 30, 1053, 4} }
 };
 
 /* 960x600 (CVT) */
-struct crt_mode_table CRTM960x600[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM960x600[] = {
+        /* r_rate,          hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_45_250M, M960X600_R60_HSP, M960X600_R60_VSP,
+        {REFRESH_60, M960X600_R60_HSP, M960X600_R60_VSP,
          {1216, 960, 960, 256, 992, 96, 624, 600, 600, 24, 603, 6} }
 };
 
 /* 1000x600 (GTF) */
-struct crt_mode_table CRTM1000x600[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1000x600[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_48_000M, M1000X600_R60_HSP, M1000X600_R60_VSP,
+        {REFRESH_60, M1000X600_R60_HSP, M1000X600_R60_VSP,
          {1288, 1000, 1000, 288, 1040, 104, 622, 600, 600, 22, 601, 3} }
 };
 
 /* 1024x576 (GTF) */
-struct crt_mode_table CRTM1024x576[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1024x576[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_46_996M, M1024X576_R60_HSP, M1024X576_R60_VSP,
+        {REFRESH_60, M1024X576_R60_HSP, M1024X576_R60_VSP,
          {1312, 1024, 1024, 288, 1064, 104, 597, 576, 576, 21, 577, 3} }
 };
 
 /* 1088x612 (CVT) */
-struct crt_mode_table CRTM1088x612[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1088x612[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_52_977M, M1088X612_R60_HSP, M1088X612_R60_VSP,
+        {REFRESH_60, M1088X612_R60_HSP, M1088X612_R60_VSP,
          {1392, 1088, 1088, 304, 1136, 104, 636, 612, 612, 24, 615, 5} }
 };
 
 /* 1152x720 (CVT) */
-struct crt_mode_table CRTM1152x720[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1152x720[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_66_750M, M1152X720_R60_HSP, M1152X720_R60_VSP,
+        {REFRESH_60, M1152X720_R60_HSP, M1152X720_R60_VSP,
          {1488, 1152, 1152, 336, 1208, 112, 748, 720, 720, 28, 723, 6} }
 };
 
 /* 1200x720 (GTF) */
-struct crt_mode_table CRTM1200x720[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1200x720[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_70_159M, M1200X720_R60_HSP, M1200X720_R60_VSP,
+        {REFRESH_60, M1200X720_R60_HSP, M1200X720_R60_VSP,
          {1568, 1200, 1200, 368, 1256, 128, 746, 720, 720, 26, 721, 3} }
 };
 
 /* 1200x900 (DCON) */
-struct crt_mode_table DCON1200x900[] = {
-        /* r_rate,          vclk,               hsp,               vsp   */
-        {REFRESH_60, CLK_57_275M, M1200X900_R60_HSP, M1200X900_R60_VSP,
+static struct crt_mode_table DCON1200x900[] = {
+        /* r_rate,               hsp,               vsp   */
+        {REFRESH_49, M1200X900_R60_HSP, M1200X900_R60_VSP,
         /* The correct htotal is 1240, but this doesn't raster on VX855. */
         /* Via suggested changing to a multiple of 16, hence 1264.       */
         /*  HT,   HA,  HBS, HBE,  HSS, HSE,  VT,  VA, VBS, VBE, VSS, VSE */
@@ -772,126 +591,122 @@ struct crt_mode_table DCON1200x900[] = {
 };
 
 /* 1280x600 (GTF) */
-struct crt_mode_table CRTM1280x600[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1280x600[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE, VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_61_500M, M1280x600_R60_HSP, M1280x600_R60_VSP,
+        {REFRESH_60, M1280x600_R60_HSP, M1280x600_R60_VSP,
          {1648, 1280, 1280, 368, 1336, 128, 622, 600, 600, 22, 601, 3} }
 };
 
 /* 1360x768 (CVT) */
-struct crt_mode_table CRTM1360x768[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1360x768[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_84_750M, M1360X768_R60_HSP, M1360X768_R60_VSP,
+        {REFRESH_60, M1360X768_R60_HSP, M1360X768_R60_VSP,
          {1776, 1360, 1360, 416, 1432, 136, 798, 768, 768, 30, 771, 5} }
 };
 
 /* 1360x768 (CVT Reduce Blanking) */
-struct crt_mode_table CRTM1360x768_RB[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1360x768_RB[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_72_000M, M1360X768_RB_R60_HSP,
-         M1360X768_RB_R60_VSP,
+        {REFRESH_60, M1360X768_RB_R60_HSP, M1360X768_RB_R60_VSP,
          {1520, 1360, 1360, 160, 1408, 32, 790, 768, 768, 22, 771, 5} }
 };
 
 /* 1366x768 (GTF) */
-struct crt_mode_table CRTM1366x768[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1366x768[] = {
+        /* r_rate,          hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_85_860M, M1368X768_R60_HSP, M1368X768_R60_VSP,
+        {REFRESH_60, M1368X768_R60_HSP, M1368X768_R60_VSP,
          {1800, 1368, 1368, 432, 1440, 144, 795, 768, 768, 27, 769, 3} },
-        {REFRESH_50, CLK_69_924M, M1368X768_R50_HSP, M1368X768_R50_VSP,
+        {REFRESH_50, M1368X768_R50_HSP, M1368X768_R50_VSP,
          {1768, 1368, 1368, 400, 1424, 144, 791, 768, 768, 23, 769, 3} }
 };
 
 /* 1440x900 (CVT) */
-struct crt_mode_table CRTM1440x900[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1440x900[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_106_500M, M1440X900_R60_HSP, M1440X900_R60_VSP,
+        {REFRESH_60, M1440X900_R60_HSP, M1440X900_R60_VSP,
          {1904, 1440, 1440, 464, 1520, 152, 934, 900, 900, 34, 903, 6} },
-        {REFRESH_75, CLK_136_700M, M1440X900_R75_HSP, M1440X900_R75_VSP,
+        {REFRESH_75, M1440X900_R75_HSP, M1440X900_R75_VSP,
          {1936, 1440, 1440, 496, 1536, 152, 942, 900, 900, 42, 903, 6} }
 };
 
 /* 1440x900 (CVT Reduce Blanking) */
-struct crt_mode_table CRTM1440x900_RB[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1440x900_RB[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_88_750M, M1440X900_RB_R60_HSP,
-         M1440X900_RB_R60_VSP,
+        {REFRESH_60, M1440X900_RB_R60_HSP, M1440X900_RB_R60_VSP,
          {1600, 1440, 1440, 160, 1488, 32, 926, 900, 900, 26, 903, 6} }
 };
 
 /* 1600x900 (CVT) */
-struct crt_mode_table CRTM1600x900[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1600x900[] = {
+        /* r_rate,          hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_118_840M, M1600X900_R60_HSP, M1600X900_R60_VSP,
+        {REFRESH_60, M1600X900_R60_HSP, M1600X900_R60_VSP,
          {2112, 1600, 1600, 512, 1688, 168, 934, 900, 900, 34, 903, 5} }
 };
 
 /* 1600x900 (CVT Reduce Blanking) */
-struct crt_mode_table CRTM1600x900_RB[] = {
-        /* r_rate,        vclk,           hsp,        vsp   */
+static struct crt_mode_table CRTM1600x900_RB[] = {
+        /* r_rate,           hsp,        vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_97_750M, M1600X900_RB_R60_HSP,
-         M1600X900_RB_R60_VSP,
+        {REFRESH_60, M1600X900_RB_R60_HSP, M1600X900_RB_R60_VSP,
          {1760, 1600, 1600, 160, 1648, 32, 926, 900, 900, 26, 903, 5} }
 };
 
 /* 1600x1024 (GTF) */
-struct crt_mode_table CRTM1600x1024[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1600x1024[] = {
+        /* r_rate,          hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE,   VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_136_700M, M1600X1024_R60_HSP, M1600X1024_R60_VSP,
+        {REFRESH_60, M1600X1024_R60_HSP, M1600X1024_R60_VSP,
          {2144, 1600, 1600, 544, 1704, 168, 1060, 1024, 1024, 36, 1025, 3} }
 };
 
 /* 1792x1344 (DMT) */
-struct crt_mode_table CRTM1792x1344[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1792x1344[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE,   VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_204_000M, M1792x1344_R60_HSP, M1792x1344_R60_VSP,
+        {REFRESH_60, M1792x1344_R60_HSP, M1792x1344_R60_VSP,
          {2448, 1792, 1792, 656, 1920, 200, 1394, 1344, 1344, 50, 1345, 3} }
 };
 
 /* 1856x1392 (DMT) */
-struct crt_mode_table CRTM1856x1392[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1856x1392[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE,   VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_218_500M, M1856x1392_R60_HSP, M1856x1392_R60_VSP,
+        {REFRESH_60, M1856x1392_R60_HSP, M1856x1392_R60_VSP,
          {2528, 1856, 1856, 672, 1952, 224, 1439, 1392, 1392, 47, 1393, 3} }
 };
 
 /* 1920x1200 (CVT) */
-struct crt_mode_table CRTM1920x1200[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1920x1200[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_193_295M, M1920X1200_R60_HSP, M1920X1200_R60_VSP,
+        {REFRESH_60, M1920X1200_R60_HSP, M1920X1200_R60_VSP,
          {2592, 1920, 1920, 672, 2056, 200, 1245, 1200, 1200, 45, 1203, 6} }
 };
 
 /* 1920x1200 (CVT with Reduce Blanking) */
-struct crt_mode_table CRTM1920x1200_RB[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM1920x1200_RB[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE, HSS, HSE, VT,  VA,  VBS, VBE, VSS, VSE */
-        {REFRESH_60, CLK_153_920M, M1920X1200_RB_R60_HSP,
-         M1920X1200_RB_R60_VSP,
+        {REFRESH_60, M1920X1200_RB_R60_HSP, M1920X1200_RB_R60_VSP,
          {2080, 1920, 1920, 160, 1968, 32, 1235, 1200, 1200, 35, 1203, 6} }
 };
 
 /* 2048x1536 (CVT) */
-struct crt_mode_table CRTM2048x1536[] = {
-        /* r_rate,          vclk,              hsp,             vsp   */
+static struct crt_mode_table CRTM2048x1536[] = {
+        /* r_rate,              hsp,             vsp   */
         /* HT,  HA,  HBS, HBE,  HSS, HSE,   VT,  VA,  VBS, VBE,  VSS, VSE */
-        {REFRESH_60, CLK_267_250M, M2048x1536_R60_HSP, M2048x1536_R60_VSP,
+        {REFRESH_60, M2048x1536_R60_HSP, M2048x1536_R60_VSP,
          {2800, 2048, 2048, 752, 2200, 224, 1592, 1536, 1536, 56, 1539, 4} }
 };
 
-struct VideoModeTable viafb_modes[] = {
+static struct VideoModeTable viafb_modes[] = {
         /* Display : 480x640 (GTF) */
         {CRTM480x640, ARRAY_SIZE(CRTM480x640)},
 
@@ -1016,7 +831,7 @@ struct VideoModeTable viafb_modes[] = {
         {CRTM1400x1050, ARRAY_SIZE(CRTM1400x1050)}
 };
 
-struct VideoModeTable viafb_rb_modes[] = {
+static struct VideoModeTable viafb_rb_modes[] = {
         /* Display : 1360x768 (CVT Reduce Blanking) */
         {CRTM1360x768_RB, ARRAY_SIZE(CRTM1360x768_RB)},
 
@@ -1039,26 +854,6 @@ struct VideoModeTable viafb_rb_modes[] = {
         {CRTM1920x1200_RB, ARRAY_SIZE(CRTM1920x1200_RB)}
 };
 
-struct crt_mode_table CEAM1280x720[] = {
-        {REFRESH_60, CLK_74_270M, M1280X720_CEA_R60_HSP,
-         M1280X720_CEA_R60_VSP,
-         /* HT,    HA,   HBS,  HBE,  HSS, HSE,  VT,   VA,  VBS, VBE, VSS, VSE */
-         {1650, 1280, 1280, 370, 1390, 40, 750, 720, 720, 30, 725, 5} }
-};
-struct crt_mode_table CEAM1920x1080[] = {
-        {REFRESH_60, CLK_148_500M, M1920X1080_CEA_R60_HSP,
-         M1920X1080_CEA_R60_VSP,
-         /* HT,    HA,   HBS,  HBE,  HSS, HSE,  VT,  VA, VBS, VBE,  VSS, VSE */
-         {2200, 1920, 1920, 300, 2008, 44, 1125, 1080, 1080, 45, 1084, 5} }
-};
-struct VideoModeTable CEA_HDMI_Modes[] = {
-        /* Display : 1280x720 */
-        {CEAM1280x720, ARRAY_SIZE(CEAM1280x720)},
-        {CEAM1920x1080, ARRAY_SIZE(CEAM1920x1080)}
-};
-
-int NUM_TOTAL_RES_MAP_REFRESH = ARRAY_SIZE(res_map_refresh_tbl);
-int NUM_TOTAL_CEA_MODES = ARRAY_SIZE(CEA_HDMI_Modes);
 int NUM_TOTAL_CN400_ModeXregs = ARRAY_SIZE(CN400_ModeXregs);
 int NUM_TOTAL_CN700_ModeXregs = ARRAY_SIZE(CN700_ModeXregs);
 int NUM_TOTAL_KM400_ModeXregs = ARRAY_SIZE(KM400_ModeXregs);
diff --git a/drivers/video/via/viamode.h b/drivers/video/via/viamode.h
index 5b1ced86514b..3751289eb450 100644
--- a/drivers/video/via/viamode.h
+++ b/drivers/video/via/viamode.h
@@ -41,15 +41,6 @@ struct patch_table {
         struct io_reg *io_reg_table;
 };
 
-struct res_map_refresh {
-        int hres;
-        int vres;
-        int pixclock;
-        int vmode_refresh;
-};
-
-extern int NUM_TOTAL_RES_MAP_REFRESH;
-extern int NUM_TOTAL_CEA_MODES;
 extern int NUM_TOTAL_CN400_ModeXregs;
 extern int NUM_TOTAL_CN700_ModeXregs;
 extern int NUM_TOTAL_KM400_ModeXregs;
@@ -58,15 +49,6 @@ extern int NUM_TOTAL_VX855_ModeXregs;
 extern int NUM_TOTAL_CLE266_ModeXregs;
 extern int NUM_TOTAL_PATCH_MODE;
 
-/********************/
-/* Mode Table       */
-/********************/
-
-extern struct crt_mode_table CEAM1280x720[];
-extern struct crt_mode_table CEAM1920x1080[];
-extern struct VideoModeTable CEA_HDMI_Modes[];
-
-extern struct res_map_refresh res_map_refresh_tbl[];
 extern struct io_reg CN400_ModeXregs[];
 extern struct io_reg CN700_ModeXregs[];
 extern struct io_reg KM400_ModeXregs[];
diff --git a/drivers/video/via/vt1636.c b/drivers/video/via/vt1636.c
index d65bf1aee87c..ee2903b472cf 100644
--- a/drivers/video/via/vt1636.c
+++ b/drivers/video/via/vt1636.c
@@ -23,6 +23,34 @@
 #include <linux/via_i2c.h>
 #include "global.h"
 
+static const struct IODATA common_init_data[] = {
+/*  Index, Mask, Value */
+        /* Set panel power sequence timing */
+        {0x10, 0xC0, 0x00},
+        /* T1: VDD on - Data on. Each increment is 1 ms. (50ms = 031h) */
+        {0x0B, 0xFF, 0x40},
+        /* T2: Data on - Backlight on. Each increment is 2 ms. (210ms = 068h) */
+        {0x0C, 0xFF, 0x31},
+        /* T3: Backlight off -Data off. Each increment is 2 ms. (210ms = 068h)*/
+        {0x0D, 0xFF, 0x31},
+        /* T4: Data off - VDD off. Each increment is 1 ms. (50ms = 031h) */
+        {0x0E, 0xFF, 0x68},
+        /* T5: VDD off - VDD on. Each increment is 100 ms. (500ms = 04h) */
+        {0x0F, 0xFF, 0x68},
+        /* LVDS output power up */
+        {0x09, 0xA0, 0xA0},
+        /* turn on back light */
+        {0x10, 0x33, 0x13}
+};
+
+/* Index, Mask, Value */
+static const struct IODATA dual_channel_enable_data = {0x08, 0xF0, 0xE0};
+static const struct IODATA single_channel_enable_data = {0x08, 0xF0, 0x00};
+static const struct IODATA dithering_enable_data = {0x0A, 0x70, 0x50};
+static const struct IODATA dithering_disable_data = {0x0A, 0x70, 0x00};
+static const struct IODATA vdd_on_data = {0x10, 0x20, 0x20};
+static const struct IODATA vdd_off_data = {0x10, 0x20, 0x00};
+
 u8 viafb_gpio_i2c_read_lvds(struct lvds_setting_information
         *plvds_setting_info, struct lvds_chip_information *plvds_chip_info,
         u8 index)
@@ -55,108 +83,41 @@ void viafb_init_lvds_vt1636(struct lvds_setting_information
         int reg_num, i;
 
         /* Common settings: */
-        reg_num = ARRAY_SIZE(COMMON_INIT_TBL_VT1636);
-
-        for (i = 0; i < reg_num; i++) {
+        reg_num = ARRAY_SIZE(common_init_data);
+        for (i = 0; i < reg_num; i++)
                 viafb_gpio_i2c_write_mask_lvds(plvds_setting_info,
-                                         plvds_chip_info,
-                                         COMMON_INIT_TBL_VT1636[i]);
-        }
+                        plvds_chip_info, common_init_data[i]);
 
         /* Input Data Mode Select */
-        if (plvds_setting_info->device_lcd_dualedge) {
+        if (plvds_setting_info->device_lcd_dualedge)
                 viafb_gpio_i2c_write_mask_lvds(plvds_setting_info,
-                                         plvds_chip_info,
-                                         DUAL_CHANNEL_ENABLE_TBL_VT1636[0]);
-        } else {
+                        plvds_chip_info, dual_channel_enable_data);
+        else
                 viafb_gpio_i2c_write_mask_lvds(plvds_setting_info,
-                                         plvds_chip_info,
-                                         SINGLE_CHANNEL_ENABLE_TBL_VT1636[0]);
-        }
+                        plvds_chip_info, single_channel_enable_data);
 
-        if (plvds_setting_info->LCDDithering) {
+        if (plvds_setting_info->LCDDithering)
                 viafb_gpio_i2c_write_mask_lvds(plvds_setting_info,
-                                         plvds_chip_info,
-                                         DITHERING_ENABLE_TBL_VT1636[0]);
-        } else {
+                        plvds_chip_info, dithering_enable_data);
+        else
                 viafb_gpio_i2c_write_mask_lvds(plvds_setting_info,
-                                         plvds_chip_info,
-                                         DITHERING_DISABLE_TBL_VT1636[0]);
-        }
+                        plvds_chip_info, dithering_disable_data);
 }
 
 void viafb_enable_lvds_vt1636(struct lvds_setting_information
                         *plvds_setting_info,
                         struct lvds_chip_information *plvds_chip_info)
 {
-
         viafb_gpio_i2c_write_mask_lvds(plvds_setting_info, plvds_chip_info,
-                                 VDD_ON_TBL_VT1636[0]);
-
-        /* Pad on: */
-        switch (plvds_chip_info->output_interface) {
-        case INTERFACE_DVP0:
-                {
-                        viafb_write_reg_mask(SR1E, VIASR, 0xC0, 0xC0);
-                        break;
-                }
-
-        case INTERFACE_DVP1:
-                {
-                        viafb_write_reg_mask(SR1E, VIASR, 0x30, 0x30);
-                        break;
-                }
-
-        case INTERFACE_DFP_LOW:
-                {
-                        viafb_write_reg_mask(SR2A, VIASR, 0x03, 0x03);
-                        break;
-                }
-
-        case INTERFACE_DFP_HIGH:
-                {
-                        viafb_write_reg_mask(SR2A, VIASR, 0x03, 0x0C);
-                        break;
-                }
-
-        }
+                vdd_on_data);
 }
 
 void viafb_disable_lvds_vt1636(struct lvds_setting_information
                          *plvds_setting_info,
                          struct lvds_chip_information *plvds_chip_info)
 {
-
         viafb_gpio_i2c_write_mask_lvds(plvds_setting_info, plvds_chip_info,
-                                 VDD_OFF_TBL_VT1636[0]);
-
-        /* Pad off: */
-        switch (plvds_chip_info->output_interface) {
-        case INTERFACE_DVP0:
-                {
-                        viafb_write_reg_mask(SR1E, VIASR, 0x00, 0xC0);
-                        break;
-                }
-
-        case INTERFACE_DVP1:
-                {
-                        viafb_write_reg_mask(SR1E, VIASR, 0x00, 0x30);
-                        break;
-                }
-
-        case INTERFACE_DFP_LOW:
-                {
-                        viafb_write_reg_mask(SR2A, VIASR, 0x00, 0x03);
-                        break;
-                }
-
-        case INTERFACE_DFP_HIGH:
-                {
-                        viafb_write_reg_mask(SR2A, VIASR, 0x00, 0x0C);
-                        break;
-                }
-
-        }
+                vdd_off_data);
 }
 
 bool viafb_lvds_identify_vt1636(u8 i2c_adapter)
@@ -206,22 +167,6 @@ static int get_clk_range_index(u32 Clk)
                 return DPA_CLK_RANGE_150M;
 }
 
-static int get_lvds_dpa_setting_index(int panel_size_id,
-                             struct VT1636_DPA_SETTING *p_vt1636_dpasetting_tbl,
-                               int tbl_size)
-{
-        int i;
-
-        for (i = 0; i < tbl_size; i++) {
-                if (panel_size_id == p_vt1636_dpasetting_tbl->PanelSizeID)
-                        return i;
-
-                p_vt1636_dpasetting_tbl++;
-        }
-
-        return 0;
-}
-
 static void set_dpa_vt1636(struct lvds_setting_information
         *plvds_setting_info, struct lvds_chip_information *plvds_chip_info,
                     struct VT1636_DPA_SETTING *p_vt1636_dpa_setting)
@@ -245,7 +190,9 @@ void viafb_vt1636_patch_skew_on_vt3324(
         struct lvds_setting_information *plvds_setting_info,
         struct lvds_chip_information *plvds_chip_info)
 {
-        int index, size;
+        struct VT1636_DPA_SETTING dpa = {0x00, 0x00}, dpa_16x12 = {0x0B, 0x03},
+                *pdpa;
+        int index;
 
         DEBUG_MSG(KERN_INFO "viafb_vt1636_patch_skew_on_vt3324.\n");
 
@@ -255,19 +202,21 @@ void viafb_vt1636_patch_skew_on_vt3324(
                     &GFX_DPA_SETTING_TBL_VT3324[index]);
 
         /* LVDS Transmitter DPA settings: */
-        size = ARRAY_SIZE(VT1636_DPA_SETTING_TBL_VT3324);
-        index =
-            get_lvds_dpa_setting_index(plvds_setting_info->lcd_panel_id,
-                                       VT1636_DPA_SETTING_TBL_VT3324, size);
-        set_dpa_vt1636(plvds_setting_info, plvds_chip_info,
-                       &VT1636_DPA_SETTING_TBL_VT3324[index]);
+        if (plvds_setting_info->lcd_panel_hres == 1600 &&
+                plvds_setting_info->lcd_panel_vres == 1200)
+                pdpa = &dpa_16x12;
+        else
+                pdpa = &dpa;
+
+        set_dpa_vt1636(plvds_setting_info, plvds_chip_info, pdpa);
 }
 
 void viafb_vt1636_patch_skew_on_vt3327(
         struct lvds_setting_information *plvds_setting_info,
         struct lvds_chip_information *plvds_chip_info)
 {
-        int index, size;
+        struct VT1636_DPA_SETTING dpa = {0x00, 0x00};
+        int index;
 
         DEBUG_MSG(KERN_INFO "viafb_vt1636_patch_skew_on_vt3327.\n");
 
@@ -277,12 +226,7 @@ void viafb_vt1636_patch_skew_on_vt3327(
                     &GFX_DPA_SETTING_TBL_VT3327[index]);
 
         /* LVDS Transmitter DPA settings: */
-        size = ARRAY_SIZE(VT1636_DPA_SETTING_TBL_VT3327);
-        index =
-            get_lvds_dpa_setting_index(plvds_setting_info->lcd_panel_id,
-                                       VT1636_DPA_SETTING_TBL_VT3327, size);
-        set_dpa_vt1636(plvds_setting_info, plvds_chip_info,
-                       &VT1636_DPA_SETTING_TBL_VT3327[index]);
+        set_dpa_vt1636(plvds_setting_info, plvds_chip_info, &dpa);
 }
 
 void viafb_vt1636_patch_skew_on_vt3364(
diff --git a/drivers/video/vt8500lcdfb.c b/drivers/video/vt8500lcdfb.c
new file mode 100644
index 000000000000..0e120d67eb65
--- /dev/null
+++ b/drivers/video/vt8500lcdfb.c
@@ -0,0 +1,475 @@
+/*
+ *  linux/drivers/video/vt8500lcdfb.c
+ *
+ *  Copyright (C) 2010 Alexey Charkov <alchark@gmail.com>
+ *
+ * Based on skeletonfb.c and pxafb.c
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+
+#include <mach/vt8500fb.h>
+
+#include "vt8500lcdfb.h"
+#include "wmt_ge_rops.h"
+
+#define to_vt8500lcd_info(__info) container_of(__info, \
+                                                struct vt8500lcd_info, fb)
+
+static int vt8500lcd_set_par(struct fb_info *info)
+{
+        struct vt8500lcd_info *fbi = to_vt8500lcd_info(info);
+        int reg_bpp = 5; /* 16bpp */
+        int i;
+        unsigned long control0;
+
+        if (!fbi)
+                return -EINVAL;
+
+        if (info->var.bits_per_pixel <= 8) {
+                /* palettized */
+                info->var.red.offset    = 0;
+                info->var.red.length    = info->var.bits_per_pixel;
+                info->var.red.msb_right = 0;
+
+                info->var.green.offset  = 0;
+                info->var.green.length  = info->var.bits_per_pixel;
+                info->var.green.msb_right = 0;
+
+                info->var.blue.offset   = 0;
+                info->var.blue.length   = info->var.bits_per_pixel;
+                info->var.blue.msb_right = 0;
+
+                info->var.transp.offset = 0;
+                info->var.transp.length = 0;
+                info->var.transp.msb_right = 0;
+
+                info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
+                info->fix.line_length = info->var.xres_virtual /
+                                                (8/info->var.bits_per_pixel);
+        } else {
+                /* non-palettized */
+                info->var.transp.offset = 0;
+                info->var.transp.length = 0;
+                info->var.transp.msb_right = 0;
+
+                if (info->var.bits_per_pixel == 16) {
+                        /* RGB565 */
+                        info->var.red.offset = 11;
+                        info->var.red.length = 5;
+                        info->var.red.msb_right = 0;
+                        info->var.green.offset = 5;
+                        info->var.green.length = 6;
+                        info->var.green.msb_right = 0;
+                        info->var.blue.offset = 0;
+                        info->var.blue.length = 5;
+                        info->var.blue.msb_right = 0;
+                } else {
+                        /* Equal depths per channel */
+                        info->var.red.offset = info->var.bits_per_pixel
+                                                        * 2 / 3;
+                        info->var.red.length = info->var.bits_per_pixel / 3;
+                        info->var.red.msb_right = 0;
+                        info->var.green.offset = info->var.bits_per_pixel / 3;
+                        info->var.green.length = info->var.bits_per_pixel / 3;
+                        info->var.green.msb_right = 0;
+                        info->var.blue.offset = 0;
+                        info->var.blue.length = info->var.bits_per_pixel / 3;
+                        info->var.blue.msb_right = 0;
+                }
+
+                info->fix.visual = FB_VISUAL_TRUECOLOR;
+                info->fix.line_length = info->var.bits_per_pixel > 16 ?
+                                        info->var.xres_virtual << 2 :
+                                        info->var.xres_virtual << 1;
+        }
+
+        for (i = 0; i < 8; i++) {
+                if (bpp_values[i] == info->var.bits_per_pixel) {
+                        reg_bpp = i;
+                        continue;
+                }
+        }
+
+        control0 = readl(fbi->regbase) & ~0xf;
+        writel(0, fbi->regbase);
+        while (readl(fbi->regbase + 0x38) & 0x10)
+                /* wait */;
+        writel((((info->var.hsync_len - 1) & 0x3f) << 26)
+                | ((info->var.left_margin & 0xff) << 18)
+                | (((info->var.xres - 1) & 0x3ff) << 8)
+                | (info->var.right_margin & 0xff), fbi->regbase + 0x4);
+        writel((((info->var.vsync_len - 1) & 0x3f) << 26)
+                | ((info->var.upper_margin & 0xff) << 18)
+                | (((info->var.yres - 1) & 0x3ff) << 8)
+                | (info->var.lower_margin & 0xff), fbi->regbase + 0x8);
+        writel((((info->var.yres - 1) & 0x400) << 2)
+                | ((info->var.xres - 1) & 0x400), fbi->regbase + 0x10);
+        writel(0x80000000, fbi->regbase + 0x20);
+        writel(control0 | (reg_bpp << 1) | 0x100, fbi->regbase);
+
+        return 0;
+}
+
+static inline u_int chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+        chan &= 0xffff;
+        chan >>= 16 - bf->length;
+        return chan << bf->offset;
+}
+
+static int vt8500lcd_setcolreg(unsigned regno, unsigned red, unsigned green,
+                           unsigned blue, unsigned transp,
+                           struct fb_info *info) {
+        struct vt8500lcd_info *fbi = to_vt8500lcd_info(info);
+        int ret = 1;
+        unsigned int val;
+        if (regno >= 256)
+                return -EINVAL;
+
+        if (info->var.grayscale)
+                red = green = blue =
+                        (19595 * red + 38470 * green + 7471 * blue) >> 16;
+
+        switch (fbi->fb.fix.visual) {
+        case FB_VISUAL_TRUECOLOR:
+                if (regno < 16) {
+                        u32 *pal = fbi->fb.pseudo_palette;
+
+                        val  = chan_to_field(red, &fbi->fb.var.red);
+                        val |= chan_to_field(green, &fbi->fb.var.green);
+                        val |= chan_to_field(blue, &fbi->fb.var.blue);
+
+                        pal[regno] = val;
+                        ret = 0;
+                }
+                break;
+
+        case FB_VISUAL_STATIC_PSEUDOCOLOR:
+        case FB_VISUAL_PSEUDOCOLOR:
+                writew((red & 0xf800)
+                      | ((green >> 5) & 0x7e0)
+                      | ((blue >> 11) & 0x1f),
+                       fbi->palette_cpu + sizeof(u16) * regno);
+                break;
+        }
+
+        return ret;
+}
+
+static int vt8500lcd_ioctl(struct fb_info *info, unsigned int cmd,
+                         unsigned long arg)
+{
+        int ret = 0;
+        struct vt8500lcd_info *fbi = to_vt8500lcd_info(info);
+
+        if (cmd == FBIO_WAITFORVSYNC) {
+                /* Unmask End of Frame interrupt */
+                writel(0xffffffff ^ (1 << 3), fbi->regbase + 0x3c);
+                ret = wait_event_interruptible_timeout(fbi->wait,
+                        readl(fbi->regbase + 0x38) & (1 << 3), HZ / 10);
+                /* Mask back to reduce unwanted interrupt traffic */
+                writel(0xffffffff, fbi->regbase + 0x3c);
+                if (ret < 0)
+                        return ret;
+                if (ret == 0)
+                        return -ETIMEDOUT;
+        }
+
+        return ret;
+}
+
+static int vt8500lcd_pan_display(struct fb_var_screeninfo *var,
+                                struct fb_info *info)
+{
+        unsigned pixlen = info->fix.line_length / info->var.xres_virtual;
+        unsigned off = pixlen * var->xoffset
+                      + info->fix.line_length * var->yoffset;
+        struct vt8500lcd_info *fbi = to_vt8500lcd_info(info);
+
+        writel((1 << 31)
+                | (((var->xres_virtual - var->xres) * pixlen / 4) << 20)
+                | (off >> 2), fbi->regbase + 0x20);
+        return 0;
+}
+
+/*
+ * vt8500lcd_blank():
+ *      Blank the display by setting all palette values to zero.  Note,
+ *      True Color modes do not really use the palette, so this will not
+ *      blank the display in all modes.
+ */
+static int vt8500lcd_blank(int blank, struct fb_info *info)
+{
+        int i;
+
+        switch (blank) {
+        case FB_BLANK_POWERDOWN:
+        case FB_BLANK_VSYNC_SUSPEND:
+        case FB_BLANK_HSYNC_SUSPEND:
+        case FB_BLANK_NORMAL:
+                if (info->fix.visual == FB_VISUAL_PSEUDOCOLOR ||
+                    info->fix.visual == FB_VISUAL_STATIC_PSEUDOCOLOR)
+                        for (i = 0; i < 256; i++)
+                                vt8500lcd_setcolreg(i, 0, 0, 0, 0, info);
+        case FB_BLANK_UNBLANK:
+                if (info->fix.visual == FB_VISUAL_PSEUDOCOLOR ||
+                    info->fix.visual == FB_VISUAL_STATIC_PSEUDOCOLOR)
+                        fb_set_cmap(&info->cmap, info);
+        }
+        return 0;
+}
+
+static struct fb_ops vt8500lcd_ops = {
+        .owner          = THIS_MODULE,
+        .fb_set_par     = vt8500lcd_set_par,
+        .fb_setcolreg   = vt8500lcd_setcolreg,
+        .fb_fillrect    = wmt_ge_fillrect,
+        .fb_copyarea    = wmt_ge_copyarea,
+        .fb_imageblit   = sys_imageblit,
+        .fb_sync        = wmt_ge_sync,
+        .fb_ioctl       = vt8500lcd_ioctl,
+        .fb_pan_display = vt8500lcd_pan_display,
+        .fb_blank       = vt8500lcd_blank,
+};
+
+static irqreturn_t vt8500lcd_handle_irq(int irq, void *dev_id)
+{
+        struct vt8500lcd_info *fbi = dev_id;
+
+        if (readl(fbi->regbase + 0x38) & (1 << 3))
+                wake_up_interruptible(&fbi->wait);
+
+        writel(0xffffffff, fbi->regbase + 0x38);
+        return IRQ_HANDLED;
+}
+
+static int __devinit vt8500lcd_probe(struct platform_device *pdev)
+{
+        struct vt8500lcd_info *fbi;
+        struct resource *res;
+        struct vt8500fb_platform_data *pdata = pdev->dev.platform_data;
+        void *addr;
+        int irq, ret;
+
+        ret = -ENOMEM;
+        fbi = NULL;
+
+        fbi = kzalloc(sizeof(struct vt8500lcd_info) + sizeof(u32) * 16,
+                                                        GFP_KERNEL);
+        if (!fbi) {
+                dev_err(&pdev->dev, "Failed to initialize framebuffer device\n");
+                ret = -ENOMEM;
+                goto failed;
+        }
+
+        strcpy(fbi->fb.fix.id, "VT8500 LCD");
+
+        fbi->fb.fix.type        = FB_TYPE_PACKED_PIXELS;
+        fbi->fb.fix.xpanstep    = 0;
+        fbi->fb.fix.ypanstep    = 1;
+        fbi->fb.fix.ywrapstep   = 0;
+        fbi->fb.fix.accel       = FB_ACCEL_NONE;
+
+        fbi->fb.var.nonstd      = 0;
+        fbi->fb.var.activate    = FB_ACTIVATE_NOW;
+        fbi->fb.var.height      = -1;
+        fbi->fb.var.width       = -1;
+        fbi->fb.var.vmode       = FB_VMODE_NONINTERLACED;
+
+        fbi->fb.fbops           = &vt8500lcd_ops;
+        fbi->fb.flags           = FBINFO_DEFAULT
+                                | FBINFO_HWACCEL_COPYAREA
+                                | FBINFO_HWACCEL_FILLRECT
+                                | FBINFO_HWACCEL_YPAN
+                                | FBINFO_VIRTFB
+                                | FBINFO_PARTIAL_PAN_OK;
+        fbi->fb.node            = -1;
+
+        addr = fbi;
+        addr = addr + sizeof(struct vt8500lcd_info);
+        fbi->fb.pseudo_palette  = addr;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "no I/O memory resource defined\n");
+                ret = -ENODEV;
+                goto failed_fbi;
+        }
+
+        res = request_mem_region(res->start, resource_size(res), "vt8500lcd");
+        if (res == NULL) {
+                dev_err(&pdev->dev, "failed to request I/O memory\n");
+                ret = -EBUSY;
+                goto failed_fbi;
+        }
+
+        fbi->regbase = ioremap(res->start, resource_size(res));
+        if (fbi->regbase == NULL) {
+                dev_err(&pdev->dev, "failed to map I/O memory\n");
+                ret = -EBUSY;
+                goto failed_free_res;
+        }
+
+        fbi->fb.fix.smem_start  = pdata->video_mem_phys;
+        fbi->fb.fix.smem_len    = pdata->video_mem_len;
+        fbi->fb.screen_base     = pdata->video_mem_virt;
+
+        fbi->palette_size       = PAGE_ALIGN(512);
+        fbi->palette_cpu        = dma_alloc_coherent(&pdev->dev,
+                                                     fbi->palette_size,
+                                                     &fbi->palette_phys,
+                                                     GFP_KERNEL);
+        if (fbi->palette_cpu == NULL) {
+                dev_err(&pdev->dev, "Failed to allocate palette buffer\n");
+                ret = -ENOMEM;
+                goto failed_free_io;
+        }
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                dev_err(&pdev->dev, "no IRQ defined\n");
+                ret = -ENODEV;
+                goto failed_free_palette;
+        }
+
+        ret = request_irq(irq, vt8500lcd_handle_irq, IRQF_DISABLED, "LCD", fbi);
+        if (ret) {
+                dev_err(&pdev->dev, "request_irq failed: %d\n", ret);
+                ret = -EBUSY;
+                goto failed_free_palette;
+        }
+
+        init_waitqueue_head(&fbi->wait);
+
+        if (fb_alloc_cmap(&fbi->fb.cmap, 256, 0) < 0) {
+                dev_err(&pdev->dev, "Failed to allocate color map\n");
+                ret = -ENOMEM;
+                goto failed_free_irq;
+        }
+
+        fb_videomode_to_var(&fbi->fb.var, &pdata->mode);
+        fbi->fb.var.bits_per_pixel      = pdata->bpp;
+        fbi->fb.var.xres_virtual        = pdata->xres_virtual;
+        fbi->fb.var.yres_virtual        = pdata->yres_virtual;
+
+        ret = vt8500lcd_set_par(&fbi->fb);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to set parameters\n");
+                goto failed_free_cmap;
+        }
+
+        writel(fbi->fb.fix.smem_start >> 22, fbi->regbase + 0x1c);
+        writel((fbi->palette_phys & 0xfffffe00) | 1, fbi->regbase + 0x18);
+
+        platform_set_drvdata(pdev, fbi);
+
+        ret = register_framebuffer(&fbi->fb);
+        if (ret < 0) {
+                dev_err(&pdev->dev,
+                        "Failed to register framebuffer device: %d\n", ret);
+                goto failed_free_cmap;
+        }
+
+        /*
+         * Ok, now enable the LCD controller
+         */
+        writel(readl(fbi->regbase) | 1, fbi->regbase);
+
+        return 0;
+
+failed_free_cmap:
+        if (fbi->fb.cmap.len)
+                fb_dealloc_cmap(&fbi->fb.cmap);
+failed_free_irq:
+        free_irq(irq, fbi);
+failed_free_palette:
+        dma_free_coherent(&pdev->dev, fbi->palette_size,
+                          fbi->palette_cpu, fbi->palette_phys);
+failed_free_io:
+        iounmap(fbi->regbase);
+failed_free_res:
+        release_mem_region(res->start, resource_size(res));
+failed_fbi:
+        platform_set_drvdata(pdev, NULL);
+        kfree(fbi);
+failed:
+        return ret;
+}
+
+static int __devexit vt8500lcd_remove(struct platform_device *pdev)
+{
+        struct vt8500lcd_info *fbi = platform_get_drvdata(pdev);
+        struct resource *res;
+        int irq;
+
+        unregister_framebuffer(&fbi->fb);
+
+        writel(0, fbi->regbase);
+
+        if (fbi->fb.cmap.len)
+                fb_dealloc_cmap(&fbi->fb.cmap);
+
+        irq = platform_get_irq(pdev, 0);
+        free_irq(irq, fbi);
+
+        dma_free_coherent(&pdev->dev, fbi->palette_size,
+                          fbi->palette_cpu, fbi->palette_phys);
+
+        iounmap(fbi->regbase);
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        release_mem_region(res->start, resource_size(res));
+
+        kfree(fbi);
+
+        return 0;
+}
+
+static struct platform_driver vt8500lcd_driver = {
+        .probe          = vt8500lcd_probe,
+        .remove         = __devexit_p(vt8500lcd_remove),
+        .driver         = {
+                .owner  = THIS_MODULE,
+                .name   = "vt8500-lcd",
+        },
+};
+
+static int __init vt8500lcd_init(void)
+{
+        return platform_driver_register(&vt8500lcd_driver);
+}
+
+static void __exit vt8500lcd_exit(void)
+{
+        platform_driver_unregister(&vt8500lcd_driver);
+}
+
+module_init(vt8500lcd_init);
+module_exit(vt8500lcd_exit);
+
+MODULE_AUTHOR("Alexey Charkov <alchark@gmail.com>");
+MODULE_DESCRIPTION("LCD controller driver for VIA VT8500");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/vt8500lcdfb.h b/drivers/video/vt8500lcdfb.h
new file mode 100644
index 000000000000..36ca3ca09d83
--- /dev/null
+++ b/drivers/video/vt8500lcdfb.h
@@ -0,0 +1,34 @@
+/*
+ *  linux/drivers/video/vt8500lcdfb.h
+ *
+ *  Copyright (C) 2010 Alexey Charkov <alchark@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+struct vt8500lcd_info {
+        struct fb_info          fb;
+        void __iomem            *regbase;
+        void __iomem            *palette_cpu;
+        dma_addr_t              palette_phys;
+        size_t                  palette_size;
+        wait_queue_head_t       wait;
+};
+
+static int bpp_values[] = {
+        1,
+        2,
+        4,
+        8,
+        12,
+        16,
+        18,
+        24,
+};
diff --git a/drivers/video/vt8623fb.c b/drivers/video/vt8623fb.c
index 85d76ec4c63e..f9b3e3dc2421 100644
--- a/drivers/video/vt8623fb.c
+++ b/drivers/video/vt8623fb.c
@@ -23,7 +23,7 @@
 #include <linux/svga.h>
 #include <linux/init.h>
 #include <linux/pci.h>
-#include <linux/console.h> /* Why should fb driver call console functions? because acquire_console_sem() */
+#include <linux/console.h> /* Why should fb driver call console functions? because console_lock() */
 #include <video/vga.h>
 
 #ifdef CONFIG_MTRR
@@ -121,13 +121,19 @@ MODULE_PARM_DESC(mtrr, "Enable write-combining with MTRR (1=enable, 0=disable, d
 
 /* ------------------------------------------------------------------------- */
 
+static void vt8623fb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+        struct vt8623fb_info *par = info->par;
+
+        svga_tilecursor(par->state.vgabase, info, cursor);
+}
 
 static struct fb_tile_ops vt8623fb_tile_ops = {
         .fb_settile     = svga_settile,
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = vt8623fb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -253,6 +259,7 @@ static void vt8623fb_fillrect(struct fb_info *info, const struct fb_fillrect *re
 
 static void vt8623_set_pixclock(struct fb_info *info, u32 pixclock)
 {
+        struct vt8623fb_info *par = info->par;
         u16 m, n, r;
         u8 regval;
         int rv;
@@ -264,18 +271,18 @@ static void vt8623_set_pixclock(struct fb_info *info, u32 pixclock)
         }
 
         /* Set VGA misc register  */
-        regval = vga_r(NULL, VGA_MIS_R);
-        vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+        regval = vga_r(par->state.vgabase, VGA_MIS_R);
+        vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
         /* Set clock registers */
-        vga_wseq(NULL, 0x46, (n  | (r << 6)));
-        vga_wseq(NULL, 0x47, m);
+        vga_wseq(par->state.vgabase, 0x46, (n  | (r << 6)));
+        vga_wseq(par->state.vgabase, 0x47, m);
 
         udelay(1000);
 
         /* PLL reset */
-        svga_wseq_mask(0x40, 0x02, 0x02);
-        svga_wseq_mask(0x40, 0x00, 0x02);
+        svga_wseq_mask(par->state.vgabase, 0x40, 0x02, 0x02);
+        svga_wseq_mask(par->state.vgabase, 0x40, 0x00, 0x02);
 }
 
 
@@ -285,7 +292,10 @@ static int vt8623fb_open(struct fb_info *info, int user)
 
         mutex_lock(&(par->open_lock));
         if (par->ref_count == 0) {
+                void __iomem *vgabase = par->state.vgabase;
+
                 memset(&(par->state), 0, sizeof(struct vgastate));
+                par->state.vgabase = vgabase;
                 par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
                 par->state.num_crtc = 0xA2;
                 par->state.num_seq = 0x50;
@@ -373,6 +383,7 @@ static int vt8623fb_check_var(struct fb_var_screeninfo *var, struct fb_info *inf
 static int vt8623fb_set_par(struct fb_info *info)
 {
         u32 mode, offset_value, fetch_value, screen_size;
+        struct vt8623fb_info *par = info->par;
         u32 bpp = info->var.bits_per_pixel;
 
         if (bpp != 0) {
@@ -414,82 +425,82 @@ static int vt8623fb_set_par(struct fb_info *info)
         info->var.activate = FB_ACTIVATE_NOW;
 
         /* Unlock registers */
-        svga_wseq_mask(0x10, 0x01, 0x01);
-        svga_wcrt_mask(0x11, 0x00, 0x80);
-        svga_wcrt_mask(0x47, 0x00, 0x01);
+        svga_wseq_mask(par->state.vgabase, 0x10, 0x01, 0x01);
+        svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
+        svga_wcrt_mask(par->state.vgabase, 0x47, 0x00, 0x01);
 
         /* Device, screen and sync off */
-        svga_wseq_mask(0x01, 0x20, 0x20);
-        svga_wcrt_mask(0x36, 0x30, 0x30);
-        svga_wcrt_mask(0x17, 0x00, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x36, 0x30, 0x30);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
         /* Set default values */
-        svga_set_default_gfx_regs();
-        svga_set_default_atc_regs();
-        svga_set_default_seq_regs();
-        svga_set_default_crt_regs();
-        svga_wcrt_multi(vt8623_line_compare_regs, 0xFFFFFFFF);
-        svga_wcrt_multi(vt8623_start_address_regs, 0);
+        svga_set_default_gfx_regs(par->state.vgabase);
+        svga_set_default_atc_regs(par->state.vgabase);
+        svga_set_default_seq_regs(par->state.vgabase);
+        svga_set_default_crt_regs(par->state.vgabase);
+        svga_wcrt_multi(par->state.vgabase, vt8623_line_compare_regs, 0xFFFFFFFF);
+        svga_wcrt_multi(par->state.vgabase, vt8623_start_address_regs, 0);
 
-        svga_wcrt_multi(vt8623_offset_regs, offset_value);
-        svga_wseq_multi(vt8623_fetch_count_regs, fetch_value);
+        svga_wcrt_multi(par->state.vgabase, vt8623_offset_regs, offset_value);
+        svga_wseq_multi(par->state.vgabase, vt8623_fetch_count_regs, fetch_value);
 
         /* Clear H/V Skew */
-        svga_wcrt_mask(0x03, 0x00, 0x60);
-        svga_wcrt_mask(0x05, 0x00, 0x60);
+        svga_wcrt_mask(par->state.vgabase, 0x03, 0x00, 0x60);
+        svga_wcrt_mask(par->state.vgabase, 0x05, 0x00, 0x60);
 
         if (info->var.vmode & FB_VMODE_DOUBLE)
-                svga_wcrt_mask(0x09, 0x80, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
         else
-                svga_wcrt_mask(0x09, 0x00, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
-        svga_wseq_mask(0x1E, 0xF0, 0xF0); // DI/DVP bus
-        svga_wseq_mask(0x2A, 0x0F, 0x0F); // DI/DVP bus
-        svga_wseq_mask(0x16, 0x08, 0xBF); // FIFO read threshold
-        vga_wseq(NULL, 0x17, 0x1F);       // FIFO depth
-        vga_wseq(NULL, 0x18, 0x4E);
-        svga_wseq_mask(0x1A, 0x08, 0x08); // enable MMIO ?
+        svga_wseq_mask(par->state.vgabase, 0x1E, 0xF0, 0xF0); // DI/DVP bus
+        svga_wseq_mask(par->state.vgabase, 0x2A, 0x0F, 0x0F); // DI/DVP bus
+        svga_wseq_mask(par->state.vgabase, 0x16, 0x08, 0xBF); // FIFO read threshold
+        vga_wseq(par->state.vgabase, 0x17, 0x1F);       // FIFO depth
+        vga_wseq(par->state.vgabase, 0x18, 0x4E);
+        svga_wseq_mask(par->state.vgabase, 0x1A, 0x08, 0x08); // enable MMIO ?
 
-        vga_wcrt(NULL, 0x32, 0x00);
-        vga_wcrt(NULL, 0x34, 0x00);
-        vga_wcrt(NULL, 0x6A, 0x80);
-        vga_wcrt(NULL, 0x6A, 0xC0);
+        vga_wcrt(par->state.vgabase, 0x32, 0x00);
+        vga_wcrt(par->state.vgabase, 0x34, 0x00);
+        vga_wcrt(par->state.vgabase, 0x6A, 0x80);
+        vga_wcrt(par->state.vgabase, 0x6A, 0xC0);
 
-        vga_wgfx(NULL, 0x20, 0x00);
-        vga_wgfx(NULL, 0x21, 0x00);
-        vga_wgfx(NULL, 0x22, 0x00);
+        vga_wgfx(par->state.vgabase, 0x20, 0x00);
+        vga_wgfx(par->state.vgabase, 0x21, 0x00);
+        vga_wgfx(par->state.vgabase, 0x22, 0x00);
 
         /* Set SR15 according to number of bits per pixel */
         mode = svga_match_format(vt8623fb_formats, &(info->var), &(info->fix));
         switch (mode) {
         case 0:
                 pr_debug("fb%d: text mode\n", info->node);
-                svga_set_textmode_vga_regs();
-                svga_wseq_mask(0x15, 0x00, 0xFE);
-                svga_wcrt_mask(0x11, 0x60, 0x70);
+                svga_set_textmode_vga_regs(par->state.vgabase);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x00, 0xFE);
+                svga_wcrt_mask(par->state.vgabase, 0x11, 0x60, 0x70);
                 break;
         case 1:
                 pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-                vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
-                svga_wseq_mask(0x15, 0x20, 0xFE);
-                svga_wcrt_mask(0x11, 0x00, 0x70);
+                vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x20, 0xFE);
+                svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x70);
                 break;
         case 2:
                 pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
-                svga_wseq_mask(0x15, 0x00, 0xFE);
-                svga_wcrt_mask(0x11, 0x00, 0x70);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x00, 0xFE);
+                svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x70);
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-                svga_wseq_mask(0x15, 0x22, 0xFE);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x22, 0xFE);
                 break;
         case 4:
                 pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
-                svga_wseq_mask(0x15, 0xB6, 0xFE);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0xB6, 0xFE);
                 break;
         case 5:
                 pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
-                svga_wseq_mask(0x15, 0xAE, 0xFE);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0xAE, 0xFE);
                 break;
         default:
                 printk(KERN_ERR "vt8623fb: unsupported mode - bug\n");
@@ -497,16 +508,16 @@ static int vt8623fb_set_par(struct fb_info *info)
         }
 
         vt8623_set_pixclock(info, info->var.pixclock);
-        svga_set_timings(&vt8623_timing_regs, &(info->var), 1, 1,
+        svga_set_timings(par->state.vgabase, &vt8623_timing_regs, &(info->var), 1, 1,
                          (info->var.vmode & FB_VMODE_DOUBLE) ? 2 : 1, 1,
                          1, info->node);
 
         memset_io(info->screen_base, 0x00, screen_size);
 
         /* Device and screen back on */
-        svga_wcrt_mask(0x17, 0x80, 0x80);
-        svga_wcrt_mask(0x36, 0x00, 0x30);
-        svga_wseq_mask(0x01, 0x00, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+        svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
         return 0;
 }
@@ -569,31 +580,33 @@ static int vt8623fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int vt8623fb_blank(int blank_mode, struct fb_info *info)
 {
+        struct vt8623fb_info *par = info->par;
+
         switch (blank_mode) {
         case FB_BLANK_UNBLANK:
                 pr_debug("fb%d: unblank\n", info->node);
-                svga_wcrt_mask(0x36, 0x00, 0x30);
-                svga_wseq_mask(0x01, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
                 break;
         case FB_BLANK_NORMAL:
                 pr_debug("fb%d: blank\n", info->node);
-                svga_wcrt_mask(0x36, 0x00, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_HSYNC_SUSPEND:
                 pr_debug("fb%d: DPMS standby (hsync off)\n", info->node);
-                svga_wcrt_mask(0x36, 0x10, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x10, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_VSYNC_SUSPEND:
                 pr_debug("fb%d: DPMS suspend (vsync off)\n", info->node);
-                svga_wcrt_mask(0x36, 0x20, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x20, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_POWERDOWN:
                 pr_debug("fb%d: DPMS off (no sync)\n", info->node);
-                svga_wcrt_mask(0x36, 0x30, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x30, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         }
 
@@ -603,6 +616,7 @@ static int vt8623fb_blank(int blank_mode, struct fb_info *info)
 
 static int vt8623fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+        struct vt8623fb_info *par = info->par;
         unsigned int offset;
 
         /* Calculate the offset */
@@ -616,7 +630,7 @@ static int vt8623fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *i
         }
 
         /* Set the offset */
-        svga_wcrt_multi(vt8623_start_address_regs, offset);
+        svga_wcrt_multi(par->state.vgabase, vt8623_start_address_regs, offset);
 
         return 0;
 }
@@ -647,6 +661,8 @@ static struct fb_ops vt8623fb_ops = {
 
 static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+        struct pci_bus_region bus_reg;
+        struct resource vga_res;
         struct fb_info *info;
         struct vt8623fb_info *par;
         unsigned int memsize1, memsize2;
@@ -705,9 +721,18 @@ static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_devi
                 goto err_iomap_2;
         }
 
+        bus_reg.start = 0;
+        bus_reg.end = 64 * 1024;
+
+        vga_res.flags = IORESOURCE_IO;
+
+        pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+        par->state.vgabase = (void __iomem *) vga_res.start;
+
         /* Find how many physical memory there is on card */
-        memsize1 = (vga_rseq(NULL, 0x34) + 1) >> 1;
-        memsize2 = vga_rseq(NULL, 0x39) << 2;
+        memsize1 = (vga_rseq(par->state.vgabase, 0x34) + 1) >> 1;
+        memsize2 = vga_rseq(par->state.vgabase, 0x39) << 2;
 
         if ((16 <= memsize1) && (memsize1 <= 64) && (memsize1 == memsize2))
                 info->screen_size = memsize1 << 20;
@@ -819,12 +844,12 @@ static int vt8623_pci_suspend(struct pci_dev* dev, pm_message_t state)
 
         dev_info(info->device, "suspend\n");
 
-        acquire_console_sem();
+        console_lock();
         mutex_lock(&(par->open_lock));
 
         if ((state.event == PM_EVENT_FREEZE) || (par->ref_count == 0)) {
                 mutex_unlock(&(par->open_lock));
-                release_console_sem();
+                console_unlock();
                 return 0;
         }
 
@@ -835,7 +860,7 @@ static int vt8623_pci_suspend(struct pci_dev* dev, pm_message_t state)
         pci_set_power_state(dev, pci_choose_state(dev, state));
 
         mutex_unlock(&(par->open_lock));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
@@ -850,7 +875,7 @@ static int vt8623_pci_resume(struct pci_dev* dev)
 
         dev_info(info->device, "resume\n");
 
-        acquire_console_sem();
+        console_lock();
         mutex_lock(&(par->open_lock));
 
         if (par->ref_count == 0)
@@ -869,7 +894,7 @@ static int vt8623_pci_resume(struct pci_dev* dev)
 
 fail:
         mutex_unlock(&(par->open_lock));
-        release_console_sem();
+        console_unlock();
 
         return 0;
 }
diff --git a/drivers/video/w100fb.c b/drivers/video/w100fb.c
index d8b12c32e3ef..c8be8af0cc6d 100644
--- a/drivers/video/w100fb.c
+++ b/drivers/video/w100fb.c
@@ -1306,7 +1306,7 @@ static void w100_init_lcd(struct w100fb_par *par)
         union graphic_v_disp_u graphic_v_disp;
         union crtc_total_u crtc_total;
 
-        /* w3200 doesnt like undefined bits being set so zero register values first */
+        /* w3200 doesn't like undefined bits being set so zero register values first */
 
         active_h_disp.val = 0;
         active_h_disp.f.active_h_start=mode->left_margin;
diff --git a/drivers/video/wm8505fb.c b/drivers/video/wm8505fb.c
new file mode 100644
index 000000000000..96e34a569169
--- /dev/null
+++ b/drivers/video/wm8505fb.c
@@ -0,0 +1,422 @@
+/*
+ *  WonderMedia WM8505 Frame Buffer device driver
+ *
+ *  Copyright (C) 2010 Ed Spiridonov <edo.rus@gmail.com>
+ *    Based on vt8500lcdfb.c
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+
+#include <mach/vt8500fb.h>
+
+#include "wm8505fb_regs.h"
+#include "wmt_ge_rops.h"
+
+#define DRIVER_NAME "wm8505-fb"
+
+#define to_wm8505fb_info(__info) container_of(__info, \
+                                                struct wm8505fb_info, fb)
+struct wm8505fb_info {
+        struct fb_info          fb;
+        void __iomem            *regbase;
+        unsigned int            contrast;
+};
+
+
+static int wm8505fb_init_hw(struct fb_info *info)
+{
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+
+        int i;
+
+        /* I know the purpose only of few registers, so clear unknown */
+        for (i = 0; i < 0x200; i += 4)
+                writel(0, fbi->regbase + i);
+
+        /* Set frame buffer address */
+        writel(fbi->fb.fix.smem_start, fbi->regbase + WMT_GOVR_FBADDR);
+        writel(fbi->fb.fix.smem_start, fbi->regbase + WMT_GOVR_FBADDR1);
+
+        /* Set in-memory picture format to RGB 32bpp */
+        writel(0x1c,                   fbi->regbase + WMT_GOVR_COLORSPACE);
+        writel(1,                      fbi->regbase + WMT_GOVR_COLORSPACE1);
+
+        /* Virtual buffer size */
+        writel(info->var.xres,         fbi->regbase + WMT_GOVR_XRES);
+        writel(info->var.xres_virtual, fbi->regbase + WMT_GOVR_XRES_VIRTUAL);
+
+        /* black magic ;) */
+        writel(0xf,                    fbi->regbase + WMT_GOVR_FHI);
+        writel(4,                      fbi->regbase + WMT_GOVR_DVO_SET);
+        writel(1,                      fbi->regbase + WMT_GOVR_MIF_ENABLE);
+        writel(1,                      fbi->regbase + WMT_GOVR_REG_UPDATE);
+
+        return 0;
+}
+
+static int wm8505fb_set_timing(struct fb_info *info)
+{
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+
+        int h_start = info->var.left_margin;
+        int h_end = h_start + info->var.xres;
+        int h_all = h_end + info->var.right_margin;
+        int h_sync = info->var.hsync_len;
+
+        int v_start = info->var.upper_margin;
+        int v_end = v_start + info->var.yres;
+        int v_all = v_end + info->var.lower_margin;
+        int v_sync = info->var.vsync_len;
+
+        writel(0, fbi->regbase + WMT_GOVR_TG);
+
+        writel(h_start, fbi->regbase + WMT_GOVR_TIMING_H_START);
+        writel(h_end,   fbi->regbase + WMT_GOVR_TIMING_H_END);
+        writel(h_all,   fbi->regbase + WMT_GOVR_TIMING_H_ALL);
+        writel(h_sync,  fbi->regbase + WMT_GOVR_TIMING_H_SYNC);
+
+        writel(v_start, fbi->regbase + WMT_GOVR_TIMING_V_START);
+        writel(v_end,   fbi->regbase + WMT_GOVR_TIMING_V_END);
+        writel(v_all,   fbi->regbase + WMT_GOVR_TIMING_V_ALL);
+        writel(v_sync,  fbi->regbase + WMT_GOVR_TIMING_V_SYNC);
+
+        writel(1, fbi->regbase + WMT_GOVR_TG);
+
+        return 0;
+}
+
+
+static int wm8505fb_set_par(struct fb_info *info)
+{
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+
+        if (!fbi)
+                return -EINVAL;
+
+        if (info->var.bits_per_pixel == 32) {
+                info->var.red.offset = 16;
+                info->var.red.length = 8;
+                info->var.red.msb_right = 0;
+                info->var.green.offset = 8;
+                info->var.green.length = 8;
+                info->var.green.msb_right = 0;
+                info->var.blue.offset = 0;
+                info->var.blue.length = 8;
+                info->var.blue.msb_right = 0;
+                info->fix.visual = FB_VISUAL_TRUECOLOR;
+                info->fix.line_length = info->var.xres_virtual << 2;
+        }
+
+        wm8505fb_set_timing(info);
+
+        writel(fbi->contrast<<16 | fbi->contrast<<8 | fbi->contrast,
+                fbi->regbase + WMT_GOVR_CONTRAST);
+
+        return 0;
+}
+
+static ssize_t contrast_show(struct device *dev,
+                             struct device_attribute *attr, char *buf)
+{
+        struct fb_info *info = dev_get_drvdata(dev);
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+
+        return sprintf(buf, "%d\n", fbi->contrast);
+}
+
+static ssize_t contrast_store(struct device *dev,
+                              struct device_attribute *attr,
+                              const char *buf, size_t count)
+{
+        struct fb_info *info = dev_get_drvdata(dev);
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+        unsigned long tmp;
+
+        if (strict_strtoul(buf, 10, &tmp) || (tmp > 0xff))
+                return -EINVAL;
+        fbi->contrast = tmp;
+
+        wm8505fb_set_par(info);
+
+        return count;
+}
+
+static DEVICE_ATTR(contrast, 0644, contrast_show, contrast_store);
+
+static inline u_int chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+        chan &= 0xffff;
+        chan >>= 16 - bf->length;
+        return chan << bf->offset;
+}
+
+static int wm8505fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+                           unsigned blue, unsigned transp,
+                           struct fb_info *info) {
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+        int ret = 1;
+        unsigned int val;
+        if (regno >= 256)
+                return -EINVAL;
+
+        if (info->var.grayscale)
+                red = green = blue =
+                        (19595 * red + 38470 * green + 7471 * blue) >> 16;
+
+        switch (fbi->fb.fix.visual) {
+        case FB_VISUAL_TRUECOLOR:
+                if (regno < 16) {
+                        u32 *pal = info->pseudo_palette;
+
+                        val  = chan_to_field(red, &fbi->fb.var.red);
+                        val |= chan_to_field(green, &fbi->fb.var.green);
+                        val |= chan_to_field(blue, &fbi->fb.var.blue);
+
+                        pal[regno] = val;
+                        ret = 0;
+                }
+                break;
+        }
+
+        return ret;
+}
+
+static int wm8505fb_pan_display(struct fb_var_screeninfo *var,
+                                struct fb_info *info)
+{
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+
+        writel(var->xoffset, fbi->regbase + WMT_GOVR_XPAN);
+        writel(var->yoffset, fbi->regbase + WMT_GOVR_YPAN);
+        return 0;
+}
+
+static int wm8505fb_blank(int blank, struct fb_info *info)
+{
+        struct wm8505fb_info *fbi = to_wm8505fb_info(info);
+
+        switch (blank) {
+        case FB_BLANK_UNBLANK:
+                wm8505fb_set_timing(info);
+                break;
+        default:
+                writel(0,  fbi->regbase + WMT_GOVR_TIMING_V_SYNC);
+                break;
+        }
+
+        return 0;
+}
+
+static struct fb_ops wm8505fb_ops = {
+        .owner          = THIS_MODULE,
+        .fb_set_par     = wm8505fb_set_par,
+        .fb_setcolreg   = wm8505fb_setcolreg,
+        .fb_fillrect    = wmt_ge_fillrect,
+        .fb_copyarea    = wmt_ge_copyarea,
+        .fb_imageblit   = sys_imageblit,
+        .fb_sync        = wmt_ge_sync,
+        .fb_pan_display = wm8505fb_pan_display,
+        .fb_blank       = wm8505fb_blank,
+};
+
+static int __devinit wm8505fb_probe(struct platform_device *pdev)
+{
+        struct wm8505fb_info    *fbi;
+        struct resource         *res;
+        void                    *addr;
+        struct vt8500fb_platform_data *pdata;
+        int ret;
+
+        pdata = pdev->dev.platform_data;
+
+        ret = -ENOMEM;
+        fbi = NULL;
+
+        fbi = kzalloc(sizeof(struct wm8505fb_info) + sizeof(u32) * 16,
+                                                        GFP_KERNEL);
+        if (!fbi) {
+                dev_err(&pdev->dev, "Failed to initialize framebuffer device\n");
+                ret = -ENOMEM;
+                goto failed;
+        }
+
+        strcpy(fbi->fb.fix.id, DRIVER_NAME);
+
+        fbi->fb.fix.type        = FB_TYPE_PACKED_PIXELS;
+        fbi->fb.fix.xpanstep    = 1;
+        fbi->fb.fix.ypanstep    = 1;
+        fbi->fb.fix.ywrapstep   = 0;
+        fbi->fb.fix.accel       = FB_ACCEL_NONE;
+
+        fbi->fb.fbops           = &wm8505fb_ops;
+        fbi->fb.flags           = FBINFO_DEFAULT
+                                | FBINFO_HWACCEL_COPYAREA
+                                | FBINFO_HWACCEL_FILLRECT
+                                | FBINFO_HWACCEL_XPAN
+                                | FBINFO_HWACCEL_YPAN
+                                | FBINFO_VIRTFB
+                                | FBINFO_PARTIAL_PAN_OK;
+        fbi->fb.node            = -1;
+
+        addr = fbi;
+        addr = addr + sizeof(struct wm8505fb_info);
+        fbi->fb.pseudo_palette  = addr;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "no I/O memory resource defined\n");
+                ret = -ENODEV;
+                goto failed_fbi;
+        }
+
+        res = request_mem_region(res->start, resource_size(res), DRIVER_NAME);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "failed to request I/O memory\n");
+                ret = -EBUSY;
+                goto failed_fbi;
+        }
+
+        fbi->regbase = ioremap(res->start, resource_size(res));
+        if (fbi->regbase == NULL) {
+                dev_err(&pdev->dev, "failed to map I/O memory\n");
+                ret = -EBUSY;
+                goto failed_free_res;
+        }
+
+        fb_videomode_to_var(&fbi->fb.var, &pdata->mode);
+
+        fbi->fb.var.nonstd              = 0;
+        fbi->fb.var.activate            = FB_ACTIVATE_NOW;
+
+        fbi->fb.var.height              = -1;
+        fbi->fb.var.width               = -1;
+        fbi->fb.var.xres_virtual        = pdata->xres_virtual;
+        fbi->fb.var.yres_virtual        = pdata->yres_virtual;
+        fbi->fb.var.bits_per_pixel      = pdata->bpp;
+
+        fbi->fb.fix.smem_start  = pdata->video_mem_phys;
+        fbi->fb.fix.smem_len    = pdata->video_mem_len;
+        fbi->fb.screen_base     = pdata->video_mem_virt;
+        fbi->fb.screen_size     = pdata->video_mem_len;
+
+        if (fb_alloc_cmap(&fbi->fb.cmap, 256, 0) < 0) {
+                dev_err(&pdev->dev, "Failed to allocate color map\n");
+                ret = -ENOMEM;
+                goto failed_free_io;
+        }
+
+        wm8505fb_init_hw(&fbi->fb);
+
+        fbi->contrast = 0x80;
+        ret = wm8505fb_set_par(&fbi->fb);
+        if (ret) {
+                dev_err(&pdev->dev, "Failed to set parameters\n");
+                goto failed_free_cmap;
+        }
+
+        platform_set_drvdata(pdev, fbi);
+
+        ret = register_framebuffer(&fbi->fb);
+        if (ret < 0) {
+                dev_err(&pdev->dev,
+                        "Failed to register framebuffer device: %d\n", ret);
+                goto failed_free_cmap;
+        }
+
+        ret = device_create_file(&pdev->dev, &dev_attr_contrast);
+        if (ret < 0) {
+                printk(KERN_WARNING "fb%d: failed to register attributes (%d)\n",
+                        fbi->fb.node, ret);
+        }
+
+        printk(KERN_INFO "fb%d: %s frame buffer at 0x%lx-0x%lx\n",
+               fbi->fb.node, fbi->fb.fix.id, fbi->fb.fix.smem_start,
+               fbi->fb.fix.smem_start + fbi->fb.fix.smem_len - 1);
+
+        return 0;
+
+failed_free_cmap:
+        if (fbi->fb.cmap.len)
+                fb_dealloc_cmap(&fbi->fb.cmap);
+failed_free_io:
+        iounmap(fbi->regbase);
+failed_free_res:
+        release_mem_region(res->start, resource_size(res));
+failed_fbi:
+        platform_set_drvdata(pdev, NULL);
+        kfree(fbi);
+failed:
+        return ret;
+}
+
+static int __devexit wm8505fb_remove(struct platform_device *pdev)
+{
+        struct wm8505fb_info *fbi = platform_get_drvdata(pdev);
+        struct resource *res;
+
+        device_remove_file(&pdev->dev, &dev_attr_contrast);
+
+        unregister_framebuffer(&fbi->fb);
+
+        writel(0, fbi->regbase);
+
+        if (fbi->fb.cmap.len)
+                fb_dealloc_cmap(&fbi->fb.cmap);
+
+        iounmap(fbi->regbase);
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        release_mem_region(res->start, resource_size(res));
+
+        kfree(fbi);
+
+        return 0;
+}
+
+static struct platform_driver wm8505fb_driver = {
+        .probe          = wm8505fb_probe,
+        .remove         = __devexit_p(wm8505fb_remove),
+        .driver         = {
+                .owner  = THIS_MODULE,
+                .name   = DRIVER_NAME,
+        },
+};
+
+static int __init wm8505fb_init(void)
+{
+        return platform_driver_register(&wm8505fb_driver);
+}
+
+static void __exit wm8505fb_exit(void)
+{
+        platform_driver_unregister(&wm8505fb_driver);
+}
+
+module_init(wm8505fb_init);
+module_exit(wm8505fb_exit);
+
+MODULE_AUTHOR("Ed Spiridonov <edo.rus@gmail.com>");
+MODULE_DESCRIPTION("Framebuffer driver for WMT WM8505");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/wm8505fb_regs.h b/drivers/video/wm8505fb_regs.h
new file mode 100644
index 000000000000..4dd41668c6d1
--- /dev/null
+++ b/drivers/video/wm8505fb_regs.h
@@ -0,0 +1,76 @@
+/*
+ *  GOVR registers list for WM8505 chips
+ *
+ *  Copyright (C) 2010 Ed Spiridonov <edo.rus@gmail.com>
+ *   Based on VIA/WonderMedia wm8510-govrh-reg.h
+ *   http://github.com/projectgus/kernel_wm8505/blob/wm8505_2.6.29/
+ *         drivers/video/wmt/register/wm8510/wm8510-govrh-reg.h
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#ifndef _WM8505FB_REGS_H
+#define _WM8505FB_REGS_H
+
+/*
+ * Color space select register, default value 0x1c
+ *   BIT0 GOVRH_DVO_YUV2RGB_ENABLE
+ *   BIT1 GOVRH_VGA_YUV2RGB_ENABLE
+ *   BIT2 GOVRH_RGB_MODE
+ *   BIT3 GOVRH_DAC_CLKINV
+ *   BIT4 GOVRH_BLANK_ZERO
+ */
+#define WMT_GOVR_COLORSPACE     0x1e4
+/*
+ * Another colorspace select register, default value 1
+ *   BIT0 GOVRH_DVO_RGB
+ *   BIT1 GOVRH_DVO_YUV422
+ */
+#define WMT_GOVR_COLORSPACE1     0x30
+
+#define WMT_GOVR_CONTRAST       0x1b8
+#define WMT_GOVR_BRGHTNESS      0x1bc /* incompatible with RGB? */
+
+/* Framubeffer address */
+#define WMT_GOVR_FBADDR          0x90
+#define WMT_GOVR_FBADDR1         0x94 /* UV offset in YUV mode */
+
+/* Offset of visible window */
+#define WMT_GOVR_XPAN            0xa4
+#define WMT_GOVR_YPAN            0xa0
+
+#define WMT_GOVR_XRES            0x98
+#define WMT_GOVR_XRES_VIRTUAL    0x9c
+
+#define WMT_GOVR_MIF_ENABLE      0x80
+#define WMT_GOVR_FHI             0xa8
+#define WMT_GOVR_REG_UPDATE      0xe4
+
+/*
+ *   BIT0 GOVRH_DVO_OUTWIDTH
+ *   BIT1 GOVRH_DVO_SYNC_POLAR
+ *   BIT2 GOVRH_DVO_ENABLE
+ */
+#define WMT_GOVR_DVO_SET        0x148
+
+/* Timing generator? */
+#define WMT_GOVR_TG             0x100
+
+/* Timings */
+#define WMT_GOVR_TIMING_H_ALL   0x108
+#define WMT_GOVR_TIMING_V_ALL   0x10c
+#define WMT_GOVR_TIMING_V_START 0x110
+#define WMT_GOVR_TIMING_V_END   0x114
+#define WMT_GOVR_TIMING_H_START 0x118
+#define WMT_GOVR_TIMING_H_END   0x11c
+#define WMT_GOVR_TIMING_V_SYNC  0x128
+#define WMT_GOVR_TIMING_H_SYNC  0x12c
+
+#endif /* _WM8505FB_REGS_H */
diff --git a/drivers/video/wmt_ge_rops.c b/drivers/video/wmt_ge_rops.c
new file mode 100644
index 000000000000..45832b7ef7d2
--- /dev/null
+++ b/drivers/video/wmt_ge_rops.c
@@ -0,0 +1,186 @@
+/*
+ *  linux/drivers/video/wmt_ge_rops.c
+ *
+ *  Accelerators for raster operations using WonderMedia Graphics Engine
+ *
+ *  Copyright (C) 2010 Alexey Charkov <alchark@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/fb.h>
+#include <linux/platform_device.h>
+#include "fb_draw.h"
+
+#define GE_COMMAND_OFF          0x00
+#define GE_DEPTH_OFF            0x04
+#define GE_HIGHCOLOR_OFF        0x08
+#define GE_ROPCODE_OFF          0x14
+#define GE_FIRE_OFF             0x18
+#define GE_SRCBASE_OFF          0x20
+#define GE_SRCDISPW_OFF         0x24
+#define GE_SRCDISPH_OFF         0x28
+#define GE_SRCAREAX_OFF         0x2c
+#define GE_SRCAREAY_OFF         0x30
+#define GE_SRCAREAW_OFF         0x34
+#define GE_SRCAREAH_OFF         0x38
+#define GE_DESTBASE_OFF         0x3c
+#define GE_DESTDISPW_OFF        0x40
+#define GE_DESTDISPH_OFF        0x44
+#define GE_DESTAREAX_OFF        0x48
+#define GE_DESTAREAY_OFF        0x4c
+#define GE_DESTAREAW_OFF        0x50
+#define GE_DESTAREAH_OFF        0x54
+#define GE_PAT0C_OFF            0x88    /* Pattern 0 color */
+#define GE_ENABLE_OFF           0xec
+#define GE_INTEN_OFF            0xf0
+#define GE_STATUS_OFF           0xf8
+
+static void __iomem *regbase;
+
+void wmt_ge_fillrect(struct fb_info *p, const struct fb_fillrect *rect)
+{
+        unsigned long fg, pat;
+
+        if (p->state != FBINFO_STATE_RUNNING)
+                return;
+
+        if (p->fix.visual == FB_VISUAL_TRUECOLOR ||
+            p->fix.visual == FB_VISUAL_DIRECTCOLOR)
+                fg = ((u32 *) (p->pseudo_palette))[rect->color];
+        else
+                fg = rect->color;
+
+        pat = pixel_to_pat(p->var.bits_per_pixel, fg);
+
+        if (p->fbops->fb_sync)
+                p->fbops->fb_sync(p);
+
+        writel(p->var.bits_per_pixel == 32 ? 3 :
+              (p->var.bits_per_pixel == 8 ? 0 : 1), regbase + GE_DEPTH_OFF);
+        writel(p->var.bits_per_pixel == 15 ? 1 : 0, regbase + GE_HIGHCOLOR_OFF);
+        writel(p->fix.smem_start, regbase + GE_DESTBASE_OFF);
+        writel(p->var.xres_virtual - 1, regbase + GE_DESTDISPW_OFF);
+        writel(p->var.yres_virtual - 1, regbase + GE_DESTDISPH_OFF);
+        writel(rect->dx, regbase + GE_DESTAREAX_OFF);
+        writel(rect->dy, regbase + GE_DESTAREAY_OFF);
+        writel(rect->width - 1, regbase + GE_DESTAREAW_OFF);
+        writel(rect->height - 1, regbase + GE_DESTAREAH_OFF);
+
+        writel(pat, regbase + GE_PAT0C_OFF);
+        writel(1, regbase + GE_COMMAND_OFF);
+        writel(rect->rop == ROP_XOR ? 0x5a : 0xf0, regbase + GE_ROPCODE_OFF);
+        writel(1, regbase + GE_FIRE_OFF);
+}
+EXPORT_SYMBOL_GPL(wmt_ge_fillrect);
+
+void wmt_ge_copyarea(struct fb_info *p, const struct fb_copyarea *area)
+{
+        if (p->state != FBINFO_STATE_RUNNING)
+                return;
+
+        if (p->fbops->fb_sync)
+                p->fbops->fb_sync(p);
+
+        writel(p->var.bits_per_pixel > 16 ? 3 :
+              (p->var.bits_per_pixel > 8 ? 1 : 0), regbase + GE_DEPTH_OFF);
+
+        writel(p->fix.smem_start, regbase + GE_SRCBASE_OFF);
+        writel(p->var.xres_virtual - 1, regbase + GE_SRCDISPW_OFF);
+        writel(p->var.yres_virtual - 1, regbase + GE_SRCDISPH_OFF);
+        writel(area->sx, regbase + GE_SRCAREAX_OFF);
+        writel(area->sy, regbase + GE_SRCAREAY_OFF);
+        writel(area->width - 1, regbase + GE_SRCAREAW_OFF);
+        writel(area->height - 1, regbase + GE_SRCAREAH_OFF);
+
+        writel(p->fix.smem_start, regbase + GE_DESTBASE_OFF);
+        writel(p->var.xres_virtual - 1, regbase + GE_DESTDISPW_OFF);
+        writel(p->var.yres_virtual - 1, regbase + GE_DESTDISPH_OFF);
+        writel(area->dx, regbase + GE_DESTAREAX_OFF);
+        writel(area->dy, regbase + GE_DESTAREAY_OFF);
+        writel(area->width - 1, regbase + GE_DESTAREAW_OFF);
+        writel(area->height - 1, regbase + GE_DESTAREAH_OFF);
+
+        writel(0xcc, regbase + GE_ROPCODE_OFF);
+        writel(1, regbase + GE_COMMAND_OFF);
+        writel(1, regbase + GE_FIRE_OFF);
+}
+EXPORT_SYMBOL_GPL(wmt_ge_copyarea);
+
+int wmt_ge_sync(struct fb_info *p)
+{
+        int loops = 5000000;
+        while ((readl(regbase + GE_STATUS_OFF) & 4) && --loops)
+                cpu_relax();
+        return loops > 0 ? 0 : -EBUSY;
+}
+EXPORT_SYMBOL_GPL(wmt_ge_sync);
+
+static int __devinit wmt_ge_rops_probe(struct platform_device *pdev)
+{
+        struct resource *res;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (res == NULL) {
+                dev_err(&pdev->dev, "no I/O memory resource defined\n");
+                return -ENODEV;
+        }
+
+        /* Only one ROP engine is presently supported. */
+        if (unlikely(regbase)) {
+                WARN_ON(1);
+                return -EBUSY;
+        }
+
+        regbase = ioremap(res->start, resource_size(res));
+        if (regbase == NULL) {
+                dev_err(&pdev->dev, "failed to map I/O memory\n");
+                return -EBUSY;
+        }
+
+        writel(1, regbase + GE_ENABLE_OFF);
+        printk(KERN_INFO "Enabled support for WMT GE raster acceleration\n");
+
+        return 0;
+}
+
+static int __devexit wmt_ge_rops_remove(struct platform_device *pdev)
+{
+        iounmap(regbase);
+        return 0;
+}
+
+static struct platform_driver wmt_ge_rops_driver = {
+        .probe          = wmt_ge_rops_probe,
+        .remove         = __devexit_p(wmt_ge_rops_remove),
+        .driver         = {
+                .owner  = THIS_MODULE,
+                .name   = "wmt_ge_rops",
+        },
+};
+
+static int __init wmt_ge_rops_init(void)
+{
+        return platform_driver_register(&wmt_ge_rops_driver);
+}
+
+static void __exit wmt_ge_rops_exit(void)
+{
+        platform_driver_unregister(&wmt_ge_rops_driver);
+}
+
+module_init(wmt_ge_rops_init);
+module_exit(wmt_ge_rops_exit);
+
+MODULE_AUTHOR("Alexey Charkov <alchark@gmail.com");
+MODULE_DESCRIPTION("Accelerators for raster operations using "
+                   "WonderMedia Graphics Engine");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/wmt_ge_rops.h b/drivers/video/wmt_ge_rops.h
new file mode 100644
index 000000000000..87380751a443
--- /dev/null
+++ b/drivers/video/wmt_ge_rops.h
@@ -0,0 +1,5 @@
+extern void wmt_ge_fillrect(struct fb_info *info,
+                            const struct fb_fillrect *rect);
+extern void wmt_ge_copyarea(struct fb_info *info,
+                            const struct fb_copyarea *area);
+extern int wmt_ge_sync(struct fb_info *info);
diff --git a/drivers/video/xen-fbfront.c b/drivers/video/xen-fbfront.c
index 7c7f42a12796..beac52fc1c0e 100644
--- a/drivers/video/xen-fbfront.c
+++ b/drivers/video/xen-fbfront.c
@@ -395,10 +395,9 @@ static int __devinit xenfb_probe(struct xenbus_device *dev,
         spin_lock_init(&info->dirty_lock);
         spin_lock_init(&info->resize_lock);
 
-        info->fb = vmalloc(fb_size);
+        info->fb = vzalloc(fb_size);
         if (info->fb == NULL)
                 goto error_nomem;
-        memset(info->fb, 0, fb_size);
 
         info->nr_pages = (fb_size + PAGE_SIZE - 1) >> PAGE_SHIFT;
 
@@ -491,12 +490,12 @@ xenfb_make_preferred_console(void)
         if (console_set_on_cmdline)
                 return;
 
-        acquire_console_sem();
-        for (c = console_drivers; c; c = c->next) {
+        console_lock();
+        for_each_console(c) {
                 if (!strcmp(c->name, "tty") && c->index == 0)
                         break;
         }
-        release_console_sem();
+        console_unlock();
         if (c) {
                 unregister_console(c);
                 c->flags |= CON_CONSDEV;
@@ -562,26 +561,24 @@ static void xenfb_init_shared_page(struct xenfb_info *info,
 static int xenfb_connect_backend(struct xenbus_device *dev,
                                  struct xenfb_info *info)
 {
-        int ret, evtchn;
+        int ret, evtchn, irq;
         struct xenbus_transaction xbt;
 
         ret = xenbus_alloc_evtchn(dev, &evtchn);
         if (ret)
                 return ret;
-        ret = bind_evtchn_to_irqhandler(evtchn, xenfb_event_handler,
+        irq = bind_evtchn_to_irqhandler(evtchn, xenfb_event_handler,
                                         0, dev->devicetype, info);
-        if (ret < 0) {
+        if (irq < 0) {
                 xenbus_free_evtchn(dev, evtchn);
                 xenbus_dev_fatal(dev, ret, "bind_evtchn_to_irqhandler");
-                return ret;
+                return irq;
         }
-        info->irq = ret;
-
  again:
         ret = xenbus_transaction_start(&xbt);
         if (ret) {
                 xenbus_dev_fatal(dev, ret, "starting transaction");
-                return ret;
+                goto unbind_irq;
         }
         ret = xenbus_printf(xbt, dev->nodename, "page-ref", "%lu",
                             virt_to_mfn(info->page));
@@ -603,20 +600,25 @@ static int xenfb_connect_backend(struct xenbus_device *dev,
                 if (ret == -EAGAIN)
                         goto again;
                 xenbus_dev_fatal(dev, ret, "completing transaction");
-                return ret;
+                goto unbind_irq;
         }
 
         xenbus_switch_state(dev, XenbusStateInitialised);
+        info->irq = irq;
         return 0;
 
  error_xenbus:
         xenbus_transaction_end(xbt, 1);
         xenbus_dev_fatal(dev, ret, "writing xenstore");
+ unbind_irq:
+        unbind_from_irqhandler(irq, info);
         return ret;
 }
 
 static void xenfb_disconnect_backend(struct xenfb_info *info)
 {
+        /* Prevent xenfb refresh */
+        info->update_wanted = 0;
         if (info->irq >= 0)
                 unbind_from_irqhandler(info->irq, info);
         info->irq = -1;
@@ -631,6 +633,8 @@ static void xenfb_backend_changed(struct xenbus_device *dev,
         switch (backend_state) {
         case XenbusStateInitialising:
         case XenbusStateInitialised:
+        case XenbusStateReconfiguring:
+        case XenbusStateReconfigured:
         case XenbusStateUnknown:
         case XenbusStateClosed:
                 break;
diff --git a/drivers/video/xilinxfb.c b/drivers/video/xilinxfb.c
index 0c9ce88e95e8..77dea015ff69 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);
@@ -396,22 +404,19 @@ static int xilinxfb_release(struct device *dev)
  * OF bus binding
  */
 
-static int __devinit
-xilinxfb_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit xilinxfb_of_probe(struct platform_device *op)
 {
         const u32 *prop;
         u32 *p;
         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!) */
         pdata = xilinx_fb_default_pdata;
 
-        dev_dbg(&op->dev, "xilinxfb_of_probe(%p, %p)\n", op, match);
-
         /* Allocate the driver data region */
         drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
         if (!drvdata) {
@@ -437,7 +442,10 @@ xilinxfb_of_probe(struct platform_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 +455,7 @@ xilinxfb_of_probe(struct platform_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)) {
@@ -493,7 +502,7 @@ static struct of_device_id xilinxfb_of_match[] __devinitdata = {
 };
 MODULE_DEVICE_TABLE(of, xilinxfb_of_match);
 
-static struct of_platform_driver xilinxfb_of_driver = {
+static struct platform_driver xilinxfb_of_driver = {
         .probe = xilinxfb_of_probe,
         .remove = __devexit_p(xilinxfb_of_remove),
         .driver = {
@@ -511,13 +520,13 @@ static struct of_platform_driver xilinxfb_of_driver = {
 static int __init
 xilinxfb_init(void)
 {
-        return of_register_platform_driver(&xilinxfb_of_driver);
+        return platform_driver_register(&xilinxfb_of_driver);
 }
 
 static void __exit
 xilinxfb_cleanup(void)
 {
-        of_unregister_platform_driver(&xilinxfb_of_driver);
+        platform_driver_unregister(&xilinxfb_of_driver);
 }
 
 module_init(xilinxfb_init);