142 files changed, 36836 insertions, 1701 deletions

diff --git a/Documentation/devicetree/bindings/display/simple-framebuffer.txt b/Documentation/devicetree/bindings/display/simple-framebuffer.txt
index 4474ef6e0b95..8c9e9f515c87 100644
--- a/Documentation/devicetree/bindings/display/simple-framebuffer.txt
+++ b/Documentation/devicetree/bindings/display/simple-framebuffer.txt
@@ -47,10 +47,14 @@ Required properties:
   - a8b8g8r8 (32-bit pixels, d[31:24]=a, d[23:16]=b, d[15:8]=g, d[7:0]=r).
 
 Optional properties:
-- clocks : List of clocks used by the framebuffer. Clocks listed here
-           are expected to already be configured correctly. The OS must
-           ensure these clocks are not modified or disabled while the
-           simple framebuffer remains active.
+- clocks : List of clocks used by the framebuffer.
+- *-supply : Any number of regulators used by the framebuffer. These should
+             be named according to the names in the device's design.
+
+  The above resources are expected to already be configured correctly.
+  The OS must ensure they are not modified or disabled while the simple
+  framebuffer remains active.
+
 - display : phandle pointing to the primary display hardware node
 
 Example:
@@ -68,6 +72,7 @@ chosen {
                 stride = <(1600 * 2)>;
                 format = "r5g6b5";
                 clocks = <&ahb_gates 36>, <&ahb_gates 43>, <&ahb_gates 44>;
+                lcd-supply = <&reg_dc1sw>;
                 display = <&lcdc0>;
         };
         stdout-path = "display0";
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index f858aa25fbb2..8727d841bfd2 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -62,7 +62,7 @@ obj-$(CONFIG_DRM_ARMADA) += armada/
 obj-$(CONFIG_DRM_ATMEL_HLCDC)   += atmel-hlcdc/
 obj-$(CONFIG_DRM_RCAR_DU) += rcar-du/
 obj-$(CONFIG_DRM_SHMOBILE) +=shmobile/
-obj-$(CONFIG_DRM_OMAP)  += omapdrm/
+obj-y                   += omapdrm/
 obj-y                   += tilcdc/
 obj-$(CONFIG_DRM_QXL) += qxl/
 obj-$(CONFIG_DRM_BOCHS) += bochs/
diff --git a/drivers/gpu/drm/omapdrm/Kconfig b/drivers/gpu/drm/omapdrm/Kconfig
index 6c220cd3497a..336ad4de9981 100644
--- a/drivers/gpu/drm/omapdrm/Kconfig
+++ b/drivers/gpu/drm/omapdrm/Kconfig
@@ -1,9 +1,8 @@
-
 config DRM_OMAP
         tristate "OMAP DRM"
         depends on DRM
         depends on ARCH_OMAP2PLUS || ARCH_MULTIPLATFORM
-        depends on OMAP2_DSS
+        select OMAP2_DSS
         select DRM_KMS_HELPER
         select DRM_KMS_FB_HELPER
         select FB_SYS_FILLRECT
@@ -14,13 +13,18 @@ config DRM_OMAP
         help
           DRM display driver for OMAP2/3/4 based boards.
 
+if DRM_OMAP
+
 config DRM_OMAP_NUM_CRTCS
         int "Number of CRTCs"
         range 1 10
         default 1  if ARCH_OMAP2 || ARCH_OMAP3
         default 2  if ARCH_OMAP4
-        depends on DRM_OMAP
         help
           Select the number of video overlays which can be used as framebuffers.
           The remaining overlays are reserved for video.
 
+source "drivers/gpu/drm/omapdrm/dss/Kconfig"
+source "drivers/gpu/drm/omapdrm/displays/Kconfig"
+
+endif
diff --git a/drivers/gpu/drm/omapdrm/Makefile b/drivers/gpu/drm/omapdrm/Makefile
index 368c1ec6805a..fe4c2228bc18 100644
--- a/drivers/gpu/drm/omapdrm/Makefile
+++ b/drivers/gpu/drm/omapdrm/Makefile
@@ -3,6 +3,9 @@
 # Direct Rendering Infrastructure (DRI)
 #
 
+obj-y += dss/
+obj-y += displays/
+
 ccflags-y := -Iinclude/drm -Werror
 omapdrm-y := omap_drv.o \
         omap_irq.o \
diff --git a/drivers/video/fbdev/omap2/displays-new/Kconfig b/drivers/gpu/drm/omapdrm/displays/Kconfig
index 574710141a61..2a618afe0f53 100644
--- a/drivers/video/fbdev/omap2/displays-new/Kconfig
+++ b/drivers/gpu/drm/omapdrm/displays/Kconfig
@@ -1,5 +1,4 @@
-menu "OMAP Display Device Drivers (new device model)"
-        depends on OMAP2_DSS
+menu "OMAPDRM External Display Device Drivers"
 
 config DISPLAY_ENCODER_OPA362
         tristate "OPA362 external analog amplifier"
diff --git a/drivers/video/fbdev/omap2/displays-new/Makefile b/drivers/gpu/drm/omapdrm/displays/Makefile
index 9aa176bfbf2e..9aa176bfbf2e 100644
--- a/drivers/video/fbdev/omap2/displays-new/Makefile
+++ b/drivers/gpu/drm/omapdrm/displays/Makefile
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c b/drivers/gpu/drm/omapdrm/displays/connector-analog-tv.c
index 8511c648a15c..8511c648a15c 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c
+++ b/drivers/gpu/drm/omapdrm/displays/connector-analog-tv.c
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-dvi.c b/drivers/gpu/drm/omapdrm/displays/connector-dvi.c
index d811e6dcaef7..d811e6dcaef7 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-dvi.c
+++ b/drivers/gpu/drm/omapdrm/displays/connector-dvi.c
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c b/drivers/gpu/drm/omapdrm/displays/connector-hdmi.c
index 6ee4129bc0c0..6ee4129bc0c0 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
+++ b/drivers/gpu/drm/omapdrm/displays/connector-hdmi.c
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c b/drivers/gpu/drm/omapdrm/displays/encoder-opa362.c
index 8c246c213e06..8c246c213e06 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c
+++ b/drivers/gpu/drm/omapdrm/displays/encoder-opa362.c
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c b/drivers/gpu/drm/omapdrm/displays/encoder-tfp410.c
index d9048b3df495..d9048b3df495 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c
+++ b/drivers/gpu/drm/omapdrm/displays/encoder-tfp410.c
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c b/drivers/gpu/drm/omapdrm/displays/encoder-tpd12s015.c
index 990af6baeb0f..990af6baeb0f 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c
+++ b/drivers/gpu/drm/omapdrm/displays/encoder-tpd12s015.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c b/drivers/gpu/drm/omapdrm/displays/panel-dpi.c
index f7be3489f744..e780fd4f8b46 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-dpi.c
@@ -83,8 +83,7 @@ static int panel_dpi_enable(struct omap_dss_device *dssdev)
         if (r)
                 return r;
 
-        if (ddata->enable_gpio)
-                gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+        gpiod_set_value_cansleep(ddata->enable_gpio, 1);
 
         if (gpio_is_valid(ddata->backlight_gpio))
                 gpio_set_value_cansleep(ddata->backlight_gpio, 1);
@@ -102,12 +101,11 @@ static void panel_dpi_disable(struct omap_dss_device *dssdev)
         if (!omapdss_device_is_enabled(dssdev))
                 return;
 
-        if (ddata->enable_gpio)
-                gpiod_set_value_cansleep(ddata->enable_gpio, 0);
-
         if (gpio_is_valid(ddata->backlight_gpio))
                 gpio_set_value_cansleep(ddata->backlight_gpio, 0);
 
+        gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
         in->ops.dpi->disable(in);
 
         dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c b/drivers/gpu/drm/omapdrm/displays/panel-dsi-cm.c
index 3414c2609320..3414c2609320 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-dsi-cm.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c b/drivers/gpu/drm/omapdrm/displays/panel-lgphilips-lb035q02.c
index 18eb60e9c9ec..18eb60e9c9ec 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-lgphilips-lb035q02.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c b/drivers/gpu/drm/omapdrm/displays/panel-nec-nl8048hl11.c
index 8a928c9a2fc9..8a928c9a2fc9 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-nec-nl8048hl11.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c b/drivers/gpu/drm/omapdrm/displays/panel-sharp-ls037v7dw01.c
index abfd1f6e3327..abfd1f6e3327 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-sharp-ls037v7dw01.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-sony-acx565akm.c b/drivers/gpu/drm/omapdrm/displays/panel-sony-acx565akm.c
index 31efcca801bd..31efcca801bd 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sony-acx565akm.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-sony-acx565akm.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td028ttec1.c
index 4d657f3ab679..4d657f3ab679 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td028ttec1.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td043mtea1.c
index 68e3b68a2920..68e3b68a2920 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td043mtea1.c
diff --git a/drivers/video/fbdev/omap2/dss/Kconfig b/drivers/gpu/drm/omapdrm/dss/Kconfig
index d1fa730c7d54..d1fa730c7d54 100644
--- a/drivers/video/fbdev/omap2/dss/Kconfig
+++ b/drivers/gpu/drm/omapdrm/dss/Kconfig
diff --git a/drivers/video/fbdev/omap2/dss/Makefile b/drivers/gpu/drm/omapdrm/dss/Makefile
index b5136d3d4b77..b5136d3d4b77 100644
--- a/drivers/video/fbdev/omap2/dss/Makefile
+++ b/drivers/gpu/drm/omapdrm/dss/Makefile
diff --git a/drivers/video/fbdev/omap2/dss/apply.c b/drivers/gpu/drm/omapdrm/dss/apply.c
index 663ccc3bf4e5..663ccc3bf4e5 100644
--- a/drivers/video/fbdev/omap2/dss/apply.c
+++ b/drivers/gpu/drm/omapdrm/dss/apply.c
diff --git a/drivers/video/fbdev/omap2/dss/core.c b/drivers/gpu/drm/omapdrm/dss/core.c
index 54eeb507f9b3..54eeb507f9b3 100644
--- a/drivers/video/fbdev/omap2/dss/core.c
+++ b/drivers/gpu/drm/omapdrm/dss/core.c
diff --git a/drivers/video/fbdev/omap2/dss/dispc-compat.c b/drivers/gpu/drm/omapdrm/dss/dispc-compat.c
index 633c461fbc6e..0918b3bfe82a 100644
--- a/drivers/video/fbdev/omap2/dss/dispc-compat.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc-compat.c
@@ -476,7 +476,7 @@ static void dispc_mgr_disable_lcd_out(enum omap_channel channel)
         int r;
         u32 irq;
 
-        if (dispc_mgr_is_enabled(channel) == false)
+        if (!dispc_mgr_is_enabled(channel))
                 return;
 
         /*
@@ -524,7 +524,7 @@ static void dispc_mgr_enable_digit_out(void)
         int r;
         u32 irq_mask;
 
-        if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT) == true)
+        if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
                 return;
 
         /*
@@ -562,7 +562,7 @@ static void dispc_mgr_disable_digit_out(void)
         u32 irq_mask;
         int num_irqs;
 
-        if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT) == false)
+        if (!dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
                 return;
 
         /*
diff --git a/drivers/video/fbdev/omap2/dss/dispc-compat.h b/drivers/gpu/drm/omapdrm/dss/dispc-compat.h
index 14a69b3d4fb0..14a69b3d4fb0 100644
--- a/drivers/video/fbdev/omap2/dss/dispc-compat.h
+++ b/drivers/gpu/drm/omapdrm/dss/dispc-compat.h
diff --git a/drivers/video/fbdev/omap2/dss/dispc.c b/drivers/gpu/drm/omapdrm/dss/dispc.c
index be716c9ffb88..6b50476ec669 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc.c
@@ -99,6 +99,11 @@ struct dispc_features {
 
         /* PIXEL_INC is not added to the last pixel of a line */
         bool last_pixel_inc_missing:1;
+
+        /* POL_FREQ has ALIGN bit */
+        bool supports_sync_align:1;
+
+        bool has_writeback:1;
 };
 
 #define DISPC_MAX_NR_FIFOS 5
@@ -246,6 +251,11 @@ struct color_conv_coef {
         int full_range;
 };
 
+static unsigned long dispc_fclk_rate(void);
+static unsigned long dispc_core_clk_rate(void);
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
+
 static unsigned long dispc_plane_pclk_rate(enum omap_plane plane);
 static unsigned long dispc_plane_lclk_rate(enum omap_plane plane);
 
@@ -571,7 +581,7 @@ EXPORT_SYMBOL(dispc_mgr_go_busy);
 
 void dispc_mgr_go(enum omap_channel channel)
 {
-        WARN_ON(dispc_mgr_is_enabled(channel) == false);
+        WARN_ON(!dispc_mgr_is_enabled(channel));
         WARN_ON(dispc_mgr_go_busy(channel));
 
         DSSDBG("GO %s\n", mgr_desc[channel].name);
@@ -707,19 +717,20 @@ static void dispc_setup_color_conv_coef(void)
 {
         int i;
         int num_ovl = dss_feat_get_num_ovls();
-        int num_wb = dss_feat_get_num_wbs();
         const struct color_conv_coef ctbl_bt601_5_ovl = {
+                /* YUV -> RGB */
                 298, 409, 0, 298, -208, -100, 298, 0, 517, 0,
         };
         const struct color_conv_coef ctbl_bt601_5_wb = {
-                66, 112, -38, 129, -94, -74, 25, -18, 112, 0,
+                /* RGB -> YUV */
+                66, 129, 25, 112, -94, -18, -38, -74, 112, 0,
         };
 
         for (i = 1; i < num_ovl; i++)
                 dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_ovl);
 
-        for (; i < num_wb; i++)
-                dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_wb);
+        if (dispc.feat->has_writeback)
+                dispc_ovl_write_color_conv_coef(OMAP_DSS_WB, &ctbl_bt601_5_wb);
 }
 
 static void dispc_ovl_set_ba0(enum omap_plane plane, u32 paddr)
@@ -970,6 +981,10 @@ void dispc_ovl_set_channel_out(enum omap_plane plane, enum omap_channel channel)
                                 return;
                         }
                         break;
+                case OMAP_DSS_CHANNEL_WB:
+                        chan = 0;
+                        chan2 = 3;
+                        break;
                 default:
                         BUG();
                         return;
@@ -988,7 +1003,6 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
 {
         int shift;
         u32 val;
-        enum omap_channel channel;
 
         switch (plane) {
         case OMAP_DSS_GFX:
@@ -1006,23 +1020,23 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
 
         val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 
-        if (dss_has_feature(FEAT_MGR_LCD3)) {
-                if (FLD_GET(val, 31, 30) == 0)
-                        channel = FLD_GET(val, shift, shift);
-                else if (FLD_GET(val, 31, 30) == 1)
-                        channel = OMAP_DSS_CHANNEL_LCD2;
-                else
-                        channel = OMAP_DSS_CHANNEL_LCD3;
-        } else if (dss_has_feature(FEAT_MGR_LCD2)) {
-                if (FLD_GET(val, 31, 30) == 0)
-                        channel = FLD_GET(val, shift, shift);
-                else
-                        channel = OMAP_DSS_CHANNEL_LCD2;
-        } else {
-                channel = FLD_GET(val, shift, shift);
-        }
+        if (FLD_GET(val, shift, shift) == 1)
+                return OMAP_DSS_CHANNEL_DIGIT;
+
+        if (!dss_has_feature(FEAT_MGR_LCD2))
+                return OMAP_DSS_CHANNEL_LCD;
 
-        return channel;
+        switch (FLD_GET(val, 31, 30)) {
+        case 0:
+        default:
+                return OMAP_DSS_CHANNEL_LCD;
+        case 1:
+                return OMAP_DSS_CHANNEL_LCD2;
+        case 2:
+                return OMAP_DSS_CHANNEL_LCD3;
+        case 3:
+                return OMAP_DSS_CHANNEL_WB;
+        }
 }
 
 void dispc_wb_set_channel_in(enum dss_writeback_channel channel)
@@ -1050,6 +1064,8 @@ static void dispc_configure_burst_sizes(void)
         /* Configure burst size always to maximum size */
         for (i = 0; i < dss_feat_get_num_ovls(); ++i)
                 dispc_ovl_set_burst_size(i, burst_size);
+        if (dispc.feat->has_writeback)
+                dispc_ovl_set_burst_size(OMAP_DSS_WB, burst_size);
 }
 
 static u32 dispc_ovl_get_burst_size(enum omap_plane plane)
@@ -1196,6 +1212,17 @@ static void dispc_init_fifos(void)
 
                 dispc_ovl_set_fifo_threshold(i, low, high);
         }
+
+        if (dispc.feat->has_writeback) {
+                u32 low, high;
+                const bool use_fifomerge = false;
+                const bool manual_update = false;
+
+                dispc_ovl_compute_fifo_thresholds(OMAP_DSS_WB, &low, &high,
+                        use_fifomerge, manual_update);
+
+                dispc_ovl_set_fifo_threshold(OMAP_DSS_WB, low, high);
+        }
 }
 
 static u32 dispc_ovl_get_fifo_size(enum omap_plane plane)
@@ -1248,7 +1275,6 @@ void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
                         plane != OMAP_DSS_WB)
                 dispc_write_reg(DISPC_OVL_PRELOAD(plane), min(high, 0xfffu));
 }
-EXPORT_SYMBOL(dispc_ovl_set_fifo_threshold);
 
 void dispc_enable_fifomerge(bool enable)
 {
@@ -1307,7 +1333,6 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
                 *fifo_high = total_fifo_size - buf_unit;
         }
 }
-EXPORT_SYMBOL(dispc_ovl_compute_fifo_thresholds);
 
 static void dispc_ovl_set_mflag(enum omap_plane plane, bool enable)
 {
@@ -1364,6 +1389,25 @@ static void dispc_init_mflag(void)
 
                 dispc_ovl_set_mflag_threshold(i, low, high);
         }
+
+        if (dispc.feat->has_writeback) {
+                u32 size = dispc_ovl_get_fifo_size(OMAP_DSS_WB);
+                u32 unit = dss_feat_get_buffer_size_unit();
+                u32 low, high;
+
+                dispc_ovl_set_mflag(OMAP_DSS_WB, true);
+
+                /*
+                 * Simulation team suggests below thesholds:
+                 * HT = fifosize * 5 / 8;
+                 * LT = fifosize * 4 / 8;
+                 */
+
+                low = size * 4 / 8 / unit;
+                high = size * 5 / 8 / unit;
+
+                dispc_ovl_set_mflag_threshold(OMAP_DSS_WB, low, high);
+        }
 }
 
 static void dispc_ovl_set_fir(enum omap_plane plane,
@@ -2438,7 +2482,7 @@ static int dispc_ovl_calc_scaling(unsigned long pclk, unsigned long lclk,
         if (width == out_width && height == out_height)
                 return 0;
 
-        if (pclk == 0 || mgr_timings->pixelclock == 0) {
+        if (!mem_to_mem && (pclk == 0 || mgr_timings->pixelclock == 0)) {
                 DSSERR("cannot calculate scaling settings: pclk is zero\n");
                 return -EINVAL;
         }
@@ -2816,8 +2860,25 @@ int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
         l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
         l = FLD_MOD(l, truncation, 10, 10);     /* TRUNCATIONENABLE */
         l = FLD_MOD(l, mem_to_mem, 19, 19);     /* WRITEBACKMODE */
+        if (mem_to_mem)
+                l = FLD_MOD(l, 1, 26, 24);      /* CAPTUREMODE */
+        else
+                l = FLD_MOD(l, 0, 26, 24);      /* CAPTUREMODE */
         dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
 
+        if (mem_to_mem) {
+                /* WBDELAYCOUNT */
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 7, 0);
+        } else {
+                int wbdelay;
+
+                wbdelay = min(mgr_timings->vfp + mgr_timings->vsw +
+                        mgr_timings->vbp, 255);
+
+                /* WBDELAYCOUNT */
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), wbdelay, 7, 0);
+        }
+
         return r;
 }
 
@@ -2896,7 +2957,7 @@ static void dispc_mgr_set_lcd_type_tft(enum omap_channel channel)
         mgr_fld_write(channel, DISPC_MGR_FLD_STNTFT, 1);
 }
 
-void dispc_set_loadmode(enum omap_dss_load_mode mode)
+static void dispc_set_loadmode(enum omap_dss_load_mode mode)
 {
         REG_FLD_MOD(DISPC_CONFIG, mode, 2, 1);
 }
@@ -3163,6 +3224,10 @@ static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
                 FLD_VAL(hs, 13, 13) |
                 FLD_VAL(vs, 12, 12);
 
+        /* always set ALIGN bit when available */
+        if (dispc.feat->supports_sync_align)
+                l |= (1 << 18);
+
         dispc_write_reg(DISPC_POL_FREQ(channel), l);
 
         if (dispc.syscon_pol) {
@@ -3220,7 +3285,7 @@ void dispc_mgr_set_timings(enum omap_channel channel,
 
                 DSSDBG("hsync %luHz, vsync %luHz\n", ht, vt);
         } else {
-                if (t.interlace == true)
+                if (t.interlace)
                         t.y_res /= 2;
         }
 
@@ -3237,7 +3302,7 @@ static void dispc_mgr_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
         dispc_write_reg(DISPC_DIVISORo(channel),
                         FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
 
-        if (dss_has_feature(FEAT_CORE_CLK_DIV) == false &&
+        if (!dss_has_feature(FEAT_CORE_CLK_DIV) &&
                         channel == OMAP_DSS_CHANNEL_LCD)
                 dispc.core_clk_rate = dispc_fclk_rate() / lck_div;
 }
@@ -3251,7 +3316,7 @@ static void dispc_mgr_get_lcd_divisor(enum omap_channel channel, int *lck_div,
         *pck_div = FLD_GET(l, 7, 0);
 }
 
-unsigned long dispc_fclk_rate(void)
+static unsigned long dispc_fclk_rate(void)
 {
         struct dss_pll *pll;
         unsigned long r = 0;
@@ -3282,7 +3347,7 @@ unsigned long dispc_fclk_rate(void)
         return r;
 }
 
-unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
 {
         struct dss_pll *pll;
         int lcd;
@@ -3323,7 +3388,7 @@ unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
         }
 }
 
-unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
 {
         unsigned long r;
 
@@ -3348,7 +3413,7 @@ void dispc_set_tv_pclk(unsigned long pclk)
         dispc.tv_pclk_rate = pclk;
 }
 
-unsigned long dispc_core_clk_rate(void)
+static unsigned long dispc_core_clk_rate(void)
 {
         return dispc.core_clk_rate;
 }
@@ -3448,6 +3513,7 @@ static void dispc_dump_regs(struct seq_file *s)
                 [OMAP_DSS_VIDEO1]       = "VID1",
                 [OMAP_DSS_VIDEO2]       = "VID2",
                 [OMAP_DSS_VIDEO3]       = "VID3",
+                [OMAP_DSS_WB]           = "WB",
         };
         const char **p_names;
 
@@ -3554,6 +3620,35 @@ static void dispc_dump_regs(struct seq_file *s)
                         DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
         }
 
+        if (dispc.feat->has_writeback) {
+                i = OMAP_DSS_WB;
+                DUMPREG(i, DISPC_OVL_BA0);
+                DUMPREG(i, DISPC_OVL_BA1);
+                DUMPREG(i, DISPC_OVL_SIZE);
+                DUMPREG(i, DISPC_OVL_ATTRIBUTES);
+                DUMPREG(i, DISPC_OVL_FIFO_THRESHOLD);
+                DUMPREG(i, DISPC_OVL_FIFO_SIZE_STATUS);
+                DUMPREG(i, DISPC_OVL_ROW_INC);
+                DUMPREG(i, DISPC_OVL_PIXEL_INC);
+
+                if (dss_has_feature(FEAT_MFLAG))
+                        DUMPREG(i, DISPC_OVL_MFLAG_THRESHOLD);
+
+                DUMPREG(i, DISPC_OVL_FIR);
+                DUMPREG(i, DISPC_OVL_PICTURE_SIZE);
+                DUMPREG(i, DISPC_OVL_ACCU0);
+                DUMPREG(i, DISPC_OVL_ACCU1);
+                if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                        DUMPREG(i, DISPC_OVL_BA0_UV);
+                        DUMPREG(i, DISPC_OVL_BA1_UV);
+                        DUMPREG(i, DISPC_OVL_FIR2);
+                        DUMPREG(i, DISPC_OVL_ACCU2_0);
+                        DUMPREG(i, DISPC_OVL_ACCU2_1);
+                }
+                if (dss_has_feature(FEAT_ATTR2))
+                        DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
+        }
+
 #undef DISPC_REG
 #undef DUMPREG
 
@@ -3854,6 +3949,8 @@ static const struct dispc_features omap44xx_dispc_feats = {
         .num_fifos              =       5,
         .gfx_fifo_workaround    =       true,
         .set_max_preload        =       true,
+        .supports_sync_align    =       true,
+        .has_writeback          =       true,
 };
 
 static const struct dispc_features omap54xx_dispc_feats = {
@@ -3875,6 +3972,8 @@ static const struct dispc_features omap54xx_dispc_feats = {
         .gfx_fifo_workaround    =       true,
         .mstandby_workaround    =       true,
         .set_max_preload        =       true,
+        .supports_sync_align    =       true,
+        .has_writeback          =       true,
 };
 
 static int dispc_init_features(struct platform_device *pdev)
diff --git a/drivers/video/fbdev/omap2/dss/dispc.h b/drivers/gpu/drm/omapdrm/dss/dispc.h
index 3043d6e0a5f9..483744223dd1 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.h
+++ b/drivers/gpu/drm/omapdrm/dss/dispc.h
@@ -908,6 +908,8 @@ static inline u16 DISPC_MFLAG_THRESHOLD_OFFSET(enum omap_plane plane)
                 return 0x0868;
         case OMAP_DSS_VIDEO3:
                 return 0x086c;
+        case OMAP_DSS_WB:
+                return 0x0870;
         default:
                 BUG();
                 return 0;
diff --git a/drivers/video/fbdev/omap2/dss/dispc_coefs.c b/drivers/gpu/drm/omapdrm/dss/dispc_coefs.c
index 038c15b04215..038c15b04215 100644
--- a/drivers/video/fbdev/omap2/dss/dispc_coefs.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc_coefs.c
diff --git a/drivers/video/fbdev/omap2/dss/display-sysfs.c b/drivers/gpu/drm/omapdrm/dss/display-sysfs.c
index 6ad0991f8259..6ad0991f8259 100644
--- a/drivers/video/fbdev/omap2/dss/display-sysfs.c
+++ b/drivers/gpu/drm/omapdrm/dss/display-sysfs.c
diff --git a/drivers/video/fbdev/omap2/dss/display.c b/drivers/gpu/drm/omapdrm/dss/display.c
index ef5b9027985d..ef5b9027985d 100644
--- a/drivers/video/fbdev/omap2/dss/display.c
+++ b/drivers/gpu/drm/omapdrm/dss/display.c
diff --git a/drivers/video/fbdev/omap2/dss/dpi.c b/drivers/gpu/drm/omapdrm/dss/dpi.c
index fb45b6432968..7953e6a52346 100644
--- a/drivers/video/fbdev/omap2/dss/dpi.c
+++ b/drivers/gpu/drm/omapdrm/dss/dpi.c
@@ -395,7 +395,7 @@ static int dpi_display_enable(struct omap_dss_device *dssdev)
                 goto err_no_reg;
         }
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("failed to enable display: no output/manager\n");
                 r = -ENODEV;
                 goto err_no_out_mgr;
diff --git a/drivers/video/fbdev/omap2/dss/dsi.c b/drivers/gpu/drm/omapdrm/dss/dsi.c
index b3606def5b7b..43be4b2a7b05 100644
--- a/drivers/video/fbdev/omap2/dss/dsi.c
+++ b/drivers/gpu/drm/omapdrm/dss/dsi.c
@@ -144,7 +144,7 @@ struct dsi_reg { u16 module; u16 idx; };
 #define DSI_IRQ_TA_TIMEOUT      (1 << 20)
 #define DSI_IRQ_ERROR_MASK \
         (DSI_IRQ_HS_TX_TIMEOUT | DSI_IRQ_LP_RX_TIMEOUT | DSI_IRQ_SYNC_LOST | \
-        DSI_IRQ_TA_TIMEOUT | DSI_IRQ_SYNC_LOST)
+        DSI_IRQ_TA_TIMEOUT)
 #define DSI_IRQ_CHANNEL_MASK    0xf
 
 /* Virtual channel interrupts */
@@ -3833,7 +3833,7 @@ static int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
         u16 word_count;
         int r;
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("failed to enable display: no output/manager\n");
                 return -ENODEV;
         }
diff --git a/drivers/video/fbdev/omap2/dss/dss-of.c b/drivers/gpu/drm/omapdrm/dss/dss-of.c
index bf407b6ba15c..bf407b6ba15c 100644
--- a/drivers/video/fbdev/omap2/dss/dss-of.c
+++ b/drivers/gpu/drm/omapdrm/dss/dss-of.c
diff --git a/drivers/video/fbdev/omap2/dss/dss.c b/drivers/gpu/drm/omapdrm/dss/dss.c
index 9200a8668b49..f95ff319e68e 100644
--- a/drivers/video/fbdev/omap2/dss/dss.c
+++ b/drivers/gpu/drm/omapdrm/dss/dss.c
@@ -1264,12 +1264,18 @@ static int dss_runtime_suspend(struct device *dev)
 {
         dss_save_context();
         dss_set_min_bus_tput(dev, 0);
+
+        pinctrl_pm_select_sleep_state(dev);
+
         return 0;
 }
 
 static int dss_runtime_resume(struct device *dev)
 {
         int r;
+
+        pinctrl_pm_select_default_state(dev);
+
         /*
          * Set an arbitrarily high tput request to ensure OPP100.
          * What we should really do is to make a request to stay in OPP100,
diff --git a/drivers/video/fbdev/omap2/dss/dss.h b/drivers/gpu/drm/omapdrm/dss/dss.h
index 2406bcdb831a..9a6453235585 100644
--- a/drivers/video/fbdev/omap2/dss/dss.h
+++ b/drivers/gpu/drm/omapdrm/dss/dss.h
@@ -343,7 +343,8 @@ u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt);
 #else
 static inline u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
 {
-        WARN("%s: DSI not compiled in, returning pixel_size as 0\n", __func__);
+        WARN(1, "%s: DSI not compiled in, returning pixel_size as 0\n",
+             __func__);
         return 0;
 }
 #endif
@@ -378,7 +379,6 @@ void dispc_lcd_enable_signal(bool enable);
 void dispc_pck_free_enable(bool enable);
 void dispc_enable_fifomerge(bool enable);
 void dispc_enable_gamma_table(bool enable);
-void dispc_set_loadmode(enum omap_dss_load_mode mode);
 
 typedef bool (*dispc_div_calc_func)(int lckd, int pckd, unsigned long lck,
                 unsigned long pck, void *data);
@@ -388,7 +388,6 @@ bool dispc_div_calc(unsigned long dispc,
 
 bool dispc_mgr_timings_ok(enum omap_channel channel,
                 const struct omap_video_timings *timings);
-unsigned long dispc_fclk_rate(void);
 int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
                 struct dispc_clock_info *cinfo);
 
@@ -398,9 +397,6 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
                 u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
                 bool manual_update);
 
-unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
-unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
-unsigned long dispc_core_clk_rate(void);
 void dispc_mgr_set_clock_div(enum omap_channel channel,
                 const struct dispc_clock_info *cinfo);
 int dispc_mgr_get_clock_div(enum omap_channel channel,
diff --git a/drivers/video/fbdev/omap2/dss/dss_features.c b/drivers/gpu/drm/omapdrm/dss/dss_features.c
index b0b6dfd657bf..c886a2927f73 100644
--- a/drivers/video/fbdev/omap2/dss/dss_features.c
+++ b/drivers/gpu/drm/omapdrm/dss/dss_features.c
@@ -46,7 +46,6 @@ struct omap_dss_features {
 
         const int num_mgrs;
         const int num_ovls;
-        const int num_wbs;
         const enum omap_display_type *supported_displays;
         const enum omap_dss_output_id *supported_outputs;
         const enum omap_color_mode *supported_color_modes;
@@ -735,7 +734,6 @@ static const struct omap_dss_features omap4430_es1_0_dss_features  = {
 
         .num_mgrs = 3,
         .num_ovls = 4,
-        .num_wbs = 1,
         .supported_displays = omap4_dss_supported_displays,
         .supported_outputs = omap4_dss_supported_outputs,
         .supported_color_modes = omap4_dss_supported_color_modes,
@@ -757,7 +755,6 @@ static const struct omap_dss_features omap4430_es2_0_1_2_dss_features = {
 
         .num_mgrs = 3,
         .num_ovls = 4,
-        .num_wbs = 1,
         .supported_displays = omap4_dss_supported_displays,
         .supported_outputs = omap4_dss_supported_outputs,
         .supported_color_modes = omap4_dss_supported_color_modes,
@@ -779,7 +776,6 @@ static const struct omap_dss_features omap4_dss_features = {
 
         .num_mgrs = 3,
         .num_ovls = 4,
-        .num_wbs = 1,
         .supported_displays = omap4_dss_supported_displays,
         .supported_outputs = omap4_dss_supported_outputs,
         .supported_color_modes = omap4_dss_supported_color_modes,
@@ -825,11 +821,6 @@ int dss_feat_get_num_ovls(void)
 }
 EXPORT_SYMBOL(dss_feat_get_num_ovls);
 
-int dss_feat_get_num_wbs(void)
-{
-        return omap_current_dss_features->num_wbs;
-}
-
 unsigned long dss_feat_get_param_min(enum dss_range_param param)
 {
         return omap_current_dss_features->dss_params[param].min;
@@ -844,13 +835,11 @@ enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel
 {
         return omap_current_dss_features->supported_displays[channel];
 }
-EXPORT_SYMBOL(dss_feat_get_supported_displays);
 
 enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel)
 {
         return omap_current_dss_features->supported_outputs[channel];
 }
-EXPORT_SYMBOL(dss_feat_get_supported_outputs);
 
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
 {
diff --git a/drivers/video/fbdev/omap2/dss/dss_features.h b/drivers/gpu/drm/omapdrm/dss/dss_features.h
index 100f7a2d0638..3d67d39f192f 100644
--- a/drivers/video/fbdev/omap2/dss/dss_features.h
+++ b/drivers/gpu/drm/omapdrm/dss/dss_features.h
@@ -86,7 +86,6 @@ enum dss_range_param {
 };
 
 /* DSS Feature Functions */
-int dss_feat_get_num_wbs(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_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane);
@@ -102,4 +101,8 @@ bool dss_feat_rotation_type_supported(enum omap_dss_rotation_type rot_type);
 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(enum omapdss_version version);
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
+
 #endif
diff --git a/drivers/video/fbdev/omap2/dss/hdmi.h b/drivers/gpu/drm/omapdrm/dss/hdmi.h
index 53616b02b613..53616b02b613 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi.h
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi.h
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4.c b/drivers/gpu/drm/omapdrm/dss/hdmi4.c
index 94c8d5549b4c..7103c659a534 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4.c
@@ -343,7 +343,7 @@ static int hdmi_display_enable(struct omap_dss_device *dssdev)
 
         mutex_lock(&hdmi.lock);
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("failed to enable display: no output/manager\n");
                 r = -ENODEV;
                 goto err0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4_core.c b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.c
index fa72e735dad2..fa72e735dad2 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4_core.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4_core.h b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.h
index a069f96ec6f6..a069f96ec6f6 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4_core.h
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.h
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5.c b/drivers/gpu/drm/omapdrm/dss/hdmi5.c
index b59ba7902be1..a955a2c4c061 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5.c
@@ -373,7 +373,7 @@ static int hdmi_display_enable(struct omap_dss_device *dssdev)
 
         mutex_lock(&hdmi.lock);
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("failed to enable display: no output/manager\n");
                 r = -ENODEV;
                 goto err0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.c b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.c
index 8ea531d2652c..8ea531d2652c 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5_core.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.h b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.h
index f2f1022c5516..f2f1022c5516 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5_core.h
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.h
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_common.c b/drivers/gpu/drm/omapdrm/dss/hdmi_common.c
index 1b8fcc6c4ba1..1b8fcc6c4ba1 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_common.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_common.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_phy.c b/drivers/gpu/drm/omapdrm/dss/hdmi_phy.c
index 1f5d19c119ce..1f5d19c119ce 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_phy.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_phy.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_pll.c b/drivers/gpu/drm/omapdrm/dss/hdmi_pll.c
index 06e23a7c432c..06e23a7c432c 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_pll.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_pll.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_wp.c b/drivers/gpu/drm/omapdrm/dss/hdmi_wp.c
index 7c544bc56fb5..7c544bc56fb5 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_wp.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_wp.c
diff --git a/drivers/video/fbdev/omap2/dss/manager-sysfs.c b/drivers/gpu/drm/omapdrm/dss/manager-sysfs.c
index a7414fb12830..a7414fb12830 100644
--- a/drivers/video/fbdev/omap2/dss/manager-sysfs.c
+++ b/drivers/gpu/drm/omapdrm/dss/manager-sysfs.c
diff --git a/drivers/video/fbdev/omap2/dss/manager.c b/drivers/gpu/drm/omapdrm/dss/manager.c
index 1aac9b4191a9..08a67f4f6a20 100644
--- a/drivers/video/fbdev/omap2/dss/manager.c
+++ b/drivers/gpu/drm/omapdrm/dss/manager.c
@@ -210,7 +210,7 @@ static int dss_mgr_check_lcd_config(struct omap_overlay_manager *mgr,
                 return -EINVAL;
 
         /* fifohandcheck should be used only with stallmode */
-        if (stallmode == false && fifohandcheck == true)
+        if (!stallmode && fifohandcheck)
                 return -EINVAL;
 
         /*
diff --git a/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c b/drivers/gpu/drm/omapdrm/dss/omapdss-boot-init.c
index 8b6f6d5fdd68..136d30484d02 100644
--- a/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c
+++ b/drivers/gpu/drm/omapdrm/dss/omapdss-boot-init.c
@@ -199,10 +199,8 @@ static int __init omapdss_boot_init(void)
         omapdss_walk_device(dss, true);
 
         for_each_available_child_of_node(dss, child) {
-                if (!of_find_property(child, "compatible", NULL)) {
-                        of_node_put(child);
+                if (!of_find_property(child, "compatible", NULL))
                         continue;
-                }
 
                 omapdss_walk_device(child, true);
         }
diff --git a/drivers/video/fbdev/omap2/dss/output.c b/drivers/gpu/drm/omapdrm/dss/output.c
index 16072159bd24..16072159bd24 100644
--- a/drivers/video/fbdev/omap2/dss/output.c
+++ b/drivers/gpu/drm/omapdrm/dss/output.c
diff --git a/drivers/video/fbdev/omap2/dss/overlay-sysfs.c b/drivers/gpu/drm/omapdrm/dss/overlay-sysfs.c
index 4cc5ddebfb34..4cc5ddebfb34 100644
--- a/drivers/video/fbdev/omap2/dss/overlay-sysfs.c
+++ b/drivers/gpu/drm/omapdrm/dss/overlay-sysfs.c
diff --git a/drivers/video/fbdev/omap2/dss/overlay.c b/drivers/gpu/drm/omapdrm/dss/overlay.c
index 2f7cee985cdd..2f7cee985cdd 100644
--- a/drivers/video/fbdev/omap2/dss/overlay.c
+++ b/drivers/gpu/drm/omapdrm/dss/overlay.c
diff --git a/drivers/video/fbdev/omap2/dss/pll.c b/drivers/gpu/drm/omapdrm/dss/pll.c
index f974ddcd3b6e..f974ddcd3b6e 100644
--- a/drivers/video/fbdev/omap2/dss/pll.c
+++ b/drivers/gpu/drm/omapdrm/dss/pll.c
diff --git a/drivers/video/fbdev/omap2/dss/rfbi.c b/drivers/gpu/drm/omapdrm/dss/rfbi.c
index 1525a494d057..aea6a1d0fb20 100644
--- a/drivers/video/fbdev/omap2/dss/rfbi.c
+++ b/drivers/gpu/drm/omapdrm/dss/rfbi.c
@@ -880,7 +880,7 @@ static int rfbi_display_enable(struct omap_dss_device *dssdev)
         struct omap_dss_device *out = &rfbi.output;
         int r;
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("failed to enable display: no output/manager\n");
                 return -ENODEV;
         }
diff --git a/drivers/video/fbdev/omap2/dss/sdi.c b/drivers/gpu/drm/omapdrm/dss/sdi.c
index 5843580a1deb..d747cc6b59e1 100644
--- a/drivers/video/fbdev/omap2/dss/sdi.c
+++ b/drivers/gpu/drm/omapdrm/dss/sdi.c
@@ -136,7 +136,7 @@ static int sdi_display_enable(struct omap_dss_device *dssdev)
         unsigned long pck;
         int r;
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("failed to enable display: no output/manager\n");
                 return -ENODEV;
         }
diff --git a/drivers/video/fbdev/omap2/dss/venc.c b/drivers/gpu/drm/omapdrm/dss/venc.c
index d05a54922ba6..08f9def76e27 100644
--- a/drivers/video/fbdev/omap2/dss/venc.c
+++ b/drivers/gpu/drm/omapdrm/dss/venc.c
@@ -515,7 +515,7 @@ static int venc_display_enable(struct omap_dss_device *dssdev)
 
         mutex_lock(&venc.venc_lock);
 
-        if (out == NULL || out->manager == NULL) {
+        if (out->manager == NULL) {
                 DSSERR("Failed to enable display: no output/manager\n");
                 r = -ENODEV;
                 goto err0;
diff --git a/drivers/video/fbdev/omap2/dss/video-pll.c b/drivers/gpu/drm/omapdrm/dss/video-pll.c
index b1ec59e42940..b1ec59e42940 100644
--- a/drivers/video/fbdev/omap2/dss/video-pll.c
+++ b/drivers/gpu/drm/omapdrm/dss/video-pll.c
diff --git a/drivers/media/platform/omap/Kconfig b/drivers/media/platform/omap/Kconfig
index 217d613b0fe7..e8e2db181a7a 100644
--- a/drivers/media/platform/omap/Kconfig
+++ b/drivers/media/platform/omap/Kconfig
@@ -5,9 +5,9 @@ config VIDEO_OMAP2_VOUT
         tristate "OMAP2/OMAP3 V4L2-Display driver"
         depends on MMU
         depends on ARCH_OMAP2 || ARCH_OMAP3
+        depends on FB_OMAP2
         select VIDEOBUF_GEN
         select VIDEOBUF_DMA_CONTIG
-        select OMAP2_DSS if HAS_IOMEM && ARCH_OMAP2PLUS
         select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
         select VIDEO_OMAP2_VOUT_VRFB if VIDEO_OMAP2_VOUT && OMAP2_VRFB
         select FRAME_VECTOR
diff --git a/drivers/video/fbdev/Kconfig b/drivers/video/fbdev/Kconfig
index e6d16d65e4e6..8ea45a5cd806 100644
--- a/drivers/video/fbdev/Kconfig
+++ b/drivers/video/fbdev/Kconfig
@@ -5,6 +5,7 @@
 menuconfig FB
         tristate "Support for frame buffer devices"
         select FB_CMDLINE
+        select FB_NOTIFY
         ---help---
           The frame buffer device provides an abstraction for the graphics
           hardware. It represents the frame buffer of some video hardware and
@@ -56,6 +57,9 @@ config FIRMWARE_EDID
 config FB_CMDLINE
         bool
 
+config FB_NOTIFY
+        bool
+
 config FB_DDC
        tristate
        depends on FB
@@ -1506,6 +1510,7 @@ config FB_SIS
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
         select FB_BOOT_VESA_SUPPORT if FB_SIS = y
+        select FB_SIS_300 if !FB_SIS_315
         help
           This is the frame buffer device driver for the SiS 300, 315, 330
           and 340 series as well as XGI V3XT, V5, V8, Z7 graphics chipsets.
@@ -1880,6 +1885,8 @@ config FB_PXA
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
+        select VIDEOMODE_HELPERS if OF
+        select FB_MODE_HELPERS if OF
         ---help---
           Frame buffer driver for the built-in LCD controller in the Intel
           PXA2x0 processor.
@@ -1990,16 +1997,6 @@ config FB_SH_MOBILE_LCDC
         ---help---
           Frame buffer driver for the on-chip SH-Mobile LCD controller.
 
-config FB_SH_MOBILE_HDMI
-        tristate "SuperH Mobile HDMI controller support"
-        depends on FB_SH_MOBILE_LCDC
-        select FB_MODE_HELPERS
-        select SOUND
-        select SND
-        select SND_SOC
-        ---help---
-          Driver for the on-chip SH-Mobile HDMI controller.
-
 config FB_TMIO
         tristate "Toshiba Mobile IO FrameBuffer support"
         depends on FB && (MFD_TMIO || COMPILE_TEST)
diff --git a/drivers/video/fbdev/Makefile b/drivers/video/fbdev/Makefile
index 50ed1b4fc2bf..65fb15075c8f 100644
--- a/drivers/video/fbdev/Makefile
+++ b/drivers/video/fbdev/Makefile
@@ -118,7 +118,6 @@ obj-$(CONFIG_FB_UDL)		  += udlfb.o
 obj-$(CONFIG_FB_SMSCUFX)          += smscufx.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/
diff --git a/drivers/video/fbdev/auo_k190x.c b/drivers/video/fbdev/auo_k190x.c
index 8d2499d1cafb..9580374667ba 100644
--- a/drivers/video/fbdev/auo_k190x.c
+++ b/drivers/video/fbdev/auo_k190x.c
@@ -773,9 +773,7 @@ static void auok190x_recover(struct auok190xfb_par *par)
 /*
  * Power-management
  */
-
-#ifdef CONFIG_PM
-static int auok190x_runtime_suspend(struct device *dev)
+static int __maybe_unused auok190x_runtime_suspend(struct device *dev)
 {
         struct platform_device *pdev = to_platform_device(dev);
         struct fb_info *info = platform_get_drvdata(pdev);
@@ -822,7 +820,7 @@ finish:
         return 0;
 }
 
-static int auok190x_runtime_resume(struct device *dev)
+static int __maybe_unused auok190x_runtime_resume(struct device *dev)
 {
         struct platform_device *pdev = to_platform_device(dev);
         struct fb_info *info = platform_get_drvdata(pdev);
@@ -856,7 +854,7 @@ static int auok190x_runtime_resume(struct device *dev)
         return 0;
 }
 
-static int auok190x_suspend(struct device *dev)
+static int __maybe_unused auok190x_suspend(struct device *dev)
 {
         struct platform_device *pdev = to_platform_device(dev);
         struct fb_info *info = platform_get_drvdata(pdev);
@@ -896,7 +894,7 @@ static int auok190x_suspend(struct device *dev)
         return 0;
 }
 
-static int auok190x_resume(struct device *dev)
+static int __maybe_unused auok190x_resume(struct device *dev)
 {
         struct platform_device *pdev = to_platform_device(dev);
         struct fb_info *info = platform_get_drvdata(pdev);
@@ -933,7 +931,6 @@ static int auok190x_resume(struct device *dev)
 
         return 0;
 }
-#endif
 
 const struct dev_pm_ops auok190x_pm = {
         SET_RUNTIME_PM_OPS(auok190x_runtime_suspend, auok190x_runtime_resume,
diff --git a/drivers/video/fbdev/core/Makefile b/drivers/video/fbdev/core/Makefile
index 23d86a8b7d7b..9e3ddf225393 100644
--- a/drivers/video/fbdev/core/Makefile
+++ b/drivers/video/fbdev/core/Makefile
@@ -1,5 +1,5 @@
-obj-y                             += fb_notify.o
 obj-$(CONFIG_FB_CMDLINE)          += fb_cmdline.o
+obj-$(CONFIG_FB_NOTIFY)           += fb_notify.o
 obj-$(CONFIG_FB)                  += fb.o
 fb-y                              := fbmem.o fbmon.o fbcmap.o fbsysfs.o \
                                      modedb.o fbcvt.o
diff --git a/drivers/video/fbdev/core/fbmem.c b/drivers/video/fbdev/core/fbmem.c
index 0705d8883ede..4e73b6f6b1c0 100644
--- a/drivers/video/fbdev/core/fbmem.c
+++ b/drivers/video/fbdev/core/fbmem.c
@@ -1608,6 +1608,11 @@ static int do_remove_conflicting_framebuffers(struct apertures_struct *a,
         return 0;
 }
 
+static bool lockless_register_fb;
+module_param_named_unsafe(lockless_register_fb, lockless_register_fb, bool, 0400);
+MODULE_PARM_DESC(lockless_register_fb,
+        "Lockless framebuffer registration for debugging [default=off]");
+
 static int do_register_framebuffer(struct fb_info *fb_info)
 {
         int i, ret;
@@ -1675,15 +1680,18 @@ static int do_register_framebuffer(struct fb_info *fb_info)
         registered_fb[i] = fb_info;
 
         event.info = fb_info;
-        console_lock();
+        if (!lockless_register_fb)
+                console_lock();
         if (!lock_fb_info(fb_info)) {
-                console_unlock();
+                if (!lockless_register_fb)
+                        console_unlock();
                 return -ENODEV;
         }
 
         fb_notifier_call_chain(FB_EVENT_FB_REGISTERED, &event);
         unlock_fb_info(fb_info);
-        console_unlock();
+        if (!lockless_register_fb)
+                console_unlock();
         return 0;
 }
 
diff --git a/drivers/video/fbdev/geode/display_gx1.c b/drivers/video/fbdev/geode/display_gx1.c
index 926d53eeb549..b383eb9882bf 100644
--- a/drivers/video/fbdev/geode/display_gx1.c
+++ b/drivers/video/fbdev/geode/display_gx1.c
@@ -208,7 +208,7 @@ static void gx1_set_hw_palette_reg(struct fb_info *info, unsigned regno,
         writel(val, par->dc_regs + DC_PAL_DATA);
 }
 
-struct geode_dc_ops gx1_dc_ops = {
+const struct geode_dc_ops gx1_dc_ops = {
         .set_mode        = gx1_set_mode,
         .set_palette_reg = gx1_set_hw_palette_reg,
 };
diff --git a/drivers/video/fbdev/geode/display_gx1.h b/drivers/video/fbdev/geode/display_gx1.h
index 671c05558c79..e1cc41b343ca 100644
--- a/drivers/video/fbdev/geode/display_gx1.h
+++ b/drivers/video/fbdev/geode/display_gx1.h
@@ -18,7 +18,7 @@
 unsigned gx1_gx_base(void);
 int gx1_frame_buffer_size(void);
 
-extern struct geode_dc_ops gx1_dc_ops;
+extern const struct geode_dc_ops gx1_dc_ops;
 
 /* GX1 configuration I/O registers */
 
diff --git a/drivers/video/fbdev/geode/geodefb.h b/drivers/video/fbdev/geode/geodefb.h
index ae04820e0c57..e2e07934868f 100644
--- a/drivers/video/fbdev/geode/geodefb.h
+++ b/drivers/video/fbdev/geode/geodefb.h
@@ -31,8 +31,8 @@ struct geodefb_par {
         int panel_y;
         void __iomem *dc_regs;
         void __iomem *vid_regs;
-        struct geode_dc_ops  *dc_ops;
-        struct geode_vid_ops *vid_ops;
+        const struct geode_dc_ops  *dc_ops;
+        const struct geode_vid_ops *vid_ops;
 };
 
 #endif /* !__GEODEFB_H__ */
diff --git a/drivers/video/fbdev/geode/video_cs5530.c b/drivers/video/fbdev/geode/video_cs5530.c
index 649c3943d431..880613200ecf 100644
--- a/drivers/video/fbdev/geode/video_cs5530.c
+++ b/drivers/video/fbdev/geode/video_cs5530.c
@@ -186,7 +186,7 @@ static int cs5530_blank_display(struct fb_info *info, int blank_mode)
         return 0;
 }
 
-struct geode_vid_ops cs5530_vid_ops = {
+const struct geode_vid_ops cs5530_vid_ops = {
         .set_dclk          = cs5530_set_dclk_frequency,
         .configure_display = cs5530_configure_display,
         .blank_display     = cs5530_blank_display,
diff --git a/drivers/video/fbdev/geode/video_cs5530.h b/drivers/video/fbdev/geode/video_cs5530.h
index 56cecca7f1ce..c843348bfa20 100644
--- a/drivers/video/fbdev/geode/video_cs5530.h
+++ b/drivers/video/fbdev/geode/video_cs5530.h
@@ -15,7 +15,7 @@
 #ifndef __VIDEO_CS5530_H__
 #define __VIDEO_CS5530_H__
 
-extern struct geode_vid_ops cs5530_vid_ops;
+extern const struct geode_vid_ops cs5530_vid_ops;
 
 /* CS5530 Video device registers */
 
diff --git a/drivers/video/fbdev/i740fb.c b/drivers/video/fbdev/i740fb.c
index 452e1163ad02..cf5ccd0f2252 100644
--- a/drivers/video/fbdev/i740fb.c
+++ b/drivers/video/fbdev/i740fb.c
@@ -346,11 +346,10 @@ static void i740_calc_vclk(u32 freq, struct i740fb_par *par)
         const u32 err_target = freq / (1000 * I740_RFREQ / I740_FFIX);
         u32 err_best = 512 * I740_FFIX;
         u32 f_err, f_vco;
-        int m_best = 0, n_best = 0, p_best = 0, d_best = 0;
+        int m_best = 0, n_best = 0, p_best = 0;
         int m, n;
 
         p_best = min(15, ilog2(I740_MAX_VCO_FREQ / (freq / I740_RFREQ_FIX)));
-        d_best = 0;
         f_vco = (freq * (1 << p_best)) / I740_RFREQ_FIX;
         freq = freq / I740_RFREQ_FIX;
 
@@ -363,7 +362,7 @@ static void i740_calc_vclk(u32 freq, struct i740fb_par *par)
                         m = 3;
 
                 {
-                        u32 f_out = (((m * I740_REF_FREQ * (4 << 2 * d_best))
+                        u32 f_out = (((m * I740_REF_FREQ * 4)
                                  / n) + ((1 << p_best) / 2)) / (1 << p_best);
 
                         f_err = (freq - f_out);
@@ -386,8 +385,7 @@ static void i740_calc_vclk(u32 freq, struct i740fb_par *par)
         par->video_clk2_n = (n_best - 2) & 0xFF;
         par->video_clk2_mn_msbs = ((((n_best - 2) >> 4) & VCO_N_MSBS)
                                  | (((m_best - 2) >> 8) & VCO_M_MSBS));
-        par->video_clk2_div_sel =
-                ((p_best << 4) | (d_best ? 4 : 0) | REF_DIV_1);
+        par->video_clk2_div_sel = ((p_best << 4) | REF_DIV_1);
 }
 
 static int i740fb_decode_var(const struct fb_var_screeninfo *var,
diff --git a/drivers/video/fbdev/omap2/Kconfig b/drivers/video/fbdev/omap2/Kconfig
index c22955d2de9a..0921c4de8407 100644
--- a/drivers/video/fbdev/omap2/Kconfig
+++ b/drivers/video/fbdev/omap2/Kconfig
@@ -1,10 +1,5 @@
-config OMAP2_VRFB
-        bool
-
 if ARCH_OMAP2PLUS
 
-source "drivers/video/fbdev/omap2/dss/Kconfig"
 source "drivers/video/fbdev/omap2/omapfb/Kconfig"
-source "drivers/video/fbdev/omap2/displays-new/Kconfig"
 
 endif
diff --git a/drivers/video/fbdev/omap2/Makefile b/drivers/video/fbdev/omap2/Makefile
index f8745ec369cc..71ab5ac91106 100644
--- a/drivers/video/fbdev/omap2/Makefile
+++ b/drivers/video/fbdev/omap2/Makefile
@@ -1,5 +1 @@
-obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
-
-obj-y += dss/
-obj-y += displays-new/
-obj-$(CONFIG_FB_OMAP2) += omapfb/
+obj-y += omapfb/
diff --git a/drivers/video/fbdev/omap2/omapfb/Kconfig b/drivers/video/fbdev/omap2/omapfb/Kconfig
index 4cb12ce68855..e6226aeed17e 100644
--- a/drivers/video/fbdev/omap2/omapfb/Kconfig
+++ b/drivers/video/fbdev/omap2/omapfb/Kconfig
@@ -1,7 +1,12 @@
+config OMAP2_VRFB
+        bool
+
 menuconfig FB_OMAP2
         tristate "OMAP2+ frame buffer support"
-        depends on FB && OMAP2_DSS && !DRM_OMAP
+        depends on FB
+        depends on DRM_OMAP = n
 
+        select FB_OMAP2_DSS
         select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
@@ -9,6 +14,8 @@ menuconfig FB_OMAP2
         help
           Frame buffer driver for OMAP2+ based boards.
 
+if FB_OMAP2
+
 config FB_OMAP2_DEBUG_SUPPORT
         bool "Debug support for OMAP2+ FB"
         default y
@@ -25,3 +32,8 @@ config FB_OMAP2_NUM_FBS
         help
           Select the number of framebuffers created. OMAP2/3 has 3 overlays
           so normally this would be 3.
+
+source "drivers/video/fbdev/omap2/omapfb/dss/Kconfig"
+source "drivers/video/fbdev/omap2/omapfb/displays/Kconfig"
+
+endif
diff --git a/drivers/video/fbdev/omap2/omapfb/Makefile b/drivers/video/fbdev/omap2/omapfb/Makefile
index 51c2e00d9bf8..ad68ecf141af 100644
--- a/drivers/video/fbdev/omap2/omapfb/Makefile
+++ b/drivers/video/fbdev/omap2/omapfb/Makefile
@@ -1,2 +1,5 @@
+obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
+obj-y += dss/
+obj-y += displays/
 obj-$(CONFIG_FB_OMAP2) += omapfb.o
 omapfb-y := omapfb-main.o omapfb-sysfs.o omapfb-ioctl.o
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/Kconfig b/drivers/video/fbdev/omap2/omapfb/displays/Kconfig
new file mode 100644
index 000000000000..08f12039dd02
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/Kconfig
@@ -0,0 +1,86 @@
+menu "OMAPFB Panel and Encoder Drivers"
+        depends on FB_OMAP2_DSS
+
+config FB_OMAP2_ENCODER_OPA362
+        tristate "OPA362 external analog amplifier"
+        help
+          Driver for OPA362 external analog TV amplifier controlled
+          through a GPIO.
+
+config FB_OMAP2_ENCODER_TFP410
+        tristate "TFP410 DPI to DVI Encoder"
+        help
+          Driver for TFP410 DPI to DVI encoder.
+
+config FB_OMAP2_ENCODER_TPD12S015
+        tristate "TPD12S015 HDMI ESD protection and level shifter"
+        help
+          Driver for TPD12S015, which offers HDMI ESD protection and level
+          shifting.
+
+config FB_OMAP2_CONNECTOR_DVI
+        tristate "DVI Connector"
+        depends on I2C
+        help
+          Driver for a generic DVI connector.
+
+config FB_OMAP2_CONNECTOR_HDMI
+        tristate "HDMI Connector"
+        help
+          Driver for a generic HDMI connector.
+
+config FB_OMAP2_CONNECTOR_ANALOG_TV
+        tristate "Analog TV Connector"
+        help
+          Driver for a generic analog TV connector.
+
+config FB_OMAP2_PANEL_DPI
+        tristate "Generic DPI panel"
+        help
+          Driver for generic DPI panels.
+
+config FB_OMAP2_PANEL_DSI_CM
+        tristate "Generic DSI Command Mode Panel"
+        depends on BACKLIGHT_CLASS_DEVICE
+        help
+          Driver for generic DSI command mode panels.
+
+config FB_OMAP2_PANEL_SONY_ACX565AKM
+        tristate "ACX565AKM Panel"
+        depends on SPI && BACKLIGHT_CLASS_DEVICE
+        help
+          This is the LCD panel used on Nokia N900
+
+config FB_OMAP2_PANEL_LGPHILIPS_LB035Q02
+        tristate "LG.Philips LB035Q02 LCD Panel"
+        depends on SPI
+        help
+          LCD Panel used on the Gumstix Overo Palo35
+
+config FB_OMAP2_PANEL_SHARP_LS037V7DW01
+        tristate "Sharp LS037V7DW01 LCD Panel"
+        depends on BACKLIGHT_CLASS_DEVICE
+        help
+          LCD Panel used in TI's SDP3430 and EVM boards
+
+config FB_OMAP2_PANEL_TPO_TD028TTEC1
+        tristate "TPO TD028TTEC1 LCD Panel"
+        depends on SPI
+        help
+          LCD panel used in Openmoko.
+
+config FB_OMAP2_PANEL_TPO_TD043MTEA1
+        tristate "TPO TD043MTEA1 LCD Panel"
+        depends on SPI
+        help
+          LCD Panel used in OMAP3 Pandora
+
+config FB_OMAP2_PANEL_NEC_NL8048HL11
+        tristate "NEC NL8048HL11 Panel"
+        depends on SPI
+        depends on BACKLIGHT_CLASS_DEVICE
+        help
+                This NEC NL8048HL11 panel is TFT LCD used in the
+                Zoom2/3/3630 sdp boards.
+
+endmenu
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/Makefile b/drivers/video/fbdev/omap2/omapfb/displays/Makefile
new file mode 100644
index 000000000000..4f7459272256
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/Makefile
@@ -0,0 +1,14 @@
+obj-$(CONFIG_FB_OMAP2_ENCODER_OPA362) += encoder-opa362.o
+obj-$(CONFIG_FB_OMAP2_ENCODER_TFP410) += encoder-tfp410.o
+obj-$(CONFIG_FB_OMAP2_ENCODER_TPD12S015) += encoder-tpd12s015.o
+obj-$(CONFIG_FB_OMAP2_CONNECTOR_DVI) += connector-dvi.o
+obj-$(CONFIG_FB_OMAP2_CONNECTOR_HDMI) += connector-hdmi.o
+obj-$(CONFIG_FB_OMAP2_CONNECTOR_ANALOG_TV) += connector-analog-tv.o
+obj-$(CONFIG_FB_OMAP2_PANEL_DPI) += panel-dpi.o
+obj-$(CONFIG_FB_OMAP2_PANEL_DSI_CM) += panel-dsi-cm.o
+obj-$(CONFIG_FB_OMAP2_PANEL_SONY_ACX565AKM) += panel-sony-acx565akm.o
+obj-$(CONFIG_FB_OMAP2_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
+obj-$(CONFIG_FB_OMAP2_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
+obj-$(CONFIG_FB_OMAP2_PANEL_TPO_TD028TTEC1) += panel-tpo-td028ttec1.o
+obj-$(CONFIG_FB_OMAP2_PANEL_TPO_TD043MTEA1) += panel-tpo-td043mtea1.o
+obj-$(CONFIG_FB_OMAP2_PANEL_NEC_NL8048HL11) += panel-nec-nl8048hl11.o
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/connector-analog-tv.c b/drivers/video/fbdev/omap2/omapfb/displays/connector-analog-tv.c
new file mode 100644
index 000000000000..8511c648a15c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/connector-analog-tv.c
@@ -0,0 +1,320 @@
+/*
+ * Analog TV Connector driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct device *dev;
+
+        struct omap_video_timings timings;
+
+        enum omap_dss_venc_type connector_type;
+        bool invert_polarity;
+};
+
+static const struct omap_video_timings tvc_pal_timings = {
+        .x_res          = 720,
+        .y_res          = 574,
+        .pixelclock     = 13500000,
+        .hsw            = 64,
+        .hfp            = 12,
+        .hbp            = 68,
+        .vsw            = 5,
+        .vfp            = 5,
+        .vbp            = 41,
+
+        .interlace      = true,
+};
+
+static const struct of_device_id tvc_of_match[];
+
+struct tvc_of_data {
+        enum omap_dss_venc_type connector_type;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int tvc_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(ddata->dev, "connect\n");
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.atv->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void tvc_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(ddata->dev, "disconnect\n");
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.atv->disconnect(in, dssdev);
+}
+
+static int tvc_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(ddata->dev, "enable\n");
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        in->ops.atv->set_timings(in, &ddata->timings);
+
+        if (!ddata->dev->of_node) {
+                in->ops.atv->set_type(in, ddata->connector_type);
+
+                in->ops.atv->invert_vid_out_polarity(in,
+                        ddata->invert_polarity);
+        }
+
+        r = in->ops.atv->enable(in);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return r;
+}
+
+static void tvc_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(ddata->dev, "disable\n");
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        in->ops.atv->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tvc_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->timings = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.atv->set_timings(in, timings);
+}
+
+static void tvc_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->timings;
+}
+
+static int tvc_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.atv->check_timings(in, timings);
+}
+
+static u32 tvc_get_wss(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.atv->get_wss(in);
+}
+
+static int tvc_set_wss(struct omap_dss_device *dssdev, u32 wss)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.atv->set_wss(in, wss);
+}
+
+static struct omap_dss_driver tvc_driver = {
+        .connect                = tvc_connect,
+        .disconnect             = tvc_disconnect,
+
+        .enable                 = tvc_enable,
+        .disable                = tvc_disable,
+
+        .set_timings            = tvc_set_timings,
+        .get_timings            = tvc_get_timings,
+        .check_timings          = tvc_check_timings,
+
+        .get_resolution         = omapdss_default_get_resolution,
+
+        .get_wss                = tvc_get_wss,
+        .set_wss                = tvc_set_wss,
+};
+
+static int tvc_probe_pdata(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct connector_atv_platform_data *pdata;
+        struct omap_dss_device *in, *dssdev;
+
+        pdata = dev_get_platdata(&pdev->dev);
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&pdev->dev, "Failed to find video source\n");
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        ddata->connector_type = pdata->connector_type;
+        ddata->invert_polarity = pdata->invert_polarity;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int tvc_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int tvc_probe(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+        ddata->dev = &pdev->dev;
+
+        if (dev_get_platdata(&pdev->dev)) {
+                r = tvc_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = tvc_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        ddata->timings = tvc_pal_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->driver = &tvc_driver;
+        dssdev->dev = &pdev->dev;
+        dssdev->type = OMAP_DISPLAY_TYPE_VENC;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = tvc_pal_timings;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+err_reg:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit tvc_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_display(&ddata->dssdev);
+
+        tvc_disable(dssdev);
+        tvc_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id tvc_of_match[] = {
+        { .compatible = "omapdss,svideo-connector", },
+        { .compatible = "omapdss,composite-video-connector", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, tvc_of_match);
+
+static struct platform_driver tvc_connector_driver = {
+        .probe  = tvc_probe,
+        .remove = __exit_p(tvc_remove),
+        .driver = {
+                .name   = "connector-analog-tv",
+                .of_match_table = tvc_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(tvc_connector_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Analog TV Connector driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/connector-dvi.c b/drivers/video/fbdev/omap2/omapfb/displays/connector-dvi.c
new file mode 100644
index 000000000000..d811e6dcaef7
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/connector-dvi.c
@@ -0,0 +1,398 @@
+/*
+ * Generic DVI Connector driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <drm/drm_edid.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+static const struct omap_video_timings dvic_default_timings = {
+        .x_res          = 640,
+        .y_res          = 480,
+
+        .pixelclock     = 23500000,
+
+        .hfp            = 48,
+        .hsw            = 32,
+        .hbp            = 80,
+
+        .vfp            = 3,
+        .vsw            = 4,
+        .vbp            = 7,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_HIGH,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_HIGH,
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct omap_video_timings timings;
+
+        struct i2c_adapter *i2c_adapter;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int dvic_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dvi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void dvic_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dvi->disconnect(in, dssdev);
+}
+
+static int dvic_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        in->ops.dvi->set_timings(in, &ddata->timings);
+
+        r = in->ops.dvi->enable(in);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void dvic_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        in->ops.dvi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void dvic_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->timings = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dvi->set_timings(in, timings);
+}
+
+static void dvic_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->timings;
+}
+
+static int dvic_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dvi->check_timings(in, timings);
+}
+
+static int dvic_ddc_read(struct i2c_adapter *adapter,
+                unsigned char *buf, u16 count, u8 offset)
+{
+        int r, retries;
+
+        for (retries = 3; retries > 0; retries--) {
+                struct i2c_msg msgs[] = {
+                        {
+                                .addr   = DDC_ADDR,
+                                .flags  = 0,
+                                .len    = 1,
+                                .buf    = &offset,
+                        }, {
+                                .addr   = DDC_ADDR,
+                                .flags  = I2C_M_RD,
+                                .len    = count,
+                                .buf    = buf,
+                        }
+                };
+
+                r = i2c_transfer(adapter, msgs, 2);
+                if (r == 2)
+                        return 0;
+
+                if (r != -EAGAIN)
+                        break;
+        }
+
+        return r < 0 ? r : -EIO;
+}
+
+static int dvic_read_edid(struct omap_dss_device *dssdev,
+                u8 *edid, int len)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        int r, l, bytes_read;
+
+        if (!ddata->i2c_adapter)
+                return -ENODEV;
+
+        l = min(EDID_LENGTH, len);
+        r = dvic_ddc_read(ddata->i2c_adapter, edid, l, 0);
+        if (r)
+                return r;
+
+        bytes_read = l;
+
+        /* if there are extensions, read second block */
+        if (len > EDID_LENGTH && edid[0x7e] > 0) {
+                l = min(EDID_LENGTH, len - EDID_LENGTH);
+
+                r = dvic_ddc_read(ddata->i2c_adapter, edid + EDID_LENGTH,
+                                l, EDID_LENGTH);
+                if (r)
+                        return r;
+
+                bytes_read += l;
+        }
+
+        return bytes_read;
+}
+
+static bool dvic_detect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        unsigned char out;
+        int r;
+
+        if (!ddata->i2c_adapter)
+                return true;
+
+        r = dvic_ddc_read(ddata->i2c_adapter, &out, 1, 0);
+
+        return r == 0;
+}
+
+static struct omap_dss_driver dvic_driver = {
+        .connect        = dvic_connect,
+        .disconnect     = dvic_disconnect,
+
+        .enable         = dvic_enable,
+        .disable        = dvic_disable,
+
+        .set_timings    = dvic_set_timings,
+        .get_timings    = dvic_get_timings,
+        .check_timings  = dvic_check_timings,
+
+        .get_resolution = omapdss_default_get_resolution,
+
+        .read_edid      = dvic_read_edid,
+        .detect         = dvic_detect,
+};
+
+static int dvic_probe_pdata(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct connector_dvi_platform_data *pdata;
+        struct omap_dss_device *in, *dssdev;
+        int i2c_bus_num;
+
+        pdata = dev_get_platdata(&pdev->dev);
+        i2c_bus_num = pdata->i2c_bus_num;
+
+        if (i2c_bus_num != -1) {
+                struct i2c_adapter *adapter;
+
+                adapter = i2c_get_adapter(i2c_bus_num);
+                if (!adapter) {
+                        dev_err(&pdev->dev,
+                                        "Failed to get I2C adapter, bus %d\n",
+                                        i2c_bus_num);
+                        return -EPROBE_DEFER;
+                }
+
+                ddata->i2c_adapter = adapter;
+        }
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                i2c_put_adapter(ddata->i2c_adapter);
+
+                dev_err(&pdev->dev, "Failed to find video source\n");
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int dvic_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+        struct device_node *adapter_node;
+        struct i2c_adapter *adapter;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        adapter_node = of_parse_phandle(node, "ddc-i2c-bus", 0);
+        if (adapter_node) {
+                adapter = of_get_i2c_adapter_by_node(adapter_node);
+                if (adapter == NULL) {
+                        dev_err(&pdev->dev, "failed to parse ddc-i2c-bus\n");
+                        omap_dss_put_device(ddata->in);
+                        return -EPROBE_DEFER;
+                }
+
+                ddata->i2c_adapter = adapter;
+        }
+
+        return 0;
+}
+
+static int dvic_probe(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+
+        if (dev_get_platdata(&pdev->dev)) {
+                r = dvic_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = dvic_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        ddata->timings = dvic_default_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->driver = &dvic_driver;
+        dssdev->dev = &pdev->dev;
+        dssdev->type = OMAP_DISPLAY_TYPE_DVI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = dvic_default_timings;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        omap_dss_put_device(ddata->in);
+
+        i2c_put_adapter(ddata->i2c_adapter);
+
+        return r;
+}
+
+static int __exit dvic_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_display(&ddata->dssdev);
+
+        dvic_disable(dssdev);
+        dvic_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        i2c_put_adapter(ddata->i2c_adapter);
+
+        return 0;
+}
+
+static const struct of_device_id dvic_of_match[] = {
+        { .compatible = "omapdss,dvi-connector", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, dvic_of_match);
+
+static struct platform_driver dvi_connector_driver = {
+        .probe  = dvic_probe,
+        .remove = __exit_p(dvic_remove),
+        .driver = {
+                .name   = "connector-dvi",
+                .of_match_table = dvic_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(dvi_connector_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Generic DVI Connector driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/connector-hdmi.c b/drivers/video/fbdev/omap2/omapfb/displays/connector-hdmi.c
new file mode 100644
index 000000000000..6ee4129bc0c0
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/connector-hdmi.c
@@ -0,0 +1,348 @@
+/*
+ * HDMI Connector driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include <drm/drm_edid.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+static const struct omap_video_timings hdmic_default_timings = {
+        .x_res          = 640,
+        .y_res          = 480,
+        .pixelclock     = 25175000,
+        .hsw            = 96,
+        .hfp            = 16,
+        .hbp            = 48,
+        .vsw            = 2,
+        .vfp            = 11,
+        .vbp            = 31,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+
+        .interlace      = false,
+};
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct device *dev;
+
+        struct omap_video_timings timings;
+
+        int hpd_gpio;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int hdmic_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(ddata->dev, "connect\n");
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.hdmi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void hdmic_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(ddata->dev, "disconnect\n");
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.hdmi->disconnect(in, dssdev);
+}
+
+static int hdmic_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(ddata->dev, "enable\n");
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        in->ops.hdmi->set_timings(in, &ddata->timings);
+
+        r = in->ops.hdmi->enable(in);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return r;
+}
+
+static void hdmic_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(ddata->dev, "disable\n");
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        in->ops.hdmi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void hdmic_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->timings = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.hdmi->set_timings(in, timings);
+}
+
+static void hdmic_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->timings;
+}
+
+static int hdmic_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.hdmi->check_timings(in, timings);
+}
+
+static int hdmic_read_edid(struct omap_dss_device *dssdev,
+                u8 *edid, int len)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.hdmi->read_edid(in, edid, len);
+}
+
+static bool hdmic_detect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (gpio_is_valid(ddata->hpd_gpio))
+                return gpio_get_value_cansleep(ddata->hpd_gpio);
+        else
+                return in->ops.hdmi->detect(in);
+}
+
+static int hdmic_set_hdmi_mode(struct omap_dss_device *dssdev, bool hdmi_mode)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.hdmi->set_hdmi_mode(in, hdmi_mode);
+}
+
+static int hdmic_set_infoframe(struct omap_dss_device *dssdev,
+                const struct hdmi_avi_infoframe *avi)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.hdmi->set_infoframe(in, avi);
+}
+
+static struct omap_dss_driver hdmic_driver = {
+        .connect                = hdmic_connect,
+        .disconnect             = hdmic_disconnect,
+
+        .enable                 = hdmic_enable,
+        .disable                = hdmic_disable,
+
+        .set_timings            = hdmic_set_timings,
+        .get_timings            = hdmic_get_timings,
+        .check_timings          = hdmic_check_timings,
+
+        .get_resolution         = omapdss_default_get_resolution,
+
+        .read_edid              = hdmic_read_edid,
+        .detect                 = hdmic_detect,
+        .set_hdmi_mode          = hdmic_set_hdmi_mode,
+        .set_hdmi_infoframe     = hdmic_set_infoframe,
+};
+
+static int hdmic_probe_pdata(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct connector_hdmi_platform_data *pdata;
+        struct omap_dss_device *in, *dssdev;
+
+        pdata = dev_get_platdata(&pdev->dev);
+
+        ddata->hpd_gpio = -ENODEV;
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&pdev->dev, "Failed to find video source\n");
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int hdmic_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+        int gpio;
+
+        /* HPD GPIO */
+        gpio = of_get_named_gpio(node, "hpd-gpios", 0);
+        if (gpio_is_valid(gpio))
+                ddata->hpd_gpio = gpio;
+        else
+                ddata->hpd_gpio = -ENODEV;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int hdmic_probe(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+        ddata->dev = &pdev->dev;
+
+        if (dev_get_platdata(&pdev->dev)) {
+                r = hdmic_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = hdmic_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        if (gpio_is_valid(ddata->hpd_gpio)) {
+                r = devm_gpio_request_one(&pdev->dev, ddata->hpd_gpio,
+                                GPIOF_DIR_IN, "hdmi_hpd");
+                if (r)
+                        goto err_reg;
+        }
+
+        ddata->timings = hdmic_default_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->driver = &hdmic_driver;
+        dssdev->dev = &pdev->dev;
+        dssdev->type = OMAP_DISPLAY_TYPE_HDMI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = hdmic_default_timings;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+err_reg:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit hdmic_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_display(&ddata->dssdev);
+
+        hdmic_disable(dssdev);
+        hdmic_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id hdmic_of_match[] = {
+        { .compatible = "omapdss,hdmi-connector", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, hdmic_of_match);
+
+static struct platform_driver hdmi_connector_driver = {
+        .probe  = hdmic_probe,
+        .remove = __exit_p(hdmic_remove),
+        .driver = {
+                .name   = "connector-hdmi",
+                .of_match_table = hdmic_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(hdmi_connector_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("HDMI Connector driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/encoder-opa362.c b/drivers/video/fbdev/omap2/omapfb/displays/encoder-opa362.c
new file mode 100644
index 000000000000..8c246c213e06
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/encoder-opa362.c
@@ -0,0 +1,278 @@
+/*
+ * OPA362 analog video amplifier with output/power control
+ *
+ * Copyright (C) 2014 Golden Delicious Computers
+ * Author: H. Nikolaus Schaller <hns@goldelico.com>
+ *
+ * based on encoder-tfp410
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct gpio_desc *enable_gpio;
+
+        struct omap_video_timings timings;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int opa362_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(dssdev->dev, "connect\n");
+
+        if (omapdss_device_is_connected(dssdev))
+                return -EBUSY;
+
+        r = in->ops.atv->connect(in, dssdev);
+        if (r)
+                return r;
+
+        dst->src = dssdev;
+        dssdev->dst = dst;
+
+        return 0;
+}
+
+static void opa362_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(dssdev->dev, "disconnect\n");
+
+        WARN_ON(!omapdss_device_is_connected(dssdev));
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        WARN_ON(dst != dssdev->dst);
+        if (dst != dssdev->dst)
+                return;
+
+        dst->src = NULL;
+        dssdev->dst = NULL;
+
+        in->ops.atv->disconnect(in, &ddata->dssdev);
+}
+
+static int opa362_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(dssdev->dev, "enable\n");
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        in->ops.atv->set_timings(in, &ddata->timings);
+
+        r = in->ops.atv->enable(in);
+        if (r)
+                return r;
+
+        if (ddata->enable_gpio)
+                gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void opa362_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(dssdev->dev, "disable\n");
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        if (ddata->enable_gpio)
+                gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
+        in->ops.atv->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void opa362_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(dssdev->dev, "set_timings\n");
+
+        ddata->timings = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.atv->set_timings(in, timings);
+}
+
+static void opa362_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        dev_dbg(dssdev->dev, "get_timings\n");
+
+        *timings = ddata->timings;
+}
+
+static int opa362_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(dssdev->dev, "check_timings\n");
+
+        return in->ops.atv->check_timings(in, timings);
+}
+
+static void opa362_set_type(struct omap_dss_device *dssdev,
+                enum omap_dss_venc_type type)
+{
+        /* we can only drive a COMPOSITE output */
+        WARN_ON(type != OMAP_DSS_VENC_TYPE_COMPOSITE);
+
+}
+
+static const struct omapdss_atv_ops opa362_atv_ops = {
+        .connect        = opa362_connect,
+        .disconnect     = opa362_disconnect,
+
+        .enable         = opa362_enable,
+        .disable        = opa362_disable,
+
+        .check_timings  = opa362_check_timings,
+        .set_timings    = opa362_set_timings,
+        .get_timings    = opa362_get_timings,
+
+        .set_type       = opa362_set_type,
+};
+
+static int opa362_probe(struct platform_device *pdev)
+{
+        struct device_node *node = pdev->dev.of_node;
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev, *in;
+        struct gpio_desc *gpio;
+        int r;
+
+        dev_dbg(&pdev->dev, "probe\n");
+
+        if (node == NULL) {
+                dev_err(&pdev->dev, "Unable to find device tree\n");
+                return -EINVAL;
+        }
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+
+        gpio = devm_gpiod_get_optional(&pdev->dev, "enable", GPIOD_OUT_LOW);
+        if (IS_ERR(gpio))
+                return PTR_ERR(gpio);
+
+        ddata->enable_gpio = gpio;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        dssdev = &ddata->dssdev;
+        dssdev->ops.atv = &opa362_atv_ops;
+        dssdev->dev = &pdev->dev;
+        dssdev->type = OMAP_DISPLAY_TYPE_VENC;
+        dssdev->output_type = OMAP_DISPLAY_TYPE_VENC;
+        dssdev->owner = THIS_MODULE;
+
+        r = omapdss_register_output(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register output\n");
+                goto err_reg;
+        }
+
+        return 0;
+err_reg:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit opa362_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_output(&ddata->dssdev);
+
+        WARN_ON(omapdss_device_is_enabled(dssdev));
+        if (omapdss_device_is_enabled(dssdev))
+                opa362_disable(dssdev);
+
+        WARN_ON(omapdss_device_is_connected(dssdev));
+        if (omapdss_device_is_connected(dssdev))
+                opa362_disconnect(dssdev, dssdev->dst);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id opa362_of_match[] = {
+        { .compatible = "omapdss,ti,opa362", },
+        {},
+};
+MODULE_DEVICE_TABLE(of, opa362_of_match);
+
+static struct platform_driver opa362_driver = {
+        .probe  = opa362_probe,
+        .remove = __exit_p(opa362_remove),
+        .driver = {
+                .name   = "amplifier-opa362",
+                .of_match_table = opa362_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(opa362_driver);
+
+MODULE_AUTHOR("H. Nikolaus Schaller <hns@goldelico.com>");
+MODULE_DESCRIPTION("OPA362 analog video amplifier with output/power control");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/encoder-tfp410.c b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tfp410.c
new file mode 100644
index 000000000000..d9048b3df495
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tfp410.c
@@ -0,0 +1,320 @@
+/*
+ * TFP410 DPI-to-DVI encoder driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        int pd_gpio;
+        int data_lines;
+
+        struct omap_video_timings timings;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int tfp410_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return -EBUSY;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        dst->src = dssdev;
+        dssdev->dst = dst;
+
+        return 0;
+}
+
+static void tfp410_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        WARN_ON(!omapdss_device_is_connected(dssdev));
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        WARN_ON(dst != dssdev->dst);
+        if (dst != dssdev->dst)
+                return;
+
+        dst->src = NULL;
+        dssdev->dst = NULL;
+
+        in->ops.dpi->disconnect(in, &ddata->dssdev);
+}
+
+static int tfp410_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        in->ops.dpi->set_timings(in, &ddata->timings);
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+
+        r = in->ops.dpi->enable(in);
+        if (r)
+                return r;
+
+        if (gpio_is_valid(ddata->pd_gpio))
+                gpio_set_value_cansleep(ddata->pd_gpio, 1);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void tfp410_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        if (gpio_is_valid(ddata->pd_gpio))
+                gpio_set_value_cansleep(ddata->pd_gpio, 0);
+
+        in->ops.dpi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tfp410_fix_timings(struct omap_video_timings *timings)
+{
+        timings->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        timings->sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        timings->de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+}
+
+static void tfp410_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        tfp410_fix_timings(timings);
+
+        ddata->timings = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void tfp410_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->timings;
+}
+
+static int tfp410_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        tfp410_fix_timings(timings);
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static const struct omapdss_dvi_ops tfp410_dvi_ops = {
+        .connect        = tfp410_connect,
+        .disconnect     = tfp410_disconnect,
+
+        .enable         = tfp410_enable,
+        .disable        = tfp410_disable,
+
+        .check_timings  = tfp410_check_timings,
+        .set_timings    = tfp410_set_timings,
+        .get_timings    = tfp410_get_timings,
+};
+
+static int tfp410_probe_pdata(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct encoder_tfp410_platform_data *pdata;
+        struct omap_dss_device *dssdev, *in;
+
+        pdata = dev_get_platdata(&pdev->dev);
+
+        ddata->pd_gpio = pdata->power_down_gpio;
+
+        ddata->data_lines = pdata->data_lines;
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&pdev->dev, "Failed to find video source\n");
+                return -ENODEV;
+        }
+
+        ddata->in = in;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int tfp410_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+        int gpio;
+
+        gpio = of_get_named_gpio(node, "powerdown-gpios", 0);
+
+        if (gpio_is_valid(gpio) || gpio == -ENOENT) {
+                ddata->pd_gpio = gpio;
+        } else {
+                dev_err(&pdev->dev, "failed to parse PD gpio\n");
+                return gpio;
+        }
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int tfp410_probe(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+
+        if (dev_get_platdata(&pdev->dev)) {
+                r = tfp410_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = tfp410_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        if (gpio_is_valid(ddata->pd_gpio)) {
+                r = devm_gpio_request_one(&pdev->dev, ddata->pd_gpio,
+                                GPIOF_OUT_INIT_LOW, "tfp410 PD");
+                if (r) {
+                        dev_err(&pdev->dev, "Failed to request PD GPIO %d\n",
+                                        ddata->pd_gpio);
+                        goto err_gpio;
+                }
+        }
+
+        dssdev = &ddata->dssdev;
+        dssdev->ops.dvi = &tfp410_dvi_ops;
+        dssdev->dev = &pdev->dev;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->output_type = OMAP_DISPLAY_TYPE_DVI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->phy.dpi.data_lines = ddata->data_lines;
+        dssdev->port_num = 1;
+
+        r = omapdss_register_output(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register output\n");
+                goto err_reg;
+        }
+
+        return 0;
+err_reg:
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit tfp410_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_output(&ddata->dssdev);
+
+        WARN_ON(omapdss_device_is_enabled(dssdev));
+        if (omapdss_device_is_enabled(dssdev))
+                tfp410_disable(dssdev);
+
+        WARN_ON(omapdss_device_is_connected(dssdev));
+        if (omapdss_device_is_connected(dssdev))
+                tfp410_disconnect(dssdev, dssdev->dst);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id tfp410_of_match[] = {
+        { .compatible = "omapdss,ti,tfp410", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, tfp410_of_match);
+
+static struct platform_driver tfp410_driver = {
+        .probe  = tfp410_probe,
+        .remove = __exit_p(tfp410_remove),
+        .driver = {
+                .name   = "tfp410",
+                .of_match_table = tfp410_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(tfp410_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("TFP410 DPI to DVI encoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/encoder-tpd12s015.c b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tpd12s015.c
new file mode 100644
index 000000000000..677e2545fcbe
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tpd12s015.c
@@ -0,0 +1,328 @@
+/*
+ * TPD12S015 HDMI ESD protection & level shifter chip driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/consumer.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct gpio_desc *ct_cp_hpd_gpio;
+        struct gpio_desc *ls_oe_gpio;
+        struct gpio_desc *hpd_gpio;
+
+        struct omap_video_timings timings;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int tpd_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        r = in->ops.hdmi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        dst->src = dssdev;
+        dssdev->dst = dst;
+
+        if (ddata->ct_cp_hpd_gpio) {
+                gpiod_set_value_cansleep(ddata->ct_cp_hpd_gpio, 1);
+                /* DC-DC converter needs at max 300us to get to 90% of 5V */
+                udelay(300);
+        }
+
+        return 0;
+}
+
+static void tpd_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        gpiod_set_value_cansleep(ddata->ct_cp_hpd_gpio, 0);
+
+        dst->src = NULL;
+        dssdev->dst = NULL;
+
+        in->ops.hdmi->disconnect(in, &ddata->dssdev);
+}
+
+static int tpd_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
+        in->ops.hdmi->set_timings(in, &ddata->timings);
+
+        r = in->ops.hdmi->enable(in);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return r;
+}
+
+static void tpd_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
+        in->ops.hdmi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tpd_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->timings = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.hdmi->set_timings(in, timings);
+}
+
+static void tpd_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->timings;
+}
+
+static int tpd_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        r = in->ops.hdmi->check_timings(in, timings);
+
+        return r;
+}
+
+static int tpd_read_edid(struct omap_dss_device *dssdev,
+                u8 *edid, int len)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!gpiod_get_value_cansleep(ddata->hpd_gpio))
+                return -ENODEV;
+
+        gpiod_set_value_cansleep(ddata->ls_oe_gpio, 1);
+
+        r = in->ops.hdmi->read_edid(in, edid, len);
+
+        gpiod_set_value_cansleep(ddata->ls_oe_gpio, 0);
+
+        return r;
+}
+
+static bool tpd_detect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        return gpiod_get_value_cansleep(ddata->hpd_gpio);
+}
+
+static int tpd_set_infoframe(struct omap_dss_device *dssdev,
+                const struct hdmi_avi_infoframe *avi)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.hdmi->set_infoframe(in, avi);
+}
+
+static int tpd_set_hdmi_mode(struct omap_dss_device *dssdev,
+                bool hdmi_mode)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.hdmi->set_hdmi_mode(in, hdmi_mode);
+}
+
+static const struct omapdss_hdmi_ops tpd_hdmi_ops = {
+        .connect                = tpd_connect,
+        .disconnect             = tpd_disconnect,
+
+        .enable                 = tpd_enable,
+        .disable                = tpd_disable,
+
+        .check_timings          = tpd_check_timings,
+        .set_timings            = tpd_set_timings,
+        .get_timings            = tpd_get_timings,
+
+        .read_edid              = tpd_read_edid,
+        .detect                 = tpd_detect,
+        .set_infoframe          = tpd_set_infoframe,
+        .set_hdmi_mode          = tpd_set_hdmi_mode,
+};
+
+static int tpd_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int tpd_probe(struct platform_device *pdev)
+{
+        struct omap_dss_device *in, *dssdev;
+        struct panel_drv_data *ddata;
+        int r;
+        struct gpio_desc *gpio;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+
+        if (pdev->dev.of_node) {
+                r = tpd_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+
+        gpio = devm_gpiod_get_index_optional(&pdev->dev, NULL, 0,
+                GPIOD_OUT_LOW);
+        if (IS_ERR(gpio))
+                goto err_gpio;
+
+        ddata->ct_cp_hpd_gpio = gpio;
+
+        gpio = devm_gpiod_get_index_optional(&pdev->dev, NULL, 1,
+                GPIOD_OUT_LOW);
+        if (IS_ERR(gpio))
+                goto err_gpio;
+
+        ddata->ls_oe_gpio = gpio;
+
+        gpio = devm_gpiod_get_index(&pdev->dev, NULL, 2,
+                GPIOD_IN);
+        if (IS_ERR(gpio))
+                goto err_gpio;
+
+        ddata->hpd_gpio = gpio;
+
+        dssdev = &ddata->dssdev;
+        dssdev->ops.hdmi = &tpd_hdmi_ops;
+        dssdev->dev = &pdev->dev;
+        dssdev->type = OMAP_DISPLAY_TYPE_HDMI;
+        dssdev->output_type = OMAP_DISPLAY_TYPE_HDMI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->port_num = 1;
+
+        in = ddata->in;
+
+        r = omapdss_register_output(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register output\n");
+                goto err_reg;
+        }
+
+        return 0;
+err_reg:
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit tpd_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_output(&ddata->dssdev);
+
+        WARN_ON(omapdss_device_is_enabled(dssdev));
+        if (omapdss_device_is_enabled(dssdev))
+                tpd_disable(dssdev);
+
+        WARN_ON(omapdss_device_is_connected(dssdev));
+        if (omapdss_device_is_connected(dssdev))
+                tpd_disconnect(dssdev, dssdev->dst);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id tpd_of_match[] = {
+        { .compatible = "omapdss,ti,tpd12s015", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, tpd_of_match);
+
+static struct platform_driver tpd_driver = {
+        .probe  = tpd_probe,
+        .remove = __exit_p(tpd_remove),
+        .driver = {
+                .name   = "tpd12s015",
+                .of_match_table = tpd_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(tpd_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("TPD12S015 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-dpi.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-dpi.c
new file mode 100644
index 000000000000..e780fd4f8b46
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-dpi.c
@@ -0,0 +1,328 @@
+/*
+ * Generic MIPI DPI Panel Driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+#include <video/of_display_timing.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        int data_lines;
+
+        struct omap_video_timings videomode;
+
+        /* used for non-DT boot, to be removed */
+        int backlight_gpio;
+
+        struct gpio_desc *enable_gpio;
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int panel_dpi_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void panel_dpi_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int panel_dpi_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+        in->ops.dpi->set_timings(in, &ddata->videomode);
+
+        r = in->ops.dpi->enable(in);
+        if (r)
+                return r;
+
+        gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+
+        if (gpio_is_valid(ddata->backlight_gpio))
+                gpio_set_value_cansleep(ddata->backlight_gpio, 1);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void panel_dpi_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        if (gpio_is_valid(ddata->backlight_gpio))
+                gpio_set_value_cansleep(ddata->backlight_gpio, 0);
+
+        gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
+        in->ops.dpi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void panel_dpi_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void panel_dpi_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int panel_dpi_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver panel_dpi_ops = {
+        .connect        = panel_dpi_connect,
+        .disconnect     = panel_dpi_disconnect,
+
+        .enable         = panel_dpi_enable,
+        .disable        = panel_dpi_disable,
+
+        .set_timings    = panel_dpi_set_timings,
+        .get_timings    = panel_dpi_get_timings,
+        .check_timings  = panel_dpi_check_timings,
+
+        .get_resolution = omapdss_default_get_resolution,
+};
+
+static int panel_dpi_probe_pdata(struct platform_device *pdev)
+{
+        const struct panel_dpi_platform_data *pdata;
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev, *in;
+        struct videomode vm;
+        int r;
+
+        pdata = dev_get_platdata(&pdev->dev);
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&pdev->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        ddata->data_lines = pdata->data_lines;
+
+        videomode_from_timing(pdata->display_timing, &vm);
+        videomode_to_omap_video_timings(&vm, &ddata->videomode);
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        r = devm_gpio_request_one(&pdev->dev, pdata->enable_gpio,
+                                        GPIOF_OUT_INIT_LOW, "panel enable");
+        if (r)
+                goto err_gpio;
+
+        ddata->enable_gpio = gpio_to_desc(pdata->enable_gpio);
+
+        ddata->backlight_gpio = pdata->backlight_gpio;
+
+        return 0;
+
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int panel_dpi_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+        int r;
+        struct display_timing timing;
+        struct videomode vm;
+        struct gpio_desc *gpio;
+
+        gpio = devm_gpiod_get_optional(&pdev->dev, "enable", GPIOD_OUT_LOW);
+        if (IS_ERR(gpio))
+                return PTR_ERR(gpio);
+
+        ddata->enable_gpio = gpio;
+
+        ddata->backlight_gpio = -ENOENT;
+
+        r = of_get_display_timing(node, "panel-timing", &timing);
+        if (r) {
+                dev_err(&pdev->dev, "failed to get video timing\n");
+                return r;
+        }
+
+        videomode_from_timing(&timing, &vm);
+        videomode_to_omap_video_timings(&vm, &ddata->videomode);
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int panel_dpi_probe(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+
+        if (dev_get_platdata(&pdev->dev)) {
+                r = panel_dpi_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = panel_dpi_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        if (gpio_is_valid(ddata->backlight_gpio)) {
+                r = devm_gpio_request_one(&pdev->dev, ddata->backlight_gpio,
+                                GPIOF_OUT_INIT_LOW, "panel backlight");
+                if (r)
+                        goto err_gpio;
+        }
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &pdev->dev;
+        dssdev->driver = &panel_dpi_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+        dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit panel_dpi_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_display(dssdev);
+
+        panel_dpi_disable(dssdev);
+        panel_dpi_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id panel_dpi_of_match[] = {
+        { .compatible = "omapdss,panel-dpi", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, panel_dpi_of_match);
+
+static struct platform_driver panel_dpi_driver = {
+        .probe = panel_dpi_probe,
+        .remove = __exit_p(panel_dpi_remove),
+        .driver = {
+                .name = "panel-dpi",
+                .of_match_table = panel_dpi_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(panel_dpi_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Generic MIPI DPI Panel Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-dsi-cm.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-dsi-cm.c
new file mode 100644
index 000000000000..3414c2609320
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-dsi-cm.c
@@ -0,0 +1,1388 @@
+/*
+ * Generic DSI Command Mode panel driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+/* #define DEBUG */
+
+#include <linux/backlight.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+#include <video/mipi_display.h>
+
+/* DSI Virtual channel. Hardcoded for now. */
+#define TCH 0
+
+#define DCS_READ_NUM_ERRORS     0x05
+#define DCS_BRIGHTNESS          0x51
+#define DCS_CTRL_DISPLAY        0x53
+#define DCS_GET_ID1             0xda
+#define DCS_GET_ID2             0xdb
+#define DCS_GET_ID3             0xdc
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct omap_video_timings timings;
+
+        struct platform_device *pdev;
+
+        struct mutex lock;
+
+        struct backlight_device *bldev;
+
+        unsigned long   hw_guard_end;   /* next value of jiffies when we can
+                                         * issue the next sleep in/out command
+                                         */
+        unsigned long   hw_guard_wait;  /* max guard time in jiffies */
+
+        /* panel HW configuration from DT or platform data */
+        int reset_gpio;
+        int ext_te_gpio;
+
+        bool use_dsi_backlight;
+
+        struct omap_dsi_pin_config pin_config;
+
+        /* runtime variables */
+        bool enabled;
+
+        bool te_enabled;
+
+        atomic_t do_update;
+        int channel;
+
+        struct delayed_work te_timeout_work;
+
+        bool intro_printed;
+
+        struct workqueue_struct *workqueue;
+
+        bool ulps_enabled;
+        unsigned ulps_timeout;
+        struct delayed_work ulps_work;
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static irqreturn_t dsicm_te_isr(int irq, void *data);
+static void dsicm_te_timeout_work_callback(struct work_struct *work);
+static int _dsicm_enable_te(struct panel_drv_data *ddata, bool enable);
+
+static int dsicm_panel_reset(struct panel_drv_data *ddata);
+
+static void dsicm_ulps_work(struct work_struct *work);
+
+static void hw_guard_start(struct panel_drv_data *ddata, int guard_msec)
+{
+        ddata->hw_guard_wait = msecs_to_jiffies(guard_msec);
+        ddata->hw_guard_end = jiffies + ddata->hw_guard_wait;
+}
+
+static void hw_guard_wait(struct panel_drv_data *ddata)
+{
+        unsigned long wait = ddata->hw_guard_end - jiffies;
+
+        if ((long)wait > 0 && wait <= ddata->hw_guard_wait) {
+                set_current_state(TASK_UNINTERRUPTIBLE);
+                schedule_timeout(wait);
+        }
+}
+
+static int dsicm_dcs_read_1(struct panel_drv_data *ddata, u8 dcs_cmd, u8 *data)
+{
+        struct omap_dss_device *in = ddata->in;
+        int r;
+        u8 buf[1];
+
+        r = in->ops.dsi->dcs_read(in, ddata->channel, dcs_cmd, buf, 1);
+
+        if (r < 0)
+                return r;
+
+        *data = buf[0];
+
+        return 0;
+}
+
+static int dsicm_dcs_write_0(struct panel_drv_data *ddata, u8 dcs_cmd)
+{
+        struct omap_dss_device *in = ddata->in;
+        return in->ops.dsi->dcs_write(in, ddata->channel, &dcs_cmd, 1);
+}
+
+static int dsicm_dcs_write_1(struct panel_drv_data *ddata, u8 dcs_cmd, u8 param)
+{
+        struct omap_dss_device *in = ddata->in;
+        u8 buf[2] = { dcs_cmd, param };
+
+        return in->ops.dsi->dcs_write(in, ddata->channel, buf, 2);
+}
+
+static int dsicm_sleep_in(struct panel_drv_data *ddata)
+
+{
+        struct omap_dss_device *in = ddata->in;
+        u8 cmd;
+        int r;
+
+        hw_guard_wait(ddata);
+
+        cmd = MIPI_DCS_ENTER_SLEEP_MODE;
+        r = in->ops.dsi->dcs_write_nosync(in, ddata->channel, &cmd, 1);
+        if (r)
+                return r;
+
+        hw_guard_start(ddata, 120);
+
+        usleep_range(5000, 10000);
+
+        return 0;
+}
+
+static int dsicm_sleep_out(struct panel_drv_data *ddata)
+{
+        int r;
+
+        hw_guard_wait(ddata);
+
+        r = dsicm_dcs_write_0(ddata, MIPI_DCS_EXIT_SLEEP_MODE);
+        if (r)
+                return r;
+
+        hw_guard_start(ddata, 120);
+
+        usleep_range(5000, 10000);
+
+        return 0;
+}
+
+static int dsicm_get_id(struct panel_drv_data *ddata, u8 *id1, u8 *id2, u8 *id3)
+{
+        int r;
+
+        r = dsicm_dcs_read_1(ddata, DCS_GET_ID1, id1);
+        if (r)
+                return r;
+        r = dsicm_dcs_read_1(ddata, DCS_GET_ID2, id2);
+        if (r)
+                return r;
+        r = dsicm_dcs_read_1(ddata, DCS_GET_ID3, id3);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static int dsicm_set_update_window(struct panel_drv_data *ddata,
+                u16 x, u16 y, u16 w, u16 h)
+{
+        struct omap_dss_device *in = ddata->in;
+        int r;
+        u16 x1 = x;
+        u16 x2 = x + w - 1;
+        u16 y1 = y;
+        u16 y2 = y + h - 1;
+
+        u8 buf[5];
+        buf[0] = MIPI_DCS_SET_COLUMN_ADDRESS;
+        buf[1] = (x1 >> 8) & 0xff;
+        buf[2] = (x1 >> 0) & 0xff;
+        buf[3] = (x2 >> 8) & 0xff;
+        buf[4] = (x2 >> 0) & 0xff;
+
+        r = in->ops.dsi->dcs_write_nosync(in, ddata->channel, buf, sizeof(buf));
+        if (r)
+                return r;
+
+        buf[0] = MIPI_DCS_SET_PAGE_ADDRESS;
+        buf[1] = (y1 >> 8) & 0xff;
+        buf[2] = (y1 >> 0) & 0xff;
+        buf[3] = (y2 >> 8) & 0xff;
+        buf[4] = (y2 >> 0) & 0xff;
+
+        r = in->ops.dsi->dcs_write_nosync(in, ddata->channel, buf, sizeof(buf));
+        if (r)
+                return r;
+
+        in->ops.dsi->bta_sync(in, ddata->channel);
+
+        return r;
+}
+
+static void dsicm_queue_ulps_work(struct panel_drv_data *ddata)
+{
+        if (ddata->ulps_timeout > 0)
+                queue_delayed_work(ddata->workqueue, &ddata->ulps_work,
+                                msecs_to_jiffies(ddata->ulps_timeout));
+}
+
+static void dsicm_cancel_ulps_work(struct panel_drv_data *ddata)
+{
+        cancel_delayed_work(&ddata->ulps_work);
+}
+
+static int dsicm_enter_ulps(struct panel_drv_data *ddata)
+{
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (ddata->ulps_enabled)
+                return 0;
+
+        dsicm_cancel_ulps_work(ddata);
+
+        r = _dsicm_enable_te(ddata, false);
+        if (r)
+                goto err;
+
+        if (gpio_is_valid(ddata->ext_te_gpio))
+                disable_irq(gpio_to_irq(ddata->ext_te_gpio));
+
+        in->ops.dsi->disable(in, false, true);
+
+        ddata->ulps_enabled = true;
+
+        return 0;
+
+err:
+        dev_err(&ddata->pdev->dev, "enter ULPS failed");
+        dsicm_panel_reset(ddata);
+
+        ddata->ulps_enabled = false;
+
+        dsicm_queue_ulps_work(ddata);
+
+        return r;
+}
+
+static int dsicm_exit_ulps(struct panel_drv_data *ddata)
+{
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!ddata->ulps_enabled)
+                return 0;
+
+        r = in->ops.dsi->enable(in);
+        if (r) {
+                dev_err(&ddata->pdev->dev, "failed to enable DSI\n");
+                goto err1;
+        }
+
+        in->ops.dsi->enable_hs(in, ddata->channel, true);
+
+        r = _dsicm_enable_te(ddata, true);
+        if (r) {
+                dev_err(&ddata->pdev->dev, "failed to re-enable TE");
+                goto err2;
+        }
+
+        if (gpio_is_valid(ddata->ext_te_gpio))
+                enable_irq(gpio_to_irq(ddata->ext_te_gpio));
+
+        dsicm_queue_ulps_work(ddata);
+
+        ddata->ulps_enabled = false;
+
+        return 0;
+
+err2:
+        dev_err(&ddata->pdev->dev, "failed to exit ULPS");
+
+        r = dsicm_panel_reset(ddata);
+        if (!r) {
+                if (gpio_is_valid(ddata->ext_te_gpio))
+                        enable_irq(gpio_to_irq(ddata->ext_te_gpio));
+                ddata->ulps_enabled = false;
+        }
+err1:
+        dsicm_queue_ulps_work(ddata);
+
+        return r;
+}
+
+static int dsicm_wake_up(struct panel_drv_data *ddata)
+{
+        if (ddata->ulps_enabled)
+                return dsicm_exit_ulps(ddata);
+
+        dsicm_cancel_ulps_work(ddata);
+        dsicm_queue_ulps_work(ddata);
+        return 0;
+}
+
+static int dsicm_bl_update_status(struct backlight_device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+        int level;
+
+        if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+                        dev->props.power == FB_BLANK_UNBLANK)
+                level = dev->props.brightness;
+        else
+                level = 0;
+
+        dev_dbg(&ddata->pdev->dev, "update brightness to %d\n", level);
+
+        mutex_lock(&ddata->lock);
+
+        if (ddata->enabled) {
+                in->ops.dsi->bus_lock(in);
+
+                r = dsicm_wake_up(ddata);
+                if (!r)
+                        r = dsicm_dcs_write_1(ddata, DCS_BRIGHTNESS, level);
+
+                in->ops.dsi->bus_unlock(in);
+        } else {
+                r = 0;
+        }
+
+        mutex_unlock(&ddata->lock);
+
+        return r;
+}
+
+static int dsicm_bl_get_intensity(struct backlight_device *dev)
+{
+        if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+                        dev->props.power == FB_BLANK_UNBLANK)
+                return dev->props.brightness;
+
+        return 0;
+}
+
+static const struct backlight_ops dsicm_bl_ops = {
+        .get_brightness = dsicm_bl_get_intensity,
+        .update_status  = dsicm_bl_update_status,
+};
+
+static void dsicm_get_resolution(struct omap_dss_device *dssdev,
+                u16 *xres, u16 *yres)
+{
+        *xres = dssdev->panel.timings.x_res;
+        *yres = dssdev->panel.timings.y_res;
+}
+
+static ssize_t dsicm_num_errors_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *in = ddata->in;
+        u8 errors = 0;
+        int r;
+
+        mutex_lock(&ddata->lock);
+
+        if (ddata->enabled) {
+                in->ops.dsi->bus_lock(in);
+
+                r = dsicm_wake_up(ddata);
+                if (!r)
+                        r = dsicm_dcs_read_1(ddata, DCS_READ_NUM_ERRORS,
+                                        &errors);
+
+                in->ops.dsi->bus_unlock(in);
+        } else {
+                r = -ENODEV;
+        }
+
+        mutex_unlock(&ddata->lock);
+
+        if (r)
+                return r;
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", errors);
+}
+
+static ssize_t dsicm_hw_revision_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *in = ddata->in;
+        u8 id1, id2, id3;
+        int r;
+
+        mutex_lock(&ddata->lock);
+
+        if (ddata->enabled) {
+                in->ops.dsi->bus_lock(in);
+
+                r = dsicm_wake_up(ddata);
+                if (!r)
+                        r = dsicm_get_id(ddata, &id1, &id2, &id3);
+
+                in->ops.dsi->bus_unlock(in);
+        } else {
+                r = -ENODEV;
+        }
+
+        mutex_unlock(&ddata->lock);
+
+        if (r)
+                return r;
+
+        return snprintf(buf, PAGE_SIZE, "%02x.%02x.%02x\n", id1, id2, id3);
+}
+
+static ssize_t dsicm_store_ulps(struct device *dev,
+                struct device_attribute *attr,
+                const char *buf, size_t count)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *in = ddata->in;
+        unsigned long t;
+        int r;
+
+        r = kstrtoul(buf, 0, &t);
+        if (r)
+                return r;
+
+        mutex_lock(&ddata->lock);
+
+        if (ddata->enabled) {
+                in->ops.dsi->bus_lock(in);
+
+                if (t)
+                        r = dsicm_enter_ulps(ddata);
+                else
+                        r = dsicm_wake_up(ddata);
+
+                in->ops.dsi->bus_unlock(in);
+        }
+
+        mutex_unlock(&ddata->lock);
+
+        if (r)
+                return r;
+
+        return count;
+}
+
+static ssize_t dsicm_show_ulps(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        unsigned t;
+
+        mutex_lock(&ddata->lock);
+        t = ddata->ulps_enabled;
+        mutex_unlock(&ddata->lock);
+
+        return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static ssize_t dsicm_store_ulps_timeout(struct device *dev,
+                struct device_attribute *attr,
+                const char *buf, size_t count)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *in = ddata->in;
+        unsigned long t;
+        int r;
+
+        r = kstrtoul(buf, 0, &t);
+        if (r)
+                return r;
+
+        mutex_lock(&ddata->lock);
+        ddata->ulps_timeout = t;
+
+        if (ddata->enabled) {
+                /* dsicm_wake_up will restart the timer */
+                in->ops.dsi->bus_lock(in);
+                r = dsicm_wake_up(ddata);
+                in->ops.dsi->bus_unlock(in);
+        }
+
+        mutex_unlock(&ddata->lock);
+
+        if (r)
+                return r;
+
+        return count;
+}
+
+static ssize_t dsicm_show_ulps_timeout(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        unsigned t;
+
+        mutex_lock(&ddata->lock);
+        t = ddata->ulps_timeout;
+        mutex_unlock(&ddata->lock);
+
+        return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static DEVICE_ATTR(num_dsi_errors, S_IRUGO, dsicm_num_errors_show, NULL);
+static DEVICE_ATTR(hw_revision, S_IRUGO, dsicm_hw_revision_show, NULL);
+static DEVICE_ATTR(ulps, S_IRUGO | S_IWUSR,
+                dsicm_show_ulps, dsicm_store_ulps);
+static DEVICE_ATTR(ulps_timeout, S_IRUGO | S_IWUSR,
+                dsicm_show_ulps_timeout, dsicm_store_ulps_timeout);
+
+static struct attribute *dsicm_attrs[] = {
+        &dev_attr_num_dsi_errors.attr,
+        &dev_attr_hw_revision.attr,
+        &dev_attr_ulps.attr,
+        &dev_attr_ulps_timeout.attr,
+        NULL,
+};
+
+static struct attribute_group dsicm_attr_group = {
+        .attrs = dsicm_attrs,
+};
+
+static void dsicm_hw_reset(struct panel_drv_data *ddata)
+{
+        if (!gpio_is_valid(ddata->reset_gpio))
+                return;
+
+        gpio_set_value(ddata->reset_gpio, 1);
+        udelay(10);
+        /* reset the panel */
+        gpio_set_value(ddata->reset_gpio, 0);
+        /* assert reset */
+        udelay(10);
+        gpio_set_value(ddata->reset_gpio, 1);
+        /* wait after releasing reset */
+        usleep_range(5000, 10000);
+}
+
+static int dsicm_power_on(struct panel_drv_data *ddata)
+{
+        struct omap_dss_device *in = ddata->in;
+        u8 id1, id2, id3;
+        int r;
+        struct omap_dss_dsi_config dsi_config = {
+                .mode = OMAP_DSS_DSI_CMD_MODE,
+                .pixel_format = OMAP_DSS_DSI_FMT_RGB888,
+                .timings = &ddata->timings,
+                .hs_clk_min = 150000000,
+                .hs_clk_max = 300000000,
+                .lp_clk_min = 7000000,
+                .lp_clk_max = 10000000,
+        };
+
+        if (ddata->pin_config.num_pins > 0) {
+                r = in->ops.dsi->configure_pins(in, &ddata->pin_config);
+                if (r) {
+                        dev_err(&ddata->pdev->dev,
+                                "failed to configure DSI pins\n");
+                        goto err0;
+                }
+        }
+
+        r = in->ops.dsi->set_config(in, &dsi_config);
+        if (r) {
+                dev_err(&ddata->pdev->dev, "failed to configure DSI\n");
+                goto err0;
+        }
+
+        r = in->ops.dsi->enable(in);
+        if (r) {
+                dev_err(&ddata->pdev->dev, "failed to enable DSI\n");
+                goto err0;
+        }
+
+        dsicm_hw_reset(ddata);
+
+        in->ops.dsi->enable_hs(in, ddata->channel, false);
+
+        r = dsicm_sleep_out(ddata);
+        if (r)
+                goto err;
+
+        r = dsicm_get_id(ddata, &id1, &id2, &id3);
+        if (r)
+                goto err;
+
+        r = dsicm_dcs_write_1(ddata, DCS_BRIGHTNESS, 0xff);
+        if (r)
+                goto err;
+
+        r = dsicm_dcs_write_1(ddata, DCS_CTRL_DISPLAY,
+                        (1<<2) | (1<<5));       /* BL | BCTRL */
+        if (r)
+                goto err;
+
+        r = dsicm_dcs_write_1(ddata, MIPI_DCS_SET_PIXEL_FORMAT,
+                MIPI_DCS_PIXEL_FMT_24BIT);
+        if (r)
+                goto err;
+
+        r = dsicm_dcs_write_0(ddata, MIPI_DCS_SET_DISPLAY_ON);
+        if (r)
+                goto err;
+
+        r = _dsicm_enable_te(ddata, ddata->te_enabled);
+        if (r)
+                goto err;
+
+        r = in->ops.dsi->enable_video_output(in, ddata->channel);
+        if (r)
+                goto err;
+
+        ddata->enabled = 1;
+
+        if (!ddata->intro_printed) {
+                dev_info(&ddata->pdev->dev, "panel revision %02x.%02x.%02x\n",
+                        id1, id2, id3);
+                ddata->intro_printed = true;
+        }
+
+        in->ops.dsi->enable_hs(in, ddata->channel, true);
+
+        return 0;
+err:
+        dev_err(&ddata->pdev->dev, "error while enabling panel, issuing HW reset\n");
+
+        dsicm_hw_reset(ddata);
+
+        in->ops.dsi->disable(in, true, false);
+err0:
+        return r;
+}
+
+static void dsicm_power_off(struct panel_drv_data *ddata)
+{
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        in->ops.dsi->disable_video_output(in, ddata->channel);
+
+        r = dsicm_dcs_write_0(ddata, MIPI_DCS_SET_DISPLAY_OFF);
+        if (!r)
+                r = dsicm_sleep_in(ddata);
+
+        if (r) {
+                dev_err(&ddata->pdev->dev,
+                                "error disabling panel, issuing HW reset\n");
+                dsicm_hw_reset(ddata);
+        }
+
+        in->ops.dsi->disable(in, true, false);
+
+        ddata->enabled = 0;
+}
+
+static int dsicm_panel_reset(struct panel_drv_data *ddata)
+{
+        dev_err(&ddata->pdev->dev, "performing LCD reset\n");
+
+        dsicm_power_off(ddata);
+        dsicm_hw_reset(ddata);
+        return dsicm_power_on(ddata);
+}
+
+static int dsicm_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        struct device *dev = &ddata->pdev->dev;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dsi->connect(in, dssdev);
+        if (r) {
+                dev_err(dev, "Failed to connect to video source\n");
+                return r;
+        }
+
+        r = in->ops.dsi->request_vc(ddata->in, &ddata->channel);
+        if (r) {
+                dev_err(dev, "failed to get virtual channel\n");
+                goto err_req_vc;
+        }
+
+        r = in->ops.dsi->set_vc_id(ddata->in, ddata->channel, TCH);
+        if (r) {
+                dev_err(dev, "failed to set VC_ID\n");
+                goto err_vc_id;
+        }
+
+        return 0;
+
+err_vc_id:
+        in->ops.dsi->release_vc(ddata->in, ddata->channel);
+err_req_vc:
+        in->ops.dsi->disconnect(in, dssdev);
+        return r;
+}
+
+static void dsicm_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dsi->release_vc(in, ddata->channel);
+        in->ops.dsi->disconnect(in, dssdev);
+}
+
+static int dsicm_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(&ddata->pdev->dev, "enable\n");
+
+        mutex_lock(&ddata->lock);
+
+        if (!omapdss_device_is_connected(dssdev)) {
+                r = -ENODEV;
+                goto err;
+        }
+
+        if (omapdss_device_is_enabled(dssdev)) {
+                r = 0;
+                goto err;
+        }
+
+        in->ops.dsi->bus_lock(in);
+
+        r = dsicm_power_on(ddata);
+
+        in->ops.dsi->bus_unlock(in);
+
+        if (r)
+                goto err;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        mutex_unlock(&ddata->lock);
+
+        return 0;
+err:
+        dev_dbg(&ddata->pdev->dev, "enable failed\n");
+        mutex_unlock(&ddata->lock);
+        return r;
+}
+
+static void dsicm_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(&ddata->pdev->dev, "disable\n");
+
+        mutex_lock(&ddata->lock);
+
+        dsicm_cancel_ulps_work(ddata);
+
+        in->ops.dsi->bus_lock(in);
+
+        if (omapdss_device_is_enabled(dssdev)) {
+                r = dsicm_wake_up(ddata);
+                if (!r)
+                        dsicm_power_off(ddata);
+        }
+
+        in->ops.dsi->bus_unlock(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        mutex_unlock(&ddata->lock);
+}
+
+static void dsicm_framedone_cb(int err, void *data)
+{
+        struct panel_drv_data *ddata = data;
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(&ddata->pdev->dev, "framedone, err %d\n", err);
+        in->ops.dsi->bus_unlock(ddata->in);
+}
+
+static irqreturn_t dsicm_te_isr(int irq, void *data)
+{
+        struct panel_drv_data *ddata = data;
+        struct omap_dss_device *in = ddata->in;
+        int old;
+        int r;
+
+        old = atomic_cmpxchg(&ddata->do_update, 1, 0);
+
+        if (old) {
+                cancel_delayed_work(&ddata->te_timeout_work);
+
+                r = in->ops.dsi->update(in, ddata->channel, dsicm_framedone_cb,
+                                ddata);
+                if (r)
+                        goto err;
+        }
+
+        return IRQ_HANDLED;
+err:
+        dev_err(&ddata->pdev->dev, "start update failed\n");
+        in->ops.dsi->bus_unlock(in);
+        return IRQ_HANDLED;
+}
+
+static void dsicm_te_timeout_work_callback(struct work_struct *work)
+{
+        struct panel_drv_data *ddata = container_of(work, struct panel_drv_data,
+                                        te_timeout_work.work);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_err(&ddata->pdev->dev, "TE not received for 250ms!\n");
+
+        atomic_set(&ddata->do_update, 0);
+        in->ops.dsi->bus_unlock(in);
+}
+
+static int dsicm_update(struct omap_dss_device *dssdev,
+                                    u16 x, u16 y, u16 w, u16 h)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(&ddata->pdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
+
+        mutex_lock(&ddata->lock);
+        in->ops.dsi->bus_lock(in);
+
+        r = dsicm_wake_up(ddata);
+        if (r)
+                goto err;
+
+        if (!ddata->enabled) {
+                r = 0;
+                goto err;
+        }
+
+        /* XXX no need to send this every frame, but dsi break if not done */
+        r = dsicm_set_update_window(ddata, 0, 0,
+                        dssdev->panel.timings.x_res,
+                        dssdev->panel.timings.y_res);
+        if (r)
+                goto err;
+
+        if (ddata->te_enabled && gpio_is_valid(ddata->ext_te_gpio)) {
+                schedule_delayed_work(&ddata->te_timeout_work,
+                                msecs_to_jiffies(250));
+                atomic_set(&ddata->do_update, 1);
+        } else {
+                r = in->ops.dsi->update(in, ddata->channel, dsicm_framedone_cb,
+                                ddata);
+                if (r)
+                        goto err;
+        }
+
+        /* note: no bus_unlock here. unlock is in framedone_cb */
+        mutex_unlock(&ddata->lock);
+        return 0;
+err:
+        in->ops.dsi->bus_unlock(in);
+        mutex_unlock(&ddata->lock);
+        return r;
+}
+
+static int dsicm_sync(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(&ddata->pdev->dev, "sync\n");
+
+        mutex_lock(&ddata->lock);
+        in->ops.dsi->bus_lock(in);
+        in->ops.dsi->bus_unlock(in);
+        mutex_unlock(&ddata->lock);
+
+        dev_dbg(&ddata->pdev->dev, "sync done\n");
+
+        return 0;
+}
+
+static int _dsicm_enable_te(struct panel_drv_data *ddata, bool enable)
+{
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (enable)
+                r = dsicm_dcs_write_1(ddata, MIPI_DCS_SET_TEAR_ON, 0);
+        else
+                r = dsicm_dcs_write_0(ddata, MIPI_DCS_SET_TEAR_OFF);
+
+        if (!gpio_is_valid(ddata->ext_te_gpio))
+                in->ops.dsi->enable_te(in, enable);
+
+        /* possible panel bug */
+        msleep(100);
+
+        return r;
+}
+
+static int dsicm_enable_te(struct omap_dss_device *dssdev, bool enable)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        mutex_lock(&ddata->lock);
+
+        if (ddata->te_enabled == enable)
+                goto end;
+
+        in->ops.dsi->bus_lock(in);
+
+        if (ddata->enabled) {
+                r = dsicm_wake_up(ddata);
+                if (r)
+                        goto err;
+
+                r = _dsicm_enable_te(ddata, enable);
+                if (r)
+                        goto err;
+        }
+
+        ddata->te_enabled = enable;
+
+        in->ops.dsi->bus_unlock(in);
+end:
+        mutex_unlock(&ddata->lock);
+
+        return 0;
+err:
+        in->ops.dsi->bus_unlock(in);
+        mutex_unlock(&ddata->lock);
+
+        return r;
+}
+
+static int dsicm_get_te(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        int r;
+
+        mutex_lock(&ddata->lock);
+        r = ddata->te_enabled;
+        mutex_unlock(&ddata->lock);
+
+        return r;
+}
+
+static int dsicm_memory_read(struct omap_dss_device *dssdev,
+                void *buf, size_t size,
+                u16 x, u16 y, u16 w, u16 h)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+        int first = 1;
+        int plen;
+        unsigned buf_used = 0;
+
+        if (size < w * h * 3)
+                return -ENOMEM;
+
+        mutex_lock(&ddata->lock);
+
+        if (!ddata->enabled) {
+                r = -ENODEV;
+                goto err1;
+        }
+
+        size = min(w * h * 3,
+                        dssdev->panel.timings.x_res *
+                        dssdev->panel.timings.y_res * 3);
+
+        in->ops.dsi->bus_lock(in);
+
+        r = dsicm_wake_up(ddata);
+        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
+         * an error. */
+        if (size % 2)
+                plen = 1;
+        else
+                plen = 2;
+
+        dsicm_set_update_window(ddata, x, y, w, h);
+
+        r = in->ops.dsi->set_max_rx_packet_size(in, ddata->channel, plen);
+        if (r)
+                goto err2;
+
+        while (buf_used < size) {
+                u8 dcs_cmd = first ? 0x2e : 0x3e;
+                first = 0;
+
+                r = in->ops.dsi->dcs_read(in, ddata->channel, dcs_cmd,
+                                buf + buf_used, size - buf_used);
+
+                if (r < 0) {
+                        dev_err(dssdev->dev, "read error\n");
+                        goto err3;
+                }
+
+                buf_used += r;
+
+                if (r < plen) {
+                        dev_err(&ddata->pdev->dev, "short read\n");
+                        break;
+                }
+
+                if (signal_pending(current)) {
+                        dev_err(&ddata->pdev->dev, "signal pending, "
+                                        "aborting memory read\n");
+                        r = -ERESTARTSYS;
+                        goto err3;
+                }
+        }
+
+        r = buf_used;
+
+err3:
+        in->ops.dsi->set_max_rx_packet_size(in, ddata->channel, 1);
+err2:
+        in->ops.dsi->bus_unlock(in);
+err1:
+        mutex_unlock(&ddata->lock);
+        return r;
+}
+
+static void dsicm_ulps_work(struct work_struct *work)
+{
+        struct panel_drv_data *ddata = container_of(work, struct panel_drv_data,
+                        ulps_work.work);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        mutex_lock(&ddata->lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE || !ddata->enabled) {
+                mutex_unlock(&ddata->lock);
+                return;
+        }
+
+        in->ops.dsi->bus_lock(in);
+
+        dsicm_enter_ulps(ddata);
+
+        in->ops.dsi->bus_unlock(in);
+        mutex_unlock(&ddata->lock);
+}
+
+static struct omap_dss_driver dsicm_ops = {
+        .connect        = dsicm_connect,
+        .disconnect     = dsicm_disconnect,
+
+        .enable         = dsicm_enable,
+        .disable        = dsicm_disable,
+
+        .update         = dsicm_update,
+        .sync           = dsicm_sync,
+
+        .get_resolution = dsicm_get_resolution,
+        .get_recommended_bpp = omapdss_default_get_recommended_bpp,
+
+        .enable_te      = dsicm_enable_te,
+        .get_te         = dsicm_get_te,
+
+        .memory_read    = dsicm_memory_read,
+};
+
+static int dsicm_probe_pdata(struct platform_device *pdev)
+{
+        const struct panel_dsicm_platform_data *pdata;
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev, *in;
+
+        pdata = dev_get_platdata(&pdev->dev);
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return -EPROBE_DEFER;
+        }
+        ddata->in = in;
+
+        ddata->reset_gpio = pdata->reset_gpio;
+
+        if (pdata->use_ext_te)
+                ddata->ext_te_gpio = pdata->ext_te_gpio;
+        else
+                ddata->ext_te_gpio = -1;
+
+        ddata->ulps_timeout = pdata->ulps_timeout;
+
+        ddata->use_dsi_backlight = pdata->use_dsi_backlight;
+
+        ddata->pin_config = pdata->pin_config;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int dsicm_probe_of(struct platform_device *pdev)
+{
+        struct device_node *node = pdev->dev.of_node;
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *in;
+        int gpio;
+
+        gpio = of_get_named_gpio(node, "reset-gpios", 0);
+        if (!gpio_is_valid(gpio)) {
+                dev_err(&pdev->dev, "failed to parse reset gpio\n");
+                return gpio;
+        }
+        ddata->reset_gpio = gpio;
+
+        gpio = of_get_named_gpio(node, "te-gpios", 0);
+        if (gpio_is_valid(gpio) || gpio == -ENOENT) {
+                ddata->ext_te_gpio = gpio;
+        } else {
+                dev_err(&pdev->dev, "failed to parse TE gpio\n");
+                return gpio;
+        }
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        /* TODO: ulps, backlight */
+
+        return 0;
+}
+
+static int dsicm_probe(struct platform_device *pdev)
+{
+        struct backlight_properties props;
+        struct panel_drv_data *ddata;
+        struct backlight_device *bldev = NULL;
+        struct device *dev = &pdev->dev;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        dev_dbg(dev, "probe\n");
+
+        ddata = devm_kzalloc(dev, sizeof(*ddata), GFP_KERNEL);
+        if (!ddata)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+        ddata->pdev = pdev;
+
+        if (dev_get_platdata(dev)) {
+                r = dsicm_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = dsicm_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        ddata->timings.x_res = 864;
+        ddata->timings.y_res = 480;
+        ddata->timings.pixelclock = 864 * 480 * 60;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = dev;
+        dssdev->driver = &dsicm_ops;
+        dssdev->panel.timings = ddata->timings;
+        dssdev->type = OMAP_DISPLAY_TYPE_DSI;
+        dssdev->owner = THIS_MODULE;
+
+        dssdev->panel.dsi_pix_fmt = OMAP_DSS_DSI_FMT_RGB888;
+        dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
+                OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        mutex_init(&ddata->lock);
+
+        atomic_set(&ddata->do_update, 0);
+
+        if (gpio_is_valid(ddata->reset_gpio)) {
+                r = devm_gpio_request_one(dev, ddata->reset_gpio,
+                                GPIOF_OUT_INIT_LOW, "taal rst");
+                if (r) {
+                        dev_err(dev, "failed to request reset gpio\n");
+                        return r;
+                }
+        }
+
+        if (gpio_is_valid(ddata->ext_te_gpio)) {
+                r = devm_gpio_request_one(dev, ddata->ext_te_gpio,
+                                GPIOF_IN, "taal irq");
+                if (r) {
+                        dev_err(dev, "GPIO request failed\n");
+                        return r;
+                }
+
+                r = devm_request_irq(dev, gpio_to_irq(ddata->ext_te_gpio),
+                                dsicm_te_isr,
+                                IRQF_TRIGGER_RISING,
+                                "taal vsync", ddata);
+
+                if (r) {
+                        dev_err(dev, "IRQ request failed\n");
+                        return r;
+                }
+
+                INIT_DEFERRABLE_WORK(&ddata->te_timeout_work,
+                                        dsicm_te_timeout_work_callback);
+
+                dev_dbg(dev, "Using GPIO TE\n");
+        }
+
+        ddata->workqueue = create_singlethread_workqueue("dsicm_wq");
+        if (ddata->workqueue == NULL) {
+                dev_err(dev, "can't create workqueue\n");
+                return -ENOMEM;
+        }
+        INIT_DELAYED_WORK(&ddata->ulps_work, dsicm_ulps_work);
+
+        dsicm_hw_reset(ddata);
+
+        if (ddata->use_dsi_backlight) {
+                memset(&props, 0, sizeof(struct backlight_properties));
+                props.max_brightness = 255;
+
+                props.type = BACKLIGHT_RAW;
+                bldev = backlight_device_register(dev_name(dev),
+                                dev, ddata, &dsicm_bl_ops, &props);
+                if (IS_ERR(bldev)) {
+                        r = PTR_ERR(bldev);
+                        goto err_bl;
+                }
+
+                ddata->bldev = bldev;
+
+                bldev->props.fb_blank = FB_BLANK_UNBLANK;
+                bldev->props.power = FB_BLANK_UNBLANK;
+                bldev->props.brightness = 255;
+
+                dsicm_bl_update_status(bldev);
+        }
+
+        r = sysfs_create_group(&dev->kobj, &dsicm_attr_group);
+        if (r) {
+                dev_err(dev, "failed to create sysfs files\n");
+                goto err_sysfs_create;
+        }
+
+        return 0;
+
+err_sysfs_create:
+        if (bldev != NULL)
+                backlight_device_unregister(bldev);
+err_bl:
+        destroy_workqueue(ddata->workqueue);
+err_reg:
+        return r;
+}
+
+static int __exit dsicm_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct backlight_device *bldev;
+
+        dev_dbg(&pdev->dev, "remove\n");
+
+        omapdss_unregister_display(dssdev);
+
+        dsicm_disable(dssdev);
+        dsicm_disconnect(dssdev);
+
+        sysfs_remove_group(&pdev->dev.kobj, &dsicm_attr_group);
+
+        bldev = ddata->bldev;
+        if (bldev != NULL) {
+                bldev->props.power = FB_BLANK_POWERDOWN;
+                dsicm_bl_update_status(bldev);
+                backlight_device_unregister(bldev);
+        }
+
+        omap_dss_put_device(ddata->in);
+
+        dsicm_cancel_ulps_work(ddata);
+        destroy_workqueue(ddata->workqueue);
+
+        /* reset, to be sure that the panel is in a valid state */
+        dsicm_hw_reset(ddata);
+
+        return 0;
+}
+
+static const struct of_device_id dsicm_of_match[] = {
+        { .compatible = "omapdss,panel-dsi-cm", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, dsicm_of_match);
+
+static struct platform_driver dsicm_driver = {
+        .probe = dsicm_probe,
+        .remove = __exit_p(dsicm_remove),
+        .driver = {
+                .name = "panel-dsi-cm",
+                .of_match_table = dsicm_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(dsicm_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Generic DSI Command Mode Panel Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-lgphilips-lb035q02.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-lgphilips-lb035q02.c
new file mode 100644
index 000000000000..18eb60e9c9ec
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-lgphilips-lb035q02.c
@@ -0,0 +1,404 @@
+/*
+ * LG.Philips LB035Q02 LCD Panel driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ * Based on a driver by: 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.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/mutex.h>
+#include <linux/gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+static struct omap_video_timings lb035q02_timings = {
+        .x_res = 320,
+        .y_res = 240,
+
+        .pixelclock     = 6500000,
+
+        .hsw            = 2,
+        .hfp            = 20,
+        .hbp            = 68,
+
+        .vsw            = 2,
+        .vfp            = 4,
+        .vbp            = 18,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        struct spi_device *spi;
+
+        int data_lines;
+
+        struct omap_video_timings videomode;
+
+        /* used for non-DT boot, to be removed */
+        int backlight_gpio;
+
+        struct gpio_desc *enable_gpio;
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+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 lb035q02_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        init_lb035q02_panel(ddata->spi);
+
+        return 0;
+}
+
+static void lb035q02_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int lb035q02_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+        in->ops.dpi->set_timings(in, &ddata->videomode);
+
+        r = in->ops.dpi->enable(in);
+        if (r)
+                return r;
+
+        if (ddata->enable_gpio)
+                gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+
+        if (gpio_is_valid(ddata->backlight_gpio))
+                gpio_set_value_cansleep(ddata->backlight_gpio, 1);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void lb035q02_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        if (ddata->enable_gpio)
+                gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
+        if (gpio_is_valid(ddata->backlight_gpio))
+                gpio_set_value_cansleep(ddata->backlight_gpio, 0);
+
+        in->ops.dpi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void lb035q02_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void lb035q02_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int lb035q02_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver lb035q02_ops = {
+        .connect        = lb035q02_connect,
+        .disconnect     = lb035q02_disconnect,
+
+        .enable         = lb035q02_enable,
+        .disable        = lb035q02_disable,
+
+        .set_timings    = lb035q02_set_timings,
+        .get_timings    = lb035q02_get_timings,
+        .check_timings  = lb035q02_check_timings,
+
+        .get_resolution = omapdss_default_get_resolution,
+};
+
+static int lb035q02_probe_pdata(struct spi_device *spi)
+{
+        const struct panel_lb035q02_platform_data *pdata;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev, *in;
+        int r;
+
+        pdata = dev_get_platdata(&spi->dev);
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&spi->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        ddata->data_lines = pdata->data_lines;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        r = devm_gpio_request_one(&spi->dev, pdata->enable_gpio,
+                                        GPIOF_OUT_INIT_LOW, "panel enable");
+        if (r)
+                goto err_gpio;
+
+        ddata->enable_gpio = gpio_to_desc(pdata->enable_gpio);
+
+        ddata->backlight_gpio = pdata->backlight_gpio;
+
+        return 0;
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int lb035q02_probe_of(struct spi_device *spi)
+{
+        struct device_node *node = spi->dev.of_node;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *in;
+        struct gpio_desc *gpio;
+
+        gpio = devm_gpiod_get(&spi->dev, "enable", GPIOD_OUT_LOW);
+        if (IS_ERR(gpio)) {
+                dev_err(&spi->dev, "failed to parse enable gpio\n");
+                return PTR_ERR(gpio);
+        }
+
+        ddata->enable_gpio = gpio;
+
+        ddata->backlight_gpio = -ENOENT;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&spi->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int lb035q02_panel_spi_probe(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        dev_set_drvdata(&spi->dev, ddata);
+
+        ddata->spi = spi;
+
+        if (dev_get_platdata(&spi->dev)) {
+                r = lb035q02_probe_pdata(spi);
+                if (r)
+                        return r;
+        } else if (spi->dev.of_node) {
+                r = lb035q02_probe_of(spi);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        if (gpio_is_valid(ddata->backlight_gpio)) {
+                r = devm_gpio_request_one(&spi->dev, ddata->backlight_gpio,
+                                GPIOF_OUT_INIT_LOW, "panel backlight");
+                if (r)
+                        goto err_gpio;
+        }
+
+        ddata->videomode = lb035q02_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &spi->dev;
+        dssdev->driver = &lb035q02_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+        dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&spi->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int lb035q02_panel_spi_remove(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_display(dssdev);
+
+        lb035q02_disable(dssdev);
+        lb035q02_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id lb035q02_of_match[] = {
+        { .compatible = "omapdss,lgphilips,lb035q02", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, lb035q02_of_match);
+
+static struct spi_driver lb035q02_spi_driver = {
+        .probe          = lb035q02_panel_spi_probe,
+        .remove         = lb035q02_panel_spi_remove,
+        .driver         = {
+                .name   = "panel_lgphilips_lb035q02",
+                .of_match_table = lb035q02_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_spi_driver(lb035q02_spi_driver);
+
+MODULE_ALIAS("spi:lgphilips,lb035q02");
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("LG.Philips LB035Q02 LCD Panel driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-nec-nl8048hl11.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-nec-nl8048hl11.c
new file mode 100644
index 000000000000..8a928c9a2fc9
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-nec-nl8048hl11.c
@@ -0,0 +1,437 @@
+/*
+ * NEC NL8048HL11 Panel driver
+ *
+ * Copyright (C) 2010 Texas Instruments Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ * Converted to new DSS device model: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+        struct omap_dss_device  dssdev;
+        struct omap_dss_device *in;
+
+        struct omap_video_timings videomode;
+
+        int data_lines;
+
+        int res_gpio;
+        int qvga_gpio;
+
+        struct spi_device *spi;
+};
+
+#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         23800000
+
+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 },
+};
+
+static const struct omap_video_timings nec_8048_panel_timings = {
+        .x_res          = LCD_XRES,
+        .y_res          = LCD_YRES,
+        .pixelclock     = LCD_PIXEL_CLOCK,
+        .hfp            = 6,
+        .hsw            = 1,
+        .hbp            = 4,
+        .vfp            = 3,
+        .vsw            = 1,
+        .vbp            = 4,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+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_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void nec_8048_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int nec_8048_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+        in->ops.dpi->set_timings(in, &ddata->videomode);
+
+        r = in->ops.dpi->enable(in);
+        if (r)
+                return r;
+
+        if (gpio_is_valid(ddata->res_gpio))
+                gpio_set_value_cansleep(ddata->res_gpio, 1);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void nec_8048_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        if (gpio_is_valid(ddata->res_gpio))
+                gpio_set_value_cansleep(ddata->res_gpio, 0);
+
+        in->ops.dpi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void nec_8048_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void nec_8048_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int nec_8048_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver nec_8048_ops = {
+        .connect        = nec_8048_connect,
+        .disconnect     = nec_8048_disconnect,
+
+        .enable         = nec_8048_enable,
+        .disable        = nec_8048_disable,
+
+        .set_timings    = nec_8048_set_timings,
+        .get_timings    = nec_8048_get_timings,
+        .check_timings  = nec_8048_check_timings,
+
+        .get_resolution = omapdss_default_get_resolution,
+};
+
+
+static int nec_8048_probe_pdata(struct spi_device *spi)
+{
+        const struct panel_nec_nl8048hl11_platform_data *pdata;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev, *in;
+
+        pdata = dev_get_platdata(&spi->dev);
+
+        ddata->qvga_gpio = pdata->qvga_gpio;
+        ddata->res_gpio = pdata->res_gpio;
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&spi->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+        ddata->in = in;
+
+        ddata->data_lines = pdata->data_lines;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int nec_8048_probe_of(struct spi_device *spi)
+{
+        struct device_node *node = spi->dev.of_node;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *in;
+        int gpio;
+
+        gpio = of_get_named_gpio(node, "reset-gpios", 0);
+        if (!gpio_is_valid(gpio)) {
+                dev_err(&spi->dev, "failed to parse enable gpio\n");
+                return gpio;
+        }
+        ddata->res_gpio = gpio;
+
+        /* XXX the panel spec doesn't mention any QVGA pin?? */
+        ddata->qvga_gpio = -ENOENT;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&spi->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int nec_8048_probe(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        dev_dbg(&spi->dev, "%s\n", __func__);
+
+        spi->mode = SPI_MODE_0;
+        spi->bits_per_word = 32;
+
+        r = spi_setup(spi);
+        if (r < 0) {
+                dev_err(&spi->dev, "spi_setup failed: %d\n", r);
+                return r;
+        }
+
+        init_nec_8048_wvga_lcd(spi);
+
+        ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        dev_set_drvdata(&spi->dev, ddata);
+
+        ddata->spi = spi;
+
+        if (dev_get_platdata(&spi->dev)) {
+                r = nec_8048_probe_pdata(spi);
+                if (r)
+                        return r;
+        } else if (spi->dev.of_node) {
+                r = nec_8048_probe_of(spi);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        if (gpio_is_valid(ddata->qvga_gpio)) {
+                r = devm_gpio_request_one(&spi->dev, ddata->qvga_gpio,
+                                GPIOF_OUT_INIT_HIGH, "lcd QVGA");
+                if (r)
+                        goto err_gpio;
+        }
+
+        if (gpio_is_valid(ddata->res_gpio)) {
+                r = devm_gpio_request_one(&spi->dev, ddata->res_gpio,
+                                GPIOF_OUT_INIT_LOW, "lcd RES");
+                if (r)
+                        goto err_gpio;
+        }
+
+        ddata->videomode = nec_8048_panel_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &spi->dev;
+        dssdev->driver = &nec_8048_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&spi->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int nec_8048_remove(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+        omapdss_unregister_display(dssdev);
+
+        nec_8048_disable(dssdev);
+        nec_8048_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int nec_8048_suspend(struct device *dev)
+{
+        struct spi_device *spi = to_spi_device(dev);
+
+        nec_8048_spi_send(spi, 2, 0x01);
+        mdelay(40);
+
+        return 0;
+}
+
+static int nec_8048_resume(struct device *dev)
+{
+        struct spi_device *spi = to_spi_device(dev);
+
+        /* reinitialize the panel */
+        spi_setup(spi);
+        nec_8048_spi_send(spi, 2, 0x00);
+        init_nec_8048_wvga_lcd(spi);
+
+        return 0;
+}
+static SIMPLE_DEV_PM_OPS(nec_8048_pm_ops, nec_8048_suspend,
+                nec_8048_resume);
+#define NEC_8048_PM_OPS (&nec_8048_pm_ops)
+#else
+#define NEC_8048_PM_OPS NULL
+#endif
+
+static const struct of_device_id nec_8048_of_match[] = {
+        { .compatible = "omapdss,nec,nl8048hl11", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, nec_8048_of_match);
+
+static struct spi_driver nec_8048_driver = {
+        .driver = {
+                .name   = "panel-nec-nl8048hl11",
+                .pm     = NEC_8048_PM_OPS,
+                .of_match_table = nec_8048_of_match,
+                .suppress_bind_attrs = true,
+        },
+        .probe  = nec_8048_probe,
+        .remove = nec_8048_remove,
+};
+
+module_spi_driver(nec_8048_driver);
+
+MODULE_ALIAS("spi:nec,nl8048hl11");
+MODULE_AUTHOR("Erik Gilling <konkers@android.com>");
+MODULE_DESCRIPTION("NEC-NL8048HL11 Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-sharp-ls037v7dw01.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-sharp-ls037v7dw01.c
new file mode 100644
index 000000000000..abfd1f6e3327
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-sharp-ls037v7dw01.c
@@ -0,0 +1,415 @@
+/*
+ * LCD panel driver for Sharp LS037V7DW01
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+        struct regulator *vcc;
+
+        int data_lines;
+
+        struct omap_video_timings videomode;
+
+        struct gpio_desc *resb_gpio;    /* low = reset active min 20 us */
+        struct gpio_desc *ini_gpio;     /* high = power on */
+        struct gpio_desc *mo_gpio;      /* low = 480x640, high = 240x320 */
+        struct gpio_desc *lr_gpio;      /* high = conventional horizontal scanning */
+        struct gpio_desc *ud_gpio;      /* high = conventional vertical scanning */
+};
+
+static const struct omap_video_timings sharp_ls_timings = {
+        .x_res = 480,
+        .y_res = 640,
+
+        .pixelclock     = 19200000,
+
+        .hsw            = 2,
+        .hfp            = 1,
+        .hbp            = 28,
+
+        .vsw            = 1,
+        .vfp            = 1,
+        .vbp            = 1,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int sharp_ls_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void sharp_ls_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int sharp_ls_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+        in->ops.dpi->set_timings(in, &ddata->videomode);
+
+        if (ddata->vcc) {
+                r = regulator_enable(ddata->vcc);
+                if (r != 0)
+                        return r;
+        }
+
+        r = in->ops.dpi->enable(in);
+        if (r) {
+                regulator_disable(ddata->vcc);
+                return r;
+        }
+
+        /* wait couple of vsyncs until enabling the LCD */
+        msleep(50);
+
+        if (ddata->resb_gpio)
+                gpiod_set_value_cansleep(ddata->resb_gpio, 1);
+
+        if (ddata->ini_gpio)
+                gpiod_set_value_cansleep(ddata->ini_gpio, 1);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void sharp_ls_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        if (ddata->ini_gpio)
+                gpiod_set_value_cansleep(ddata->ini_gpio, 0);
+
+        if (ddata->resb_gpio)
+                gpiod_set_value_cansleep(ddata->resb_gpio, 0);
+
+        /* wait at least 5 vsyncs after disabling the LCD */
+
+        msleep(100);
+
+        in->ops.dpi->disable(in);
+
+        if (ddata->vcc)
+                regulator_disable(ddata->vcc);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void sharp_ls_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void sharp_ls_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int sharp_ls_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver sharp_ls_ops = {
+        .connect        = sharp_ls_connect,
+        .disconnect     = sharp_ls_disconnect,
+
+        .enable         = sharp_ls_enable,
+        .disable        = sharp_ls_disable,
+
+        .set_timings    = sharp_ls_set_timings,
+        .get_timings    = sharp_ls_get_timings,
+        .check_timings  = sharp_ls_check_timings,
+
+        .get_resolution = omapdss_default_get_resolution,
+};
+
+static int sharp_ls_get_gpio(struct device *dev, int gpio, unsigned long flags,
+                  char *desc, struct gpio_desc **gpiod)
+{
+        struct gpio_desc *gd;
+        int r;
+
+        *gpiod = NULL;
+
+        r = devm_gpio_request_one(dev, gpio, flags, desc);
+        if (r)
+                return r == -ENOENT ? 0 : r;
+
+        gd = gpio_to_desc(gpio);
+        if (IS_ERR(gd))
+                return PTR_ERR(gd) == -ENOENT ? 0 : PTR_ERR(gd);
+
+        *gpiod = gd;
+        return 0;
+}
+
+static int sharp_ls_probe_pdata(struct platform_device *pdev)
+{
+        const struct panel_sharp_ls037v7dw01_platform_data *pdata;
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev, *in;
+        int r;
+
+        pdata = dev_get_platdata(&pdev->dev);
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&pdev->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        ddata->data_lines = pdata->data_lines;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        r = sharp_ls_get_gpio(&pdev->dev, pdata->mo_gpio, GPIOF_OUT_INIT_LOW,
+                "lcd MO", &ddata->mo_gpio);
+        if (r)
+                return r;
+        r = sharp_ls_get_gpio(&pdev->dev, pdata->lr_gpio, GPIOF_OUT_INIT_HIGH,
+                "lcd LR", &ddata->lr_gpio);
+        if (r)
+                return r;
+        r = sharp_ls_get_gpio(&pdev->dev, pdata->ud_gpio, GPIOF_OUT_INIT_HIGH,
+                "lcd UD", &ddata->ud_gpio);
+        if (r)
+                return r;
+        r = sharp_ls_get_gpio(&pdev->dev, pdata->resb_gpio, GPIOF_OUT_INIT_LOW,
+                "lcd RESB", &ddata->resb_gpio);
+        if (r)
+                return r;
+        r = sharp_ls_get_gpio(&pdev->dev, pdata->ini_gpio, GPIOF_OUT_INIT_LOW,
+                "lcd INI", &ddata->ini_gpio);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static  int sharp_ls_get_gpio_of(struct device *dev, int index, int val,
+        const char *desc, struct gpio_desc **gpiod)
+{
+        struct gpio_desc *gd;
+
+        *gpiod = NULL;
+
+        gd = devm_gpiod_get_index(dev, desc, index, GPIOD_OUT_LOW);
+        if (IS_ERR(gd))
+                return PTR_ERR(gd);
+
+        *gpiod = gd;
+        return 0;
+}
+
+static int sharp_ls_probe_of(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct device_node *node = pdev->dev.of_node;
+        struct omap_dss_device *in;
+        int r;
+
+        ddata->vcc = devm_regulator_get(&pdev->dev, "envdd");
+        if (IS_ERR(ddata->vcc)) {
+                dev_err(&pdev->dev, "failed to get regulator\n");
+                return PTR_ERR(ddata->vcc);
+        }
+
+        /* lcd INI */
+        r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "enable", &ddata->ini_gpio);
+        if (r)
+                return r;
+
+        /* lcd RESB */
+        r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "reset", &ddata->resb_gpio);
+        if (r)
+                return r;
+
+        /* lcd MO */
+        r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "mode", &ddata->mo_gpio);
+        if (r)
+                return r;
+
+        /* lcd LR */
+        r = sharp_ls_get_gpio_of(&pdev->dev, 1, 1, "mode", &ddata->lr_gpio);
+        if (r)
+                return r;
+
+        /* lcd UD */
+        r = sharp_ls_get_gpio_of(&pdev->dev, 2, 1, "mode", &ddata->ud_gpio);
+        if (r)
+                return r;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&pdev->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int sharp_ls_probe(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, ddata);
+
+        if (dev_get_platdata(&pdev->dev)) {
+                r = sharp_ls_probe_pdata(pdev);
+                if (r)
+                        return r;
+        } else if (pdev->dev.of_node) {
+                r = sharp_ls_probe_of(pdev);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        ddata->videomode = sharp_ls_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &pdev->dev;
+        dssdev->driver = &sharp_ls_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+        dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&pdev->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int __exit sharp_ls_remove(struct platform_device *pdev)
+{
+        struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        omapdss_unregister_display(dssdev);
+
+        sharp_ls_disable(dssdev);
+        sharp_ls_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id sharp_ls_of_match[] = {
+        { .compatible = "omapdss,sharp,ls037v7dw01", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, sharp_ls_of_match);
+
+static struct platform_driver sharp_ls_driver = {
+        .probe = sharp_ls_probe,
+        .remove = __exit_p(sharp_ls_remove),
+        .driver = {
+                .name = "panel-sharp-ls037v7dw01",
+                .of_match_table = sharp_ls_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_platform_driver(sharp_ls_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Sharp LS037V7DW01 Panel Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-sony-acx565akm.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-sony-acx565akm.c
new file mode 100644
index 000000000000..31efcca801bd
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-sony-acx565akm.c
@@ -0,0 +1,917 @@
+/*
+ * Sony ACX565AKM LCD Panel driver
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ *
+ * Original Driver Author: Imre Deak <imre.deak@nokia.com>
+ * Based on panel-generic.c by Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ * Adapted to new DSS2 framework: Roger Quadros <roger.quadros@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/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include <linux/backlight.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+#define MIPID_CMD_READ_DISP_ID          0x04
+#define MIPID_CMD_READ_RED              0x06
+#define MIPID_CMD_READ_GREEN            0x07
+#define MIPID_CMD_READ_BLUE             0x08
+#define MIPID_CMD_READ_DISP_STATUS      0x09
+#define MIPID_CMD_RDDSDR                0x0F
+#define MIPID_CMD_SLEEP_IN              0x10
+#define MIPID_CMD_SLEEP_OUT             0x11
+#define MIPID_CMD_DISP_OFF              0x28
+#define MIPID_CMD_DISP_ON               0x29
+#define MIPID_CMD_WRITE_DISP_BRIGHTNESS 0x51
+#define MIPID_CMD_READ_DISP_BRIGHTNESS  0x52
+#define MIPID_CMD_WRITE_CTRL_DISP       0x53
+
+#define CTRL_DISP_BRIGHTNESS_CTRL_ON    (1 << 5)
+#define CTRL_DISP_AMBIENT_LIGHT_CTRL_ON (1 << 4)
+#define CTRL_DISP_BACKLIGHT_ON          (1 << 2)
+#define CTRL_DISP_AUTO_BRIGHTNESS_ON    (1 << 1)
+
+#define MIPID_CMD_READ_CTRL_DISP        0x54
+#define MIPID_CMD_WRITE_CABC            0x55
+#define MIPID_CMD_READ_CABC             0x56
+
+#define MIPID_VER_LPH8923               3
+#define MIPID_VER_LS041Y3               4
+#define MIPID_VER_L4F00311              8
+#define MIPID_VER_ACX565AKM             9
+
+struct panel_drv_data {
+        struct omap_dss_device  dssdev;
+        struct omap_dss_device *in;
+
+        int reset_gpio;
+        int datapairs;
+
+        struct omap_video_timings videomode;
+
+        char            *name;
+        int             enabled;
+        int             model;
+        int             revision;
+        u8              display_id[3];
+        unsigned        has_bc:1;
+        unsigned        has_cabc:1;
+        unsigned        cabc_mode;
+        unsigned long   hw_guard_end;           /* next value of jiffies
+                                                   when we can issue the
+                                                   next sleep in/out command */
+        unsigned long   hw_guard_wait;          /* max guard time in jiffies */
+
+        struct spi_device       *spi;
+        struct mutex            mutex;
+
+        struct backlight_device *bl_dev;
+};
+
+static const struct omap_video_timings acx565akm_panel_timings = {
+        .x_res          = 800,
+        .y_res          = 480,
+        .pixelclock     = 24000000,
+        .hfp            = 28,
+        .hsw            = 4,
+        .hbp            = 24,
+        .vfp            = 3,
+        .vsw            = 3,
+        .vbp            = 4,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static void acx565akm_transfer(struct panel_drv_data *ddata, int cmd,
+                              const u8 *wbuf, int wlen, u8 *rbuf, int rlen)
+{
+        struct spi_message      m;
+        struct spi_transfer     *x, xfer[5];
+        int                     r;
+
+        BUG_ON(ddata->spi == NULL);
+
+        spi_message_init(&m);
+
+        memset(xfer, 0, sizeof(xfer));
+        x = &xfer[0];
+
+        cmd &=  0xff;
+        x->tx_buf = &cmd;
+        x->bits_per_word = 9;
+        x->len = 2;
+
+        if (rlen > 1 && wlen == 0) {
+                /*
+                 * Between the command and the response data there is a
+                 * dummy clock cycle. Add an extra bit after the command
+                 * word to account for this.
+                 */
+                x->bits_per_word = 10;
+                cmd <<= 1;
+        }
+        spi_message_add_tail(x, &m);
+
+        if (wlen) {
+                x++;
+                x->tx_buf = wbuf;
+                x->len = wlen;
+                x->bits_per_word = 9;
+                spi_message_add_tail(x, &m);
+        }
+
+        if (rlen) {
+                x++;
+                x->rx_buf       = rbuf;
+                x->len          = rlen;
+                spi_message_add_tail(x, &m);
+        }
+
+        r = spi_sync(ddata->spi, &m);
+        if (r < 0)
+                dev_dbg(&ddata->spi->dev, "spi_sync %d\n", r);
+}
+
+static inline void acx565akm_cmd(struct panel_drv_data *ddata, int cmd)
+{
+        acx565akm_transfer(ddata, cmd, NULL, 0, NULL, 0);
+}
+
+static inline void acx565akm_write(struct panel_drv_data *ddata,
+                               int reg, const u8 *buf, int len)
+{
+        acx565akm_transfer(ddata, reg, buf, len, NULL, 0);
+}
+
+static inline void acx565akm_read(struct panel_drv_data *ddata,
+                              int reg, u8 *buf, int len)
+{
+        acx565akm_transfer(ddata, reg, NULL, 0, buf, len);
+}
+
+static void hw_guard_start(struct panel_drv_data *ddata, int guard_msec)
+{
+        ddata->hw_guard_wait = msecs_to_jiffies(guard_msec);
+        ddata->hw_guard_end = jiffies + ddata->hw_guard_wait;
+}
+
+static void hw_guard_wait(struct panel_drv_data *ddata)
+{
+        unsigned long wait = ddata->hw_guard_end - jiffies;
+
+        if ((long)wait > 0 && wait <= ddata->hw_guard_wait) {
+                set_current_state(TASK_UNINTERRUPTIBLE);
+                schedule_timeout(wait);
+        }
+}
+
+static void set_sleep_mode(struct panel_drv_data *ddata, int on)
+{
+        int cmd;
+
+        if (on)
+                cmd = MIPID_CMD_SLEEP_IN;
+        else
+                cmd = MIPID_CMD_SLEEP_OUT;
+        /*
+         * We have to keep 120msec between sleep in/out commands.
+         * (8.2.15, 8.2.16).
+         */
+        hw_guard_wait(ddata);
+        acx565akm_cmd(ddata, cmd);
+        hw_guard_start(ddata, 120);
+}
+
+static void set_display_state(struct panel_drv_data *ddata, int enabled)
+{
+        int cmd = enabled ? MIPID_CMD_DISP_ON : MIPID_CMD_DISP_OFF;
+
+        acx565akm_cmd(ddata, cmd);
+}
+
+static int panel_enabled(struct panel_drv_data *ddata)
+{
+        u32 disp_status;
+        int enabled;
+
+        acx565akm_read(ddata, MIPID_CMD_READ_DISP_STATUS,
+                        (u8 *)&disp_status, 4);
+        disp_status = __be32_to_cpu(disp_status);
+        enabled = (disp_status & (1 << 17)) && (disp_status & (1 << 10));
+        dev_dbg(&ddata->spi->dev,
+                "LCD panel %senabled by bootloader (status 0x%04x)\n",
+                enabled ? "" : "not ", disp_status);
+        return enabled;
+}
+
+static int panel_detect(struct panel_drv_data *ddata)
+{
+        acx565akm_read(ddata, MIPID_CMD_READ_DISP_ID, ddata->display_id, 3);
+        dev_dbg(&ddata->spi->dev, "MIPI display ID: %02x%02x%02x\n",
+                ddata->display_id[0],
+                ddata->display_id[1],
+                ddata->display_id[2]);
+
+        switch (ddata->display_id[0]) {
+        case 0x10:
+                ddata->model = MIPID_VER_ACX565AKM;
+                ddata->name = "acx565akm";
+                ddata->has_bc = 1;
+                ddata->has_cabc = 1;
+                break;
+        case 0x29:
+                ddata->model = MIPID_VER_L4F00311;
+                ddata->name = "l4f00311";
+                break;
+        case 0x45:
+                ddata->model = MIPID_VER_LPH8923;
+                ddata->name = "lph8923";
+                break;
+        case 0x83:
+                ddata->model = MIPID_VER_LS041Y3;
+                ddata->name = "ls041y3";
+                break;
+        default:
+                ddata->name = "unknown";
+                dev_err(&ddata->spi->dev, "invalid display ID\n");
+                return -ENODEV;
+        }
+
+        ddata->revision = ddata->display_id[1];
+
+        dev_info(&ddata->spi->dev, "omapfb: %s rev %02x LCD detected\n",
+                        ddata->name, ddata->revision);
+
+        return 0;
+}
+
+/*----------------------Backlight Control-------------------------*/
+
+static void enable_backlight_ctrl(struct panel_drv_data *ddata, int enable)
+{
+        u16 ctrl;
+
+        acx565akm_read(ddata, MIPID_CMD_READ_CTRL_DISP, (u8 *)&ctrl, 1);
+        if (enable) {
+                ctrl |= CTRL_DISP_BRIGHTNESS_CTRL_ON |
+                        CTRL_DISP_BACKLIGHT_ON;
+        } else {
+                ctrl &= ~(CTRL_DISP_BRIGHTNESS_CTRL_ON |
+                          CTRL_DISP_BACKLIGHT_ON);
+        }
+
+        ctrl |= 1 << 8;
+        acx565akm_write(ddata, MIPID_CMD_WRITE_CTRL_DISP, (u8 *)&ctrl, 2);
+}
+
+static void set_cabc_mode(struct panel_drv_data *ddata, unsigned mode)
+{
+        u16 cabc_ctrl;
+
+        ddata->cabc_mode = mode;
+        if (!ddata->enabled)
+                return;
+        cabc_ctrl = 0;
+        acx565akm_read(ddata, MIPID_CMD_READ_CABC, (u8 *)&cabc_ctrl, 1);
+        cabc_ctrl &= ~3;
+        cabc_ctrl |= (1 << 8) | (mode & 3);
+        acx565akm_write(ddata, MIPID_CMD_WRITE_CABC, (u8 *)&cabc_ctrl, 2);
+}
+
+static unsigned get_cabc_mode(struct panel_drv_data *ddata)
+{
+        return ddata->cabc_mode;
+}
+
+static unsigned get_hw_cabc_mode(struct panel_drv_data *ddata)
+{
+        u8 cabc_ctrl;
+
+        acx565akm_read(ddata, MIPID_CMD_READ_CABC, &cabc_ctrl, 1);
+        return cabc_ctrl & 3;
+}
+
+static void acx565akm_set_brightness(struct panel_drv_data *ddata, int level)
+{
+        int bv;
+
+        bv = level | (1 << 8);
+        acx565akm_write(ddata, MIPID_CMD_WRITE_DISP_BRIGHTNESS, (u8 *)&bv, 2);
+
+        if (level)
+                enable_backlight_ctrl(ddata, 1);
+        else
+                enable_backlight_ctrl(ddata, 0);
+}
+
+static int acx565akm_get_actual_brightness(struct panel_drv_data *ddata)
+{
+        u8 bv;
+
+        acx565akm_read(ddata, MIPID_CMD_READ_DISP_BRIGHTNESS, &bv, 1);
+
+        return bv;
+}
+
+
+static int acx565akm_bl_update_status(struct backlight_device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+        int level;
+
+        dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+        if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+                        dev->props.power == FB_BLANK_UNBLANK)
+                level = dev->props.brightness;
+        else
+                level = 0;
+
+        if (ddata->has_bc)
+                acx565akm_set_brightness(ddata, level);
+        else
+                return -ENODEV;
+
+        return 0;
+}
+
+static int acx565akm_bl_get_intensity(struct backlight_device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+
+        dev_dbg(&dev->dev, "%s\n", __func__);
+
+        if (!ddata->has_bc)
+                return -ENODEV;
+
+        if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+                        dev->props.power == FB_BLANK_UNBLANK) {
+                if (ddata->has_bc)
+                        return acx565akm_get_actual_brightness(ddata);
+                else
+                        return dev->props.brightness;
+        }
+
+        return 0;
+}
+
+static int acx565akm_bl_update_status_locked(struct backlight_device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+        int r;
+
+        mutex_lock(&ddata->mutex);
+        r = acx565akm_bl_update_status(dev);
+        mutex_unlock(&ddata->mutex);
+
+        return r;
+}
+
+static int acx565akm_bl_get_intensity_locked(struct backlight_device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+        int r;
+
+        mutex_lock(&ddata->mutex);
+        r = acx565akm_bl_get_intensity(dev);
+        mutex_unlock(&ddata->mutex);
+
+        return r;
+}
+
+static const struct backlight_ops acx565akm_bl_ops = {
+        .get_brightness = acx565akm_bl_get_intensity_locked,
+        .update_status  = acx565akm_bl_update_status_locked,
+};
+
+/*--------------------Auto Brightness control via Sysfs---------------------*/
+
+static const char * const cabc_modes[] = {
+        "off",          /* always used when CABC is not supported */
+        "ui",
+        "still-image",
+        "moving-image",
+};
+
+static ssize_t show_cabc_mode(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        const char *mode_str;
+        int mode;
+        int len;
+
+        if (!ddata->has_cabc)
+                mode = 0;
+        else
+                mode = get_cabc_mode(ddata);
+        mode_str = "unknown";
+        if (mode >= 0 && mode < ARRAY_SIZE(cabc_modes))
+                mode_str = cabc_modes[mode];
+        len = snprintf(buf, PAGE_SIZE, "%s\n", mode_str);
+
+        return len < PAGE_SIZE - 1 ? len : PAGE_SIZE - 1;
+}
+
+static ssize_t store_cabc_mode(struct device *dev,
+                struct device_attribute *attr,
+                const char *buf, size_t count)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(cabc_modes); i++) {
+                const char *mode_str = cabc_modes[i];
+                int cmp_len = strlen(mode_str);
+
+                if (count > 0 && buf[count - 1] == '\n')
+                        count--;
+                if (count != cmp_len)
+                        continue;
+
+                if (strncmp(buf, mode_str, cmp_len) == 0)
+                        break;
+        }
+
+        if (i == ARRAY_SIZE(cabc_modes))
+                return -EINVAL;
+
+        if (!ddata->has_cabc && i != 0)
+                return -EINVAL;
+
+        mutex_lock(&ddata->mutex);
+        set_cabc_mode(ddata, i);
+        mutex_unlock(&ddata->mutex);
+
+        return count;
+}
+
+static ssize_t show_cabc_available_modes(struct device *dev,
+                struct device_attribute *attr,
+                char *buf)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        int len;
+        int i;
+
+        if (!ddata->has_cabc)
+                return snprintf(buf, PAGE_SIZE, "%s\n", cabc_modes[0]);
+
+        for (i = 0, len = 0;
+             len < PAGE_SIZE && i < ARRAY_SIZE(cabc_modes); i++)
+                len += snprintf(&buf[len], PAGE_SIZE - len, "%s%s%s",
+                        i ? " " : "", cabc_modes[i],
+                        i == ARRAY_SIZE(cabc_modes) - 1 ? "\n" : "");
+
+        return len < PAGE_SIZE ? len : PAGE_SIZE - 1;
+}
+
+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 struct attribute *bldev_attrs[] = {
+        &dev_attr_cabc_mode.attr,
+        &dev_attr_cabc_available_modes.attr,
+        NULL,
+};
+
+static struct attribute_group bldev_attr_group = {
+        .attrs = bldev_attrs,
+};
+
+static int acx565akm_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.sdi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void acx565akm_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.sdi->disconnect(in, dssdev);
+}
+
+static int acx565akm_panel_power_on(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+        in->ops.sdi->set_timings(in, &ddata->videomode);
+
+        if (ddata->datapairs > 0)
+                in->ops.sdi->set_datapairs(in, ddata->datapairs);
+
+        r = in->ops.sdi->enable(in);
+        if (r) {
+                pr_err("%s sdi enable failed\n", __func__);
+                return r;
+        }
+
+        /*FIXME tweak me */
+        msleep(50);
+
+        if (gpio_is_valid(ddata->reset_gpio))
+                gpio_set_value(ddata->reset_gpio, 1);
+
+        if (ddata->enabled) {
+                dev_dbg(&ddata->spi->dev, "panel already enabled\n");
+                return 0;
+        }
+
+        /*
+         * We have to meet all the following delay requirements:
+         * 1. tRW: reset pulse width 10usec (7.12.1)
+         * 2. tRT: reset cancel time 5msec (7.12.1)
+         * 3. Providing PCLK,HS,VS signals for 2 frames = ~50msec worst
+         *    case (7.6.2)
+         * 4. 120msec before the sleep out command (7.12.1)
+         */
+        msleep(120);
+
+        set_sleep_mode(ddata, 0);
+        ddata->enabled = 1;
+
+        /* 5msec between sleep out and the next command. (8.2.16) */
+        usleep_range(5000, 10000);
+        set_display_state(ddata, 1);
+        set_cabc_mode(ddata, ddata->cabc_mode);
+
+        return acx565akm_bl_update_status(ddata->bl_dev);
+}
+
+static void acx565akm_panel_power_off(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(dssdev->dev, "%s\n", __func__);
+
+        if (!ddata->enabled)
+                return;
+
+        set_display_state(ddata, 0);
+        set_sleep_mode(ddata, 1);
+        ddata->enabled = 0;
+        /*
+         * We have to provide PCLK,HS,VS signals for 2 frames (worst case
+         * ~50msec) after sending the sleep in command and asserting the
+         * reset signal. We probably could assert the reset w/o the delay
+         * but we still delay to avoid possible artifacts. (7.6.1)
+         */
+        msleep(50);
+
+        if (gpio_is_valid(ddata->reset_gpio))
+                gpio_set_value(ddata->reset_gpio, 0);
+
+        /* FIXME need to tweak this delay */
+        msleep(100);
+
+        in->ops.sdi->disable(in);
+}
+
+static int acx565akm_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        int r;
+
+        dev_dbg(dssdev->dev, "%s\n", __func__);
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        mutex_lock(&ddata->mutex);
+        r = acx565akm_panel_power_on(dssdev);
+        mutex_unlock(&ddata->mutex);
+        if (r)
+                return r;
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void acx565akm_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        dev_dbg(dssdev->dev, "%s\n", __func__);
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        mutex_lock(&ddata->mutex);
+        acx565akm_panel_power_off(dssdev);
+        mutex_unlock(&ddata->mutex);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void acx565akm_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.sdi->set_timings(in, timings);
+}
+
+static void acx565akm_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int acx565akm_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.sdi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver acx565akm_ops = {
+        .connect        = acx565akm_connect,
+        .disconnect     = acx565akm_disconnect,
+
+        .enable         = acx565akm_enable,
+        .disable        = acx565akm_disable,
+
+        .set_timings    = acx565akm_set_timings,
+        .get_timings    = acx565akm_get_timings,
+        .check_timings  = acx565akm_check_timings,
+
+        .get_resolution = omapdss_default_get_resolution,
+};
+
+static int acx565akm_probe_pdata(struct spi_device *spi)
+{
+        const struct panel_acx565akm_platform_data *pdata;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev, *in;
+
+        pdata = dev_get_platdata(&spi->dev);
+
+        ddata->reset_gpio = pdata->reset_gpio;
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&spi->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+        ddata->in = in;
+
+        ddata->datapairs = pdata->datapairs;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int acx565akm_probe_of(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct device_node *np = spi->dev.of_node;
+
+        ddata->reset_gpio = of_get_named_gpio(np, "reset-gpios", 0);
+
+        ddata->in = omapdss_of_find_source_for_first_ep(np);
+        if (IS_ERR(ddata->in)) {
+                dev_err(&spi->dev, "failed to find video source\n");
+                return PTR_ERR(ddata->in);
+        }
+
+        return 0;
+}
+
+static int acx565akm_probe(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        struct backlight_device *bldev;
+        int max_brightness, brightness;
+        struct backlight_properties props;
+        int r;
+
+        dev_dbg(&spi->dev, "%s\n", __func__);
+
+        spi->mode = SPI_MODE_3;
+
+        ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        dev_set_drvdata(&spi->dev, ddata);
+
+        ddata->spi = spi;
+
+        mutex_init(&ddata->mutex);
+
+        if (dev_get_platdata(&spi->dev)) {
+                r = acx565akm_probe_pdata(spi);
+                if (r)
+                        return r;
+        } else if (spi->dev.of_node) {
+                r = acx565akm_probe_of(spi);
+                if (r)
+                        return r;
+        } else {
+                dev_err(&spi->dev, "platform data missing!\n");
+                return -ENODEV;
+        }
+
+        if (gpio_is_valid(ddata->reset_gpio)) {
+                r = devm_gpio_request_one(&spi->dev, ddata->reset_gpio,
+                                GPIOF_OUT_INIT_LOW, "lcd reset");
+                if (r)
+                        goto err_gpio;
+        }
+
+        if (gpio_is_valid(ddata->reset_gpio))
+                gpio_set_value(ddata->reset_gpio, 1);
+
+        /*
+         * After reset we have to wait 5 msec before the first
+         * command can be sent.
+         */
+        usleep_range(5000, 10000);
+
+        ddata->enabled = panel_enabled(ddata);
+
+        r = panel_detect(ddata);
+
+        if (!ddata->enabled && gpio_is_valid(ddata->reset_gpio))
+                gpio_set_value(ddata->reset_gpio, 0);
+
+        if (r) {
+                dev_err(&spi->dev, "%s panel detect error\n", __func__);
+                goto err_detect;
+        }
+
+        memset(&props, 0, sizeof(props));
+        props.fb_blank = FB_BLANK_UNBLANK;
+        props.power = FB_BLANK_UNBLANK;
+        props.type = BACKLIGHT_RAW;
+
+        bldev = backlight_device_register("acx565akm", &ddata->spi->dev,
+                        ddata, &acx565akm_bl_ops, &props);
+        if (IS_ERR(bldev)) {
+                r = PTR_ERR(bldev);
+                goto err_reg_bl;
+        }
+        ddata->bl_dev = bldev;
+        if (ddata->has_cabc) {
+                r = sysfs_create_group(&bldev->dev.kobj, &bldev_attr_group);
+                if (r) {
+                        dev_err(&bldev->dev,
+                                "%s failed to create sysfs files\n", __func__);
+                        goto err_sysfs;
+                }
+                ddata->cabc_mode = get_hw_cabc_mode(ddata);
+        }
+
+        max_brightness = 255;
+
+        if (ddata->has_bc)
+                brightness = acx565akm_get_actual_brightness(ddata);
+        else
+                brightness = 0;
+
+        bldev->props.max_brightness = max_brightness;
+        bldev->props.brightness = brightness;
+
+        acx565akm_bl_update_status(bldev);
+
+
+        ddata->videomode = acx565akm_panel_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &spi->dev;
+        dssdev->driver = &acx565akm_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_SDI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&spi->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        sysfs_remove_group(&bldev->dev.kobj, &bldev_attr_group);
+err_sysfs:
+        backlight_device_unregister(bldev);
+err_reg_bl:
+err_detect:
+err_gpio:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int acx565akm_remove(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+        sysfs_remove_group(&ddata->bl_dev->dev.kobj, &bldev_attr_group);
+        backlight_device_unregister(ddata->bl_dev);
+
+        omapdss_unregister_display(dssdev);
+
+        acx565akm_disable(dssdev);
+        acx565akm_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id acx565akm_of_match[] = {
+        { .compatible = "omapdss,sony,acx565akm", },
+        {},
+};
+MODULE_DEVICE_TABLE(of, acx565akm_of_match);
+
+static struct spi_driver acx565akm_driver = {
+        .driver = {
+                .name   = "acx565akm",
+                .of_match_table = acx565akm_of_match,
+                .suppress_bind_attrs = true,
+        },
+        .probe  = acx565akm_probe,
+        .remove = acx565akm_remove,
+};
+
+module_spi_driver(acx565akm_driver);
+
+MODULE_AUTHOR("Nokia Corporation");
+MODULE_DESCRIPTION("acx565akm LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td028ttec1.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td028ttec1.c
new file mode 100644
index 000000000000..4d657f3ab679
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td028ttec1.c
@@ -0,0 +1,511 @@
+/*
+ * Toppoly TD028TTEC1 panel support
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Neo 1973 code (jbt6k74.c):
+ * Copyright (C) 2006-2007 by OpenMoko, Inc.
+ * Author: Harald Welte <laforge@openmoko.org>
+ *
+ * Ported and adapted from Neo 1973 U-Boot by:
+ * H. Nikolaus Schaller <hns@goldelico.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/gpio.h>
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+        struct omap_dss_device dssdev;
+        struct omap_dss_device *in;
+
+        int data_lines;
+
+        struct omap_video_timings videomode;
+
+        struct spi_device *spi_dev;
+};
+
+static struct omap_video_timings td028ttec1_panel_timings = {
+        .x_res          = 480,
+        .y_res          = 640,
+        .pixelclock     = 22153000,
+        .hfp            = 24,
+        .hsw            = 8,
+        .hbp            = 8,
+        .vfp            = 4,
+        .vsw            = 2,
+        .vbp            = 2,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+};
+
+#define JBT_COMMAND     0x000
+#define JBT_DATA        0x100
+
+static int jbt_ret_write_0(struct panel_drv_data *ddata, u8 reg)
+{
+        int rc;
+        u16 tx_buf = JBT_COMMAND | reg;
+
+        rc = spi_write(ddata->spi_dev, (u8 *)&tx_buf,
+                        1*sizeof(u16));
+        if (rc != 0)
+                dev_err(&ddata->spi_dev->dev,
+                        "jbt_ret_write_0 spi_write ret %d\n", rc);
+
+        return rc;
+}
+
+static int jbt_reg_write_1(struct panel_drv_data *ddata, u8 reg, u8 data)
+{
+        int rc;
+        u16 tx_buf[2];
+
+        tx_buf[0] = JBT_COMMAND | reg;
+        tx_buf[1] = JBT_DATA | data;
+        rc = spi_write(ddata->spi_dev, (u8 *)tx_buf,
+                        2*sizeof(u16));
+        if (rc != 0)
+                dev_err(&ddata->spi_dev->dev,
+                        "jbt_reg_write_1 spi_write ret %d\n", rc);
+
+        return rc;
+}
+
+static int jbt_reg_write_2(struct panel_drv_data *ddata, u8 reg, u16 data)
+{
+        int rc;
+        u16 tx_buf[3];
+
+        tx_buf[0] = JBT_COMMAND | reg;
+        tx_buf[1] = JBT_DATA | (data >> 8);
+        tx_buf[2] = JBT_DATA | (data & 0xff);
+
+        rc = spi_write(ddata->spi_dev, (u8 *)tx_buf,
+                        3*sizeof(u16));
+
+        if (rc != 0)
+                dev_err(&ddata->spi_dev->dev,
+                        "jbt_reg_write_2 spi_write ret %d\n", rc);
+
+        return rc;
+}
+
+enum jbt_register {
+        JBT_REG_SLEEP_IN                = 0x10,
+        JBT_REG_SLEEP_OUT               = 0x11,
+
+        JBT_REG_DISPLAY_OFF             = 0x28,
+        JBT_REG_DISPLAY_ON              = 0x29,
+
+        JBT_REG_RGB_FORMAT              = 0x3a,
+        JBT_REG_QUAD_RATE               = 0x3b,
+
+        JBT_REG_POWER_ON_OFF            = 0xb0,
+        JBT_REG_BOOSTER_OP              = 0xb1,
+        JBT_REG_BOOSTER_MODE            = 0xb2,
+        JBT_REG_BOOSTER_FREQ            = 0xb3,
+        JBT_REG_OPAMP_SYSCLK            = 0xb4,
+        JBT_REG_VSC_VOLTAGE             = 0xb5,
+        JBT_REG_VCOM_VOLTAGE            = 0xb6,
+        JBT_REG_EXT_DISPL               = 0xb7,
+        JBT_REG_OUTPUT_CONTROL          = 0xb8,
+        JBT_REG_DCCLK_DCEV              = 0xb9,
+        JBT_REG_DISPLAY_MODE1           = 0xba,
+        JBT_REG_DISPLAY_MODE2           = 0xbb,
+        JBT_REG_DISPLAY_MODE            = 0xbc,
+        JBT_REG_ASW_SLEW                = 0xbd,
+        JBT_REG_DUMMY_DISPLAY           = 0xbe,
+        JBT_REG_DRIVE_SYSTEM            = 0xbf,
+
+        JBT_REG_SLEEP_OUT_FR_A          = 0xc0,
+        JBT_REG_SLEEP_OUT_FR_B          = 0xc1,
+        JBT_REG_SLEEP_OUT_FR_C          = 0xc2,
+        JBT_REG_SLEEP_IN_LCCNT_D        = 0xc3,
+        JBT_REG_SLEEP_IN_LCCNT_E        = 0xc4,
+        JBT_REG_SLEEP_IN_LCCNT_F        = 0xc5,
+        JBT_REG_SLEEP_IN_LCCNT_G        = 0xc6,
+
+        JBT_REG_GAMMA1_FINE_1           = 0xc7,
+        JBT_REG_GAMMA1_FINE_2           = 0xc8,
+        JBT_REG_GAMMA1_INCLINATION      = 0xc9,
+        JBT_REG_GAMMA1_BLUE_OFFSET      = 0xca,
+
+        JBT_REG_BLANK_CONTROL           = 0xcf,
+        JBT_REG_BLANK_TH_TV             = 0xd0,
+        JBT_REG_CKV_ON_OFF              = 0xd1,
+        JBT_REG_CKV_1_2                 = 0xd2,
+        JBT_REG_OEV_TIMING              = 0xd3,
+        JBT_REG_ASW_TIMING_1            = 0xd4,
+        JBT_REG_ASW_TIMING_2            = 0xd5,
+
+        JBT_REG_HCLOCK_VGA              = 0xec,
+        JBT_REG_HCLOCK_QVGA             = 0xed,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int td028ttec1_panel_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void td028ttec1_panel_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int td028ttec1_panel_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+        in->ops.dpi->set_timings(in, &ddata->videomode);
+
+        r = in->ops.dpi->enable(in);
+        if (r)
+                return r;
+
+        dev_dbg(dssdev->dev, "td028ttec1_panel_enable() - state %d\n",
+                dssdev->state);
+
+        /* three times command zero */
+        r |= jbt_ret_write_0(ddata, 0x00);
+        usleep_range(1000, 2000);
+        r |= jbt_ret_write_0(ddata, 0x00);
+        usleep_range(1000, 2000);
+        r |= jbt_ret_write_0(ddata, 0x00);
+        usleep_range(1000, 2000);
+
+        if (r) {
+                dev_warn(dssdev->dev, "transfer error\n");
+                goto transfer_err;
+        }
+
+        /* deep standby out */
+        r |= jbt_reg_write_1(ddata, JBT_REG_POWER_ON_OFF, 0x17);
+
+        /* RGB I/F on, RAM write off, QVGA through, SIGCON enable */
+        r |= jbt_reg_write_1(ddata, JBT_REG_DISPLAY_MODE, 0x80);
+
+        /* Quad mode off */
+        r |= jbt_reg_write_1(ddata, JBT_REG_QUAD_RATE, 0x00);
+
+        /* AVDD on, XVDD on */
+        r |= jbt_reg_write_1(ddata, JBT_REG_POWER_ON_OFF, 0x16);
+
+        /* Output control */
+        r |= jbt_reg_write_2(ddata, JBT_REG_OUTPUT_CONTROL, 0xfff9);
+
+        /* Sleep mode off */
+        r |= jbt_ret_write_0(ddata, JBT_REG_SLEEP_OUT);
+
+        /* at this point we have like 50% grey */
+
+        /* initialize register set */
+        r |= jbt_reg_write_1(ddata, JBT_REG_DISPLAY_MODE1, 0x01);
+        r |= jbt_reg_write_1(ddata, JBT_REG_DISPLAY_MODE2, 0x00);
+        r |= jbt_reg_write_1(ddata, JBT_REG_RGB_FORMAT, 0x60);
+        r |= jbt_reg_write_1(ddata, JBT_REG_DRIVE_SYSTEM, 0x10);
+        r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_OP, 0x56);
+        r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_MODE, 0x33);
+        r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_FREQ, 0x11);
+        r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_FREQ, 0x11);
+        r |= jbt_reg_write_1(ddata, JBT_REG_OPAMP_SYSCLK, 0x02);
+        r |= jbt_reg_write_1(ddata, JBT_REG_VSC_VOLTAGE, 0x2b);
+        r |= jbt_reg_write_1(ddata, JBT_REG_VCOM_VOLTAGE, 0x40);
+        r |= jbt_reg_write_1(ddata, JBT_REG_EXT_DISPL, 0x03);
+        r |= jbt_reg_write_1(ddata, JBT_REG_DCCLK_DCEV, 0x04);
+        /*
+         * default of 0x02 in JBT_REG_ASW_SLEW responsible for 72Hz requirement
+         * to avoid red / blue flicker
+         */
+        r |= jbt_reg_write_1(ddata, JBT_REG_ASW_SLEW, 0x04);
+        r |= jbt_reg_write_1(ddata, JBT_REG_DUMMY_DISPLAY, 0x00);
+
+        r |= jbt_reg_write_1(ddata, JBT_REG_SLEEP_OUT_FR_A, 0x11);
+        r |= jbt_reg_write_1(ddata, JBT_REG_SLEEP_OUT_FR_B, 0x11);
+        r |= jbt_reg_write_1(ddata, JBT_REG_SLEEP_OUT_FR_C, 0x11);
+        r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_D, 0x2040);
+        r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_E, 0x60c0);
+        r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_F, 0x1020);
+        r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_G, 0x60c0);
+
+        r |= jbt_reg_write_2(ddata, JBT_REG_GAMMA1_FINE_1, 0x5533);
+        r |= jbt_reg_write_1(ddata, JBT_REG_GAMMA1_FINE_2, 0x00);
+        r |= jbt_reg_write_1(ddata, JBT_REG_GAMMA1_INCLINATION, 0x00);
+        r |= jbt_reg_write_1(ddata, JBT_REG_GAMMA1_BLUE_OFFSET, 0x00);
+
+        r |= jbt_reg_write_2(ddata, JBT_REG_HCLOCK_VGA, 0x1f0);
+        r |= jbt_reg_write_1(ddata, JBT_REG_BLANK_CONTROL, 0x02);
+        r |= jbt_reg_write_2(ddata, JBT_REG_BLANK_TH_TV, 0x0804);
+
+        r |= jbt_reg_write_1(ddata, JBT_REG_CKV_ON_OFF, 0x01);
+        r |= jbt_reg_write_2(ddata, JBT_REG_CKV_1_2, 0x0000);
+
+        r |= jbt_reg_write_2(ddata, JBT_REG_OEV_TIMING, 0x0d0e);
+        r |= jbt_reg_write_2(ddata, JBT_REG_ASW_TIMING_1, 0x11a4);
+        r |= jbt_reg_write_1(ddata, JBT_REG_ASW_TIMING_2, 0x0e);
+
+        r |= jbt_ret_write_0(ddata, JBT_REG_DISPLAY_ON);
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+transfer_err:
+
+        return r ? -EIO : 0;
+}
+
+static void td028ttec1_panel_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        dev_dbg(dssdev->dev, "td028ttec1_panel_disable()\n");
+
+        jbt_ret_write_0(ddata, JBT_REG_DISPLAY_OFF);
+        jbt_reg_write_2(ddata, JBT_REG_OUTPUT_CONTROL, 0x8002);
+        jbt_ret_write_0(ddata, JBT_REG_SLEEP_IN);
+        jbt_reg_write_1(ddata, JBT_REG_POWER_ON_OFF, 0x00);
+
+        in->ops.dpi->disable(in);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void td028ttec1_panel_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void td028ttec1_panel_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int td028ttec1_panel_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver td028ttec1_ops = {
+        .connect        = td028ttec1_panel_connect,
+        .disconnect     = td028ttec1_panel_disconnect,
+
+        .enable         = td028ttec1_panel_enable,
+        .disable        = td028ttec1_panel_disable,
+
+        .set_timings    = td028ttec1_panel_set_timings,
+        .get_timings    = td028ttec1_panel_get_timings,
+        .check_timings  = td028ttec1_panel_check_timings,
+};
+
+static int td028ttec1_panel_probe_pdata(struct spi_device *spi)
+{
+        const struct panel_tpo_td028ttec1_platform_data *pdata;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev, *in;
+
+        pdata = dev_get_platdata(&spi->dev);
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&spi->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+
+        ddata->in = in;
+
+        ddata->data_lines = pdata->data_lines;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int td028ttec1_probe_of(struct spi_device *spi)
+{
+        struct device_node *node = spi->dev.of_node;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *in;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&spi->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int td028ttec1_panel_probe(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        dev_dbg(&spi->dev, "%s\n", __func__);
+
+        spi->bits_per_word = 9;
+        spi->mode = SPI_MODE_3;
+
+        r = spi_setup(spi);
+        if (r < 0) {
+                dev_err(&spi->dev, "spi_setup failed: %d\n", r);
+                return r;
+        }
+
+        ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        dev_set_drvdata(&spi->dev, ddata);
+
+        ddata->spi_dev = spi;
+
+        if (dev_get_platdata(&spi->dev)) {
+                r = td028ttec1_panel_probe_pdata(spi);
+                if (r)
+                        return r;
+        } else if (spi->dev.of_node) {
+                r = td028ttec1_probe_of(spi);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        ddata->videomode = td028ttec1_panel_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &spi->dev;
+        dssdev->driver = &td028ttec1_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+        dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&spi->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int td028ttec1_panel_remove(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(&ddata->spi_dev->dev, "%s\n", __func__);
+
+        omapdss_unregister_display(dssdev);
+
+        td028ttec1_panel_disable(dssdev);
+        td028ttec1_panel_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        return 0;
+}
+
+static const struct of_device_id td028ttec1_of_match[] = {
+        { .compatible = "omapdss,toppoly,td028ttec1", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, td028ttec1_of_match);
+
+static struct spi_driver td028ttec1_spi_driver = {
+        .probe          = td028ttec1_panel_probe,
+        .remove         = td028ttec1_panel_remove,
+
+        .driver         = {
+                .name   = "panel-tpo-td028ttec1",
+                .of_match_table = td028ttec1_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+module_spi_driver(td028ttec1_spi_driver);
+
+MODULE_ALIAS("spi:toppoly,td028ttec1");
+MODULE_AUTHOR("H. Nikolaus Schaller <hns@goldelico.com>");
+MODULE_DESCRIPTION("Toppoly TD028TTEC1 panel driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td043mtea1.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td043mtea1.c
new file mode 100644
index 000000000000..68e3b68a2920
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td043mtea1.c
@@ -0,0 +1,686 @@
+/*
+ * TPO TD043MTEA1 Panel driver
+ *
+ * Author: Gražvydas Ignotas <notasas@gmail.com>
+ * Converted to new DSS device model: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+#define TPO_R02_MODE(x)         ((x) & 7)
+#define TPO_R02_MODE_800x480    7
+#define TPO_R02_NCLK_RISING     BIT(3)
+#define TPO_R02_HSYNC_HIGH      BIT(4)
+#define TPO_R02_VSYNC_HIGH      BIT(5)
+
+#define TPO_R03_NSTANDBY        BIT(0)
+#define TPO_R03_EN_CP_CLK       BIT(1)
+#define TPO_R03_EN_VGL_PUMP     BIT(2)
+#define TPO_R03_EN_PWM          BIT(3)
+#define TPO_R03_DRIVING_CAP_100 BIT(4)
+#define TPO_R03_EN_PRE_CHARGE   BIT(6)
+#define TPO_R03_SOFTWARE_CTL    BIT(7)
+
+#define TPO_R04_NFLIP_H         BIT(0)
+#define TPO_R04_NFLIP_V         BIT(1)
+#define TPO_R04_CP_CLK_FREQ_1H  BIT(2)
+#define TPO_R04_VGL_FREQ_1H     BIT(4)
+
+#define TPO_R03_VAL_NORMAL (TPO_R03_NSTANDBY | TPO_R03_EN_CP_CLK | \
+                        TPO_R03_EN_VGL_PUMP |  TPO_R03_EN_PWM | \
+                        TPO_R03_DRIVING_CAP_100 | TPO_R03_EN_PRE_CHARGE | \
+                        TPO_R03_SOFTWARE_CTL)
+
+#define TPO_R03_VAL_STANDBY (TPO_R03_DRIVING_CAP_100 | \
+                        TPO_R03_EN_PRE_CHARGE | TPO_R03_SOFTWARE_CTL)
+
+static const u16 tpo_td043_def_gamma[12] = {
+        105, 315, 381, 431, 490, 537, 579, 686, 780, 837, 880, 1023
+};
+
+struct panel_drv_data {
+        struct omap_dss_device  dssdev;
+        struct omap_dss_device *in;
+
+        struct omap_video_timings videomode;
+
+        int data_lines;
+
+        struct spi_device *spi;
+        struct regulator *vcc_reg;
+        int nreset_gpio;
+        u16 gamma[12];
+        u32 mode;
+        u32 hmirror:1;
+        u32 vmirror:1;
+        u32 powered_on:1;
+        u32 spi_suspended:1;
+        u32 power_on_resume:1;
+};
+
+static const struct omap_video_timings tpo_td043_timings = {
+        .x_res          = 800,
+        .y_res          = 480,
+
+        .pixelclock     = 36000000,
+
+        .hsw            = 1,
+        .hfp            = 68,
+        .hbp            = 214,
+
+        .vsw            = 1,
+        .vfp            = 39,
+        .vbp            = 34,
+
+        .vsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .hsync_level    = OMAPDSS_SIG_ACTIVE_LOW,
+        .data_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+        .de_level       = OMAPDSS_SIG_ACTIVE_HIGH,
+        .sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int tpo_td043_write(struct spi_device *spi, u8 addr, u8 data)
+{
+        struct spi_message      m;
+        struct spi_transfer     xfer;
+        u16                     w;
+        int                     r;
+
+        spi_message_init(&m);
+
+        memset(&xfer, 0, sizeof(xfer));
+
+        w = ((u16)addr << 10) | (1 << 8) | data;
+        xfer.tx_buf = &w;
+        xfer.bits_per_word = 16;
+        xfer.len = 2;
+        spi_message_add_tail(&xfer, &m);
+
+        r = spi_sync(spi, &m);
+        if (r < 0)
+                dev_warn(&spi->dev, "failed to write to LCD reg (%d)\n", r);
+        return r;
+}
+
+static void tpo_td043_write_gamma(struct spi_device *spi, u16 gamma[12])
+{
+        u8 i, val;
+
+        /* gamma bits [9:8] */
+        for (val = i = 0; i < 4; i++)
+                val |= (gamma[i] & 0x300) >> ((i + 1) * 2);
+        tpo_td043_write(spi, 0x11, val);
+
+        for (val = i = 0; i < 4; i++)
+                val |= (gamma[i+4] & 0x300) >> ((i + 1) * 2);
+        tpo_td043_write(spi, 0x12, val);
+
+        for (val = i = 0; i < 4; i++)
+                val |= (gamma[i+8] & 0x300) >> ((i + 1) * 2);
+        tpo_td043_write(spi, 0x13, val);
+
+        /* gamma bits [7:0] */
+        for (val = i = 0; i < 12; i++)
+                tpo_td043_write(spi, 0x14 + i, gamma[i] & 0xff);
+}
+
+static int tpo_td043_write_mirror(struct spi_device *spi, bool h, bool v)
+{
+        u8 reg4 = TPO_R04_NFLIP_H | TPO_R04_NFLIP_V |
+                TPO_R04_CP_CLK_FREQ_1H | TPO_R04_VGL_FREQ_1H;
+        if (h)
+                reg4 &= ~TPO_R04_NFLIP_H;
+        if (v)
+                reg4 &= ~TPO_R04_NFLIP_V;
+
+        return tpo_td043_write(spi, 4, reg4);
+}
+
+static int tpo_td043_set_hmirror(struct omap_dss_device *dssdev, bool enable)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dssdev->dev);
+
+        ddata->hmirror = enable;
+        return tpo_td043_write_mirror(ddata->spi, ddata->hmirror,
+                        ddata->vmirror);
+}
+
+static bool tpo_td043_get_hmirror(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dssdev->dev);
+
+        return ddata->hmirror;
+}
+
+static ssize_t tpo_td043_vmirror_show(struct device *dev,
+        struct device_attribute *attr, char *buf)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", ddata->vmirror);
+}
+
+static ssize_t tpo_td043_vmirror_store(struct device *dev,
+        struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        int val;
+        int ret;
+
+        ret = kstrtoint(buf, 0, &val);
+        if (ret < 0)
+                return ret;
+
+        val = !!val;
+
+        ret = tpo_td043_write_mirror(ddata->spi, ddata->hmirror, val);
+        if (ret < 0)
+                return ret;
+
+        ddata->vmirror = val;
+
+        return count;
+}
+
+static ssize_t tpo_td043_mode_show(struct device *dev,
+        struct device_attribute *attr, char *buf)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", ddata->mode);
+}
+
+static ssize_t tpo_td043_mode_store(struct device *dev,
+        struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        long val;
+        int ret;
+
+        ret = kstrtol(buf, 0, &val);
+        if (ret != 0 || val & ~7)
+                return -EINVAL;
+
+        ddata->mode = val;
+
+        val |= TPO_R02_NCLK_RISING;
+        tpo_td043_write(ddata->spi, 2, val);
+
+        return count;
+}
+
+static ssize_t tpo_td043_gamma_show(struct device *dev,
+        struct device_attribute *attr, char *buf)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        ssize_t len = 0;
+        int ret;
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(ddata->gamma); i++) {
+                ret = snprintf(buf + len, PAGE_SIZE - len, "%u ",
+                                ddata->gamma[i]);
+                if (ret < 0)
+                        return ret;
+                len += ret;
+        }
+        buf[len - 1] = '\n';
+
+        return len;
+}
+
+static ssize_t tpo_td043_gamma_store(struct device *dev,
+        struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        unsigned int g[12];
+        int ret;
+        int i;
+
+        ret = sscanf(buf, "%u %u %u %u %u %u %u %u %u %u %u %u",
+                        &g[0], &g[1], &g[2], &g[3], &g[4], &g[5],
+                        &g[6], &g[7], &g[8], &g[9], &g[10], &g[11]);
+
+        if (ret != 12)
+                return -EINVAL;
+
+        for (i = 0; i < 12; i++)
+                ddata->gamma[i] = g[i];
+
+        tpo_td043_write_gamma(ddata->spi, ddata->gamma);
+
+        return count;
+}
+
+static DEVICE_ATTR(vmirror, S_IRUGO | S_IWUSR,
+                tpo_td043_vmirror_show, tpo_td043_vmirror_store);
+static DEVICE_ATTR(mode, S_IRUGO | S_IWUSR,
+                tpo_td043_mode_show, tpo_td043_mode_store);
+static DEVICE_ATTR(gamma, S_IRUGO | S_IWUSR,
+                tpo_td043_gamma_show, tpo_td043_gamma_store);
+
+static struct attribute *tpo_td043_attrs[] = {
+        &dev_attr_vmirror.attr,
+        &dev_attr_mode.attr,
+        &dev_attr_gamma.attr,
+        NULL,
+};
+
+static struct attribute_group tpo_td043_attr_group = {
+        .attrs = tpo_td043_attrs,
+};
+
+static int tpo_td043_power_on(struct panel_drv_data *ddata)
+{
+        int r;
+
+        if (ddata->powered_on)
+                return 0;
+
+        r = regulator_enable(ddata->vcc_reg);
+        if (r != 0)
+                return r;
+
+        /* wait for panel to stabilize */
+        msleep(160);
+
+        if (gpio_is_valid(ddata->nreset_gpio))
+                gpio_set_value(ddata->nreset_gpio, 1);
+
+        tpo_td043_write(ddata->spi, 2,
+                        TPO_R02_MODE(ddata->mode) | TPO_R02_NCLK_RISING);
+        tpo_td043_write(ddata->spi, 3, TPO_R03_VAL_NORMAL);
+        tpo_td043_write(ddata->spi, 0x20, 0xf0);
+        tpo_td043_write(ddata->spi, 0x21, 0xf0);
+        tpo_td043_write_mirror(ddata->spi, ddata->hmirror,
+                        ddata->vmirror);
+        tpo_td043_write_gamma(ddata->spi, ddata->gamma);
+
+        ddata->powered_on = 1;
+        return 0;
+}
+
+static void tpo_td043_power_off(struct panel_drv_data *ddata)
+{
+        if (!ddata->powered_on)
+                return;
+
+        tpo_td043_write(ddata->spi, 3,
+                        TPO_R03_VAL_STANDBY | TPO_R03_EN_PWM);
+
+        if (gpio_is_valid(ddata->nreset_gpio))
+                gpio_set_value(ddata->nreset_gpio, 0);
+
+        /* wait for at least 2 vsyncs before cutting off power */
+        msleep(50);
+
+        tpo_td043_write(ddata->spi, 3, TPO_R03_VAL_STANDBY);
+
+        regulator_disable(ddata->vcc_reg);
+
+        ddata->powered_on = 0;
+}
+
+static int tpo_td043_connect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (omapdss_device_is_connected(dssdev))
+                return 0;
+
+        r = in->ops.dpi->connect(in, dssdev);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void tpo_td043_disconnect(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return;
+
+        in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int tpo_td043_enable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+        int r;
+
+        if (!omapdss_device_is_connected(dssdev))
+                return -ENODEV;
+
+        if (omapdss_device_is_enabled(dssdev))
+                return 0;
+
+        if (ddata->data_lines)
+                in->ops.dpi->set_data_lines(in, ddata->data_lines);
+        in->ops.dpi->set_timings(in, &ddata->videomode);
+
+        r = in->ops.dpi->enable(in);
+        if (r)
+                return r;
+
+        /*
+         * If we are resuming from system suspend, SPI clocks might not be
+         * enabled yet, so we'll program the LCD from SPI PM resume callback.
+         */
+        if (!ddata->spi_suspended) {
+                r = tpo_td043_power_on(ddata);
+                if (r) {
+                        in->ops.dpi->disable(in);
+                        return r;
+                }
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        return 0;
+}
+
+static void tpo_td043_disable(struct omap_dss_device *dssdev)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        if (!omapdss_device_is_enabled(dssdev))
+                return;
+
+        in->ops.dpi->disable(in);
+
+        if (!ddata->spi_suspended)
+                tpo_td043_power_off(ddata);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tpo_td043_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        ddata->videomode = *timings;
+        dssdev->panel.timings = *timings;
+
+        in->ops.dpi->set_timings(in, timings);
+}
+
+static void tpo_td043_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+        *timings = ddata->videomode;
+}
+
+static int tpo_td043_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct panel_drv_data *ddata = to_panel_data(dssdev);
+        struct omap_dss_device *in = ddata->in;
+
+        return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver tpo_td043_ops = {
+        .connect        = tpo_td043_connect,
+        .disconnect     = tpo_td043_disconnect,
+
+        .enable         = tpo_td043_enable,
+        .disable        = tpo_td043_disable,
+
+        .set_timings    = tpo_td043_set_timings,
+        .get_timings    = tpo_td043_get_timings,
+        .check_timings  = tpo_td043_check_timings,
+
+        .set_mirror     = tpo_td043_set_hmirror,
+        .get_mirror     = tpo_td043_get_hmirror,
+
+        .get_resolution = omapdss_default_get_resolution,
+};
+
+
+static int tpo_td043_probe_pdata(struct spi_device *spi)
+{
+        const struct panel_tpo_td043mtea1_platform_data *pdata;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev, *in;
+
+        pdata = dev_get_platdata(&spi->dev);
+
+        ddata->nreset_gpio = pdata->nreset_gpio;
+
+        in = omap_dss_find_output(pdata->source);
+        if (in == NULL) {
+                dev_err(&spi->dev, "failed to find video source '%s'\n",
+                                pdata->source);
+                return -EPROBE_DEFER;
+        }
+        ddata->in = in;
+
+        ddata->data_lines = pdata->data_lines;
+
+        dssdev = &ddata->dssdev;
+        dssdev->name = pdata->name;
+
+        return 0;
+}
+
+static int tpo_td043_probe_of(struct spi_device *spi)
+{
+        struct device_node *node = spi->dev.of_node;
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *in;
+        int gpio;
+
+        gpio = of_get_named_gpio(node, "reset-gpios", 0);
+        if (!gpio_is_valid(gpio)) {
+                dev_err(&spi->dev, "failed to parse enable gpio\n");
+                return gpio;
+        }
+        ddata->nreset_gpio = gpio;
+
+        in = omapdss_of_find_source_for_first_ep(node);
+        if (IS_ERR(in)) {
+                dev_err(&spi->dev, "failed to find video source\n");
+                return PTR_ERR(in);
+        }
+
+        ddata->in = in;
+
+        return 0;
+}
+
+static int tpo_td043_probe(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata;
+        struct omap_dss_device *dssdev;
+        int r;
+
+        dev_dbg(&spi->dev, "%s\n", __func__);
+
+        spi->bits_per_word = 16;
+        spi->mode = SPI_MODE_0;
+
+        r = spi_setup(spi);
+        if (r < 0) {
+                dev_err(&spi->dev, "spi_setup failed: %d\n", r);
+                return r;
+        }
+
+        ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+        if (ddata == NULL)
+                return -ENOMEM;
+
+        dev_set_drvdata(&spi->dev, ddata);
+
+        ddata->spi = spi;
+
+        if (dev_get_platdata(&spi->dev)) {
+                r = tpo_td043_probe_pdata(spi);
+                if (r)
+                        return r;
+        } else if (spi->dev.of_node) {
+                r = tpo_td043_probe_of(spi);
+                if (r)
+                        return r;
+        } else {
+                return -ENODEV;
+        }
+
+        ddata->mode = TPO_R02_MODE_800x480;
+        memcpy(ddata->gamma, tpo_td043_def_gamma, sizeof(ddata->gamma));
+
+        ddata->vcc_reg = devm_regulator_get(&spi->dev, "vcc");
+        if (IS_ERR(ddata->vcc_reg)) {
+                dev_err(&spi->dev, "failed to get LCD VCC regulator\n");
+                r = PTR_ERR(ddata->vcc_reg);
+                goto err_regulator;
+        }
+
+        if (gpio_is_valid(ddata->nreset_gpio)) {
+                r = devm_gpio_request_one(&spi->dev,
+                                ddata->nreset_gpio, GPIOF_OUT_INIT_LOW,
+                                "lcd reset");
+                if (r < 0) {
+                        dev_err(&spi->dev, "couldn't request reset GPIO\n");
+                        goto err_gpio_req;
+                }
+        }
+
+        r = sysfs_create_group(&spi->dev.kobj, &tpo_td043_attr_group);
+        if (r) {
+                dev_err(&spi->dev, "failed to create sysfs files\n");
+                goto err_sysfs;
+        }
+
+        ddata->videomode = tpo_td043_timings;
+
+        dssdev = &ddata->dssdev;
+        dssdev->dev = &spi->dev;
+        dssdev->driver = &tpo_td043_ops;
+        dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+        dssdev->owner = THIS_MODULE;
+        dssdev->panel.timings = ddata->videomode;
+
+        r = omapdss_register_display(dssdev);
+        if (r) {
+                dev_err(&spi->dev, "Failed to register panel\n");
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        sysfs_remove_group(&spi->dev.kobj, &tpo_td043_attr_group);
+err_sysfs:
+err_gpio_req:
+err_regulator:
+        omap_dss_put_device(ddata->in);
+        return r;
+}
+
+static int tpo_td043_remove(struct spi_device *spi)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+        struct omap_dss_device *dssdev = &ddata->dssdev;
+        struct omap_dss_device *in = ddata->in;
+
+        dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+        omapdss_unregister_display(dssdev);
+
+        tpo_td043_disable(dssdev);
+        tpo_td043_disconnect(dssdev);
+
+        omap_dss_put_device(in);
+
+        sysfs_remove_group(&spi->dev.kobj, &tpo_td043_attr_group);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tpo_td043_spi_suspend(struct device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+
+        dev_dbg(dev, "tpo_td043_spi_suspend, tpo %p\n", ddata);
+
+        ddata->power_on_resume = ddata->powered_on;
+        tpo_td043_power_off(ddata);
+        ddata->spi_suspended = 1;
+
+        return 0;
+}
+
+static int tpo_td043_spi_resume(struct device *dev)
+{
+        struct panel_drv_data *ddata = dev_get_drvdata(dev);
+        int ret;
+
+        dev_dbg(dev, "tpo_td043_spi_resume\n");
+
+        if (ddata->power_on_resume) {
+                ret = tpo_td043_power_on(ddata);
+                if (ret)
+                        return ret;
+        }
+        ddata->spi_suspended = 0;
+
+        return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(tpo_td043_spi_pm,
+        tpo_td043_spi_suspend, tpo_td043_spi_resume);
+
+static const struct of_device_id tpo_td043_of_match[] = {
+        { .compatible = "omapdss,tpo,td043mtea1", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, tpo_td043_of_match);
+
+static struct spi_driver tpo_td043_spi_driver = {
+        .driver = {
+                .name   = "panel-tpo-td043mtea1",
+                .pm     = &tpo_td043_spi_pm,
+                .of_match_table = tpo_td043_of_match,
+                .suppress_bind_attrs = true,
+        },
+        .probe  = tpo_td043_probe,
+        .remove = tpo_td043_remove,
+};
+
+module_spi_driver(tpo_td043_spi_driver);
+
+MODULE_ALIAS("spi:tpo,td043mtea1");
+MODULE_AUTHOR("Gražvydas Ignotas <notasas@gmail.com>");
+MODULE_DESCRIPTION("TPO TD043MTEA1 LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/Kconfig b/drivers/video/fbdev/omap2/omapfb/dss/Kconfig
new file mode 100644
index 000000000000..27d220212870
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/Kconfig
@@ -0,0 +1,129 @@
+config FB_OMAP2_DSS_INIT
+        bool
+
+config FB_OMAP2_DSS
+        tristate
+        select VIDEOMODE_HELPERS
+        select FB_OMAP2_DSS_INIT
+        select HDMI
+
+config FB_OMAP2_DSS_DEBUG
+        bool "Debug support"
+        default n
+        help
+          This enables printing of debug messages. Alternatively, debug messages
+          can also be enabled by setting CONFIG_DYNAMIC_DEBUG and then setting
+          appropriate flags in <debugfs>/dynamic_debug/control.
+
+config FB_OMAP2_DSS_DEBUGFS
+        bool "Debugfs filesystem support"
+        depends on DEBUG_FS
+        default n
+        help
+          This enables debugfs for OMAPDSS at <debugfs>/omapdss. This enables
+          querying about clock configuration and register configuration of dss,
+          dispc, dsi, hdmi and rfbi.
+
+config FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        bool "Collect DSS IRQ statistics"
+        depends on FB_OMAP2_DSS_DEBUGFS
+        default n
+        help
+          Collect DSS IRQ statistics, printable via debugfs.
+
+          The statistics can be found from
+          <debugfs>/omapdss/dispc_irq for DISPC interrupts, and
+          <debugfs>/omapdss/dsi_irq for DSI interrupts.
+
+config FB_OMAP2_DSS_DPI
+        bool "DPI support"
+        default y
+        help
+          DPI Interface. This is the Parallel Display Interface.
+
+config FB_OMAP2_DSS_RFBI
+        bool "RFBI support"
+        depends on BROKEN
+        default n
+        help
+          MIPI DBI support (RFBI, Remote Framebuffer Interface, in Texas
+          Instrument's terminology).
+
+          DBI is a bus between the host processor and a peripheral,
+          such as a display or a framebuffer chip.
+
+          See http://www.mipi.org/ for DBI specifications.
+
+config FB_OMAP2_DSS_VENC
+        bool "VENC support"
+        default y
+        help
+          OMAP Video Encoder support for S-Video and composite TV-out.
+
+config FB_OMAP2_DSS_HDMI_COMMON
+        bool
+
+config FB_OMAP4_DSS_HDMI
+        bool "HDMI support for OMAP4"
+        default y
+        select FB_OMAP2_DSS_HDMI_COMMON
+        help
+          HDMI support for OMAP4 based SoCs.
+
+config FB_OMAP5_DSS_HDMI
+        bool "HDMI support for OMAP5"
+        default n
+        select FB_OMAP2_DSS_HDMI_COMMON
+        help
+          HDMI Interface for OMAP5 and similar cores. This adds the High
+          Definition Multimedia Interface. See http://www.hdmi.org/ for HDMI
+          specification.
+
+config FB_OMAP2_DSS_SDI
+        bool "SDI support"
+        default n
+        help
+          SDI (Serial Display Interface) support.
+
+          SDI is a high speed one-way display serial bus between the host
+          processor and a display.
+
+config FB_OMAP2_DSS_DSI
+        bool "DSI support"
+        default n
+        help
+          MIPI DSI (Display Serial Interface) support.
+
+          DSI is a high speed half-duplex serial interface between the host
+          processor and a peripheral, such as a display or a framebuffer chip.
+
+          See http://www.mipi.org/ for DSI specifications.
+
+config FB_OMAP2_DSS_MIN_FCK_PER_PCK
+        int "Minimum FCK/PCK ratio (for scaling)"
+        range 0 32
+        default 0
+        help
+          This can be used to adjust the minimum FCK/PCK ratio.
+
+          With this you can make sure that DISPC FCK is at least
+          n x PCK. Video plane scaling requires higher FCK than
+          normally.
+
+          If this is set to 0, there's no extra constraint on the
+          DISPC FCK. However, the FCK will at minimum be
+          2xPCK (if active matrix) or 3xPCK (if passive matrix).
+
+          Max FCK is 173MHz, so this doesn't work if your PCK
+          is very high.
+
+config FB_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.
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/Makefile b/drivers/video/fbdev/omap2/omapfb/dss/Makefile
new file mode 100644
index 000000000000..02308e24f3ef
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/Makefile
@@ -0,0 +1,18 @@
+obj-$(CONFIG_FB_OMAP2_DSS_INIT) += omapdss-boot-init.o
+obj-$(CONFIG_FB_OMAP2_DSS) += omapdss.o
+# Core DSS files
+omapdss-y := core.o dss.o dss_features.o dispc.o dispc_coefs.o display.o \
+        output.o dss-of.o pll.o video-pll.o
+# DSS compat layer files
+omapdss-y += manager.o manager-sysfs.o overlay.o overlay-sysfs.o apply.o \
+        dispc-compat.o display-sysfs.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_DPI) += dpi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_RFBI) += rfbi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_VENC) += venc.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_SDI) += sdi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_DSI) += dsi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_HDMI_COMMON) += hdmi_common.o hdmi_wp.o hdmi_pll.o \
+        hdmi_phy.o
+omapdss-$(CONFIG_FB_OMAP4_DSS_HDMI) += hdmi4.o hdmi4_core.o
+omapdss-$(CONFIG_FB_OMAP5_DSS_HDMI) += hdmi5.o hdmi5_core.o
+ccflags-$(CONFIG_FB_OMAP2_DSS_DEBUG) += -DDEBUG
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/apply.c b/drivers/video/fbdev/omap2/omapfb/dss/apply.c
new file mode 100644
index 000000000000..663ccc3bf4e5
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/apply.c
@@ -0,0 +1,1702 @@
+/*
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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 "APPLY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+#include "dispc-compat.h"
+
+/*
+ * We have 4 levels of cache for the dispc settings. First two are in SW and
+ * the latter two in HW.
+ *
+ *       set_info()
+ *          v
+ * +--------------------+
+ * |     user_info      |
+ * +--------------------+
+ *          v
+ *        apply()
+ *          v
+ * +--------------------+
+ * |       info         |
+ * +--------------------+
+ *          v
+ *      write_regs()
+ *          v
+ * +--------------------+
+ * |  shadow registers  |
+ * +--------------------+
+ *          v
+ * VFP or lcd/digit_enable
+ *          v
+ * +--------------------+
+ * |      registers     |
+ * +--------------------+
+ */
+
+struct ovl_priv_data {
+
+        bool user_info_dirty;
+        struct omap_overlay_info user_info;
+
+        bool info_dirty;
+        struct omap_overlay_info info;
+
+        bool shadow_info_dirty;
+
+        bool extra_info_dirty;
+        bool shadow_extra_info_dirty;
+
+        bool enabled;
+        u32 fifo_low, fifo_high;
+
+        /*
+         * True if overlay is to be enabled. Used to check and calculate configs
+         * for the overlay before it is enabled in the HW.
+         */
+        bool enabling;
+};
+
+struct mgr_priv_data {
+
+        bool user_info_dirty;
+        struct omap_overlay_manager_info user_info;
+
+        bool info_dirty;
+        struct omap_overlay_manager_info info;
+
+        bool shadow_info_dirty;
+
+        /* If true, GO bit is up and shadow registers cannot be written.
+         * Never true for manual update displays */
+        bool busy;
+
+        /* If true, dispc output is enabled */
+        bool updating;
+
+        /* If true, a display is enabled using this manager */
+        bool enabled;
+
+        bool extra_info_dirty;
+        bool shadow_extra_info_dirty;
+
+        struct omap_video_timings timings;
+        struct dss_lcd_mgr_config lcd_config;
+
+        void (*framedone_handler)(void *);
+        void *framedone_handler_data;
+};
+
+static struct {
+        struct ovl_priv_data ovl_priv_data_array[MAX_DSS_OVERLAYS];
+        struct mgr_priv_data mgr_priv_data_array[MAX_DSS_MANAGERS];
+
+        bool irq_enabled;
+} dss_data;
+
+/* protects dss_data */
+static spinlock_t data_lock;
+/* lock for blocking functions */
+static DEFINE_MUTEX(apply_lock);
+static DECLARE_COMPLETION(extra_updated_completion);
+
+static void dss_register_vsync_isr(void);
+
+static struct ovl_priv_data *get_ovl_priv(struct omap_overlay *ovl)
+{
+        return &dss_data.ovl_priv_data_array[ovl->id];
+}
+
+static struct mgr_priv_data *get_mgr_priv(struct omap_overlay_manager *mgr)
+{
+        return &dss_data.mgr_priv_data_array[mgr->id];
+}
+
+static void apply_init_priv(void)
+{
+        const int num_ovls = dss_feat_get_num_ovls();
+        struct mgr_priv_data *mp;
+        int i;
+
+        spin_lock_init(&data_lock);
+
+        for (i = 0; i < num_ovls; ++i) {
+                struct ovl_priv_data *op;
+
+                op = &dss_data.ovl_priv_data_array[i];
+
+                op->info.color_mode = OMAP_DSS_COLOR_RGB16;
+                op->info.rotation_type = OMAP_DSS_ROT_DMA;
+
+                op->info.global_alpha = 255;
+
+                switch (i) {
+                case 0:
+                        op->info.zorder = 0;
+                        break;
+                case 1:
+                        op->info.zorder =
+                                dss_has_feature(FEAT_ALPHA_FREE_ZORDER) ? 3 : 0;
+                        break;
+                case 2:
+                        op->info.zorder =
+                                dss_has_feature(FEAT_ALPHA_FREE_ZORDER) ? 2 : 0;
+                        break;
+                case 3:
+                        op->info.zorder =
+                                dss_has_feature(FEAT_ALPHA_FREE_ZORDER) ? 1 : 0;
+                        break;
+                }
+
+                op->user_info = op->info;
+        }
+
+        /*
+         * Initialize some of the lcd_config fields for TV manager, this lets
+         * us prevent checking if the manager is LCD or TV at some places
+         */
+        mp = &dss_data.mgr_priv_data_array[OMAP_DSS_CHANNEL_DIGIT];
+
+        mp->lcd_config.video_port_width = 24;
+        mp->lcd_config.clock_info.lck_div = 1;
+        mp->lcd_config.clock_info.pck_div = 1;
+}
+
+/*
+ * A LCD manager's stallmode decides whether it is in manual or auto update. TV
+ * manager is always auto update, stallmode field for TV manager is false by
+ * default
+ */
+static bool ovl_manual_update(struct omap_overlay *ovl)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(ovl->manager);
+
+        return mp->lcd_config.stallmode;
+}
+
+static bool mgr_manual_update(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        return mp->lcd_config.stallmode;
+}
+
+static int dss_check_settings_low(struct omap_overlay_manager *mgr,
+                bool applying)
+{
+        struct omap_overlay_info *oi;
+        struct omap_overlay_manager_info *mi;
+        struct omap_overlay *ovl;
+        struct omap_overlay_info *ois[MAX_DSS_OVERLAYS];
+        struct ovl_priv_data *op;
+        struct mgr_priv_data *mp;
+
+        mp = get_mgr_priv(mgr);
+
+        if (!mp->enabled)
+                return 0;
+
+        if (applying && mp->user_info_dirty)
+                mi = &mp->user_info;
+        else
+                mi = &mp->info;
+
+        /* collect the infos to be tested into the array */
+        list_for_each_entry(ovl, &mgr->overlays, list) {
+                op = get_ovl_priv(ovl);
+
+                if (!op->enabled && !op->enabling)
+                        oi = NULL;
+                else if (applying && op->user_info_dirty)
+                        oi = &op->user_info;
+                else
+                        oi = &op->info;
+
+                ois[ovl->id] = oi;
+        }
+
+        return dss_mgr_check(mgr, mi, &mp->timings, &mp->lcd_config, ois);
+}
+
+/*
+ * check manager and overlay settings using overlay_info from data->info
+ */
+static int dss_check_settings(struct omap_overlay_manager *mgr)
+{
+        return dss_check_settings_low(mgr, false);
+}
+
+/*
+ * check manager and overlay settings using overlay_info from ovl->info if
+ * dirty and from data->info otherwise
+ */
+static int dss_check_settings_apply(struct omap_overlay_manager *mgr)
+{
+        return dss_check_settings_low(mgr, true);
+}
+
+static bool need_isr(void)
+{
+        const int num_mgrs = dss_feat_get_num_mgrs();
+        int i;
+
+        for (i = 0; i < num_mgrs; ++i) {
+                struct omap_overlay_manager *mgr;
+                struct mgr_priv_data *mp;
+                struct omap_overlay *ovl;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                mp = get_mgr_priv(mgr);
+
+                if (!mp->enabled)
+                        continue;
+
+                if (mgr_manual_update(mgr)) {
+                        /* to catch FRAMEDONE */
+                        if (mp->updating)
+                                return true;
+                } else {
+                        /* to catch GO bit going down */
+                        if (mp->busy)
+                                return true;
+
+                        /* to write new values to registers */
+                        if (mp->info_dirty)
+                                return true;
+
+                        /* to set GO bit */
+                        if (mp->shadow_info_dirty)
+                                return true;
+
+                        /*
+                         * NOTE: we don't check extra_info flags for disabled
+                         * managers, once the manager is enabled, the extra_info
+                         * related manager changes will be taken in by HW.
+                         */
+
+                        /* to write new values to registers */
+                        if (mp->extra_info_dirty)
+                                return true;
+
+                        /* to set GO bit */
+                        if (mp->shadow_extra_info_dirty)
+                                return true;
+
+                        list_for_each_entry(ovl, &mgr->overlays, list) {
+                                struct ovl_priv_data *op;
+
+                                op = get_ovl_priv(ovl);
+
+                                /*
+                                 * NOTE: we check extra_info flags even for
+                                 * disabled overlays, as extra_infos need to be
+                                 * always written.
+                                 */
+
+                                /* to write new values to registers */
+                                if (op->extra_info_dirty)
+                                        return true;
+
+                                /* to set GO bit */
+                                if (op->shadow_extra_info_dirty)
+                                        return true;
+
+                                if (!op->enabled)
+                                        continue;
+
+                                /* to write new values to registers */
+                                if (op->info_dirty)
+                                        return true;
+
+                                /* to set GO bit */
+                                if (op->shadow_info_dirty)
+                                        return true;
+                        }
+                }
+        }
+
+        return false;
+}
+
+static bool need_go(struct omap_overlay_manager *mgr)
+{
+        struct omap_overlay *ovl;
+        struct mgr_priv_data *mp;
+        struct ovl_priv_data *op;
+
+        mp = get_mgr_priv(mgr);
+
+        if (mp->shadow_info_dirty || mp->shadow_extra_info_dirty)
+                return true;
+
+        list_for_each_entry(ovl, &mgr->overlays, list) {
+                op = get_ovl_priv(ovl);
+                if (op->shadow_info_dirty || op->shadow_extra_info_dirty)
+                        return true;
+        }
+
+        return false;
+}
+
+/* returns true if an extra_info field is currently being updated */
+static bool extra_info_update_ongoing(void)
+{
+        const int num_mgrs = dss_feat_get_num_mgrs();
+        int i;
+
+        for (i = 0; i < num_mgrs; ++i) {
+                struct omap_overlay_manager *mgr;
+                struct omap_overlay *ovl;
+                struct mgr_priv_data *mp;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                mp = get_mgr_priv(mgr);
+
+                if (!mp->enabled)
+                        continue;
+
+                if (!mp->updating)
+                        continue;
+
+                if (mp->extra_info_dirty || mp->shadow_extra_info_dirty)
+                        return true;
+
+                list_for_each_entry(ovl, &mgr->overlays, list) {
+                        struct ovl_priv_data *op = get_ovl_priv(ovl);
+
+                        if (op->extra_info_dirty || op->shadow_extra_info_dirty)
+                                return true;
+                }
+        }
+
+        return false;
+}
+
+/* wait until no extra_info updates are pending */
+static void wait_pending_extra_info_updates(void)
+{
+        bool updating;
+        unsigned long flags;
+        unsigned long t;
+        int r;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        updating = extra_info_update_ongoing();
+
+        if (!updating) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                return;
+        }
+
+        init_completion(&extra_updated_completion);
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        t = msecs_to_jiffies(500);
+        r = wait_for_completion_timeout(&extra_updated_completion, t);
+        if (r == 0)
+                DSSWARN("timeout in wait_pending_extra_info_updates\n");
+}
+
+static struct omap_dss_device *dss_mgr_get_device(struct omap_overlay_manager *mgr)
+{
+        struct omap_dss_device *dssdev;
+
+        dssdev = mgr->output;
+        if (dssdev == NULL)
+                return NULL;
+
+        while (dssdev->dst)
+                dssdev = dssdev->dst;
+
+        if (dssdev->driver)
+                return dssdev;
+        else
+                return NULL;
+}
+
+static struct omap_dss_device *dss_ovl_get_device(struct omap_overlay *ovl)
+{
+        return ovl->manager ? dss_mgr_get_device(ovl->manager) : NULL;
+}
+
+static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
+{
+        unsigned long timeout = msecs_to_jiffies(500);
+        u32 irq;
+        int r;
+
+        if (mgr->output == NULL)
+                return -ENODEV;
+
+        r = dispc_runtime_get();
+        if (r)
+                return r;
+
+        switch (mgr->output->id) {
+        case OMAP_DSS_OUTPUT_VENC:
+                irq = DISPC_IRQ_EVSYNC_ODD;
+                break;
+        case OMAP_DSS_OUTPUT_HDMI:
+                irq = DISPC_IRQ_EVSYNC_EVEN;
+                break;
+        default:
+                irq = dispc_mgr_get_vsync_irq(mgr->id);
+                break;
+        }
+
+        r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
+
+        dispc_runtime_put();
+
+        return r;
+}
+
+static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
+{
+        unsigned long timeout = msecs_to_jiffies(500);
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        u32 irq;
+        unsigned long flags;
+        int r;
+        int i;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (mgr_manual_update(mgr)) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                return 0;
+        }
+
+        if (!mp->enabled) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                return 0;
+        }
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        r = dispc_runtime_get();
+        if (r)
+                return r;
+
+        irq = dispc_mgr_get_vsync_irq(mgr->id);
+
+        i = 0;
+        while (1) {
+                bool shadow_dirty, dirty;
+
+                spin_lock_irqsave(&data_lock, flags);
+                dirty = mp->info_dirty;
+                shadow_dirty = mp->shadow_info_dirty;
+                spin_unlock_irqrestore(&data_lock, flags);
+
+                if (!dirty && !shadow_dirty) {
+                        r = 0;
+                        break;
+                }
+
+                /* 4 iterations is the worst case:
+                 * 1 - initial iteration, dirty = true (between VFP and VSYNC)
+                 * 2 - first VSYNC, dirty = true
+                 * 3 - dirty = false, shadow_dirty = true
+                 * 4 - shadow_dirty = false */
+                if (i++ == 3) {
+                        DSSERR("mgr(%d)->wait_for_go() not finishing\n",
+                                        mgr->id);
+                        r = 0;
+                        break;
+                }
+
+                r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
+                if (r == -ERESTARTSYS)
+                        break;
+
+                if (r) {
+                        DSSERR("mgr(%d)->wait_for_go() timeout\n", mgr->id);
+                        break;
+                }
+        }
+
+        dispc_runtime_put();
+
+        return r;
+}
+
+static int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
+{
+        unsigned long timeout = msecs_to_jiffies(500);
+        struct ovl_priv_data *op;
+        struct mgr_priv_data *mp;
+        u32 irq;
+        unsigned long flags;
+        int r;
+        int i;
+
+        if (!ovl->manager)
+                return 0;
+
+        mp = get_mgr_priv(ovl->manager);
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (ovl_manual_update(ovl)) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                return 0;
+        }
+
+        if (!mp->enabled) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                return 0;
+        }
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        r = dispc_runtime_get();
+        if (r)
+                return r;
+
+        irq = dispc_mgr_get_vsync_irq(ovl->manager->id);
+
+        op = get_ovl_priv(ovl);
+        i = 0;
+        while (1) {
+                bool shadow_dirty, dirty;
+
+                spin_lock_irqsave(&data_lock, flags);
+                dirty = op->info_dirty;
+                shadow_dirty = op->shadow_info_dirty;
+                spin_unlock_irqrestore(&data_lock, flags);
+
+                if (!dirty && !shadow_dirty) {
+                        r = 0;
+                        break;
+                }
+
+                /* 4 iterations is the worst case:
+                 * 1 - initial iteration, dirty = true (between VFP and VSYNC)
+                 * 2 - first VSYNC, dirty = true
+                 * 3 - dirty = false, shadow_dirty = true
+                 * 4 - shadow_dirty = false */
+                if (i++ == 3) {
+                        DSSERR("ovl(%d)->wait_for_go() not finishing\n",
+                                        ovl->id);
+                        r = 0;
+                        break;
+                }
+
+                r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
+                if (r == -ERESTARTSYS)
+                        break;
+
+                if (r) {
+                        DSSERR("ovl(%d)->wait_for_go() timeout\n", ovl->id);
+                        break;
+                }
+        }
+
+        dispc_runtime_put();
+
+        return r;
+}
+
+static void dss_ovl_write_regs(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        struct omap_overlay_info *oi;
+        bool replication;
+        struct mgr_priv_data *mp;
+        int r;
+
+        DSSDBG("writing ovl %d regs\n", ovl->id);
+
+        if (!op->enabled || !op->info_dirty)
+                return;
+
+        oi = &op->info;
+
+        mp = get_mgr_priv(ovl->manager);
+
+        replication = dss_ovl_use_replication(mp->lcd_config, oi->color_mode);
+
+        r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings, false);
+        if (r) {
+                /*
+                 * We can't do much here, as this function can be called from
+                 * vsync interrupt.
+                 */
+                DSSERR("dispc_ovl_setup failed for ovl %d\n", ovl->id);
+
+                /* This will leave fifo configurations in a nonoptimal state */
+                op->enabled = false;
+                dispc_ovl_enable(ovl->id, false);
+                return;
+        }
+
+        op->info_dirty = false;
+        if (mp->updating)
+                op->shadow_info_dirty = true;
+}
+
+static void dss_ovl_write_regs_extra(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        struct mgr_priv_data *mp;
+
+        DSSDBG("writing ovl %d regs extra\n", ovl->id);
+
+        if (!op->extra_info_dirty)
+                return;
+
+        /* note: write also when op->enabled == false, so that the ovl gets
+         * disabled */
+
+        dispc_ovl_enable(ovl->id, op->enabled);
+        dispc_ovl_set_fifo_threshold(ovl->id, op->fifo_low, op->fifo_high);
+
+        mp = get_mgr_priv(ovl->manager);
+
+        op->extra_info_dirty = false;
+        if (mp->updating)
+                op->shadow_extra_info_dirty = true;
+}
+
+static void dss_mgr_write_regs(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        struct omap_overlay *ovl;
+
+        DSSDBG("writing mgr %d regs\n", mgr->id);
+
+        if (!mp->enabled)
+                return;
+
+        WARN_ON(mp->busy);
+
+        /* Commit overlay settings */
+        list_for_each_entry(ovl, &mgr->overlays, list) {
+                dss_ovl_write_regs(ovl);
+                dss_ovl_write_regs_extra(ovl);
+        }
+
+        if (mp->info_dirty) {
+                dispc_mgr_setup(mgr->id, &mp->info);
+
+                mp->info_dirty = false;
+                if (mp->updating)
+                        mp->shadow_info_dirty = true;
+        }
+}
+
+static void dss_mgr_write_regs_extra(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        DSSDBG("writing mgr %d regs extra\n", mgr->id);
+
+        if (!mp->extra_info_dirty)
+                return;
+
+        dispc_mgr_set_timings(mgr->id, &mp->timings);
+
+        /* lcd_config parameters */
+        if (dss_mgr_is_lcd(mgr->id))
+                dispc_mgr_set_lcd_config(mgr->id, &mp->lcd_config);
+
+        mp->extra_info_dirty = false;
+        if (mp->updating)
+                mp->shadow_extra_info_dirty = true;
+}
+
+static void dss_write_regs(void)
+{
+        const int num_mgrs = omap_dss_get_num_overlay_managers();
+        int i;
+
+        for (i = 0; i < num_mgrs; ++i) {
+                struct omap_overlay_manager *mgr;
+                struct mgr_priv_data *mp;
+                int r;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                mp = get_mgr_priv(mgr);
+
+                if (!mp->enabled || mgr_manual_update(mgr) || mp->busy)
+                        continue;
+
+                r = dss_check_settings(mgr);
+                if (r) {
+                        DSSERR("cannot write registers for manager %s: "
+                                        "illegal configuration\n", mgr->name);
+                        continue;
+                }
+
+                dss_mgr_write_regs(mgr);
+                dss_mgr_write_regs_extra(mgr);
+        }
+}
+
+static void dss_set_go_bits(void)
+{
+        const int num_mgrs = omap_dss_get_num_overlay_managers();
+        int i;
+
+        for (i = 0; i < num_mgrs; ++i) {
+                struct omap_overlay_manager *mgr;
+                struct mgr_priv_data *mp;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                mp = get_mgr_priv(mgr);
+
+                if (!mp->enabled || mgr_manual_update(mgr) || mp->busy)
+                        continue;
+
+                if (!need_go(mgr))
+                        continue;
+
+                mp->busy = true;
+
+                if (!dss_data.irq_enabled && need_isr())
+                        dss_register_vsync_isr();
+
+                dispc_mgr_go(mgr->id);
+        }
+
+}
+
+static void mgr_clear_shadow_dirty(struct omap_overlay_manager *mgr)
+{
+        struct omap_overlay *ovl;
+        struct mgr_priv_data *mp;
+        struct ovl_priv_data *op;
+
+        mp = get_mgr_priv(mgr);
+        mp->shadow_info_dirty = false;
+        mp->shadow_extra_info_dirty = false;
+
+        list_for_each_entry(ovl, &mgr->overlays, list) {
+                op = get_ovl_priv(ovl);
+                op->shadow_info_dirty = false;
+                op->shadow_extra_info_dirty = false;
+        }
+}
+
+static int dss_mgr_connect_compat(struct omap_overlay_manager *mgr,
+                struct omap_dss_device *dst)
+{
+        return mgr->set_output(mgr, dst);
+}
+
+static void dss_mgr_disconnect_compat(struct omap_overlay_manager *mgr,
+                struct omap_dss_device *dst)
+{
+        mgr->unset_output(mgr);
+}
+
+static void dss_mgr_start_update_compat(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        WARN_ON(mp->updating);
+
+        r = dss_check_settings(mgr);
+        if (r) {
+                DSSERR("cannot start manual update: illegal configuration\n");
+                spin_unlock_irqrestore(&data_lock, flags);
+                return;
+        }
+
+        dss_mgr_write_regs(mgr);
+        dss_mgr_write_regs_extra(mgr);
+
+        mp->updating = true;
+
+        if (!dss_data.irq_enabled && need_isr())
+                dss_register_vsync_isr();
+
+        dispc_mgr_enable_sync(mgr->id);
+
+        spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static void dss_apply_irq_handler(void *data, u32 mask);
+
+static void dss_register_vsync_isr(void)
+{
+        const int num_mgrs = dss_feat_get_num_mgrs();
+        u32 mask;
+        int r, i;
+
+        mask = 0;
+        for (i = 0; i < num_mgrs; ++i)
+                mask |= dispc_mgr_get_vsync_irq(i);
+
+        for (i = 0; i < num_mgrs; ++i)
+                mask |= dispc_mgr_get_framedone_irq(i);
+
+        r = omap_dispc_register_isr(dss_apply_irq_handler, NULL, mask);
+        WARN_ON(r);
+
+        dss_data.irq_enabled = true;
+}
+
+static void dss_unregister_vsync_isr(void)
+{
+        const int num_mgrs = dss_feat_get_num_mgrs();
+        u32 mask;
+        int r, i;
+
+        mask = 0;
+        for (i = 0; i < num_mgrs; ++i)
+                mask |= dispc_mgr_get_vsync_irq(i);
+
+        for (i = 0; i < num_mgrs; ++i)
+                mask |= dispc_mgr_get_framedone_irq(i);
+
+        r = omap_dispc_unregister_isr(dss_apply_irq_handler, NULL, mask);
+        WARN_ON(r);
+
+        dss_data.irq_enabled = false;
+}
+
+static void dss_apply_irq_handler(void *data, u32 mask)
+{
+        const int num_mgrs = dss_feat_get_num_mgrs();
+        int i;
+        bool extra_updating;
+
+        spin_lock(&data_lock);
+
+        /* clear busy, updating flags, shadow_dirty flags */
+        for (i = 0; i < num_mgrs; i++) {
+                struct omap_overlay_manager *mgr;
+                struct mgr_priv_data *mp;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                mp = get_mgr_priv(mgr);
+
+                if (!mp->enabled)
+                        continue;
+
+                mp->updating = dispc_mgr_is_enabled(i);
+
+                if (!mgr_manual_update(mgr)) {
+                        bool was_busy = mp->busy;
+                        mp->busy = dispc_mgr_go_busy(i);
+
+                        if (was_busy && !mp->busy)
+                                mgr_clear_shadow_dirty(mgr);
+                }
+        }
+
+        dss_write_regs();
+        dss_set_go_bits();
+
+        extra_updating = extra_info_update_ongoing();
+        if (!extra_updating)
+                complete_all(&extra_updated_completion);
+
+        /* call framedone handlers for manual update displays */
+        for (i = 0; i < num_mgrs; i++) {
+                struct omap_overlay_manager *mgr;
+                struct mgr_priv_data *mp;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                mp = get_mgr_priv(mgr);
+
+                if (!mgr_manual_update(mgr) || !mp->framedone_handler)
+                        continue;
+
+                if (mask & dispc_mgr_get_framedone_irq(i))
+                        mp->framedone_handler(mp->framedone_handler_data);
+        }
+
+        if (!need_isr())
+                dss_unregister_vsync_isr();
+
+        spin_unlock(&data_lock);
+}
+
+static void omap_dss_mgr_apply_ovl(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op;
+
+        op = get_ovl_priv(ovl);
+
+        if (!op->user_info_dirty)
+                return;
+
+        op->user_info_dirty = false;
+        op->info_dirty = true;
+        op->info = op->user_info;
+}
+
+static void omap_dss_mgr_apply_mgr(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp;
+
+        mp = get_mgr_priv(mgr);
+
+        if (!mp->user_info_dirty)
+                return;
+
+        mp->user_info_dirty = false;
+        mp->info_dirty = true;
+        mp->info = mp->user_info;
+}
+
+static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
+{
+        unsigned long flags;
+        struct omap_overlay *ovl;
+        int r;
+
+        DSSDBG("omap_dss_mgr_apply(%s)\n", mgr->name);
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        r = dss_check_settings_apply(mgr);
+        if (r) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                DSSERR("failed to apply settings: illegal configuration.\n");
+                return r;
+        }
+
+        /* Configure overlays */
+        list_for_each_entry(ovl, &mgr->overlays, list)
+                omap_dss_mgr_apply_ovl(ovl);
+
+        /* Configure manager */
+        omap_dss_mgr_apply_mgr(mgr);
+
+        dss_write_regs();
+        dss_set_go_bits();
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        return 0;
+}
+
+static void dss_apply_ovl_enable(struct omap_overlay *ovl, bool enable)
+{
+        struct ovl_priv_data *op;
+
+        op = get_ovl_priv(ovl);
+
+        if (op->enabled == enable)
+                return;
+
+        op->enabled = enable;
+        op->extra_info_dirty = true;
+}
+
+static void dss_apply_ovl_fifo_thresholds(struct omap_overlay *ovl,
+                u32 fifo_low, u32 fifo_high)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+
+        if (op->fifo_low == fifo_low && op->fifo_high == fifo_high)
+                return;
+
+        op->fifo_low = fifo_low;
+        op->fifo_high = fifo_high;
+        op->extra_info_dirty = true;
+}
+
+static void dss_ovl_setup_fifo(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        u32 fifo_low, fifo_high;
+        bool use_fifo_merge = false;
+
+        if (!op->enabled && !op->enabling)
+                return;
+
+        dispc_ovl_compute_fifo_thresholds(ovl->id, &fifo_low, &fifo_high,
+                        use_fifo_merge, ovl_manual_update(ovl));
+
+        dss_apply_ovl_fifo_thresholds(ovl, fifo_low, fifo_high);
+}
+
+static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr)
+{
+        struct omap_overlay *ovl;
+        struct mgr_priv_data *mp;
+
+        mp = get_mgr_priv(mgr);
+
+        if (!mp->enabled)
+                return;
+
+        list_for_each_entry(ovl, &mgr->overlays, list)
+                dss_ovl_setup_fifo(ovl);
+}
+
+static void dss_setup_fifos(void)
+{
+        const int num_mgrs = omap_dss_get_num_overlay_managers();
+        struct omap_overlay_manager *mgr;
+        int i;
+
+        for (i = 0; i < num_mgrs; ++i) {
+                mgr = omap_dss_get_overlay_manager(i);
+                dss_mgr_setup_fifos(mgr);
+        }
+}
+
+static int dss_mgr_enable_compat(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
+        int r;
+
+        mutex_lock(&apply_lock);
+
+        if (mp->enabled)
+                goto out;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        mp->enabled = true;
+
+        r = dss_check_settings(mgr);
+        if (r) {
+                DSSERR("failed to enable manager %d: check_settings failed\n",
+                                mgr->id);
+                goto err;
+        }
+
+        dss_setup_fifos();
+
+        dss_write_regs();
+        dss_set_go_bits();
+
+        if (!mgr_manual_update(mgr))
+                mp->updating = true;
+
+        if (!dss_data.irq_enabled && need_isr())
+                dss_register_vsync_isr();
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        if (!mgr_manual_update(mgr))
+                dispc_mgr_enable_sync(mgr->id);
+
+out:
+        mutex_unlock(&apply_lock);
+
+        return 0;
+
+err:
+        mp->enabled = false;
+        spin_unlock_irqrestore(&data_lock, flags);
+        mutex_unlock(&apply_lock);
+        return r;
+}
+
+static void dss_mgr_disable_compat(struct omap_overlay_manager *mgr)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
+
+        mutex_lock(&apply_lock);
+
+        if (!mp->enabled)
+                goto out;
+
+        wait_pending_extra_info_updates();
+
+        if (!mgr_manual_update(mgr))
+                dispc_mgr_disable_sync(mgr->id);
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        mp->updating = false;
+        mp->enabled = false;
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+out:
+        mutex_unlock(&apply_lock);
+}
+
+static int dss_mgr_set_info(struct omap_overlay_manager *mgr,
+                struct omap_overlay_manager_info *info)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
+        int r;
+
+        r = dss_mgr_simple_check(mgr, info);
+        if (r)
+                return r;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        mp->user_info = *info;
+        mp->user_info_dirty = true;
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        return 0;
+}
+
+static void dss_mgr_get_info(struct omap_overlay_manager *mgr,
+                struct omap_overlay_manager_info *info)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        *info = mp->user_info;
+
+        spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static int dss_mgr_set_output(struct omap_overlay_manager *mgr,
+                struct omap_dss_device *output)
+{
+        int r;
+
+        mutex_lock(&apply_lock);
+
+        if (mgr->output) {
+                DSSERR("manager %s is already connected to an output\n",
+                        mgr->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        if ((mgr->supported_outputs & output->id) == 0) {
+                DSSERR("output does not support manager %s\n",
+                        mgr->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        output->manager = mgr;
+        mgr->output = output;
+
+        mutex_unlock(&apply_lock);
+
+        return 0;
+err:
+        mutex_unlock(&apply_lock);
+        return r;
+}
+
+static int dss_mgr_unset_output(struct omap_overlay_manager *mgr)
+{
+        int r;
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+        unsigned long flags;
+
+        mutex_lock(&apply_lock);
+
+        if (!mgr->output) {
+                DSSERR("failed to unset output, output not set\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (mp->enabled) {
+                DSSERR("output can't be unset when manager is enabled\n");
+                r = -EINVAL;
+                goto err1;
+        }
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        mgr->output->manager = NULL;
+        mgr->output = NULL;
+
+        mutex_unlock(&apply_lock);
+
+        return 0;
+err1:
+        spin_unlock_irqrestore(&data_lock, flags);
+err:
+        mutex_unlock(&apply_lock);
+
+        return r;
+}
+
+static void dss_apply_mgr_timings(struct omap_overlay_manager *mgr,
+                const struct omap_video_timings *timings)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        mp->timings = *timings;
+        mp->extra_info_dirty = true;
+}
+
+static void dss_mgr_set_timings_compat(struct omap_overlay_manager *mgr,
+                const struct omap_video_timings *timings)
+{
+        unsigned long flags;
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (mp->updating) {
+                DSSERR("cannot set timings for %s: manager needs to be disabled\n",
+                        mgr->name);
+                goto out;
+        }
+
+        dss_apply_mgr_timings(mgr, timings);
+out:
+        spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static void dss_apply_mgr_lcd_config(struct omap_overlay_manager *mgr,
+                const struct dss_lcd_mgr_config *config)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        mp->lcd_config = *config;
+        mp->extra_info_dirty = true;
+}
+
+static void dss_mgr_set_lcd_config_compat(struct omap_overlay_manager *mgr,
+                const struct dss_lcd_mgr_config *config)
+{
+        unsigned long flags;
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (mp->enabled) {
+                DSSERR("cannot apply lcd config for %s: manager needs to be disabled\n",
+                        mgr->name);
+                goto out;
+        }
+
+        dss_apply_mgr_lcd_config(mgr, config);
+out:
+        spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static int dss_ovl_set_info(struct omap_overlay *ovl,
+                struct omap_overlay_info *info)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+        int r;
+
+        r = dss_ovl_simple_check(ovl, info);
+        if (r)
+                return r;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        op->user_info = *info;
+        op->user_info_dirty = true;
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        return 0;
+}
+
+static void dss_ovl_get_info(struct omap_overlay *ovl,
+                struct omap_overlay_info *info)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        *info = op->user_info;
+
+        spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static int dss_ovl_set_manager(struct omap_overlay *ovl,
+                struct omap_overlay_manager *mgr)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+        int r;
+
+        if (!mgr)
+                return -EINVAL;
+
+        mutex_lock(&apply_lock);
+
+        if (ovl->manager) {
+                DSSERR("overlay '%s' already has a manager '%s'\n",
+                                ovl->name, ovl->manager->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        r = dispc_runtime_get();
+        if (r)
+                goto err;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (op->enabled) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                DSSERR("overlay has to be disabled to change the manager\n");
+                r = -EINVAL;
+                goto err1;
+        }
+
+        dispc_ovl_set_channel_out(ovl->id, mgr->id);
+
+        ovl->manager = mgr;
+        list_add_tail(&ovl->list, &mgr->overlays);
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        dispc_runtime_put();
+
+        mutex_unlock(&apply_lock);
+
+        return 0;
+
+err1:
+        dispc_runtime_put();
+err:
+        mutex_unlock(&apply_lock);
+        return r;
+}
+
+static int dss_ovl_unset_manager(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+        int r;
+
+        mutex_lock(&apply_lock);
+
+        if (!ovl->manager) {
+                DSSERR("failed to detach overlay: manager not set\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (op->enabled) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                DSSERR("overlay has to be disabled to unset the manager\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        /* wait for pending extra_info updates to ensure the ovl is disabled */
+        wait_pending_extra_info_updates();
+
+        /*
+         * For a manual update display, there is no guarantee that the overlay
+         * is really disabled in HW, we may need an extra update from this
+         * manager before the configurations can go in. Return an error if the
+         * overlay needed an update from the manager.
+         *
+         * TODO: Instead of returning an error, try to do a dummy manager update
+         * here to disable the overlay in hardware. Use the *GATED fields in
+         * the DISPC_CONFIG registers to do a dummy update.
+         */
+        spin_lock_irqsave(&data_lock, flags);
+
+        if (ovl_manual_update(ovl) && op->extra_info_dirty) {
+                spin_unlock_irqrestore(&data_lock, flags);
+                DSSERR("need an update to change the manager\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        ovl->manager = NULL;
+        list_del(&ovl->list);
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        mutex_unlock(&apply_lock);
+
+        return 0;
+err:
+        mutex_unlock(&apply_lock);
+        return r;
+}
+
+static bool dss_ovl_is_enabled(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+        bool e;
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        e = op->enabled;
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        return e;
+}
+
+static int dss_ovl_enable(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+        int r;
+
+        mutex_lock(&apply_lock);
+
+        if (op->enabled) {
+                r = 0;
+                goto err1;
+        }
+
+        if (ovl->manager == NULL || ovl->manager->output == NULL) {
+                r = -EINVAL;
+                goto err1;
+        }
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        op->enabling = true;
+
+        r = dss_check_settings(ovl->manager);
+        if (r) {
+                DSSERR("failed to enable overlay %d: check_settings failed\n",
+                                ovl->id);
+                goto err2;
+        }
+
+        dss_setup_fifos();
+
+        op->enabling = false;
+        dss_apply_ovl_enable(ovl, true);
+
+        dss_write_regs();
+        dss_set_go_bits();
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        mutex_unlock(&apply_lock);
+
+        return 0;
+err2:
+        op->enabling = false;
+        spin_unlock_irqrestore(&data_lock, flags);
+err1:
+        mutex_unlock(&apply_lock);
+        return r;
+}
+
+static int dss_ovl_disable(struct omap_overlay *ovl)
+{
+        struct ovl_priv_data *op = get_ovl_priv(ovl);
+        unsigned long flags;
+        int r;
+
+        mutex_lock(&apply_lock);
+
+        if (!op->enabled) {
+                r = 0;
+                goto err;
+        }
+
+        if (ovl->manager == NULL || ovl->manager->output == NULL) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        spin_lock_irqsave(&data_lock, flags);
+
+        dss_apply_ovl_enable(ovl, false);
+        dss_write_regs();
+        dss_set_go_bits();
+
+        spin_unlock_irqrestore(&data_lock, flags);
+
+        mutex_unlock(&apply_lock);
+
+        return 0;
+
+err:
+        mutex_unlock(&apply_lock);
+        return r;
+}
+
+static int dss_mgr_register_framedone_handler_compat(struct omap_overlay_manager *mgr,
+                void (*handler)(void *), void *data)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        if (mp->framedone_handler)
+                return -EBUSY;
+
+        mp->framedone_handler = handler;
+        mp->framedone_handler_data = data;
+
+        return 0;
+}
+
+static void dss_mgr_unregister_framedone_handler_compat(struct omap_overlay_manager *mgr,
+                void (*handler)(void *), void *data)
+{
+        struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+        WARN_ON(mp->framedone_handler != handler ||
+                        mp->framedone_handler_data != data);
+
+        mp->framedone_handler = NULL;
+        mp->framedone_handler_data = NULL;
+}
+
+static const struct dss_mgr_ops apply_mgr_ops = {
+        .connect = dss_mgr_connect_compat,
+        .disconnect = dss_mgr_disconnect_compat,
+        .start_update = dss_mgr_start_update_compat,
+        .enable = dss_mgr_enable_compat,
+        .disable = dss_mgr_disable_compat,
+        .set_timings = dss_mgr_set_timings_compat,
+        .set_lcd_config = dss_mgr_set_lcd_config_compat,
+        .register_framedone_handler = dss_mgr_register_framedone_handler_compat,
+        .unregister_framedone_handler = dss_mgr_unregister_framedone_handler_compat,
+};
+
+static int compat_refcnt;
+static DEFINE_MUTEX(compat_init_lock);
+
+int omapdss_compat_init(void)
+{
+        struct platform_device *pdev = dss_get_core_pdev();
+        int i, r;
+
+        mutex_lock(&compat_init_lock);
+
+        if (compat_refcnt++ > 0)
+                goto out;
+
+        apply_init_priv();
+
+        dss_init_overlay_managers_sysfs(pdev);
+        dss_init_overlays(pdev);
+
+        for (i = 0; i < omap_dss_get_num_overlay_managers(); i++) {
+                struct omap_overlay_manager *mgr;
+
+                mgr = omap_dss_get_overlay_manager(i);
+
+                mgr->set_output = &dss_mgr_set_output;
+                mgr->unset_output = &dss_mgr_unset_output;
+                mgr->apply = &omap_dss_mgr_apply;
+                mgr->set_manager_info = &dss_mgr_set_info;
+                mgr->get_manager_info = &dss_mgr_get_info;
+                mgr->wait_for_go = &dss_mgr_wait_for_go;
+                mgr->wait_for_vsync = &dss_mgr_wait_for_vsync;
+                mgr->get_device = &dss_mgr_get_device;
+        }
+
+        for (i = 0; i < omap_dss_get_num_overlays(); i++) {
+                struct omap_overlay *ovl = omap_dss_get_overlay(i);
+
+                ovl->is_enabled = &dss_ovl_is_enabled;
+                ovl->enable = &dss_ovl_enable;
+                ovl->disable = &dss_ovl_disable;
+                ovl->set_manager = &dss_ovl_set_manager;
+                ovl->unset_manager = &dss_ovl_unset_manager;
+                ovl->set_overlay_info = &dss_ovl_set_info;
+                ovl->get_overlay_info = &dss_ovl_get_info;
+                ovl->wait_for_go = &dss_mgr_wait_for_go_ovl;
+                ovl->get_device = &dss_ovl_get_device;
+        }
+
+        r = dss_install_mgr_ops(&apply_mgr_ops);
+        if (r)
+                goto err_mgr_ops;
+
+        r = display_init_sysfs(pdev);
+        if (r)
+                goto err_disp_sysfs;
+
+        dispc_runtime_get();
+
+        r = dss_dispc_initialize_irq();
+        if (r)
+                goto err_init_irq;
+
+        dispc_runtime_put();
+
+out:
+        mutex_unlock(&compat_init_lock);
+
+        return 0;
+
+err_init_irq:
+        dispc_runtime_put();
+        display_uninit_sysfs(pdev);
+
+err_disp_sysfs:
+        dss_uninstall_mgr_ops();
+
+err_mgr_ops:
+        dss_uninit_overlay_managers_sysfs(pdev);
+        dss_uninit_overlays(pdev);
+
+        compat_refcnt--;
+
+        mutex_unlock(&compat_init_lock);
+
+        return r;
+}
+EXPORT_SYMBOL(omapdss_compat_init);
+
+void omapdss_compat_uninit(void)
+{
+        struct platform_device *pdev = dss_get_core_pdev();
+
+        mutex_lock(&compat_init_lock);
+
+        if (--compat_refcnt > 0)
+                goto out;
+
+        dss_dispc_uninitialize_irq();
+
+        display_uninit_sysfs(pdev);
+
+        dss_uninstall_mgr_ops();
+
+        dss_uninit_overlay_managers_sysfs(pdev);
+        dss_uninit_overlays(pdev);
+out:
+        mutex_unlock(&compat_init_lock);
+}
+EXPORT_SYMBOL(omapdss_compat_uninit);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/core.c b/drivers/video/fbdev/omap2/omapfb/dss/core.c
new file mode 100644
index 000000000000..5a87179b7312
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/core.c
@@ -0,0 +1,343 @@
+/*
+ * linux/drivers/video/omap2/dss/core.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "CORE"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/suspend.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static struct {
+        struct platform_device *pdev;
+
+        const char *default_display_name;
+} core;
+
+static char *def_disp_name;
+module_param_named(def_disp, def_disp_name, charp, 0);
+MODULE_PARM_DESC(def_disp, "default display name");
+
+const char *omapdss_get_default_display_name(void)
+{
+        return core.default_display_name;
+}
+EXPORT_SYMBOL(omapdss_get_default_display_name);
+
+enum omapdss_version omapdss_get_version(void)
+{
+        struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
+        return pdata->version;
+}
+EXPORT_SYMBOL(omapdss_get_version);
+
+struct platform_device *dss_get_core_pdev(void)
+{
+        return core.pdev;
+}
+
+int dss_dsi_enable_pads(int dsi_id, unsigned lane_mask)
+{
+        struct omap_dss_board_info *board_data = core.pdev->dev.platform_data;
+
+        if (!board_data->dsi_enable_pads)
+                return -ENOENT;
+
+        return board_data->dsi_enable_pads(dsi_id, lane_mask);
+}
+
+void dss_dsi_disable_pads(int dsi_id, unsigned lane_mask)
+{
+        struct omap_dss_board_info *board_data = core.pdev->dev.platform_data;
+
+        if (!board_data->dsi_disable_pads)
+                return;
+
+        return board_data->dsi_disable_pads(dsi_id, lane_mask);
+}
+
+int dss_set_min_bus_tput(struct device *dev, unsigned long tput)
+{
+        struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
+
+        if (pdata->set_min_bus_tput)
+                return pdata->set_min_bus_tput(dev, tput);
+        else
+                return 0;
+}
+
+#if defined(CONFIG_FB_OMAP2_DSS_DEBUGFS)
+static int dss_debug_show(struct seq_file *s, void *unused)
+{
+        void (*func)(struct seq_file *) = s->private;
+        func(s);
+        return 0;
+}
+
+static int dss_debug_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, dss_debug_show, inode->i_private);
+}
+
+static const struct file_operations dss_debug_fops = {
+        .open           = dss_debug_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
+
+static struct dentry *dss_debugfs_dir;
+
+static int dss_initialize_debugfs(void)
+{
+        dss_debugfs_dir = debugfs_create_dir("omapdss", NULL);
+        if (IS_ERR(dss_debugfs_dir)) {
+                int err = PTR_ERR(dss_debugfs_dir);
+                dss_debugfs_dir = NULL;
+                return err;
+        }
+
+        debugfs_create_file("clk", S_IRUGO, dss_debugfs_dir,
+                        &dss_debug_dump_clocks, &dss_debug_fops);
+
+        return 0;
+}
+
+static void dss_uninitialize_debugfs(void)
+{
+        if (dss_debugfs_dir)
+                debugfs_remove_recursive(dss_debugfs_dir);
+}
+
+int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *))
+{
+        struct dentry *d;
+
+        d = debugfs_create_file(name, S_IRUGO, dss_debugfs_dir,
+                        write, &dss_debug_fops);
+
+        return PTR_ERR_OR_ZERO(d);
+}
+#else /* CONFIG_FB_OMAP2_DSS_DEBUGFS */
+static inline int dss_initialize_debugfs(void)
+{
+        return 0;
+}
+static inline void dss_uninitialize_debugfs(void)
+{
+}
+int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *))
+{
+        return 0;
+}
+#endif /* CONFIG_FB_OMAP2_DSS_DEBUGFS */
+
+/* PLATFORM DEVICE */
+static int omap_dss_pm_notif(struct notifier_block *b, unsigned long v, void *d)
+{
+        DSSDBG("pm notif %lu\n", v);
+
+        switch (v) {
+        case PM_SUSPEND_PREPARE:
+        case PM_HIBERNATION_PREPARE:
+        case PM_RESTORE_PREPARE:
+                DSSDBG("suspending displays\n");
+                return dss_suspend_all_devices();
+
+        case PM_POST_SUSPEND:
+        case PM_POST_HIBERNATION:
+        case PM_POST_RESTORE:
+                DSSDBG("resuming displays\n");
+                return dss_resume_all_devices();
+
+        default:
+                return 0;
+        }
+}
+
+static struct notifier_block omap_dss_pm_notif_block = {
+        .notifier_call = omap_dss_pm_notif,
+};
+
+static int __init omap_dss_probe(struct platform_device *pdev)
+{
+        struct omap_dss_board_info *pdata = pdev->dev.platform_data;
+        int r;
+
+        core.pdev = pdev;
+
+        dss_features_init(omapdss_get_version());
+
+        r = dss_initialize_debugfs();
+        if (r)
+                goto err_debugfs;
+
+        if (def_disp_name)
+                core.default_display_name = def_disp_name;
+        else if (pdata->default_display_name)
+                core.default_display_name = pdata->default_display_name;
+        else if (pdata->default_device)
+                core.default_display_name = pdata->default_device->name;
+
+        register_pm_notifier(&omap_dss_pm_notif_block);
+
+        return 0;
+
+err_debugfs:
+
+        return r;
+}
+
+static int omap_dss_remove(struct platform_device *pdev)
+{
+        unregister_pm_notifier(&omap_dss_pm_notif_block);
+
+        dss_uninitialize_debugfs();
+
+        return 0;
+}
+
+static void omap_dss_shutdown(struct platform_device *pdev)
+{
+        DSSDBG("shutdown\n");
+        dss_disable_all_devices();
+}
+
+static struct platform_driver omap_dss_driver = {
+        .remove         = omap_dss_remove,
+        .shutdown       = omap_dss_shutdown,
+        .driver         = {
+                .name   = "omapdss",
+        },
+};
+
+/* INIT */
+static int (*dss_output_drv_reg_funcs[])(void) __initdata = {
+        dss_init_platform_driver,
+        dispc_init_platform_driver,
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+        dsi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_DPI
+        dpi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_SDI
+        sdi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_RFBI
+        rfbi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+        venc_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP4_DSS_HDMI
+        hdmi4_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP5_DSS_HDMI
+        hdmi5_init_platform_driver,
+#endif
+};
+
+static void (*dss_output_drv_unreg_funcs[])(void) = {
+#ifdef CONFIG_FB_OMAP5_DSS_HDMI
+        hdmi5_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP4_DSS_HDMI
+        hdmi4_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+        venc_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_RFBI
+        rfbi_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_SDI
+        sdi_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_DPI
+        dpi_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+        dsi_uninit_platform_driver,
+#endif
+        dispc_uninit_platform_driver,
+        dss_uninit_platform_driver,
+};
+
+static int __init omap_dss_init(void)
+{
+        int r;
+        int i;
+
+        r = platform_driver_probe(&omap_dss_driver, omap_dss_probe);
+        if (r)
+                return r;
+
+        for (i = 0; i < ARRAY_SIZE(dss_output_drv_reg_funcs); ++i) {
+                r = dss_output_drv_reg_funcs[i]();
+                if (r)
+                        goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        for (i = ARRAY_SIZE(dss_output_drv_reg_funcs) - i;
+                        i < ARRAY_SIZE(dss_output_drv_reg_funcs);
+                        ++i)
+                dss_output_drv_unreg_funcs[i]();
+
+        platform_driver_unregister(&omap_dss_driver);
+
+        return r;
+}
+
+static void __exit omap_dss_exit(void)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dss_output_drv_unreg_funcs); ++i)
+                dss_output_drv_unreg_funcs[i]();
+
+        platform_driver_unregister(&omap_dss_driver);
+}
+
+module_init(omap_dss_init);
+module_exit(omap_dss_exit);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@nokia.com>");
+MODULE_DESCRIPTION("OMAP2/3 Display Subsystem");
+MODULE_LICENSE("GPL v2");
+
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.c b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.c
new file mode 100644
index 000000000000..6607db37a5e4
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.c
@@ -0,0 +1,667 @@
+/*
+ * Copyright (C) 2012 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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 "APPLY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/jiffies.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/seq_file.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+#include "dispc-compat.h"
+
+#define DISPC_IRQ_MASK_ERROR            (DISPC_IRQ_GFX_FIFO_UNDERFLOW | \
+                                         DISPC_IRQ_OCP_ERR | \
+                                         DISPC_IRQ_VID1_FIFO_UNDERFLOW | \
+                                         DISPC_IRQ_VID2_FIFO_UNDERFLOW | \
+                                         DISPC_IRQ_SYNC_LOST | \
+                                         DISPC_IRQ_SYNC_LOST_DIGIT)
+
+#define DISPC_MAX_NR_ISRS               8
+
+struct omap_dispc_isr_data {
+        omap_dispc_isr_t        isr;
+        void                    *arg;
+        u32                     mask;
+};
+
+struct dispc_irq_stats {
+        unsigned long last_reset;
+        unsigned irq_count;
+        unsigned irqs[32];
+};
+
+static struct {
+        spinlock_t irq_lock;
+        u32 irq_error_mask;
+        struct omap_dispc_isr_data registered_isr[DISPC_MAX_NR_ISRS];
+        u32 error_irqs;
+        struct work_struct error_work;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        spinlock_t irq_stats_lock;
+        struct dispc_irq_stats irq_stats;
+#endif
+} dispc_compat;
+
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dispc_dump_irqs(struct seq_file *s)
+{
+        unsigned long flags;
+        struct dispc_irq_stats stats;
+
+        spin_lock_irqsave(&dispc_compat.irq_stats_lock, flags);
+
+        stats = dispc_compat.irq_stats;
+        memset(&dispc_compat.irq_stats, 0, sizeof(dispc_compat.irq_stats));
+        dispc_compat.irq_stats.last_reset = jiffies;
+
+        spin_unlock_irqrestore(&dispc_compat.irq_stats_lock, flags);
+
+        seq_printf(s, "period %u ms\n",
+                        jiffies_to_msecs(jiffies - stats.last_reset));
+
+        seq_printf(s, "irqs %d\n", stats.irq_count);
+#define PIS(x) \
+        seq_printf(s, "%-20s %10d\n", #x, stats.irqs[ffs(DISPC_IRQ_##x)-1]);
+
+        PIS(FRAMEDONE);
+        PIS(VSYNC);
+        PIS(EVSYNC_EVEN);
+        PIS(EVSYNC_ODD);
+        PIS(ACBIAS_COUNT_STAT);
+        PIS(PROG_LINE_NUM);
+        PIS(GFX_FIFO_UNDERFLOW);
+        PIS(GFX_END_WIN);
+        PIS(PAL_GAMMA_MASK);
+        PIS(OCP_ERR);
+        PIS(VID1_FIFO_UNDERFLOW);
+        PIS(VID1_END_WIN);
+        PIS(VID2_FIFO_UNDERFLOW);
+        PIS(VID2_END_WIN);
+        if (dss_feat_get_num_ovls() > 3) {
+                PIS(VID3_FIFO_UNDERFLOW);
+                PIS(VID3_END_WIN);
+        }
+        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);
+        }
+        if (dss_has_feature(FEAT_MGR_LCD3)) {
+                PIS(FRAMEDONE3);
+                PIS(VSYNC3);
+                PIS(ACBIAS_COUNT_STAT3);
+                PIS(SYNC_LOST3);
+        }
+#undef PIS
+}
+#endif
+
+/* dispc.irq_lock has to be locked by the caller */
+static void _omap_dispc_set_irqs(void)
+{
+        u32 mask;
+        int i;
+        struct omap_dispc_isr_data *isr_data;
+
+        mask = dispc_compat.irq_error_mask;
+
+        for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+                isr_data = &dispc_compat.registered_isr[i];
+
+                if (isr_data->isr == NULL)
+                        continue;
+
+                mask |= isr_data->mask;
+        }
+
+        dispc_write_irqenable(mask);
+}
+
+int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
+{
+        int i;
+        int ret;
+        unsigned long flags;
+        struct omap_dispc_isr_data *isr_data;
+
+        if (isr == NULL)
+                return -EINVAL;
+
+        spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+
+        /* check for duplicate entry */
+        for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+                isr_data = &dispc_compat.registered_isr[i];
+                if (isr_data->isr == isr && isr_data->arg == arg &&
+                                isr_data->mask == mask) {
+                        ret = -EINVAL;
+                        goto err;
+                }
+        }
+
+        isr_data = NULL;
+        ret = -EBUSY;
+
+        for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+                isr_data = &dispc_compat.registered_isr[i];
+
+                if (isr_data->isr != NULL)
+                        continue;
+
+                isr_data->isr = isr;
+                isr_data->arg = arg;
+                isr_data->mask = mask;
+                ret = 0;
+
+                break;
+        }
+
+        if (ret)
+                goto err;
+
+        _omap_dispc_set_irqs();
+
+        spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+        return 0;
+err:
+        spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+        return ret;
+}
+EXPORT_SYMBOL(omap_dispc_register_isr);
+
+int omap_dispc_unregister_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
+{
+        int i;
+        unsigned long flags;
+        int ret = -EINVAL;
+        struct omap_dispc_isr_data *isr_data;
+
+        spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+
+        for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+                isr_data = &dispc_compat.registered_isr[i];
+                if (isr_data->isr != isr || isr_data->arg != arg ||
+                                isr_data->mask != mask)
+                        continue;
+
+                /* found the correct isr */
+
+                isr_data->isr = NULL;
+                isr_data->arg = NULL;
+                isr_data->mask = 0;
+
+                ret = 0;
+                break;
+        }
+
+        if (ret == 0)
+                _omap_dispc_set_irqs();
+
+        spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+        return ret;
+}
+EXPORT_SYMBOL(omap_dispc_unregister_isr);
+
+static void print_irq_status(u32 status)
+{
+        if ((status & dispc_compat.irq_error_mask) == 0)
+                return;
+
+#define PIS(x) (status & DISPC_IRQ_##x) ? (#x " ") : ""
+
+        pr_debug("DISPC IRQ: 0x%x: %s%s%s%s%s%s%s%s%s\n",
+                status,
+                PIS(OCP_ERR),
+                PIS(GFX_FIFO_UNDERFLOW),
+                PIS(VID1_FIFO_UNDERFLOW),
+                PIS(VID2_FIFO_UNDERFLOW),
+                dss_feat_get_num_ovls() > 3 ? PIS(VID3_FIFO_UNDERFLOW) : "",
+                PIS(SYNC_LOST),
+                PIS(SYNC_LOST_DIGIT),
+                dss_has_feature(FEAT_MGR_LCD2) ? PIS(SYNC_LOST2) : "",
+                dss_has_feature(FEAT_MGR_LCD3) ? PIS(SYNC_LOST3) : "");
+#undef PIS
+}
+
+/* Called from dss.c. Note that we don't touch clocks here,
+ * 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. */
+static irqreturn_t omap_dispc_irq_handler(int irq, void *arg)
+{
+        int i;
+        u32 irqstatus, irqenable;
+        u32 handledirqs = 0;
+        u32 unhandled_errors;
+        struct omap_dispc_isr_data *isr_data;
+        struct omap_dispc_isr_data registered_isr[DISPC_MAX_NR_ISRS];
+
+        spin_lock(&dispc_compat.irq_lock);
+
+        irqstatus = dispc_read_irqstatus();
+        irqenable = dispc_read_irqenable();
+
+        /* IRQ is not for us */
+        if (!(irqstatus & irqenable)) {
+                spin_unlock(&dispc_compat.irq_lock);
+                return IRQ_NONE;
+        }
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        spin_lock(&dispc_compat.irq_stats_lock);
+        dispc_compat.irq_stats.irq_count++;
+        dss_collect_irq_stats(irqstatus, dispc_compat.irq_stats.irqs);
+        spin_unlock(&dispc_compat.irq_stats_lock);
+#endif
+
+        print_irq_status(irqstatus);
+
+        /* Ack the interrupt. Do it here before clocks are possibly turned
+         * off */
+        dispc_clear_irqstatus(irqstatus);
+        /* flush posted write */
+        dispc_read_irqstatus();
+
+        /* make a copy and unlock, so that isrs can unregister
+         * themselves */
+        memcpy(registered_isr, dispc_compat.registered_isr,
+                        sizeof(registered_isr));
+
+        spin_unlock(&dispc_compat.irq_lock);
+
+        for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+                isr_data = &registered_isr[i];
+
+                if (!isr_data->isr)
+                        continue;
+
+                if (isr_data->mask & irqstatus) {
+                        isr_data->isr(isr_data->arg, irqstatus);
+                        handledirqs |= isr_data->mask;
+                }
+        }
+
+        spin_lock(&dispc_compat.irq_lock);
+
+        unhandled_errors = irqstatus & ~handledirqs & dispc_compat.irq_error_mask;
+
+        if (unhandled_errors) {
+                dispc_compat.error_irqs |= unhandled_errors;
+
+                dispc_compat.irq_error_mask &= ~unhandled_errors;
+                _omap_dispc_set_irqs();
+
+                schedule_work(&dispc_compat.error_work);
+        }
+
+        spin_unlock(&dispc_compat.irq_lock);
+
+        return IRQ_HANDLED;
+}
+
+static void dispc_error_worker(struct work_struct *work)
+{
+        int i;
+        u32 errors;
+        unsigned long flags;
+        static const unsigned fifo_underflow_bits[] = {
+                DISPC_IRQ_GFX_FIFO_UNDERFLOW,
+                DISPC_IRQ_VID1_FIFO_UNDERFLOW,
+                DISPC_IRQ_VID2_FIFO_UNDERFLOW,
+                DISPC_IRQ_VID3_FIFO_UNDERFLOW,
+        };
+
+        spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+        errors = dispc_compat.error_irqs;
+        dispc_compat.error_irqs = 0;
+        spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+        dispc_runtime_get();
+
+        for (i = 0; i < omap_dss_get_num_overlays(); ++i) {
+                struct omap_overlay *ovl;
+                unsigned bit;
+
+                ovl = omap_dss_get_overlay(i);
+                bit = fifo_underflow_bits[i];
+
+                if (bit & errors) {
+                        DSSERR("FIFO UNDERFLOW on %s, disabling the overlay\n",
+                                        ovl->name);
+                        ovl->disable(ovl);
+                        msleep(50);
+                }
+        }
+
+        for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+                struct omap_overlay_manager *mgr;
+                unsigned bit;
+
+                mgr = omap_dss_get_overlay_manager(i);
+                bit = dispc_mgr_get_sync_lost_irq(i);
+
+                if (bit & errors) {
+                        int j;
+
+                        DSSERR("SYNC_LOST on channel %s, restarting the output "
+                                        "with video overlays disabled\n",
+                                        mgr->name);
+
+                        dss_mgr_disable(mgr);
+
+                        for (j = 0; j < omap_dss_get_num_overlays(); ++j) {
+                                struct omap_overlay *ovl;
+                                ovl = omap_dss_get_overlay(j);
+
+                                if (ovl->id != OMAP_DSS_GFX &&
+                                                ovl->manager == mgr)
+                                        ovl->disable(ovl);
+                        }
+
+                        dss_mgr_enable(mgr);
+                }
+        }
+
+        if (errors & DISPC_IRQ_OCP_ERR) {
+                DSSERR("OCP_ERR\n");
+                for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+                        struct omap_overlay_manager *mgr;
+
+                        mgr = omap_dss_get_overlay_manager(i);
+                        dss_mgr_disable(mgr);
+                }
+        }
+
+        spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+        dispc_compat.irq_error_mask |= errors;
+        _omap_dispc_set_irqs();
+        spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+        dispc_runtime_put();
+}
+
+int dss_dispc_initialize_irq(void)
+{
+        int r;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        spin_lock_init(&dispc_compat.irq_stats_lock);
+        dispc_compat.irq_stats.last_reset = jiffies;
+        dss_debugfs_create_file("dispc_irq", dispc_dump_irqs);
+#endif
+
+        spin_lock_init(&dispc_compat.irq_lock);
+
+        memset(dispc_compat.registered_isr, 0,
+                        sizeof(dispc_compat.registered_isr));
+
+        dispc_compat.irq_error_mask = DISPC_IRQ_MASK_ERROR;
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                dispc_compat.irq_error_mask |= DISPC_IRQ_SYNC_LOST2;
+        if (dss_has_feature(FEAT_MGR_LCD3))
+                dispc_compat.irq_error_mask |= DISPC_IRQ_SYNC_LOST3;
+        if (dss_feat_get_num_ovls() > 3)
+                dispc_compat.irq_error_mask |= DISPC_IRQ_VID3_FIFO_UNDERFLOW;
+
+        /*
+         * there's SYNC_LOST_DIGIT waiting after enabling the DSS,
+         * so clear it
+         */
+        dispc_clear_irqstatus(dispc_read_irqstatus());
+
+        INIT_WORK(&dispc_compat.error_work, dispc_error_worker);
+
+        _omap_dispc_set_irqs();
+
+        r = dispc_request_irq(omap_dispc_irq_handler, &dispc_compat);
+        if (r) {
+                DSSERR("dispc_request_irq failed\n");
+                return r;
+        }
+
+        return 0;
+}
+
+void dss_dispc_uninitialize_irq(void)
+{
+        dispc_free_irq(&dispc_compat);
+}
+
+static void dispc_mgr_disable_isr(void *data, u32 mask)
+{
+        struct completion *compl = data;
+        complete(compl);
+}
+
+static void dispc_mgr_enable_lcd_out(enum omap_channel channel)
+{
+        dispc_mgr_enable(channel, true);
+}
+
+static void dispc_mgr_disable_lcd_out(enum omap_channel channel)
+{
+        DECLARE_COMPLETION_ONSTACK(framedone_compl);
+        int r;
+        u32 irq;
+
+        if (!dispc_mgr_is_enabled(channel))
+                return;
+
+        /*
+         * When we disable LCD output, we need to wait for FRAMEDONE to know
+         * that DISPC has finished with the LCD output.
+         */
+
+        irq = dispc_mgr_get_framedone_irq(channel);
+
+        r = omap_dispc_register_isr(dispc_mgr_disable_isr, &framedone_compl,
+                        irq);
+        if (r)
+                DSSERR("failed to register FRAMEDONE isr\n");
+
+        dispc_mgr_enable(channel, false);
+
+        /* if we couldn't register for framedone, just sleep and exit */
+        if (r) {
+                msleep(100);
+                return;
+        }
+
+        if (!wait_for_completion_timeout(&framedone_compl,
+                                msecs_to_jiffies(100)))
+                DSSERR("timeout waiting for FRAME DONE\n");
+
+        r = omap_dispc_unregister_isr(dispc_mgr_disable_isr, &framedone_compl,
+                        irq);
+        if (r)
+                DSSERR("failed to unregister FRAMEDONE isr\n");
+}
+
+static void dispc_digit_out_enable_isr(void *data, u32 mask)
+{
+        struct completion *compl = data;
+
+        /* ignore any sync lost interrupts */
+        if (mask & (DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD))
+                complete(compl);
+}
+
+static void dispc_mgr_enable_digit_out(void)
+{
+        DECLARE_COMPLETION_ONSTACK(vsync_compl);
+        int r;
+        u32 irq_mask;
+
+        if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
+                return;
+
+        /*
+         * Digit output produces some sync lost interrupts during the first
+         * frame when enabling. Those need to be ignored, so we register for the
+         * sync lost irq to prevent the error handler from triggering.
+         */
+
+        irq_mask = dispc_mgr_get_vsync_irq(OMAP_DSS_CHANNEL_DIGIT) |
+                dispc_mgr_get_sync_lost_irq(OMAP_DSS_CHANNEL_DIGIT);
+
+        r = omap_dispc_register_isr(dispc_digit_out_enable_isr, &vsync_compl,
+                        irq_mask);
+        if (r) {
+                DSSERR("failed to register %x isr\n", irq_mask);
+                return;
+        }
+
+        dispc_mgr_enable(OMAP_DSS_CHANNEL_DIGIT, true);
+
+        /* wait for the first evsync */
+        if (!wait_for_completion_timeout(&vsync_compl, msecs_to_jiffies(100)))
+                DSSERR("timeout waiting for digit out to start\n");
+
+        r = omap_dispc_unregister_isr(dispc_digit_out_enable_isr, &vsync_compl,
+                        irq_mask);
+        if (r)
+                DSSERR("failed to unregister %x isr\n", irq_mask);
+}
+
+static void dispc_mgr_disable_digit_out(void)
+{
+        DECLARE_COMPLETION_ONSTACK(framedone_compl);
+        int r, i;
+        u32 irq_mask;
+        int num_irqs;
+
+        if (!dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
+                return;
+
+        /*
+         * When we disable the digit output, we need to wait for FRAMEDONE to
+         * know that DISPC has finished with the output.
+         */
+
+        irq_mask = dispc_mgr_get_framedone_irq(OMAP_DSS_CHANNEL_DIGIT);
+        num_irqs = 1;
+
+        if (!irq_mask) {
+                /*
+                 * omap 2/3 don't have framedone irq for TV, so we need to use
+                 * vsyncs for this.
+                 */
+
+                irq_mask = dispc_mgr_get_vsync_irq(OMAP_DSS_CHANNEL_DIGIT);
+                /*
+                 * We need to wait for both even and odd vsyncs. Note that this
+                 * is not totally reliable, as we could get a vsync interrupt
+                 * before we disable the output, which leads to timeout in the
+                 * wait_for_completion.
+                 */
+                num_irqs = 2;
+        }
+
+        r = omap_dispc_register_isr(dispc_mgr_disable_isr, &framedone_compl,
+                        irq_mask);
+        if (r)
+                DSSERR("failed to register %x isr\n", irq_mask);
+
+        dispc_mgr_enable(OMAP_DSS_CHANNEL_DIGIT, false);
+
+        /* if we couldn't register the irq, just sleep and exit */
+        if (r) {
+                msleep(100);
+                return;
+        }
+
+        for (i = 0; i < num_irqs; ++i) {
+                if (!wait_for_completion_timeout(&framedone_compl,
+                                        msecs_to_jiffies(100)))
+                        DSSERR("timeout waiting for digit out to stop\n");
+        }
+
+        r = omap_dispc_unregister_isr(dispc_mgr_disable_isr, &framedone_compl,
+                        irq_mask);
+        if (r)
+                DSSERR("failed to unregister %x isr\n", irq_mask);
+}
+
+void dispc_mgr_enable_sync(enum omap_channel channel)
+{
+        if (dss_mgr_is_lcd(channel))
+                dispc_mgr_enable_lcd_out(channel);
+        else if (channel == OMAP_DSS_CHANNEL_DIGIT)
+                dispc_mgr_enable_digit_out();
+        else
+                WARN_ON(1);
+}
+
+void dispc_mgr_disable_sync(enum omap_channel channel)
+{
+        if (dss_mgr_is_lcd(channel))
+                dispc_mgr_disable_lcd_out(channel);
+        else if (channel == OMAP_DSS_CHANNEL_DIGIT)
+                dispc_mgr_disable_digit_out();
+        else
+                WARN_ON(1);
+}
+
+static inline void dispc_irq_wait_handler(void *data, u32 mask)
+{
+        complete((struct completion *)data);
+}
+
+int omap_dispc_wait_for_irq_interruptible_timeout(u32 irqmask,
+                unsigned long timeout)
+{
+
+        int r;
+        DECLARE_COMPLETION_ONSTACK(completion);
+
+        r = omap_dispc_register_isr(dispc_irq_wait_handler, &completion,
+                        irqmask);
+
+        if (r)
+                return r;
+
+        timeout = wait_for_completion_interruptible_timeout(&completion,
+                        timeout);
+
+        omap_dispc_unregister_isr(dispc_irq_wait_handler, &completion, irqmask);
+
+        if (timeout == 0)
+                return -ETIMEDOUT;
+
+        if (timeout == -ERESTARTSYS)
+                return -ERESTARTSYS;
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.h b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.h
new file mode 100644
index 000000000000..14a69b3d4fb0
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2012 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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_DISPC_COMPAT_H
+#define __OMAP2_DSS_DISPC_COMPAT_H
+
+void dispc_mgr_enable_sync(enum omap_channel channel);
+void dispc_mgr_disable_sync(enum omap_channel channel);
+
+int omap_dispc_wait_for_irq_interruptible_timeout(u32 irqmask,
+                unsigned long timeout);
+
+int dss_dispc_initialize_irq(void);
+void dss_dispc_uninitialize_irq(void);
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc.c b/drivers/video/fbdev/omap2/omapfb/dss/dispc.c
new file mode 100644
index 000000000000..5491e304f4fe
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc.c
@@ -0,0 +1,4234 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "DISPC"
+
+#include <linux/kernel.h>
+#include <linux/dma-mapping.h>
+#include <linux/vmalloc.h>
+#include <linux/export.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/seq_file.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/hardirq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/sizes.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+#include "dispc.h"
+
+/* DISPC */
+#define DISPC_SZ_REGS                   SZ_4K
+
+enum omap_burst_size {
+        BURST_SIZE_X2 = 0,
+        BURST_SIZE_X4 = 1,
+        BURST_SIZE_X8 = 2,
+};
+
+#define REG_GET(idx, start, end) \
+        FLD_GET(dispc_read_reg(idx), start, end)
+
+#define REG_FLD_MOD(idx, val, start, end)                               \
+        dispc_write_reg(idx, FLD_MOD(dispc_read_reg(idx), val, start, end))
+
+struct dispc_features {
+        u8 sw_start;
+        u8 fp_start;
+        u8 bp_start;
+        u16 sw_max;
+        u16 vp_max;
+        u16 hp_max;
+        u8 mgr_width_start;
+        u8 mgr_height_start;
+        u16 mgr_width_max;
+        u16 mgr_height_max;
+        unsigned long max_lcd_pclk;
+        unsigned long max_tv_pclk;
+        int (*calc_scaling) (unsigned long pclk, unsigned long lclk,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem);
+        unsigned long (*calc_core_clk) (unsigned long pclk,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                bool mem_to_mem);
+        u8 num_fifos;
+
+        /* swap GFX & WB fifos */
+        bool gfx_fifo_workaround:1;
+
+        /* no DISPC_IRQ_FRAMEDONETV on this SoC */
+        bool no_framedone_tv:1;
+
+        /* revert to the OMAP4 mechanism of DISPC Smart Standby operation */
+        bool mstandby_workaround:1;
+
+        bool set_max_preload:1;
+
+        /* PIXEL_INC is not added to the last pixel of a line */
+        bool last_pixel_inc_missing:1;
+
+        /* POL_FREQ has ALIGN bit */
+        bool supports_sync_align:1;
+
+        bool has_writeback:1;
+};
+
+#define DISPC_MAX_NR_FIFOS 5
+
+static struct {
+        struct platform_device *pdev;
+        void __iomem    *base;
+
+        int irq;
+        irq_handler_t user_handler;
+        void *user_data;
+
+        unsigned long core_clk_rate;
+        unsigned long tv_pclk_rate;
+
+        u32 fifo_size[DISPC_MAX_NR_FIFOS];
+        /* maps which plane is using a fifo. fifo-id -> plane-id */
+        int fifo_assignment[DISPC_MAX_NR_FIFOS];
+
+        bool            ctx_valid;
+        u32             ctx[DISPC_SZ_REGS / sizeof(u32)];
+
+        const struct dispc_features *feat;
+
+        bool is_enabled;
+
+        struct regmap *syscon_pol;
+        u32 syscon_pol_offset;
+
+        /* DISPC_CONTROL & DISPC_CONFIG lock*/
+        spinlock_t control_lock;
+} 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,
+};
+
+enum mgr_reg_fields {
+        DISPC_MGR_FLD_ENABLE,
+        DISPC_MGR_FLD_STNTFT,
+        DISPC_MGR_FLD_GO,
+        DISPC_MGR_FLD_TFTDATALINES,
+        DISPC_MGR_FLD_STALLMODE,
+        DISPC_MGR_FLD_TCKENABLE,
+        DISPC_MGR_FLD_TCKSELECTION,
+        DISPC_MGR_FLD_CPR,
+        DISPC_MGR_FLD_FIFOHANDCHECK,
+        /* used to maintain a count of the above fields */
+        DISPC_MGR_FLD_NUM,
+};
+
+struct dispc_reg_field {
+        u16 reg;
+        u8 high;
+        u8 low;
+};
+
+static const struct {
+        const char *name;
+        u32 vsync_irq;
+        u32 framedone_irq;
+        u32 sync_lost_irq;
+        struct dispc_reg_field reg_desc[DISPC_MGR_FLD_NUM];
+} mgr_desc[] = {
+        [OMAP_DSS_CHANNEL_LCD] = {
+                .name           = "LCD",
+                .vsync_irq      = DISPC_IRQ_VSYNC,
+                .framedone_irq  = DISPC_IRQ_FRAMEDONE,
+                .sync_lost_irq  = DISPC_IRQ_SYNC_LOST,
+                .reg_desc       = {
+                        [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL,  0,  0 },
+                        [DISPC_MGR_FLD_STNTFT]          = { DISPC_CONTROL,  3,  3 },
+                        [DISPC_MGR_FLD_GO]              = { DISPC_CONTROL,  5,  5 },
+                        [DISPC_MGR_FLD_TFTDATALINES]    = { DISPC_CONTROL,  9,  8 },
+                        [DISPC_MGR_FLD_STALLMODE]       = { DISPC_CONTROL, 11, 11 },
+                        [DISPC_MGR_FLD_TCKENABLE]       = { DISPC_CONFIG,  10, 10 },
+                        [DISPC_MGR_FLD_TCKSELECTION]    = { DISPC_CONFIG,  11, 11 },
+                        [DISPC_MGR_FLD_CPR]             = { DISPC_CONFIG,  15, 15 },
+                        [DISPC_MGR_FLD_FIFOHANDCHECK]   = { DISPC_CONFIG,  16, 16 },
+                },
+        },
+        [OMAP_DSS_CHANNEL_DIGIT] = {
+                .name           = "DIGIT",
+                .vsync_irq      = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN,
+                .framedone_irq  = DISPC_IRQ_FRAMEDONETV,
+                .sync_lost_irq  = DISPC_IRQ_SYNC_LOST_DIGIT,
+                .reg_desc       = {
+                        [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL,  1,  1 },
+                        [DISPC_MGR_FLD_STNTFT]          = { },
+                        [DISPC_MGR_FLD_GO]              = { DISPC_CONTROL,  6,  6 },
+                        [DISPC_MGR_FLD_TFTDATALINES]    = { },
+                        [DISPC_MGR_FLD_STALLMODE]       = { },
+                        [DISPC_MGR_FLD_TCKENABLE]       = { DISPC_CONFIG,  12, 12 },
+                        [DISPC_MGR_FLD_TCKSELECTION]    = { DISPC_CONFIG,  13, 13 },
+                        [DISPC_MGR_FLD_CPR]             = { },
+                        [DISPC_MGR_FLD_FIFOHANDCHECK]   = { DISPC_CONFIG,  16, 16 },
+                },
+        },
+        [OMAP_DSS_CHANNEL_LCD2] = {
+                .name           = "LCD2",
+                .vsync_irq      = DISPC_IRQ_VSYNC2,
+                .framedone_irq  = DISPC_IRQ_FRAMEDONE2,
+                .sync_lost_irq  = DISPC_IRQ_SYNC_LOST2,
+                .reg_desc       = {
+                        [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL2,  0,  0 },
+                        [DISPC_MGR_FLD_STNTFT]          = { DISPC_CONTROL2,  3,  3 },
+                        [DISPC_MGR_FLD_GO]              = { DISPC_CONTROL2,  5,  5 },
+                        [DISPC_MGR_FLD_TFTDATALINES]    = { DISPC_CONTROL2,  9,  8 },
+                        [DISPC_MGR_FLD_STALLMODE]       = { DISPC_CONTROL2, 11, 11 },
+                        [DISPC_MGR_FLD_TCKENABLE]       = { DISPC_CONFIG2,  10, 10 },
+                        [DISPC_MGR_FLD_TCKSELECTION]    = { DISPC_CONFIG2,  11, 11 },
+                        [DISPC_MGR_FLD_CPR]             = { DISPC_CONFIG2,  15, 15 },
+                        [DISPC_MGR_FLD_FIFOHANDCHECK]   = { DISPC_CONFIG2,  16, 16 },
+                },
+        },
+        [OMAP_DSS_CHANNEL_LCD3] = {
+                .name           = "LCD3",
+                .vsync_irq      = DISPC_IRQ_VSYNC3,
+                .framedone_irq  = DISPC_IRQ_FRAMEDONE3,
+                .sync_lost_irq  = DISPC_IRQ_SYNC_LOST3,
+                .reg_desc       = {
+                        [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL3,  0,  0 },
+                        [DISPC_MGR_FLD_STNTFT]          = { DISPC_CONTROL3,  3,  3 },
+                        [DISPC_MGR_FLD_GO]              = { DISPC_CONTROL3,  5,  5 },
+                        [DISPC_MGR_FLD_TFTDATALINES]    = { DISPC_CONTROL3,  9,  8 },
+                        [DISPC_MGR_FLD_STALLMODE]       = { DISPC_CONTROL3, 11, 11 },
+                        [DISPC_MGR_FLD_TCKENABLE]       = { DISPC_CONFIG3,  10, 10 },
+                        [DISPC_MGR_FLD_TCKSELECTION]    = { DISPC_CONFIG3,  11, 11 },
+                        [DISPC_MGR_FLD_CPR]             = { DISPC_CONFIG3,  15, 15 },
+                        [DISPC_MGR_FLD_FIFOHANDCHECK]   = { DISPC_CONFIG3,  16, 16 },
+                },
+        },
+};
+
+struct color_conv_coef {
+        int ry, rcr, rcb, gy, gcr, gcb, by, bcr, bcb;
+        int full_range;
+};
+
+static unsigned long dispc_fclk_rate(void);
+static unsigned long dispc_core_clk_rate(void);
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
+
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane);
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane);
+
+static inline void dispc_write_reg(const u16 idx, u32 val)
+{
+        __raw_writel(val, dispc.base + idx);
+}
+
+static inline u32 dispc_read_reg(const u16 idx)
+{
+        return __raw_readl(dispc.base + idx);
+}
+
+static u32 mgr_fld_read(enum omap_channel channel, enum mgr_reg_fields regfld)
+{
+        const struct dispc_reg_field rfld = mgr_desc[channel].reg_desc[regfld];
+        return REG_GET(rfld.reg, rfld.high, rfld.low);
+}
+
+static void mgr_fld_write(enum omap_channel channel,
+                                        enum mgr_reg_fields regfld, int val) {
+        const struct dispc_reg_field rfld = mgr_desc[channel].reg_desc[regfld];
+        const bool need_lock = rfld.reg == DISPC_CONTROL || rfld.reg == DISPC_CONFIG;
+        unsigned long flags;
+
+        if (need_lock)
+                spin_lock_irqsave(&dispc.control_lock, flags);
+
+        REG_FLD_MOD(rfld.reg, val, rfld.high, rfld.low);
+
+        if (need_lock)
+                spin_unlock_irqrestore(&dispc.control_lock, flags);
+}
+
+#define SR(reg) \
+        dispc.ctx[DISPC_##reg / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
+#define RR(reg) \
+        dispc_write_reg(DISPC_##reg, dispc.ctx[DISPC_##reg / sizeof(u32)])
+
+static void dispc_save_context(void)
+{
+        int i, j;
+
+        DSSDBG("dispc_save_context\n");
+
+        SR(IRQENABLE);
+        SR(CONTROL);
+        SR(CONFIG);
+        SR(LINE_NUMBER);
+        if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER) ||
+                        dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+                SR(GLOBAL_ALPHA);
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                SR(CONTROL2);
+                SR(CONFIG2);
+        }
+        if (dss_has_feature(FEAT_MGR_LCD3)) {
+                SR(CONTROL3);
+                SR(CONFIG3);
+        }
+
+        for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
+                SR(DEFAULT_COLOR(i));
+                SR(TRANS_COLOR(i));
+                SR(SIZE_MGR(i));
+                if (i == OMAP_DSS_CHANNEL_DIGIT)
+                        continue;
+                SR(TIMING_H(i));
+                SR(TIMING_V(i));
+                SR(POL_FREQ(i));
+                SR(DIVISORo(i));
+
+                SR(DATA_CYCLE1(i));
+                SR(DATA_CYCLE2(i));
+                SR(DATA_CYCLE3(i));
+
+                if (dss_has_feature(FEAT_CPR)) {
+                        SR(CPR_COEF_R(i));
+                        SR(CPR_COEF_G(i));
+                        SR(CPR_COEF_B(i));
+                }
+        }
+
+        for (i = 0; i < dss_feat_get_num_ovls(); i++) {
+                SR(OVL_BA0(i));
+                SR(OVL_BA1(i));
+                SR(OVL_POSITION(i));
+                SR(OVL_SIZE(i));
+                SR(OVL_ATTRIBUTES(i));
+                SR(OVL_FIFO_THRESHOLD(i));
+                SR(OVL_ROW_INC(i));
+                SR(OVL_PIXEL_INC(i));
+                if (dss_has_feature(FEAT_PRELOAD))
+                        SR(OVL_PRELOAD(i));
+                if (i == OMAP_DSS_GFX) {
+                        SR(OVL_WINDOW_SKIP(i));
+                        SR(OVL_TABLE_BA(i));
+                        continue;
+                }
+                SR(OVL_FIR(i));
+                SR(OVL_PICTURE_SIZE(i));
+                SR(OVL_ACCU0(i));
+                SR(OVL_ACCU1(i));
+
+                for (j = 0; j < 8; j++)
+                        SR(OVL_FIR_COEF_H(i, j));
+
+                for (j = 0; j < 8; j++)
+                        SR(OVL_FIR_COEF_HV(i, j));
+
+                for (j = 0; j < 5; j++)
+                        SR(OVL_CONV_COEF(i, j));
+
+                if (dss_has_feature(FEAT_FIR_COEF_V)) {
+                        for (j = 0; j < 8; j++)
+                                SR(OVL_FIR_COEF_V(i, j));
+                }
+
+                if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                        SR(OVL_BA0_UV(i));
+                        SR(OVL_BA1_UV(i));
+                        SR(OVL_FIR2(i));
+                        SR(OVL_ACCU2_0(i));
+                        SR(OVL_ACCU2_1(i));
+
+                        for (j = 0; j < 8; j++)
+                                SR(OVL_FIR_COEF_H2(i, j));
+
+                        for (j = 0; j < 8; j++)
+                                SR(OVL_FIR_COEF_HV2(i, j));
+
+                        for (j = 0; j < 8; j++)
+                                SR(OVL_FIR_COEF_V2(i, j));
+                }
+                if (dss_has_feature(FEAT_ATTR2))
+                        SR(OVL_ATTRIBUTES2(i));
+        }
+
+        if (dss_has_feature(FEAT_CORE_CLK_DIV))
+                SR(DIVISOR);
+
+        dispc.ctx_valid = true;
+
+        DSSDBG("context saved\n");
+}
+
+static void dispc_restore_context(void)
+{
+        int i, j;
+
+        DSSDBG("dispc_restore_context\n");
+
+        if (!dispc.ctx_valid)
+                return;
+
+        /*RR(IRQENABLE);*/
+        /*RR(CONTROL);*/
+        RR(CONFIG);
+        RR(LINE_NUMBER);
+        if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER) ||
+                        dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+                RR(GLOBAL_ALPHA);
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                RR(CONFIG2);
+        if (dss_has_feature(FEAT_MGR_LCD3))
+                RR(CONFIG3);
+
+        for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
+                RR(DEFAULT_COLOR(i));
+                RR(TRANS_COLOR(i));
+                RR(SIZE_MGR(i));
+                if (i == OMAP_DSS_CHANNEL_DIGIT)
+                        continue;
+                RR(TIMING_H(i));
+                RR(TIMING_V(i));
+                RR(POL_FREQ(i));
+                RR(DIVISORo(i));
+
+                RR(DATA_CYCLE1(i));
+                RR(DATA_CYCLE2(i));
+                RR(DATA_CYCLE3(i));
+
+                if (dss_has_feature(FEAT_CPR)) {
+                        RR(CPR_COEF_R(i));
+                        RR(CPR_COEF_G(i));
+                        RR(CPR_COEF_B(i));
+                }
+        }
+
+        for (i = 0; i < dss_feat_get_num_ovls(); i++) {
+                RR(OVL_BA0(i));
+                RR(OVL_BA1(i));
+                RR(OVL_POSITION(i));
+                RR(OVL_SIZE(i));
+                RR(OVL_ATTRIBUTES(i));
+                RR(OVL_FIFO_THRESHOLD(i));
+                RR(OVL_ROW_INC(i));
+                RR(OVL_PIXEL_INC(i));
+                if (dss_has_feature(FEAT_PRELOAD))
+                        RR(OVL_PRELOAD(i));
+                if (i == OMAP_DSS_GFX) {
+                        RR(OVL_WINDOW_SKIP(i));
+                        RR(OVL_TABLE_BA(i));
+                        continue;
+                }
+                RR(OVL_FIR(i));
+                RR(OVL_PICTURE_SIZE(i));
+                RR(OVL_ACCU0(i));
+                RR(OVL_ACCU1(i));
+
+                for (j = 0; j < 8; j++)
+                        RR(OVL_FIR_COEF_H(i, j));
+
+                for (j = 0; j < 8; j++)
+                        RR(OVL_FIR_COEF_HV(i, j));
+
+                for (j = 0; j < 5; j++)
+                        RR(OVL_CONV_COEF(i, j));
+
+                if (dss_has_feature(FEAT_FIR_COEF_V)) {
+                        for (j = 0; j < 8; j++)
+                                RR(OVL_FIR_COEF_V(i, j));
+                }
+
+                if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                        RR(OVL_BA0_UV(i));
+                        RR(OVL_BA1_UV(i));
+                        RR(OVL_FIR2(i));
+                        RR(OVL_ACCU2_0(i));
+                        RR(OVL_ACCU2_1(i));
+
+                        for (j = 0; j < 8; j++)
+                                RR(OVL_FIR_COEF_H2(i, j));
+
+                        for (j = 0; j < 8; j++)
+                                RR(OVL_FIR_COEF_HV2(i, j));
+
+                        for (j = 0; j < 8; j++)
+                                RR(OVL_FIR_COEF_V2(i, j));
+                }
+                if (dss_has_feature(FEAT_ATTR2))
+                        RR(OVL_ATTRIBUTES2(i));
+        }
+
+        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);
+        if (dss_has_feature(FEAT_MGR_LCD3))
+                RR(CONTROL3);
+        /* clear spurious SYNC_LOST_DIGIT interrupts */
+        dispc_clear_irqstatus(DISPC_IRQ_SYNC_LOST_DIGIT);
+
+        /*
+         * enable last so IRQs won't trigger before
+         * the context is fully restored
+         */
+        RR(IRQENABLE);
+
+        DSSDBG("context restored\n");
+}
+
+#undef SR
+#undef RR
+
+int dispc_runtime_get(void)
+{
+        int r;
+
+        DSSDBG("dispc_runtime_get\n");
+
+        r = pm_runtime_get_sync(&dispc.pdev->dev);
+        WARN_ON(r < 0);
+        return r < 0 ? r : 0;
+}
+EXPORT_SYMBOL(dispc_runtime_get);
+
+void dispc_runtime_put(void)
+{
+        int r;
+
+        DSSDBG("dispc_runtime_put\n");
+
+        r = pm_runtime_put_sync(&dispc.pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS);
+}
+EXPORT_SYMBOL(dispc_runtime_put);
+
+u32 dispc_mgr_get_vsync_irq(enum omap_channel channel)
+{
+        return mgr_desc[channel].vsync_irq;
+}
+EXPORT_SYMBOL(dispc_mgr_get_vsync_irq);
+
+u32 dispc_mgr_get_framedone_irq(enum omap_channel channel)
+{
+        if (channel == OMAP_DSS_CHANNEL_DIGIT && dispc.feat->no_framedone_tv)
+                return 0;
+
+        return mgr_desc[channel].framedone_irq;
+}
+EXPORT_SYMBOL(dispc_mgr_get_framedone_irq);
+
+u32 dispc_mgr_get_sync_lost_irq(enum omap_channel channel)
+{
+        return mgr_desc[channel].sync_lost_irq;
+}
+EXPORT_SYMBOL(dispc_mgr_get_sync_lost_irq);
+
+u32 dispc_wb_get_framedone_irq(void)
+{
+        return DISPC_IRQ_FRAMEDONEWB;
+}
+
+bool dispc_mgr_go_busy(enum omap_channel channel)
+{
+        return mgr_fld_read(channel, DISPC_MGR_FLD_GO) == 1;
+}
+EXPORT_SYMBOL(dispc_mgr_go_busy);
+
+void dispc_mgr_go(enum omap_channel channel)
+{
+        WARN_ON(!dispc_mgr_is_enabled(channel));
+        WARN_ON(dispc_mgr_go_busy(channel));
+
+        DSSDBG("GO %s\n", mgr_desc[channel].name);
+
+        mgr_fld_write(channel, DISPC_MGR_FLD_GO, 1);
+}
+EXPORT_SYMBOL(dispc_mgr_go);
+
+bool dispc_wb_go_busy(void)
+{
+        return REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+}
+
+void dispc_wb_go(void)
+{
+        enum omap_plane plane = OMAP_DSS_WB;
+        bool enable, go;
+
+        enable = REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0) == 1;
+
+        if (!enable)
+                return;
+
+        go = REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+        if (go) {
+                DSSERR("GO bit not down for WB\n");
+                return;
+        }
+
+        REG_FLD_MOD(DISPC_CONTROL2, 1, 6, 6);
+}
+
+static void dispc_ovl_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_ovl_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_ovl_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_ovl_write_firh2_reg(enum omap_plane plane, int reg, u32 value)
+{
+        BUG_ON(plane == OMAP_DSS_GFX);
+
+        dispc_write_reg(DISPC_OVL_FIR_COEF_H2(plane, reg), value);
+}
+
+static void dispc_ovl_write_firhv2_reg(enum omap_plane plane, int reg,
+                u32 value)
+{
+        BUG_ON(plane == OMAP_DSS_GFX);
+
+        dispc_write_reg(DISPC_OVL_FIR_COEF_HV2(plane, reg), value);
+}
+
+static void dispc_ovl_write_firv2_reg(enum omap_plane plane, int reg, u32 value)
+{
+        BUG_ON(plane == OMAP_DSS_GFX);
+
+        dispc_write_reg(DISPC_OVL_FIR_COEF_V2(plane, reg), value);
+}
+
+static void dispc_ovl_set_scale_coef(enum omap_plane plane, int fir_hinc,
+                                int fir_vinc, int five_taps,
+                                enum omap_color_component color_comp)
+{
+        const struct dispc_coef *h_coef, *v_coef;
+        int i;
+
+        h_coef = dispc_ovl_get_scale_coef(fir_hinc, true);
+        v_coef = dispc_ovl_get_scale_coef(fir_vinc, five_taps);
+
+        for (i = 0; i < 8; i++) {
+                u32 h, hv;
+
+                h = FLD_VAL(h_coef[i].hc0_vc00, 7, 0)
+                        | FLD_VAL(h_coef[i].hc1_vc0, 15, 8)
+                        | FLD_VAL(h_coef[i].hc2_vc1, 23, 16)
+                        | FLD_VAL(h_coef[i].hc3_vc2, 31, 24);
+                hv = FLD_VAL(h_coef[i].hc4_vc22, 7, 0)
+                        | FLD_VAL(v_coef[i].hc1_vc0, 15, 8)
+                        | FLD_VAL(v_coef[i].hc2_vc1, 23, 16)
+                        | FLD_VAL(v_coef[i].hc3_vc2, 31, 24);
+
+                if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+                        dispc_ovl_write_firh_reg(plane, i, h);
+                        dispc_ovl_write_firhv_reg(plane, i, hv);
+                } else {
+                        dispc_ovl_write_firh2_reg(plane, i, h);
+                        dispc_ovl_write_firhv2_reg(plane, i, hv);
+                }
+
+        }
+
+        if (five_taps) {
+                for (i = 0; i < 8; i++) {
+                        u32 v;
+                        v = FLD_VAL(v_coef[i].hc0_vc00, 7, 0)
+                                | FLD_VAL(v_coef[i].hc4_vc22, 15, 8);
+                        if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y)
+                                dispc_ovl_write_firv_reg(plane, i, v);
+                        else
+                                dispc_ovl_write_firv2_reg(plane, i, v);
+                }
+        }
+}
+
+
+static void dispc_ovl_write_color_conv_coef(enum omap_plane plane,
+                const struct color_conv_coef *ct)
+{
+#define CVAL(x, y) (FLD_VAL(x, 26, 16) | FLD_VAL(y, 10, 0))
+
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 0), CVAL(ct->rcr, ct->ry));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 1), CVAL(ct->gy,  ct->rcb));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 2), CVAL(ct->gcb, ct->gcr));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 3), CVAL(ct->bcr, ct->by));
+        dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 4), CVAL(0, ct->bcb));
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), ct->full_range, 11, 11);
+
+#undef CVAL
+}
+
+static void dispc_setup_color_conv_coef(void)
+{
+        int i;
+        int num_ovl = dss_feat_get_num_ovls();
+        const struct color_conv_coef ctbl_bt601_5_ovl = {
+                /* YUV -> RGB */
+                298, 409, 0, 298, -208, -100, 298, 0, 517, 0,
+        };
+        const struct color_conv_coef ctbl_bt601_5_wb = {
+                /* RGB -> YUV */
+                66, 129, 25, 112, -94, -18, -38, -74, 112, 0,
+        };
+
+        for (i = 1; i < num_ovl; i++)
+                dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_ovl);
+
+        if (dispc.feat->has_writeback)
+                dispc_ovl_write_color_conv_coef(OMAP_DSS_WB, &ctbl_bt601_5_wb);
+}
+
+static void dispc_ovl_set_ba0(enum omap_plane plane, u32 paddr)
+{
+        dispc_write_reg(DISPC_OVL_BA0(plane), paddr);
+}
+
+static void dispc_ovl_set_ba1(enum omap_plane plane, u32 paddr)
+{
+        dispc_write_reg(DISPC_OVL_BA1(plane), paddr);
+}
+
+static void dispc_ovl_set_ba0_uv(enum omap_plane plane, u32 paddr)
+{
+        dispc_write_reg(DISPC_OVL_BA0_UV(plane), paddr);
+}
+
+static void dispc_ovl_set_ba1_uv(enum omap_plane plane, u32 paddr)
+{
+        dispc_write_reg(DISPC_OVL_BA1_UV(plane), paddr);
+}
+
+static void dispc_ovl_set_pos(enum omap_plane plane,
+                enum omap_overlay_caps caps, int x, int y)
+{
+        u32 val;
+
+        if ((caps & OMAP_DSS_OVL_CAP_POS) == 0)
+                return;
+
+        val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
+
+        dispc_write_reg(DISPC_OVL_POSITION(plane), val);
+}
+
+static void dispc_ovl_set_input_size(enum omap_plane plane, int width,
+                int height)
+{
+        u32 val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
+
+        if (plane == OMAP_DSS_GFX || plane == OMAP_DSS_WB)
+                dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+        else
+                dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
+}
+
+static void dispc_ovl_set_output_size(enum omap_plane plane, int width,
+                int height)
+{
+        u32 val;
+
+        BUG_ON(plane == OMAP_DSS_GFX);
+
+        val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
+
+        if (plane == OMAP_DSS_WB)
+                dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
+        else
+                dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+}
+
+static void dispc_ovl_set_zorder(enum omap_plane plane,
+                enum omap_overlay_caps caps, u8 zorder)
+{
+        if ((caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+                return;
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), zorder, 27, 26);
+}
+
+static void dispc_ovl_enable_zorder_planes(void)
+{
+        int i;
+
+        if (!dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+                return;
+
+        for (i = 0; i < dss_feat_get_num_ovls(); i++)
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(i), 1, 25, 25);
+}
+
+static void dispc_ovl_set_pre_mult_alpha(enum omap_plane plane,
+                enum omap_overlay_caps caps, bool enable)
+{
+        if ((caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+                return;
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 28, 28);
+}
+
+static void dispc_ovl_setup_global_alpha(enum omap_plane plane,
+                enum omap_overlay_caps caps, u8 global_alpha)
+{
+        static const unsigned shifts[] = { 0, 8, 16, 24, };
+        int shift;
+
+        if ((caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+                return;
+
+        shift = shifts[plane];
+        REG_FLD_MOD(DISPC_GLOBAL_ALPHA, global_alpha, shift + 7, shift);
+}
+
+static void dispc_ovl_set_pix_inc(enum omap_plane plane, s32 inc)
+{
+        dispc_write_reg(DISPC_OVL_PIXEL_INC(plane), inc);
+}
+
+static void dispc_ovl_set_row_inc(enum omap_plane plane, s32 inc)
+{
+        dispc_write_reg(DISPC_OVL_ROW_INC(plane), inc);
+}
+
+static void dispc_ovl_set_color_mode(enum omap_plane plane,
+                enum omap_color_mode color_mode)
+{
+        u32 m = 0;
+        if (plane != OMAP_DSS_GFX) {
+                switch (color_mode) {
+                case OMAP_DSS_COLOR_NV12:
+                        m = 0x0; break;
+                case OMAP_DSS_COLOR_RGBX16:
+                        m = 0x1; break;
+                case OMAP_DSS_COLOR_RGBA16:
+                        m = 0x2; 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(); return;
+                }
+        } 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_RGBX16:
+                        m = 0xa; break;
+                case OMAP_DSS_COLOR_RGBA16:
+                        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(); return;
+                }
+        }
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), m, 4, 1);
+}
+
+static void dispc_ovl_configure_burst_type(enum omap_plane plane,
+                enum omap_dss_rotation_type rotation_type)
+{
+        if (dss_has_feature(FEAT_BURST_2D) == 0)
+                return;
+
+        if (rotation_type == OMAP_DSS_ROT_TILER)
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), 1, 29, 29);
+        else
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), 0, 29, 29);
+}
+
+void dispc_ovl_set_channel_out(enum omap_plane plane, enum omap_channel channel)
+{
+        int shift;
+        u32 val;
+        int chan = 0, chan2 = 0;
+
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                shift = 8;
+                break;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+        case OMAP_DSS_VIDEO3:
+                shift = 16;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        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;
+                case OMAP_DSS_CHANNEL_LCD3:
+                        if (dss_has_feature(FEAT_MGR_LCD3)) {
+                                chan = 0;
+                                chan2 = 2;
+                        } else {
+                                BUG();
+                                return;
+                        }
+                        break;
+                case OMAP_DSS_CHANNEL_WB:
+                        chan = 0;
+                        chan2 = 3;
+                        break;
+                default:
+                        BUG();
+                        return;
+                }
+
+                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);
+}
+EXPORT_SYMBOL(dispc_ovl_set_channel_out);
+
+static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
+{
+        int shift;
+        u32 val;
+
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                shift = 8;
+                break;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+        case OMAP_DSS_VIDEO3:
+                shift = 16;
+                break;
+        default:
+                BUG();
+                return 0;
+        }
+
+        val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+
+        if (FLD_GET(val, shift, shift) == 1)
+                return OMAP_DSS_CHANNEL_DIGIT;
+
+        if (!dss_has_feature(FEAT_MGR_LCD2))
+                return OMAP_DSS_CHANNEL_LCD;
+
+        switch (FLD_GET(val, 31, 30)) {
+        case 0:
+        default:
+                return OMAP_DSS_CHANNEL_LCD;
+        case 1:
+                return OMAP_DSS_CHANNEL_LCD2;
+        case 2:
+                return OMAP_DSS_CHANNEL_LCD3;
+        case 3:
+                return OMAP_DSS_CHANNEL_WB;
+        }
+}
+
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel)
+{
+        enum omap_plane plane = OMAP_DSS_WB;
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), channel, 18, 16);
+}
+
+static void dispc_ovl_set_burst_size(enum omap_plane plane,
+                enum omap_burst_size burst_size)
+{
+        static const unsigned shifts[] = { 6, 14, 14, 14, 14, };
+        int shift;
+
+        shift = shifts[plane];
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), burst_size, shift + 1, shift);
+}
+
+static void dispc_configure_burst_sizes(void)
+{
+        int i;
+        const int burst_size = BURST_SIZE_X8;
+
+        /* Configure burst size always to maximum size */
+        for (i = 0; i < dss_feat_get_num_ovls(); ++i)
+                dispc_ovl_set_burst_size(i, burst_size);
+        if (dispc.feat->has_writeback)
+                dispc_ovl_set_burst_size(OMAP_DSS_WB, burst_size);
+}
+
+static u32 dispc_ovl_get_burst_size(enum omap_plane plane)
+{
+        unsigned unit = dss_feat_get_burst_size_unit();
+        /* burst multiplier is always x8 (see dispc_configure_burst_sizes()) */
+        return unit * 8;
+}
+
+void dispc_enable_gamma_table(bool enable)
+{
+        /*
+         * This is partially implemented to support only disabling of
+         * the gamma table.
+         */
+        if (enable) {
+                DSSWARN("Gamma table enabling for TV not yet supported");
+                return;
+        }
+
+        REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
+}
+
+static void dispc_mgr_enable_cpr(enum omap_channel channel, bool enable)
+{
+        if (channel == OMAP_DSS_CHANNEL_DIGIT)
+                return;
+
+        mgr_fld_write(channel, DISPC_MGR_FLD_CPR, enable);
+}
+
+static void dispc_mgr_set_cpr_coef(enum omap_channel channel,
+                const struct omap_dss_cpr_coefs *coefs)
+{
+        u32 coef_r, coef_g, coef_b;
+
+        if (!dss_mgr_is_lcd(channel))
+                return;
+
+        coef_r = FLD_VAL(coefs->rr, 31, 22) | FLD_VAL(coefs->rg, 20, 11) |
+                FLD_VAL(coefs->rb, 9, 0);
+        coef_g = FLD_VAL(coefs->gr, 31, 22) | FLD_VAL(coefs->gg, 20, 11) |
+                FLD_VAL(coefs->gb, 9, 0);
+        coef_b = FLD_VAL(coefs->br, 31, 22) | FLD_VAL(coefs->bg, 20, 11) |
+                FLD_VAL(coefs->bb, 9, 0);
+
+        dispc_write_reg(DISPC_CPR_COEF_R(channel), coef_r);
+        dispc_write_reg(DISPC_CPR_COEF_G(channel), coef_g);
+        dispc_write_reg(DISPC_CPR_COEF_B(channel), coef_b);
+}
+
+static void dispc_ovl_set_vid_color_conv(enum omap_plane plane, bool enable)
+{
+        u32 val;
+
+        BUG_ON(plane == OMAP_DSS_GFX);
+
+        val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+        val = FLD_MOD(val, enable, 9, 9);
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
+}
+
+static void dispc_ovl_enable_replication(enum omap_plane plane,
+                enum omap_overlay_caps caps, bool enable)
+{
+        static const unsigned shifts[] = { 5, 10, 10, 10 };
+        int shift;
+
+        if ((caps & OMAP_DSS_OVL_CAP_REPLICATION) == 0)
+                return;
+
+        shift = shifts[plane];
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, shift, shift);
+}
+
+static void dispc_mgr_set_size(enum omap_channel channel, u16 width,
+                u16 height)
+{
+        u32 val;
+
+        val = FLD_VAL(height - 1, dispc.feat->mgr_height_start, 16) |
+                FLD_VAL(width - 1, dispc.feat->mgr_width_start, 0);
+
+        dispc_write_reg(DISPC_SIZE_MGR(channel), val);
+}
+
+static void dispc_init_fifos(void)
+{
+        u32 size;
+        int fifo;
+        u8 start, end;
+        u32 unit;
+        int i;
+
+        unit = dss_feat_get_buffer_size_unit();
+
+        dss_feat_get_reg_field(FEAT_REG_FIFOSIZE, &start, &end);
+
+        for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+                size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(fifo), start, end);
+                size *= unit;
+                dispc.fifo_size[fifo] = size;
+
+                /*
+                 * By default fifos are mapped directly to overlays, fifo 0 to
+                 * ovl 0, fifo 1 to ovl 1, etc.
+                 */
+                dispc.fifo_assignment[fifo] = fifo;
+        }
+
+        /*
+         * The GFX fifo on OMAP4 is smaller than the other fifos. The small fifo
+         * causes problems with certain use cases, like using the tiler in 2D
+         * mode. The below hack swaps the fifos of GFX and WB planes, thus
+         * giving GFX plane a larger fifo. WB but should work fine with a
+         * smaller fifo.
+         */
+        if (dispc.feat->gfx_fifo_workaround) {
+                u32 v;
+
+                v = dispc_read_reg(DISPC_GLOBAL_BUFFER);
+
+                v = FLD_MOD(v, 4, 2, 0); /* GFX BUF top to WB */
+                v = FLD_MOD(v, 4, 5, 3); /* GFX BUF bottom to WB */
+                v = FLD_MOD(v, 0, 26, 24); /* WB BUF top to GFX */
+                v = FLD_MOD(v, 0, 29, 27); /* WB BUF bottom to GFX */
+
+                dispc_write_reg(DISPC_GLOBAL_BUFFER, v);
+
+                dispc.fifo_assignment[OMAP_DSS_GFX] = OMAP_DSS_WB;
+                dispc.fifo_assignment[OMAP_DSS_WB] = OMAP_DSS_GFX;
+        }
+
+        /*
+         * Setup default fifo thresholds.
+         */
+        for (i = 0; i < dss_feat_get_num_ovls(); ++i) {
+                u32 low, high;
+                const bool use_fifomerge = false;
+                const bool manual_update = false;
+
+                dispc_ovl_compute_fifo_thresholds(i, &low, &high,
+                        use_fifomerge, manual_update);
+
+                dispc_ovl_set_fifo_threshold(i, low, high);
+        }
+
+        if (dispc.feat->has_writeback) {
+                u32 low, high;
+                const bool use_fifomerge = false;
+                const bool manual_update = false;
+
+                dispc_ovl_compute_fifo_thresholds(OMAP_DSS_WB, &low, &high,
+                        use_fifomerge, manual_update);
+
+                dispc_ovl_set_fifo_threshold(OMAP_DSS_WB, low, high);
+        }
+}
+
+static u32 dispc_ovl_get_fifo_size(enum omap_plane plane)
+{
+        int fifo;
+        u32 size = 0;
+
+        for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+                if (dispc.fifo_assignment[fifo] == plane)
+                        size += dispc.fifo_size[fifo];
+        }
+
+        return size;
+}
+
+void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
+{
+        u8 hi_start, hi_end, lo_start, lo_end;
+        u32 unit;
+
+        unit = dss_feat_get_buffer_size_unit();
+
+        WARN_ON(low % unit != 0);
+        WARN_ON(high % unit != 0);
+
+        low /= unit;
+        high /= unit;
+
+        dss_feat_get_reg_field(FEAT_REG_FIFOHIGHTHRESHOLD, &hi_start, &hi_end);
+        dss_feat_get_reg_field(FEAT_REG_FIFOLOWTHRESHOLD, &lo_start, &lo_end);
+
+        DSSDBG("fifo(%d) threshold (bytes), old %u/%u, new %u/%u\n",
+                        plane,
+                        REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+                                lo_start, lo_end) * unit,
+                        REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+                                hi_start, hi_end) * unit,
+                        low * unit, high * unit);
+
+        dispc_write_reg(DISPC_OVL_FIFO_THRESHOLD(plane),
+                        FLD_VAL(high, hi_start, hi_end) |
+                        FLD_VAL(low, lo_start, lo_end));
+
+        /*
+         * configure the preload to the pipeline's high threhold, if HT it's too
+         * large for the preload field, set the threshold to the maximum value
+         * that can be held by the preload register
+         */
+        if (dss_has_feature(FEAT_PRELOAD) && dispc.feat->set_max_preload &&
+                        plane != OMAP_DSS_WB)
+                dispc_write_reg(DISPC_OVL_PRELOAD(plane), min(high, 0xfffu));
+}
+
+void dispc_enable_fifomerge(bool enable)
+{
+        if (!dss_has_feature(FEAT_FIFO_MERGE)) {
+                WARN_ON(enable);
+                return;
+        }
+
+        DSSDBG("FIFO merge %s\n", enable ? "enabled" : "disabled");
+        REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 14, 14);
+}
+
+void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
+                u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
+                bool manual_update)
+{
+        /*
+         * All sizes are in bytes. Both the buffer and burst are made of
+         * buffer_units, and the fifo thresholds must be buffer_unit aligned.
+         */
+
+        unsigned buf_unit = dss_feat_get_buffer_size_unit();
+        unsigned ovl_fifo_size, total_fifo_size, burst_size;
+        int i;
+
+        burst_size = dispc_ovl_get_burst_size(plane);
+        ovl_fifo_size = dispc_ovl_get_fifo_size(plane);
+
+        if (use_fifomerge) {
+                total_fifo_size = 0;
+                for (i = 0; i < dss_feat_get_num_ovls(); ++i)
+                        total_fifo_size += dispc_ovl_get_fifo_size(i);
+        } else {
+                total_fifo_size = ovl_fifo_size;
+        }
+
+        /*
+         * We use the same low threshold for both fifomerge and non-fifomerge
+         * cases, but for fifomerge we calculate the high threshold using the
+         * combined fifo size
+         */
+
+        if (manual_update && dss_has_feature(FEAT_OMAP3_DSI_FIFO_BUG)) {
+                *fifo_low = ovl_fifo_size - burst_size * 2;
+                *fifo_high = total_fifo_size - burst_size;
+        } else if (plane == OMAP_DSS_WB) {
+                /*
+                 * Most optimal configuration for writeback is to push out data
+                 * to the interconnect the moment writeback pushes enough pixels
+                 * in the FIFO to form a burst
+                 */
+                *fifo_low = 0;
+                *fifo_high = burst_size;
+        } else {
+                *fifo_low = ovl_fifo_size - burst_size;
+                *fifo_high = total_fifo_size - buf_unit;
+        }
+}
+
+static void dispc_ovl_set_mflag(enum omap_plane plane, bool enable)
+{
+        int bit;
+
+        if (plane == OMAP_DSS_GFX)
+                bit = 14;
+        else
+                bit = 23;
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, bit, bit);
+}
+
+static void dispc_ovl_set_mflag_threshold(enum omap_plane plane,
+        int low, int high)
+{
+        dispc_write_reg(DISPC_OVL_MFLAG_THRESHOLD(plane),
+                FLD_VAL(high, 31, 16) | FLD_VAL(low, 15, 0));
+}
+
+static void dispc_init_mflag(void)
+{
+        int i;
+
+        /*
+         * HACK: NV12 color format and MFLAG seem to have problems working
+         * together: using two displays, and having an NV12 overlay on one of
+         * the displays will cause underflows/synclosts when MFLAG_CTRL=2.
+         * Changing MFLAG thresholds and PRELOAD to certain values seem to
+         * remove the errors, but there doesn't seem to be a clear logic on
+         * which values work and which not.
+         *
+         * As a work-around, set force MFLAG to always on.
+         */
+        dispc_write_reg(DISPC_GLOBAL_MFLAG_ATTRIBUTE,
+                (1 << 0) |      /* MFLAG_CTRL = force always on */
+                (0 << 2));      /* MFLAG_START = disable */
+
+        for (i = 0; i < dss_feat_get_num_ovls(); ++i) {
+                u32 size = dispc_ovl_get_fifo_size(i);
+                u32 unit = dss_feat_get_buffer_size_unit();
+                u32 low, high;
+
+                dispc_ovl_set_mflag(i, true);
+
+                /*
+                 * Simulation team suggests below thesholds:
+                 * HT = fifosize * 5 / 8;
+                 * LT = fifosize * 4 / 8;
+                 */
+
+                low = size * 4 / 8 / unit;
+                high = size * 5 / 8 / unit;
+
+                dispc_ovl_set_mflag_threshold(i, low, high);
+        }
+
+        if (dispc.feat->has_writeback) {
+                u32 size = dispc_ovl_get_fifo_size(OMAP_DSS_WB);
+                u32 unit = dss_feat_get_buffer_size_unit();
+                u32 low, high;
+
+                dispc_ovl_set_mflag(OMAP_DSS_WB, true);
+
+                /*
+                 * Simulation team suggests below thesholds:
+                 * HT = fifosize * 5 / 8;
+                 * LT = fifosize * 4 / 8;
+                 */
+
+                low = size * 4 / 8 / unit;
+                high = size * 5 / 8 / unit;
+
+                dispc_ovl_set_mflag_threshold(OMAP_DSS_WB, low, high);
+        }
+}
+
+static void dispc_ovl_set_fir(enum omap_plane plane,
+                                int hinc, int vinc,
+                                enum omap_color_component color_comp)
+{
+        u32 val;
+
+        if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+                u8 hinc_start, hinc_end, vinc_start, vinc_end;
+
+                dss_feat_get_reg_field(FEAT_REG_FIRHINC,
+                                        &hinc_start, &hinc_end);
+                dss_feat_get_reg_field(FEAT_REG_FIRVINC,
+                                        &vinc_start, &vinc_end);
+                val = FLD_VAL(vinc, vinc_start, vinc_end) |
+                                FLD_VAL(hinc, hinc_start, hinc_end);
+
+                dispc_write_reg(DISPC_OVL_FIR(plane), val);
+        } else {
+                val = FLD_VAL(vinc, 28, 16) | FLD_VAL(hinc, 12, 0);
+                dispc_write_reg(DISPC_OVL_FIR2(plane), val);
+        }
+}
+
+static void dispc_ovl_set_vid_accu0(enum omap_plane plane, int haccu, int vaccu)
+{
+        u32 val;
+        u8 hor_start, hor_end, vert_start, vert_end;
+
+        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);
+
+        dispc_write_reg(DISPC_OVL_ACCU0(plane), val);
+}
+
+static void dispc_ovl_set_vid_accu1(enum omap_plane plane, int haccu, int vaccu)
+{
+        u32 val;
+        u8 hor_start, hor_end, vert_start, vert_end;
+
+        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);
+
+        dispc_write_reg(DISPC_OVL_ACCU1(plane), val);
+}
+
+static void dispc_ovl_set_vid_accu2_0(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_0(plane), val);
+}
+
+static void dispc_ovl_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_ovl_set_scale_param(enum omap_plane plane,
+                u16 orig_width, u16 orig_height,
+                u16 out_width, u16 out_height,
+                bool five_taps, u8 rotation,
+                enum omap_color_component color_comp)
+{
+        int fir_hinc, fir_vinc;
+
+        fir_hinc = 1024 * orig_width / out_width;
+        fir_vinc = 1024 * orig_height / out_height;
+
+        dispc_ovl_set_scale_coef(plane, fir_hinc, fir_vinc, five_taps,
+                                color_comp);
+        dispc_ovl_set_fir(plane, fir_hinc, fir_vinc, color_comp);
+}
+
+static void dispc_ovl_set_accu_uv(enum omap_plane plane,
+                u16 orig_width, u16 orig_height, u16 out_width, u16 out_height,
+                bool ilace, enum omap_color_mode color_mode, u8 rotation)
+{
+        int h_accu2_0, h_accu2_1;
+        int v_accu2_0, v_accu2_1;
+        int chroma_hinc, chroma_vinc;
+        int idx;
+
+        struct accu {
+                s8 h0_m, h0_n;
+                s8 h1_m, h1_n;
+                s8 v0_m, v0_n;
+                s8 v1_m, v1_n;
+        };
+
+        const struct accu *accu_table;
+        const struct accu *accu_val;
+
+        static const struct accu accu_nv12[4] = {
+                {  0, 1,  0, 1 , -1, 2, 0, 1 },
+                {  1, 2, -3, 4 ,  0, 1, 0, 1 },
+                { -1, 1,  0, 1 , -1, 2, 0, 1 },
+                { -1, 2, -1, 2 , -1, 1, 0, 1 },
+        };
+
+        static const struct accu accu_nv12_ilace[4] = {
+                {  0, 1,  0, 1 , -3, 4, -1, 4 },
+                { -1, 4, -3, 4 ,  0, 1,  0, 1 },
+                { -1, 1,  0, 1 , -1, 4, -3, 4 },
+                { -3, 4, -3, 4 , -1, 1,  0, 1 },
+        };
+
+        static const struct accu accu_yuv[4] = {
+                {  0, 1, 0, 1,  0, 1, 0, 1 },
+                {  0, 1, 0, 1,  0, 1, 0, 1 },
+                { -1, 1, 0, 1,  0, 1, 0, 1 },
+                {  0, 1, 0, 1, -1, 1, 0, 1 },
+        };
+
+        switch (rotation) {
+        case OMAP_DSS_ROT_0:
+                idx = 0;
+                break;
+        case OMAP_DSS_ROT_90:
+                idx = 1;
+                break;
+        case OMAP_DSS_ROT_180:
+                idx = 2;
+                break;
+        case OMAP_DSS_ROT_270:
+                idx = 3;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_NV12:
+                if (ilace)
+                        accu_table = accu_nv12_ilace;
+                else
+                        accu_table = accu_nv12;
+                break;
+        case OMAP_DSS_COLOR_YUV2:
+        case OMAP_DSS_COLOR_UYVY:
+                accu_table = accu_yuv;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        accu_val = &accu_table[idx];
+
+        chroma_hinc = 1024 * orig_width / out_width;
+        chroma_vinc = 1024 * orig_height / out_height;
+
+        h_accu2_0 = (accu_val->h0_m * chroma_hinc / accu_val->h0_n) % 1024;
+        h_accu2_1 = (accu_val->h1_m * chroma_hinc / accu_val->h1_n) % 1024;
+        v_accu2_0 = (accu_val->v0_m * chroma_vinc / accu_val->v0_n) % 1024;
+        v_accu2_1 = (accu_val->v1_m * chroma_vinc / accu_val->v1_n) % 1024;
+
+        dispc_ovl_set_vid_accu2_0(plane, h_accu2_0, v_accu2_0);
+        dispc_ovl_set_vid_accu2_1(plane, h_accu2_1, v_accu2_1);
+}
+
+static void dispc_ovl_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;
+
+        dispc_ovl_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));
+
+        /* 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;
+
+        /* 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);
+        }
+
+        /* LINEBUFFERSPLIT */
+        if (dss_has_feature(FEAT_LINEBUFFERSPLIT)) {
+                l &= ~(0x1 << 22);
+                l |= five_taps ? (1 << 22) : 0;
+        }
+
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
+
+        /*
+         * field 0 = even field = bottom field
+         * field 1 = odd field = top field
+         */
+        if (ilace && !fieldmode) {
+                accu1 = 0;
+                accu0 = ((1024 * orig_height / out_height) / 2) & 0x3ff;
+                if (accu0 >= 1024/2) {
+                        accu1 = 1024/2;
+                        accu0 -= accu1;
+                }
+        }
+
+        dispc_ovl_set_vid_accu0(plane, 0, accu0);
+        dispc_ovl_set_vid_accu1(plane, 0, accu1);
+}
+
+static void dispc_ovl_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;
+        bool chroma_upscale = plane != OMAP_DSS_WB ? true : false;
+
+        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  */
+                if (plane != OMAP_DSS_WB)
+                        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
+                return;
+        }
+
+        dispc_ovl_set_accu_uv(plane, orig_width, orig_height, out_width,
+                        out_height, ilace, color_mode, rotation);
+
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_NV12:
+                if (chroma_upscale) {
+                        /* UV is subsampled by 2 horizontally and vertically */
+                        orig_height >>= 1;
+                        orig_width >>= 1;
+                } else {
+                        /* UV is downsampled by 2 horizontally and vertically */
+                        orig_height <<= 1;
+                        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) {
+                        if (chroma_upscale)
+                                /* UV is subsampled by 2 horizontally */
+                                orig_width >>= 1;
+                        else
+                                /* UV is downsampled by 2 horizontally */
+                                orig_width <<= 1;
+                }
+
+                /* must use FIR for YUV422 if rotated */
+                if (rotation != OMAP_DSS_ROT_0)
+                        scale_x = scale_y = true;
+
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        if (out_width != orig_width)
+                scale_x = true;
+        if (out_height != orig_height)
+                scale_y = true;
+
+        dispc_ovl_set_scale_param(plane, orig_width, orig_height,
+                        out_width, out_height, five_taps,
+                                rotation, DISPC_COLOR_COMPONENT_UV);
+
+        if (plane != OMAP_DSS_WB)
+                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);
+}
+
+static void dispc_ovl_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_ovl_set_scaling_common(plane,
+                        orig_width, orig_height,
+                        out_width, out_height,
+                        ilace, five_taps,
+                        fieldmode, color_mode,
+                        rotation);
+
+        dispc_ovl_set_scaling_uv(plane,
+                orig_width, orig_height,
+                out_width, out_height,
+                ilace, five_taps,
+                fieldmode, color_mode,
+                rotation);
+}
+
+static void dispc_ovl_set_rotation_attrs(enum omap_plane plane, u8 rotation,
+                enum omap_dss_rotation_type rotation_type,
+                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) {
+
+                if (mirroring) {
+                        switch (rotation) {
+                        case OMAP_DSS_ROT_0:
+                                vidrot = 2;
+                                break;
+                        case OMAP_DSS_ROT_90:
+                                vidrot = 1;
+                                break;
+                        case OMAP_DSS_ROT_180:
+                                vidrot = 0;
+                                break;
+                        case OMAP_DSS_ROT_270:
+                                vidrot = 3;
+                                break;
+                        }
+                } else {
+                        switch (rotation) {
+                        case OMAP_DSS_ROT_0:
+                                vidrot = 0;
+                                break;
+                        case OMAP_DSS_ROT_90:
+                                vidrot = 1;
+                                break;
+                        case OMAP_DSS_ROT_180:
+                                vidrot = 2;
+                                break;
+                        case OMAP_DSS_ROT_270:
+                                vidrot = 3;
+                                break;
+                        }
+                }
+
+                if (rotation == OMAP_DSS_ROT_90 || rotation == OMAP_DSS_ROT_270)
+                        row_repeat = true;
+                else
+                        row_repeat = false;
+        }
+
+        /*
+         * OMAP4/5 Errata i631:
+         * NV12 in 1D mode must use ROTATION=1. Otherwise DSS will fetch extra
+         * rows beyond the framebuffer, which may cause OCP error.
+         */
+        if (color_mode == OMAP_DSS_COLOR_NV12 &&
+                        rotation_type != OMAP_DSS_ROT_TILER)
+                vidrot = 1;
+
+        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);
+
+        if (color_mode == OMAP_DSS_COLOR_NV12) {
+                bool doublestride = (rotation_type == OMAP_DSS_ROT_TILER) &&
+                                        (rotation == OMAP_DSS_ROT_0 ||
+                                        rotation == OMAP_DSS_ROT_180);
+                /* DOUBLESTRIDE */
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), doublestride, 22, 22);
+        }
+
+}
+
+static int color_mode_to_bpp(enum omap_color_mode color_mode)
+{
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_CLUT1:
+                return 1;
+        case OMAP_DSS_COLOR_CLUT2:
+                return 2;
+        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;
+        case OMAP_DSS_COLOR_RGB24U:
+        case OMAP_DSS_COLOR_ARGB32:
+        case OMAP_DSS_COLOR_RGBA32:
+        case OMAP_DSS_COLOR_RGBX32:
+                return 32;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static s32 pixinc(int pixels, u8 ps)
+{
+        if (pixels == 1)
+                return 1;
+        else if (pixels > 1)
+                return 1 + (pixels - 1) * ps;
+        else if (pixels < 0)
+                return 1 - (-pixels + 1) * ps;
+        else
+                BUG();
+                return 0;
+}
+
+static void calc_vrfb_rotation_offset(u8 rotation, bool mirror,
+                u16 screen_width,
+                u16 width, u16 height,
+                enum omap_color_mode color_mode, bool fieldmode,
+                unsigned int field_offset,
+                unsigned *offset0, unsigned *offset1,
+                s32 *row_inc, s32 *pix_inc, int x_predecim, int y_predecim)
+{
+        u8 ps;
+
+        /* FIXME CLUT formats */
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_CLUT1:
+        case OMAP_DSS_COLOR_CLUT2:
+        case OMAP_DSS_COLOR_CLUT4:
+        case OMAP_DSS_COLOR_CLUT8:
+                BUG();
+                return;
+        case OMAP_DSS_COLOR_YUV2:
+        case OMAP_DSS_COLOR_UYVY:
+                ps = 4;
+                break;
+        default:
+                ps = color_mode_to_bpp(color_mode) / 8;
+                break;
+        }
+
+        DSSDBG("calc_rot(%d): scrw %d, %dx%d\n", rotation, screen_width,
+                        width, height);
+
+        /*
+         * field 0 = even field = bottom field
+         * field 1 = odd field = top field
+         */
+        switch (rotation + mirror * 4) {
+        case OMAP_DSS_ROT_0:
+        case OMAP_DSS_ROT_180:
+                /*
+                 * If the pixel format is YUV or UYVY divide the width
+                 * of the image by 2 for 0 and 180 degree rotation.
+                 */
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY)
+                        width = width >> 1;
+        case OMAP_DSS_ROT_90:
+        case OMAP_DSS_ROT_270:
+                *offset1 = 0;
+                if (field_offset)
+                        *offset0 = field_offset * screen_width * ps;
+                else
+                        *offset0 = 0;
+
+                *row_inc = pixinc(1 +
+                        (y_predecim * screen_width - x_predecim * width) +
+                        (fieldmode ? screen_width : 0), ps);
+                *pix_inc = pixinc(x_predecim, ps);
+                break;
+
+        case OMAP_DSS_ROT_0 + 4:
+        case OMAP_DSS_ROT_180 + 4:
+                /* If the pixel format is YUV or UYVY divide the width
+                 * of the image by 2  for 0 degree and 180 degree
+                 */
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY)
+                        width = width >> 1;
+        case OMAP_DSS_ROT_90 + 4:
+        case OMAP_DSS_ROT_270 + 4:
+                *offset1 = 0;
+                if (field_offset)
+                        *offset0 = field_offset * screen_width * ps;
+                else
+                        *offset0 = 0;
+                *row_inc = pixinc(1 -
+                        (y_predecim * screen_width + x_predecim * width) -
+                        (fieldmode ? screen_width : 0), ps);
+                *pix_inc = pixinc(x_predecim, ps);
+                break;
+
+        default:
+                BUG();
+                return;
+        }
+}
+
+static void calc_dma_rotation_offset(u8 rotation, bool mirror,
+                u16 screen_width,
+                u16 width, u16 height,
+                enum omap_color_mode color_mode, bool fieldmode,
+                unsigned int field_offset,
+                unsigned *offset0, unsigned *offset1,
+                s32 *row_inc, s32 *pix_inc, int x_predecim, int y_predecim)
+{
+        u8 ps;
+        u16 fbw, fbh;
+
+        /* FIXME CLUT formats */
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_CLUT1:
+        case OMAP_DSS_COLOR_CLUT2:
+        case OMAP_DSS_COLOR_CLUT4:
+        case OMAP_DSS_COLOR_CLUT8:
+                BUG();
+                return;
+        default:
+                ps = color_mode_to_bpp(color_mode) / 8;
+                break;
+        }
+
+        DSSDBG("calc_rot(%d): scrw %d, %dx%d\n", rotation, screen_width,
+                        width, height);
+
+        /* width & height are overlay sizes, convert to fb sizes */
+
+        if (rotation == OMAP_DSS_ROT_0 || rotation == OMAP_DSS_ROT_180) {
+                fbw = width;
+                fbh = height;
+        } else {
+                fbw = height;
+                fbh = width;
+        }
+
+        /*
+         * field 0 = even field = bottom field
+         * field 1 = odd field = top field
+         */
+        switch (rotation + mirror * 4) {
+        case OMAP_DSS_ROT_0:
+                *offset1 = 0;
+                if (field_offset)
+                        *offset0 = *offset1 + field_offset * screen_width * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(1 +
+                        (y_predecim * screen_width - fbw * x_predecim) +
+                        (fieldmode ? screen_width : 0), ps);
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY)
+                        *pix_inc = pixinc(x_predecim, 2 * ps);
+                else
+                        *pix_inc = pixinc(x_predecim, ps);
+                break;
+        case OMAP_DSS_ROT_90:
+                *offset1 = screen_width * (fbh - 1) * ps;
+                if (field_offset)
+                        *offset0 = *offset1 + field_offset * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(screen_width * (fbh * x_predecim - 1) +
+                                y_predecim + (fieldmode ? 1 : 0), ps);
+                *pix_inc = pixinc(-x_predecim * screen_width, ps);
+                break;
+        case OMAP_DSS_ROT_180:
+                *offset1 = (screen_width * (fbh - 1) + fbw - 1) * ps;
+                if (field_offset)
+                        *offset0 = *offset1 - field_offset * screen_width * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(-1 -
+                        (y_predecim * screen_width - fbw * x_predecim) -
+                        (fieldmode ? screen_width : 0), ps);
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY)
+                        *pix_inc = pixinc(-x_predecim, 2 * ps);
+                else
+                        *pix_inc = pixinc(-x_predecim, ps);
+                break;
+        case OMAP_DSS_ROT_270:
+                *offset1 = (fbw - 1) * ps;
+                if (field_offset)
+                        *offset0 = *offset1 - field_offset * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(-screen_width * (fbh * x_predecim - 1) -
+                                y_predecim - (fieldmode ? 1 : 0), ps);
+                *pix_inc = pixinc(x_predecim * screen_width, ps);
+                break;
+
+        /* mirroring */
+        case OMAP_DSS_ROT_0 + 4:
+                *offset1 = (fbw - 1) * ps;
+                if (field_offset)
+                        *offset0 = *offset1 + field_offset * screen_width * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(y_predecim * screen_width * 2 - 1 +
+                                (fieldmode ? screen_width : 0),
+                                ps);
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY)
+                        *pix_inc = pixinc(-x_predecim, 2 * ps);
+                else
+                        *pix_inc = pixinc(-x_predecim, ps);
+                break;
+
+        case OMAP_DSS_ROT_90 + 4:
+                *offset1 = 0;
+                if (field_offset)
+                        *offset0 = *offset1 + field_offset * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(-screen_width * (fbh * x_predecim - 1) +
+                                y_predecim + (fieldmode ? 1 : 0),
+                                ps);
+                *pix_inc = pixinc(x_predecim * screen_width, ps);
+                break;
+
+        case OMAP_DSS_ROT_180 + 4:
+                *offset1 = screen_width * (fbh - 1) * ps;
+                if (field_offset)
+                        *offset0 = *offset1 - field_offset * screen_width * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(1 - y_predecim * screen_width * 2 -
+                                (fieldmode ? screen_width : 0),
+                                ps);
+                if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY)
+                        *pix_inc = pixinc(x_predecim, 2 * ps);
+                else
+                        *pix_inc = pixinc(x_predecim, ps);
+                break;
+
+        case OMAP_DSS_ROT_270 + 4:
+                *offset1 = (screen_width * (fbh - 1) + fbw - 1) * ps;
+                if (field_offset)
+                        *offset0 = *offset1 - field_offset * ps;
+                else
+                        *offset0 = *offset1;
+                *row_inc = pixinc(screen_width * (fbh * x_predecim - 1) -
+                                y_predecim - (fieldmode ? 1 : 0),
+                                ps);
+                *pix_inc = pixinc(-x_predecim * screen_width, ps);
+                break;
+
+        default:
+                BUG();
+                return;
+        }
+}
+
+static void calc_tiler_rotation_offset(u16 screen_width, u16 width,
+                enum omap_color_mode color_mode, bool fieldmode,
+                unsigned int field_offset, unsigned *offset0, unsigned *offset1,
+                s32 *row_inc, s32 *pix_inc, int x_predecim, int y_predecim)
+{
+        u8 ps;
+
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_CLUT1:
+        case OMAP_DSS_COLOR_CLUT2:
+        case OMAP_DSS_COLOR_CLUT4:
+        case OMAP_DSS_COLOR_CLUT8:
+                BUG();
+                return;
+        default:
+                ps = color_mode_to_bpp(color_mode) / 8;
+                break;
+        }
+
+        DSSDBG("scrw %d, width %d\n", screen_width, width);
+
+        /*
+         * field 0 = even field = bottom field
+         * field 1 = odd field = top field
+         */
+        *offset1 = 0;
+        if (field_offset)
+                *offset0 = *offset1 + field_offset * screen_width * ps;
+        else
+                *offset0 = *offset1;
+        *row_inc = pixinc(1 + (y_predecim * screen_width - width * x_predecim) +
+                        (fieldmode ? screen_width : 0), ps);
+        if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                color_mode == OMAP_DSS_COLOR_UYVY)
+                *pix_inc = pixinc(x_predecim, 2 * ps);
+        else
+                *pix_inc = pixinc(x_predecim, ps);
+}
+
+/*
+ * This function is used to avoid synclosts in OMAP3, because of some
+ * undocumented horizontal position and timing related limitations.
+ */
+static int check_horiz_timing_omap3(unsigned long pclk, unsigned long lclk,
+                const struct omap_video_timings *t, u16 pos_x,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                bool five_taps)
+{
+        const int ds = DIV_ROUND_UP(height, out_height);
+        unsigned long nonactive;
+        static const u8 limits[3] = { 8, 10, 20 };
+        u64 val, blank;
+        int i;
+
+        nonactive = t->x_res + t->hfp + t->hsw + t->hbp - out_width;
+
+        i = 0;
+        if (out_height < height)
+                i++;
+        if (out_width < width)
+                i++;
+        blank = div_u64((u64)(t->hbp + t->hsw + t->hfp) * lclk, pclk);
+        DSSDBG("blanking period + ppl = %llu (limit = %u)\n", blank, limits[i]);
+        if (blank <= limits[i])
+                return -EINVAL;
+
+        /* FIXME add checks for 3-tap filter once the limitations are known */
+        if (!five_taps)
+                return 0;
+
+        /*
+         * Pixel data should be prepared before visible display point starts.
+         * So, atleast DS-2 lines must have already been fetched by DISPC
+         * during nonactive - pos_x period.
+         */
+        val = div_u64((u64)(nonactive - pos_x) * lclk, pclk);
+        DSSDBG("(nonactive - pos_x) * pcd = %llu max(0, DS - 2) * width = %d\n",
+                val, max(0, ds - 2) * width);
+        if (val < max(0, ds - 2) * width)
+                return -EINVAL;
+
+        /*
+         * All lines need to be refilled during the nonactive period of which
+         * only one line can be loaded during the active period. So, atleast
+         * DS - 1 lines should be loaded during nonactive period.
+         */
+        val =  div_u64((u64)nonactive * lclk, pclk);
+        DSSDBG("nonactive * pcd  = %llu, max(0, DS - 1) * width = %d\n",
+                val, max(0, ds - 1) * width);
+        if (val < max(0, ds - 1) * width)
+                return -EINVAL;
+
+        return 0;
+}
+
+static unsigned long calc_core_clk_five_taps(unsigned long pclk,
+                const struct omap_video_timings *mgr_timings, u16 width,
+                u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode)
+{
+        u32 core_clk = 0;
+        u64 tmp;
+
+        if (height <= out_height && width <= out_width)
+                return (unsigned long) pclk;
+
+        if (height > out_height) {
+                unsigned int ppl = mgr_timings->x_res;
+
+                tmp = (u64)pclk * height * out_width;
+                do_div(tmp, 2 * out_height * ppl);
+                core_clk = tmp;
+
+                if (height > 2 * out_height) {
+                        if (ppl == out_width)
+                                return 0;
+
+                        tmp = (u64)pclk * (height - 2 * out_height) * out_width;
+                        do_div(tmp, 2 * out_height * (ppl - out_width));
+                        core_clk = max_t(u32, core_clk, tmp);
+                }
+        }
+
+        if (width > out_width) {
+                tmp = (u64)pclk * width;
+                do_div(tmp, out_width);
+                core_clk = max_t(u32, core_clk, tmp);
+
+                if (color_mode == OMAP_DSS_COLOR_RGB24U)
+                        core_clk <<= 1;
+        }
+
+        return core_clk;
+}
+
+static unsigned long calc_core_clk_24xx(unsigned long pclk, u16 width,
+                u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+        if (height > out_height && width > out_width)
+                return pclk * 4;
+        else
+                return pclk * 2;
+}
+
+static unsigned long calc_core_clk_34xx(unsigned long pclk, u16 width,
+                u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+        unsigned int hf, vf;
+
+        /*
+         * FIXME how to determine the 'A' factor
+         * for the no downscaling case ?
+         */
+
+        if (width > 3 * out_width)
+                hf = 4;
+        else if (width > 2 * out_width)
+                hf = 3;
+        else if (width > out_width)
+                hf = 2;
+        else
+                hf = 1;
+        if (height > out_height)
+                vf = 2;
+        else
+                vf = 1;
+
+        return pclk * vf * hf;
+}
+
+static unsigned long calc_core_clk_44xx(unsigned long pclk, u16 width,
+                u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+        /*
+         * If the overlay/writeback is in mem to mem mode, there are no
+         * downscaling limitations with respect to pixel clock, return 1 as
+         * required core clock to represent that we have sufficient enough
+         * core clock to do maximum downscaling
+         */
+        if (mem_to_mem)
+                return 1;
+
+        if (width > out_width)
+                return DIV_ROUND_UP(pclk, out_width) * width;
+        else
+                return pclk;
+}
+
+static int dispc_ovl_calc_scaling_24xx(unsigned long pclk, unsigned long lclk,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+        int error;
+        u16 in_width, in_height;
+        int min_factor = min(*decim_x, *decim_y);
+        const int maxsinglelinewidth =
+                        dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+        *five_taps = false;
+
+        do {
+                in_height = height / *decim_y;
+                in_width = width / *decim_x;
+                *core_clk = dispc.feat->calc_core_clk(pclk, in_width,
+                                in_height, out_width, out_height, mem_to_mem);
+                error = (in_width > maxsinglelinewidth || !*core_clk ||
+                        *core_clk > dispc_core_clk_rate());
+                if (error) {
+                        if (*decim_x == *decim_y) {
+                                *decim_x = min_factor;
+                                ++*decim_y;
+                        } else {
+                                swap(*decim_x, *decim_y);
+                                if (*decim_x < *decim_y)
+                                        ++*decim_x;
+                        }
+                }
+        } while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+        if (error) {
+                DSSERR("failed to find scaling settings\n");
+                return -EINVAL;
+        }
+
+        if (in_width > maxsinglelinewidth) {
+                DSSERR("Cannot scale max input width exceeded");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int dispc_ovl_calc_scaling_34xx(unsigned long pclk, unsigned long lclk,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+        int error;
+        u16 in_width, in_height;
+        const int maxsinglelinewidth =
+                        dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+        do {
+                in_height = height / *decim_y;
+                in_width = width / *decim_x;
+                *five_taps = in_height > out_height;
+
+                if (in_width > maxsinglelinewidth)
+                        if (in_height > out_height &&
+                                                in_height < out_height * 2)
+                                *five_taps = false;
+again:
+                if (*five_taps)
+                        *core_clk = calc_core_clk_five_taps(pclk, mgr_timings,
+                                                in_width, in_height, out_width,
+                                                out_height, color_mode);
+                else
+                        *core_clk = dispc.feat->calc_core_clk(pclk, in_width,
+                                        in_height, out_width, out_height,
+                                        mem_to_mem);
+
+                error = check_horiz_timing_omap3(pclk, lclk, mgr_timings,
+                                pos_x, in_width, in_height, out_width,
+                                out_height, *five_taps);
+                if (error && *five_taps) {
+                        *five_taps = false;
+                        goto again;
+                }
+
+                error = (error || in_width > maxsinglelinewidth * 2 ||
+                        (in_width > maxsinglelinewidth && *five_taps) ||
+                        !*core_clk || *core_clk > dispc_core_clk_rate());
+
+                if (!error) {
+                        /* verify that we're inside the limits of scaler */
+                        if (in_width / 4 > out_width)
+                                        error = 1;
+
+                        if (*five_taps) {
+                                if (in_height / 4 > out_height)
+                                        error = 1;
+                        } else {
+                                if (in_height / 2 > out_height)
+                                        error = 1;
+                        }
+                }
+
+                if (error)
+                        ++*decim_y;
+        } while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+        if (error) {
+                DSSERR("failed to find scaling settings\n");
+                return -EINVAL;
+        }
+
+        if (check_horiz_timing_omap3(pclk, lclk, mgr_timings, pos_x, in_width,
+                                in_height, out_width, out_height, *five_taps)) {
+                        DSSERR("horizontal timing too tight\n");
+                        return -EINVAL;
+        }
+
+        if (in_width > (maxsinglelinewidth * 2)) {
+                DSSERR("Cannot setup scaling");
+                DSSERR("width exceeds maximum width possible");
+                return -EINVAL;
+        }
+
+        if (in_width > maxsinglelinewidth && *five_taps) {
+                DSSERR("cannot setup scaling with five taps");
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int dispc_ovl_calc_scaling_44xx(unsigned long pclk, unsigned long lclk,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+                u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+        u16 in_width, in_width_max;
+        int decim_x_min = *decim_x;
+        u16 in_height = height / *decim_y;
+        const int maxsinglelinewidth =
+                                dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+        const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+
+        if (mem_to_mem) {
+                in_width_max = out_width * maxdownscale;
+        } else {
+                in_width_max = dispc_core_clk_rate() /
+                                        DIV_ROUND_UP(pclk, out_width);
+        }
+
+        *decim_x = DIV_ROUND_UP(width, in_width_max);
+
+        *decim_x = *decim_x > decim_x_min ? *decim_x : decim_x_min;
+        if (*decim_x > *x_predecim)
+                return -EINVAL;
+
+        do {
+                in_width = width / *decim_x;
+        } while (*decim_x <= *x_predecim &&
+                        in_width > maxsinglelinewidth && ++*decim_x);
+
+        if (in_width > maxsinglelinewidth) {
+                DSSERR("Cannot scale width exceeds max line width");
+                return -EINVAL;
+        }
+
+        *core_clk = dispc.feat->calc_core_clk(pclk, in_width, in_height,
+                                out_width, out_height, mem_to_mem);
+        return 0;
+}
+
+#define DIV_FRAC(dividend, divisor) \
+        ((dividend) * 100 / (divisor) - ((dividend) / (divisor) * 100))
+
+static int dispc_ovl_calc_scaling(unsigned long pclk, unsigned long lclk,
+                enum omap_overlay_caps caps,
+                const struct omap_video_timings *mgr_timings,
+                u16 width, u16 height, u16 out_width, u16 out_height,
+                enum omap_color_mode color_mode, bool *five_taps,
+                int *x_predecim, int *y_predecim, u16 pos_x,
+                enum omap_dss_rotation_type rotation_type, bool mem_to_mem)
+{
+        const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+        const int max_decim_limit = 16;
+        unsigned long core_clk = 0;
+        int decim_x, decim_y, ret;
+
+        if (width == out_width && height == out_height)
+                return 0;
+
+        if (!mem_to_mem && (pclk == 0 || mgr_timings->pixelclock == 0)) {
+                DSSERR("cannot calculate scaling settings: pclk is zero\n");
+                return -EINVAL;
+        }
+
+        if ((caps & OMAP_DSS_OVL_CAP_SCALE) == 0)
+                return -EINVAL;
+
+        if (mem_to_mem) {
+                *x_predecim = *y_predecim = 1;
+        } else {
+                *x_predecim = max_decim_limit;
+                *y_predecim = (rotation_type == OMAP_DSS_ROT_TILER &&
+                                dss_has_feature(FEAT_BURST_2D)) ?
+                                2 : max_decim_limit;
+        }
+
+        if (color_mode == OMAP_DSS_COLOR_CLUT1 ||
+            color_mode == OMAP_DSS_COLOR_CLUT2 ||
+            color_mode == OMAP_DSS_COLOR_CLUT4 ||
+            color_mode == OMAP_DSS_COLOR_CLUT8) {
+                *x_predecim = 1;
+                *y_predecim = 1;
+                *five_taps = false;
+                return 0;
+        }
+
+        decim_x = DIV_ROUND_UP(DIV_ROUND_UP(width, out_width), maxdownscale);
+        decim_y = DIV_ROUND_UP(DIV_ROUND_UP(height, out_height), maxdownscale);
+
+        if (decim_x > *x_predecim || out_width > width * 8)
+                return -EINVAL;
+
+        if (decim_y > *y_predecim || out_height > height * 8)
+                return -EINVAL;
+
+        ret = dispc.feat->calc_scaling(pclk, lclk, mgr_timings, width, height,
+                out_width, out_height, color_mode, five_taps,
+                x_predecim, y_predecim, &decim_x, &decim_y, pos_x, &core_clk,
+                mem_to_mem);
+        if (ret)
+                return ret;
+
+        DSSDBG("%dx%d -> %dx%d (%d.%02d x %d.%02d), decim %dx%d %dx%d (%d.%02d x %d.%02d), taps %d, req clk %lu, cur clk %lu\n",
+                width, height,
+                out_width, out_height,
+                out_width / width, DIV_FRAC(out_width, width),
+                out_height / height, DIV_FRAC(out_height, height),
+
+                decim_x, decim_y,
+                width / decim_x, height / decim_y,
+                out_width / (width / decim_x), DIV_FRAC(out_width, width / decim_x),
+                out_height / (height / decim_y), DIV_FRAC(out_height, height / decim_y),
+
+                *five_taps ? 5 : 3,
+                core_clk, dispc_core_clk_rate());
+
+        if (!core_clk || core_clk > dispc_core_clk_rate()) {
+                DSSERR("failed to set up scaling, "
+                        "required core clk rate = %lu Hz, "
+                        "current core clk rate = %lu Hz\n",
+                        core_clk, dispc_core_clk_rate());
+                return -EINVAL;
+        }
+
+        *x_predecim = decim_x;
+        *y_predecim = decim_y;
+        return 0;
+}
+
+int dispc_ovl_check(enum omap_plane plane, enum omap_channel channel,
+                const struct omap_overlay_info *oi,
+                const struct omap_video_timings *timings,
+                int *x_predecim, int *y_predecim)
+{
+        enum omap_overlay_caps caps = dss_feat_get_overlay_caps(plane);
+        bool five_taps = true;
+        bool fieldmode = false;
+        u16 in_height = oi->height;
+        u16 in_width = oi->width;
+        bool ilace = timings->interlace;
+        u16 out_width, out_height;
+        int pos_x = oi->pos_x;
+        unsigned long pclk = dispc_mgr_pclk_rate(channel);
+        unsigned long lclk = dispc_mgr_lclk_rate(channel);
+
+        out_width = oi->out_width == 0 ? oi->width : oi->out_width;
+        out_height = oi->out_height == 0 ? oi->height : oi->out_height;
+
+        if (ilace && oi->height == out_height)
+                fieldmode = true;
+
+        if (ilace) {
+                if (fieldmode)
+                        in_height /= 2;
+                out_height /= 2;
+
+                DSSDBG("adjusting for ilace: height %d, out_height %d\n",
+                                in_height, out_height);
+        }
+
+        if (!dss_feat_color_mode_supported(plane, oi->color_mode))
+                return -EINVAL;
+
+        return dispc_ovl_calc_scaling(pclk, lclk, caps, timings, in_width,
+                        in_height, out_width, out_height, oi->color_mode,
+                        &five_taps, x_predecim, y_predecim, pos_x,
+                        oi->rotation_type, false);
+}
+EXPORT_SYMBOL(dispc_ovl_check);
+
+static int dispc_ovl_setup_common(enum omap_plane plane,
+                enum omap_overlay_caps caps, u32 paddr, u32 p_uv_addr,
+                u16 screen_width, int pos_x, int pos_y, u16 width, u16 height,
+                u16 out_width, u16 out_height, enum omap_color_mode color_mode,
+                u8 rotation, bool mirror, u8 zorder, u8 pre_mult_alpha,
+                u8 global_alpha, enum omap_dss_rotation_type rotation_type,
+                bool replication, const struct omap_video_timings *mgr_timings,
+                bool mem_to_mem)
+{
+        bool five_taps = true;
+        bool fieldmode = false;
+        int r, cconv = 0;
+        unsigned offset0, offset1;
+        s32 row_inc;
+        s32 pix_inc;
+        u16 frame_width, frame_height;
+        unsigned int field_offset = 0;
+        u16 in_height = height;
+        u16 in_width = width;
+        int x_predecim = 1, y_predecim = 1;
+        bool ilace = mgr_timings->interlace;
+        unsigned long pclk = dispc_plane_pclk_rate(plane);
+        unsigned long lclk = dispc_plane_lclk_rate(plane);
+
+        if (paddr == 0 && rotation_type != OMAP_DSS_ROT_TILER)
+                return -EINVAL;
+
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_YUV2:
+        case OMAP_DSS_COLOR_UYVY:
+        case OMAP_DSS_COLOR_NV12:
+                if (in_width & 1) {
+                        DSSERR("input width %d is not even for YUV format\n",
+                                in_width);
+                        return -EINVAL;
+                }
+                break;
+
+        default:
+                break;
+        }
+
+        out_width = out_width == 0 ? width : out_width;
+        out_height = out_height == 0 ? height : out_height;
+
+        if (ilace && height == out_height)
+                fieldmode = true;
+
+        if (ilace) {
+                if (fieldmode)
+                        in_height /= 2;
+                pos_y /= 2;
+                out_height /= 2;
+
+                DSSDBG("adjusting for ilace: height %d, pos_y %d, "
+                        "out_height %d\n", in_height, pos_y,
+                        out_height);
+        }
+
+        if (!dss_feat_color_mode_supported(plane, color_mode))
+                return -EINVAL;
+
+        r = dispc_ovl_calc_scaling(pclk, lclk, caps, mgr_timings, in_width,
+                        in_height, out_width, out_height, color_mode,
+                        &five_taps, &x_predecim, &y_predecim, pos_x,
+                        rotation_type, mem_to_mem);
+        if (r)
+                return r;
+
+        in_width = in_width / x_predecim;
+        in_height = in_height / y_predecim;
+
+        if (x_predecim > 1 || y_predecim > 1)
+                DSSDBG("predecimation %d x %x, new input size %d x %d\n",
+                        x_predecim, y_predecim, in_width, in_height);
+
+        switch (color_mode) {
+        case OMAP_DSS_COLOR_YUV2:
+        case OMAP_DSS_COLOR_UYVY:
+        case OMAP_DSS_COLOR_NV12:
+                if (in_width & 1) {
+                        DSSDBG("predecimated input width is not even for YUV format\n");
+                        DSSDBG("adjusting input width %d -> %d\n",
+                                in_width, in_width & ~1);
+
+                        in_width &= ~1;
+                }
+                break;
+
+        default:
+                break;
+        }
+
+        if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+                        color_mode == OMAP_DSS_COLOR_UYVY ||
+                        color_mode == OMAP_DSS_COLOR_NV12)
+                cconv = 1;
+
+        if (ilace && !fieldmode) {
+                /*
+                 * when downscaling the bottom field may have to start several
+                 * source lines below the top field. Unfortunately ACCUI
+                 * registers will only hold the fractional part of the offset
+                 * so the integer part must be added to the base address of the
+                 * bottom field.
+                 */
+                if (!in_height || in_height == out_height)
+                        field_offset = 0;
+                else
+                        field_offset = in_height / out_height / 2;
+        }
+
+        /* Fields are independent but interleaved in memory. */
+        if (fieldmode)
+                field_offset = 1;
+
+        offset0 = 0;
+        offset1 = 0;
+        row_inc = 0;
+        pix_inc = 0;
+
+        if (plane == OMAP_DSS_WB) {
+                frame_width = out_width;
+                frame_height = out_height;
+        } else {
+                frame_width = in_width;
+                frame_height = height;
+        }
+
+        if (rotation_type == OMAP_DSS_ROT_TILER)
+                calc_tiler_rotation_offset(screen_width, frame_width,
+                                color_mode, fieldmode, field_offset,
+                                &offset0, &offset1, &row_inc, &pix_inc,
+                                x_predecim, y_predecim);
+        else if (rotation_type == OMAP_DSS_ROT_DMA)
+                calc_dma_rotation_offset(rotation, mirror, screen_width,
+                                frame_width, frame_height,
+                                color_mode, fieldmode, field_offset,
+                                &offset0, &offset1, &row_inc, &pix_inc,
+                                x_predecim, y_predecim);
+        else
+                calc_vrfb_rotation_offset(rotation, mirror,
+                                screen_width, frame_width, frame_height,
+                                color_mode, fieldmode, field_offset,
+                                &offset0, &offset1, &row_inc, &pix_inc,
+                                x_predecim, y_predecim);
+
+        DSSDBG("offset0 %u, offset1 %u, row_inc %d, pix_inc %d\n",
+                        offset0, offset1, row_inc, pix_inc);
+
+        dispc_ovl_set_color_mode(plane, color_mode);
+
+        dispc_ovl_configure_burst_type(plane, rotation_type);
+
+        dispc_ovl_set_ba0(plane, paddr + offset0);
+        dispc_ovl_set_ba1(plane, paddr + offset1);
+
+        if (OMAP_DSS_COLOR_NV12 == color_mode) {
+                dispc_ovl_set_ba0_uv(plane, p_uv_addr + offset0);
+                dispc_ovl_set_ba1_uv(plane, p_uv_addr + offset1);
+        }
+
+        if (dispc.feat->last_pixel_inc_missing)
+                row_inc += pix_inc - 1;
+
+        dispc_ovl_set_row_inc(plane, row_inc);
+        dispc_ovl_set_pix_inc(plane, pix_inc);
+
+        DSSDBG("%d,%d %dx%d -> %dx%d\n", pos_x, pos_y, in_width,
+                        in_height, out_width, out_height);
+
+        dispc_ovl_set_pos(plane, caps, pos_x, pos_y);
+
+        dispc_ovl_set_input_size(plane, in_width, in_height);
+
+        if (caps & OMAP_DSS_OVL_CAP_SCALE) {
+                dispc_ovl_set_scaling(plane, in_width, in_height, out_width,
+                                   out_height, ilace, five_taps, fieldmode,
+                                   color_mode, rotation);
+                dispc_ovl_set_output_size(plane, out_width, out_height);
+                dispc_ovl_set_vid_color_conv(plane, cconv);
+        }
+
+        dispc_ovl_set_rotation_attrs(plane, rotation, rotation_type, mirror,
+                        color_mode);
+
+        dispc_ovl_set_zorder(plane, caps, zorder);
+        dispc_ovl_set_pre_mult_alpha(plane, caps, pre_mult_alpha);
+        dispc_ovl_setup_global_alpha(plane, caps, global_alpha);
+
+        dispc_ovl_enable_replication(plane, caps, replication);
+
+        return 0;
+}
+
+int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
+                bool replication, const struct omap_video_timings *mgr_timings,
+                bool mem_to_mem)
+{
+        int r;
+        enum omap_overlay_caps caps = dss_feat_get_overlay_caps(plane);
+        enum omap_channel channel;
+
+        channel = dispc_ovl_get_channel_out(plane);
+
+        DSSDBG("dispc_ovl_setup %d, pa %pad, pa_uv %pad, sw %d, %d,%d, %dx%d ->"
+                " %dx%d, cmode %x, rot %d, mir %d, chan %d repl %d\n",
+                plane, &oi->paddr, &oi->p_uv_addr, oi->screen_width, oi->pos_x,
+                oi->pos_y, oi->width, oi->height, oi->out_width, oi->out_height,
+                oi->color_mode, oi->rotation, oi->mirror, channel, replication);
+
+        r = dispc_ovl_setup_common(plane, caps, oi->paddr, oi->p_uv_addr,
+                oi->screen_width, oi->pos_x, oi->pos_y, oi->width, oi->height,
+                oi->out_width, oi->out_height, oi->color_mode, oi->rotation,
+                oi->mirror, oi->zorder, oi->pre_mult_alpha, oi->global_alpha,
+                oi->rotation_type, replication, mgr_timings, mem_to_mem);
+
+        return r;
+}
+EXPORT_SYMBOL(dispc_ovl_setup);
+
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+                bool mem_to_mem, const struct omap_video_timings *mgr_timings)
+{
+        int r;
+        u32 l;
+        enum omap_plane plane = OMAP_DSS_WB;
+        const int pos_x = 0, pos_y = 0;
+        const u8 zorder = 0, global_alpha = 0;
+        const bool replication = false;
+        bool truncation;
+        int in_width = mgr_timings->x_res;
+        int in_height = mgr_timings->y_res;
+        enum omap_overlay_caps caps =
+                OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA;
+
+        DSSDBG("dispc_wb_setup, pa %x, pa_uv %x, %d,%d -> %dx%d, cmode %x, "
+                "rot %d, mir %d\n", wi->paddr, wi->p_uv_addr, in_width,
+                in_height, wi->width, wi->height, wi->color_mode, wi->rotation,
+                wi->mirror);
+
+        r = dispc_ovl_setup_common(plane, caps, wi->paddr, wi->p_uv_addr,
+                wi->buf_width, pos_x, pos_y, in_width, in_height, wi->width,
+                wi->height, wi->color_mode, wi->rotation, wi->mirror, zorder,
+                wi->pre_mult_alpha, global_alpha, wi->rotation_type,
+                replication, mgr_timings, mem_to_mem);
+
+        switch (wi->color_mode) {
+        case OMAP_DSS_COLOR_RGB16:
+        case OMAP_DSS_COLOR_RGB24P:
+        case OMAP_DSS_COLOR_ARGB16:
+        case OMAP_DSS_COLOR_RGBA16:
+        case OMAP_DSS_COLOR_RGB12U:
+        case OMAP_DSS_COLOR_ARGB16_1555:
+        case OMAP_DSS_COLOR_XRGB16_1555:
+        case OMAP_DSS_COLOR_RGBX16:
+                truncation = true;
+                break;
+        default:
+                truncation = false;
+                break;
+        }
+
+        /* setup extra DISPC_WB_ATTRIBUTES */
+        l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+        l = FLD_MOD(l, truncation, 10, 10);     /* TRUNCATIONENABLE */
+        l = FLD_MOD(l, mem_to_mem, 19, 19);     /* WRITEBACKMODE */
+        if (mem_to_mem)
+                l = FLD_MOD(l, 1, 26, 24);      /* CAPTUREMODE */
+        else
+                l = FLD_MOD(l, 0, 26, 24);      /* CAPTUREMODE */
+        dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
+
+        if (mem_to_mem) {
+                /* WBDELAYCOUNT */
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 7, 0);
+        } else {
+                int wbdelay;
+
+                wbdelay = min(mgr_timings->vfp + mgr_timings->vsw +
+                        mgr_timings->vbp, 255);
+
+                /* WBDELAYCOUNT */
+                REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), wbdelay, 7, 0);
+        }
+
+        return r;
+}
+
+int dispc_ovl_enable(enum omap_plane plane, bool enable)
+{
+        DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
+
+        REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 0, 0);
+
+        return 0;
+}
+EXPORT_SYMBOL(dispc_ovl_enable);
+
+bool dispc_ovl_enabled(enum omap_plane plane)
+{
+        return REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0);
+}
+EXPORT_SYMBOL(dispc_ovl_enabled);
+
+void dispc_mgr_enable(enum omap_channel channel, bool enable)
+{
+        mgr_fld_write(channel, DISPC_MGR_FLD_ENABLE, enable);
+        /* flush posted write */
+        mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
+}
+EXPORT_SYMBOL(dispc_mgr_enable);
+
+bool dispc_mgr_is_enabled(enum omap_channel channel)
+{
+        return !!mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
+}
+EXPORT_SYMBOL(dispc_mgr_is_enabled);
+
+void dispc_wb_enable(bool enable)
+{
+        dispc_ovl_enable(OMAP_DSS_WB, enable);
+}
+
+bool dispc_wb_is_enabled(void)
+{
+        return dispc_ovl_enabled(OMAP_DSS_WB);
+}
+
+static void dispc_lcd_enable_signal_polarity(bool act_high)
+{
+        if (!dss_has_feature(FEAT_LCDENABLEPOL))
+                return;
+
+        REG_FLD_MOD(DISPC_CONTROL, act_high ? 1 : 0, 29, 29);
+}
+
+void dispc_lcd_enable_signal(bool enable)
+{
+        if (!dss_has_feature(FEAT_LCDENABLESIGNAL))
+                return;
+
+        REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 28, 28);
+}
+
+void dispc_pck_free_enable(bool enable)
+{
+        if (!dss_has_feature(FEAT_PCKFREEENABLE))
+                return;
+
+        REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 27, 27);
+}
+
+static void dispc_mgr_enable_fifohandcheck(enum omap_channel channel, bool enable)
+{
+        mgr_fld_write(channel, DISPC_MGR_FLD_FIFOHANDCHECK, enable);
+}
+
+
+static void dispc_mgr_set_lcd_type_tft(enum omap_channel channel)
+{
+        mgr_fld_write(channel, DISPC_MGR_FLD_STNTFT, 1);
+}
+
+static void dispc_set_loadmode(enum omap_dss_load_mode mode)
+{
+        REG_FLD_MOD(DISPC_CONFIG, mode, 2, 1);
+}
+
+
+static void dispc_mgr_set_default_color(enum omap_channel channel, u32 color)
+{
+        dispc_write_reg(DISPC_DEFAULT_COLOR(channel), color);
+}
+
+static void dispc_mgr_set_trans_key(enum omap_channel ch,
+                enum omap_dss_trans_key_type type,
+                u32 trans_key)
+{
+        mgr_fld_write(ch, DISPC_MGR_FLD_TCKSELECTION, type);
+
+        dispc_write_reg(DISPC_TRANS_COLOR(ch), trans_key);
+}
+
+static void dispc_mgr_enable_trans_key(enum omap_channel ch, bool enable)
+{
+        mgr_fld_write(ch, DISPC_MGR_FLD_TCKENABLE, enable);
+}
+
+static void dispc_mgr_enable_alpha_fixed_zorder(enum omap_channel ch,
+                bool enable)
+{
+        if (!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER))
+                return;
+
+        if (ch == OMAP_DSS_CHANNEL_LCD)
+                REG_FLD_MOD(DISPC_CONFIG, enable, 18, 18);
+        else if (ch == OMAP_DSS_CHANNEL_DIGIT)
+                REG_FLD_MOD(DISPC_CONFIG, enable, 19, 19);
+}
+
+void dispc_mgr_setup(enum omap_channel channel,
+                const struct omap_overlay_manager_info *info)
+{
+        dispc_mgr_set_default_color(channel, info->default_color);
+        dispc_mgr_set_trans_key(channel, info->trans_key_type, info->trans_key);
+        dispc_mgr_enable_trans_key(channel, info->trans_enabled);
+        dispc_mgr_enable_alpha_fixed_zorder(channel,
+                        info->partial_alpha_enabled);
+        if (dss_has_feature(FEAT_CPR)) {
+                dispc_mgr_enable_cpr(channel, info->cpr_enable);
+                dispc_mgr_set_cpr_coef(channel, &info->cpr_coefs);
+        }
+}
+EXPORT_SYMBOL(dispc_mgr_setup);
+
+static void dispc_mgr_set_tft_data_lines(enum omap_channel channel, u8 data_lines)
+{
+        int code;
+
+        switch (data_lines) {
+        case 12:
+                code = 0;
+                break;
+        case 16:
+                code = 1;
+                break;
+        case 18:
+                code = 2;
+                break;
+        case 24:
+                code = 3;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        mgr_fld_write(channel, DISPC_MGR_FLD_TFTDATALINES, code);
+}
+
+static void dispc_mgr_set_io_pad_mode(enum dss_io_pad_mode mode)
+{
+        u32 l;
+        int gpout0, gpout1;
+
+        switch (mode) {
+        case DSS_IO_PAD_MODE_RESET:
+                gpout0 = 0;
+                gpout1 = 0;
+                break;
+        case DSS_IO_PAD_MODE_RFBI:
+                gpout0 = 1;
+                gpout1 = 0;
+                break;
+        case DSS_IO_PAD_MODE_BYPASS:
+                gpout0 = 1;
+                gpout1 = 1;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        l = dispc_read_reg(DISPC_CONTROL);
+        l = FLD_MOD(l, gpout0, 15, 15);
+        l = FLD_MOD(l, gpout1, 16, 16);
+        dispc_write_reg(DISPC_CONTROL, l);
+}
+
+static void dispc_mgr_enable_stallmode(enum omap_channel channel, bool enable)
+{
+        mgr_fld_write(channel, DISPC_MGR_FLD_STALLMODE, enable);
+}
+
+void dispc_mgr_set_lcd_config(enum omap_channel channel,
+                const struct dss_lcd_mgr_config *config)
+{
+        dispc_mgr_set_io_pad_mode(config->io_pad_mode);
+
+        dispc_mgr_enable_stallmode(channel, config->stallmode);
+        dispc_mgr_enable_fifohandcheck(channel, config->fifohandcheck);
+
+        dispc_mgr_set_clock_div(channel, &config->clock_info);
+
+        dispc_mgr_set_tft_data_lines(channel, config->video_port_width);
+
+        dispc_lcd_enable_signal_polarity(config->lcden_sig_polarity);
+
+        dispc_mgr_set_lcd_type_tft(channel);
+}
+EXPORT_SYMBOL(dispc_mgr_set_lcd_config);
+
+static bool _dispc_mgr_size_ok(u16 width, u16 height)
+{
+        return width <= dispc.feat->mgr_width_max &&
+                height <= dispc.feat->mgr_height_max;
+}
+
+static bool _dispc_lcd_timings_ok(int hsw, int hfp, int hbp,
+                int vsw, int vfp, int vbp)
+{
+        if (hsw < 1 || hsw > dispc.feat->sw_max ||
+                        hfp < 1 || hfp > dispc.feat->hp_max ||
+                        hbp < 1 || hbp > dispc.feat->hp_max ||
+                        vsw < 1 || vsw > dispc.feat->sw_max ||
+                        vfp < 0 || vfp > dispc.feat->vp_max ||
+                        vbp < 0 || vbp > dispc.feat->vp_max)
+                return false;
+        return true;
+}
+
+static bool _dispc_mgr_pclk_ok(enum omap_channel channel,
+                unsigned long pclk)
+{
+        if (dss_mgr_is_lcd(channel))
+                return pclk <= dispc.feat->max_lcd_pclk ? true : false;
+        else
+                return pclk <= dispc.feat->max_tv_pclk ? true : false;
+}
+
+bool dispc_mgr_timings_ok(enum omap_channel channel,
+                const struct omap_video_timings *timings)
+{
+        if (!_dispc_mgr_size_ok(timings->x_res, timings->y_res))
+                return false;
+
+        if (!_dispc_mgr_pclk_ok(channel, timings->pixelclock))
+                return false;
+
+        if (dss_mgr_is_lcd(channel)) {
+                /* TODO: OMAP4+ supports interlace for LCD outputs */
+                if (timings->interlace)
+                        return false;
+
+                if (!_dispc_lcd_timings_ok(timings->hsw, timings->hfp,
+                                timings->hbp, timings->vsw, timings->vfp,
+                                timings->vbp))
+                        return false;
+        }
+
+        return true;
+}
+
+static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
+                int hfp, int hbp, int vsw, int vfp, int vbp,
+                enum omap_dss_signal_level vsync_level,
+                enum omap_dss_signal_level hsync_level,
+                enum omap_dss_signal_edge data_pclk_edge,
+                enum omap_dss_signal_level de_level,
+                enum omap_dss_signal_edge sync_pclk_edge)
+
+{
+        u32 timing_h, timing_v, l;
+        bool onoff, rf, ipc, vs, hs, de;
+
+        timing_h = FLD_VAL(hsw-1, dispc.feat->sw_start, 0) |
+                        FLD_VAL(hfp-1, dispc.feat->fp_start, 8) |
+                        FLD_VAL(hbp-1, dispc.feat->bp_start, 20);
+        timing_v = FLD_VAL(vsw-1, dispc.feat->sw_start, 0) |
+                        FLD_VAL(vfp, dispc.feat->fp_start, 8) |
+                        FLD_VAL(vbp, dispc.feat->bp_start, 20);
+
+        dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
+        dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
+
+        switch (vsync_level) {
+        case OMAPDSS_SIG_ACTIVE_LOW:
+                vs = true;
+                break;
+        case OMAPDSS_SIG_ACTIVE_HIGH:
+                vs = false;
+                break;
+        default:
+                BUG();
+        }
+
+        switch (hsync_level) {
+        case OMAPDSS_SIG_ACTIVE_LOW:
+                hs = true;
+                break;
+        case OMAPDSS_SIG_ACTIVE_HIGH:
+                hs = false;
+                break;
+        default:
+                BUG();
+        }
+
+        switch (de_level) {
+        case OMAPDSS_SIG_ACTIVE_LOW:
+                de = true;
+                break;
+        case OMAPDSS_SIG_ACTIVE_HIGH:
+                de = false;
+                break;
+        default:
+                BUG();
+        }
+
+        switch (data_pclk_edge) {
+        case OMAPDSS_DRIVE_SIG_RISING_EDGE:
+                ipc = false;
+                break;
+        case OMAPDSS_DRIVE_SIG_FALLING_EDGE:
+                ipc = true;
+                break;
+        default:
+                BUG();
+        }
+
+        /* always use the 'rf' setting */
+        onoff = true;
+
+        switch (sync_pclk_edge) {
+        case OMAPDSS_DRIVE_SIG_FALLING_EDGE:
+                rf = false;
+                break;
+        case OMAPDSS_DRIVE_SIG_RISING_EDGE:
+                rf = true;
+                break;
+        default:
+                BUG();
+        }
+
+        l = FLD_VAL(onoff, 17, 17) |
+                FLD_VAL(rf, 16, 16) |
+                FLD_VAL(de, 15, 15) |
+                FLD_VAL(ipc, 14, 14) |
+                FLD_VAL(hs, 13, 13) |
+                FLD_VAL(vs, 12, 12);
+
+        /* always set ALIGN bit when available */
+        if (dispc.feat->supports_sync_align)
+                l |= (1 << 18);
+
+        dispc_write_reg(DISPC_POL_FREQ(channel), l);
+
+        if (dispc.syscon_pol) {
+                const int shifts[] = {
+                        [OMAP_DSS_CHANNEL_LCD] = 0,
+                        [OMAP_DSS_CHANNEL_LCD2] = 1,
+                        [OMAP_DSS_CHANNEL_LCD3] = 2,
+                };
+
+                u32 mask, val;
+
+                mask = (1 << 0) | (1 << 3) | (1 << 6);
+                val = (rf << 0) | (ipc << 3) | (onoff << 6);
+
+                mask <<= 16 + shifts[channel];
+                val <<= 16 + shifts[channel];
+
+                regmap_update_bits(dispc.syscon_pol, dispc.syscon_pol_offset,
+                        mask, val);
+        }
+}
+
+/* change name to mode? */
+void dispc_mgr_set_timings(enum omap_channel channel,
+                const struct omap_video_timings *timings)
+{
+        unsigned xtot, ytot;
+        unsigned long ht, vt;
+        struct omap_video_timings t = *timings;
+
+        DSSDBG("channel %d xres %u yres %u\n", channel, t.x_res, t.y_res);
+
+        if (!dispc_mgr_timings_ok(channel, &t)) {
+                BUG();
+                return;
+        }
+
+        if (dss_mgr_is_lcd(channel)) {
+                _dispc_mgr_set_lcd_timings(channel, t.hsw, t.hfp, t.hbp, t.vsw,
+                                t.vfp, t.vbp, t.vsync_level, t.hsync_level,
+                                t.data_pclk_edge, t.de_level, t.sync_pclk_edge);
+
+                xtot = t.x_res + t.hfp + t.hsw + t.hbp;
+                ytot = t.y_res + t.vfp + t.vsw + t.vbp;
+
+                ht = timings->pixelclock / xtot;
+                vt = timings->pixelclock / xtot / ytot;
+
+                DSSDBG("pck %u\n", timings->pixelclock);
+                DSSDBG("hsw %d hfp %d hbp %d vsw %d vfp %d vbp %d\n",
+                        t.hsw, t.hfp, t.hbp, t.vsw, t.vfp, t.vbp);
+                DSSDBG("vsync_level %d hsync_level %d data_pclk_edge %d de_level %d sync_pclk_edge %d\n",
+                        t.vsync_level, t.hsync_level, t.data_pclk_edge,
+                        t.de_level, t.sync_pclk_edge);
+
+                DSSDBG("hsync %luHz, vsync %luHz\n", ht, vt);
+        } else {
+                if (t.interlace)
+                        t.y_res /= 2;
+        }
+
+        dispc_mgr_set_size(channel, t.x_res, t.y_res);
+}
+EXPORT_SYMBOL(dispc_mgr_set_timings);
+
+static void dispc_mgr_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
+                u16 pck_div)
+{
+        BUG_ON(lck_div < 1);
+        BUG_ON(pck_div < 1);
+
+        dispc_write_reg(DISPC_DIVISORo(channel),
+                        FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
+
+        if (!dss_has_feature(FEAT_CORE_CLK_DIV) &&
+                        channel == OMAP_DSS_CHANNEL_LCD)
+                dispc.core_clk_rate = dispc_fclk_rate() / lck_div;
+}
+
+static void dispc_mgr_get_lcd_divisor(enum omap_channel channel, int *lck_div,
+                int *pck_div)
+{
+        u32 l;
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
+        *lck_div = FLD_GET(l, 23, 16);
+        *pck_div = FLD_GET(l, 7, 0);
+}
+
+static unsigned long dispc_fclk_rate(void)
+{
+        struct dss_pll *pll;
+        unsigned long r = 0;
+
+        switch (dss_get_dispc_clk_source()) {
+        case OMAP_DSS_CLK_SRC_FCK:
+                r = dss_get_dispc_clk_rate();
+                break;
+        case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                pll = dss_pll_find("dsi0");
+                if (!pll)
+                        pll = dss_pll_find("video0");
+
+                r = pll->cinfo.clkout[0];
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                pll = dss_pll_find("dsi1");
+                if (!pll)
+                        pll = dss_pll_find("video1");
+
+                r = pll->cinfo.clkout[0];
+                break;
+        default:
+                BUG();
+                return 0;
+        }
+
+        return r;
+}
+
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
+{
+        struct dss_pll *pll;
+        int lcd;
+        unsigned long r;
+        u32 l;
+
+        if (dss_mgr_is_lcd(channel)) {
+                l = dispc_read_reg(DISPC_DIVISORo(channel));
+
+                lcd = FLD_GET(l, 23, 16);
+
+                switch (dss_get_lcd_clk_source(channel)) {
+                case OMAP_DSS_CLK_SRC_FCK:
+                        r = dss_get_dispc_clk_rate();
+                        break;
+                case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                        pll = dss_pll_find("dsi0");
+                        if (!pll)
+                                pll = dss_pll_find("video0");
+
+                        r = pll->cinfo.clkout[0];
+                        break;
+                case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                        pll = dss_pll_find("dsi1");
+                        if (!pll)
+                                pll = dss_pll_find("video1");
+
+                        r = pll->cinfo.clkout[0];
+                        break;
+                default:
+                        BUG();
+                        return 0;
+                }
+
+                return r / lcd;
+        } else {
+                return dispc_fclk_rate();
+        }
+}
+
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
+{
+        unsigned long r;
+
+        if (dss_mgr_is_lcd(channel)) {
+                int pcd;
+                u32 l;
+
+                l = dispc_read_reg(DISPC_DIVISORo(channel));
+
+                pcd = FLD_GET(l, 7, 0);
+
+                r = dispc_mgr_lclk_rate(channel);
+
+                return r / pcd;
+        } else {
+                return dispc.tv_pclk_rate;
+        }
+}
+
+void dispc_set_tv_pclk(unsigned long pclk)
+{
+        dispc.tv_pclk_rate = pclk;
+}
+
+static unsigned long dispc_core_clk_rate(void)
+{
+        return dispc.core_clk_rate;
+}
+
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane)
+{
+        enum omap_channel channel;
+
+        if (plane == OMAP_DSS_WB)
+                return 0;
+
+        channel = dispc_ovl_get_channel_out(plane);
+
+        return dispc_mgr_pclk_rate(channel);
+}
+
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane)
+{
+        enum omap_channel channel;
+
+        if (plane == OMAP_DSS_WB)
+                return 0;
+
+        channel = dispc_ovl_get_channel_out(plane);
+
+        return dispc_mgr_lclk_rate(channel);
+}
+
+static void dispc_dump_clocks_channel(struct seq_file *s, enum omap_channel channel)
+{
+        int lcd, pcd;
+        enum omap_dss_clk_source lcd_clk_src;
+
+        seq_printf(s, "- %s -\n", mgr_desc[channel].name);
+
+        lcd_clk_src = dss_get_lcd_clk_source(channel);
+
+        seq_printf(s, "%s clk source = %s (%s)\n", mgr_desc[channel].name,
+                dss_get_generic_clk_source_name(lcd_clk_src),
+                dss_feat_get_clk_source_name(lcd_clk_src));
+
+        dispc_mgr_get_lcd_divisor(channel, &lcd, &pcd);
+
+        seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+                dispc_mgr_lclk_rate(channel), lcd);
+        seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
+                dispc_mgr_pclk_rate(channel), pcd);
+}
+
+void dispc_dump_clocks(struct seq_file *s)
+{
+        int lcd;
+        u32 l;
+        enum omap_dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
+
+        if (dispc_runtime_get())
+                return;
+
+        seq_printf(s, "- DISPC -\n");
+
+        seq_printf(s, "dispc fclk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(dispc_clk_src),
+                        dss_feat_get_clk_source_name(dispc_clk_src));
+
+        seq_printf(s, "fck\t\t%-16lu\n", dispc_fclk_rate());
+
+        if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+                seq_printf(s, "- DISPC-CORE-CLK -\n");
+                l = dispc_read_reg(DISPC_DIVISOR);
+                lcd = FLD_GET(l, 23, 16);
+
+                seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+                                (dispc_fclk_rate()/lcd), lcd);
+        }
+
+        dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD);
+
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD2);
+        if (dss_has_feature(FEAT_MGR_LCD3))
+                dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD3);
+
+        dispc_runtime_put();
+}
+
+static void dispc_dump_regs(struct seq_file *s)
+{
+        int i, j;
+        const char *mgr_names[] = {
+                [OMAP_DSS_CHANNEL_LCD]          = "LCD",
+                [OMAP_DSS_CHANNEL_DIGIT]        = "TV",
+                [OMAP_DSS_CHANNEL_LCD2]         = "LCD2",
+                [OMAP_DSS_CHANNEL_LCD3]         = "LCD3",
+        };
+        const char *ovl_names[] = {
+                [OMAP_DSS_GFX]          = "GFX",
+                [OMAP_DSS_VIDEO1]       = "VID1",
+                [OMAP_DSS_VIDEO2]       = "VID2",
+                [OMAP_DSS_VIDEO3]       = "VID3",
+                [OMAP_DSS_WB]           = "WB",
+        };
+        const char **p_names;
+
+#define DUMPREG(r) seq_printf(s, "%-50s %08x\n", #r, dispc_read_reg(r))
+
+        if (dispc_runtime_get())
+                return;
+
+        /* DISPC common registers */
+        DUMPREG(DISPC_REVISION);
+        DUMPREG(DISPC_SYSCONFIG);
+        DUMPREG(DISPC_SYSSTATUS);
+        DUMPREG(DISPC_IRQSTATUS);
+        DUMPREG(DISPC_IRQENABLE);
+        DUMPREG(DISPC_CONTROL);
+        DUMPREG(DISPC_CONFIG);
+        DUMPREG(DISPC_CAPABLE);
+        DUMPREG(DISPC_LINE_STATUS);
+        DUMPREG(DISPC_LINE_NUMBER);
+        if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER) ||
+                        dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+                DUMPREG(DISPC_GLOBAL_ALPHA);
+        if (dss_has_feature(FEAT_MGR_LCD2)) {
+                DUMPREG(DISPC_CONTROL2);
+                DUMPREG(DISPC_CONFIG2);
+        }
+        if (dss_has_feature(FEAT_MGR_LCD3)) {
+                DUMPREG(DISPC_CONTROL3);
+                DUMPREG(DISPC_CONFIG3);
+        }
+        if (dss_has_feature(FEAT_MFLAG))
+                DUMPREG(DISPC_GLOBAL_MFLAG_ATTRIBUTE);
+
+#undef DUMPREG
+
+#define DISPC_REG(i, name) name(i)
+#define DUMPREG(i, r) seq_printf(s, "%s(%s)%*s %08x\n", #r, p_names[i], \
+        (int)(48 - strlen(#r) - strlen(p_names[i])), " ", \
+        dispc_read_reg(DISPC_REG(i, r)))
+
+        p_names = mgr_names;
+
+        /* DISPC channel specific registers */
+        for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
+                DUMPREG(i, DISPC_DEFAULT_COLOR);
+                DUMPREG(i, DISPC_TRANS_COLOR);
+                DUMPREG(i, DISPC_SIZE_MGR);
+
+                if (i == OMAP_DSS_CHANNEL_DIGIT)
+                        continue;
+
+                DUMPREG(i, DISPC_TIMING_H);
+                DUMPREG(i, DISPC_TIMING_V);
+                DUMPREG(i, DISPC_POL_FREQ);
+                DUMPREG(i, DISPC_DIVISORo);
+
+                DUMPREG(i, DISPC_DATA_CYCLE1);
+                DUMPREG(i, DISPC_DATA_CYCLE2);
+                DUMPREG(i, DISPC_DATA_CYCLE3);
+
+                if (dss_has_feature(FEAT_CPR)) {
+                        DUMPREG(i, DISPC_CPR_COEF_R);
+                        DUMPREG(i, DISPC_CPR_COEF_G);
+                        DUMPREG(i, DISPC_CPR_COEF_B);
+                }
+        }
+
+        p_names = ovl_names;
+
+        for (i = 0; i < dss_feat_get_num_ovls(); i++) {
+                DUMPREG(i, DISPC_OVL_BA0);
+                DUMPREG(i, DISPC_OVL_BA1);
+                DUMPREG(i, DISPC_OVL_POSITION);
+                DUMPREG(i, DISPC_OVL_SIZE);
+                DUMPREG(i, DISPC_OVL_ATTRIBUTES);
+                DUMPREG(i, DISPC_OVL_FIFO_THRESHOLD);
+                DUMPREG(i, DISPC_OVL_FIFO_SIZE_STATUS);
+                DUMPREG(i, DISPC_OVL_ROW_INC);
+                DUMPREG(i, DISPC_OVL_PIXEL_INC);
+
+                if (dss_has_feature(FEAT_PRELOAD))
+                        DUMPREG(i, DISPC_OVL_PRELOAD);
+                if (dss_has_feature(FEAT_MFLAG))
+                        DUMPREG(i, DISPC_OVL_MFLAG_THRESHOLD);
+
+                if (i == OMAP_DSS_GFX) {
+                        DUMPREG(i, DISPC_OVL_WINDOW_SKIP);
+                        DUMPREG(i, DISPC_OVL_TABLE_BA);
+                        continue;
+                }
+
+                DUMPREG(i, DISPC_OVL_FIR);
+                DUMPREG(i, DISPC_OVL_PICTURE_SIZE);
+                DUMPREG(i, DISPC_OVL_ACCU0);
+                DUMPREG(i, DISPC_OVL_ACCU1);
+                if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                        DUMPREG(i, DISPC_OVL_BA0_UV);
+                        DUMPREG(i, DISPC_OVL_BA1_UV);
+                        DUMPREG(i, DISPC_OVL_FIR2);
+                        DUMPREG(i, DISPC_OVL_ACCU2_0);
+                        DUMPREG(i, DISPC_OVL_ACCU2_1);
+                }
+                if (dss_has_feature(FEAT_ATTR2))
+                        DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
+        }
+
+        if (dispc.feat->has_writeback) {
+                i = OMAP_DSS_WB;
+                DUMPREG(i, DISPC_OVL_BA0);
+                DUMPREG(i, DISPC_OVL_BA1);
+                DUMPREG(i, DISPC_OVL_SIZE);
+                DUMPREG(i, DISPC_OVL_ATTRIBUTES);
+                DUMPREG(i, DISPC_OVL_FIFO_THRESHOLD);
+                DUMPREG(i, DISPC_OVL_FIFO_SIZE_STATUS);
+                DUMPREG(i, DISPC_OVL_ROW_INC);
+                DUMPREG(i, DISPC_OVL_PIXEL_INC);
+
+                if (dss_has_feature(FEAT_MFLAG))
+                        DUMPREG(i, DISPC_OVL_MFLAG_THRESHOLD);
+
+                DUMPREG(i, DISPC_OVL_FIR);
+                DUMPREG(i, DISPC_OVL_PICTURE_SIZE);
+                DUMPREG(i, DISPC_OVL_ACCU0);
+                DUMPREG(i, DISPC_OVL_ACCU1);
+                if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                        DUMPREG(i, DISPC_OVL_BA0_UV);
+                        DUMPREG(i, DISPC_OVL_BA1_UV);
+                        DUMPREG(i, DISPC_OVL_FIR2);
+                        DUMPREG(i, DISPC_OVL_ACCU2_0);
+                        DUMPREG(i, DISPC_OVL_ACCU2_1);
+                }
+                if (dss_has_feature(FEAT_ATTR2))
+                        DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
+        }
+
+#undef DISPC_REG
+#undef DUMPREG
+
+#define DISPC_REG(plane, name, i) name(plane, i)
+#define DUMPREG(plane, name, i) \
+        seq_printf(s, "%s_%d(%s)%*s %08x\n", #name, i, p_names[plane], \
+        (int)(46 - strlen(#name) - strlen(p_names[plane])), " ", \
+        dispc_read_reg(DISPC_REG(plane, name, i)))
+
+        /* Video pipeline coefficient registers */
+
+        /* start from OMAP_DSS_VIDEO1 */
+        for (i = 1; i < dss_feat_get_num_ovls(); i++) {
+                for (j = 0; j < 8; j++)
+                        DUMPREG(i, DISPC_OVL_FIR_COEF_H, j);
+
+                for (j = 0; j < 8; j++)
+                        DUMPREG(i, DISPC_OVL_FIR_COEF_HV, j);
+
+                for (j = 0; j < 5; j++)
+                        DUMPREG(i, DISPC_OVL_CONV_COEF, j);
+
+                if (dss_has_feature(FEAT_FIR_COEF_V)) {
+                        for (j = 0; j < 8; j++)
+                                DUMPREG(i, DISPC_OVL_FIR_COEF_V, j);
+                }
+
+                if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+                        for (j = 0; j < 8; j++)
+                                DUMPREG(i, DISPC_OVL_FIR_COEF_H2, j);
+
+                        for (j = 0; j < 8; j++)
+                                DUMPREG(i, DISPC_OVL_FIR_COEF_HV2, j);
+
+                        for (j = 0; j < 8; j++)
+                                DUMPREG(i, DISPC_OVL_FIR_COEF_V2, j);
+                }
+        }
+
+        dispc_runtime_put();
+
+#undef DISPC_REG
+#undef DUMPREG
+}
+
+/* calculate clock rates using dividers in cinfo */
+int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
+                struct dispc_clock_info *cinfo)
+{
+        if (cinfo->lck_div > 255 || cinfo->lck_div == 0)
+                return -EINVAL;
+        if (cinfo->pck_div < 1 || cinfo->pck_div > 255)
+                return -EINVAL;
+
+        cinfo->lck = dispc_fclk_rate / cinfo->lck_div;
+        cinfo->pck = cinfo->lck / cinfo->pck_div;
+
+        return 0;
+}
+
+bool dispc_div_calc(unsigned long dispc,
+                unsigned long pck_min, unsigned long pck_max,
+                dispc_div_calc_func func, void *data)
+{
+        int lckd, lckd_start, lckd_stop;
+        int pckd, pckd_start, pckd_stop;
+        unsigned long pck, lck;
+        unsigned long lck_max;
+        unsigned long pckd_hw_min, pckd_hw_max;
+        unsigned min_fck_per_pck;
+        unsigned long fck;
+
+#ifdef CONFIG_FB_OMAP2_DSS_MIN_FCK_PER_PCK
+        min_fck_per_pck = CONFIG_FB_OMAP2_DSS_MIN_FCK_PER_PCK;
+#else
+        min_fck_per_pck = 0;
+#endif
+
+        pckd_hw_min = dss_feat_get_param_min(FEAT_PARAM_DSS_PCD);
+        pckd_hw_max = dss_feat_get_param_max(FEAT_PARAM_DSS_PCD);
+
+        lck_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+        pck_min = pck_min ? pck_min : 1;
+        pck_max = pck_max ? pck_max : ULONG_MAX;
+
+        lckd_start = max(DIV_ROUND_UP(dispc, lck_max), 1ul);
+        lckd_stop = min(dispc / pck_min, 255ul);
+
+        for (lckd = lckd_start; lckd <= lckd_stop; ++lckd) {
+                lck = dispc / lckd;
+
+                pckd_start = max(DIV_ROUND_UP(lck, pck_max), pckd_hw_min);
+                pckd_stop = min(lck / pck_min, pckd_hw_max);
+
+                for (pckd = pckd_start; pckd <= pckd_stop; ++pckd) {
+                        pck = lck / pckd;
+
+                        /*
+                         * For OMAP2/3 the DISPC fclk is the same as LCD's logic
+                         * clock, which means we're configuring DISPC fclk here
+                         * also. Thus we need to use the calculated lck. For
+                         * OMAP4+ the DISPC fclk is a separate clock.
+                         */
+                        if (dss_has_feature(FEAT_CORE_CLK_DIV))
+                                fck = dispc_core_clk_rate();
+                        else
+                                fck = lck;
+
+                        if (fck < pck * min_fck_per_pck)
+                                continue;
+
+                        if (func(lckd, pckd, lck, pck, data))
+                                return true;
+                }
+        }
+
+        return false;
+}
+
+void dispc_mgr_set_clock_div(enum omap_channel channel,
+                const 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_mgr_set_lcd_divisor(channel, cinfo->lck_div, cinfo->pck_div);
+}
+
+int dispc_mgr_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_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;
+
+        return 0;
+}
+
+u32 dispc_read_irqstatus(void)
+{
+        return dispc_read_reg(DISPC_IRQSTATUS);
+}
+EXPORT_SYMBOL(dispc_read_irqstatus);
+
+void dispc_clear_irqstatus(u32 mask)
+{
+        dispc_write_reg(DISPC_IRQSTATUS, mask);
+}
+EXPORT_SYMBOL(dispc_clear_irqstatus);
+
+u32 dispc_read_irqenable(void)
+{
+        return dispc_read_reg(DISPC_IRQENABLE);
+}
+EXPORT_SYMBOL(dispc_read_irqenable);
+
+void dispc_write_irqenable(u32 mask)
+{
+        u32 old_mask = dispc_read_reg(DISPC_IRQENABLE);
+
+        /* clear the irqstatus for newly enabled irqs */
+        dispc_clear_irqstatus((mask ^ old_mask) & mask);
+
+        dispc_write_reg(DISPC_IRQENABLE, mask);
+}
+EXPORT_SYMBOL(dispc_write_irqenable);
+
+void dispc_enable_sidle(void)
+{
+        REG_FLD_MOD(DISPC_SYSCONFIG, 2, 4, 3);  /* SIDLEMODE: smart idle */
+}
+
+void dispc_disable_sidle(void)
+{
+        REG_FLD_MOD(DISPC_SYSCONFIG, 1, 4, 3);  /* SIDLEMODE: no idle */
+}
+
+static void _omap_dispc_initial_config(void)
+{
+        u32 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);
+
+                dispc.core_clk_rate = dispc_fclk_rate();
+        }
+
+        /* FUNCGATED */
+        if (dss_has_feature(FEAT_FUNCGATED))
+                REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
+
+        dispc_setup_color_conv_coef();
+
+        dispc_set_loadmode(OMAP_DSS_LOAD_FRAME_ONLY);
+
+        dispc_init_fifos();
+
+        dispc_configure_burst_sizes();
+
+        dispc_ovl_enable_zorder_planes();
+
+        if (dispc.feat->mstandby_workaround)
+                REG_FLD_MOD(DISPC_MSTANDBY_CTRL, 1, 0, 0);
+
+        if (dss_has_feature(FEAT_MFLAG))
+                dispc_init_mflag();
+}
+
+static const struct dispc_features omap24xx_dispc_feats = {
+        .sw_start               =       5,
+        .fp_start               =       15,
+        .bp_start               =       27,
+        .sw_max                 =       64,
+        .vp_max                 =       255,
+        .hp_max                 =       256,
+        .mgr_width_start        =       10,
+        .mgr_height_start       =       26,
+        .mgr_width_max          =       2048,
+        .mgr_height_max         =       2048,
+        .max_lcd_pclk           =       66500000,
+        .calc_scaling           =       dispc_ovl_calc_scaling_24xx,
+        .calc_core_clk          =       calc_core_clk_24xx,
+        .num_fifos              =       3,
+        .no_framedone_tv        =       true,
+        .set_max_preload        =       false,
+        .last_pixel_inc_missing =       true,
+};
+
+static const struct dispc_features omap34xx_rev1_0_dispc_feats = {
+        .sw_start               =       5,
+        .fp_start               =       15,
+        .bp_start               =       27,
+        .sw_max                 =       64,
+        .vp_max                 =       255,
+        .hp_max                 =       256,
+        .mgr_width_start        =       10,
+        .mgr_height_start       =       26,
+        .mgr_width_max          =       2048,
+        .mgr_height_max         =       2048,
+        .max_lcd_pclk           =       173000000,
+        .max_tv_pclk            =       59000000,
+        .calc_scaling           =       dispc_ovl_calc_scaling_34xx,
+        .calc_core_clk          =       calc_core_clk_34xx,
+        .num_fifos              =       3,
+        .no_framedone_tv        =       true,
+        .set_max_preload        =       false,
+        .last_pixel_inc_missing =       true,
+};
+
+static const struct dispc_features omap34xx_rev3_0_dispc_feats = {
+        .sw_start               =       7,
+        .fp_start               =       19,
+        .bp_start               =       31,
+        .sw_max                 =       256,
+        .vp_max                 =       4095,
+        .hp_max                 =       4096,
+        .mgr_width_start        =       10,
+        .mgr_height_start       =       26,
+        .mgr_width_max          =       2048,
+        .mgr_height_max         =       2048,
+        .max_lcd_pclk           =       173000000,
+        .max_tv_pclk            =       59000000,
+        .calc_scaling           =       dispc_ovl_calc_scaling_34xx,
+        .calc_core_clk          =       calc_core_clk_34xx,
+        .num_fifos              =       3,
+        .no_framedone_tv        =       true,
+        .set_max_preload        =       false,
+        .last_pixel_inc_missing =       true,
+};
+
+static const struct dispc_features omap44xx_dispc_feats = {
+        .sw_start               =       7,
+        .fp_start               =       19,
+        .bp_start               =       31,
+        .sw_max                 =       256,
+        .vp_max                 =       4095,
+        .hp_max                 =       4096,
+        .mgr_width_start        =       10,
+        .mgr_height_start       =       26,
+        .mgr_width_max          =       2048,
+        .mgr_height_max         =       2048,
+        .max_lcd_pclk           =       170000000,
+        .max_tv_pclk            =       185625000,
+        .calc_scaling           =       dispc_ovl_calc_scaling_44xx,
+        .calc_core_clk          =       calc_core_clk_44xx,
+        .num_fifos              =       5,
+        .gfx_fifo_workaround    =       true,
+        .set_max_preload        =       true,
+        .supports_sync_align    =       true,
+        .has_writeback          =       true,
+};
+
+static const struct dispc_features omap54xx_dispc_feats = {
+        .sw_start               =       7,
+        .fp_start               =       19,
+        .bp_start               =       31,
+        .sw_max                 =       256,
+        .vp_max                 =       4095,
+        .hp_max                 =       4096,
+        .mgr_width_start        =       11,
+        .mgr_height_start       =       27,
+        .mgr_width_max          =       4096,
+        .mgr_height_max         =       4096,
+        .max_lcd_pclk           =       170000000,
+        .max_tv_pclk            =       186000000,
+        .calc_scaling           =       dispc_ovl_calc_scaling_44xx,
+        .calc_core_clk          =       calc_core_clk_44xx,
+        .num_fifos              =       5,
+        .gfx_fifo_workaround    =       true,
+        .mstandby_workaround    =       true,
+        .set_max_preload        =       true,
+        .supports_sync_align    =       true,
+        .has_writeback          =       true,
+};
+
+static int dispc_init_features(struct platform_device *pdev)
+{
+        const struct dispc_features *src;
+        struct dispc_features *dst;
+
+        dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+        if (!dst) {
+                dev_err(&pdev->dev, "Failed to allocate DISPC Features\n");
+                return -ENOMEM;
+        }
+
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP24xx:
+                src = &omap24xx_dispc_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP34xx_ES1:
+                src = &omap34xx_rev1_0_dispc_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP34xx_ES3:
+        case OMAPDSS_VER_OMAP3630:
+        case OMAPDSS_VER_AM35xx:
+        case OMAPDSS_VER_AM43xx:
+                src = &omap34xx_rev3_0_dispc_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                src = &omap44xx_dispc_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP5:
+        case OMAPDSS_VER_DRA7xx:
+                src = &omap54xx_dispc_feats;
+                break;
+
+        default:
+                return -ENODEV;
+        }
+
+        memcpy(dst, src, sizeof(*dst));
+        dispc.feat = dst;
+
+        return 0;
+}
+
+static irqreturn_t dispc_irq_handler(int irq, void *arg)
+{
+        if (!dispc.is_enabled)
+                return IRQ_NONE;
+
+        return dispc.user_handler(irq, dispc.user_data);
+}
+
+int dispc_request_irq(irq_handler_t handler, void *dev_id)
+{
+        int r;
+
+        if (dispc.user_handler != NULL)
+                return -EBUSY;
+
+        dispc.user_handler = handler;
+        dispc.user_data = dev_id;
+
+        /* ensure the dispc_irq_handler sees the values above */
+        smp_wmb();
+
+        r = devm_request_irq(&dispc.pdev->dev, dispc.irq, dispc_irq_handler,
+                             IRQF_SHARED, "OMAP DISPC", &dispc);
+        if (r) {
+                dispc.user_handler = NULL;
+                dispc.user_data = NULL;
+        }
+
+        return r;
+}
+EXPORT_SYMBOL(dispc_request_irq);
+
+void dispc_free_irq(void *dev_id)
+{
+        devm_free_irq(&dispc.pdev->dev, dispc.irq, &dispc);
+
+        dispc.user_handler = NULL;
+        dispc.user_data = NULL;
+}
+EXPORT_SYMBOL(dispc_free_irq);
+
+/* DISPC HW IP initialisation */
+static int dispc_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        u32 rev;
+        int r = 0;
+        struct resource *dispc_mem;
+        struct device_node *np = pdev->dev.of_node;
+
+        dispc.pdev = pdev;
+
+        spin_lock_init(&dispc.control_lock);
+
+        r = dispc_init_features(dispc.pdev);
+        if (r)
+                return r;
+
+        dispc_mem = platform_get_resource(dispc.pdev, IORESOURCE_MEM, 0);
+        if (!dispc_mem) {
+                DSSERR("can't get IORESOURCE_MEM DISPC\n");
+                return -EINVAL;
+        }
+
+        dispc.base = devm_ioremap(&pdev->dev, dispc_mem->start,
+                                  resource_size(dispc_mem));
+        if (!dispc.base) {
+                DSSERR("can't ioremap DISPC\n");
+                return -ENOMEM;
+        }
+
+        dispc.irq = platform_get_irq(dispc.pdev, 0);
+        if (dispc.irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                return -ENODEV;
+        }
+
+        if (np && of_property_read_bool(np, "syscon-pol")) {
+                dispc.syscon_pol = syscon_regmap_lookup_by_phandle(np, "syscon-pol");
+                if (IS_ERR(dispc.syscon_pol)) {
+                        dev_err(&pdev->dev, "failed to get syscon-pol regmap\n");
+                        return PTR_ERR(dispc.syscon_pol);
+                }
+
+                if (of_property_read_u32_index(np, "syscon-pol", 1,
+                                &dispc.syscon_pol_offset)) {
+                        dev_err(&pdev->dev, "failed to get syscon-pol offset\n");
+                        return -EINVAL;
+                }
+        }
+
+        pm_runtime_enable(&pdev->dev);
+
+        r = dispc_runtime_get();
+        if (r)
+                goto err_runtime_get;
+
+        _omap_dispc_initial_config();
+
+        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));
+
+        dispc_runtime_put();
+
+        dss_init_overlay_managers();
+
+        dss_debugfs_create_file("dispc", dispc_dump_regs);
+
+        return 0;
+
+err_runtime_get:
+        pm_runtime_disable(&pdev->dev);
+        return r;
+}
+
+static void dispc_unbind(struct device *dev, struct device *master,
+                               void *data)
+{
+        pm_runtime_disable(dev);
+
+        dss_uninit_overlay_managers();
+}
+
+static const struct component_ops dispc_component_ops = {
+        .bind   = dispc_bind,
+        .unbind = dispc_unbind,
+};
+
+static int dispc_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &dispc_component_ops);
+}
+
+static int dispc_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &dispc_component_ops);
+        return 0;
+}
+
+static int dispc_runtime_suspend(struct device *dev)
+{
+        dispc.is_enabled = false;
+        /* ensure the dispc_irq_handler sees the is_enabled value */
+        smp_wmb();
+        /* wait for current handler to finish before turning the DISPC off */
+        synchronize_irq(dispc.irq);
+
+        dispc_save_context();
+
+        return 0;
+}
+
+static int dispc_runtime_resume(struct device *dev)
+{
+        /*
+         * The reset value for load mode is 0 (OMAP_DSS_LOAD_CLUT_AND_FRAME)
+         * but we always initialize it to 2 (OMAP_DSS_LOAD_FRAME_ONLY) in
+         * _omap_dispc_initial_config(). We can thus use it to detect if
+         * we have lost register context.
+         */
+        if (REG_GET(DISPC_CONFIG, 2, 1) != OMAP_DSS_LOAD_FRAME_ONLY) {
+                _omap_dispc_initial_config();
+
+                dispc_restore_context();
+        }
+
+        dispc.is_enabled = true;
+        /* ensure the dispc_irq_handler sees the is_enabled value */
+        smp_wmb();
+
+        return 0;
+}
+
+static const struct dev_pm_ops dispc_pm_ops = {
+        .runtime_suspend = dispc_runtime_suspend,
+        .runtime_resume = dispc_runtime_resume,
+};
+
+static const struct of_device_id dispc_of_match[] = {
+        { .compatible = "ti,omap2-dispc", },
+        { .compatible = "ti,omap3-dispc", },
+        { .compatible = "ti,omap4-dispc", },
+        { .compatible = "ti,omap5-dispc", },
+        { .compatible = "ti,dra7-dispc", },
+        {},
+};
+
+static struct platform_driver omap_dispchw_driver = {
+        .probe          = dispc_probe,
+        .remove         = dispc_remove,
+        .driver         = {
+                .name   = "omapdss_dispc",
+                .pm     = &dispc_pm_ops,
+                .of_match_table = dispc_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init dispc_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dispchw_driver);
+}
+
+void dispc_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_dispchw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc.h b/drivers/video/fbdev/omap2/omapfb/dss/dispc.h
new file mode 100644
index 000000000000..483744223dd1
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc.h
@@ -0,0 +1,918 @@
+/*
+ * 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
+#define DISPC_GLOBAL_BUFFER             0x0800
+#define DISPC_CONTROL3                  0x0848
+#define DISPC_CONFIG3                   0x084C
+#define DISPC_MSTANDBY_CTRL             0x0858
+#define DISPC_GLOBAL_MFLAG_ATTRIBUTE    0x085C
+
+/* 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))
+#define DISPC_OVL_MFLAG_THRESHOLD(n)    DISPC_MFLAG_THRESHOLD_OFFSET(n)
+
+/* DISPC up/downsampling FIR filter coefficient structure */
+struct dispc_coef {
+        s8 hc4_vc22;
+        s8 hc3_vc2;
+        u8 hc2_vc1;
+        s8 hc1_vc0;
+        s8 hc0_vc00;
+};
+
+const struct dispc_coef *dispc_ovl_get_scale_coef(int inc, int five_taps);
+
+/* 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;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0814;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0818;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x0400;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0840;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x0404;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0844;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x0408;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x083C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_DIVISORo(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return 0x0070;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                BUG();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x040C;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0838;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0834;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03C0;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0828;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03C4;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x082C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03C8;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0830;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03BC;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0824;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03B8;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x0820;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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();
+                return 0;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return 0x03B4;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return 0x081C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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;
+        case OMAP_DSS_VIDEO3:
+                return 0x0300;
+        case OMAP_DSS_WB:
+                return 0x0500;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0008;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x000C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_BA0_UV_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0544;
+        case OMAP_DSS_VIDEO2:
+                return 0x04BC;
+        case OMAP_DSS_VIDEO3:
+                return 0x0310;
+        case OMAP_DSS_WB:
+                return 0x0118;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_BA1_UV_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0548;
+        case OMAP_DSS_VIDEO2:
+                return 0x04C0;
+        case OMAP_DSS_VIDEO3:
+                return 0x0314;
+        case OMAP_DSS_WB:
+                return 0x011C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+                return 0x009C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x00A8;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0070;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_ATTR2_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0568;
+        case OMAP_DSS_VIDEO2:
+                return 0x04DC;
+        case OMAP_DSS_VIDEO3:
+                return 0x032C;
+        case OMAP_DSS_WB:
+                return 0x0310;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x008C;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0088;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x00A4;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0098;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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:
+        case OMAP_DSS_VIDEO3:
+                BUG();
+                return 0;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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:
+        case OMAP_DSS_VIDEO3:
+                BUG();
+                return 0;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_FIR_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0024;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0090;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_FIR2_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0580;
+        case OMAP_DSS_VIDEO2:
+                return 0x055C;
+        case OMAP_DSS_VIDEO3:
+                return 0x0424;
+        case OMAP_DSS_WB:
+                return 0x290;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_PIC_SIZE_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0028;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0094;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+
+static inline u16 DISPC_ACCU0_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x002C;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0000;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_ACCU2_0_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0584;
+        case OMAP_DSS_VIDEO2:
+                return 0x0560;
+        case OMAP_DSS_VIDEO3:
+                return 0x0428;
+        case OMAP_DSS_WB:
+                return 0x0294;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_ACCU1_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0030;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0004;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_ACCU2_1_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                BUG();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0588;
+        case OMAP_DSS_VIDEO2:
+                return 0x0564;
+        case OMAP_DSS_VIDEO3:
+                return 0x042C;
+        case OMAP_DSS_WB:
+                return 0x0298;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0034 + i * 0x8;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0010 + i * 0x8;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x058C + i * 0x8;
+        case OMAP_DSS_VIDEO2:
+                return 0x0568 + i * 0x8;
+        case OMAP_DSS_VIDEO3:
+                return 0x0430 + i * 0x8;
+        case OMAP_DSS_WB:
+                return 0x02A0 + i * 0x8;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+                return 0x0038 + i * 0x8;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0014 + i * 0x8;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0590 + i * 8;
+        case OMAP_DSS_VIDEO2:
+                return 0x056C + i * 0x8;
+        case OMAP_DSS_VIDEO3:
+                return 0x0434 + i * 0x8;
+        case OMAP_DSS_WB:
+                return 0x02A4 + i * 0x8;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+        case OMAP_DSS_VIDEO2:
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0074 + i * 0x4;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x0124 + i * 0x4;
+        case OMAP_DSS_VIDEO2:
+                return 0x00B4 + i * 0x4;
+        case OMAP_DSS_VIDEO3:
+        case OMAP_DSS_WB:
+                return 0x0050 + i * 0x4;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+/* 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();
+                return 0;
+        case OMAP_DSS_VIDEO1:
+                return 0x05CC + i * 0x4;
+        case OMAP_DSS_VIDEO2:
+                return 0x05A8 + i * 0x4;
+        case OMAP_DSS_VIDEO3:
+                return 0x0470 + i * 0x4;
+        case OMAP_DSS_WB:
+                return 0x02E0 + i * 0x4;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+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;
+        case OMAP_DSS_VIDEO3:
+                return 0x00A0;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static inline u16 DISPC_MFLAG_THRESHOLD_OFFSET(enum omap_plane plane)
+{
+        switch (plane) {
+        case OMAP_DSS_GFX:
+                return 0x0860;
+        case OMAP_DSS_VIDEO1:
+                return 0x0864;
+        case OMAP_DSS_VIDEO2:
+                return 0x0868;
+        case OMAP_DSS_VIDEO3:
+                return 0x086c;
+        case OMAP_DSS_WB:
+                return 0x0870;
+        default:
+                BUG();
+                return 0;
+        }
+}
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc_coefs.c b/drivers/video/fbdev/omap2/omapfb/dss/dispc_coefs.c
new file mode 100644
index 000000000000..038c15b04215
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc_coefs.c
@@ -0,0 +1,325 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc_coefs.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Chandrabhanu Mahapatra <cmahapatra@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 <video/omapdss.h>
+
+#include "dispc.h"
+
+static const struct dispc_coef coef3_M8[8] = {
+        { 0,  0, 128,  0, 0 },
+        { 0, -4, 123,  9, 0 },
+        { 0, -4, 108, 24, 0 },
+        { 0, -2,  87, 43, 0 },
+        { 0, 64,  64,  0, 0 },
+        { 0, 43,  87, -2, 0 },
+        { 0, 24, 108, -4, 0 },
+        { 0,  9, 123, -4, 0 },
+};
+
+static const struct dispc_coef coef3_M9[8] = {
+        { 0,  6, 116,  6, 0 },
+        { 0,  0, 112, 16, 0 },
+        { 0, -2, 100, 30, 0 },
+        { 0, -2,  83, 47, 0 },
+        { 0, 64,  64,  0, 0 },
+        { 0, 47,  83, -2, 0 },
+        { 0, 30, 100, -2, 0 },
+        { 0, 16, 112,  0, 0 },
+};
+
+static const struct dispc_coef coef3_M10[8] = {
+        { 0, 10, 108, 10, 0 },
+        { 0,  3, 104, 21, 0 },
+        { 0,  0,  94, 34, 0 },
+        { 0, -1,  80, 49, 0 },
+        { 0, 64,  64,  0, 0 },
+        { 0, 49,  80, -1, 0 },
+        { 0, 34,  94,  0, 0 },
+        { 0, 21, 104,  3, 0 },
+};
+
+static const struct dispc_coef coef3_M11[8] = {
+        { 0, 14, 100, 14, 0 },
+        { 0,  6,  98, 24, 0 },
+        { 0,  2,  90, 36, 0 },
+        { 0,  0,  78, 50, 0 },
+        { 0, 64,  64,  0, 0 },
+        { 0, 50,  78,  0, 0 },
+        { 0, 36,  90,  2, 0 },
+        { 0, 24,  98,  6, 0 },
+};
+
+static const struct dispc_coef coef3_M12[8] = {
+        { 0, 16,  96, 16, 0 },
+        { 0,  9,  93, 26, 0 },
+        { 0,  4,  86, 38, 0 },
+        { 0,  1,  76, 51, 0 },
+        { 0, 64,  64,  0, 0 },
+        { 0, 51,  76,  1, 0 },
+        { 0, 38,  86,  4, 0 },
+        { 0, 26,  93,  9, 0 },
+};
+
+static const struct dispc_coef coef3_M13[8] = {
+        { 0, 18,  92, 18, 0 },
+        { 0, 10,  90, 28, 0 },
+        { 0,  5,  83, 40, 0 },
+        { 0,  1,  75, 52, 0 },
+        { 0, 64,  64,  0, 0 },
+        { 0, 52,  75,  1, 0 },
+        { 0, 40,  83,  5, 0 },
+        { 0, 28,  90, 10, 0 },
+};
+
+static const struct dispc_coef coef3_M14[8] = {
+        { 0, 20, 88, 20, 0 },
+        { 0, 12, 86, 30, 0 },
+        { 0,  6, 81, 41, 0 },
+        { 0,  2, 74, 52, 0 },
+        { 0, 64, 64,  0, 0 },
+        { 0, 52, 74,  2, 0 },
+        { 0, 41, 81,  6, 0 },
+        { 0, 30, 86, 12, 0 },
+};
+
+static const struct dispc_coef coef3_M16[8] = {
+        { 0, 22, 84, 22, 0 },
+        { 0, 14, 82, 32, 0 },
+        { 0,  8, 78, 42, 0 },
+        { 0,  3, 72, 53, 0 },
+        { 0, 64, 64,  0, 0 },
+        { 0, 53, 72,  3, 0 },
+        { 0, 42, 78,  8, 0 },
+        { 0, 32, 82, 14, 0 },
+};
+
+static const struct dispc_coef coef3_M19[8] = {
+        { 0, 24, 80, 24, 0 },
+        { 0, 16, 79, 33, 0 },
+        { 0,  9, 76, 43, 0 },
+        { 0,  4, 70, 54, 0 },
+        { 0, 64, 64,  0, 0 },
+        { 0, 54, 70,  4, 0 },
+        { 0, 43, 76,  9, 0 },
+        { 0, 33, 79, 16, 0 },
+};
+
+static const struct dispc_coef coef3_M22[8] = {
+        { 0, 25, 78, 25, 0 },
+        { 0, 17, 77, 34, 0 },
+        { 0, 10, 74, 44, 0 },
+        { 0,  5, 69, 54, 0 },
+        { 0, 64, 64,  0, 0 },
+        { 0, 54, 69,  5, 0 },
+        { 0, 44, 74, 10, 0 },
+        { 0, 34, 77, 17, 0 },
+};
+
+static const struct dispc_coef coef3_M26[8] = {
+        { 0, 26, 76, 26, 0 },
+        { 0, 19, 74, 35, 0 },
+        { 0, 11, 72, 45, 0 },
+        { 0,  5, 69, 54, 0 },
+        { 0, 64, 64,  0, 0 },
+        { 0, 54, 69,  5, 0 },
+        { 0, 45, 72, 11, 0 },
+        { 0, 35, 74, 19, 0 },
+};
+
+static const struct dispc_coef coef3_M32[8] = {
+        { 0, 27, 74, 27, 0 },
+        { 0, 19, 73, 36, 0 },
+        { 0, 12, 71, 45, 0 },
+        { 0,  6, 68, 54, 0 },
+        { 0, 64, 64,  0, 0 },
+        { 0, 54, 68,  6, 0 },
+        { 0, 45, 71, 12, 0 },
+        { 0, 36, 73, 19, 0 },
+};
+
+static const struct dispc_coef coef5_M8[8] = {
+        {   0,   0, 128,   0,   0 },
+        {  -2,  14, 125, -10,   1 },
+        {  -6,  33, 114, -15,   2 },
+        { -10,  55,  98, -16,   1 },
+        {   0, -14,  78,  78, -14 },
+        {   1, -16,  98,  55, -10 },
+        {   2, -15, 114,  33,  -6 },
+        {   1, -10, 125,  14,  -2 },
+};
+
+static const struct dispc_coef coef5_M9[8] = {
+        {  -3,  10, 114,  10,  -3 },
+        {  -6,  24, 111,   0,  -1 },
+        {  -8,  40, 103,  -7,   0 },
+        { -11,  58,  91, -11,   1 },
+        {   0, -12,  76,  76, -12 },
+        {   1, -11,  91,  58, -11 },
+        {   0,  -7, 103,  40,  -8 },
+        {  -1,   0, 111,  24,  -6 },
+};
+
+static const struct dispc_coef coef5_M10[8] = {
+        {  -4,  18, 100,  18,  -4 },
+        {  -6,  30,  99,   8,  -3 },
+        {  -8,  44,  93,   0,  -1 },
+        {  -9,  58,  84,  -5,   0 },
+        {   0,  -8,  72,  72,  -8 },
+        {   0,  -5,  84,  58,  -9 },
+        {  -1,   0,  93,  44,  -8 },
+        {  -3,   8,  99,  30,  -6 },
+};
+
+static const struct dispc_coef coef5_M11[8] = {
+        {  -5,  23,  92,  23,  -5 },
+        {  -6,  34,  90,  13,  -3 },
+        {  -6,  45,  85,   6,  -2 },
+        {  -6,  57,  78,   0,  -1 },
+        {   0,  -4,  68,  68,  -4 },
+        {  -1,   0,  78,  57,  -6 },
+        {  -2,   6,  85,  45,  -6 },
+        {  -3,  13,  90,  34,  -6 },
+};
+
+static const struct dispc_coef coef5_M12[8] = {
+        {  -4,  26,  84,  26,  -4 },
+        {  -5,  36,  82,  18,  -3 },
+        {  -4,  46,  78,  10,  -2 },
+        {  -3,  55,  72,   5,  -1 },
+        {   0,   0,  64,  64,   0 },
+        {  -1,   5,  72,  55,  -3 },
+        {  -2,  10,  78,  46,  -4 },
+        {  -3,  18,  82,  36,  -5 },
+};
+
+static const struct dispc_coef coef5_M13[8] = {
+        {  -3,  28,  78,  28,  -3 },
+        {  -3,  37,  76,  21,  -3 },
+        {  -2,  45,  73,  14,  -2 },
+        {   0,  53,  68,   8,  -1 },
+        {   0,   3,  61,  61,   3 },
+        {  -1,   8,  68,  53,   0 },
+        {  -2,  14,  73,  45,  -2 },
+        {  -3,  21,  76,  37,  -3 },
+};
+
+static const struct dispc_coef coef5_M14[8] = {
+        {  -2,  30,  72,  30,  -2 },
+        {  -1,  37,  71,  23,  -2 },
+        {   0,  45,  69,  16,  -2 },
+        {   3,  52,  64,  10,  -1 },
+        {   0,   6,  58,  58,   6 },
+        {  -1,  10,  64,  52,   3 },
+        {  -2,  16,  69,  45,   0 },
+        {  -2,  23,  71,  37,  -1 },
+};
+
+static const struct dispc_coef coef5_M16[8] = {
+        {   0,  31,  66,  31,   0 },
+        {   1,  38,  65,  25,  -1 },
+        {   3,  44,  62,  20,  -1 },
+        {   6,  49,  59,  14,   0 },
+        {   0,  10,  54,  54,  10 },
+        {   0,  14,  59,  49,   6 },
+        {  -1,  20,  62,  44,   3 },
+        {  -1,  25,  65,  38,   1 },
+};
+
+static const struct dispc_coef coef5_M19[8] = {
+        {   3,  32,  58,  32,   3 },
+        {   4,  38,  58,  27,   1 },
+        {   7,  42,  55,  23,   1 },
+        {  10,  46,  54,  18,   0 },
+        {   0,  14,  50,  50,  14 },
+        {   0,  18,  54,  46,  10 },
+        {   1,  23,  55,  42,   7 },
+        {   1,  27,  58,  38,   4 },
+};
+
+static const struct dispc_coef coef5_M22[8] = {
+        {   4,  33,  54,  33,   4 },
+        {   6,  37,  54,  28,   3 },
+        {   9,  41,  53,  24,   1 },
+        {  12,  45,  51,  20,   0 },
+        {   0,  16,  48,  48,  16 },
+        {   0,  20,  51,  45,  12 },
+        {   1,  24,  53,  41,   9 },
+        {   3,  28,  54,  37,   6 },
+};
+
+static const struct dispc_coef coef5_M26[8] = {
+        {   6,  33,  50,  33,   6 },
+        {   8,  36,  51,  29,   4 },
+        {  11,  40,  50,  25,   2 },
+        {  14,  43,  48,  22,   1 },
+        {   0,  18,  46,  46,  18 },
+        {   1,  22,  48,  43,  14 },
+        {   2,  25,  50,  40,  11 },
+        {   4,  29,  51,  36,   8 },
+};
+
+static const struct dispc_coef coef5_M32[8] = {
+        {   7,  33,  48,  33,   7 },
+        {  10,  36,  48,  29,   5 },
+        {  13,  39,  47,  26,   3 },
+        {  16,  42,  46,  23,   1 },
+        {   0,  19,  45,  45,  19 },
+        {   1,  23,  46,  42,  16 },
+        {   3,  26,  47,  39,  13 },
+        {   5,  29,  48,  36,  10 },
+};
+
+const struct dispc_coef *dispc_ovl_get_scale_coef(int inc, int five_taps)
+{
+        int i;
+        static const struct {
+                int Mmin;
+                int Mmax;
+                const struct dispc_coef *coef_3;
+                const struct dispc_coef *coef_5;
+        } coefs[] = {
+                { 27, 32, coef3_M32, coef5_M32 },
+                { 23, 26, coef3_M26, coef5_M26 },
+                { 20, 22, coef3_M22, coef5_M22 },
+                { 17, 19, coef3_M19, coef5_M19 },
+                { 15, 16, coef3_M16, coef5_M16 },
+                { 14, 14, coef3_M14, coef5_M14 },
+                { 13, 13, coef3_M13, coef5_M13 },
+                { 12, 12, coef3_M12, coef5_M12 },
+                { 11, 11, coef3_M11, coef5_M11 },
+                { 10, 10, coef3_M10, coef5_M10 },
+                {  9,  9,  coef3_M9,  coef5_M9 },
+                {  4,  8,  coef3_M8,  coef5_M8 },
+                /*
+                 * When upscaling more than two times, blockiness and outlines
+                 * around the image are observed when M8 tables are used. M11,
+                 * M16 and M19 tables are used to prevent this.
+                 */
+                {  3,  3, coef3_M11, coef5_M11 },
+                {  2,  2, coef3_M16, coef5_M16 },
+                {  0,  1, coef3_M19, coef5_M19 },
+        };
+
+        inc /= 128;
+        for (i = 0; i < ARRAY_SIZE(coefs); ++i)
+                if (inc >= coefs[i].Mmin && inc <= coefs[i].Mmax)
+                        return five_taps ? coefs[i].coef_5 : coefs[i].coef_3;
+        return NULL;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/display-sysfs.c b/drivers/video/fbdev/omap2/omapfb/dss/display-sysfs.c
new file mode 100644
index 000000000000..75b5286029ee
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/display-sysfs.c
@@ -0,0 +1,356 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "DISPLAY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sysfs.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+
+static ssize_t display_name_show(struct omap_dss_device *dssdev, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n",
+                        dssdev->name ?
+                        dssdev->name : "");
+}
+
+static ssize_t display_enabled_show(struct omap_dss_device *dssdev, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        omapdss_device_is_enabled(dssdev));
+}
+
+static ssize_t display_enabled_store(struct omap_dss_device *dssdev,
+                const char *buf, size_t size)
+{
+        int r;
+        bool enable;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        if (enable == omapdss_device_is_enabled(dssdev))
+                return size;
+
+        if (omapdss_device_is_connected(dssdev) == false)
+                return -ENODEV;
+
+        if (enable) {
+                r = dssdev->driver->enable(dssdev);
+                if (r)
+                        return r;
+        } else {
+                dssdev->driver->disable(dssdev);
+        }
+
+        return size;
+}
+
+static ssize_t display_tear_show(struct omap_dss_device *dssdev, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        dssdev->driver->get_te ?
+                        dssdev->driver->get_te(dssdev) : 0);
+}
+
+static ssize_t display_tear_store(struct omap_dss_device *dssdev,
+        const char *buf, size_t size)
+{
+        int r;
+        bool te;
+
+        if (!dssdev->driver->enable_te || !dssdev->driver->get_te)
+                return -ENOENT;
+
+        r = strtobool(buf, &te);
+        if (r)
+                return r;
+
+        r = dssdev->driver->enable_te(dssdev, te);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t display_timings_show(struct omap_dss_device *dssdev, char *buf)
+{
+        struct omap_video_timings t;
+
+        if (!dssdev->driver->get_timings)
+                return -ENOENT;
+
+        dssdev->driver->get_timings(dssdev, &t);
+
+        return snprintf(buf, PAGE_SIZE, "%u,%u/%u/%u/%u,%u/%u/%u/%u\n",
+                        t.pixelclock,
+                        t.x_res, t.hfp, t.hbp, t.hsw,
+                        t.y_res, t.vfp, t.vbp, t.vsw);
+}
+
+static ssize_t display_timings_store(struct omap_dss_device *dssdev,
+        const char *buf, size_t size)
+{
+        struct omap_video_timings t = dssdev->panel.timings;
+        int r, found;
+
+        if (!dssdev->driver->set_timings || !dssdev->driver->check_timings)
+                return -ENOENT;
+
+        found = 0;
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+        if (strncmp("pal", buf, 3) == 0) {
+                t = omap_dss_pal_timings;
+                found = 1;
+        } else if (strncmp("ntsc", buf, 4) == 0) {
+                t = omap_dss_ntsc_timings;
+                found = 1;
+        }
+#endif
+        if (!found && sscanf(buf, "%u,%hu/%hu/%hu/%hu,%hu/%hu/%hu/%hu",
+                                &t.pixelclock,
+                                &t.x_res, &t.hfp, &t.hbp, &t.hsw,
+                                &t.y_res, &t.vfp, &t.vbp, &t.vsw) != 9)
+                return -EINVAL;
+
+        r = dssdev->driver->check_timings(dssdev, &t);
+        if (r)
+                return r;
+
+        dssdev->driver->disable(dssdev);
+        dssdev->driver->set_timings(dssdev, &t);
+        r = dssdev->driver->enable(dssdev);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t display_rotate_show(struct omap_dss_device *dssdev, char *buf)
+{
+        int rotate;
+        if (!dssdev->driver->get_rotate)
+                return -ENOENT;
+        rotate = dssdev->driver->get_rotate(dssdev);
+        return snprintf(buf, PAGE_SIZE, "%u\n", rotate);
+}
+
+static ssize_t display_rotate_store(struct omap_dss_device *dssdev,
+        const char *buf, size_t size)
+{
+        int rot, r;
+
+        if (!dssdev->driver->set_rotate || !dssdev->driver->get_rotate)
+                return -ENOENT;
+
+        r = kstrtoint(buf, 0, &rot);
+        if (r)
+                return r;
+
+        r = dssdev->driver->set_rotate(dssdev, rot);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t display_mirror_show(struct omap_dss_device *dssdev, char *buf)
+{
+        int mirror;
+        if (!dssdev->driver->get_mirror)
+                return -ENOENT;
+        mirror = dssdev->driver->get_mirror(dssdev);
+        return snprintf(buf, PAGE_SIZE, "%u\n", mirror);
+}
+
+static ssize_t display_mirror_store(struct omap_dss_device *dssdev,
+        const char *buf, size_t size)
+{
+        int r;
+        bool mirror;
+
+        if (!dssdev->driver->set_mirror || !dssdev->driver->get_mirror)
+                return -ENOENT;
+
+        r = strtobool(buf, &mirror);
+        if (r)
+                return r;
+
+        r = dssdev->driver->set_mirror(dssdev, mirror);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t display_wss_show(struct omap_dss_device *dssdev, char *buf)
+{
+        unsigned int wss;
+
+        if (!dssdev->driver->get_wss)
+                return -ENOENT;
+
+        wss = dssdev->driver->get_wss(dssdev);
+
+        return snprintf(buf, PAGE_SIZE, "0x%05x\n", wss);
+}
+
+static ssize_t display_wss_store(struct omap_dss_device *dssdev,
+        const char *buf, size_t size)
+{
+        u32 wss;
+        int r;
+
+        if (!dssdev->driver->get_wss || !dssdev->driver->set_wss)
+                return -ENOENT;
+
+        r = kstrtou32(buf, 0, &wss);
+        if (r)
+                return r;
+
+        if (wss > 0xfffff)
+                return -EINVAL;
+
+        r = dssdev->driver->set_wss(dssdev, wss);
+        if (r)
+                return r;
+
+        return size;
+}
+
+struct display_attribute {
+        struct attribute attr;
+        ssize_t (*show)(struct omap_dss_device *, char *);
+        ssize_t (*store)(struct omap_dss_device *, const char *, size_t);
+};
+
+#define DISPLAY_ATTR(_name, _mode, _show, _store) \
+        struct display_attribute display_attr_##_name = \
+        __ATTR(_name, _mode, _show, _store)
+
+static DISPLAY_ATTR(name, S_IRUGO, display_name_show, NULL);
+static DISPLAY_ATTR(display_name, S_IRUGO, display_name_show, NULL);
+static DISPLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
+                display_enabled_show, display_enabled_store);
+static DISPLAY_ATTR(tear_elim, S_IRUGO|S_IWUSR,
+                display_tear_show, display_tear_store);
+static DISPLAY_ATTR(timings, S_IRUGO|S_IWUSR,
+                display_timings_show, display_timings_store);
+static DISPLAY_ATTR(rotate, S_IRUGO|S_IWUSR,
+                display_rotate_show, display_rotate_store);
+static DISPLAY_ATTR(mirror, S_IRUGO|S_IWUSR,
+                display_mirror_show, display_mirror_store);
+static DISPLAY_ATTR(wss, S_IRUGO|S_IWUSR,
+                display_wss_show, display_wss_store);
+
+static struct attribute *display_sysfs_attrs[] = {
+        &display_attr_name.attr,
+        &display_attr_display_name.attr,
+        &display_attr_enabled.attr,
+        &display_attr_tear_elim.attr,
+        &display_attr_timings.attr,
+        &display_attr_rotate.attr,
+        &display_attr_mirror.attr,
+        &display_attr_wss.attr,
+        NULL
+};
+
+static ssize_t display_attr_show(struct kobject *kobj, struct attribute *attr,
+                char *buf)
+{
+        struct omap_dss_device *dssdev;
+        struct display_attribute *display_attr;
+
+        dssdev = container_of(kobj, struct omap_dss_device, kobj);
+        display_attr = container_of(attr, struct display_attribute, attr);
+
+        if (!display_attr->show)
+                return -ENOENT;
+
+        return display_attr->show(dssdev, buf);
+}
+
+static ssize_t display_attr_store(struct kobject *kobj, struct attribute *attr,
+                const char *buf, size_t size)
+{
+        struct omap_dss_device *dssdev;
+        struct display_attribute *display_attr;
+
+        dssdev = container_of(kobj, struct omap_dss_device, kobj);
+        display_attr = container_of(attr, struct display_attribute, attr);
+
+        if (!display_attr->store)
+                return -ENOENT;
+
+        return display_attr->store(dssdev, buf, size);
+}
+
+static const struct sysfs_ops display_sysfs_ops = {
+        .show = display_attr_show,
+        .store = display_attr_store,
+};
+
+static struct kobj_type display_ktype = {
+        .sysfs_ops = &display_sysfs_ops,
+        .default_attrs = display_sysfs_attrs,
+};
+
+int display_init_sysfs(struct platform_device *pdev)
+{
+        struct omap_dss_device *dssdev = NULL;
+        int r;
+
+        for_each_dss_dev(dssdev) {
+                r = kobject_init_and_add(&dssdev->kobj, &display_ktype,
+                        &pdev->dev.kobj, "%s", dssdev->alias);
+                if (r) {
+                        DSSERR("failed to create sysfs files\n");
+                        omap_dss_put_device(dssdev);
+                        goto err;
+                }
+        }
+
+        return 0;
+
+err:
+        display_uninit_sysfs(pdev);
+
+        return r;
+}
+
+void display_uninit_sysfs(struct platform_device *pdev)
+{
+        struct omap_dss_device *dssdev = NULL;
+
+        for_each_dss_dev(dssdev) {
+                if (kobject_name(&dssdev->kobj) == NULL)
+                        continue;
+
+                kobject_del(&dssdev->kobj);
+                kobject_put(&dssdev->kobj);
+
+                memset(&dssdev->kobj, 0, sizeof(dssdev->kobj));
+        }
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/display.c b/drivers/video/fbdev/omap2/omapfb/dss/display.c
new file mode 100644
index 000000000000..ef5b9027985d
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/display.c
@@ -0,0 +1,338 @@
+/*
+ * linux/drivers/video/omap2/dss/display.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "DISPLAY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+#include "dss_features.h"
+
+void omapdss_default_get_resolution(struct omap_dss_device *dssdev,
+                        u16 *xres, u16 *yres)
+{
+        *xres = dssdev->panel.timings.x_res;
+        *yres = dssdev->panel.timings.y_res;
+}
+EXPORT_SYMBOL(omapdss_default_get_resolution);
+
+int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
+{
+        switch (dssdev->type) {
+        case OMAP_DISPLAY_TYPE_DPI:
+                if (dssdev->phy.dpi.data_lines == 24)
+                        return 24;
+                else
+                        return 16;
+
+        case OMAP_DISPLAY_TYPE_DBI:
+                if (dssdev->ctrl.pixel_size == 24)
+                        return 24;
+                else
+                        return 16;
+        case OMAP_DISPLAY_TYPE_DSI:
+                if (dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt) > 16)
+                        return 24;
+                else
+                        return 16;
+        case OMAP_DISPLAY_TYPE_VENC:
+        case OMAP_DISPLAY_TYPE_SDI:
+        case OMAP_DISPLAY_TYPE_HDMI:
+        case OMAP_DISPLAY_TYPE_DVI:
+                return 24;
+        default:
+                BUG();
+                return 0;
+        }
+}
+EXPORT_SYMBOL(omapdss_default_get_recommended_bpp);
+
+void omapdss_default_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        *timings = dssdev->panel.timings;
+}
+EXPORT_SYMBOL(omapdss_default_get_timings);
+
+int dss_suspend_all_devices(void)
+{
+        struct omap_dss_device *dssdev = NULL;
+
+        for_each_dss_dev(dssdev) {
+                if (!dssdev->driver)
+                        continue;
+
+                if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+                        dssdev->driver->disable(dssdev);
+                        dssdev->activate_after_resume = true;
+                } else {
+                        dssdev->activate_after_resume = false;
+                }
+        }
+
+        return 0;
+}
+
+int dss_resume_all_devices(void)
+{
+        struct omap_dss_device *dssdev = NULL;
+
+        for_each_dss_dev(dssdev) {
+                if (!dssdev->driver)
+                        continue;
+
+                if (dssdev->activate_after_resume) {
+                        dssdev->driver->enable(dssdev);
+                        dssdev->activate_after_resume = false;
+                }
+        }
+
+        return 0;
+}
+
+void dss_disable_all_devices(void)
+{
+        struct omap_dss_device *dssdev = NULL;
+
+        for_each_dss_dev(dssdev) {
+                if (!dssdev->driver)
+                        continue;
+
+                if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                        dssdev->driver->disable(dssdev);
+        }
+}
+
+static LIST_HEAD(panel_list);
+static DEFINE_MUTEX(panel_list_mutex);
+static int disp_num_counter;
+
+int omapdss_register_display(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_driver *drv = dssdev->driver;
+        int id;
+
+        /*
+         * Note: this presumes all the displays are either using DT or non-DT,
+         * which normally should be the case. This also presumes that all
+         * displays either have an DT alias, or none has.
+         */
+
+        if (dssdev->dev->of_node) {
+                id = of_alias_get_id(dssdev->dev->of_node, "display");
+
+                if (id < 0)
+                        id = disp_num_counter++;
+        } else {
+                id = disp_num_counter++;
+        }
+
+        snprintf(dssdev->alias, sizeof(dssdev->alias), "display%d", id);
+
+        /* Use 'label' property for name, if it exists */
+        if (dssdev->dev->of_node)
+                of_property_read_string(dssdev->dev->of_node, "label",
+                        &dssdev->name);
+
+        if (dssdev->name == NULL)
+                dssdev->name = dssdev->alias;
+
+        if (drv && drv->get_resolution == NULL)
+                drv->get_resolution = omapdss_default_get_resolution;
+        if (drv && drv->get_recommended_bpp == NULL)
+                drv->get_recommended_bpp = omapdss_default_get_recommended_bpp;
+        if (drv && drv->get_timings == NULL)
+                drv->get_timings = omapdss_default_get_timings;
+
+        mutex_lock(&panel_list_mutex);
+        list_add_tail(&dssdev->panel_list, &panel_list);
+        mutex_unlock(&panel_list_mutex);
+        return 0;
+}
+EXPORT_SYMBOL(omapdss_register_display);
+
+void omapdss_unregister_display(struct omap_dss_device *dssdev)
+{
+        mutex_lock(&panel_list_mutex);
+        list_del(&dssdev->panel_list);
+        mutex_unlock(&panel_list_mutex);
+}
+EXPORT_SYMBOL(omapdss_unregister_display);
+
+struct omap_dss_device *omap_dss_get_device(struct omap_dss_device *dssdev)
+{
+        if (!try_module_get(dssdev->owner))
+                return NULL;
+
+        if (get_device(dssdev->dev) == NULL) {
+                module_put(dssdev->owner);
+                return NULL;
+        }
+
+        return dssdev;
+}
+EXPORT_SYMBOL(omap_dss_get_device);
+
+void omap_dss_put_device(struct omap_dss_device *dssdev)
+{
+        put_device(dssdev->dev);
+        module_put(dssdev->owner);
+}
+EXPORT_SYMBOL(omap_dss_put_device);
+
+/*
+ * ref count of the found device is incremented.
+ * ref count of from-device is decremented.
+ */
+struct omap_dss_device *omap_dss_get_next_device(struct omap_dss_device *from)
+{
+        struct list_head *l;
+        struct omap_dss_device *dssdev;
+
+        mutex_lock(&panel_list_mutex);
+
+        if (list_empty(&panel_list)) {
+                dssdev = NULL;
+                goto out;
+        }
+
+        if (from == NULL) {
+                dssdev = list_first_entry(&panel_list, struct omap_dss_device,
+                                panel_list);
+                omap_dss_get_device(dssdev);
+                goto out;
+        }
+
+        omap_dss_put_device(from);
+
+        list_for_each(l, &panel_list) {
+                dssdev = list_entry(l, struct omap_dss_device, panel_list);
+                if (dssdev == from) {
+                        if (list_is_last(l, &panel_list)) {
+                                dssdev = NULL;
+                                goto out;
+                        }
+
+                        dssdev = list_entry(l->next, struct omap_dss_device,
+                                        panel_list);
+                        omap_dss_get_device(dssdev);
+                        goto out;
+                }
+        }
+
+        WARN(1, "'from' dssdev not found\n");
+
+        dssdev = NULL;
+out:
+        mutex_unlock(&panel_list_mutex);
+        return dssdev;
+}
+EXPORT_SYMBOL(omap_dss_get_next_device);
+
+struct omap_dss_device *omap_dss_find_device(void *data,
+                int (*match)(struct omap_dss_device *dssdev, void *data))
+{
+        struct omap_dss_device *dssdev = NULL;
+
+        while ((dssdev = omap_dss_get_next_device(dssdev)) != NULL) {
+                if (match(dssdev, data))
+                        return dssdev;
+        }
+
+        return NULL;
+}
+EXPORT_SYMBOL(omap_dss_find_device);
+
+void videomode_to_omap_video_timings(const struct videomode *vm,
+                struct omap_video_timings *ovt)
+{
+        memset(ovt, 0, sizeof(*ovt));
+
+        ovt->pixelclock = vm->pixelclock;
+        ovt->x_res = vm->hactive;
+        ovt->hbp = vm->hback_porch;
+        ovt->hfp = vm->hfront_porch;
+        ovt->hsw = vm->hsync_len;
+        ovt->y_res = vm->vactive;
+        ovt->vbp = vm->vback_porch;
+        ovt->vfp = vm->vfront_porch;
+        ovt->vsw = vm->vsync_len;
+
+        ovt->vsync_level = vm->flags & DISPLAY_FLAGS_VSYNC_HIGH ?
+                OMAPDSS_SIG_ACTIVE_HIGH :
+                OMAPDSS_SIG_ACTIVE_LOW;
+        ovt->hsync_level = vm->flags & DISPLAY_FLAGS_HSYNC_HIGH ?
+                OMAPDSS_SIG_ACTIVE_HIGH :
+                OMAPDSS_SIG_ACTIVE_LOW;
+        ovt->de_level = vm->flags & DISPLAY_FLAGS_DE_HIGH ?
+                OMAPDSS_SIG_ACTIVE_HIGH :
+                OMAPDSS_SIG_ACTIVE_LOW;
+        ovt->data_pclk_edge = vm->flags & DISPLAY_FLAGS_PIXDATA_POSEDGE ?
+                OMAPDSS_DRIVE_SIG_RISING_EDGE :
+                OMAPDSS_DRIVE_SIG_FALLING_EDGE;
+
+        ovt->sync_pclk_edge = ovt->data_pclk_edge;
+}
+EXPORT_SYMBOL(videomode_to_omap_video_timings);
+
+void omap_video_timings_to_videomode(const struct omap_video_timings *ovt,
+                struct videomode *vm)
+{
+        memset(vm, 0, sizeof(*vm));
+
+        vm->pixelclock = ovt->pixelclock;
+
+        vm->hactive = ovt->x_res;
+        vm->hback_porch = ovt->hbp;
+        vm->hfront_porch = ovt->hfp;
+        vm->hsync_len = ovt->hsw;
+        vm->vactive = ovt->y_res;
+        vm->vback_porch = ovt->vbp;
+        vm->vfront_porch = ovt->vfp;
+        vm->vsync_len = ovt->vsw;
+
+        if (ovt->hsync_level == OMAPDSS_SIG_ACTIVE_HIGH)
+                vm->flags |= DISPLAY_FLAGS_HSYNC_HIGH;
+        else
+                vm->flags |= DISPLAY_FLAGS_HSYNC_LOW;
+
+        if (ovt->vsync_level == OMAPDSS_SIG_ACTIVE_HIGH)
+                vm->flags |= DISPLAY_FLAGS_VSYNC_HIGH;
+        else
+                vm->flags |= DISPLAY_FLAGS_VSYNC_LOW;
+
+        if (ovt->de_level == OMAPDSS_SIG_ACTIVE_HIGH)
+                vm->flags |= DISPLAY_FLAGS_DE_HIGH;
+        else
+                vm->flags |= DISPLAY_FLAGS_DE_LOW;
+
+        if (ovt->data_pclk_edge == OMAPDSS_DRIVE_SIG_RISING_EDGE)
+                vm->flags |= DISPLAY_FLAGS_PIXDATA_POSEDGE;
+        else
+                vm->flags |= DISPLAY_FLAGS_PIXDATA_NEGEDGE;
+}
+EXPORT_SYMBOL(omap_video_timings_to_videomode);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dpi.c b/drivers/video/fbdev/omap2/omapfb/dss/dpi.c
new file mode 100644
index 000000000000..7953e6a52346
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dpi.c
@@ -0,0 +1,899 @@
+/*
+ * linux/drivers/video/omap2/dss/dpi.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "DPI"
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/string.h>
+#include <linux/of.h>
+#include <linux/clk.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+#define HSDIV_DISPC     0
+
+struct dpi_data {
+        struct platform_device *pdev;
+
+        struct regulator *vdds_dsi_reg;
+        struct dss_pll *pll;
+
+        struct mutex lock;
+
+        struct omap_video_timings timings;
+        struct dss_lcd_mgr_config mgr_config;
+        int data_lines;
+
+        struct omap_dss_device output;
+
+        bool port_initialized;
+};
+
+static struct dpi_data *dpi_get_data_from_dssdev(struct omap_dss_device *dssdev)
+{
+        return container_of(dssdev, struct dpi_data, output);
+}
+
+/* only used in non-DT mode */
+static struct dpi_data *dpi_get_data_from_pdev(struct platform_device *pdev)
+{
+        return dev_get_drvdata(&pdev->dev);
+}
+
+static struct dss_pll *dpi_get_pll(enum omap_channel channel)
+{
+        /*
+         * XXX we can't currently use DSI PLL for DPI with OMAP3, as the DSI PLL
+         * would also be used for DISPC fclk. Meaning, when the DPI output is
+         * disabled, DISPC clock will be disabled, and TV out will stop.
+         */
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP24xx:
+        case OMAPDSS_VER_OMAP34xx_ES1:
+        case OMAPDSS_VER_OMAP34xx_ES3:
+        case OMAPDSS_VER_OMAP3630:
+        case OMAPDSS_VER_AM35xx:
+        case OMAPDSS_VER_AM43xx:
+                return NULL;
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                switch (channel) {
+                case OMAP_DSS_CHANNEL_LCD:
+                        return dss_pll_find("dsi0");
+                case OMAP_DSS_CHANNEL_LCD2:
+                        return dss_pll_find("dsi1");
+                default:
+                        return NULL;
+                }
+
+        case OMAPDSS_VER_OMAP5:
+                switch (channel) {
+                case OMAP_DSS_CHANNEL_LCD:
+                        return dss_pll_find("dsi0");
+                case OMAP_DSS_CHANNEL_LCD3:
+                        return dss_pll_find("dsi1");
+                default:
+                        return NULL;
+                }
+
+        case OMAPDSS_VER_DRA7xx:
+                switch (channel) {
+                case OMAP_DSS_CHANNEL_LCD:
+                case OMAP_DSS_CHANNEL_LCD2:
+                        return dss_pll_find("video0");
+                case OMAP_DSS_CHANNEL_LCD3:
+                        return dss_pll_find("video1");
+                default:
+                        return NULL;
+                }
+
+        default:
+                return NULL;
+        }
+}
+
+static enum omap_dss_clk_source dpi_get_alt_clk_src(enum omap_channel channel)
+{
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                return OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC;
+        case OMAP_DSS_CHANNEL_LCD2:
+                return OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+        case OMAP_DSS_CHANNEL_LCD3:
+                return OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+        default:
+                /* this shouldn't happen */
+                WARN_ON(1);
+                return OMAP_DSS_CLK_SRC_FCK;
+        }
+}
+
+struct dpi_clk_calc_ctx {
+        struct dss_pll *pll;
+
+        /* inputs */
+
+        unsigned long pck_min, pck_max;
+
+        /* outputs */
+
+        struct dss_pll_clock_info dsi_cinfo;
+        unsigned long fck;
+        struct dispc_clock_info dispc_cinfo;
+};
+
+static bool dpi_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+                unsigned long pck, void *data)
+{
+        struct dpi_clk_calc_ctx *ctx = data;
+
+        /*
+         * Odd dividers give us uneven duty cycle, causing problem when level
+         * shifted. So skip all odd dividers when the pixel clock is on the
+         * higher side.
+         */
+        if (ctx->pck_min >= 100000000) {
+                if (lckd > 1 && lckd % 2 != 0)
+                        return false;
+
+                if (pckd > 1 && pckd % 2 != 0)
+                        return false;
+        }
+
+        ctx->dispc_cinfo.lck_div = lckd;
+        ctx->dispc_cinfo.pck_div = pckd;
+        ctx->dispc_cinfo.lck = lck;
+        ctx->dispc_cinfo.pck = pck;
+
+        return true;
+}
+
+
+static bool dpi_calc_hsdiv_cb(int m_dispc, unsigned long dispc,
+                void *data)
+{
+        struct dpi_clk_calc_ctx *ctx = data;
+
+        /*
+         * Odd dividers give us uneven duty cycle, causing problem when level
+         * shifted. So skip all odd dividers when the pixel clock is on the
+         * higher side.
+         */
+        if (m_dispc > 1 && m_dispc % 2 != 0 && ctx->pck_min >= 100000000)
+                return false;
+
+        ctx->dsi_cinfo.mX[HSDIV_DISPC] = m_dispc;
+        ctx->dsi_cinfo.clkout[HSDIV_DISPC] = dispc;
+
+        return dispc_div_calc(dispc, ctx->pck_min, ctx->pck_max,
+                        dpi_calc_dispc_cb, ctx);
+}
+
+
+static bool dpi_calc_pll_cb(int n, int m, unsigned long fint,
+                unsigned long clkdco,
+                void *data)
+{
+        struct dpi_clk_calc_ctx *ctx = data;
+
+        ctx->dsi_cinfo.n = n;
+        ctx->dsi_cinfo.m = m;
+        ctx->dsi_cinfo.fint = fint;
+        ctx->dsi_cinfo.clkdco = clkdco;
+
+        return dss_pll_hsdiv_calc(ctx->pll, clkdco,
+                ctx->pck_min, dss_feat_get_param_max(FEAT_PARAM_DSS_FCK),
+                dpi_calc_hsdiv_cb, ctx);
+}
+
+static bool dpi_calc_dss_cb(unsigned long fck, void *data)
+{
+        struct dpi_clk_calc_ctx *ctx = data;
+
+        ctx->fck = fck;
+
+        return dispc_div_calc(fck, ctx->pck_min, ctx->pck_max,
+                        dpi_calc_dispc_cb, ctx);
+}
+
+static bool dpi_dsi_clk_calc(struct dpi_data *dpi, unsigned long pck,
+                struct dpi_clk_calc_ctx *ctx)
+{
+        unsigned long clkin;
+        unsigned long pll_min, pll_max;
+
+        memset(ctx, 0, sizeof(*ctx));
+        ctx->pll = dpi->pll;
+        ctx->pck_min = pck - 1000;
+        ctx->pck_max = pck + 1000;
+
+        pll_min = 0;
+        pll_max = 0;
+
+        clkin = clk_get_rate(ctx->pll->clkin);
+
+        return dss_pll_calc(ctx->pll, clkin,
+                        pll_min, pll_max,
+                        dpi_calc_pll_cb, ctx);
+}
+
+static bool dpi_dss_clk_calc(unsigned long pck, struct dpi_clk_calc_ctx *ctx)
+{
+        int i;
+
+        /*
+         * DSS fck gives us very few possibilities, so finding a good pixel
+         * clock may not be possible. We try multiple times to find the clock,
+         * each time widening the pixel clock range we look for, up to
+         * +/- ~15MHz.
+         */
+
+        for (i = 0; i < 25; ++i) {
+                bool ok;
+
+                memset(ctx, 0, sizeof(*ctx));
+                if (pck > 1000 * i * i * i)
+                        ctx->pck_min = max(pck - 1000 * i * i * i, 0lu);
+                else
+                        ctx->pck_min = 0;
+                ctx->pck_max = pck + 1000 * i * i * i;
+
+                ok = dss_div_calc(pck, ctx->pck_min, dpi_calc_dss_cb, ctx);
+                if (ok)
+                        return ok;
+        }
+
+        return false;
+}
+
+
+
+static int dpi_set_dsi_clk(struct dpi_data *dpi, enum omap_channel channel,
+                unsigned long pck_req, unsigned long *fck, int *lck_div,
+                int *pck_div)
+{
+        struct dpi_clk_calc_ctx ctx;
+        int r;
+        bool ok;
+
+        ok = dpi_dsi_clk_calc(dpi, pck_req, &ctx);
+        if (!ok)
+                return -EINVAL;
+
+        r = dss_pll_set_config(dpi->pll, &ctx.dsi_cinfo);
+        if (r)
+                return r;
+
+        dss_select_lcd_clk_source(channel,
+                        dpi_get_alt_clk_src(channel));
+
+        dpi->mgr_config.clock_info = ctx.dispc_cinfo;
+
+        *fck = ctx.dsi_cinfo.clkout[HSDIV_DISPC];
+        *lck_div = ctx.dispc_cinfo.lck_div;
+        *pck_div = ctx.dispc_cinfo.pck_div;
+
+        return 0;
+}
+
+static int dpi_set_dispc_clk(struct dpi_data *dpi, unsigned long pck_req,
+                unsigned long *fck, int *lck_div, int *pck_div)
+{
+        struct dpi_clk_calc_ctx ctx;
+        int r;
+        bool ok;
+
+        ok = dpi_dss_clk_calc(pck_req, &ctx);
+        if (!ok)
+                return -EINVAL;
+
+        r = dss_set_fck_rate(ctx.fck);
+        if (r)
+                return r;
+
+        dpi->mgr_config.clock_info = ctx.dispc_cinfo;
+
+        *fck = ctx.fck;
+        *lck_div = ctx.dispc_cinfo.lck_div;
+        *pck_div = ctx.dispc_cinfo.pck_div;
+
+        return 0;
+}
+
+static int dpi_set_mode(struct dpi_data *dpi)
+{
+        struct omap_dss_device *out = &dpi->output;
+        struct omap_overlay_manager *mgr = out->manager;
+        struct omap_video_timings *t = &dpi->timings;
+        int lck_div = 0, pck_div = 0;
+        unsigned long fck = 0;
+        unsigned long pck;
+        int r = 0;
+
+        if (dpi->pll)
+                r = dpi_set_dsi_clk(dpi, mgr->id, t->pixelclock, &fck,
+                                &lck_div, &pck_div);
+        else
+                r = dpi_set_dispc_clk(dpi, t->pixelclock, &fck,
+                                &lck_div, &pck_div);
+        if (r)
+                return r;
+
+        pck = fck / lck_div / pck_div;
+
+        if (pck != t->pixelclock) {
+                DSSWARN("Could not find exact pixel clock. Requested %d Hz, got %lu Hz\n",
+                        t->pixelclock, pck);
+
+                t->pixelclock = pck;
+        }
+
+        dss_mgr_set_timings(mgr, t);
+
+        return 0;
+}
+
+static void dpi_config_lcd_manager(struct dpi_data *dpi)
+{
+        struct omap_dss_device *out = &dpi->output;
+        struct omap_overlay_manager *mgr = out->manager;
+
+        dpi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+
+        dpi->mgr_config.stallmode = false;
+        dpi->mgr_config.fifohandcheck = false;
+
+        dpi->mgr_config.video_port_width = dpi->data_lines;
+
+        dpi->mgr_config.lcden_sig_polarity = 0;
+
+        dss_mgr_set_lcd_config(mgr, &dpi->mgr_config);
+}
+
+static int dpi_display_enable(struct omap_dss_device *dssdev)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+        struct omap_dss_device *out = &dpi->output;
+        int r;
+
+        mutex_lock(&dpi->lock);
+
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI) && !dpi->vdds_dsi_reg) {
+                DSSERR("no VDSS_DSI regulator\n");
+                r = -ENODEV;
+                goto err_no_reg;
+        }
+
+        if (out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                r = -ENODEV;
+                goto err_no_out_mgr;
+        }
+
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI)) {
+                r = regulator_enable(dpi->vdds_dsi_reg);
+                if (r)
+                        goto err_reg_enable;
+        }
+
+        r = dispc_runtime_get();
+        if (r)
+                goto err_get_dispc;
+
+        r = dss_dpi_select_source(out->port_num, out->manager->id);
+        if (r)
+                goto err_src_sel;
+
+        if (dpi->pll) {
+                r = dss_pll_enable(dpi->pll);
+                if (r)
+                        goto err_dsi_pll_init;
+        }
+
+        r = dpi_set_mode(dpi);
+        if (r)
+                goto err_set_mode;
+
+        dpi_config_lcd_manager(dpi);
+
+        mdelay(2);
+
+        r = dss_mgr_enable(out->manager);
+        if (r)
+                goto err_mgr_enable;
+
+        mutex_unlock(&dpi->lock);
+
+        return 0;
+
+err_mgr_enable:
+err_set_mode:
+        if (dpi->pll)
+                dss_pll_disable(dpi->pll);
+err_dsi_pll_init:
+err_src_sel:
+        dispc_runtime_put();
+err_get_dispc:
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
+                regulator_disable(dpi->vdds_dsi_reg);
+err_reg_enable:
+err_no_out_mgr:
+err_no_reg:
+        mutex_unlock(&dpi->lock);
+        return r;
+}
+
+static void dpi_display_disable(struct omap_dss_device *dssdev)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+        struct omap_overlay_manager *mgr = dpi->output.manager;
+
+        mutex_lock(&dpi->lock);
+
+        dss_mgr_disable(mgr);
+
+        if (dpi->pll) {
+                dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+                dss_pll_disable(dpi->pll);
+        }
+
+        dispc_runtime_put();
+
+        if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
+                regulator_disable(dpi->vdds_dsi_reg);
+
+        mutex_unlock(&dpi->lock);
+}
+
+static void dpi_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+
+        DSSDBG("dpi_set_timings\n");
+
+        mutex_lock(&dpi->lock);
+
+        dpi->timings = *timings;
+
+        mutex_unlock(&dpi->lock);
+}
+
+static void dpi_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+
+        mutex_lock(&dpi->lock);
+
+        *timings = dpi->timings;
+
+        mutex_unlock(&dpi->lock);
+}
+
+static int dpi_check_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+        struct omap_overlay_manager *mgr = dpi->output.manager;
+        int lck_div, pck_div;
+        unsigned long fck;
+        unsigned long pck;
+        struct dpi_clk_calc_ctx ctx;
+        bool ok;
+
+        if (mgr && !dispc_mgr_timings_ok(mgr->id, timings))
+                return -EINVAL;
+
+        if (timings->pixelclock == 0)
+                return -EINVAL;
+
+        if (dpi->pll) {
+                ok = dpi_dsi_clk_calc(dpi, timings->pixelclock, &ctx);
+                if (!ok)
+                        return -EINVAL;
+
+                fck = ctx.dsi_cinfo.clkout[HSDIV_DISPC];
+        } else {
+                ok = dpi_dss_clk_calc(timings->pixelclock, &ctx);
+                if (!ok)
+                        return -EINVAL;
+
+                fck = ctx.fck;
+        }
+
+        lck_div = ctx.dispc_cinfo.lck_div;
+        pck_div = ctx.dispc_cinfo.pck_div;
+
+        pck = fck / lck_div / pck_div;
+
+        timings->pixelclock = pck;
+
+        return 0;
+}
+
+static void dpi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+
+        mutex_lock(&dpi->lock);
+
+        dpi->data_lines = data_lines;
+
+        mutex_unlock(&dpi->lock);
+}
+
+static int dpi_verify_dsi_pll(struct dss_pll *pll)
+{
+        int r;
+
+        /* do initial setup with the PLL to see if it is operational */
+
+        r = dss_pll_enable(pll);
+        if (r)
+                return r;
+
+        dss_pll_disable(pll);
+
+        return 0;
+}
+
+static int dpi_init_regulator(struct dpi_data *dpi)
+{
+        struct regulator *vdds_dsi;
+
+        if (!dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
+                return 0;
+
+        if (dpi->vdds_dsi_reg)
+                return 0;
+
+        vdds_dsi = devm_regulator_get(&dpi->pdev->dev, "vdds_dsi");
+        if (IS_ERR(vdds_dsi)) {
+                if (PTR_ERR(vdds_dsi) != -EPROBE_DEFER)
+                        DSSERR("can't get VDDS_DSI regulator\n");
+                return PTR_ERR(vdds_dsi);
+        }
+
+        dpi->vdds_dsi_reg = vdds_dsi;
+
+        return 0;
+}
+
+static void dpi_init_pll(struct dpi_data *dpi)
+{
+        struct dss_pll *pll;
+
+        if (dpi->pll)
+                return;
+
+        pll = dpi_get_pll(dpi->output.dispc_channel);
+        if (!pll)
+                return;
+
+        /* On DRA7 we need to set a mux to use the PLL */
+        if (omapdss_get_version() == OMAPDSS_VER_DRA7xx)
+                dss_ctrl_pll_set_control_mux(pll->id, dpi->output.dispc_channel);
+
+        if (dpi_verify_dsi_pll(pll)) {
+                DSSWARN("DSI PLL not operational\n");
+                return;
+        }
+
+        dpi->pll = pll;
+}
+
+/*
+ * Return a hardcoded channel for the DPI output. This should work for
+ * current use cases, but this can be later expanded to either resolve
+ * the channel in some more dynamic manner, or get the channel as a user
+ * parameter.
+ */
+static enum omap_channel dpi_get_channel(int port_num)
+{
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP24xx:
+        case OMAPDSS_VER_OMAP34xx_ES1:
+        case OMAPDSS_VER_OMAP34xx_ES3:
+        case OMAPDSS_VER_OMAP3630:
+        case OMAPDSS_VER_AM35xx:
+        case OMAPDSS_VER_AM43xx:
+                return OMAP_DSS_CHANNEL_LCD;
+
+        case OMAPDSS_VER_DRA7xx:
+                switch (port_num) {
+                case 2:
+                        return OMAP_DSS_CHANNEL_LCD3;
+                case 1:
+                        return OMAP_DSS_CHANNEL_LCD2;
+                case 0:
+                default:
+                        return OMAP_DSS_CHANNEL_LCD;
+                }
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                return OMAP_DSS_CHANNEL_LCD2;
+
+        case OMAPDSS_VER_OMAP5:
+                return OMAP_DSS_CHANNEL_LCD3;
+
+        default:
+                DSSWARN("unsupported DSS version\n");
+                return OMAP_DSS_CHANNEL_LCD;
+        }
+}
+
+static int dpi_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+        struct omap_overlay_manager *mgr;
+        int r;
+
+        r = dpi_init_regulator(dpi);
+        if (r)
+                return r;
+
+        dpi_init_pll(dpi);
+
+        mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+        if (!mgr)
+                return -ENODEV;
+
+        r = dss_mgr_connect(mgr, dssdev);
+        if (r)
+                return r;
+
+        r = omapdss_output_set_device(dssdev, dst);
+        if (r) {
+                DSSERR("failed to connect output to new device: %s\n",
+                                dst->name);
+                dss_mgr_disconnect(mgr, dssdev);
+                return r;
+        }
+
+        return 0;
+}
+
+static void dpi_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        omapdss_output_unset_device(dssdev);
+
+        if (dssdev->manager)
+                dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_dpi_ops dpi_ops = {
+        .connect = dpi_connect,
+        .disconnect = dpi_disconnect,
+
+        .enable = dpi_display_enable,
+        .disable = dpi_display_disable,
+
+        .check_timings = dpi_check_timings,
+        .set_timings = dpi_set_timings,
+        .get_timings = dpi_get_timings,
+
+        .set_data_lines = dpi_set_data_lines,
+};
+
+static void dpi_init_output(struct platform_device *pdev)
+{
+        struct dpi_data *dpi = dpi_get_data_from_pdev(pdev);
+        struct omap_dss_device *out = &dpi->output;
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_DPI;
+        out->output_type = OMAP_DISPLAY_TYPE_DPI;
+        out->name = "dpi.0";
+        out->dispc_channel = dpi_get_channel(0);
+        out->ops.dpi = &dpi_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void dpi_uninit_output(struct platform_device *pdev)
+{
+        struct dpi_data *dpi = dpi_get_data_from_pdev(pdev);
+        struct omap_dss_device *out = &dpi->output;
+
+        omapdss_unregister_output(out);
+}
+
+static void dpi_init_output_port(struct platform_device *pdev,
+        struct device_node *port)
+{
+        struct dpi_data *dpi = port->data;
+        struct omap_dss_device *out = &dpi->output;
+        int r;
+        u32 port_num;
+
+        r = of_property_read_u32(port, "reg", &port_num);
+        if (r)
+                port_num = 0;
+
+        switch (port_num) {
+        case 2:
+                out->name = "dpi.2";
+                break;
+        case 1:
+                out->name = "dpi.1";
+                break;
+        case 0:
+        default:
+                out->name = "dpi.0";
+                break;
+        }
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_DPI;
+        out->output_type = OMAP_DISPLAY_TYPE_DPI;
+        out->dispc_channel = dpi_get_channel(port_num);
+        out->port_num = port_num;
+        out->ops.dpi = &dpi_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void dpi_uninit_output_port(struct device_node *port)
+{
+        struct dpi_data *dpi = port->data;
+        struct omap_dss_device *out = &dpi->output;
+
+        omapdss_unregister_output(out);
+}
+
+static int dpi_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct dpi_data *dpi;
+
+        dpi = devm_kzalloc(&pdev->dev, sizeof(*dpi), GFP_KERNEL);
+        if (!dpi)
+                return -ENOMEM;
+
+        dpi->pdev = pdev;
+
+        dev_set_drvdata(&pdev->dev, dpi);
+
+        mutex_init(&dpi->lock);
+
+        dpi_init_output(pdev);
+
+        return 0;
+}
+
+static void dpi_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        dpi_uninit_output(pdev);
+}
+
+static const struct component_ops dpi_component_ops = {
+        .bind   = dpi_bind,
+        .unbind = dpi_unbind,
+};
+
+static int dpi_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &dpi_component_ops);
+}
+
+static int dpi_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &dpi_component_ops);
+        return 0;
+}
+
+static struct platform_driver omap_dpi_driver = {
+        .probe          = dpi_probe,
+        .remove         = dpi_remove,
+        .driver         = {
+                .name   = "omapdss_dpi",
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init dpi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dpi_driver);
+}
+
+void dpi_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_dpi_driver);
+}
+
+int dpi_init_port(struct platform_device *pdev, struct device_node *port)
+{
+        struct dpi_data *dpi;
+        struct device_node *ep;
+        u32 datalines;
+        int r;
+
+        dpi = devm_kzalloc(&pdev->dev, sizeof(*dpi), GFP_KERNEL);
+        if (!dpi)
+                return -ENOMEM;
+
+        ep = omapdss_of_get_next_endpoint(port, NULL);
+        if (!ep)
+                return 0;
+
+        r = of_property_read_u32(ep, "data-lines", &datalines);
+        if (r) {
+                DSSERR("failed to parse datalines\n");
+                goto err_datalines;
+        }
+
+        dpi->data_lines = datalines;
+
+        of_node_put(ep);
+
+        dpi->pdev = pdev;
+        port->data = dpi;
+
+        mutex_init(&dpi->lock);
+
+        dpi_init_output_port(pdev, port);
+
+        dpi->port_initialized = true;
+
+        return 0;
+
+err_datalines:
+        of_node_put(ep);
+
+        return r;
+}
+
+void dpi_uninit_port(struct device_node *port)
+{
+        struct dpi_data *dpi = port->data;
+
+        if (!dpi->port_initialized)
+                return;
+
+        dpi_uninit_output_port(port);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dsi.c b/drivers/video/fbdev/omap2/omapfb/dss/dsi.c
new file mode 100644
index 000000000000..0eec073b3919
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dsi.c
@@ -0,0 +1,5607 @@
+/*
+ * linux/drivers/video/omap2/dss/dsi.c
+ *
+ * Copyright (C) 2009 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/>.
+ */
+
+#define DSS_SUBSYS_NAME "DSI"
+
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/semaphore.h>
+#include <linux/seq_file.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+#include <video/mipi_display.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+#define DSI_CATCH_MISSING_TE
+
+struct dsi_reg { u16 module; u16 idx; };
+
+#define DSI_REG(mod, idx)               ((const struct dsi_reg) { mod, idx })
+
+/* DSI Protocol Engine */
+
+#define DSI_PROTO                       0
+#define DSI_PROTO_SZ                    0x200
+
+#define DSI_REVISION                    DSI_REG(DSI_PROTO, 0x0000)
+#define DSI_SYSCONFIG                   DSI_REG(DSI_PROTO, 0x0010)
+#define DSI_SYSSTATUS                   DSI_REG(DSI_PROTO, 0x0014)
+#define DSI_IRQSTATUS                   DSI_REG(DSI_PROTO, 0x0018)
+#define DSI_IRQENABLE                   DSI_REG(DSI_PROTO, 0x001C)
+#define DSI_CTRL                        DSI_REG(DSI_PROTO, 0x0040)
+#define DSI_GNQ                         DSI_REG(DSI_PROTO, 0x0044)
+#define DSI_COMPLEXIO_CFG1              DSI_REG(DSI_PROTO, 0x0048)
+#define DSI_COMPLEXIO_IRQ_STATUS        DSI_REG(DSI_PROTO, 0x004C)
+#define DSI_COMPLEXIO_IRQ_ENABLE        DSI_REG(DSI_PROTO, 0x0050)
+#define DSI_CLK_CTRL                    DSI_REG(DSI_PROTO, 0x0054)
+#define DSI_TIMING1                     DSI_REG(DSI_PROTO, 0x0058)
+#define DSI_TIMING2                     DSI_REG(DSI_PROTO, 0x005C)
+#define DSI_VM_TIMING1                  DSI_REG(DSI_PROTO, 0x0060)
+#define DSI_VM_TIMING2                  DSI_REG(DSI_PROTO, 0x0064)
+#define DSI_VM_TIMING3                  DSI_REG(DSI_PROTO, 0x0068)
+#define DSI_CLK_TIMING                  DSI_REG(DSI_PROTO, 0x006C)
+#define DSI_TX_FIFO_VC_SIZE             DSI_REG(DSI_PROTO, 0x0070)
+#define DSI_RX_FIFO_VC_SIZE             DSI_REG(DSI_PROTO, 0x0074)
+#define DSI_COMPLEXIO_CFG2              DSI_REG(DSI_PROTO, 0x0078)
+#define DSI_RX_FIFO_VC_FULLNESS         DSI_REG(DSI_PROTO, 0x007C)
+#define DSI_VM_TIMING4                  DSI_REG(DSI_PROTO, 0x0080)
+#define DSI_TX_FIFO_VC_EMPTINESS        DSI_REG(DSI_PROTO, 0x0084)
+#define DSI_VM_TIMING5                  DSI_REG(DSI_PROTO, 0x0088)
+#define DSI_VM_TIMING6                  DSI_REG(DSI_PROTO, 0x008C)
+#define DSI_VM_TIMING7                  DSI_REG(DSI_PROTO, 0x0090)
+#define DSI_STOPCLK_TIMING              DSI_REG(DSI_PROTO, 0x0094)
+#define DSI_VC_CTRL(n)                  DSI_REG(DSI_PROTO, 0x0100 + (n * 0x20))
+#define DSI_VC_TE(n)                    DSI_REG(DSI_PROTO, 0x0104 + (n * 0x20))
+#define DSI_VC_LONG_PACKET_HEADER(n)    DSI_REG(DSI_PROTO, 0x0108 + (n * 0x20))
+#define DSI_VC_LONG_PACKET_PAYLOAD(n)   DSI_REG(DSI_PROTO, 0x010C + (n * 0x20))
+#define DSI_VC_SHORT_PACKET_HEADER(n)   DSI_REG(DSI_PROTO, 0x0110 + (n * 0x20))
+#define DSI_VC_IRQSTATUS(n)             DSI_REG(DSI_PROTO, 0x0118 + (n * 0x20))
+#define DSI_VC_IRQENABLE(n)             DSI_REG(DSI_PROTO, 0x011C + (n * 0x20))
+
+/* DSIPHY_SCP */
+
+#define DSI_PHY                         1
+#define DSI_PHY_OFFSET                  0x200
+#define DSI_PHY_SZ                      0x40
+
+#define DSI_DSIPHY_CFG0                 DSI_REG(DSI_PHY, 0x0000)
+#define DSI_DSIPHY_CFG1                 DSI_REG(DSI_PHY, 0x0004)
+#define DSI_DSIPHY_CFG2                 DSI_REG(DSI_PHY, 0x0008)
+#define DSI_DSIPHY_CFG5                 DSI_REG(DSI_PHY, 0x0014)
+#define DSI_DSIPHY_CFG10                DSI_REG(DSI_PHY, 0x0028)
+
+/* DSI_PLL_CTRL_SCP */
+
+#define DSI_PLL                         2
+#define DSI_PLL_OFFSET                  0x300
+#define DSI_PLL_SZ                      0x20
+
+#define DSI_PLL_CONTROL                 DSI_REG(DSI_PLL, 0x0000)
+#define DSI_PLL_STATUS                  DSI_REG(DSI_PLL, 0x0004)
+#define DSI_PLL_GO                      DSI_REG(DSI_PLL, 0x0008)
+#define DSI_PLL_CONFIGURATION1          DSI_REG(DSI_PLL, 0x000C)
+#define DSI_PLL_CONFIGURATION2          DSI_REG(DSI_PLL, 0x0010)
+
+#define REG_GET(dsidev, idx, start, end) \
+        FLD_GET(dsi_read_reg(dsidev, idx), 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)
+#define DSI_IRQ_VC1             (1 << 1)
+#define DSI_IRQ_VC2             (1 << 2)
+#define DSI_IRQ_VC3             (1 << 3)
+#define DSI_IRQ_WAKEUP          (1 << 4)
+#define DSI_IRQ_RESYNC          (1 << 5)
+#define DSI_IRQ_PLL_LOCK        (1 << 7)
+#define DSI_IRQ_PLL_UNLOCK      (1 << 8)
+#define DSI_IRQ_PLL_RECALL      (1 << 9)
+#define DSI_IRQ_COMPLEXIO_ERR   (1 << 10)
+#define DSI_IRQ_HS_TX_TIMEOUT   (1 << 14)
+#define DSI_IRQ_LP_RX_TIMEOUT   (1 << 15)
+#define DSI_IRQ_TE_TRIGGER      (1 << 16)
+#define DSI_IRQ_ACK_TRIGGER     (1 << 17)
+#define DSI_IRQ_SYNC_LOST       (1 << 18)
+#define DSI_IRQ_LDO_POWER_GOOD  (1 << 19)
+#define DSI_IRQ_TA_TIMEOUT      (1 << 20)
+#define DSI_IRQ_ERROR_MASK \
+        (DSI_IRQ_HS_TX_TIMEOUT | DSI_IRQ_LP_RX_TIMEOUT | DSI_IRQ_SYNC_LOST | \
+        DSI_IRQ_TA_TIMEOUT)
+#define DSI_IRQ_CHANNEL_MASK    0xf
+
+/* Virtual channel interrupts */
+#define DSI_VC_IRQ_CS           (1 << 0)
+#define DSI_VC_IRQ_ECC_CORR     (1 << 1)
+#define DSI_VC_IRQ_PACKET_SENT  (1 << 2)
+#define DSI_VC_IRQ_FIFO_TX_OVF  (1 << 3)
+#define DSI_VC_IRQ_FIFO_RX_OVF  (1 << 4)
+#define DSI_VC_IRQ_BTA          (1 << 5)
+#define DSI_VC_IRQ_ECC_NO_CORR  (1 << 6)
+#define DSI_VC_IRQ_FIFO_TX_UDF  (1 << 7)
+#define DSI_VC_IRQ_PP_BUSY_CHANGE (1 << 8)
+#define DSI_VC_IRQ_ERROR_MASK \
+        (DSI_VC_IRQ_CS | DSI_VC_IRQ_ECC_CORR | DSI_VC_IRQ_FIFO_TX_OVF | \
+        DSI_VC_IRQ_FIFO_RX_OVF | DSI_VC_IRQ_ECC_NO_CORR | \
+        DSI_VC_IRQ_FIFO_TX_UDF)
+
+/* ComplexIO interrupts */
+#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_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_4 | DSI_CIO_IRQ_ERRCONTENTIONLP1_4 | \
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_5 | DSI_CIO_IRQ_ERRCONTENTIONLP1_5)
+
+typedef void (*omap_dsi_isr_t) (void *arg, u32 mask);
+
+static int dsi_display_init_dispc(struct platform_device *dsidev,
+        struct omap_overlay_manager *mgr);
+static void dsi_display_uninit_dispc(struct platform_device *dsidev,
+        struct omap_overlay_manager *mgr);
+
+static int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel);
+
+/* DSI PLL HSDIV indices */
+#define HSDIV_DISPC     0
+#define HSDIV_DSI       1
+
+#define DSI_MAX_NR_ISRS                2
+#define DSI_MAX_NR_LANES        5
+
+enum dsi_lane_function {
+        DSI_LANE_UNUSED = 0,
+        DSI_LANE_CLK,
+        DSI_LANE_DATA1,
+        DSI_LANE_DATA2,
+        DSI_LANE_DATA3,
+        DSI_LANE_DATA4,
+};
+
+struct dsi_lane_config {
+        enum dsi_lane_function function;
+        u8 polarity;
+};
+
+struct dsi_isr_data {
+        omap_dsi_isr_t  isr;
+        void            *arg;
+        u32             mask;
+};
+
+enum fifo_size {
+        DSI_FIFO_SIZE_0         = 0,
+        DSI_FIFO_SIZE_32        = 1,
+        DSI_FIFO_SIZE_64        = 2,
+        DSI_FIFO_SIZE_96        = 3,
+        DSI_FIFO_SIZE_128       = 4,
+};
+
+enum dsi_vc_source {
+        DSI_VC_SOURCE_L4 = 0,
+        DSI_VC_SOURCE_VP,
+};
+
+struct dsi_irq_stats {
+        unsigned long last_reset;
+        unsigned irq_count;
+        unsigned dsi_irqs[32];
+        unsigned vc_irqs[4][32];
+        unsigned cio_irqs[32];
+};
+
+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_clk_calc_ctx {
+        struct platform_device *dsidev;
+        struct dss_pll *pll;
+
+        /* inputs */
+
+        const struct omap_dss_dsi_config *config;
+
+        unsigned long req_pck_min, req_pck_nom, req_pck_max;
+
+        /* outputs */
+
+        struct dss_pll_clock_info dsi_cinfo;
+        struct dispc_clock_info dispc_cinfo;
+
+        struct omap_video_timings dispc_vm;
+        struct omap_dss_dsi_videomode_timings dsi_vm;
+};
+
+struct dsi_lp_clock_info {
+        unsigned long lp_clk;
+        u16 lp_clk_div;
+};
+
+struct dsi_data {
+        struct platform_device *pdev;
+        void __iomem *proto_base;
+        void __iomem *phy_base;
+        void __iomem *pll_base;
+
+        int module_id;
+
+        int irq;
+
+        bool is_enabled;
+
+        struct clk *dss_clk;
+
+        struct dispc_clock_info user_dispc_cinfo;
+        struct dss_pll_clock_info user_dsi_cinfo;
+
+        struct dsi_lp_clock_info user_lp_cinfo;
+        struct dsi_lp_clock_info current_lp_cinfo;
+
+        struct dss_pll pll;
+
+        bool vdds_dsi_enabled;
+        struct regulator *vdds_dsi_reg;
+
+        struct {
+                enum dsi_vc_source source;
+                struct omap_dss_device *dssdev;
+                enum fifo_size tx_fifo_size;
+                enum fifo_size rx_fifo_size;
+                int vc_id;
+        } vc[4];
+
+        struct mutex lock;
+        struct semaphore bus_lock;
+
+        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;
+#ifdef DSI_PERF_MEASURE
+        unsigned update_bytes;
+#endif
+
+        bool te_enabled;
+        bool ulps_enabled;
+
+        void (*framedone_callback)(int, void *);
+        void *framedone_data;
+
+        struct delayed_work framedone_timeout_work;
+
+#ifdef DSI_CATCH_MISSING_TE
+        struct timer_list te_timer;
+#endif
+
+        unsigned long cache_req_pck;
+        unsigned long cache_clk_freq;
+        struct dss_pll_clock_info cache_cinfo;
+
+        u32             errors;
+        spinlock_t      errors_lock;
+#ifdef DSI_PERF_MEASURE
+        ktime_t perf_setup_time;
+        ktime_t perf_start_time;
+#endif
+        int debug_read;
+        int debug_write;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        spinlock_t irq_stats_lock;
+        struct dsi_irq_stats irq_stats;
+#endif
+
+        unsigned num_lanes_supported;
+        unsigned line_buffer_size;
+
+        struct dsi_lane_config lanes[DSI_MAX_NR_LANES];
+        unsigned num_lanes_used;
+
+        unsigned scp_clk_refcount;
+
+        struct dss_lcd_mgr_config mgr_config;
+        struct omap_video_timings timings;
+        enum omap_dss_dsi_pixel_format pix_fmt;
+        enum omap_dss_dsi_mode mode;
+        struct omap_dss_dsi_videomode_timings vm_timings;
+
+        struct omap_dss_device output;
+};
+
+struct dsi_packet_sent_handler_data {
+        struct platform_device *dsidev;
+        struct completion *completion;
+};
+
+struct dsi_module_id_data {
+        u32 address;
+        int id;
+};
+
+static const struct of_device_id dsi_of_match[];
+
+#ifdef DSI_PERF_MEASURE
+static bool dsi_perf;
+module_param(dsi_perf, bool, 0644);
+#endif
+
+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)
+{
+        return to_platform_device(dssdev->dev);
+}
+
+static struct platform_device *dsi_get_dsidev_from_id(int module)
+{
+        struct omap_dss_device *out;
+        enum omap_dss_output_id id;
+
+        switch (module) {
+        case 0:
+                id = OMAP_DSS_OUTPUT_DSI1;
+                break;
+        case 1:
+                id = OMAP_DSS_OUTPUT_DSI2;
+                break;
+        default:
+                return NULL;
+        }
+
+        out = omap_dss_get_output(id);
+
+        return out ? to_platform_device(out->dev) : NULL;
+}
+
+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);
+        void __iomem *base;
+
+        switch(idx.module) {
+                case DSI_PROTO: base = dsi->proto_base; break;
+                case DSI_PHY: base = dsi->phy_base; break;
+                case DSI_PLL: base = dsi->pll_base; break;
+                default: return;
+        }
+
+        __raw_writel(val, 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);
+        void __iomem *base;
+
+        switch(idx.module) {
+                case DSI_PROTO: base = dsi->proto_base; break;
+                case DSI_PHY: base = dsi->phy_base; break;
+                case DSI_PLL: base = dsi->pll_base; break;
+                default: return 0;
+        }
+
+        return __raw_readl(base + idx.idx);
+}
+
+static void dsi_bus_lock(struct omap_dss_device *dssdev)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        down(&dsi->bus_lock);
+}
+
+static void dsi_bus_unlock(struct omap_dss_device *dssdev)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        up(&dsi->bus_lock);
+}
+
+static bool dsi_bus_is_locked(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->bus_lock.count == 0;
+}
+
+static void dsi_completion_handler(void *data, u32 mask)
+{
+        complete((struct completion *)data);
+}
+
+static inline int wait_for_bit_change(struct platform_device *dsidev,
+                const struct dsi_reg idx, int bitnum, int value)
+{
+        unsigned long timeout;
+        ktime_t wait;
+        int t;
+
+        /* first busyloop to see if the bit changes right away */
+        t = 100;
+        while (t-- > 0) {
+                if (REG_GET(dsidev, idx, bitnum, bitnum) == value)
+                        return value;
+        }
+
+        /* then loop for 500ms, sleeping for 1ms in between */
+        timeout = jiffies + msecs_to_jiffies(500);
+        while (time_before(jiffies, timeout)) {
+                if (REG_GET(dsidev, idx, bitnum, bitnum) == value)
+                        return value;
+
+                wait = ns_to_ktime(1000 * 1000);
+                set_current_state(TASK_UNINTERRUPTIBLE);
+                schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+        }
+
+        return !value;
+}
+
+u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
+{
+        switch (fmt) {
+        case OMAP_DSS_DSI_FMT_RGB888:
+        case OMAP_DSS_DSI_FMT_RGB666:
+                return 24;
+        case OMAP_DSS_DSI_FMT_RGB666_PACKED:
+                return 18;
+        case OMAP_DSS_DSI_FMT_RGB565:
+                return 16;
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+#ifdef DSI_PERF_MEASURE
+static void dsi_perf_mark_setup(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        dsi->perf_setup_time = ktime_get();
+}
+
+static void dsi_perf_mark_start(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        dsi->perf_start_time = ktime_get();
+}
+
+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;
+
+        if (!dsi_perf)
+                return;
+
+        t = ktime_get();
+
+        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_us = (u32)ktime_to_us(trans_time);
+        if (trans_us == 0)
+                trans_us = 1;
+
+        total_us = setup_us + trans_us;
+
+        total_bytes = dsi->update_bytes;
+
+        printk(KERN_INFO "DSI(%s): %u us + %u us = %u us (%uHz), "
+                        "%u bytes, %u kbytes/sec\n",
+                        name,
+                        setup_us,
+                        trans_us,
+                        total_us,
+                        1000*1000 / total_us,
+                        total_bytes,
+                        total_bytes * 1000 / total_us);
+}
+#else
+static inline void dsi_perf_mark_setup(struct platform_device *dsidev)
+{
+}
+
+static inline void dsi_perf_mark_start(struct platform_device *dsidev)
+{
+}
+
+static inline void dsi_perf_show(struct platform_device *dsidev,
+                const char *name)
+{
+}
+#endif
+
+static int verbose_irq;
+
+static void print_irq_status(u32 status)
+{
+        if (status == 0)
+                return;
+
+        if (!verbose_irq && (status & ~DSI_IRQ_CHANNEL_MASK) == 0)
+                return;
+
+#define PIS(x) (status & DSI_IRQ_##x) ? (#x " ") : ""
+
+        pr_debug("DSI IRQ: 0x%x: %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+                status,
+                verbose_irq ? PIS(VC0) : "",
+                verbose_irq ? PIS(VC1) : "",
+                verbose_irq ? PIS(VC2) : "",
+                verbose_irq ? PIS(VC3) : "",
+                PIS(WAKEUP),
+                PIS(RESYNC),
+                PIS(PLL_LOCK),
+                PIS(PLL_UNLOCK),
+                PIS(PLL_RECALL),
+                PIS(COMPLEXIO_ERR),
+                PIS(HS_TX_TIMEOUT),
+                PIS(LP_RX_TIMEOUT),
+                PIS(TE_TRIGGER),
+                PIS(ACK_TRIGGER),
+                PIS(SYNC_LOST),
+                PIS(LDO_POWER_GOOD),
+                PIS(TA_TIMEOUT));
+#undef PIS
+}
+
+static void print_irq_status_vc(int channel, u32 status)
+{
+        if (status == 0)
+                return;
+
+        if (!verbose_irq && (status & ~DSI_VC_IRQ_PACKET_SENT) == 0)
+                return;
+
+#define PIS(x) (status & DSI_VC_IRQ_##x) ? (#x " ") : ""
+
+        pr_debug("DSI VC(%d) IRQ 0x%x: %s%s%s%s%s%s%s%s%s\n",
+                channel,
+                status,
+                PIS(CS),
+                PIS(ECC_CORR),
+                PIS(ECC_NO_CORR),
+                verbose_irq ? PIS(PACKET_SENT) : "",
+                PIS(BTA),
+                PIS(FIFO_TX_OVF),
+                PIS(FIFO_RX_OVF),
+                PIS(FIFO_TX_UDF),
+                PIS(PP_BUSY_CHANGE));
+#undef PIS
+}
+
+static void print_irq_status_cio(u32 status)
+{
+        if (status == 0)
+                return;
+
+#define PIS(x) (status & DSI_CIO_IRQ_##x) ? (#x " ") : ""
+
+        pr_debug("DSI CIO IRQ 0x%x: %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+                status,
+                PIS(ERRSYNCESC1),
+                PIS(ERRSYNCESC2),
+                PIS(ERRSYNCESC3),
+                PIS(ERRESC1),
+                PIS(ERRESC2),
+                PIS(ERRESC3),
+                PIS(ERRCONTROL1),
+                PIS(ERRCONTROL2),
+                PIS(ERRCONTROL3),
+                PIS(STATEULPS1),
+                PIS(STATEULPS2),
+                PIS(STATEULPS3),
+                PIS(ERRCONTENTIONLP0_1),
+                PIS(ERRCONTENTIONLP1_1),
+                PIS(ERRCONTENTIONLP0_2),
+                PIS(ERRCONTENTIONLP1_2),
+                PIS(ERRCONTENTIONLP0_3),
+                PIS(ERRCONTENTIONLP1_3),
+                PIS(ULPSACTIVENOT_ALL0),
+                PIS(ULPSACTIVENOT_ALL1));
+#undef PIS
+}
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dsi_collect_irq_stats(struct platform_device *dsidev, u32 irqstatus,
+                u32 *vcstatus, u32 ciostatus)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int i;
+
+        spin_lock(&dsi->irq_stats_lock);
+
+        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);
+        } else if (debug_irq) {
+                print_irq_status(irqstatus);
+        }
+
+        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 (vcstatus[i] == 0)
+                        continue;
+                dsi_call_isrs(isr_tables->isr_table_vc[i],
+                                ARRAY_SIZE(isr_tables->isr_table_vc[i]),
+                                vcstatus[i]);
+        }
+
+        if (ciostatus != 0)
+                dsi_call_isrs(isr_tables->isr_table_cio,
+                                ARRAY_SIZE(isr_tables->isr_table_cio),
+                                ciostatus);
+}
+
+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;
+
+        dsidev = (struct platform_device *) arg;
+        dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (!dsi->is_enabled)
+                return IRQ_NONE;
+
+        spin_lock(&dsi->irq_lock);
+
+        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;
+                }
+
+                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(dsidev, DSI_VC_IRQSTATUS(i));
+        }
+
+        if (irqstatus & DSI_IRQ_COMPLEXIO_ERR) {
+                ciostatus = dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
+
+                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
+
+        /* make a copy and unlock, so that isrs can unregister
+         * themselves */
+        memcpy(&dsi->isr_tables_copy, &dsi->isr_tables,
+                sizeof(dsi->isr_tables));
+
+        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;
+        }
+
+        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);
+
+        /* 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(struct platform_device *dsidev)
+{
+        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;
+
+        BUG_ON(isr == NULL);
+
+        /* 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];
+
+                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;
+        }
+
+        if (free_idx == -1)
+                return -EBUSY;
+
+        isr_data = &isr_array[free_idx];
+        isr_data->isr = isr;
+        isr_data->arg = arg;
+        isr_data->mask = mask;
+
+        return 0;
+}
+
+static int _dsi_unregister_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+                struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+        struct dsi_isr_data *isr_data;
+        int i;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+                if (isr_data->isr != isr || isr_data->arg != arg ||
+                                isr_data->mask != mask)
+                        continue;
+
+                isr_data->isr = NULL;
+                isr_data->arg = NULL;
+                isr_data->mask = 0;
+
+                return 0;
+        }
+
+        return -EINVAL;
+}
+
+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;
+        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 int dsi_unregister_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;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table));
+
+        if (r == 0)
+                _omap_dsi_set_irqs(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_register_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_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;
+}
+
+static int dsi_register_isr_cio(struct platform_device *dsidev,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_cio(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_unregister_isr_cio(struct platform_device *dsidev,
+                omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi->irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_cio(dsidev);
+
+        spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+        return r;
+}
+
+static u32 dsi_get_errors(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long flags;
+        u32 e;
+        spin_lock_irqsave(&dsi->errors_lock, flags);
+        e = dsi->errors;
+        dsi->errors = 0;
+        spin_unlock_irqrestore(&dsi->errors_lock, flags);
+        return e;
+}
+
+static int dsi_runtime_get(struct platform_device *dsidev)
+{
+        int r;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        DSSDBG("dsi_runtime_get\n");
+
+        r = pm_runtime_get_sync(&dsi->pdev->dev);
+        WARN_ON(r < 0);
+        return r < 0 ? r : 0;
+}
+
+static void dsi_runtime_put(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int r;
+
+        DSSDBG("dsi_runtime_put\n");
+
+        r = pm_runtime_put_sync(&dsi->pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static int dsi_regulator_init(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct regulator *vdds_dsi;
+        int r;
+
+        if (dsi->vdds_dsi_reg != NULL)
+                return 0;
+
+        vdds_dsi = devm_regulator_get(&dsi->pdev->dev, "vdd");
+
+        if (IS_ERR(vdds_dsi)) {
+                if (PTR_ERR(vdds_dsi) != -EPROBE_DEFER)
+                        DSSERR("can't get DSI VDD regulator\n");
+                return PTR_ERR(vdds_dsi);
+        }
+
+        if (regulator_can_change_voltage(vdds_dsi)) {
+                r = regulator_set_voltage(vdds_dsi, 1800000, 1800000);
+                if (r) {
+                        devm_regulator_put(vdds_dsi);
+                        DSSERR("can't set the DSI regulator voltage\n");
+                        return r;
+                }
+        }
+
+        dsi->vdds_dsi_reg = vdds_dsi;
+
+        return 0;
+}
+
+static void _dsi_print_reset_status(struct platform_device *dsidev)
+{
+        u32 l;
+        int b0, b1, b2;
+
+        /* 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(dsidev, DSI_DSIPHY_CFG5);
+
+        if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC)) {
+                b0 = 28;
+                b1 = 27;
+                b2 = 26;
+        } else {
+                b0 = 24;
+                b1 = 25;
+                b2 = 26;
+        }
+
+#define DSI_FLD_GET(fld, start, end)\
+        FLD_GET(dsi_read_reg(dsidev, DSI_##fld), start, end)
+
+        pr_debug("DSI resets: PLL (%d) CIO (%d) PHY (%x%x%x, %d, %d, %d)\n",
+                DSI_FLD_GET(PLL_STATUS, 0, 0),
+                DSI_FLD_GET(COMPLEXIO_CFG1, 29, 29),
+                DSI_FLD_GET(DSIPHY_CFG5, b0, b0),
+                DSI_FLD_GET(DSIPHY_CFG5, b1, b1),
+                DSI_FLD_GET(DSIPHY_CFG5, b2, b2),
+                DSI_FLD_GET(DSIPHY_CFG5, 29, 29),
+                DSI_FLD_GET(DSIPHY_CFG5, 30, 30),
+                DSI_FLD_GET(DSIPHY_CFG5, 31, 31));
+
+#undef DSI_FLD_GET
+}
+
+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(dsidev, DSI_CTRL, enable, 0, 0); /* IF_EN */
+
+        if (wait_for_bit_change(dsidev, DSI_CTRL, 0, enable) != enable) {
+                        DSSERR("Failed to set dsi_if_enable to %d\n", enable);
+                        return -EIO;
+        }
+
+        return 0;
+}
+
+static unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->pll.cinfo.clkout[HSDIV_DISPC];
+}
+
+static unsigned long dsi_get_pll_hsdiv_dsi_rate(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->pll.cinfo.clkout[HSDIV_DSI];
+}
+
+static unsigned long dsi_get_txbyteclkhs(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        return dsi->pll.cinfo.clkdco / 16;
+}
+
+static unsigned long dsi_fclk_rate(struct platform_device *dsidev)
+{
+        unsigned long r;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dss_get_dsi_clk_source(dsi->module_id) == OMAP_DSS_CLK_SRC_FCK) {
+                /* DSI FCLK source is DSS_CLK_FCK */
+                r = clk_get_rate(dsi->dss_clk);
+        } else {
+                /* DSI FCLK source is dsi_pll_hsdiv_dsi_clk */
+                r = dsi_get_pll_hsdiv_dsi_rate(dsidev);
+        }
+
+        return r;
+}
+
+static int dsi_lp_clock_calc(unsigned long dsi_fclk,
+                unsigned long lp_clk_min, unsigned long lp_clk_max,
+                struct dsi_lp_clock_info *lp_cinfo)
+{
+        unsigned lp_clk_div;
+        unsigned long lp_clk;
+
+        lp_clk_div = DIV_ROUND_UP(dsi_fclk, lp_clk_max * 2);
+        lp_clk = dsi_fclk / 2 / lp_clk_div;
+
+        if (lp_clk < lp_clk_min || lp_clk > lp_clk_max)
+                return -EINVAL;
+
+        lp_cinfo->lp_clk_div = lp_clk_div;
+        lp_cinfo->lp_clk = lp_clk;
+
+        return 0;
+}
+
+static int dsi_set_lp_clk_divisor(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned long dsi_fclk;
+        unsigned lp_clk_div;
+        unsigned long lp_clk;
+        unsigned lpdiv_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_LPDIV);
+
+
+        lp_clk_div = dsi->user_lp_cinfo.lp_clk_div;
+
+        if (lp_clk_div == 0 || lp_clk_div > lpdiv_max)
+                return -EINVAL;
+
+        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_lp_cinfo.lp_clk = lp_clk;
+        dsi->current_lp_cinfo.lp_clk_div = lp_clk_div;
+
+        /* LP_CLK_DIVISOR */
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, lp_clk_div, 12, 0);
+
+        /* 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,
+        DSI_PLL_POWER_ON_HSCLK  = 0x1,
+        DSI_PLL_POWER_ON_ALL    = 0x2,
+        DSI_PLL_POWER_ON_DIV    = 0x3,
+};
+
+static int dsi_pll_power(struct platform_device *dsidev,
+                enum dsi_pll_power_state state)
+{
+        int t = 0;
+
+        /* 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(dsidev, DSI_CLK_CTRL), 29, 28) != state) {
+                if (++t > 1000) {
+                        DSSERR("Failed to set DSI PLL power mode to %d\n",
+                                        state);
+                        return -ENODEV;
+                }
+                udelay(1);
+        }
+
+        return 0;
+}
+
+
+static void dsi_pll_calc_dsi_fck(struct dss_pll_clock_info *cinfo)
+{
+        unsigned long max_dsi_fck;
+
+        max_dsi_fck = dss_feat_get_param_max(FEAT_PARAM_DSI_FCK);
+
+        cinfo->mX[HSDIV_DSI] = DIV_ROUND_UP(cinfo->clkdco, max_dsi_fck);
+        cinfo->clkout[HSDIV_DSI] = cinfo->clkdco / cinfo->mX[HSDIV_DSI];
+}
+
+static int dsi_pll_enable(struct dss_pll *pll)
+{
+        struct dsi_data *dsi = container_of(pll, struct dsi_data, pll);
+        struct platform_device *dsidev = dsi->pdev;
+        int r = 0;
+
+        DSSDBG("PLL init\n");
+
+        r = dsi_regulator_init(dsidev);
+        if (r)
+                return r;
+
+        r = dsi_runtime_get(dsidev);
+        if (r)
+                return r;
+
+        /*
+         * Note: SCP CLK is not required on OMAP3, but it is required on OMAP4.
+         */
+        dsi_enable_scp_clk(dsidev);
+
+        if (!dsi->vdds_dsi_enabled) {
+                r = regulator_enable(dsi->vdds_dsi_reg);
+                if (r)
+                        goto err0;
+                dsi->vdds_dsi_enabled = true;
+        }
+
+        /* XXX PLL does not come out of reset without this... */
+        dispc_pck_free_enable(1);
+
+        if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 0, 1) != 1) {
+                DSSERR("PLL not coming out of reset.\n");
+                r = -ENODEV;
+                dispc_pck_free_enable(0);
+                goto err1;
+        }
+
+        /* XXX ... but if left on, we get problems when planes do not
+         * fill the whole display. No idea about this */
+        dispc_pck_free_enable(0);
+
+        r = dsi_pll_power(dsidev, DSI_PLL_POWER_ON_ALL);
+
+        if (r)
+                goto err1;
+
+        DSSDBG("PLL init done\n");
+
+        return 0;
+err1:
+        if (dsi->vdds_dsi_enabled) {
+                regulator_disable(dsi->vdds_dsi_reg);
+                dsi->vdds_dsi_enabled = false;
+        }
+err0:
+        dsi_disable_scp_clk(dsidev);
+        dsi_runtime_put(dsidev);
+        return r;
+}
+
+static void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        dsi_pll_power(dsidev, DSI_PLL_POWER_OFF);
+        if (disconnect_lanes) {
+                WARN_ON(!dsi->vdds_dsi_enabled);
+                regulator_disable(dsi->vdds_dsi_reg);
+                dsi->vdds_dsi_enabled = false;
+        }
+
+        dsi_disable_scp_clk(dsidev);
+        dsi_runtime_put(dsidev);
+
+        DSSDBG("PLL uninit done\n");
+}
+
+static void dsi_pll_disable(struct dss_pll *pll)
+{
+        struct dsi_data *dsi = container_of(pll, struct dsi_data, pll);
+        struct platform_device *dsidev = dsi->pdev;
+
+        dsi_pll_uninit(dsidev, true);
+}
+
+static void dsi_dump_dsidev_clocks(struct platform_device *dsidev,
+                struct seq_file *s)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dss_pll_clock_info *cinfo = &dsi->pll.cinfo;
+        enum omap_dss_clk_source dispc_clk_src, dsi_clk_src;
+        int dsi_module = dsi->module_id;
+        struct dss_pll *pll = &dsi->pll;
+
+        dispc_clk_src = dss_get_dispc_clk_source();
+        dsi_clk_src = dss_get_dsi_clk_source(dsi_module);
+
+        if (dsi_runtime_get(dsidev))
+                return;
+
+        seq_printf(s,   "- DSI%d PLL -\n", dsi_module + 1);
+
+        seq_printf(s,   "dsi pll clkin\t%lu\n", clk_get_rate(pll->clkin));
+
+        seq_printf(s,   "Fint\t\t%-16lun %u\n", cinfo->fint, cinfo->n);
+
+        seq_printf(s,   "CLKIN4DDR\t%-16lum %u\n",
+                        cinfo->clkdco, cinfo->m);
+
+        seq_printf(s,   "DSI_PLL_HSDIV_DISPC (%s)\t%-16lum_dispc %u\t(%s)\n",
+                        dss_feat_get_clk_source_name(dsi_module == 0 ?
+                                OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+                                OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC),
+                        cinfo->clkout[HSDIV_DISPC],
+                        cinfo->mX[HSDIV_DISPC],
+                        dispc_clk_src == OMAP_DSS_CLK_SRC_FCK ?
+                        "off" : "on");
+
+        seq_printf(s,   "DSI_PLL_HSDIV_DSI (%s)\t%-16lum_dsi %u\t(%s)\n",
+                        dss_feat_get_clk_source_name(dsi_module == 0 ?
+                                OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+                                OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI),
+                        cinfo->clkout[HSDIV_DSI],
+                        cinfo->mX[HSDIV_DSI],
+                        dsi_clk_src == OMAP_DSS_CLK_SRC_FCK ?
+                        "off" : "on");
+
+        seq_printf(s,   "- DSI%d -\n", dsi_module + 1);
+
+        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(dsidev));
+
+        seq_printf(s,   "DDR_CLK\t\t%lu\n",
+                        cinfo->clkdco / 4);
+
+        seq_printf(s,   "TxByteClkHS\t%lu\n", dsi_get_txbyteclkhs(dsidev));
+
+        seq_printf(s,   "LP_CLK\t\t%lu\n", dsi->current_lp_cinfo.lp_clk);
+
+        dsi_runtime_put(dsidev);
+}
+
+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_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+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;
+
+        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;
+
+        spin_unlock_irqrestore(&dsi->irq_stats_lock, flags);
+
+        seq_printf(s, "period %u ms\n",
+                        jiffies_to_msecs(jiffies - stats.last_reset));
+
+        seq_printf(s, "irqs %d\n", stats.irq_count);
+#define PIS(x) \
+        seq_printf(s, "%-20s %10d\n", #x, stats.dsi_irqs[ffs(DSI_IRQ_##x)-1]);
+
+        seq_printf(s, "-- DSI%d interrupts --\n", dsi->module_id + 1);
+        PIS(VC0);
+        PIS(VC1);
+        PIS(VC2);
+        PIS(VC3);
+        PIS(WAKEUP);
+        PIS(RESYNC);
+        PIS(PLL_LOCK);
+        PIS(PLL_UNLOCK);
+        PIS(PLL_RECALL);
+        PIS(COMPLEXIO_ERR);
+        PIS(HS_TX_TIMEOUT);
+        PIS(LP_RX_TIMEOUT);
+        PIS(TE_TRIGGER);
+        PIS(ACK_TRIGGER);
+        PIS(SYNC_LOST);
+        PIS(LDO_POWER_GOOD);
+        PIS(TA_TIMEOUT);
+#undef PIS
+
+#define PIS(x) \
+        seq_printf(s, "%-20s %10d %10d %10d %10d\n", #x, \
+                        stats.vc_irqs[0][ffs(DSI_VC_IRQ_##x)-1], \
+                        stats.vc_irqs[1][ffs(DSI_VC_IRQ_##x)-1], \
+                        stats.vc_irqs[2][ffs(DSI_VC_IRQ_##x)-1], \
+                        stats.vc_irqs[3][ffs(DSI_VC_IRQ_##x)-1]);
+
+        seq_printf(s, "-- VC interrupts --\n");
+        PIS(CS);
+        PIS(ECC_CORR);
+        PIS(PACKET_SENT);
+        PIS(FIFO_TX_OVF);
+        PIS(FIFO_RX_OVF);
+        PIS(BTA);
+        PIS(ECC_NO_CORR);
+        PIS(FIFO_TX_UDF);
+        PIS(PP_BUSY_CHANGE);
+#undef PIS
+
+#define PIS(x) \
+        seq_printf(s, "%-20s %10d\n", #x, \
+                        stats.cio_irqs[ffs(DSI_CIO_IRQ_##x)-1]);
+
+        seq_printf(s, "-- CIO interrupts --\n");
+        PIS(ERRSYNCESC1);
+        PIS(ERRSYNCESC2);
+        PIS(ERRSYNCESC3);
+        PIS(ERRESC1);
+        PIS(ERRESC2);
+        PIS(ERRESC3);
+        PIS(ERRCONTROL1);
+        PIS(ERRCONTROL2);
+        PIS(ERRCONTROL3);
+        PIS(STATEULPS1);
+        PIS(STATEULPS2);
+        PIS(STATEULPS3);
+        PIS(ERRCONTENTIONLP0_1);
+        PIS(ERRCONTENTIONLP1_1);
+        PIS(ERRCONTENTIONLP0_2);
+        PIS(ERRCONTENTIONLP1_2);
+        PIS(ERRCONTENTIONLP0_3);
+        PIS(ERRCONTENTIONLP1_3);
+        PIS(ULPSACTIVENOT_ALL0);
+        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);
+}
+#endif
+
+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(dsidev, r))
+
+        if (dsi_runtime_get(dsidev))
+                return;
+        dsi_enable_scp_clk(dsidev);
+
+        DUMPREG(DSI_REVISION);
+        DUMPREG(DSI_SYSCONFIG);
+        DUMPREG(DSI_SYSSTATUS);
+        DUMPREG(DSI_IRQSTATUS);
+        DUMPREG(DSI_IRQENABLE);
+        DUMPREG(DSI_CTRL);
+        DUMPREG(DSI_COMPLEXIO_CFG1);
+        DUMPREG(DSI_COMPLEXIO_IRQ_STATUS);
+        DUMPREG(DSI_COMPLEXIO_IRQ_ENABLE);
+        DUMPREG(DSI_CLK_CTRL);
+        DUMPREG(DSI_TIMING1);
+        DUMPREG(DSI_TIMING2);
+        DUMPREG(DSI_VM_TIMING1);
+        DUMPREG(DSI_VM_TIMING2);
+        DUMPREG(DSI_VM_TIMING3);
+        DUMPREG(DSI_CLK_TIMING);
+        DUMPREG(DSI_TX_FIFO_VC_SIZE);
+        DUMPREG(DSI_RX_FIFO_VC_SIZE);
+        DUMPREG(DSI_COMPLEXIO_CFG2);
+        DUMPREG(DSI_RX_FIFO_VC_FULLNESS);
+        DUMPREG(DSI_VM_TIMING4);
+        DUMPREG(DSI_TX_FIFO_VC_EMPTINESS);
+        DUMPREG(DSI_VM_TIMING5);
+        DUMPREG(DSI_VM_TIMING6);
+        DUMPREG(DSI_VM_TIMING7);
+        DUMPREG(DSI_STOPCLK_TIMING);
+
+        DUMPREG(DSI_VC_CTRL(0));
+        DUMPREG(DSI_VC_TE(0));
+        DUMPREG(DSI_VC_LONG_PACKET_HEADER(0));
+        DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(0));
+        DUMPREG(DSI_VC_SHORT_PACKET_HEADER(0));
+        DUMPREG(DSI_VC_IRQSTATUS(0));
+        DUMPREG(DSI_VC_IRQENABLE(0));
+
+        DUMPREG(DSI_VC_CTRL(1));
+        DUMPREG(DSI_VC_TE(1));
+        DUMPREG(DSI_VC_LONG_PACKET_HEADER(1));
+        DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(1));
+        DUMPREG(DSI_VC_SHORT_PACKET_HEADER(1));
+        DUMPREG(DSI_VC_IRQSTATUS(1));
+        DUMPREG(DSI_VC_IRQENABLE(1));
+
+        DUMPREG(DSI_VC_CTRL(2));
+        DUMPREG(DSI_VC_TE(2));
+        DUMPREG(DSI_VC_LONG_PACKET_HEADER(2));
+        DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(2));
+        DUMPREG(DSI_VC_SHORT_PACKET_HEADER(2));
+        DUMPREG(DSI_VC_IRQSTATUS(2));
+        DUMPREG(DSI_VC_IRQENABLE(2));
+
+        DUMPREG(DSI_VC_CTRL(3));
+        DUMPREG(DSI_VC_TE(3));
+        DUMPREG(DSI_VC_LONG_PACKET_HEADER(3));
+        DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(3));
+        DUMPREG(DSI_VC_SHORT_PACKET_HEADER(3));
+        DUMPREG(DSI_VC_IRQSTATUS(3));
+        DUMPREG(DSI_VC_IRQENABLE(3));
+
+        DUMPREG(DSI_DSIPHY_CFG0);
+        DUMPREG(DSI_DSIPHY_CFG1);
+        DUMPREG(DSI_DSIPHY_CFG2);
+        DUMPREG(DSI_DSIPHY_CFG5);
+
+        DUMPREG(DSI_PLL_CONTROL);
+        DUMPREG(DSI_PLL_STATUS);
+        DUMPREG(DSI_PLL_GO);
+        DUMPREG(DSI_PLL_CONFIGURATION1);
+        DUMPREG(DSI_PLL_CONFIGURATION2);
+
+        dsi_disable_scp_clk(dsidev);
+        dsi_runtime_put(dsidev);
+#undef DUMPREG
+}
+
+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);
+}
+
+enum dsi_cio_power_state {
+        DSI_COMPLEXIO_POWER_OFF         = 0x0,
+        DSI_COMPLEXIO_POWER_ON          = 0x1,
+        DSI_COMPLEXIO_POWER_ULPS        = 0x2,
+};
+
+static int dsi_cio_power(struct platform_device *dsidev,
+                enum dsi_cio_power_state state)
+{
+        int t = 0;
+
+        /* PWR_CMD */
+        REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG1, state, 28, 27);
+
+        /* PWR_STATUS */
+        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);
+                        return -ENODEV;
+                }
+                udelay(1);
+        }
+
+        return 0;
+}
+
+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 */
+        case 7:
+                return 1920 * 3;        /* 1920x24 bits */
+        default:
+                BUG();
+                return 0;
+        }
+}
+
+static int dsi_set_lane_config(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        static const u8 offsets[] = { 0, 4, 8, 12, 16 };
+        static const enum dsi_lane_function functions[] = {
+                DSI_LANE_CLK,
+                DSI_LANE_DATA1,
+                DSI_LANE_DATA2,
+                DSI_LANE_DATA3,
+                DSI_LANE_DATA4,
+        };
+        u32 r;
+        int i;
+
+        r = dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1);
+
+        for (i = 0; i < dsi->num_lanes_used; ++i) {
+                unsigned offset = offsets[i];
+                unsigned polarity, lane_number;
+                unsigned t;
+
+                for (t = 0; t < dsi->num_lanes_supported; ++t)
+                        if (dsi->lanes[t].function == functions[i])
+                                break;
+
+                if (t == dsi->num_lanes_supported)
+                        return -EINVAL;
+
+                lane_number = t;
+                polarity = dsi->lanes[t].polarity;
+
+                r = FLD_MOD(r, lane_number + 1, offset + 2, offset);
+                r = FLD_MOD(r, polarity, offset + 3, offset + 3);
+        }
+
+        /* clear the unused lanes */
+        for (; i < dsi->num_lanes_supported; ++i) {
+                unsigned offset = offsets[i];
+
+                r = FLD_MOD(r, 0, offset + 2, offset);
+                r = FLD_MOD(r, 0, offset + 3, offset + 3);
+        }
+
+        dsi_write_reg(dsidev, DSI_COMPLEXIO_CFG1, r);
+
+        return 0;
+}
+
+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->pll.cinfo.clkdco / 4;
+        return (ns * (ddr_clk / 1000 / 1000) + 999) / 1000;
+}
+
+static inline unsigned ddr2ns(struct platform_device *dsidev, unsigned ddr)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        unsigned long ddr_clk = dsi->pll.cinfo.clkdco / 4;
+        return ddr * 1000 * 1000 / (ddr_clk / 1000);
+}
+
+static void dsi_cio_timings(struct platform_device *dsidev)
+{
+        u32 r;
+        u32 ths_prepare, ths_prepare_ths_zero, ths_trail, ths_exit;
+        u32 tlpx_half, tclk_trail, tclk_zero;
+        u32 tclk_prepare;
+
+        /* calculate timings */
+
+        /* 1 * DDR_CLK = 2 * UI */
+
+        /* min 40ns + 4*UI      max 85ns + 6*UI */
+        ths_prepare = ns2ddr(dsidev, 70) + 2;
+
+        /* min 145ns + 10*UI */
+        ths_prepare_ths_zero = ns2ddr(dsidev, 175) + 2;
+
+        /* min max(8*UI, 60ns+4*UI) */
+        ths_trail = ns2ddr(dsidev, 60) + 5;
+
+        /* min 100ns */
+        ths_exit = ns2ddr(dsidev, 145);
+
+        /* tlpx min 50n */
+        tlpx_half = ns2ddr(dsidev, 25);
+
+        /* min 60ns */
+        tclk_trail = ns2ddr(dsidev, 60) + 2;
+
+        /* min 38ns, max 95ns */
+        tclk_prepare = ns2ddr(dsidev, 65);
+
+        /* min tclk-prepare + tclk-zero = 300ns */
+        tclk_zero = ns2ddr(dsidev, 260);
+
+        DSSDBG("ths_prepare %u (%uns), ths_prepare_ths_zero %u (%uns)\n",
+                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(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(dsidev, tlpx_half),
+                        tclk_trail, ddr2ns(dsidev, tclk_trail),
+                        tclk_zero, ddr2ns(dsidev, tclk_zero));
+        DSSDBG("tclk_prepare %u (%uns)\n",
+                        tclk_prepare, ddr2ns(dsidev, tclk_prepare));
+
+        /* program timings */
+
+        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(dsidev, DSI_DSIPHY_CFG0, r);
+
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
+        r = FLD_MOD(r, tlpx_half, 20, 16);
+        r = FLD_MOD(r, tclk_trail, 15, 8);
+        r = FLD_MOD(r, tclk_zero, 7, 0);
+
+        if (dss_has_feature(FEAT_DSI_PHY_DCC)) {
+                r = FLD_MOD(r, 0, 21, 21);      /* DCCEN = disable */
+                r = FLD_MOD(r, 1, 22, 22);      /* CLKINP_DIVBY2EN = enable */
+                r = FLD_MOD(r, 1, 23, 23);      /* CLKINP_SEL = enable */
+        }
+
+        dsi_write_reg(dsidev, DSI_DSIPHY_CFG1, r);
+
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
+        r = FLD_MOD(r, tclk_prepare, 7, 0);
+        dsi_write_reg(dsidev, DSI_DSIPHY_CFG2, r);
+}
+
+/* lane masks have lane 0 at lsb. mask_p for positive lines, n for negative */
+static void dsi_cio_enable_lane_override(struct platform_device *dsidev,
+                unsigned mask_p, unsigned mask_n)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int i;
+        u32 l;
+        u8 lptxscp_start = dsi->num_lanes_supported == 3 ? 22 : 26;
+
+        l = 0;
+
+        for (i = 0; i < dsi->num_lanes_supported; ++i) {
+                unsigned p = dsi->lanes[i].polarity;
+
+                if (mask_p & (1 << i))
+                        l |= 1 << (i * 2 + (p ? 0 : 1));
+
+                if (mask_n & (1 << i))
+                        l |= 1 << (i * 2 + (p ? 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_cio_wait_tx_clk_esc_reset(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int t, i;
+        bool in_use[DSI_MAX_NR_LANES];
+        static const u8 offsets_old[] = { 28, 27, 26 };
+        static const u8 offsets_new[] = { 24, 25, 26, 27, 28 };
+        const u8 *offsets;
+
+        if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC))
+                offsets = offsets_old;
+        else
+                offsets = offsets_new;
+
+        for (i = 0; i < dsi->num_lanes_supported; ++i)
+                in_use[i] = dsi->lanes[i].function != DSI_LANE_UNUSED;
+
+        t = 100000;
+        while (true) {
+                u32 l;
+                int ok;
+
+                l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+
+                ok = 0;
+                for (i = 0; i < dsi->num_lanes_supported; ++i) {
+                        if (!in_use[i] || (l & (1 << offsets[i])))
+                                ok++;
+                }
+
+                if (ok == dsi->num_lanes_supported)
+                        break;
+
+                if (--t == 0) {
+                        for (i = 0; i < dsi->num_lanes_supported; ++i) {
+                                if (!in_use[i] || (l & (1 << offsets[i])))
+                                        continue;
+
+                                DSSERR("CIO TXCLKESC%d domain not coming " \
+                                                "out of reset\n", i);
+                        }
+                        return -EIO;
+                }
+        }
+
+        return 0;
+}
+
+/* return bitmask of enabled lanes, lane0 being the lsb */
+static unsigned dsi_get_lane_mask(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned mask = 0;
+        int i;
+
+        for (i = 0; i < dsi->num_lanes_supported; ++i) {
+                if (dsi->lanes[i].function != DSI_LANE_UNUSED)
+                        mask |= 1 << i;
+        }
+
+        return mask;
+}
+
+static int dsi_cio_init(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int r;
+        u32 l;
+
+        DSSDBG("DSI CIO init starts");
+
+        r = dss_dsi_enable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
+        if (r)
+                return r;
+
+        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(dsidev, DSI_DSIPHY_CFG5);
+
+        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;
+        }
+
+        r = dsi_set_lane_config(dsidev);
+        if (r)
+                goto err_scp_clk_dom;
+
+        /* 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) {
+                unsigned mask_p;
+                int i;
+
+                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 by setting positive lines high and negative lines
+                 * low for 1ms.
+                 */
+
+                mask_p = 0;
+
+                for (i = 0; i < dsi->num_lanes_supported; ++i) {
+                        if (dsi->lanes[i].function == DSI_LANE_UNUSED)
+                                continue;
+                        mask_p |= 1 << i;
+                }
+
+                dsi_cio_enable_lane_override(dsidev, mask_p, 0);
+        }
+
+        r = dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ON);
+        if (r)
+                goto err_cio_pwr;
+
+        if (wait_for_bit_change(dsidev, DSI_COMPLEXIO_CFG1, 29, 1) != 1) {
+                DSSERR("CIO PWR clock domain not coming out of reset.\n");
+                r = -ENODEV;
+                goto err_cio_pwr_dom;
+        }
+
+        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(dsidev);
+        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);
+        }
+
+        /* FORCE_TX_STOP_MODE_IO */
+        REG_FLD_MOD(dsidev, DSI_TIMING1, 0, 15, 15);
+
+        dsi_cio_timings(dsidev);
+
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                /* DDR_CLK_ALWAYS_ON */
+                REG_FLD_MOD(dsidev, DSI_CLK_CTRL,
+                        dsi->vm_timings.ddr_clk_always_on, 13, 13);
+        }
+
+        dsi->ulps_enabled = false;
+
+        DSSDBG("CIO init done\n");
+
+        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);
+        dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
+        return r;
+}
+
+static void dsi_cio_uninit(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        /* DDR_CLK_ALWAYS_ON */
+        REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 13, 13);
+
+        dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+        dsi_disable_scp_clk(dsidev);
+        dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
+}
+
+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].tx_fifo_size = size1;
+        dsi->vc[1].tx_fifo_size = size2;
+        dsi->vc[2].tx_fifo_size = size3;
+        dsi->vc[3].tx_fifo_size = size4;
+
+        for (i = 0; i < 4; i++) {
+                u8 v;
+                int size = dsi->vc[i].tx_fifo_size;
+
+                if (add + size > 4) {
+                        DSSERR("Illegal FIFO configuration\n");
+                        BUG();
+                        return;
+                }
+
+                v = FLD_VAL(add, 2, 0) | FLD_VAL(size, 7, 4);
+                r |= v << (8 * i);
+                /*DSSDBG("TX FIFO vc %d: size %d, add %d\n", i, size, add); */
+                add += size;
+        }
+
+        dsi_write_reg(dsidev, DSI_TX_FIFO_VC_SIZE, r);
+}
+
+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].rx_fifo_size = size1;
+        dsi->vc[1].rx_fifo_size = size2;
+        dsi->vc[2].rx_fifo_size = size3;
+        dsi->vc[3].rx_fifo_size = size4;
+
+        for (i = 0; i < 4; i++) {
+                u8 v;
+                int size = dsi->vc[i].rx_fifo_size;
+
+                if (add + size > 4) {
+                        DSSERR("Illegal FIFO configuration\n");
+                        BUG();
+                        return;
+                }
+
+                v = FLD_VAL(add, 2, 0) | FLD_VAL(size, 7, 4);
+                r |= v << (8 * i);
+                /*DSSDBG("RX FIFO vc %d: size %d, add %d\n", i, size, add); */
+                add += size;
+        }
+
+        dsi_write_reg(dsidev, DSI_RX_FIFO_VC_SIZE, r);
+}
+
+static int dsi_force_tx_stop_mode_io(struct platform_device *dsidev)
+{
+        u32 r;
+
+        r = dsi_read_reg(dsidev, DSI_TIMING1);
+        r = FLD_MOD(r, 1, 15, 15);      /* FORCE_TX_STOP_MODE_IO */
+        dsi_write_reg(dsidev, DSI_TIMING1, r);
+
+        if (wait_for_bit_change(dsidev, DSI_TIMING1, 15, 0) != 0) {
+                DSSERR("TX_STOP bit not going down\n");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+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 = dsidev,
+                .completion = &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 = dsidev,
+                .completion = &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].source) {
+        case DSI_VC_SOURCE_VP:
+                return dsi_sync_vc_vp(dsidev, channel);
+        case DSI_VC_SOURCE_L4:
+                return dsi_sync_vc_l4(dsidev, channel);
+        default:
+                BUG();
+                return -EINVAL;
+        }
+}
+
+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(dsidev, DSI_VC_CTRL(channel), enable, 0, 0);
+
+        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;
+        }
+
+        return 0;
+}
+
+static void dsi_vc_initial_config(struct platform_device *dsidev, int channel)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        u32 r;
+
+        DSSDBG("Initial config of virtual channel %d", 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",
+                                channel);
+
+        r = FLD_MOD(r, 0, 1, 1); /* SOURCE, 0 = L4 */
+        r = FLD_MOD(r, 0, 2, 2); /* BTA_SHORT_EN  */
+        r = FLD_MOD(r, 0, 3, 3); /* BTA_LONG_EN */
+        r = FLD_MOD(r, 0, 4, 4); /* MODE, 0 = command */
+        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(dsidev, DSI_VC_CTRL(channel), r);
+
+        dsi->vc[channel].source = DSI_VC_SOURCE_L4;
+}
+
+static int dsi_vc_config_source(struct platform_device *dsidev, int channel,
+                enum dsi_vc_source source)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->vc[channel].source == source)
+                return 0;
+
+        DSSDBG("Source config of virtual channel %d", channel);
+
+        dsi_sync_vc(dsidev, channel);
+
+        dsi_vc_enable(dsidev, channel, 0);
+
+        /* VC_BUSY */
+        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;
+        }
+
+        /* SOURCE, 0 = L4, 1 = video port */
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), source, 1, 1);
+
+        /* DCS_CMD_ENABLE */
+        if (dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC)) {
+                bool enable = source == DSI_VC_SOURCE_VP;
+                REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 30, 30);
+        }
+
+        dsi_vc_enable(dsidev, channel, 1);
+
+        dsi->vc[channel].source = source;
+
+        return 0;
+}
+
+static void dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
+                bool enable)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        DSSDBG("dsi_vc_enable_hs(%d, %d)\n", channel, enable);
+
+        WARN_ON(!dsi_bus_is_locked(dsidev));
+
+        dsi_vc_enable(dsidev, channel, 0);
+        dsi_if_enable(dsidev, 0);
+
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 9, 9);
+
+        dsi_vc_enable(dsidev, channel, 1);
+        dsi_if_enable(dsidev, 1);
+
+        dsi_force_tx_stop_mode_io(dsidev);
+
+        /* start the DDR clock by sending a NULL packet */
+        if (dsi->vm_timings.ddr_clk_always_on && enable)
+                dsi_vc_send_null(dssdev, channel);
+}
+
+static void dsi_vc_flush_long_data(struct platform_device *dsidev, int channel)
+{
+        while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
+                u32 val;
+                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,
+                                (val >> 16) & 0xff,
+                                (val >> 24) & 0xff);
+        }
+}
+
+static void dsi_show_rx_ack_with_err(u16 err)
+{
+        DSSERR("\tACK with ERROR (%#x):\n", err);
+        if (err & (1 << 0))
+                DSSERR("\t\tSoT Error\n");
+        if (err & (1 << 1))
+                DSSERR("\t\tSoT Sync Error\n");
+        if (err & (1 << 2))
+                DSSERR("\t\tEoT Sync Error\n");
+        if (err & (1 << 3))
+                DSSERR("\t\tEscape Mode Entry Command Error\n");
+        if (err & (1 << 4))
+                DSSERR("\t\tLP Transmit Sync Error\n");
+        if (err & (1 << 5))
+                DSSERR("\t\tHS Receive Timeout Error\n");
+        if (err & (1 << 6))
+                DSSERR("\t\tFalse Control Error\n");
+        if (err & (1 << 7))
+                DSSERR("\t\t(reserved7)\n");
+        if (err & (1 << 8))
+                DSSERR("\t\tECC Error, single-bit (corrected)\n");
+        if (err & (1 << 9))
+                DSSERR("\t\tECC Error, multi-bit (not corrected)\n");
+        if (err & (1 << 10))
+                DSSERR("\t\tChecksum Error\n");
+        if (err & (1 << 11))
+                DSSERR("\t\tData type not recognized\n");
+        if (err & (1 << 12))
+                DSSERR("\t\tInvalid VC ID\n");
+        if (err & (1 << 13))
+                DSSERR("\t\tInvalid Transmission Length\n");
+        if (err & (1 << 14))
+                DSSERR("\t\t(reserved14)\n");
+        if (err & (1 << 15))
+                DSSERR("\t\tDSI Protocol Violation\n");
+}
+
+static u16 dsi_vc_flush_receive_data(struct platform_device *dsidev,
+                int channel)
+{
+        /* RX_FIFO_NOT_EMPTY */
+        while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
+                u32 val;
+                u8 dt;
+                val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+                DSSERR("\trawval %#08x\n", val);
+                dt = FLD_GET(val, 5, 0);
+                if (dt == MIPI_DSI_RX_ACKNOWLEDGE_AND_ERROR_REPORT) {
+                        u16 err = FLD_GET(val, 23, 8);
+                        dsi_show_rx_ack_with_err(err);
+                } else if (dt == MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_1BYTE) {
+                        DSSERR("\tDCS short response, 1 byte: %#x\n",
+                                        FLD_GET(val, 23, 8));
+                } else if (dt == MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_2BYTE) {
+                        DSSERR("\tDCS short response, 2 byte: %#x\n",
+                                        FLD_GET(val, 23, 8));
+                } else if (dt == MIPI_DSI_RX_DCS_LONG_READ_RESPONSE) {
+                        DSSERR("\tDCS long response, len %d\n",
+                                        FLD_GET(val, 23, 8));
+                        dsi_vc_flush_long_data(dsidev, channel);
+                } else {
+                        DSSERR("\tunknown datatype 0x%02x\n", dt);
+                }
+        }
+        return 0;
+}
+
+static int dsi_vc_send_bta(struct platform_device *dsidev, int channel)
+{
+        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(dsidev));
+
+        /* 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(dsidev, channel);
+        }
+
+        REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 6, 6); /* BTA_EN */
+
+        /* flush posted write */
+        dsi_read_reg(dsidev, DSI_VC_CTRL(channel));
+
+        return 0;
+}
+
+static 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;
+
+        r = dsi_register_isr_vc(dsidev, channel, dsi_completion_handler,
+                        &completion, DSI_VC_IRQ_BTA);
+        if (r)
+                goto err0;
+
+        r = dsi_register_isr(dsidev, dsi_completion_handler, &completion,
+                        DSI_IRQ_ERROR_MASK);
+        if (r)
+                goto err1;
+
+        r = dsi_vc_send_bta(dsidev, channel);
+        if (r)
+                goto err2;
+
+        if (wait_for_completion_timeout(&completion,
+                                msecs_to_jiffies(500)) == 0) {
+                DSSERR("Failed to receive BTA\n");
+                r = -EIO;
+                goto err2;
+        }
+
+        err = dsi_get_errors(dsidev);
+        if (err) {
+                DSSERR("Error while sending BTA: %x\n", err);
+                r = -EIO;
+                goto err2;
+        }
+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;
+}
+
+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(dsidev));
+
+        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(dsidev, DSI_VC_LONG_PACKET_HEADER(channel), val);
+}
+
+static inline void dsi_vc_write_long_payload(struct platform_device *dsidev,
+                int channel, u8 b1, u8 b2, u8 b3, u8 b4)
+{
+        u32 val;
+
+        val = b4 << 24 | b3 << 16 | b2 << 8  | b1 << 0;
+
+/*      DSSDBG("\twriting %02x, %02x, %02x, %02x (%#010x)\n",
+                        b1, b2, b3, b4, val); */
+
+        dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_PAYLOAD(channel), val);
+}
+
+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)
+                DSSDBG("dsi_vc_send_long, %d bytes\n", len);
+
+        /* len + header */
+        if (dsi->vc[channel].tx_fifo_size * 32 * 4 < len + 4) {
+                DSSERR("unable to send long packet: packet too long.\n");
+                return -EINVAL;
+        }
+
+        dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_L4);
+
+        dsi_vc_write_long_header(dsidev, channel, data_type, len, ecc);
+
+        p = data;
+        for (i = 0; i < len >> 2; i++) {
+                if (dsi->debug_write)
+                        DSSDBG("\tsending full packet %d\n", i);
+
+                b1 = *p++;
+                b2 = *p++;
+                b3 = *p++;
+                b4 = *p++;
+
+                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)
+                        DSSDBG("\tsending remainder bytes %d\n", i);
+
+                switch (i) {
+                case 3:
+                        b1 = *p++;
+                        b2 = *p++;
+                        b3 = *p++;
+                        break;
+                case 2:
+                        b1 = *p++;
+                        b2 = *p++;
+                        break;
+                case 1:
+                        b1 = *p++;
+                        break;
+                }
+
+                dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, 0);
+        }
+
+        return r;
+}
+
+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(dsidev));
+
+        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_source(dsidev, channel, DSI_VC_SOURCE_L4);
+
+        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 | dsi->vc[channel].vc_id << 6;
+
+        r = (data_id << 0) | (data << 8) | (ecc << 24);
+
+        dsi_write_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel), r);
+
+        return 0;
+}
+
+static int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+        return dsi_vc_send_long(dsidev, channel, MIPI_DSI_NULL_PACKET, NULL,
+                0, 0);
+}
+
+static int dsi_vc_write_nosync_common(struct platform_device *dsidev,
+                int channel, u8 *data, int len, enum dss_dsi_content_type type)
+{
+        int r;
+
+        if (len == 0) {
+                BUG_ON(type == DSS_DSI_CONTENT_DCS);
+                r = dsi_vc_send_short(dsidev, channel,
+                                MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM, 0, 0);
+        } else if (len == 1) {
+                r = dsi_vc_send_short(dsidev, channel,
+                                type == DSS_DSI_CONTENT_GENERIC ?
+                                MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM :
+                                MIPI_DSI_DCS_SHORT_WRITE, data[0], 0);
+        } else if (len == 2) {
+                r = dsi_vc_send_short(dsidev, channel,
+                                type == DSS_DSI_CONTENT_GENERIC ?
+                                MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM :
+                                MIPI_DSI_DCS_SHORT_WRITE_PARAM,
+                                data[0] | (data[1] << 8), 0);
+        } else {
+                r = dsi_vc_send_long(dsidev, channel,
+                                type == DSS_DSI_CONTENT_GENERIC ?
+                                MIPI_DSI_GENERIC_LONG_WRITE :
+                                MIPI_DSI_DCS_LONG_WRITE, data, len, 0);
+        }
+
+        return r;
+}
+
+static 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);
+
+        return dsi_vc_write_nosync_common(dsidev, channel, data, len,
+                        DSS_DSI_CONTENT_DCS);
+}
+
+static int dsi_vc_generic_write_nosync(struct omap_dss_device *dssdev, int channel,
+                u8 *data, int len)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+        return dsi_vc_write_nosync_common(dsidev, channel, data, len,
+                        DSS_DSI_CONTENT_GENERIC);
+}
+
+static int dsi_vc_write_common(struct omap_dss_device *dssdev, int channel,
+                u8 *data, int len, enum dss_dsi_content_type type)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        int r;
+
+        r = dsi_vc_write_nosync_common(dsidev, channel, data, len, type);
+        if (r)
+                goto err;
+
+        r = dsi_vc_send_bta_sync(dssdev, channel);
+        if (r)
+                goto err;
+
+        /* 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(dsidev, channel);
+                r = -EIO;
+                goto err;
+        }
+
+        return 0;
+err:
+        DSSERR("dsi_vc_write_common(ch %d, cmd 0x%02x, len %d) failed\n",
+                        channel, data[0], len);
+        return r;
+}
+
+static int dsi_vc_dcs_write(struct omap_dss_device *dssdev, int channel, u8 *data,
+                int len)
+{
+        return dsi_vc_write_common(dssdev, channel, data, len,
+                        DSS_DSI_CONTENT_DCS);
+}
+
+static int dsi_vc_generic_write(struct omap_dss_device *dssdev, int channel, u8 *data,
+                int len)
+{
+        return dsi_vc_write_common(dssdev, channel, data, len,
+                        DSS_DSI_CONTENT_GENERIC);
+}
+
+static int dsi_vc_dcs_send_read_request(struct platform_device *dsidev,
+                int channel, u8 dcs_cmd)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int r;
+
+        if (dsi->debug_read)
+                DSSDBG("dsi_vc_dcs_send_read_request(ch%d, dcs_cmd %x)\n",
+                        channel, dcs_cmd);
+
+        r = dsi_vc_send_short(dsidev, channel, MIPI_DSI_DCS_READ, dcs_cmd, 0);
+        if (r) {
+                DSSERR("dsi_vc_dcs_send_read_request(ch %d, cmd 0x%02x)"
+                        " failed\n", channel, dcs_cmd);
+                return r;
+        }
+
+        return 0;
+}
+
+static int dsi_vc_generic_send_read_request(struct platform_device *dsidev,
+                int channel, u8 *reqdata, int reqlen)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        u16 data;
+        u8 data_type;
+        int r;
+
+        if (dsi->debug_read)
+                DSSDBG("dsi_vc_generic_send_read_request(ch %d, reqlen %d)\n",
+                        channel, reqlen);
+
+        if (reqlen == 0) {
+                data_type = MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM;
+                data = 0;
+        } else if (reqlen == 1) {
+                data_type = MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM;
+                data = reqdata[0];
+        } else if (reqlen == 2) {
+                data_type = MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM;
+                data = reqdata[0] | (reqdata[1] << 8);
+        } else {
+                BUG();
+                return -EINVAL;
+        }
+
+        r = dsi_vc_send_short(dsidev, channel, data_type, data, 0);
+        if (r) {
+                DSSERR("dsi_vc_generic_send_read_request(ch %d, reqlen %d)"
+                        " failed\n", channel, reqlen);
+                return r;
+        }
+
+        return 0;
+}
+
+static int dsi_vc_read_rx_fifo(struct platform_device *dsidev, int channel,
+                u8 *buf, int buflen, enum dss_dsi_content_type type)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        u32 val;
+        u8 dt;
+        int r;
+
+        /* RX_FIFO_NOT_EMPTY */
+        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(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+        if (dsi->debug_read)
+                DSSDBG("\theader: %08x\n", val);
+        dt = FLD_GET(val, 5, 0);
+        if (dt == MIPI_DSI_RX_ACKNOWLEDGE_AND_ERROR_REPORT) {
+                u16 err = FLD_GET(val, 23, 8);
+                dsi_show_rx_ack_with_err(err);
+                r = -EIO;
+                goto err;
+
+        } else if (dt == (type == DSS_DSI_CONTENT_GENERIC ?
+                        MIPI_DSI_RX_GENERIC_SHORT_READ_RESPONSE_1BYTE :
+                        MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_1BYTE)) {
+                u8 data = FLD_GET(val, 15, 8);
+                if (dsi->debug_read)
+                        DSSDBG("\t%s short response, 1 byte: %02x\n",
+                                type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" :
+                                "DCS", data);
+
+                if (buflen < 1) {
+                        r = -EIO;
+                        goto err;
+                }
+
+                buf[0] = data;
+
+                return 1;
+        } else if (dt == (type == DSS_DSI_CONTENT_GENERIC ?
+                        MIPI_DSI_RX_GENERIC_SHORT_READ_RESPONSE_2BYTE :
+                        MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_2BYTE)) {
+                u16 data = FLD_GET(val, 23, 8);
+                if (dsi->debug_read)
+                        DSSDBG("\t%s short response, 2 byte: %04x\n",
+                                type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" :
+                                "DCS", data);
+
+                if (buflen < 2) {
+                        r = -EIO;
+                        goto err;
+                }
+
+                buf[0] = data & 0xff;
+                buf[1] = (data >> 8) & 0xff;
+
+                return 2;
+        } else if (dt == (type == DSS_DSI_CONTENT_GENERIC ?
+                        MIPI_DSI_RX_GENERIC_LONG_READ_RESPONSE :
+                        MIPI_DSI_RX_DCS_LONG_READ_RESPONSE)) {
+                int w;
+                int len = FLD_GET(val, 23, 8);
+                if (dsi->debug_read)
+                        DSSDBG("\t%s long response, len %d\n",
+                                type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" :
+                                "DCS", len);
+
+                if (len > buflen) {
+                        r = -EIO;
+                        goto err;
+                }
+
+                /* two byte checksum ends the packet, not included in len */
+                for (w = 0; w < len + 2;) {
+                        int b;
+                        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,
+                                                (val >> 16) & 0xff,
+                                                (val >> 24) & 0xff);
+
+                        for (b = 0; b < 4; ++b) {
+                                if (w < len)
+                                        buf[w] = (val >> (b * 8)) & 0xff;
+                                /* we discard the 2 byte checksum */
+                                ++w;
+                        }
+                }
+
+                return len;
+        } else {
+                DSSERR("\tunknown datatype 0x%02x\n", dt);
+                r = -EIO;
+                goto err;
+        }
+
+err:
+        DSSERR("dsi_vc_read_rx_fifo(ch %d type %s) failed\n", channel,
+                type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" : "DCS");
+
+        return r;
+}
+
+static 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);
+        int r;
+
+        r = dsi_vc_dcs_send_read_request(dsidev, channel, dcs_cmd);
+        if (r)
+                goto err;
+
+        r = dsi_vc_send_bta_sync(dssdev, channel);
+        if (r)
+                goto err;
+
+        r = dsi_vc_read_rx_fifo(dsidev, channel, buf, buflen,
+                DSS_DSI_CONTENT_DCS);
+        if (r < 0)
+                goto err;
+
+        if (r != buflen) {
+                r = -EIO;
+                goto err;
+        }
+
+        return 0;
+err:
+        DSSERR("dsi_vc_dcs_read(ch %d, cmd 0x%02x) failed\n", channel, dcs_cmd);
+        return r;
+}
+
+static int dsi_vc_generic_read(struct omap_dss_device *dssdev, int channel,
+                u8 *reqdata, int reqlen, u8 *buf, int buflen)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        int r;
+
+        r = dsi_vc_generic_send_read_request(dsidev, channel, reqdata, reqlen);
+        if (r)
+                return r;
+
+        r = dsi_vc_send_bta_sync(dssdev, channel);
+        if (r)
+                return r;
+
+        r = dsi_vc_read_rx_fifo(dsidev, channel, buf, buflen,
+                DSS_DSI_CONTENT_GENERIC);
+        if (r < 0)
+                return r;
+
+        if (r != buflen) {
+                r = -EIO;
+                return r;
+        }
+
+        return 0;
+}
+
+static int dsi_vc_set_max_rx_packet_size(struct omap_dss_device *dssdev, int channel,
+                u16 len)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+        return dsi_vc_send_short(dsidev, channel,
+                        MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE, len, 0);
+}
+
+static int dsi_enter_ulps(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        DECLARE_COMPLETION_ONSTACK(completion);
+        int r, i;
+        unsigned mask;
+
+        DSSDBG("Entering ULPS");
+
+        WARN_ON(!dsi_bus_is_locked(dsidev));
+
+        WARN_ON(dsi->ulps_enabled);
+
+        if (dsi->ulps_enabled)
+                return 0;
+
+        /* DDR_CLK_ALWAYS_ON */
+        if (REG_GET(dsidev, DSI_CLK_CTRL, 13, 13)) {
+                dsi_if_enable(dsidev, 0);
+                REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 13, 13);
+                dsi_if_enable(dsidev, 1);
+        }
+
+        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;
+
+        mask = 0;
+
+        for (i = 0; i < dsi->num_lanes_supported; ++i) {
+                if (dsi->lanes[i].function == DSI_LANE_UNUSED)
+                        continue;
+                mask |= 1 << i;
+        }
+        /* Assert TxRequestEsc for data lanes and TxUlpsClk for clk lane */
+        /* LANEx_ULPS_SIG2 */
+        REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, mask, 9, 5);
+
+        /* flush posted write and wait for SCP interface to finish the write */
+        dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG2);
+
+        if (wait_for_completion_timeout(&completion,
+                                msecs_to_jiffies(1000)) == 0) {
+                DSSERR("ULPS enable timeout\n");
+                r = -EIO;
+                goto err;
+        }
+
+        dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+                        DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+
+        /* Reset LANEx_ULPS_SIG2 */
+        REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, 0, 9, 5);
+
+        /* flush posted write and wait for SCP interface to finish the write */
+        dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG2);
+
+        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;
+        u32 r;
+
+        BUG_ON(ticks > 0x1fff);
+
+        /* ticks in DSI_FCK */
+        fck = dsi_fclk_rate(dsidev);
+
+        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(dsidev, DSI_TIMING2, r);
+
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("LP_RX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_set_ta_timeout(struct platform_device *dsidev, unsigned ticks,
+                bool x8, bool x16)
+{
+        unsigned long fck;
+        unsigned long total_ticks;
+        u32 r;
+
+        BUG_ON(ticks > 0x1fff);
+
+        /* ticks in DSI_FCK */
+        fck = dsi_fclk_rate(dsidev);
+
+        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(dsidev, DSI_TIMING1, r);
+
+        total_ticks = ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1);
+
+        DSSDBG("TA_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x8 ? " x8" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_set_stop_state_counter(struct platform_device *dsidev,
+                unsigned ticks, bool x4, bool x16)
+{
+        unsigned long fck;
+        unsigned long total_ticks;
+        u32 r;
+
+        BUG_ON(ticks > 0x1fff);
+
+        /* ticks in DSI_FCK */
+        fck = dsi_fclk_rate(dsidev);
+
+        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(dsidev, DSI_TIMING1, r);
+
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("STOP_STATE_COUNTER %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_set_hs_tx_timeout(struct platform_device *dsidev,
+                unsigned ticks, bool x4, bool x16)
+{
+        unsigned long fck;
+        unsigned long total_ticks;
+        u32 r;
+
+        BUG_ON(ticks > 0x1fff);
+
+        /* ticks in TxByteClkHS */
+        fck = dsi_get_txbyteclkhs(dsidev);
+
+        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(dsidev, DSI_TIMING2, r);
+
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("HS_TX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_config_vp_num_line_buffers(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int num_line_buffers;
+
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+                struct omap_video_timings *timings = &dsi->timings;
+                /*
+                 * Don't use line buffers if width is greater than the video
+                 * port's line buffer size
+                 */
+                if (dsi->line_buffer_size <= timings->x_res * bpp / 8)
+                        num_line_buffers = 0;
+                else
+                        num_line_buffers = 2;
+        } else {
+                /* Use maximum number of line buffers in command mode */
+                num_line_buffers = 2;
+        }
+
+        /* LINE_BUFFER */
+        REG_FLD_MOD(dsidev, DSI_CTRL, num_line_buffers, 13, 12);
+}
+
+static void dsi_config_vp_sync_events(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        bool sync_end;
+        u32 r;
+
+        if (dsi->vm_timings.trans_mode == OMAP_DSS_DSI_PULSE_MODE)
+                sync_end = true;
+        else
+                sync_end = false;
+
+        r = dsi_read_reg(dsidev, DSI_CTRL);
+        r = FLD_MOD(r, 1, 9, 9);                /* VP_DE_POL */
+        r = FLD_MOD(r, 1, 10, 10);              /* VP_HSYNC_POL */
+        r = FLD_MOD(r, 1, 11, 11);              /* VP_VSYNC_POL */
+        r = FLD_MOD(r, 1, 15, 15);              /* VP_VSYNC_START */
+        r = FLD_MOD(r, sync_end, 16, 16);       /* VP_VSYNC_END */
+        r = FLD_MOD(r, 1, 17, 17);              /* VP_HSYNC_START */
+        r = FLD_MOD(r, sync_end, 18, 18);       /* VP_HSYNC_END */
+        dsi_write_reg(dsidev, DSI_CTRL, r);
+}
+
+static void dsi_config_blanking_modes(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int blanking_mode = dsi->vm_timings.blanking_mode;
+        int hfp_blanking_mode = dsi->vm_timings.hfp_blanking_mode;
+        int hbp_blanking_mode = dsi->vm_timings.hbp_blanking_mode;
+        int hsa_blanking_mode = dsi->vm_timings.hsa_blanking_mode;
+        u32 r;
+
+        /*
+         * 0 = TX FIFO packets sent or LPS in corresponding blanking periods
+         * 1 = Long blanking packets are sent in corresponding blanking periods
+         */
+        r = dsi_read_reg(dsidev, DSI_CTRL);
+        r = FLD_MOD(r, blanking_mode, 20, 20);          /* BLANKING_MODE */
+        r = FLD_MOD(r, hfp_blanking_mode, 21, 21);      /* HFP_BLANKING */
+        r = FLD_MOD(r, hbp_blanking_mode, 22, 22);      /* HBP_BLANKING */
+        r = FLD_MOD(r, hsa_blanking_mode, 23, 23);      /* HSA_BLANKING */
+        dsi_write_reg(dsidev, DSI_CTRL, r);
+}
+
+/*
+ * According to section 'HS Command Mode Interleaving' in OMAP TRM, Scenario 3
+ * results in maximum transition time for data and clock lanes to enter and
+ * exit HS mode. Hence, this is the scenario where the least amount of command
+ * mode data can be interleaved. We program the minimum amount of TXBYTECLKHS
+ * clock cycles that can be used to interleave command mode data in HS so that
+ * all scenarios are satisfied.
+ */
+static int dsi_compute_interleave_hs(int blank, bool ddr_alwon, int enter_hs,
+                int exit_hs, int exiths_clk, int ddr_pre, int ddr_post)
+{
+        int transition;
+
+        /*
+         * If DDR_CLK_ALWAYS_ON is set, we need to consider HS mode transition
+         * time of data lanes only, if it isn't set, we need to consider HS
+         * transition time of both data and clock lanes. HS transition time
+         * of Scenario 3 is considered.
+         */
+        if (ddr_alwon) {
+                transition = enter_hs + exit_hs + max(enter_hs, 2) + 1;
+        } else {
+                int trans1, trans2;
+                trans1 = ddr_pre + enter_hs + exit_hs + max(enter_hs, 2) + 1;
+                trans2 = ddr_pre + enter_hs + exiths_clk + ddr_post + ddr_pre +
+                                enter_hs + 1;
+                transition = max(trans1, trans2);
+        }
+
+        return blank > transition ? blank - transition : 0;
+}
+
+/*
+ * According to section 'LP Command Mode Interleaving' in OMAP TRM, Scenario 1
+ * results in maximum transition time for data lanes to enter and exit LP mode.
+ * Hence, this is the scenario where the least amount of command mode data can
+ * be interleaved. We program the minimum amount of bytes that can be
+ * interleaved in LP so that all scenarios are satisfied.
+ */
+static int dsi_compute_interleave_lp(int blank, int enter_hs, int exit_hs,
+                int lp_clk_div, int tdsi_fclk)
+{
+        int trans_lp;   /* time required for a LP transition, in TXBYTECLKHS */
+        int tlp_avail;  /* time left for interleaving commands, in CLKIN4DDR */
+        int ttxclkesc;  /* period of LP transmit escape clock, in CLKIN4DDR */
+        int thsbyte_clk = 16;   /* Period of TXBYTECLKHS clock, in CLKIN4DDR */
+        int lp_inter;   /* cmd mode data that can be interleaved, in bytes */
+
+        /* maximum LP transition time according to Scenario 1 */
+        trans_lp = exit_hs + max(enter_hs, 2) + 1;
+
+        /* CLKIN4DDR = 16 * TXBYTECLKHS */
+        tlp_avail = thsbyte_clk * (blank - trans_lp);
+
+        ttxclkesc = tdsi_fclk * lp_clk_div;
+
+        lp_inter = ((tlp_avail - 8 * thsbyte_clk - 5 * tdsi_fclk) / ttxclkesc -
+                        26) / 16;
+
+        return max(lp_inter, 0);
+}
+
+static void dsi_config_cmd_mode_interleaving(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int blanking_mode;
+        int hfp_blanking_mode, hbp_blanking_mode, hsa_blanking_mode;
+        int hsa, hfp, hbp, width_bytes, bllp, lp_clk_div;
+        int ddr_clk_pre, ddr_clk_post, enter_hs_mode_lat, exit_hs_mode_lat;
+        int tclk_trail, ths_exit, exiths_clk;
+        bool ddr_alwon;
+        struct omap_video_timings *timings = &dsi->timings;
+        int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+        int ndl = dsi->num_lanes_used - 1;
+        int dsi_fclk_hsdiv = dsi->user_dsi_cinfo.mX[HSDIV_DSI] + 1;
+        int hsa_interleave_hs = 0, hsa_interleave_lp = 0;
+        int hfp_interleave_hs = 0, hfp_interleave_lp = 0;
+        int hbp_interleave_hs = 0, hbp_interleave_lp = 0;
+        int bl_interleave_hs = 0, bl_interleave_lp = 0;
+        u32 r;
+
+        r = dsi_read_reg(dsidev, DSI_CTRL);
+        blanking_mode = FLD_GET(r, 20, 20);
+        hfp_blanking_mode = FLD_GET(r, 21, 21);
+        hbp_blanking_mode = FLD_GET(r, 22, 22);
+        hsa_blanking_mode = FLD_GET(r, 23, 23);
+
+        r = dsi_read_reg(dsidev, DSI_VM_TIMING1);
+        hbp = FLD_GET(r, 11, 0);
+        hfp = FLD_GET(r, 23, 12);
+        hsa = FLD_GET(r, 31, 24);
+
+        r = dsi_read_reg(dsidev, DSI_CLK_TIMING);
+        ddr_clk_post = FLD_GET(r, 7, 0);
+        ddr_clk_pre = FLD_GET(r, 15, 8);
+
+        r = dsi_read_reg(dsidev, DSI_VM_TIMING7);
+        exit_hs_mode_lat = FLD_GET(r, 15, 0);
+        enter_hs_mode_lat = FLD_GET(r, 31, 16);
+
+        r = dsi_read_reg(dsidev, DSI_CLK_CTRL);
+        lp_clk_div = FLD_GET(r, 12, 0);
+        ddr_alwon = FLD_GET(r, 13, 13);
+
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
+        ths_exit = FLD_GET(r, 7, 0);
+
+        r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
+        tclk_trail = FLD_GET(r, 15, 8);
+
+        exiths_clk = ths_exit + tclk_trail;
+
+        width_bytes = DIV_ROUND_UP(timings->x_res * bpp, 8);
+        bllp = hbp + hfp + hsa + DIV_ROUND_UP(width_bytes + 6, ndl);
+
+        if (!hsa_blanking_mode) {
+                hsa_interleave_hs = dsi_compute_interleave_hs(hsa, ddr_alwon,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        exiths_clk, ddr_clk_pre, ddr_clk_post);
+                hsa_interleave_lp = dsi_compute_interleave_lp(hsa,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        lp_clk_div, dsi_fclk_hsdiv);
+        }
+
+        if (!hfp_blanking_mode) {
+                hfp_interleave_hs = dsi_compute_interleave_hs(hfp, ddr_alwon,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        exiths_clk, ddr_clk_pre, ddr_clk_post);
+                hfp_interleave_lp = dsi_compute_interleave_lp(hfp,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        lp_clk_div, dsi_fclk_hsdiv);
+        }
+
+        if (!hbp_blanking_mode) {
+                hbp_interleave_hs = dsi_compute_interleave_hs(hbp, ddr_alwon,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        exiths_clk, ddr_clk_pre, ddr_clk_post);
+
+                hbp_interleave_lp = dsi_compute_interleave_lp(hbp,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        lp_clk_div, dsi_fclk_hsdiv);
+        }
+
+        if (!blanking_mode) {
+                bl_interleave_hs = dsi_compute_interleave_hs(bllp, ddr_alwon,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        exiths_clk, ddr_clk_pre, ddr_clk_post);
+
+                bl_interleave_lp = dsi_compute_interleave_lp(bllp,
+                                        enter_hs_mode_lat, exit_hs_mode_lat,
+                                        lp_clk_div, dsi_fclk_hsdiv);
+        }
+
+        DSSDBG("DSI HS interleaving(TXBYTECLKHS) HSA %d, HFP %d, HBP %d, BLLP %d\n",
+                hsa_interleave_hs, hfp_interleave_hs, hbp_interleave_hs,
+                bl_interleave_hs);
+
+        DSSDBG("DSI LP interleaving(bytes) HSA %d, HFP %d, HBP %d, BLLP %d\n",
+                hsa_interleave_lp, hfp_interleave_lp, hbp_interleave_lp,
+                bl_interleave_lp);
+
+        r = dsi_read_reg(dsidev, DSI_VM_TIMING4);
+        r = FLD_MOD(r, hsa_interleave_hs, 23, 16);
+        r = FLD_MOD(r, hfp_interleave_hs, 15, 8);
+        r = FLD_MOD(r, hbp_interleave_hs, 7, 0);
+        dsi_write_reg(dsidev, DSI_VM_TIMING4, r);
+
+        r = dsi_read_reg(dsidev, DSI_VM_TIMING5);
+        r = FLD_MOD(r, hsa_interleave_lp, 23, 16);
+        r = FLD_MOD(r, hfp_interleave_lp, 15, 8);
+        r = FLD_MOD(r, hbp_interleave_lp, 7, 0);
+        dsi_write_reg(dsidev, DSI_VM_TIMING5, r);
+
+        r = dsi_read_reg(dsidev, DSI_VM_TIMING6);
+        r = FLD_MOD(r, bl_interleave_hs, 31, 15);
+        r = FLD_MOD(r, bl_interleave_lp, 16, 0);
+        dsi_write_reg(dsidev, DSI_VM_TIMING6, r);
+}
+
+static int dsi_proto_config(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        u32 r;
+        int buswidth = 0;
+
+        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(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(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 (dsi_get_pixel_size(dsi->pix_fmt)) {
+        case 16:
+                buswidth = 0;
+                break;
+        case 18:
+                buswidth = 1;
+                break;
+        case 24:
+                buswidth = 2;
+                break;
+        default:
+                BUG();
+                return -EINVAL;
+        }
+
+        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 */
+        r = FLD_MOD(r, 1, 4, 4);        /* VP_CLK_RATIO, always 1, see errata*/
+        r = FLD_MOD(r, buswidth, 7, 6); /* VP_DATA_BUS_WIDTH */
+        r = FLD_MOD(r, 0, 8, 8);        /* VP_CLK_POL */
+        r = FLD_MOD(r, 1, 14, 14);      /* TRIGGER_RESET_MODE */
+        r = FLD_MOD(r, 1, 19, 19);      /* EOT_ENABLE */
+        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(dsidev, DSI_CTRL, r);
+
+        dsi_config_vp_num_line_buffers(dsidev);
+
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                dsi_config_vp_sync_events(dsidev);
+                dsi_config_blanking_modes(dsidev);
+                dsi_config_cmd_mode_interleaving(dsidev);
+        }
+
+        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 platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        unsigned tlpx, tclk_zero, tclk_prepare, tclk_trail;
+        unsigned tclk_pre, tclk_post;
+        unsigned ths_prepare, ths_prepare_ths_zero, ths_zero;
+        unsigned ths_trail, ths_exit;
+        unsigned ddr_clk_pre, ddr_clk_post;
+        unsigned enter_hs_mode_lat, exit_hs_mode_lat;
+        unsigned ths_eot;
+        int ndl = dsi->num_lanes_used - 1;
+        u32 r;
+
+        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(dsidev, DSI_DSIPHY_CFG1);
+        tlpx = FLD_GET(r, 20, 16) * 2;
+        tclk_trail = FLD_GET(r, 15, 8);
+        tclk_zero = FLD_GET(r, 7, 0);
+
+        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(dsidev, 60) + 26;
+
+        ths_eot = DIV_ROUND_UP(4, ndl);
+
+        ddr_clk_pre = DIV_ROUND_UP(tclk_pre + tlpx + tclk_zero + tclk_prepare,
+                        4);
+        ddr_clk_post = DIV_ROUND_UP(tclk_post + ths_trail, 4) + ths_eot;
+
+        BUG_ON(ddr_clk_pre == 0 || ddr_clk_pre > 255);
+        BUG_ON(ddr_clk_post == 0 || ddr_clk_post > 255);
+
+        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(dsidev, DSI_CLK_TIMING, r);
+
+        DSSDBG("ddr_clk_pre %u, ddr_clk_post %u\n",
+                        ddr_clk_pre,
+                        ddr_clk_post);
+
+        enter_hs_mode_lat = 1 + DIV_ROUND_UP(tlpx, 4) +
+                DIV_ROUND_UP(ths_prepare, 4) +
+                DIV_ROUND_UP(ths_zero + 3, 4);
+
+        exit_hs_mode_lat = DIV_ROUND_UP(ths_trail + ths_exit, 4) + 1 + ths_eot;
+
+        r = FLD_VAL(enter_hs_mode_lat, 31, 16) |
+                FLD_VAL(exit_hs_mode_lat, 15, 0);
+        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);
+
+         if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                /* TODO: Implement a video mode check_timings function */
+                int hsa = dsi->vm_timings.hsa;
+                int hfp = dsi->vm_timings.hfp;
+                int hbp = dsi->vm_timings.hbp;
+                int vsa = dsi->vm_timings.vsa;
+                int vfp = dsi->vm_timings.vfp;
+                int vbp = dsi->vm_timings.vbp;
+                int window_sync = dsi->vm_timings.window_sync;
+                bool hsync_end;
+                struct omap_video_timings *timings = &dsi->timings;
+                int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+                int tl, t_he, width_bytes;
+
+                hsync_end = dsi->vm_timings.trans_mode == OMAP_DSS_DSI_PULSE_MODE;
+                t_he = hsync_end ?
+                        ((hsa == 0 && ndl == 3) ? 1 : DIV_ROUND_UP(4, ndl)) : 0;
+
+                width_bytes = DIV_ROUND_UP(timings->x_res * bpp, 8);
+
+                /* TL = t_HS + HSA + t_HE + HFP + ceil((WC + 6) / NDL) + HBP */
+                tl = DIV_ROUND_UP(4, ndl) + (hsync_end ? hsa : 0) + t_he + hfp +
+                        DIV_ROUND_UP(width_bytes + 6, ndl) + hbp;
+
+                DSSDBG("HBP: %d, HFP: %d, HSA: %d, TL: %d TXBYTECLKHS\n", hbp,
+                        hfp, hsync_end ? hsa : 0, tl);
+                DSSDBG("VBP: %d, VFP: %d, VSA: %d, VACT: %d lines\n", vbp, vfp,
+                        vsa, timings->y_res);
+
+                r = dsi_read_reg(dsidev, DSI_VM_TIMING1);
+                r = FLD_MOD(r, hbp, 11, 0);     /* HBP */
+                r = FLD_MOD(r, hfp, 23, 12);    /* HFP */
+                r = FLD_MOD(r, hsync_end ? hsa : 0, 31, 24);    /* HSA */
+                dsi_write_reg(dsidev, DSI_VM_TIMING1, r);
+
+                r = dsi_read_reg(dsidev, DSI_VM_TIMING2);
+                r = FLD_MOD(r, vbp, 7, 0);      /* VBP */
+                r = FLD_MOD(r, vfp, 15, 8);     /* VFP */
+                r = FLD_MOD(r, vsa, 23, 16);    /* VSA */
+                r = FLD_MOD(r, window_sync, 27, 24);    /* WINDOW_SYNC */
+                dsi_write_reg(dsidev, DSI_VM_TIMING2, r);
+
+                r = dsi_read_reg(dsidev, DSI_VM_TIMING3);
+                r = FLD_MOD(r, timings->y_res, 14, 0);  /* VACT */
+                r = FLD_MOD(r, tl, 31, 16);             /* TL */
+                dsi_write_reg(dsidev, DSI_VM_TIMING3, r);
+        }
+}
+
+static int dsi_configure_pins(struct omap_dss_device *dssdev,
+                const struct omap_dsi_pin_config *pin_cfg)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int num_pins;
+        const int *pins;
+        struct dsi_lane_config lanes[DSI_MAX_NR_LANES];
+        int num_lanes;
+        int i;
+
+        static const enum dsi_lane_function functions[] = {
+                DSI_LANE_CLK,
+                DSI_LANE_DATA1,
+                DSI_LANE_DATA2,
+                DSI_LANE_DATA3,
+                DSI_LANE_DATA4,
+        };
+
+        num_pins = pin_cfg->num_pins;
+        pins = pin_cfg->pins;
+
+        if (num_pins < 4 || num_pins > dsi->num_lanes_supported * 2
+                        || num_pins % 2 != 0)
+                return -EINVAL;
+
+        for (i = 0; i < DSI_MAX_NR_LANES; ++i)
+                lanes[i].function = DSI_LANE_UNUSED;
+
+        num_lanes = 0;
+
+        for (i = 0; i < num_pins; i += 2) {
+                u8 lane, pol;
+                int dx, dy;
+
+                dx = pins[i];
+                dy = pins[i + 1];
+
+                if (dx < 0 || dx >= dsi->num_lanes_supported * 2)
+                        return -EINVAL;
+
+                if (dy < 0 || dy >= dsi->num_lanes_supported * 2)
+                        return -EINVAL;
+
+                if (dx & 1) {
+                        if (dy != dx - 1)
+                                return -EINVAL;
+                        pol = 1;
+                } else {
+                        if (dy != dx + 1)
+                                return -EINVAL;
+                        pol = 0;
+                }
+
+                lane = dx / 2;
+
+                lanes[lane].function = functions[i / 2];
+                lanes[lane].polarity = pol;
+                num_lanes++;
+        }
+
+        memcpy(dsi->lanes, lanes, sizeof(dsi->lanes));
+        dsi->num_lanes_used = num_lanes;
+
+        return 0;
+}
+
+static int dsi_enable_video_output(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);
+        struct omap_overlay_manager *mgr = dsi->output.manager;
+        int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+        struct omap_dss_device *out = &dsi->output;
+        u8 data_type;
+        u16 word_count;
+        int r;
+
+        if (out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                return -ENODEV;
+        }
+
+        r = dsi_display_init_dispc(dsidev, mgr);
+        if (r)
+                goto err_init_dispc;
+
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                switch (dsi->pix_fmt) {
+                case OMAP_DSS_DSI_FMT_RGB888:
+                        data_type = MIPI_DSI_PACKED_PIXEL_STREAM_24;
+                        break;
+                case OMAP_DSS_DSI_FMT_RGB666:
+                        data_type = MIPI_DSI_PIXEL_STREAM_3BYTE_18;
+                        break;
+                case OMAP_DSS_DSI_FMT_RGB666_PACKED:
+                        data_type = MIPI_DSI_PACKED_PIXEL_STREAM_18;
+                        break;
+                case OMAP_DSS_DSI_FMT_RGB565:
+                        data_type = MIPI_DSI_PACKED_PIXEL_STREAM_16;
+                        break;
+                default:
+                        r = -EINVAL;
+                        goto err_pix_fmt;
+                }
+
+                dsi_if_enable(dsidev, false);
+                dsi_vc_enable(dsidev, channel, false);
+
+                /* MODE, 1 = video mode */
+                REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 4, 4);
+
+                word_count = DIV_ROUND_UP(dsi->timings.x_res * bpp, 8);
+
+                dsi_vc_write_long_header(dsidev, channel, data_type,
+                                word_count, 0);
+
+                dsi_vc_enable(dsidev, channel, true);
+                dsi_if_enable(dsidev, true);
+        }
+
+        r = dss_mgr_enable(mgr);
+        if (r)
+                goto err_mgr_enable;
+
+        return 0;
+
+err_mgr_enable:
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                dsi_if_enable(dsidev, false);
+                dsi_vc_enable(dsidev, channel, false);
+        }
+err_pix_fmt:
+        dsi_display_uninit_dispc(dsidev, mgr);
+err_init_dispc:
+        return r;
+}
+
+static void dsi_disable_video_output(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);
+        struct omap_overlay_manager *mgr = dsi->output.manager;
+
+        if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+                dsi_if_enable(dsidev, false);
+                dsi_vc_enable(dsidev, channel, false);
+
+                /* MODE, 0 = command mode */
+                REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 0, 4, 4);
+
+                dsi_vc_enable(dsidev, channel, true);
+                dsi_if_enable(dsidev, true);
+        }
+
+        dss_mgr_disable(mgr);
+
+        dsi_display_uninit_dispc(dsidev, mgr);
+}
+
+static void dsi_update_screen_dispc(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_overlay_manager *mgr = dsi->output.manager;
+        unsigned bytespp;
+        unsigned bytespl;
+        unsigned bytespf;
+        unsigned total_len;
+        unsigned packet_payload;
+        unsigned packet_len;
+        u32 l;
+        int r;
+        const unsigned channel = dsi->update_channel;
+        const unsigned line_buf_size = dsi->line_buffer_size;
+        u16 w = dsi->timings.x_res;
+        u16 h = dsi->timings.y_res;
+
+        DSSDBG("dsi_update_screen_dispc(%dx%d)\n", w, h);
+
+        dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_VP);
+
+        bytespp = dsi_get_pixel_size(dsi->pix_fmt) / 8;
+        bytespl = w * bytespp;
+        bytespf = bytespl * h;
+
+        /* NOTE: packet_payload has to be equal to N * bytespl, where N is
+         * number of lines in a packet.  See errata about VP_CLK_RATIO */
+
+        if (bytespf < line_buf_size)
+                packet_payload = bytespf;
+        else
+                packet_payload = (line_buf_size) / bytespl * bytespl;
+
+        packet_len = packet_payload + 1;        /* 1 byte for DCS cmd */
+        total_len = (bytespf / packet_payload) * packet_len;
+
+        if (bytespf % packet_payload)
+                total_len += (bytespf % packet_payload) + 1;
+
+        l = FLD_VAL(total_len, 23, 0); /* TE_SIZE */
+        dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
+
+        dsi_vc_write_long_header(dsidev, channel, MIPI_DSI_DCS_LONG_WRITE,
+                packet_len, 0);
+
+        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(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
+         * framedone interrupt could be delayed for quite a long time. I think
+         * the same goes for any DSS interrupts, but for some reason I have not
+         * seen the problem anywhere else than here.
+         */
+        dispc_disable_sidle();
+
+        dsi_perf_mark_start(dsidev);
+
+        r = schedule_delayed_work(&dsi->framedone_timeout_work,
+                msecs_to_jiffies(250));
+        BUG_ON(r == 0);
+
+        dss_mgr_set_timings(mgr, &dsi->timings);
+
+        dss_mgr_start_update(mgr);
+
+        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(dsidev, DSI_TIMING2, 0, 15, 15); /* LP_RX_TO */
+
+                dsi_vc_send_bta(dsidev, channel);
+
+#ifdef DSI_CATCH_MISSING_TE
+                mod_timer(&dsi->te_timer, jiffies + msecs_to_jiffies(250));
+#endif
+        }
+}
+
+#ifdef DSI_CATCH_MISSING_TE
+static void dsi_te_timeout(unsigned long arg)
+{
+        DSSERR("TE not received for 250ms!\n");
+}
+#endif
+
+static void dsi_handle_framedone(struct platform_device *dsidev, int error)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        /* SIDLEMODE back to smart-idle */
+        dispc_enable_sidle();
+
+        if (dsi->te_enabled) {
+                /* enable LP_RX_TO again after the TE */
+                REG_FLD_MOD(dsidev, DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
+        }
+
+        dsi->framedone_callback(error, dsi->framedone_data);
+
+        if (!error)
+                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
+         * possibly scheduled framedone work. However, cancelling the transfer
+         * on the HW is buggy, and would probably require resetting the whole
+         * DSI */
+
+        DSSERR("Framedone not received for 250ms!\n");
+
+        dsi_handle_framedone(dsi->pdev, -ETIMEDOUT);
+}
+
+static void dsi_framedone_irq_callback(void *data)
+{
+        struct platform_device *dsidev = (struct platform_device *) data;
+        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.
+         */
+
+        cancel_delayed_work(&dsi->framedone_timeout_work);
+
+        dsi_handle_framedone(dsidev, 0);
+}
+
+static int dsi_update(struct omap_dss_device *dssdev, int channel,
+                void (*callback)(int, void *), void *data)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        u16 dw, dh;
+
+        dsi_perf_mark_setup(dsidev);
+
+        dsi->update_channel = channel;
+
+        dsi->framedone_callback = callback;
+        dsi->framedone_data = data;
+
+        dw = dsi->timings.x_res;
+        dh = dsi->timings.y_res;
+
+#ifdef DSI_PERF_MEASURE
+        dsi->update_bytes = dw * dh *
+                dsi_get_pixel_size(dsi->pix_fmt) / 8;
+#endif
+        dsi_update_screen_dispc(dsidev);
+
+        return 0;
+}
+
+/* Display funcs */
+
+static int dsi_configure_dispc_clocks(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dispc_clock_info dispc_cinfo;
+        int r;
+        unsigned long fck;
+
+        fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+
+        dispc_cinfo.lck_div = dsi->user_dispc_cinfo.lck_div;
+        dispc_cinfo.pck_div = dsi->user_dispc_cinfo.pck_div;
+
+        r = dispc_calc_clock_rates(fck, &dispc_cinfo);
+        if (r) {
+                DSSERR("Failed to calc dispc clocks\n");
+                return r;
+        }
+
+        dsi->mgr_config.clock_info = dispc_cinfo;
+
+        return 0;
+}
+
+static int dsi_display_init_dispc(struct platform_device *dsidev,
+                struct omap_overlay_manager *mgr)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int r;
+
+        dss_select_lcd_clk_source(mgr->id, dsi->module_id == 0 ?
+                        OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+                        OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC);
+
+        if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
+                r = dss_mgr_register_framedone_handler(mgr,
+                                dsi_framedone_irq_callback, dsidev);
+                if (r) {
+                        DSSERR("can't register FRAMEDONE handler\n");
+                        goto err;
+                }
+
+                dsi->mgr_config.stallmode = true;
+                dsi->mgr_config.fifohandcheck = true;
+        } else {
+                dsi->mgr_config.stallmode = false;
+                dsi->mgr_config.fifohandcheck = false;
+        }
+
+        /*
+         * override interlace, logic level and edge related parameters in
+         * omap_video_timings with default values
+         */
+        dsi->timings.interlace = false;
+        dsi->timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        dsi->timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        dsi->timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        dsi->timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        dsi->timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE;
+
+        dss_mgr_set_timings(mgr, &dsi->timings);
+
+        r = dsi_configure_dispc_clocks(dsidev);
+        if (r)
+                goto err1;
+
+        dsi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+        dsi->mgr_config.video_port_width =
+                        dsi_get_pixel_size(dsi->pix_fmt);
+        dsi->mgr_config.lcden_sig_polarity = 0;
+
+        dss_mgr_set_lcd_config(mgr, &dsi->mgr_config);
+
+        return 0;
+err1:
+        if (dsi->mode == OMAP_DSS_DSI_CMD_MODE)
+                dss_mgr_unregister_framedone_handler(mgr,
+                                dsi_framedone_irq_callback, dsidev);
+err:
+        dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+        return r;
+}
+
+static void dsi_display_uninit_dispc(struct platform_device *dsidev,
+                struct omap_overlay_manager *mgr)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (dsi->mode == OMAP_DSS_DSI_CMD_MODE)
+                dss_mgr_unregister_framedone_handler(mgr,
+                                dsi_framedone_irq_callback, dsidev);
+
+        dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+}
+
+static int dsi_configure_dsi_clocks(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dss_pll_clock_info cinfo;
+        int r;
+
+        cinfo = dsi->user_dsi_cinfo;
+
+        r = dss_pll_set_config(&dsi->pll, &cinfo);
+        if (r) {
+                DSSERR("Failed to set dsi clocks\n");
+                return r;
+        }
+
+        return 0;
+}
+
+static int dsi_display_init_dsi(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        int r;
+
+        r = dss_pll_enable(&dsi->pll);
+        if (r)
+                goto err0;
+
+        r = dsi_configure_dsi_clocks(dsidev);
+        if (r)
+                goto err1;
+
+        dss_select_dsi_clk_source(dsi->module_id, dsi->module_id == 0 ?
+                        OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+                        OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI);
+
+        DSSDBG("PLL OK\n");
+
+        r = dsi_cio_init(dsidev);
+        if (r)
+                goto err2;
+
+        _dsi_print_reset_status(dsidev);
+
+        dsi_proto_timings(dsidev);
+        dsi_set_lp_clk_divisor(dsidev);
+
+        if (1)
+                _dsi_print_reset_status(dsidev);
+
+        r = dsi_proto_config(dsidev);
+        if (r)
+                goto err3;
+
+        /* enable interface */
+        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_cio_uninit(dsidev);
+err2:
+        dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
+err1:
+        dss_pll_disable(&dsi->pll);
+err0:
+        return r;
+}
+
+static void dsi_display_uninit_dsi(struct platform_device *dsidev,
+                bool disconnect_lanes, bool enter_ulps)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        if (enter_ulps && !dsi->ulps_enabled)
+                dsi_enter_ulps(dsidev);
+
+        /* 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_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
+        dsi_cio_uninit(dsidev);
+        dsi_pll_uninit(dsidev, disconnect_lanes);
+}
+
+static int 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(dsidev));
+
+        mutex_lock(&dsi->lock);
+
+        r = dsi_runtime_get(dsidev);
+        if (r)
+                goto err_get_dsi;
+
+        _dsi_initialize_irq(dsidev);
+
+        r = dsi_display_init_dsi(dsidev);
+        if (r)
+                goto err_init_dsi;
+
+        mutex_unlock(&dsi->lock);
+
+        return 0;
+
+err_init_dsi:
+        dsi_runtime_put(dsidev);
+err_get_dsi:
+        mutex_unlock(&dsi->lock);
+        DSSDBG("dsi_display_enable FAILED\n");
+        return r;
+}
+
+static void 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(dsidev));
+
+        mutex_lock(&dsi->lock);
+
+        dsi_sync_vc(dsidev, 0);
+        dsi_sync_vc(dsidev, 1);
+        dsi_sync_vc(dsidev, 2);
+        dsi_sync_vc(dsidev, 3);
+
+        dsi_display_uninit_dsi(dsidev, disconnect_lanes, enter_ulps);
+
+        dsi_runtime_put(dsidev);
+
+        mutex_unlock(&dsi->lock);
+}
+
+static int dsi_enable_te(struct omap_dss_device *dssdev, bool 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;
+}
+
+#ifdef PRINT_VERBOSE_VM_TIMINGS
+static void print_dsi_vm(const char *str,
+                const struct omap_dss_dsi_videomode_timings *t)
+{
+        unsigned long byteclk = t->hsclk / 4;
+        int bl, wc, pps, tot;
+
+        wc = DIV_ROUND_UP(t->hact * t->bitspp, 8);
+        pps = DIV_ROUND_UP(wc + 6, t->ndl); /* pixel packet size */
+        bl = t->hss + t->hsa + t->hse + t->hbp + t->hfp;
+        tot = bl + pps;
+
+#define TO_DSI_T(x) ((u32)div64_u64((u64)x * 1000000000llu, byteclk))
+
+        pr_debug("%s bck %lu, %u/%u/%u/%u/%u/%u = %u+%u = %u, "
+                        "%u/%u/%u/%u/%u/%u = %u + %u = %u\n",
+                        str,
+                        byteclk,
+                        t->hss, t->hsa, t->hse, t->hbp, pps, t->hfp,
+                        bl, pps, tot,
+                        TO_DSI_T(t->hss),
+                        TO_DSI_T(t->hsa),
+                        TO_DSI_T(t->hse),
+                        TO_DSI_T(t->hbp),
+                        TO_DSI_T(pps),
+                        TO_DSI_T(t->hfp),
+
+                        TO_DSI_T(bl),
+                        TO_DSI_T(pps),
+
+                        TO_DSI_T(tot));
+#undef TO_DSI_T
+}
+
+static void print_dispc_vm(const char *str, const struct omap_video_timings *t)
+{
+        unsigned long pck = t->pixelclock;
+        int hact, bl, tot;
+
+        hact = t->x_res;
+        bl = t->hsw + t->hbp + t->hfp;
+        tot = hact + bl;
+
+#define TO_DISPC_T(x) ((u32)div64_u64((u64)x * 1000000000llu, pck))
+
+        pr_debug("%s pck %lu, %u/%u/%u/%u = %u+%u = %u, "
+                        "%u/%u/%u/%u = %u + %u = %u\n",
+                        str,
+                        pck,
+                        t->hsw, t->hbp, hact, t->hfp,
+                        bl, hact, tot,
+                        TO_DISPC_T(t->hsw),
+                        TO_DISPC_T(t->hbp),
+                        TO_DISPC_T(hact),
+                        TO_DISPC_T(t->hfp),
+                        TO_DISPC_T(bl),
+                        TO_DISPC_T(hact),
+                        TO_DISPC_T(tot));
+#undef TO_DISPC_T
+}
+
+/* note: this is not quite accurate */
+static void print_dsi_dispc_vm(const char *str,
+                const struct omap_dss_dsi_videomode_timings *t)
+{
+        struct omap_video_timings vm = { 0 };
+        unsigned long byteclk = t->hsclk / 4;
+        unsigned long pck;
+        u64 dsi_tput;
+        int dsi_hact, dsi_htot;
+
+        dsi_tput = (u64)byteclk * t->ndl * 8;
+        pck = (u32)div64_u64(dsi_tput, t->bitspp);
+        dsi_hact = DIV_ROUND_UP(DIV_ROUND_UP(t->hact * t->bitspp, 8) + 6, t->ndl);
+        dsi_htot = t->hss + t->hsa + t->hse + t->hbp + dsi_hact + t->hfp;
+
+        vm.pixelclock = pck;
+        vm.hsw = div64_u64((u64)(t->hsa + t->hse) * pck, byteclk);
+        vm.hbp = div64_u64((u64)t->hbp * pck, byteclk);
+        vm.hfp = div64_u64((u64)t->hfp * pck, byteclk);
+        vm.x_res = t->hact;
+
+        print_dispc_vm(str, &vm);
+}
+#endif /* PRINT_VERBOSE_VM_TIMINGS */
+
+static bool dsi_cm_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+                unsigned long pck, void *data)
+{
+        struct dsi_clk_calc_ctx *ctx = data;
+        struct omap_video_timings *t = &ctx->dispc_vm;
+
+        ctx->dispc_cinfo.lck_div = lckd;
+        ctx->dispc_cinfo.pck_div = pckd;
+        ctx->dispc_cinfo.lck = lck;
+        ctx->dispc_cinfo.pck = pck;
+
+        *t = *ctx->config->timings;
+        t->pixelclock = pck;
+        t->x_res = ctx->config->timings->x_res;
+        t->y_res = ctx->config->timings->y_res;
+        t->hsw = t->hfp = t->hbp = t->vsw = 1;
+        t->vfp = t->vbp = 0;
+
+        return true;
+}
+
+static bool dsi_cm_calc_hsdiv_cb(int m_dispc, unsigned long dispc,
+                void *data)
+{
+        struct dsi_clk_calc_ctx *ctx = data;
+
+        ctx->dsi_cinfo.mX[HSDIV_DISPC] = m_dispc;
+        ctx->dsi_cinfo.clkout[HSDIV_DISPC] = dispc;
+
+        return dispc_div_calc(dispc, ctx->req_pck_min, ctx->req_pck_max,
+                        dsi_cm_calc_dispc_cb, ctx);
+}
+
+static bool dsi_cm_calc_pll_cb(int n, int m, unsigned long fint,
+                unsigned long clkdco, void *data)
+{
+        struct dsi_clk_calc_ctx *ctx = data;
+
+        ctx->dsi_cinfo.n = n;
+        ctx->dsi_cinfo.m = m;
+        ctx->dsi_cinfo.fint = fint;
+        ctx->dsi_cinfo.clkdco = clkdco;
+
+        return dss_pll_hsdiv_calc(ctx->pll, clkdco, ctx->req_pck_min,
+                        dss_feat_get_param_max(FEAT_PARAM_DSS_FCK),
+                        dsi_cm_calc_hsdiv_cb, ctx);
+}
+
+static bool dsi_cm_calc(struct dsi_data *dsi,
+                const struct omap_dss_dsi_config *cfg,
+                struct dsi_clk_calc_ctx *ctx)
+{
+        unsigned long clkin;
+        int bitspp, ndl;
+        unsigned long pll_min, pll_max;
+        unsigned long pck, txbyteclk;
+
+        clkin = clk_get_rate(dsi->pll.clkin);
+        bitspp = dsi_get_pixel_size(cfg->pixel_format);
+        ndl = dsi->num_lanes_used - 1;
+
+        /*
+         * Here we should calculate minimum txbyteclk to be able to send the
+         * frame in time, and also to handle TE. That's not very simple, though,
+         * especially as we go to LP between each pixel packet due to HW
+         * "feature". So let's just estimate very roughly and multiply by 1.5.
+         */
+        pck = cfg->timings->pixelclock;
+        pck = pck * 3 / 2;
+        txbyteclk = pck * bitspp / 8 / ndl;
+
+        memset(ctx, 0, sizeof(*ctx));
+        ctx->dsidev = dsi->pdev;
+        ctx->pll = &dsi->pll;
+        ctx->config = cfg;
+        ctx->req_pck_min = pck;
+        ctx->req_pck_nom = pck;
+        ctx->req_pck_max = pck * 3 / 2;
+
+        pll_min = max(cfg->hs_clk_min * 4, txbyteclk * 4 * 4);
+        pll_max = cfg->hs_clk_max * 4;
+
+        return dss_pll_calc(ctx->pll, clkin,
+                        pll_min, pll_max,
+                        dsi_cm_calc_pll_cb, ctx);
+}
+
+static bool dsi_vm_calc_blanking(struct dsi_clk_calc_ctx *ctx)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(ctx->dsidev);
+        const struct omap_dss_dsi_config *cfg = ctx->config;
+        int bitspp = dsi_get_pixel_size(cfg->pixel_format);
+        int ndl = dsi->num_lanes_used - 1;
+        unsigned long hsclk = ctx->dsi_cinfo.clkdco / 4;
+        unsigned long byteclk = hsclk / 4;
+
+        unsigned long dispc_pck, req_pck_min, req_pck_nom, req_pck_max;
+        int xres;
+        int panel_htot, panel_hbl; /* pixels */
+        int dispc_htot, dispc_hbl; /* pixels */
+        int dsi_htot, dsi_hact, dsi_hbl, hss, hse; /* byteclks */
+        int hfp, hsa, hbp;
+        const struct omap_video_timings *req_vm;
+        struct omap_video_timings *dispc_vm;
+        struct omap_dss_dsi_videomode_timings *dsi_vm;
+        u64 dsi_tput, dispc_tput;
+
+        dsi_tput = (u64)byteclk * ndl * 8;
+
+        req_vm = cfg->timings;
+        req_pck_min = ctx->req_pck_min;
+        req_pck_max = ctx->req_pck_max;
+        req_pck_nom = ctx->req_pck_nom;
+
+        dispc_pck = ctx->dispc_cinfo.pck;
+        dispc_tput = (u64)dispc_pck * bitspp;
+
+        xres = req_vm->x_res;
+
+        panel_hbl = req_vm->hfp + req_vm->hbp + req_vm->hsw;
+        panel_htot = xres + panel_hbl;
+
+        dsi_hact = DIV_ROUND_UP(DIV_ROUND_UP(xres * bitspp, 8) + 6, ndl);
+
+        /*
+         * When there are no line buffers, DISPC and DSI must have the
+         * same tput. Otherwise DISPC tput needs to be higher than DSI's.
+         */
+        if (dsi->line_buffer_size < xres * bitspp / 8) {
+                if (dispc_tput != dsi_tput)
+                        return false;
+        } else {
+                if (dispc_tput < dsi_tput)
+                        return false;
+        }
+
+        /* DSI tput must be over the min requirement */
+        if (dsi_tput < (u64)bitspp * req_pck_min)
+                return false;
+
+        /* When non-burst mode, DSI tput must be below max requirement. */
+        if (cfg->trans_mode != OMAP_DSS_DSI_BURST_MODE) {
+                if (dsi_tput > (u64)bitspp * req_pck_max)
+                        return false;
+        }
+
+        hss = DIV_ROUND_UP(4, ndl);
+
+        if (cfg->trans_mode == OMAP_DSS_DSI_PULSE_MODE) {
+                if (ndl == 3 && req_vm->hsw == 0)
+                        hse = 1;
+                else
+                        hse = DIV_ROUND_UP(4, ndl);
+        } else {
+                hse = 0;
+        }
+
+        /* DSI htot to match the panel's nominal pck */
+        dsi_htot = div64_u64((u64)panel_htot * byteclk, req_pck_nom);
+
+        /* fail if there would be no time for blanking */
+        if (dsi_htot < hss + hse + dsi_hact)
+                return false;
+
+        /* total DSI blanking needed to achieve panel's TL */
+        dsi_hbl = dsi_htot - dsi_hact;
+
+        /* DISPC htot to match the DSI TL */
+        dispc_htot = div64_u64((u64)dsi_htot * dispc_pck, byteclk);
+
+        /* verify that the DSI and DISPC TLs are the same */
+        if ((u64)dsi_htot * dispc_pck != (u64)dispc_htot * byteclk)
+                return false;
+
+        dispc_hbl = dispc_htot - xres;
+
+        /* setup DSI videomode */
+
+        dsi_vm = &ctx->dsi_vm;
+        memset(dsi_vm, 0, sizeof(*dsi_vm));
+
+        dsi_vm->hsclk = hsclk;
+
+        dsi_vm->ndl = ndl;
+        dsi_vm->bitspp = bitspp;
+
+        if (cfg->trans_mode != OMAP_DSS_DSI_PULSE_MODE) {
+                hsa = 0;
+        } else if (ndl == 3 && req_vm->hsw == 0) {
+                hsa = 0;
+        } else {
+                hsa = div64_u64((u64)req_vm->hsw * byteclk, req_pck_nom);
+                hsa = max(hsa - hse, 1);
+        }
+
+        hbp = div64_u64((u64)req_vm->hbp * byteclk, req_pck_nom);
+        hbp = max(hbp, 1);
+
+        hfp = dsi_hbl - (hss + hsa + hse + hbp);
+        if (hfp < 1) {
+                int t;
+                /* we need to take cycles from hbp */
+
+                t = 1 - hfp;
+                hbp = max(hbp - t, 1);
+                hfp = dsi_hbl - (hss + hsa + hse + hbp);
+
+                if (hfp < 1 && hsa > 0) {
+                        /* we need to take cycles from hsa */
+                        t = 1 - hfp;
+                        hsa = max(hsa - t, 1);
+                        hfp = dsi_hbl - (hss + hsa + hse + hbp);
+                }
+        }
+
+        if (hfp < 1)
+                return false;
+
+        dsi_vm->hss = hss;
+        dsi_vm->hsa = hsa;
+        dsi_vm->hse = hse;
+        dsi_vm->hbp = hbp;
+        dsi_vm->hact = xres;
+        dsi_vm->hfp = hfp;
+
+        dsi_vm->vsa = req_vm->vsw;
+        dsi_vm->vbp = req_vm->vbp;
+        dsi_vm->vact = req_vm->y_res;
+        dsi_vm->vfp = req_vm->vfp;
+
+        dsi_vm->trans_mode = cfg->trans_mode;
+
+        dsi_vm->blanking_mode = 0;
+        dsi_vm->hsa_blanking_mode = 1;
+        dsi_vm->hfp_blanking_mode = 1;
+        dsi_vm->hbp_blanking_mode = 1;
+
+        dsi_vm->ddr_clk_always_on = cfg->ddr_clk_always_on;
+        dsi_vm->window_sync = 4;
+
+        /* setup DISPC videomode */
+
+        dispc_vm = &ctx->dispc_vm;
+        *dispc_vm = *req_vm;
+        dispc_vm->pixelclock = dispc_pck;
+
+        if (cfg->trans_mode == OMAP_DSS_DSI_PULSE_MODE) {
+                hsa = div64_u64((u64)req_vm->hsw * dispc_pck,
+                                req_pck_nom);
+                hsa = max(hsa, 1);
+        } else {
+                hsa = 1;
+        }
+
+        hbp = div64_u64((u64)req_vm->hbp * dispc_pck, req_pck_nom);
+        hbp = max(hbp, 1);
+
+        hfp = dispc_hbl - hsa - hbp;
+        if (hfp < 1) {
+                int t;
+                /* we need to take cycles from hbp */
+
+                t = 1 - hfp;
+                hbp = max(hbp - t, 1);
+                hfp = dispc_hbl - hsa - hbp;
+
+                if (hfp < 1) {
+                        /* we need to take cycles from hsa */
+                        t = 1 - hfp;
+                        hsa = max(hsa - t, 1);
+                        hfp = dispc_hbl - hsa - hbp;
+                }
+        }
+
+        if (hfp < 1)
+                return false;
+
+        dispc_vm->hfp = hfp;
+        dispc_vm->hsw = hsa;
+        dispc_vm->hbp = hbp;
+
+        return true;
+}
+
+
+static bool dsi_vm_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+                unsigned long pck, void *data)
+{
+        struct dsi_clk_calc_ctx *ctx = data;
+
+        ctx->dispc_cinfo.lck_div = lckd;
+        ctx->dispc_cinfo.pck_div = pckd;
+        ctx->dispc_cinfo.lck = lck;
+        ctx->dispc_cinfo.pck = pck;
+
+        if (dsi_vm_calc_blanking(ctx) == false)
+                return false;
+
+#ifdef PRINT_VERBOSE_VM_TIMINGS
+        print_dispc_vm("dispc", &ctx->dispc_vm);
+        print_dsi_vm("dsi  ", &ctx->dsi_vm);
+        print_dispc_vm("req  ", ctx->config->timings);
+        print_dsi_dispc_vm("act  ", &ctx->dsi_vm);
+#endif
+
+        return true;
+}
+
+static bool dsi_vm_calc_hsdiv_cb(int m_dispc, unsigned long dispc,
+                void *data)
+{
+        struct dsi_clk_calc_ctx *ctx = data;
+        unsigned long pck_max;
+
+        ctx->dsi_cinfo.mX[HSDIV_DISPC] = m_dispc;
+        ctx->dsi_cinfo.clkout[HSDIV_DISPC] = dispc;
+
+        /*
+         * In burst mode we can let the dispc pck be arbitrarily high, but it
+         * limits our scaling abilities. So for now, don't aim too high.
+         */
+
+        if (ctx->config->trans_mode == OMAP_DSS_DSI_BURST_MODE)
+                pck_max = ctx->req_pck_max + 10000000;
+        else
+                pck_max = ctx->req_pck_max;
+
+        return dispc_div_calc(dispc, ctx->req_pck_min, pck_max,
+                        dsi_vm_calc_dispc_cb, ctx);
+}
+
+static bool dsi_vm_calc_pll_cb(int n, int m, unsigned long fint,
+                unsigned long clkdco, void *data)
+{
+        struct dsi_clk_calc_ctx *ctx = data;
+
+        ctx->dsi_cinfo.n = n;
+        ctx->dsi_cinfo.m = m;
+        ctx->dsi_cinfo.fint = fint;
+        ctx->dsi_cinfo.clkdco = clkdco;
+
+        return dss_pll_hsdiv_calc(ctx->pll, clkdco, ctx->req_pck_min,
+                        dss_feat_get_param_max(FEAT_PARAM_DSS_FCK),
+                        dsi_vm_calc_hsdiv_cb, ctx);
+}
+
+static bool dsi_vm_calc(struct dsi_data *dsi,
+                const struct omap_dss_dsi_config *cfg,
+                struct dsi_clk_calc_ctx *ctx)
+{
+        const struct omap_video_timings *t = cfg->timings;
+        unsigned long clkin;
+        unsigned long pll_min;
+        unsigned long pll_max;
+        int ndl = dsi->num_lanes_used - 1;
+        int bitspp = dsi_get_pixel_size(cfg->pixel_format);
+        unsigned long byteclk_min;
+
+        clkin = clk_get_rate(dsi->pll.clkin);
+
+        memset(ctx, 0, sizeof(*ctx));
+        ctx->dsidev = dsi->pdev;
+        ctx->pll = &dsi->pll;
+        ctx->config = cfg;
+
+        /* these limits should come from the panel driver */
+        ctx->req_pck_min = t->pixelclock - 1000;
+        ctx->req_pck_nom = t->pixelclock;
+        ctx->req_pck_max = t->pixelclock + 1000;
+
+        byteclk_min = div64_u64((u64)ctx->req_pck_min * bitspp, ndl * 8);
+        pll_min = max(cfg->hs_clk_min * 4, byteclk_min * 4 * 4);
+
+        if (cfg->trans_mode == OMAP_DSS_DSI_BURST_MODE) {
+                pll_max = cfg->hs_clk_max * 4;
+        } else {
+                unsigned long byteclk_max;
+                byteclk_max = div64_u64((u64)ctx->req_pck_max * bitspp,
+                                ndl * 8);
+
+                pll_max = byteclk_max * 4 * 4;
+        }
+
+        return dss_pll_calc(ctx->pll, clkin,
+                        pll_min, pll_max,
+                        dsi_vm_calc_pll_cb, ctx);
+}
+
+static int dsi_set_config(struct omap_dss_device *dssdev,
+                const struct omap_dss_dsi_config *config)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dsi_clk_calc_ctx ctx;
+        bool ok;
+        int r;
+
+        mutex_lock(&dsi->lock);
+
+        dsi->pix_fmt = config->pixel_format;
+        dsi->mode = config->mode;
+
+        if (config->mode == OMAP_DSS_DSI_VIDEO_MODE)
+                ok = dsi_vm_calc(dsi, config, &ctx);
+        else
+                ok = dsi_cm_calc(dsi, config, &ctx);
+
+        if (!ok) {
+                DSSERR("failed to find suitable DSI clock settings\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        dsi_pll_calc_dsi_fck(&ctx.dsi_cinfo);
+
+        r = dsi_lp_clock_calc(ctx.dsi_cinfo.clkout[HSDIV_DSI],
+                config->lp_clk_min, config->lp_clk_max, &dsi->user_lp_cinfo);
+        if (r) {
+                DSSERR("failed to find suitable DSI LP clock settings\n");
+                goto err;
+        }
+
+        dsi->user_dsi_cinfo = ctx.dsi_cinfo;
+        dsi->user_dispc_cinfo = ctx.dispc_cinfo;
+
+        dsi->timings = ctx.dispc_vm;
+        dsi->vm_timings = ctx.dsi_vm;
+
+        mutex_unlock(&dsi->lock);
+
+        return 0;
+err:
+        mutex_unlock(&dsi->lock);
+
+        return r;
+}
+
+/*
+ * Return a hardcoded channel for the DSI output. This should work for
+ * current use cases, but this can be later expanded to either resolve
+ * the channel in some more dynamic manner, or get the channel as a user
+ * parameter.
+ */
+static enum omap_channel dsi_get_channel(int module_id)
+{
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP24xx:
+        case OMAPDSS_VER_AM43xx:
+                DSSWARN("DSI not supported\n");
+                return OMAP_DSS_CHANNEL_LCD;
+
+        case OMAPDSS_VER_OMAP34xx_ES1:
+        case OMAPDSS_VER_OMAP34xx_ES3:
+        case OMAPDSS_VER_OMAP3630:
+        case OMAPDSS_VER_AM35xx:
+                return OMAP_DSS_CHANNEL_LCD;
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                switch (module_id) {
+                case 0:
+                        return OMAP_DSS_CHANNEL_LCD;
+                case 1:
+                        return OMAP_DSS_CHANNEL_LCD2;
+                default:
+                        DSSWARN("unsupported module id\n");
+                        return OMAP_DSS_CHANNEL_LCD;
+                }
+
+        case OMAPDSS_VER_OMAP5:
+                switch (module_id) {
+                case 0:
+                        return OMAP_DSS_CHANNEL_LCD;
+                case 1:
+                        return OMAP_DSS_CHANNEL_LCD3;
+                default:
+                        DSSWARN("unsupported module id\n");
+                        return OMAP_DSS_CHANNEL_LCD;
+                }
+
+        default:
+                DSSWARN("unsupported DSS version\n");
+                return OMAP_DSS_CHANNEL_LCD;
+        }
+}
+
+static int 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;
+}
+
+static int 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;
+}
+
+static void 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;
+        }
+}
+
+
+static int dsi_get_clocks(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct clk *clk;
+
+        clk = devm_clk_get(&dsidev->dev, "fck");
+        if (IS_ERR(clk)) {
+                DSSERR("can't get fck\n");
+                return PTR_ERR(clk);
+        }
+
+        dsi->dss_clk = clk;
+
+        return 0;
+}
+
+static int dsi_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+        struct omap_overlay_manager *mgr;
+        int r;
+
+        r = dsi_regulator_init(dsidev);
+        if (r)
+                return r;
+
+        mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+        if (!mgr)
+                return -ENODEV;
+
+        r = dss_mgr_connect(mgr, dssdev);
+        if (r)
+                return r;
+
+        r = omapdss_output_set_device(dssdev, dst);
+        if (r) {
+                DSSERR("failed to connect output to new device: %s\n",
+                                dssdev->name);
+                dss_mgr_disconnect(mgr, dssdev);
+                return r;
+        }
+
+        return 0;
+}
+
+static void dsi_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        omapdss_output_unset_device(dssdev);
+
+        if (dssdev->manager)
+                dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_dsi_ops dsi_ops = {
+        .connect = dsi_connect,
+        .disconnect = dsi_disconnect,
+
+        .bus_lock = dsi_bus_lock,
+        .bus_unlock = dsi_bus_unlock,
+
+        .enable = dsi_display_enable,
+        .disable = dsi_display_disable,
+
+        .enable_hs = dsi_vc_enable_hs,
+
+        .configure_pins = dsi_configure_pins,
+        .set_config = dsi_set_config,
+
+        .enable_video_output = dsi_enable_video_output,
+        .disable_video_output = dsi_disable_video_output,
+
+        .update = dsi_update,
+
+        .enable_te = dsi_enable_te,
+
+        .request_vc = dsi_request_vc,
+        .set_vc_id = dsi_set_vc_id,
+        .release_vc = dsi_release_vc,
+
+        .dcs_write = dsi_vc_dcs_write,
+        .dcs_write_nosync = dsi_vc_dcs_write_nosync,
+        .dcs_read = dsi_vc_dcs_read,
+
+        .gen_write = dsi_vc_generic_write,
+        .gen_write_nosync = dsi_vc_generic_write_nosync,
+        .gen_read = dsi_vc_generic_read,
+
+        .bta_sync = dsi_vc_send_bta_sync,
+
+        .set_max_rx_packet_size = dsi_vc_set_max_rx_packet_size,
+};
+
+static void dsi_init_output(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_dss_device *out = &dsi->output;
+
+        out->dev = &dsidev->dev;
+        out->id = dsi->module_id == 0 ?
+                        OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
+
+        out->output_type = OMAP_DISPLAY_TYPE_DSI;
+        out->name = dsi->module_id == 0 ? "dsi.0" : "dsi.1";
+        out->dispc_channel = dsi_get_channel(dsi->module_id);
+        out->ops.dsi = &dsi_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void dsi_uninit_output(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct omap_dss_device *out = &dsi->output;
+
+        omapdss_unregister_output(out);
+}
+
+static int dsi_probe_of(struct platform_device *pdev)
+{
+        struct device_node *node = pdev->dev.of_node;
+        struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+        struct property *prop;
+        u32 lane_arr[10];
+        int len, num_pins;
+        int r, i;
+        struct device_node *ep;
+        struct omap_dsi_pin_config pin_cfg;
+
+        ep = omapdss_of_get_first_endpoint(node);
+        if (!ep)
+                return 0;
+
+        prop = of_find_property(ep, "lanes", &len);
+        if (prop == NULL) {
+                dev_err(&pdev->dev, "failed to find lane data\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        num_pins = len / sizeof(u32);
+
+        if (num_pins < 4 || num_pins % 2 != 0 ||
+                num_pins > dsi->num_lanes_supported * 2) {
+                dev_err(&pdev->dev, "bad number of lanes\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        r = of_property_read_u32_array(ep, "lanes", lane_arr, num_pins);
+        if (r) {
+                dev_err(&pdev->dev, "failed to read lane data\n");
+                goto err;
+        }
+
+        pin_cfg.num_pins = num_pins;
+        for (i = 0; i < num_pins; ++i)
+                pin_cfg.pins[i] = (int)lane_arr[i];
+
+        r = dsi_configure_pins(&dsi->output, &pin_cfg);
+        if (r) {
+                dev_err(&pdev->dev, "failed to configure pins");
+                goto err;
+        }
+
+        of_node_put(ep);
+
+        return 0;
+
+err:
+        of_node_put(ep);
+        return r;
+}
+
+static const struct dss_pll_ops dsi_pll_ops = {
+        .enable = dsi_pll_enable,
+        .disable = dsi_pll_disable,
+        .set_config = dss_pll_write_config_type_a,
+};
+
+static const struct dss_pll_hw dss_omap3_dsi_pll_hw = {
+        .n_max = (1 << 7) - 1,
+        .m_max = (1 << 11) - 1,
+        .mX_max = (1 << 4) - 1,
+        .fint_min = 750000,
+        .fint_max = 2100000,
+        .clkdco_low = 1000000000,
+        .clkdco_max = 1800000000,
+
+        .n_msb = 7,
+        .n_lsb = 1,
+        .m_msb = 18,
+        .m_lsb = 8,
+
+        .mX_msb[0] = 22,
+        .mX_lsb[0] = 19,
+        .mX_msb[1] = 26,
+        .mX_lsb[1] = 23,
+
+        .has_stopmode = true,
+        .has_freqsel = true,
+        .has_selfreqdco = false,
+        .has_refsel = false,
+};
+
+static const struct dss_pll_hw dss_omap4_dsi_pll_hw = {
+        .n_max = (1 << 8) - 1,
+        .m_max = (1 << 12) - 1,
+        .mX_max = (1 << 5) - 1,
+        .fint_min = 500000,
+        .fint_max = 2500000,
+        .clkdco_low = 1000000000,
+        .clkdco_max = 1800000000,
+
+        .n_msb = 8,
+        .n_lsb = 1,
+        .m_msb = 20,
+        .m_lsb = 9,
+
+        .mX_msb[0] = 25,
+        .mX_lsb[0] = 21,
+        .mX_msb[1] = 30,
+        .mX_lsb[1] = 26,
+
+        .has_stopmode = true,
+        .has_freqsel = false,
+        .has_selfreqdco = false,
+        .has_refsel = false,
+};
+
+static const struct dss_pll_hw dss_omap5_dsi_pll_hw = {
+        .n_max = (1 << 8) - 1,
+        .m_max = (1 << 12) - 1,
+        .mX_max = (1 << 5) - 1,
+        .fint_min = 150000,
+        .fint_max = 52000000,
+        .clkdco_low = 1000000000,
+        .clkdco_max = 1800000000,
+
+        .n_msb = 8,
+        .n_lsb = 1,
+        .m_msb = 20,
+        .m_lsb = 9,
+
+        .mX_msb[0] = 25,
+        .mX_lsb[0] = 21,
+        .mX_msb[1] = 30,
+        .mX_lsb[1] = 26,
+
+        .has_stopmode = true,
+        .has_freqsel = false,
+        .has_selfreqdco = true,
+        .has_refsel = true,
+};
+
+static int dsi_init_pll_data(struct platform_device *dsidev)
+{
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+        struct dss_pll *pll = &dsi->pll;
+        struct clk *clk;
+        int r;
+
+        clk = devm_clk_get(&dsidev->dev, "sys_clk");
+        if (IS_ERR(clk)) {
+                DSSERR("can't get sys_clk\n");
+                return PTR_ERR(clk);
+        }
+
+        pll->name = dsi->module_id == 0 ? "dsi0" : "dsi1";
+        pll->id = dsi->module_id == 0 ? DSS_PLL_DSI1 : DSS_PLL_DSI2;
+        pll->clkin = clk;
+        pll->base = dsi->pll_base;
+
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP34xx_ES1:
+        case OMAPDSS_VER_OMAP34xx_ES3:
+        case OMAPDSS_VER_OMAP3630:
+        case OMAPDSS_VER_AM35xx:
+                pll->hw = &dss_omap3_dsi_pll_hw;
+                break;
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                pll->hw = &dss_omap4_dsi_pll_hw;
+                break;
+
+        case OMAPDSS_VER_OMAP5:
+                pll->hw = &dss_omap5_dsi_pll_hw;
+                break;
+
+        default:
+                return -ENODEV;
+        }
+
+        pll->ops = &dsi_pll_ops;
+
+        r = dss_pll_register(pll);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+/* DSI1 HW IP initialisation */
+static int dsi_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *dsidev = to_platform_device(dev);
+        u32 rev;
+        int r, i;
+        struct dsi_data *dsi;
+        struct resource *dsi_mem;
+        struct resource *res;
+        struct resource temp_res;
+
+        dsi = devm_kzalloc(&dsidev->dev, sizeof(*dsi), GFP_KERNEL);
+        if (!dsi)
+                return -ENOMEM;
+
+        dsi->pdev = dsidev;
+        dev_set_drvdata(&dsidev->dev, dsi);
+
+        spin_lock_init(&dsi->irq_lock);
+        spin_lock_init(&dsi->errors_lock);
+        dsi->errors = 0;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        spin_lock_init(&dsi->irq_stats_lock);
+        dsi->irq_stats.last_reset = jiffies;
+#endif
+
+        mutex_init(&dsi->lock);
+        sema_init(&dsi->bus_lock, 1);
+
+        INIT_DEFERRABLE_WORK(&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;
+#endif
+
+        res = platform_get_resource_byname(dsidev, IORESOURCE_MEM, "proto");
+        if (!res) {
+                res = platform_get_resource(dsidev, IORESOURCE_MEM, 0);
+                if (!res) {
+                        DSSERR("can't get IORESOURCE_MEM DSI\n");
+                        return -EINVAL;
+                }
+
+                temp_res.start = res->start;
+                temp_res.end = temp_res.start + DSI_PROTO_SZ - 1;
+                res = &temp_res;
+        }
+
+        dsi_mem = res;
+
+        dsi->proto_base = devm_ioremap(&dsidev->dev, res->start,
+                resource_size(res));
+        if (!dsi->proto_base) {
+                DSSERR("can't ioremap DSI protocol engine\n");
+                return -ENOMEM;
+        }
+
+        res = platform_get_resource_byname(dsidev, IORESOURCE_MEM, "phy");
+        if (!res) {
+                res = platform_get_resource(dsidev, IORESOURCE_MEM, 0);
+                if (!res) {
+                        DSSERR("can't get IORESOURCE_MEM DSI\n");
+                        return -EINVAL;
+                }
+
+                temp_res.start = res->start + DSI_PHY_OFFSET;
+                temp_res.end = temp_res.start + DSI_PHY_SZ - 1;
+                res = &temp_res;
+        }
+
+        dsi->phy_base = devm_ioremap(&dsidev->dev, res->start,
+                resource_size(res));
+        if (!dsi->proto_base) {
+                DSSERR("can't ioremap DSI PHY\n");
+                return -ENOMEM;
+        }
+
+        res = platform_get_resource_byname(dsidev, IORESOURCE_MEM, "pll");
+        if (!res) {
+                res = platform_get_resource(dsidev, IORESOURCE_MEM, 0);
+                if (!res) {
+                        DSSERR("can't get IORESOURCE_MEM DSI\n");
+                        return -EINVAL;
+                }
+
+                temp_res.start = res->start + DSI_PLL_OFFSET;
+                temp_res.end = temp_res.start + DSI_PLL_SZ - 1;
+                res = &temp_res;
+        }
+
+        dsi->pll_base = devm_ioremap(&dsidev->dev, res->start,
+                resource_size(res));
+        if (!dsi->proto_base) {
+                DSSERR("can't ioremap DSI PLL\n");
+                return -ENOMEM;
+        }
+
+        dsi->irq = platform_get_irq(dsi->pdev, 0);
+        if (dsi->irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                return -ENODEV;
+        }
+
+        r = devm_request_irq(&dsidev->dev, dsi->irq, omap_dsi_irq_handler,
+                             IRQF_SHARED, dev_name(&dsidev->dev), dsi->pdev);
+        if (r < 0) {
+                DSSERR("request_irq failed\n");
+                return r;
+        }
+
+        if (dsidev->dev.of_node) {
+                const struct of_device_id *match;
+                const struct dsi_module_id_data *d;
+
+                match = of_match_node(dsi_of_match, dsidev->dev.of_node);
+                if (!match) {
+                        DSSERR("unsupported DSI module\n");
+                        return -ENODEV;
+                }
+
+                d = match->data;
+
+                while (d->address != 0 && d->address != dsi_mem->start)
+                        d++;
+
+                if (d->address == 0) {
+                        DSSERR("unsupported DSI module\n");
+                        return -ENODEV;
+                }
+
+                dsi->module_id = d->id;
+        } else {
+                dsi->module_id = dsidev->id;
+        }
+
+        /* DSI VCs initialization */
+        for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+                dsi->vc[i].source = DSI_VC_SOURCE_L4;
+                dsi->vc[i].dssdev = NULL;
+                dsi->vc[i].vc_id = 0;
+        }
+
+        r = dsi_get_clocks(dsidev);
+        if (r)
+                return r;
+
+        dsi_init_pll_data(dsidev);
+
+        pm_runtime_enable(&dsidev->dev);
+
+        r = dsi_runtime_get(dsidev);
+        if (r)
+                goto err_runtime_get;
+
+        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 on OMAP3 doesn't have register DSI_GNQ, set number
+         * of data to 3 by default */
+        if (dss_has_feature(FEAT_DSI_GNQ))
+                /* NB_DATA_LANES */
+                dsi->num_lanes_supported = 1 + REG_GET(dsidev, DSI_GNQ, 11, 9);
+        else
+                dsi->num_lanes_supported = 3;
+
+        dsi->line_buffer_size = dsi_get_line_buf_size(dsidev);
+
+        dsi_init_output(dsidev);
+
+        if (dsidev->dev.of_node) {
+                r = dsi_probe_of(dsidev);
+                if (r) {
+                        DSSERR("Invalid DSI DT data\n");
+                        goto err_probe_of;
+                }
+
+                r = of_platform_populate(dsidev->dev.of_node, NULL, NULL,
+                        &dsidev->dev);
+                if (r)
+                        DSSERR("Failed to populate DSI child devices: %d\n", r);
+        }
+
+        dsi_runtime_put(dsidev);
+
+        if (dsi->module_id == 0)
+                dss_debugfs_create_file("dsi1_regs", dsi1_dump_regs);
+        else if (dsi->module_id == 1)
+                dss_debugfs_create_file("dsi2_regs", dsi2_dump_regs);
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+        if (dsi->module_id == 0)
+                dss_debugfs_create_file("dsi1_irqs", dsi1_dump_irqs);
+        else if (dsi->module_id == 1)
+                dss_debugfs_create_file("dsi2_irqs", dsi2_dump_irqs);
+#endif
+
+        return 0;
+
+err_probe_of:
+        dsi_uninit_output(dsidev);
+        dsi_runtime_put(dsidev);
+
+err_runtime_get:
+        pm_runtime_disable(&dsidev->dev);
+        return r;
+}
+
+static void dsi_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *dsidev = to_platform_device(dev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+        of_platform_depopulate(&dsidev->dev);
+
+        WARN_ON(dsi->scp_clk_refcount > 0);
+
+        dss_pll_unregister(&dsi->pll);
+
+        dsi_uninit_output(dsidev);
+
+        pm_runtime_disable(&dsidev->dev);
+
+        if (dsi->vdds_dsi_reg != NULL && dsi->vdds_dsi_enabled) {
+                regulator_disable(dsi->vdds_dsi_reg);
+                dsi->vdds_dsi_enabled = false;
+        }
+}
+
+static const struct component_ops dsi_component_ops = {
+        .bind   = dsi_bind,
+        .unbind = dsi_unbind,
+};
+
+static int dsi_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &dsi_component_ops);
+}
+
+static int dsi_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &dsi_component_ops);
+        return 0;
+}
+
+static int dsi_runtime_suspend(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+
+        dsi->is_enabled = false;
+        /* ensure the irq handler sees the is_enabled value */
+        smp_wmb();
+        /* wait for current handler to finish before turning the DSI off */
+        synchronize_irq(dsi->irq);
+
+        dispc_runtime_put();
+
+        return 0;
+}
+
+static int dsi_runtime_resume(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+        int r;
+
+        r = dispc_runtime_get();
+        if (r)
+                return r;
+
+        dsi->is_enabled = true;
+        /* ensure the irq handler sees the is_enabled value */
+        smp_wmb();
+
+        return 0;
+}
+
+static const struct dev_pm_ops dsi_pm_ops = {
+        .runtime_suspend = dsi_runtime_suspend,
+        .runtime_resume = dsi_runtime_resume,
+};
+
+static const struct dsi_module_id_data dsi_of_data_omap3[] = {
+        { .address = 0x4804fc00, .id = 0, },
+        { },
+};
+
+static const struct dsi_module_id_data dsi_of_data_omap4[] = {
+        { .address = 0x58004000, .id = 0, },
+        { .address = 0x58005000, .id = 1, },
+        { },
+};
+
+static const struct dsi_module_id_data dsi_of_data_omap5[] = {
+        { .address = 0x58004000, .id = 0, },
+        { .address = 0x58009000, .id = 1, },
+        { },
+};
+
+static const struct of_device_id dsi_of_match[] = {
+        { .compatible = "ti,omap3-dsi", .data = dsi_of_data_omap3, },
+        { .compatible = "ti,omap4-dsi", .data = dsi_of_data_omap4, },
+        { .compatible = "ti,omap5-dsi", .data = dsi_of_data_omap5, },
+        {},
+};
+
+static struct platform_driver omap_dsihw_driver = {
+        .probe          = dsi_probe,
+        .remove         = dsi_remove,
+        .driver         = {
+                .name   = "omapdss_dsi",
+                .pm     = &dsi_pm_ops,
+                .of_match_table = dsi_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init dsi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dsihw_driver);
+}
+
+void dsi_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_dsihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss-of.c b/drivers/video/fbdev/omap2/omapfb/dss/dss-of.c
new file mode 100644
index 000000000000..bf407b6ba15c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss-of.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/seq_file.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+struct device_node *
+omapdss_of_get_next_port(const struct device_node *parent,
+                         struct device_node *prev)
+{
+        struct device_node *port = NULL;
+
+        if (!parent)
+                return NULL;
+
+        if (!prev) {
+                struct device_node *ports;
+                /*
+                 * It's the first call, we have to find a port subnode
+                 * within this node or within an optional 'ports' node.
+                 */
+                ports = of_get_child_by_name(parent, "ports");
+                if (ports)
+                        parent = ports;
+
+                port = of_get_child_by_name(parent, "port");
+
+                /* release the 'ports' node */
+                of_node_put(ports);
+        } else {
+                struct device_node *ports;
+
+                ports = of_get_parent(prev);
+                if (!ports)
+                        return NULL;
+
+                do {
+                        port = of_get_next_child(ports, prev);
+                        if (!port) {
+                                of_node_put(ports);
+                                return NULL;
+                        }
+                        prev = port;
+                } while (of_node_cmp(port->name, "port") != 0);
+
+                of_node_put(ports);
+        }
+
+        return port;
+}
+EXPORT_SYMBOL_GPL(omapdss_of_get_next_port);
+
+struct device_node *
+omapdss_of_get_next_endpoint(const struct device_node *parent,
+                             struct device_node *prev)
+{
+        struct device_node *ep = NULL;
+
+        if (!parent)
+                return NULL;
+
+        do {
+                ep = of_get_next_child(parent, prev);
+                if (!ep)
+                        return NULL;
+                prev = ep;
+        } while (of_node_cmp(ep->name, "endpoint") != 0);
+
+        return ep;
+}
+EXPORT_SYMBOL_GPL(omapdss_of_get_next_endpoint);
+
+struct device_node *dss_of_port_get_parent_device(struct device_node *port)
+{
+        struct device_node *np;
+        int i;
+
+        if (!port)
+                return NULL;
+
+        np = of_get_parent(port);
+
+        for (i = 0; i < 2 && np; ++i) {
+                struct property *prop;
+
+                prop = of_find_property(np, "compatible", NULL);
+
+                if (prop)
+                        return np;
+
+                np = of_get_next_parent(np);
+        }
+
+        return NULL;
+}
+
+u32 dss_of_port_get_port_number(struct device_node *port)
+{
+        int r;
+        u32 reg;
+
+        r = of_property_read_u32(port, "reg", &reg);
+        if (r)
+                reg = 0;
+
+        return reg;
+}
+
+static struct device_node *omapdss_of_get_remote_port(const struct device_node *node)
+{
+        struct device_node *np;
+
+        np = of_parse_phandle(node, "remote-endpoint", 0);
+        if (!np)
+                return NULL;
+
+        np = of_get_next_parent(np);
+
+        return np;
+}
+
+struct device_node *
+omapdss_of_get_first_endpoint(const struct device_node *parent)
+{
+        struct device_node *port, *ep;
+
+        port = omapdss_of_get_next_port(parent, NULL);
+
+        if (!port)
+                return NULL;
+
+        ep = omapdss_of_get_next_endpoint(port, NULL);
+
+        of_node_put(port);
+
+        return ep;
+}
+EXPORT_SYMBOL_GPL(omapdss_of_get_first_endpoint);
+
+struct omap_dss_device *
+omapdss_of_find_source_for_first_ep(struct device_node *node)
+{
+        struct device_node *ep;
+        struct device_node *src_port;
+        struct omap_dss_device *src;
+
+        ep = omapdss_of_get_first_endpoint(node);
+        if (!ep)
+                return ERR_PTR(-EINVAL);
+
+        src_port = omapdss_of_get_remote_port(ep);
+        if (!src_port) {
+                of_node_put(ep);
+                return ERR_PTR(-EINVAL);
+        }
+
+        of_node_put(ep);
+
+        src = omap_dss_find_output_by_port_node(src_port);
+
+        of_node_put(src_port);
+
+        return src ? src : ERR_PTR(-EPROBE_DEFER);
+}
+EXPORT_SYMBOL_GPL(omapdss_of_find_source_for_first_ep);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss.c b/drivers/video/fbdev/omap2/omapfb/dss/dss.c
new file mode 100644
index 000000000000..0078c4d1fc31
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss.c
@@ -0,0 +1,1329 @@
+/*
+ * linux/drivers/video/omap2/dss/dss.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "DSS"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/gfp.h>
+#include <linux/sizes.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+#include <linux/suspend.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+#define DSS_SZ_REGS                     SZ_512
+
+struct dss_reg {
+        u16 idx;
+};
+
+#define DSS_REG(idx)                    ((const struct dss_reg) { idx })
+
+#define DSS_REVISION                    DSS_REG(0x0000)
+#define DSS_SYSCONFIG                   DSS_REG(0x0010)
+#define DSS_SYSSTATUS                   DSS_REG(0x0014)
+#define DSS_CONTROL                     DSS_REG(0x0040)
+#define DSS_SDI_CONTROL                 DSS_REG(0x0044)
+#define DSS_PLL_CONTROL                 DSS_REG(0x0048)
+#define DSS_SDI_STATUS                  DSS_REG(0x005C)
+
+#define REG_GET(idx, start, end) \
+        FLD_GET(dss_read_reg(idx), start, end)
+
+#define REG_FLD_MOD(idx, val, start, end) \
+        dss_write_reg(idx, FLD_MOD(dss_read_reg(idx), val, start, end))
+
+struct dss_features {
+        u8 fck_div_max;
+        u8 dss_fck_multiplier;
+        const char *parent_clk_name;
+        const enum omap_display_type *ports;
+        int num_ports;
+        int (*dpi_select_source)(int port, enum omap_channel channel);
+};
+
+static struct {
+        struct platform_device *pdev;
+        void __iomem    *base;
+        struct regmap   *syscon_pll_ctrl;
+        u32             syscon_pll_ctrl_offset;
+
+        struct clk      *parent_clk;
+        struct clk      *dss_clk;
+        unsigned long   dss_clk_rate;
+
+        unsigned long   cache_req_pck;
+        unsigned long   cache_prate;
+        struct dispc_clock_info cache_dispc_cinfo;
+
+        enum omap_dss_clk_source dsi_clk_source[MAX_NUM_DSI];
+        enum omap_dss_clk_source dispc_clk_source;
+        enum omap_dss_clk_source lcd_clk_source[MAX_DSS_LCD_MANAGERS];
+
+        bool            ctx_valid;
+        u32             ctx[DSS_SZ_REGS / sizeof(u32)];
+
+        const struct dss_features *feat;
+
+        struct dss_pll  *video1_pll;
+        struct dss_pll  *video2_pll;
+} 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",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC] = "DSI_PLL2_HSDIV_DISPC",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]   = "DSI_PLL2_HSDIV_DSI",
+};
+
+static bool dss_initialized;
+
+bool omapdss_is_initialized(void)
+{
+        return dss_initialized;
+}
+EXPORT_SYMBOL(omapdss_is_initialized);
+
+static inline void dss_write_reg(const struct dss_reg idx, u32 val)
+{
+        __raw_writel(val, dss.base + idx.idx);
+}
+
+static inline u32 dss_read_reg(const struct dss_reg idx)
+{
+        return __raw_readl(dss.base + idx.idx);
+}
+
+#define SR(reg) \
+        dss.ctx[(DSS_##reg).idx / sizeof(u32)] = dss_read_reg(DSS_##reg)
+#define RR(reg) \
+        dss_write_reg(DSS_##reg, dss.ctx[(DSS_##reg).idx / sizeof(u32)])
+
+static void dss_save_context(void)
+{
+        DSSDBG("dss_save_context\n");
+
+        SR(CONTROL);
+
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                SR(SDI_CONTROL);
+                SR(PLL_CONTROL);
+        }
+
+        dss.ctx_valid = true;
+
+        DSSDBG("context saved\n");
+}
+
+static void dss_restore_context(void)
+{
+        DSSDBG("dss_restore_context\n");
+
+        if (!dss.ctx_valid)
+                return;
+
+        RR(CONTROL);
+
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                RR(SDI_CONTROL);
+                RR(PLL_CONTROL);
+        }
+
+        DSSDBG("context restored\n");
+}
+
+#undef SR
+#undef RR
+
+void dss_ctrl_pll_enable(enum dss_pll_id pll_id, bool enable)
+{
+        unsigned shift;
+        unsigned val;
+
+        if (!dss.syscon_pll_ctrl)
+                return;
+
+        val = !enable;
+
+        switch (pll_id) {
+        case DSS_PLL_VIDEO1:
+                shift = 0;
+                break;
+        case DSS_PLL_VIDEO2:
+                shift = 1;
+                break;
+        case DSS_PLL_HDMI:
+                shift = 2;
+                break;
+        default:
+                DSSERR("illegal DSS PLL ID %d\n", pll_id);
+                return;
+        }
+
+        regmap_update_bits(dss.syscon_pll_ctrl, dss.syscon_pll_ctrl_offset,
+                1 << shift, val << shift);
+}
+
+void dss_ctrl_pll_set_control_mux(enum dss_pll_id pll_id,
+        enum omap_channel channel)
+{
+        unsigned shift, val;
+
+        if (!dss.syscon_pll_ctrl)
+                return;
+
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                shift = 3;
+
+                switch (pll_id) {
+                case DSS_PLL_VIDEO1:
+                        val = 0; break;
+                case DSS_PLL_HDMI:
+                        val = 1; break;
+                default:
+                        DSSERR("error in PLL mux config for LCD\n");
+                        return;
+                }
+
+                break;
+        case OMAP_DSS_CHANNEL_LCD2:
+                shift = 5;
+
+                switch (pll_id) {
+                case DSS_PLL_VIDEO1:
+                        val = 0; break;
+                case DSS_PLL_VIDEO2:
+                        val = 1; break;
+                case DSS_PLL_HDMI:
+                        val = 2; break;
+                default:
+                        DSSERR("error in PLL mux config for LCD2\n");
+                        return;
+                }
+
+                break;
+        case OMAP_DSS_CHANNEL_LCD3:
+                shift = 7;
+
+                switch (pll_id) {
+                case DSS_PLL_VIDEO1:
+                        val = 1; break;
+                case DSS_PLL_VIDEO2:
+                        val = 0; break;
+                case DSS_PLL_HDMI:
+                        val = 2; break;
+                default:
+                        DSSERR("error in PLL mux config for LCD3\n");
+                        return;
+                }
+
+                break;
+        default:
+                DSSERR("error in PLL mux config\n");
+                return;
+        }
+
+        regmap_update_bits(dss.syscon_pll_ctrl, dss.syscon_pll_ctrl_offset,
+                0x3 << shift, val << shift);
+}
+
+void dss_sdi_init(int datapairs)
+{
+        u32 l;
+
+        BUG_ON(datapairs > 3 || datapairs < 1);
+
+        l = dss_read_reg(DSS_SDI_CONTROL);
+        l = FLD_MOD(l, 0xf, 19, 15);            /* SDI_PDIV */
+        l = FLD_MOD(l, datapairs-1, 3, 2);      /* SDI_PRSEL */
+        l = FLD_MOD(l, 2, 1, 0);                /* SDI_BWSEL */
+        dss_write_reg(DSS_SDI_CONTROL, l);
+
+        l = dss_read_reg(DSS_PLL_CONTROL);
+        l = FLD_MOD(l, 0x7, 25, 22);    /* SDI_PLL_FREQSEL */
+        l = FLD_MOD(l, 0xb, 16, 11);    /* SDI_PLL_REGN */
+        l = FLD_MOD(l, 0xb4, 10, 1);    /* SDI_PLL_REGM */
+        dss_write_reg(DSS_PLL_CONTROL, l);
+}
+
+int dss_sdi_enable(void)
+{
+        unsigned long timeout;
+
+        dispc_pck_free_enable(1);
+
+        /* Reset SDI PLL */
+        REG_FLD_MOD(DSS_PLL_CONTROL, 1, 18, 18); /* SDI_PLL_SYSRESET */
+        udelay(1);      /* wait 2x PCLK */
+
+        /* Lock SDI PLL */
+        REG_FLD_MOD(DSS_PLL_CONTROL, 1, 28, 28); /* SDI_PLL_GOBIT */
+
+        /* Waiting for PLL lock request to complete */
+        timeout = jiffies + msecs_to_jiffies(500);
+        while (dss_read_reg(DSS_SDI_STATUS) & (1 << 6)) {
+                if (time_after_eq(jiffies, timeout)) {
+                        DSSERR("PLL lock request timed out\n");
+                        goto err1;
+                }
+        }
+
+        /* Clearing PLL_GO bit */
+        REG_FLD_MOD(DSS_PLL_CONTROL, 0, 28, 28);
+
+        /* Waiting for PLL to lock */
+        timeout = jiffies + msecs_to_jiffies(500);
+        while (!(dss_read_reg(DSS_SDI_STATUS) & (1 << 5))) {
+                if (time_after_eq(jiffies, timeout)) {
+                        DSSERR("PLL lock timed out\n");
+                        goto err1;
+                }
+        }
+
+        dispc_lcd_enable_signal(1);
+
+        /* Waiting for SDI reset to complete */
+        timeout = jiffies + msecs_to_jiffies(500);
+        while (!(dss_read_reg(DSS_SDI_STATUS) & (1 << 2))) {
+                if (time_after_eq(jiffies, timeout)) {
+                        DSSERR("SDI reset timed out\n");
+                        goto err2;
+                }
+        }
+
+        return 0;
+
+ err2:
+        dispc_lcd_enable_signal(0);
+ err1:
+        /* Reset SDI PLL */
+        REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
+
+        dispc_pck_free_enable(0);
+
+        return -ETIMEDOUT;
+}
+
+void dss_sdi_disable(void)
+{
+        dispc_lcd_enable_signal(0);
+
+        dispc_pck_free_enable(0);
+
+        /* Reset SDI PLL */
+        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)
+{
+        const char *fclk_name, *fclk_real_name;
+        unsigned long fclk_rate;
+
+        if (dss_runtime_get())
+                return;
+
+        seq_printf(s, "- DSS -\n");
+
+        fclk_name = dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+        fclk_real_name = dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+        fclk_rate = clk_get_rate(dss.dss_clk);
+
+        seq_printf(s, "%s (%s) = %lu\n",
+                        fclk_name, fclk_real_name,
+                        fclk_rate);
+
+        dss_runtime_put();
+}
+
+static void dss_dump_regs(struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dss_read_reg(r))
+
+        if (dss_runtime_get())
+                return;
+
+        DUMPREG(DSS_REVISION);
+        DUMPREG(DSS_SYSCONFIG);
+        DUMPREG(DSS_SYSSTATUS);
+        DUMPREG(DSS_CONTROL);
+
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                DUMPREG(DSS_SDI_CONTROL);
+                DUMPREG(DSS_PLL_CONTROL);
+                DUMPREG(DSS_SDI_STATUS);
+        }
+
+        dss_runtime_put();
+#undef DUMPREG
+}
+
+static void dss_select_dispc_clk_source(enum omap_dss_clk_source clk_src)
+{
+        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;
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                b = 2;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        dss_feat_get_reg_field(FEAT_REG_DISPC_CLK_SWITCH, &start, &end);
+
+        REG_FLD_MOD(DSS_CONTROL, b, start, end);        /* DISPC_CLK_SWITCH */
+
+        dss.dispc_clk_source = clk_src;
+}
+
+void dss_select_dsi_clk_source(int dsi_module,
+                enum omap_dss_clk_source clk_src)
+{
+        int b, pos;
+
+        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;
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI:
+                BUG_ON(dsi_module != 1);
+                b = 1;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        pos = dsi_module == 0 ? 1 : 10;
+        REG_FLD_MOD(DSS_CONTROL, b, pos, pos);  /* DSIx_CLK_SWITCH */
+
+        dss.dsi_clk_source[dsi_module] = clk_src;
+}
+
+void dss_select_lcd_clk_source(enum omap_channel channel,
+                enum omap_dss_clk_source clk_src)
+{
+        int b, ix, pos;
+
+        if (!dss_has_feature(FEAT_LCD_CLK_SRC)) {
+                dss_select_dispc_clk_source(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;
+                break;
+        case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+                BUG_ON(channel != OMAP_DSS_CHANNEL_LCD2 &&
+                       channel != OMAP_DSS_CHANNEL_LCD3);
+                b = 1;
+                break;
+        default:
+                BUG();
+                return;
+        }
+
+        pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+             (channel == OMAP_DSS_CHANNEL_LCD2 ? 12 : 19);
+        REG_FLD_MOD(DSS_CONTROL, b, pos, pos);  /* LCDx_CLK_SWITCH */
+
+        ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+            (channel == OMAP_DSS_CHANNEL_LCD2 ? 1 : 2);
+        dss.lcd_clk_source[ix] = clk_src;
+}
+
+enum omap_dss_clk_source dss_get_dispc_clk_source(void)
+{
+        return dss.dispc_clk_source;
+}
+
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module)
+{
+        return dss.dsi_clk_source[dsi_module];
+}
+
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
+{
+        if (dss_has_feature(FEAT_LCD_CLK_SRC)) {
+                int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+                        (channel == OMAP_DSS_CHANNEL_LCD2 ? 1 : 2);
+                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;
+        }
+}
+
+bool dss_div_calc(unsigned long pck, unsigned long fck_min,
+                dss_div_calc_func func, void *data)
+{
+        int fckd, fckd_start, fckd_stop;
+        unsigned long fck;
+        unsigned long fck_hw_max;
+        unsigned long fckd_hw_max;
+        unsigned long prate;
+        unsigned m;
+
+        fck_hw_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+        if (dss.parent_clk == NULL) {
+                unsigned pckd;
+
+                pckd = fck_hw_max / pck;
+
+                fck = pck * pckd;
+
+                fck = clk_round_rate(dss.dss_clk, fck);
+
+                return func(fck, data);
+        }
+
+        fckd_hw_max = dss.feat->fck_div_max;
+
+        m = dss.feat->dss_fck_multiplier;
+        prate = clk_get_rate(dss.parent_clk);
+
+        fck_min = fck_min ? fck_min : 1;
+
+        fckd_start = min(prate * m / fck_min, fckd_hw_max);
+        fckd_stop = max(DIV_ROUND_UP(prate * m, fck_hw_max), 1ul);
+
+        for (fckd = fckd_start; fckd >= fckd_stop; --fckd) {
+                fck = DIV_ROUND_UP(prate, fckd) * m;
+
+                if (func(fck, data))
+                        return true;
+        }
+
+        return false;
+}
+
+int dss_set_fck_rate(unsigned long rate)
+{
+        int r;
+
+        DSSDBG("set fck to %lu\n", rate);
+
+        r = clk_set_rate(dss.dss_clk, rate);
+        if (r)
+                return r;
+
+        dss.dss_clk_rate = clk_get_rate(dss.dss_clk);
+
+        WARN_ONCE(dss.dss_clk_rate != rate,
+                        "clk rate mismatch: %lu != %lu", dss.dss_clk_rate,
+                        rate);
+
+        return 0;
+}
+
+unsigned long dss_get_dispc_clk_rate(void)
+{
+        return dss.dss_clk_rate;
+}
+
+static int dss_setup_default_clock(void)
+{
+        unsigned long max_dss_fck, prate;
+        unsigned long fck;
+        unsigned fck_div;
+        int r;
+
+        max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+        if (dss.parent_clk == NULL) {
+                fck = clk_round_rate(dss.dss_clk, max_dss_fck);
+        } else {
+                prate = clk_get_rate(dss.parent_clk);
+
+                fck_div = DIV_ROUND_UP(prate * dss.feat->dss_fck_multiplier,
+                                max_dss_fck);
+                fck = DIV_ROUND_UP(prate, fck_div) * dss.feat->dss_fck_multiplier;
+        }
+
+        r = dss_set_fck_rate(fck);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+void dss_set_venc_output(enum omap_dss_venc_type type)
+{
+        int l = 0;
+
+        if (type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+                l = 0;
+        else if (type == OMAP_DSS_VENC_TYPE_SVIDEO)
+                l = 1;
+        else
+                BUG();
+
+        /* venc out selection. 0 = comp, 1 = svideo */
+        REG_FLD_MOD(DSS_CONTROL, l, 6, 6);
+}
+
+void dss_set_dac_pwrdn_bgz(bool enable)
+{
+        REG_FLD_MOD(DSS_CONTROL, enable, 5, 5); /* DAC Power-Down Control */
+}
+
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select src)
+{
+        enum omap_display_type dp;
+        dp = dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_DIGIT);
+
+        /* Complain about invalid selections */
+        WARN_ON((src == DSS_VENC_TV_CLK) && !(dp & OMAP_DISPLAY_TYPE_VENC));
+        WARN_ON((src == DSS_HDMI_M_PCLK) && !(dp & OMAP_DISPLAY_TYPE_HDMI));
+
+        /* Select only if we have options */
+        if ((dp & OMAP_DISPLAY_TYPE_VENC) && (dp & OMAP_DISPLAY_TYPE_HDMI))
+                REG_FLD_MOD(DSS_CONTROL, src, 15, 15);  /* VENC_HDMI_SWITCH */
+}
+
+enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void)
+{
+        enum omap_display_type displays;
+
+        displays = dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_DIGIT);
+        if ((displays & OMAP_DISPLAY_TYPE_HDMI) == 0)
+                return DSS_VENC_TV_CLK;
+
+        if ((displays & OMAP_DISPLAY_TYPE_VENC) == 0)
+                return DSS_HDMI_M_PCLK;
+
+        return REG_GET(DSS_CONTROL, 15, 15);
+}
+
+static int dss_dpi_select_source_omap2_omap3(int port, enum omap_channel channel)
+{
+        if (channel != OMAP_DSS_CHANNEL_LCD)
+                return -EINVAL;
+
+        return 0;
+}
+
+static int dss_dpi_select_source_omap4(int port, enum omap_channel channel)
+{
+        int val;
+
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD2:
+                val = 0;
+                break;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                val = 1;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        REG_FLD_MOD(DSS_CONTROL, val, 17, 17);
+
+        return 0;
+}
+
+static int dss_dpi_select_source_omap5(int port, enum omap_channel channel)
+{
+        int val;
+
+        switch (channel) {
+        case OMAP_DSS_CHANNEL_LCD:
+                val = 1;
+                break;
+        case OMAP_DSS_CHANNEL_LCD2:
+                val = 2;
+                break;
+        case OMAP_DSS_CHANNEL_LCD3:
+                val = 3;
+                break;
+        case OMAP_DSS_CHANNEL_DIGIT:
+                val = 0;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        REG_FLD_MOD(DSS_CONTROL, val, 17, 16);
+
+        return 0;
+}
+
+static int dss_dpi_select_source_dra7xx(int port, enum omap_channel channel)
+{
+        switch (port) {
+        case 0:
+                return dss_dpi_select_source_omap5(port, channel);
+        case 1:
+                if (channel != OMAP_DSS_CHANNEL_LCD2)
+                        return -EINVAL;
+                break;
+        case 2:
+                if (channel != OMAP_DSS_CHANNEL_LCD3)
+                        return -EINVAL;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+int dss_dpi_select_source(int port, enum omap_channel channel)
+{
+        return dss.feat->dpi_select_source(port, channel);
+}
+
+static int dss_get_clocks(void)
+{
+        struct clk *clk;
+
+        clk = devm_clk_get(&dss.pdev->dev, "fck");
+        if (IS_ERR(clk)) {
+                DSSERR("can't get clock fck\n");
+                return PTR_ERR(clk);
+        }
+
+        dss.dss_clk = clk;
+
+        if (dss.feat->parent_clk_name) {
+                clk = clk_get(NULL, dss.feat->parent_clk_name);
+                if (IS_ERR(clk)) {
+                        DSSERR("Failed to get %s\n", dss.feat->parent_clk_name);
+                        return PTR_ERR(clk);
+                }
+        } else {
+                clk = NULL;
+        }
+
+        dss.parent_clk = clk;
+
+        return 0;
+}
+
+static void dss_put_clocks(void)
+{
+        if (dss.parent_clk)
+                clk_put(dss.parent_clk);
+}
+
+int dss_runtime_get(void)
+{
+        int r;
+
+        DSSDBG("dss_runtime_get\n");
+
+        r = pm_runtime_get_sync(&dss.pdev->dev);
+        WARN_ON(r < 0);
+        return r < 0 ? r : 0;
+}
+
+void dss_runtime_put(void)
+{
+        int r;
+
+        DSSDBG("dss_runtime_put\n");
+
+        r = pm_runtime_put_sync(&dss.pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS && r != -EBUSY);
+}
+
+/* DEBUGFS */
+#if defined(CONFIG_FB_OMAP2_DSS_DEBUGFS)
+void dss_debug_dump_clocks(struct seq_file *s)
+{
+        dss_dump_clocks(s);
+        dispc_dump_clocks(s);
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+        dsi_dump_clocks(s);
+#endif
+}
+#endif
+
+
+static const enum omap_display_type omap2plus_ports[] = {
+        OMAP_DISPLAY_TYPE_DPI,
+};
+
+static const enum omap_display_type omap34xx_ports[] = {
+        OMAP_DISPLAY_TYPE_DPI,
+        OMAP_DISPLAY_TYPE_SDI,
+};
+
+static const enum omap_display_type dra7xx_ports[] = {
+        OMAP_DISPLAY_TYPE_DPI,
+        OMAP_DISPLAY_TYPE_DPI,
+        OMAP_DISPLAY_TYPE_DPI,
+};
+
+static const struct dss_features omap24xx_dss_feats = {
+        /*
+         * fck div max is really 16, but the divider range has gaps. The range
+         * from 1 to 6 has no gaps, so let's use that as a max.
+         */
+        .fck_div_max            =       6,
+        .dss_fck_multiplier     =       2,
+        .parent_clk_name        =       "core_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+        .ports                  =       omap2plus_ports,
+        .num_ports              =       ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features omap34xx_dss_feats = {
+        .fck_div_max            =       16,
+        .dss_fck_multiplier     =       2,
+        .parent_clk_name        =       "dpll4_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+        .ports                  =       omap34xx_ports,
+        .num_ports              =       ARRAY_SIZE(omap34xx_ports),
+};
+
+static const struct dss_features omap3630_dss_feats = {
+        .fck_div_max            =       32,
+        .dss_fck_multiplier     =       1,
+        .parent_clk_name        =       "dpll4_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+        .ports                  =       omap2plus_ports,
+        .num_ports              =       ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features omap44xx_dss_feats = {
+        .fck_div_max            =       32,
+        .dss_fck_multiplier     =       1,
+        .parent_clk_name        =       "dpll_per_x2_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap4,
+        .ports                  =       omap2plus_ports,
+        .num_ports              =       ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features omap54xx_dss_feats = {
+        .fck_div_max            =       64,
+        .dss_fck_multiplier     =       1,
+        .parent_clk_name        =       "dpll_per_x2_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_omap5,
+        .ports                  =       omap2plus_ports,
+        .num_ports              =       ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features am43xx_dss_feats = {
+        .fck_div_max            =       0,
+        .dss_fck_multiplier     =       0,
+        .parent_clk_name        =       NULL,
+        .dpi_select_source      =       &dss_dpi_select_source_omap2_omap3,
+        .ports                  =       omap2plus_ports,
+        .num_ports              =       ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features dra7xx_dss_feats = {
+        .fck_div_max            =       64,
+        .dss_fck_multiplier     =       1,
+        .parent_clk_name        =       "dpll_per_x2_ck",
+        .dpi_select_source      =       &dss_dpi_select_source_dra7xx,
+        .ports                  =       dra7xx_ports,
+        .num_ports              =       ARRAY_SIZE(dra7xx_ports),
+};
+
+static int dss_init_features(struct platform_device *pdev)
+{
+        const struct dss_features *src;
+        struct dss_features *dst;
+
+        dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+        if (!dst) {
+                dev_err(&pdev->dev, "Failed to allocate local DSS Features\n");
+                return -ENOMEM;
+        }
+
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP24xx:
+                src = &omap24xx_dss_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP34xx_ES1:
+        case OMAPDSS_VER_OMAP34xx_ES3:
+        case OMAPDSS_VER_AM35xx:
+                src = &omap34xx_dss_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP3630:
+                src = &omap3630_dss_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                src = &omap44xx_dss_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP5:
+                src = &omap54xx_dss_feats;
+                break;
+
+        case OMAPDSS_VER_AM43xx:
+                src = &am43xx_dss_feats;
+                break;
+
+        case OMAPDSS_VER_DRA7xx:
+                src = &dra7xx_dss_feats;
+                break;
+
+        default:
+                return -ENODEV;
+        }
+
+        memcpy(dst, src, sizeof(*dst));
+        dss.feat = dst;
+
+        return 0;
+}
+
+static int dss_init_ports(struct platform_device *pdev)
+{
+        struct device_node *parent = pdev->dev.of_node;
+        struct device_node *port;
+        int r;
+
+        if (parent == NULL)
+                return 0;
+
+        port = omapdss_of_get_next_port(parent, NULL);
+        if (!port)
+                return 0;
+
+        if (dss.feat->num_ports == 0)
+                return 0;
+
+        do {
+                enum omap_display_type port_type;
+                u32 reg;
+
+                r = of_property_read_u32(port, "reg", &reg);
+                if (r)
+                        reg = 0;
+
+                if (reg >= dss.feat->num_ports)
+                        continue;
+
+                port_type = dss.feat->ports[reg];
+
+                switch (port_type) {
+                case OMAP_DISPLAY_TYPE_DPI:
+                        dpi_init_port(pdev, port);
+                        break;
+                case OMAP_DISPLAY_TYPE_SDI:
+                        sdi_init_port(pdev, port);
+                        break;
+                default:
+                        break;
+                }
+        } while ((port = omapdss_of_get_next_port(parent, port)) != NULL);
+
+        return 0;
+}
+
+static void dss_uninit_ports(struct platform_device *pdev)
+{
+        struct device_node *parent = pdev->dev.of_node;
+        struct device_node *port;
+
+        if (parent == NULL)
+                return;
+
+        port = omapdss_of_get_next_port(parent, NULL);
+        if (!port)
+                return;
+
+        if (dss.feat->num_ports == 0)
+                return;
+
+        do {
+                enum omap_display_type port_type;
+                u32 reg;
+                int r;
+
+                r = of_property_read_u32(port, "reg", &reg);
+                if (r)
+                        reg = 0;
+
+                if (reg >= dss.feat->num_ports)
+                        continue;
+
+                port_type = dss.feat->ports[reg];
+
+                switch (port_type) {
+                case OMAP_DISPLAY_TYPE_DPI:
+                        dpi_uninit_port(port);
+                        break;
+                case OMAP_DISPLAY_TYPE_SDI:
+                        sdi_uninit_port(port);
+                        break;
+                default:
+                        break;
+                }
+        } while ((port = omapdss_of_get_next_port(parent, port)) != NULL);
+}
+
+static int dss_video_pll_probe(struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct regulator *pll_regulator;
+        int r;
+
+        if (!np)
+                return 0;
+
+        if (of_property_read_bool(np, "syscon-pll-ctrl")) {
+                dss.syscon_pll_ctrl = syscon_regmap_lookup_by_phandle(np,
+                        "syscon-pll-ctrl");
+                if (IS_ERR(dss.syscon_pll_ctrl)) {
+                        dev_err(&pdev->dev,
+                                "failed to get syscon-pll-ctrl regmap\n");
+                        return PTR_ERR(dss.syscon_pll_ctrl);
+                }
+
+                if (of_property_read_u32_index(np, "syscon-pll-ctrl", 1,
+                                &dss.syscon_pll_ctrl_offset)) {
+                        dev_err(&pdev->dev,
+                                "failed to get syscon-pll-ctrl offset\n");
+                        return -EINVAL;
+                }
+        }
+
+        pll_regulator = devm_regulator_get(&pdev->dev, "vdda_video");
+        if (IS_ERR(pll_regulator)) {
+                r = PTR_ERR(pll_regulator);
+
+                switch (r) {
+                case -ENOENT:
+                        pll_regulator = NULL;
+                        break;
+
+                case -EPROBE_DEFER:
+                        return -EPROBE_DEFER;
+
+                default:
+                        DSSERR("can't get DPLL VDDA regulator\n");
+                        return r;
+                }
+        }
+
+        if (of_property_match_string(np, "reg-names", "pll1") >= 0) {
+                dss.video1_pll = dss_video_pll_init(pdev, 0, pll_regulator);
+                if (IS_ERR(dss.video1_pll))
+                        return PTR_ERR(dss.video1_pll);
+        }
+
+        if (of_property_match_string(np, "reg-names", "pll2") >= 0) {
+                dss.video2_pll = dss_video_pll_init(pdev, 1, pll_regulator);
+                if (IS_ERR(dss.video2_pll)) {
+                        dss_video_pll_uninit(dss.video1_pll);
+                        return PTR_ERR(dss.video2_pll);
+                }
+        }
+
+        return 0;
+}
+
+/* DSS HW IP initialisation */
+static int dss_bind(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        struct resource *dss_mem;
+        u32 rev;
+        int r;
+
+        dss.pdev = pdev;
+
+        r = dss_init_features(dss.pdev);
+        if (r)
+                return r;
+
+        dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
+        if (!dss_mem) {
+                DSSERR("can't get IORESOURCE_MEM DSS\n");
+                return -EINVAL;
+        }
+
+        dss.base = devm_ioremap(&pdev->dev, dss_mem->start,
+                                resource_size(dss_mem));
+        if (!dss.base) {
+                DSSERR("can't ioremap DSS\n");
+                return -ENOMEM;
+        }
+
+        r = dss_get_clocks();
+        if (r)
+                return r;
+
+        r = dss_setup_default_clock();
+        if (r)
+                goto err_setup_clocks;
+
+        r = dss_video_pll_probe(pdev);
+        if (r)
+                goto err_pll_init;
+
+        r = dss_init_ports(pdev);
+        if (r)
+                goto err_init_ports;
+
+        pm_runtime_enable(&pdev->dev);
+
+        r = dss_runtime_get();
+        if (r)
+                goto err_runtime_get;
+
+        dss.dss_clk_rate = clk_get_rate(dss.dss_clk);
+
+        /* Select DPLL */
+        REG_FLD_MOD(DSS_CONTROL, 0, 0, 0);
+
+        dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+        REG_FLD_MOD(DSS_CONTROL, 1, 4, 4);      /* venc dac demen */
+        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
+        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;
+
+        rev = dss_read_reg(DSS_REVISION);
+        printk(KERN_INFO "OMAP DSS rev %d.%d\n",
+                        FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+        dss_runtime_put();
+
+        r = component_bind_all(&pdev->dev, NULL);
+        if (r)
+                goto err_component;
+
+        dss_debugfs_create_file("dss", dss_dump_regs);
+
+        pm_set_vt_switch(0);
+
+        dss_initialized = true;
+
+        return 0;
+
+err_component:
+err_runtime_get:
+        pm_runtime_disable(&pdev->dev);
+        dss_uninit_ports(pdev);
+err_init_ports:
+        if (dss.video1_pll)
+                dss_video_pll_uninit(dss.video1_pll);
+
+        if (dss.video2_pll)
+                dss_video_pll_uninit(dss.video2_pll);
+err_pll_init:
+err_setup_clocks:
+        dss_put_clocks();
+        return r;
+}
+
+static void dss_unbind(struct device *dev)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        dss_initialized = false;
+
+        component_unbind_all(&pdev->dev, NULL);
+
+        if (dss.video1_pll)
+                dss_video_pll_uninit(dss.video1_pll);
+
+        if (dss.video2_pll)
+                dss_video_pll_uninit(dss.video2_pll);
+
+        dss_uninit_ports(pdev);
+
+        pm_runtime_disable(&pdev->dev);
+
+        dss_put_clocks();
+}
+
+static const struct component_master_ops dss_component_ops = {
+        .bind = dss_bind,
+        .unbind = dss_unbind,
+};
+
+static int dss_component_compare(struct device *dev, void *data)
+{
+        struct device *child = data;
+        return dev == child;
+}
+
+static int dss_add_child_component(struct device *dev, void *data)
+{
+        struct component_match **match = data;
+
+        /*
+         * HACK
+         * We don't have a working driver for rfbi, so skip it here always.
+         * Otherwise dss will never get probed successfully, as it will wait
+         * for rfbi to get probed.
+         */
+        if (strstr(dev_name(dev), "rfbi"))
+                return 0;
+
+        component_match_add(dev->parent, match, dss_component_compare, dev);
+
+        return 0;
+}
+
+static int dss_probe(struct platform_device *pdev)
+{
+        struct component_match *match = NULL;
+        int r;
+
+        /* add all the child devices as components */
+        device_for_each_child(&pdev->dev, &match, dss_add_child_component);
+
+        r = component_master_add_with_match(&pdev->dev, &dss_component_ops, match);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static int dss_remove(struct platform_device *pdev)
+{
+        component_master_del(&pdev->dev, &dss_component_ops);
+        return 0;
+}
+
+static int dss_runtime_suspend(struct device *dev)
+{
+        dss_save_context();
+        dss_set_min_bus_tput(dev, 0);
+
+        pinctrl_pm_select_sleep_state(dev);
+
+        return 0;
+}
+
+static int dss_runtime_resume(struct device *dev)
+{
+        int r;
+
+        pinctrl_pm_select_default_state(dev);
+
+        /*
+         * Set an arbitrarily high tput request to ensure OPP100.
+         * What we should really do is to make a request to stay in OPP100,
+         * without any tput requirements, but that is not currently possible
+         * via the PM layer.
+         */
+
+        r = dss_set_min_bus_tput(dev, 1000000000);
+        if (r)
+                return r;
+
+        dss_restore_context();
+        return 0;
+}
+
+static const struct dev_pm_ops dss_pm_ops = {
+        .runtime_suspend = dss_runtime_suspend,
+        .runtime_resume = dss_runtime_resume,
+};
+
+static const struct of_device_id dss_of_match[] = {
+        { .compatible = "ti,omap2-dss", },
+        { .compatible = "ti,omap3-dss", },
+        { .compatible = "ti,omap4-dss", },
+        { .compatible = "ti,omap5-dss", },
+        { .compatible = "ti,dra7-dss", },
+        {},
+};
+
+MODULE_DEVICE_TABLE(of, dss_of_match);
+
+static struct platform_driver omap_dsshw_driver = {
+        .probe          = dss_probe,
+        .remove         = dss_remove,
+        .driver         = {
+                .name   = "omapdss_dss",
+                .pm     = &dss_pm_ops,
+                .of_match_table = dss_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init dss_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dsshw_driver);
+}
+
+void dss_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_dsshw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss.h b/drivers/video/fbdev/omap2/omapfb/dss/dss.h
new file mode 100644
index 000000000000..b9066afee301
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss.h
@@ -0,0 +1,468 @@
+/*
+ * linux/drivers/video/omap2/dss/dss.h
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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_H
+#define __OMAP2_DSS_H
+
+#include <linux/interrupt.h>
+
+#ifdef pr_fmt
+#undef pr_fmt
+#endif
+
+#ifdef DSS_SUBSYS_NAME
+#define pr_fmt(fmt) DSS_SUBSYS_NAME ": " fmt
+#else
+#define pr_fmt(fmt) fmt
+#endif
+
+#define DSSDBG(format, ...) \
+        pr_debug(format, ## __VA_ARGS__)
+
+#ifdef DSS_SUBSYS_NAME
+#define DSSERR(format, ...) \
+        printk(KERN_ERR "omapdss " DSS_SUBSYS_NAME " error: " format, \
+        ## __VA_ARGS__)
+#else
+#define DSSERR(format, ...) \
+        printk(KERN_ERR "omapdss error: " format, ## __VA_ARGS__)
+#endif
+
+#ifdef DSS_SUBSYS_NAME
+#define DSSINFO(format, ...) \
+        printk(KERN_INFO "omapdss " DSS_SUBSYS_NAME ": " format, \
+        ## __VA_ARGS__)
+#else
+#define DSSINFO(format, ...) \
+        printk(KERN_INFO "omapdss: " format, ## __VA_ARGS__)
+#endif
+
+#ifdef DSS_SUBSYS_NAME
+#define DSSWARN(format, ...) \
+        printk(KERN_WARNING "omapdss " DSS_SUBSYS_NAME ": " format, \
+        ## __VA_ARGS__)
+#else
+#define DSSWARN(format, ...) \
+        printk(KERN_WARNING "omapdss: " format, ## __VA_ARGS__)
+#endif
+
+/* OMAP TRM gives bitfields as start:end, where start is the higher bit
+   number. For example 7:0 */
+#define FLD_MASK(start, end)    (((1 << ((start) - (end) + 1)) - 1) << (end))
+#define FLD_VAL(val, start, end) (((val) << (end)) & FLD_MASK(start, end))
+#define FLD_GET(val, start, end) (((val) & FLD_MASK(start, end)) >> (end))
+#define FLD_MOD(orig, val, start, end) \
+        (((orig) & ~FLD_MASK(start, end)) | FLD_VAL(val, start, end))
+
+enum dss_io_pad_mode {
+        DSS_IO_PAD_MODE_RESET,
+        DSS_IO_PAD_MODE_RFBI,
+        DSS_IO_PAD_MODE_BYPASS,
+};
+
+enum dss_hdmi_venc_clk_source_select {
+        DSS_VENC_TV_CLK = 0,
+        DSS_HDMI_M_PCLK = 1,
+};
+
+enum dss_dsi_content_type {
+        DSS_DSI_CONTENT_DCS,
+        DSS_DSI_CONTENT_GENERIC,
+};
+
+enum dss_writeback_channel {
+        DSS_WB_LCD1_MGR =       0,
+        DSS_WB_LCD2_MGR =       1,
+        DSS_WB_TV_MGR =         2,
+        DSS_WB_OVL0 =           3,
+        DSS_WB_OVL1 =           4,
+        DSS_WB_OVL2 =           5,
+        DSS_WB_OVL3 =           6,
+        DSS_WB_LCD3_MGR =       7,
+};
+
+enum dss_pll_id {
+        DSS_PLL_DSI1,
+        DSS_PLL_DSI2,
+        DSS_PLL_HDMI,
+        DSS_PLL_VIDEO1,
+        DSS_PLL_VIDEO2,
+};
+
+struct dss_pll;
+
+#define DSS_PLL_MAX_HSDIVS 4
+
+/*
+ * Type-A PLLs: clkout[]/mX[] refer to hsdiv outputs m4, m5, m6, m7.
+ * Type-B PLLs: clkout[0] refers to m2.
+ */
+struct dss_pll_clock_info {
+        /* rates that we get with dividers below */
+        unsigned long fint;
+        unsigned long clkdco;
+        unsigned long clkout[DSS_PLL_MAX_HSDIVS];
+
+        /* dividers */
+        u16 n;
+        u16 m;
+        u32 mf;
+        u16 mX[DSS_PLL_MAX_HSDIVS];
+        u16 sd;
+};
+
+struct dss_pll_ops {
+        int (*enable)(struct dss_pll *pll);
+        void (*disable)(struct dss_pll *pll);
+        int (*set_config)(struct dss_pll *pll,
+                const struct dss_pll_clock_info *cinfo);
+};
+
+struct dss_pll_hw {
+        unsigned n_max;
+        unsigned m_min;
+        unsigned m_max;
+        unsigned mX_max;
+
+        unsigned long fint_min, fint_max;
+        unsigned long clkdco_min, clkdco_low, clkdco_max;
+
+        u8 n_msb, n_lsb;
+        u8 m_msb, m_lsb;
+        u8 mX_msb[DSS_PLL_MAX_HSDIVS], mX_lsb[DSS_PLL_MAX_HSDIVS];
+
+        bool has_stopmode;
+        bool has_freqsel;
+        bool has_selfreqdco;
+        bool has_refsel;
+};
+
+struct dss_pll {
+        const char *name;
+        enum dss_pll_id id;
+
+        struct clk *clkin;
+        struct regulator *regulator;
+
+        void __iomem *base;
+
+        const struct dss_pll_hw *hw;
+
+        const struct dss_pll_ops *ops;
+
+        struct dss_pll_clock_info cinfo;
+};
+
+struct dispc_clock_info {
+        /* rates that we get with dividers below */
+        unsigned long lck;
+        unsigned long pck;
+
+        /* dividers */
+        u16 lck_div;
+        u16 pck_div;
+};
+
+struct dss_lcd_mgr_config {
+        enum dss_io_pad_mode io_pad_mode;
+
+        bool stallmode;
+        bool fifohandcheck;
+
+        struct dispc_clock_info clock_info;
+
+        int video_port_width;
+
+        int lcden_sig_polarity;
+};
+
+struct seq_file;
+struct platform_device;
+
+/* core */
+struct platform_device *dss_get_core_pdev(void);
+int dss_dsi_enable_pads(int dsi_id, unsigned lane_mask);
+void dss_dsi_disable_pads(int dsi_id, unsigned lane_mask);
+int dss_set_min_bus_tput(struct device *dev, unsigned long tput);
+int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *));
+
+/* display */
+int dss_suspend_all_devices(void);
+int dss_resume_all_devices(void);
+void dss_disable_all_devices(void);
+
+int display_init_sysfs(struct platform_device *pdev);
+void display_uninit_sysfs(struct platform_device *pdev);
+
+/* manager */
+int dss_init_overlay_managers(void);
+void dss_uninit_overlay_managers(void);
+int dss_init_overlay_managers_sysfs(struct platform_device *pdev);
+void dss_uninit_overlay_managers_sysfs(struct platform_device *pdev);
+int dss_mgr_simple_check(struct omap_overlay_manager *mgr,
+                const struct omap_overlay_manager_info *info);
+int dss_mgr_check_timings(struct omap_overlay_manager *mgr,
+                const struct omap_video_timings *timings);
+int dss_mgr_check(struct omap_overlay_manager *mgr,
+                struct omap_overlay_manager_info *info,
+                const struct omap_video_timings *mgr_timings,
+                const struct dss_lcd_mgr_config *config,
+                struct omap_overlay_info **overlay_infos);
+
+static inline bool dss_mgr_is_lcd(enum omap_channel id)
+{
+        if (id == OMAP_DSS_CHANNEL_LCD || id == OMAP_DSS_CHANNEL_LCD2 ||
+                        id == OMAP_DSS_CHANNEL_LCD3)
+                return true;
+        else
+                return false;
+}
+
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+                struct platform_device *pdev);
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr);
+
+/* overlay */
+void dss_init_overlays(struct platform_device *pdev);
+void dss_uninit_overlays(struct platform_device *pdev);
+void dss_overlay_setup_dispc_manager(struct omap_overlay_manager *mgr);
+int dss_ovl_simple_check(struct omap_overlay *ovl,
+                const struct omap_overlay_info *info);
+int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
+                const struct omap_video_timings *mgr_timings);
+bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
+                enum omap_color_mode mode);
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+                struct platform_device *pdev);
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl);
+
+/* DSS */
+int dss_init_platform_driver(void) __init;
+void dss_uninit_platform_driver(void);
+
+int dss_runtime_get(void);
+void dss_runtime_put(void);
+
+unsigned long dss_get_dispc_clk_rate(void);
+int dss_dpi_select_source(int port, enum omap_channel channel);
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
+enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void);
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src);
+void dss_dump_clocks(struct seq_file *s);
+
+/* DSS VIDEO PLL */
+struct dss_pll *dss_video_pll_init(struct platform_device *pdev, int id,
+        struct regulator *regulator);
+void dss_video_pll_uninit(struct dss_pll *pll);
+
+/* dss-of */
+struct device_node *dss_of_port_get_parent_device(struct device_node *port);
+u32 dss_of_port_get_port_number(struct device_node *port);
+
+#if defined(CONFIG_FB_OMAP2_DSS_DEBUGFS)
+void dss_debug_dump_clocks(struct seq_file *s);
+#endif
+
+void dss_ctrl_pll_enable(enum dss_pll_id pll_id, bool enable);
+void dss_ctrl_pll_set_control_mux(enum dss_pll_id pll_id,
+        enum omap_channel channel);
+
+void dss_sdi_init(int datapairs);
+int dss_sdi_enable(void);
+void dss_sdi_disable(void);
+
+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);
+
+int dss_set_fck_rate(unsigned long rate);
+
+typedef bool (*dss_div_calc_func)(unsigned long fck, void *data);
+bool dss_div_calc(unsigned long pck, unsigned long fck_min,
+                dss_div_calc_func func, void *data);
+
+/* SDI */
+int sdi_init_platform_driver(void) __init;
+void sdi_uninit_platform_driver(void);
+
+#ifdef CONFIG_FB_OMAP2_DSS_SDI
+int sdi_init_port(struct platform_device *pdev, struct device_node *port);
+void sdi_uninit_port(struct device_node *port);
+#else
+static inline int sdi_init_port(struct platform_device *pdev,
+                struct device_node *port)
+{
+        return 0;
+}
+static inline void sdi_uninit_port(struct device_node *port)
+{
+}
+#endif
+
+/* DSI */
+
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+
+struct dentry;
+struct file_operations;
+
+int dsi_init_platform_driver(void) __init;
+void dsi_uninit_platform_driver(void);
+
+void dsi_dump_clocks(struct seq_file *s);
+
+void dsi_irq_handler(void);
+u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt);
+
+#else
+static inline u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
+{
+        WARN(1, "%s: DSI not compiled in, returning pixel_size as 0\n",
+             __func__);
+        return 0;
+}
+#endif
+
+/* DPI */
+int dpi_init_platform_driver(void) __init;
+void dpi_uninit_platform_driver(void);
+
+#ifdef CONFIG_FB_OMAP2_DSS_DPI
+int dpi_init_port(struct platform_device *pdev, struct device_node *port);
+void dpi_uninit_port(struct device_node *port);
+#else
+static inline int dpi_init_port(struct platform_device *pdev,
+                struct device_node *port)
+{
+        return 0;
+}
+static inline void dpi_uninit_port(struct device_node *port)
+{
+}
+#endif
+
+/* DISPC */
+int dispc_init_platform_driver(void) __init;
+void dispc_uninit_platform_driver(void);
+void dispc_dump_clocks(struct seq_file *s);
+
+void dispc_enable_sidle(void);
+void dispc_disable_sidle(void);
+
+void dispc_lcd_enable_signal(bool enable);
+void dispc_pck_free_enable(bool enable);
+void dispc_enable_fifomerge(bool enable);
+void dispc_enable_gamma_table(bool enable);
+
+typedef bool (*dispc_div_calc_func)(int lckd, int pckd, unsigned long lck,
+                unsigned long pck, void *data);
+bool dispc_div_calc(unsigned long dispc,
+                unsigned long pck_min, unsigned long pck_max,
+                dispc_div_calc_func func, void *data);
+
+bool dispc_mgr_timings_ok(enum omap_channel channel,
+                const struct omap_video_timings *timings);
+int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
+                struct dispc_clock_info *cinfo);
+
+
+void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high);
+void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
+                u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
+                bool manual_update);
+
+void dispc_mgr_set_clock_div(enum omap_channel channel,
+                const struct dispc_clock_info *cinfo);
+int dispc_mgr_get_clock_div(enum omap_channel channel,
+                struct dispc_clock_info *cinfo);
+void dispc_set_tv_pclk(unsigned long pclk);
+
+u32 dispc_wb_get_framedone_irq(void);
+bool dispc_wb_go_busy(void);
+void dispc_wb_go(void);
+void dispc_wb_enable(bool enable);
+bool dispc_wb_is_enabled(void);
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel);
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+                bool mem_to_mem, const struct omap_video_timings *timings);
+
+/* VENC */
+int venc_init_platform_driver(void) __init;
+void venc_uninit_platform_driver(void);
+
+/* HDMI */
+int hdmi4_init_platform_driver(void) __init;
+void hdmi4_uninit_platform_driver(void);
+
+int hdmi5_init_platform_driver(void) __init;
+void hdmi5_uninit_platform_driver(void);
+
+/* RFBI */
+int rfbi_init_platform_driver(void) __init;
+void rfbi_uninit_platform_driver(void);
+
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static inline void dss_collect_irq_stats(u32 irqstatus, unsigned *irq_arr)
+{
+        int b;
+        for (b = 0; b < 32; ++b) {
+                if (irqstatus & (1 << b))
+                        irq_arr[b]++;
+        }
+}
+#endif
+
+/* PLL */
+typedef bool (*dss_pll_calc_func)(int n, int m, unsigned long fint,
+                unsigned long clkdco, void *data);
+typedef bool (*dss_hsdiv_calc_func)(int m_dispc, unsigned long dispc,
+                void *data);
+
+int dss_pll_register(struct dss_pll *pll);
+void dss_pll_unregister(struct dss_pll *pll);
+struct dss_pll *dss_pll_find(const char *name);
+int dss_pll_enable(struct dss_pll *pll);
+void dss_pll_disable(struct dss_pll *pll);
+int dss_pll_set_config(struct dss_pll *pll,
+                const struct dss_pll_clock_info *cinfo);
+
+bool dss_pll_hsdiv_calc(const struct dss_pll *pll, unsigned long clkdco,
+                unsigned long out_min, unsigned long out_max,
+                dss_hsdiv_calc_func func, void *data);
+bool dss_pll_calc(const struct dss_pll *pll, unsigned long clkin,
+                unsigned long pll_min, unsigned long pll_max,
+                dss_pll_calc_func func, void *data);
+int dss_pll_write_config_type_a(struct dss_pll *pll,
+                const struct dss_pll_clock_info *cinfo);
+int dss_pll_write_config_type_b(struct dss_pll *pll,
+                const struct dss_pll_clock_info *cinfo);
+int dss_pll_wait_reset_done(struct dss_pll *pll);
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss_features.c b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.c
new file mode 100644
index 000000000000..c886a2927f73
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.c
@@ -0,0 +1,951 @@
+/*
+ * 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/module.h>
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.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 enum dss_feat_id *features;
+        const int num_features;
+
+        const int num_mgrs;
+        const int num_ovls;
+        const enum omap_display_type *supported_displays;
+        const enum omap_dss_output_id *supported_outputs;
+        const enum omap_color_mode *supported_color_modes;
+        const enum omap_overlay_caps *overlay_caps;
+        const char * const *clksrc_names;
+        const struct dss_param_range *dss_params;
+
+        const enum omap_dss_rotation_type supported_rotation_types;
+
+        const u32 buffer_size_unit;
+        const u32 burst_size_unit;
+};
+
+/* 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 },
+};
+
+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 },
+};
+
+static const struct dss_reg_field am43xx_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 },
+};
+
+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 },
+};
+
+static const struct dss_reg_field omap5_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, 7 },
+};
+
+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 am43xx_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+};
+
+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_display_type omap5_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_HDMI | OMAP_DISPLAY_TYPE_DPI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+        OMAP_DISPLAY_TYPE_DSI,
+};
+
+static const enum omap_dss_output_id omap2_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3430_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_SDI | OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3630_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id am43xx_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+};
+
+static const enum omap_dss_output_id omap4_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DBI | OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_VENC | OMAP_DSS_OUTPUT_HDMI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI2,
+};
+
+static const enum omap_dss_output_id omap5_dss_supported_outputs[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1 | OMAP_DSS_OUTPUT_DSI2,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DSS_OUTPUT_HDMI,
+
+        /* OMAP_DSS_CHANNEL_LCD2 */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI1,
+
+        /* OMAP_DSS_CHANNEL_LCD3 */
+        OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+        OMAP_DSS_OUTPUT_DSI2,
+};
+
+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_COLOR_RGBX16 |
+        OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_XRGB16_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,
+
+        /* OMAP_DSS_VIDEO3 */
+        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_WB */
+        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 enum omap_overlay_caps omap2_dss_overlay_caps[] = {
+        /* OMAP_DSS_GFX */
+        OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO1 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const enum omap_overlay_caps omap3430_dss_overlay_caps[] = {
+        /* OMAP_DSS_GFX */
+        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO1 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const enum omap_overlay_caps omap3630_dss_overlay_caps[] = {
+        /* OMAP_DSS_GFX */
+        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO1 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const enum omap_overlay_caps omap4_dss_overlay_caps[] = {
+        /* OMAP_DSS_GFX */
+        OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
+                OMAP_DSS_OVL_CAP_ZORDER | OMAP_DSS_OVL_CAP_POS |
+                OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO1 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO2 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+        /* OMAP_DSS_VIDEO3 */
+        OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+                OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+                OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+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 char * const omap5_dss_clk_source_names[] = {
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]  = "DPLL_DSI1_A_CLK1",
+        [OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]    = "DPLL_DSI1_A_CLK2",
+        [OMAP_DSS_CLK_SRC_FCK]                  = "DSS_CLK",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC] = "DPLL_DSI1_C_CLK1",
+        [OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]   = "DPLL_DSI1_C_CLK2",
+};
+
+static const struct dss_param_range omap2_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 133000000 },
+        [FEAT_PARAM_DSS_PCD]                    = { 2, 255 },
+        [FEAT_PARAM_DOWNSCALE]                  = { 1, 2 },
+        /*
+         * Assuming the line width buffer to be 768 pixels as OMAP2 DISPC
+         * scaler cannot scale a image with width more than 768.
+         */
+        [FEAT_PARAM_LINEWIDTH]                  = { 1, 768 },
+};
+
+static const struct dss_param_range omap3_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 173000000 },
+        [FEAT_PARAM_DSS_PCD]                    = { 1, 255 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 1, (1 << 13) - 1},
+        [FEAT_PARAM_DSI_FCK]                    = { 0, 173000000 },
+        [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
+        [FEAT_PARAM_LINEWIDTH]                  = { 1, 1024 },
+};
+
+static const struct dss_param_range am43xx_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 200000000 },
+        [FEAT_PARAM_DSS_PCD]                    = { 1, 255 },
+        [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
+        [FEAT_PARAM_LINEWIDTH]                  = { 1, 1024 },
+};
+
+static const struct dss_param_range omap4_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 186000000 },
+        [FEAT_PARAM_DSS_PCD]                    = { 1, 255 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, (1 << 13) - 1 },
+        [FEAT_PARAM_DSI_FCK]                    = { 0, 170000000 },
+        [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
+        [FEAT_PARAM_LINEWIDTH]                  = { 1, 2048 },
+};
+
+static const struct dss_param_range omap5_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 209250000 },
+        [FEAT_PARAM_DSS_PCD]                    = { 1, 255 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, (1 << 13) - 1 },
+        [FEAT_PARAM_DSI_FCK]                    = { 0, 209250000 },
+        [FEAT_PARAM_DOWNSCALE]                  = { 1, 4 },
+        [FEAT_PARAM_LINEWIDTH]                  = { 1, 2048 },
+};
+
+static const enum dss_feat_id omap2_dss_feat_list[] = {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+};
+
+static const enum dss_feat_id omap3430_dss_feat_list[] = {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_LINEBUFFERSPLIT,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+        FEAT_DSI_REVERSE_TXCLKESC,
+        FEAT_VENC_REQUIRES_TV_DAC_CLK,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FIXED_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_OMAP3_DSI_FIFO_BUG,
+        FEAT_DPI_USES_VDDS_DSI,
+};
+
+static const enum dss_feat_id am35xx_dss_feat_list[] = {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_LINEBUFFERSPLIT,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+        FEAT_DSI_REVERSE_TXCLKESC,
+        FEAT_VENC_REQUIRES_TV_DAC_CLK,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FIXED_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_OMAP3_DSI_FIFO_BUG,
+};
+
+static const enum dss_feat_id am43xx_dss_feat_list[] = {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_LINEBUFFERSPLIT,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FIXED_ZORDER,
+        FEAT_FIFO_MERGE,
+};
+
+static const enum dss_feat_id omap3630_dss_feat_list[] = {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_LINEBUFFERSPLIT,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+        FEAT_DSI_PLL_PWR_BUG,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FIXED_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_OMAP3_DSI_FIFO_BUG,
+        FEAT_DPI_USES_VDDS_DSI,
+};
+
+static const enum dss_feat_id omap4430_es1_0_dss_feat_list[] = {
+        FEAT_MGR_LCD2,
+        FEAT_CORE_CLK_DIV,
+        FEAT_LCD_CLK_SRC,
+        FEAT_DSI_DCS_CMD_CONFIG_VC,
+        FEAT_DSI_VC_OCP_WIDTH,
+        FEAT_DSI_GNQ,
+        FEAT_HANDLE_UV_SEPARATE,
+        FEAT_ATTR2,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FREE_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_BURST_2D,
+};
+
+static const enum dss_feat_id omap4430_es2_0_1_2_dss_feat_list[] = {
+        FEAT_MGR_LCD2,
+        FEAT_CORE_CLK_DIV,
+        FEAT_LCD_CLK_SRC,
+        FEAT_DSI_DCS_CMD_CONFIG_VC,
+        FEAT_DSI_VC_OCP_WIDTH,
+        FEAT_DSI_GNQ,
+        FEAT_HDMI_CTS_SWMODE,
+        FEAT_HANDLE_UV_SEPARATE,
+        FEAT_ATTR2,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FREE_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_BURST_2D,
+};
+
+static const enum dss_feat_id omap4_dss_feat_list[] = {
+        FEAT_MGR_LCD2,
+        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_HDMI_AUDIO_USE_MCLK,
+        FEAT_HANDLE_UV_SEPARATE,
+        FEAT_ATTR2,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FREE_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_BURST_2D,
+};
+
+static const enum dss_feat_id omap5_dss_feat_list[] = {
+        FEAT_MGR_LCD2,
+        FEAT_MGR_LCD3,
+        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_HDMI_AUDIO_USE_MCLK,
+        FEAT_HANDLE_UV_SEPARATE,
+        FEAT_ATTR2,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FREE_ZORDER,
+        FEAT_FIFO_MERGE,
+        FEAT_BURST_2D,
+        FEAT_DSI_PHY_DCC,
+        FEAT_MFLAG,
+};
+
+/* 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),
+
+        .features = omap2_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap2_dss_feat_list),
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap2_dss_supported_displays,
+        .supported_outputs = omap2_dss_supported_outputs,
+        .supported_color_modes = omap2_dss_supported_color_modes,
+        .overlay_caps = omap2_dss_overlay_caps,
+        .clksrc_names = omap2_dss_clk_source_names,
+        .dss_params = omap2_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+        .buffer_size_unit = 1,
+        .burst_size_unit = 8,
+};
+
+/* 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),
+
+        .features = omap3430_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap3430_dss_feat_list),
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3430_dss_supported_displays,
+        .supported_outputs = omap3430_dss_supported_outputs,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .overlay_caps = omap3430_dss_overlay_caps,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+        .buffer_size_unit = 1,
+        .burst_size_unit = 8,
+};
+
+/*
+ * AM35xx DSS Features. This is basically OMAP3 DSS Features without the
+ * vdds_dsi regulator.
+ */
+static const struct omap_dss_features am35xx_dss_features = {
+        .reg_fields = omap3_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+        .features = am35xx_dss_feat_list,
+        .num_features = ARRAY_SIZE(am35xx_dss_feat_list),
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3430_dss_supported_displays,
+        .supported_outputs = omap3430_dss_supported_outputs,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .overlay_caps = omap3430_dss_overlay_caps,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+        .buffer_size_unit = 1,
+        .burst_size_unit = 8,
+};
+
+static const struct omap_dss_features am43xx_dss_features = {
+        .reg_fields = am43xx_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(am43xx_dss_reg_fields),
+
+        .features = am43xx_dss_feat_list,
+        .num_features = ARRAY_SIZE(am43xx_dss_feat_list),
+
+        .num_mgrs = 1,
+        .num_ovls = 3,
+        .supported_displays = am43xx_dss_supported_displays,
+        .supported_outputs = am43xx_dss_supported_outputs,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .overlay_caps = omap3430_dss_overlay_caps,
+        .clksrc_names = omap2_dss_clk_source_names,
+        .dss_params = am43xx_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA,
+        .buffer_size_unit = 1,
+        .burst_size_unit = 8,
+};
+
+static const struct omap_dss_features omap3630_dss_features = {
+        .reg_fields = omap3_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+        .features = omap3630_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap3630_dss_feat_list),
+
+        .num_mgrs = 2,
+        .num_ovls = 3,
+        .supported_displays = omap3630_dss_supported_displays,
+        .supported_outputs = omap3630_dss_supported_outputs,
+        .supported_color_modes = omap3_dss_supported_color_modes,
+        .overlay_caps = omap3630_dss_overlay_caps,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+        .buffer_size_unit = 1,
+        .burst_size_unit = 8,
+};
+
+/* 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),
+
+        .features = omap4430_es1_0_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap4430_es1_0_dss_feat_list),
+
+        .num_mgrs = 3,
+        .num_ovls = 4,
+        .supported_displays = omap4_dss_supported_displays,
+        .supported_outputs = omap4_dss_supported_outputs,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .overlay_caps = omap4_dss_overlay_caps,
+        .clksrc_names = omap4_dss_clk_source_names,
+        .dss_params = omap4_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+        .buffer_size_unit = 16,
+        .burst_size_unit = 16,
+};
+
+/* For OMAP4430 ES 2.0, 2.1 and 2.2 revisions */
+static const struct omap_dss_features omap4430_es2_0_1_2_dss_features = {
+        .reg_fields = omap4_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+        .features = omap4430_es2_0_1_2_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap4430_es2_0_1_2_dss_feat_list),
+
+        .num_mgrs = 3,
+        .num_ovls = 4,
+        .supported_displays = omap4_dss_supported_displays,
+        .supported_outputs = omap4_dss_supported_outputs,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .overlay_caps = omap4_dss_overlay_caps,
+        .clksrc_names = omap4_dss_clk_source_names,
+        .dss_params = omap4_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+        .buffer_size_unit = 16,
+        .burst_size_unit = 16,
+};
+
+/* For all the other OMAP4 versions */
+static const struct omap_dss_features omap4_dss_features = {
+        .reg_fields = omap4_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+        .features = omap4_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap4_dss_feat_list),
+
+        .num_mgrs = 3,
+        .num_ovls = 4,
+        .supported_displays = omap4_dss_supported_displays,
+        .supported_outputs = omap4_dss_supported_outputs,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .overlay_caps = omap4_dss_overlay_caps,
+        .clksrc_names = omap4_dss_clk_source_names,
+        .dss_params = omap4_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+        .buffer_size_unit = 16,
+        .burst_size_unit = 16,
+};
+
+/* OMAP5 DSS Features */
+static const struct omap_dss_features omap5_dss_features = {
+        .reg_fields = omap5_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap5_dss_reg_fields),
+
+        .features = omap5_dss_feat_list,
+        .num_features = ARRAY_SIZE(omap5_dss_feat_list),
+
+        .num_mgrs = 4,
+        .num_ovls = 4,
+        .supported_displays = omap5_dss_supported_displays,
+        .supported_outputs = omap5_dss_supported_outputs,
+        .supported_color_modes = omap4_dss_supported_color_modes,
+        .overlay_caps = omap4_dss_overlay_caps,
+        .clksrc_names = omap5_dss_clk_source_names,
+        .dss_params = omap5_dss_param_range,
+        .supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+        .buffer_size_unit = 16,
+        .burst_size_unit = 16,
+};
+
+/* Functions returning values related to a DSS feature */
+int dss_feat_get_num_mgrs(void)
+{
+        return omap_current_dss_features->num_mgrs;
+}
+EXPORT_SYMBOL(dss_feat_get_num_mgrs);
+
+int dss_feat_get_num_ovls(void)
+{
+        return omap_current_dss_features->num_ovls;
+}
+EXPORT_SYMBOL(dss_feat_get_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_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel)
+{
+        return omap_current_dss_features->supported_outputs[channel];
+}
+
+enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
+{
+        return omap_current_dss_features->supported_color_modes[plane];
+}
+EXPORT_SYMBOL(dss_feat_get_supported_color_modes);
+
+enum omap_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane)
+{
+        return omap_current_dss_features->overlay_caps[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];
+}
+
+u32 dss_feat_get_buffer_size_unit(void)
+{
+        return omap_current_dss_features->buffer_size_unit;
+}
+
+u32 dss_feat_get_burst_size_unit(void)
+{
+        return omap_current_dss_features->burst_size_unit;
+}
+
+/* DSS has_feature check */
+bool dss_has_feature(enum dss_feat_id id)
+{
+        int i;
+        const enum dss_feat_id *features = omap_current_dss_features->features;
+        const int num_features = omap_current_dss_features->num_features;
+
+        for (i = 0; i < num_features; i++) {
+                if (features[i] == id)
+                        return true;
+        }
+
+        return false;
+}
+
+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;
+}
+
+bool dss_feat_rotation_type_supported(enum omap_dss_rotation_type rot_type)
+{
+        return omap_current_dss_features->supported_rotation_types & rot_type;
+}
+
+void dss_features_init(enum omapdss_version version)
+{
+        switch (version) {
+        case OMAPDSS_VER_OMAP24xx:
+                omap_current_dss_features = &omap2_dss_features;
+                break;
+
+        case OMAPDSS_VER_OMAP34xx_ES1:
+        case OMAPDSS_VER_OMAP34xx_ES3:
+                omap_current_dss_features = &omap3430_dss_features;
+                break;
+
+        case OMAPDSS_VER_OMAP3630:
+                omap_current_dss_features = &omap3630_dss_features;
+                break;
+
+        case OMAPDSS_VER_OMAP4430_ES1:
+                omap_current_dss_features = &omap4430_es1_0_dss_features;
+                break;
+
+        case OMAPDSS_VER_OMAP4430_ES2:
+                omap_current_dss_features = &omap4430_es2_0_1_2_dss_features;
+                break;
+
+        case OMAPDSS_VER_OMAP4:
+                omap_current_dss_features = &omap4_dss_features;
+                break;
+
+        case OMAPDSS_VER_OMAP5:
+        case OMAPDSS_VER_DRA7xx:
+                omap_current_dss_features = &omap5_dss_features;
+                break;
+
+        case OMAPDSS_VER_AM35xx:
+                omap_current_dss_features = &am35xx_dss_features;
+                break;
+
+        case OMAPDSS_VER_AM43xx:
+                omap_current_dss_features = &am43xx_dss_features;
+                break;
+
+        default:
+                DSSWARN("Unsupported OMAP version");
+                break;
+        }
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss_features.h b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.h
new file mode 100644
index 000000000000..3d67d39f192f
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.h
@@ -0,0 +1,108 @@
+/*
+ * 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        4
+#define MAX_DSS_OVERLAYS        4
+#define MAX_DSS_LCD_MANAGERS    3
+#define MAX_NUM_DSI             2
+
+/* DSS has feature id */
+enum dss_feat_id {
+        FEAT_LCDENABLEPOL,
+        FEAT_LCDENABLESIGNAL,
+        FEAT_PCKFREEENABLE,
+        FEAT_FUNCGATED,
+        FEAT_MGR_LCD2,
+        FEAT_MGR_LCD3,
+        FEAT_LINEBUFFERSPLIT,
+        FEAT_ROWREPEATENABLE,
+        FEAT_RESIZECONF,
+        /* Independent core clk divider */
+        FEAT_CORE_CLK_DIV,
+        FEAT_LCD_CLK_SRC,
+        /* DSI-PLL power command 0x3 is not working */
+        FEAT_DSI_PLL_PWR_BUG,
+        FEAT_DSI_DCS_CMD_CONFIG_VC,
+        FEAT_DSI_VC_OCP_WIDTH,
+        FEAT_DSI_REVERSE_TXCLKESC,
+        FEAT_DSI_GNQ,
+        FEAT_DPI_USES_VDDS_DSI,
+        FEAT_HDMI_CTS_SWMODE,
+        FEAT_HDMI_AUDIO_USE_MCLK,
+        FEAT_HANDLE_UV_SEPARATE,
+        FEAT_ATTR2,
+        FEAT_VENC_REQUIRES_TV_DAC_CLK,
+        FEAT_CPR,
+        FEAT_PRELOAD,
+        FEAT_FIR_COEF_V,
+        FEAT_ALPHA_FIXED_ZORDER,
+        FEAT_ALPHA_FREE_ZORDER,
+        FEAT_FIFO_MERGE,
+        /* An unknown HW bug causing the normal FIFO thresholds not to work */
+        FEAT_OMAP3_DSI_FIFO_BUG,
+        FEAT_BURST_2D,
+        FEAT_DSI_PHY_DCC,
+        FEAT_MFLAG,
+};
+
+/* 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,
+};
+
+enum dss_range_param {
+        FEAT_PARAM_DSS_FCK,
+        FEAT_PARAM_DSS_PCD,
+        FEAT_PARAM_DSIPLL_LPDIV,
+        FEAT_PARAM_DSI_FCK,
+        FEAT_PARAM_DOWNSCALE,
+        FEAT_PARAM_LINEWIDTH,
+};
+
+/* DSS Feature Functions */
+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_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane);
+bool dss_feat_color_mode_supported(enum omap_plane plane,
+                enum omap_color_mode color_mode);
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id);
+
+u32 dss_feat_get_buffer_size_unit(void);        /* in bytes */
+u32 dss_feat_get_burst_size_unit(void);         /* in bytes */
+
+bool dss_feat_rotation_type_supported(enum omap_dss_rotation_type rot_type);
+
+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(enum omapdss_version version);
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi.h b/drivers/video/fbdev/omap2/omapfb/dss/hdmi.h
new file mode 100644
index 000000000000..53616b02b613
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi.h
@@ -0,0 +1,370 @@
+/*
+ * HDMI driver definition for TI OMAP4 Processor.
+ *
+ * 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 _HDMI_H
+#define _HDMI_H
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/hdmi.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+/* HDMI Wrapper */
+
+#define HDMI_WP_REVISION                        0x0
+#define HDMI_WP_SYSCONFIG                       0x10
+#define HDMI_WP_IRQSTATUS_RAW                   0x24
+#define HDMI_WP_IRQSTATUS                       0x28
+#define HDMI_WP_IRQENABLE_SET                   0x2C
+#define HDMI_WP_IRQENABLE_CLR                   0x30
+#define HDMI_WP_IRQWAKEEN                       0x34
+#define HDMI_WP_PWR_CTRL                        0x40
+#define HDMI_WP_DEBOUNCE                        0x44
+#define HDMI_WP_VIDEO_CFG                       0x50
+#define HDMI_WP_VIDEO_SIZE                      0x60
+#define HDMI_WP_VIDEO_TIMING_H                  0x68
+#define HDMI_WP_VIDEO_TIMING_V                  0x6C
+#define HDMI_WP_CLK                             0x70
+#define HDMI_WP_AUDIO_CFG                       0x80
+#define HDMI_WP_AUDIO_CFG2                      0x84
+#define HDMI_WP_AUDIO_CTRL                      0x88
+#define HDMI_WP_AUDIO_DATA                      0x8C
+
+/* HDMI WP IRQ flags */
+#define HDMI_IRQ_CORE                           (1 << 0)
+#define HDMI_IRQ_OCP_TIMEOUT                    (1 << 4)
+#define HDMI_IRQ_AUDIO_FIFO_UNDERFLOW           (1 << 8)
+#define HDMI_IRQ_AUDIO_FIFO_OVERFLOW            (1 << 9)
+#define HDMI_IRQ_AUDIO_FIFO_SAMPLE_REQ          (1 << 10)
+#define HDMI_IRQ_VIDEO_VSYNC                    (1 << 16)
+#define HDMI_IRQ_VIDEO_FRAME_DONE               (1 << 17)
+#define HDMI_IRQ_PHY_LINE5V_ASSERT              (1 << 24)
+#define HDMI_IRQ_LINK_CONNECT                   (1 << 25)
+#define HDMI_IRQ_LINK_DISCONNECT                (1 << 26)
+#define HDMI_IRQ_PLL_LOCK                       (1 << 29)
+#define HDMI_IRQ_PLL_UNLOCK                     (1 << 30)
+#define HDMI_IRQ_PLL_RECAL                      (1 << 31)
+
+/* HDMI PLL */
+
+#define PLLCTRL_PLL_CONTROL                     0x0
+#define PLLCTRL_PLL_STATUS                      0x4
+#define PLLCTRL_PLL_GO                          0x8
+#define PLLCTRL_CFG1                            0xC
+#define PLLCTRL_CFG2                            0x10
+#define PLLCTRL_CFG3                            0x14
+#define PLLCTRL_SSC_CFG1                        0x18
+#define PLLCTRL_SSC_CFG2                        0x1C
+#define PLLCTRL_CFG4                            0x20
+
+/* HDMI PHY */
+
+#define HDMI_TXPHY_TX_CTRL                      0x0
+#define HDMI_TXPHY_DIGITAL_CTRL                 0x4
+#define HDMI_TXPHY_POWER_CTRL                   0x8
+#define HDMI_TXPHY_PAD_CFG_CTRL                 0xC
+#define HDMI_TXPHY_BIST_CONTROL                 0x1C
+
+enum hdmi_pll_pwr {
+        HDMI_PLLPWRCMD_ALLOFF = 0,
+        HDMI_PLLPWRCMD_PLLONLY = 1,
+        HDMI_PLLPWRCMD_BOTHON_ALLCLKS = 2,
+        HDMI_PLLPWRCMD_BOTHON_NOPHYCLK = 3
+};
+
+enum hdmi_phy_pwr {
+        HDMI_PHYPWRCMD_OFF = 0,
+        HDMI_PHYPWRCMD_LDOON = 1,
+        HDMI_PHYPWRCMD_TXON = 2
+};
+
+enum hdmi_core_hdmi_dvi {
+        HDMI_DVI = 0,
+        HDMI_HDMI = 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_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_omap {
+        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_core_audio_layout {
+        HDMI_AUDIO_LAYOUT_2CH = 0,
+        HDMI_AUDIO_LAYOUT_8CH = 1,
+        HDMI_AUDIO_LAYOUT_6CH = 2
+};
+
+enum hdmi_core_cts_mode {
+        HDMI_AUDIO_CTS_MODE_HW = 0,
+        HDMI_AUDIO_CTS_MODE_SW = 1
+};
+
+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_video_format {
+        enum hdmi_packing_mode  packing_mode;
+        u32                     y_res;  /* Line per panel */
+        u32                     x_res;  /* pixel per line */
+};
+
+struct hdmi_config {
+        struct omap_video_timings timings;
+        struct hdmi_avi_infoframe infoframe;
+        enum hdmi_core_hdmi_dvi hdmi_dvi_mode;
+};
+
+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_omap        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 in_length_bits;
+        u8 justification;
+        u8 sck_edge_mode;
+        u8 vbit;
+        u8 direction;
+        u8 shift;
+        u8 active_sds;
+};
+
+struct hdmi_core_audio_config {
+        struct hdmi_core_audio_i2s_config       i2s_cfg;
+        struct snd_aes_iec958                   *iec60958_cfg;
+        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;
+};
+
+struct hdmi_wp_data {
+        void __iomem *base;
+        phys_addr_t phys_base;
+};
+
+struct hdmi_pll_data {
+        struct dss_pll pll;
+
+        void __iomem *base;
+
+        struct hdmi_wp_data *wp;
+};
+
+struct hdmi_phy_data {
+        void __iomem *base;
+
+        u8 lane_function[4];
+        u8 lane_polarity[4];
+};
+
+struct hdmi_core_data {
+        void __iomem *base;
+};
+
+static inline void hdmi_write_reg(void __iomem *base_addr, const u32 idx,
+                u32 val)
+{
+        __raw_writel(val, base_addr + idx);
+}
+
+static inline u32 hdmi_read_reg(void __iomem *base_addr, const u32 idx)
+{
+        return __raw_readl(base_addr + idx);
+}
+
+#define REG_FLD_MOD(base, idx, val, start, end) \
+        hdmi_write_reg(base, idx, FLD_MOD(hdmi_read_reg(base, idx),\
+                                                        val, start, end))
+#define REG_GET(base, idx, start, end) \
+        FLD_GET(hdmi_read_reg(base, idx), start, end)
+
+static inline int hdmi_wait_for_bit_change(void __iomem *base_addr,
+                const u32 idx, int b2, int b1, u32 val)
+{
+        u32 t = 0, v;
+        while (val != (v = REG_GET(base_addr, idx, b2, b1))) {
+                if (t++ > 10000)
+                        return v;
+                udelay(1);
+        }
+        return v;
+}
+
+/* HDMI wrapper funcs */
+int hdmi_wp_video_start(struct hdmi_wp_data *wp);
+void hdmi_wp_video_stop(struct hdmi_wp_data *wp);
+void hdmi_wp_dump(struct hdmi_wp_data *wp, struct seq_file *s);
+u32 hdmi_wp_get_irqstatus(struct hdmi_wp_data *wp);
+void hdmi_wp_set_irqstatus(struct hdmi_wp_data *wp, u32 irqstatus);
+void hdmi_wp_set_irqenable(struct hdmi_wp_data *wp, u32 mask);
+void hdmi_wp_clear_irqenable(struct hdmi_wp_data *wp, u32 mask);
+int hdmi_wp_set_phy_pwr(struct hdmi_wp_data *wp, enum hdmi_phy_pwr val);
+int hdmi_wp_set_pll_pwr(struct hdmi_wp_data *wp, enum hdmi_pll_pwr val);
+void hdmi_wp_video_config_format(struct hdmi_wp_data *wp,
+                struct hdmi_video_format *video_fmt);
+void hdmi_wp_video_config_interface(struct hdmi_wp_data *wp,
+                struct omap_video_timings *timings);
+void hdmi_wp_video_config_timing(struct hdmi_wp_data *wp,
+                struct omap_video_timings *timings);
+void hdmi_wp_init_vid_fmt_timings(struct hdmi_video_format *video_fmt,
+                struct omap_video_timings *timings, struct hdmi_config *param);
+int hdmi_wp_init(struct platform_device *pdev, struct hdmi_wp_data *wp);
+phys_addr_t hdmi_wp_get_audio_dma_addr(struct hdmi_wp_data *wp);
+
+/* HDMI PLL funcs */
+void hdmi_pll_dump(struct hdmi_pll_data *pll, struct seq_file *s);
+void hdmi_pll_compute(struct hdmi_pll_data *pll,
+        unsigned long target_tmds, struct dss_pll_clock_info *pi);
+int hdmi_pll_init(struct platform_device *pdev, struct hdmi_pll_data *pll,
+        struct hdmi_wp_data *wp);
+void hdmi_pll_uninit(struct hdmi_pll_data *hpll);
+
+/* HDMI PHY funcs */
+int hdmi_phy_configure(struct hdmi_phy_data *phy, unsigned long hfbitclk,
+        unsigned long lfbitclk);
+void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s);
+int hdmi_phy_init(struct platform_device *pdev, struct hdmi_phy_data *phy);
+int hdmi_phy_parse_lanes(struct hdmi_phy_data *phy, const u32 *lanes);
+
+/* HDMI common funcs */
+int hdmi_parse_lanes_of(struct platform_device *pdev, struct device_node *ep,
+        struct hdmi_phy_data *phy);
+
+/* Audio funcs */
+int hdmi_compute_acr(u32 pclk, u32 sample_freq, u32 *n, u32 *cts);
+int hdmi_wp_audio_enable(struct hdmi_wp_data *wp, bool enable);
+int hdmi_wp_audio_core_req_enable(struct hdmi_wp_data *wp, bool enable);
+void hdmi_wp_audio_config_format(struct hdmi_wp_data *wp,
+                struct hdmi_audio_format *aud_fmt);
+void hdmi_wp_audio_config_dma(struct hdmi_wp_data *wp,
+                struct hdmi_audio_dma *aud_dma);
+static inline bool hdmi_mode_has_audio(struct hdmi_config *cfg)
+{
+        return cfg->hdmi_dvi_mode == HDMI_HDMI ? true : false;
+}
+
+/* HDMI DRV data */
+struct omap_hdmi {
+        struct mutex lock;
+        struct platform_device *pdev;
+
+        struct hdmi_wp_data     wp;
+        struct hdmi_pll_data    pll;
+        struct hdmi_phy_data    phy;
+        struct hdmi_core_data   core;
+
+        struct hdmi_config cfg;
+
+        struct regulator *vdda_reg;
+
+        bool core_enabled;
+
+        struct omap_dss_device output;
+
+        struct platform_device *audio_pdev;
+        void (*audio_abort_cb)(struct device *dev);
+        int wp_idlemode;
+
+        bool audio_configured;
+        struct omap_dss_audio audio_config;
+
+        /* This lock should be taken when booleans bellow are touched. */
+        spinlock_t audio_playing_lock;
+        bool audio_playing;
+        bool display_enabled;
+};
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi4.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4.c
new file mode 100644
index 000000000000..7103c659a534
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4.c
@@ -0,0 +1,839 @@
+/*
+ * HDMI interface DSS driver for TI's OMAP4 family of SoCs.
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors: Yong Zhi
+ *      Mythri pk <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/component.h>
+#include <video/omapdss.h>
+#include <sound/omap-hdmi-audio.h>
+
+#include "hdmi4_core.h"
+#include "dss.h"
+#include "dss_features.h"
+#include "hdmi.h"
+
+static struct omap_hdmi hdmi;
+
+static int hdmi_runtime_get(void)
+{
+        int r;
+
+        DSSDBG("hdmi_runtime_get\n");
+
+        r = pm_runtime_get_sync(&hdmi.pdev->dev);
+        WARN_ON(r < 0);
+        if (r < 0)
+                return r;
+
+        return 0;
+}
+
+static void hdmi_runtime_put(void)
+{
+        int r;
+
+        DSSDBG("hdmi_runtime_put\n");
+
+        r = pm_runtime_put_sync(&hdmi.pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *data)
+{
+        struct hdmi_wp_data *wp = data;
+        u32 irqstatus;
+
+        irqstatus = hdmi_wp_get_irqstatus(wp);
+        hdmi_wp_set_irqstatus(wp, irqstatus);
+
+        if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
+                        irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+                /*
+                 * If we get both connect and disconnect interrupts at the same
+                 * time, turn off the PHY, clear interrupts, and restart, which
+                 * raises connect interrupt if a cable is connected, or nothing
+                 * if cable is not connected.
+                 */
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
+
+                hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
+                                HDMI_IRQ_LINK_DISCONNECT);
+
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+        } else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
+        } else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+        }
+
+        return IRQ_HANDLED;
+}
+
+static int hdmi_init_regulator(void)
+{
+        int r;
+        struct regulator *reg;
+
+        if (hdmi.vdda_reg != NULL)
+                return 0;
+
+        reg = devm_regulator_get(&hdmi.pdev->dev, "vdda");
+
+        if (IS_ERR(reg)) {
+                if (PTR_ERR(reg) != -EPROBE_DEFER)
+                        DSSERR("can't get VDDA regulator\n");
+                return PTR_ERR(reg);
+        }
+
+        if (regulator_can_change_voltage(reg)) {
+                r = regulator_set_voltage(reg, 1800000, 1800000);
+                if (r) {
+                        devm_regulator_put(reg);
+                        DSSWARN("can't set the regulator voltage\n");
+                        return r;
+                }
+        }
+
+        hdmi.vdda_reg = reg;
+
+        return 0;
+}
+
+static int hdmi_power_on_core(struct omap_dss_device *dssdev)
+{
+        int r;
+
+        r = regulator_enable(hdmi.vdda_reg);
+        if (r)
+                return r;
+
+        r = hdmi_runtime_get();
+        if (r)
+                goto err_runtime_get;
+
+        /* Make selection of HDMI in DSS */
+        dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+        hdmi.core_enabled = true;
+
+        return 0;
+
+err_runtime_get:
+        regulator_disable(hdmi.vdda_reg);
+
+        return r;
+}
+
+static void hdmi_power_off_core(struct omap_dss_device *dssdev)
+{
+        hdmi.core_enabled = false;
+
+        hdmi_runtime_put();
+        regulator_disable(hdmi.vdda_reg);
+}
+
+static int hdmi_power_on_full(struct omap_dss_device *dssdev)
+{
+        int r;
+        struct omap_video_timings *p;
+        struct omap_overlay_manager *mgr = hdmi.output.manager;
+        struct hdmi_wp_data *wp = &hdmi.wp;
+        struct dss_pll_clock_info hdmi_cinfo = { 0 };
+
+        r = hdmi_power_on_core(dssdev);
+        if (r)
+                return r;
+
+        /* disable and clear irqs */
+        hdmi_wp_clear_irqenable(wp, 0xffffffff);
+        hdmi_wp_set_irqstatus(wp, 0xffffffff);
+
+        p = &hdmi.cfg.timings;
+
+        DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
+
+        hdmi_pll_compute(&hdmi.pll, p->pixelclock, &hdmi_cinfo);
+
+        r = dss_pll_enable(&hdmi.pll.pll);
+        if (r) {
+                DSSERR("Failed to enable PLL\n");
+                goto err_pll_enable;
+        }
+
+        r = dss_pll_set_config(&hdmi.pll.pll, &hdmi_cinfo);
+        if (r) {
+                DSSERR("Failed to configure PLL\n");
+                goto err_pll_cfg;
+        }
+
+        r = hdmi_phy_configure(&hdmi.phy, hdmi_cinfo.clkdco,
+                hdmi_cinfo.clkout[0]);
+        if (r) {
+                DSSDBG("Failed to configure PHY\n");
+                goto err_phy_cfg;
+        }
+
+        r = hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+        if (r)
+                goto err_phy_pwr;
+
+        hdmi4_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
+
+        /* bypass TV gamma table */
+        dispc_enable_gamma_table(0);
+
+        /* tv size */
+        dss_mgr_set_timings(mgr, p);
+
+        r = hdmi_wp_video_start(&hdmi.wp);
+        if (r)
+                goto err_vid_enable;
+
+        r = dss_mgr_enable(mgr);
+        if (r)
+                goto err_mgr_enable;
+
+        hdmi_wp_set_irqenable(wp,
+                HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+
+        return 0;
+
+err_mgr_enable:
+        hdmi_wp_video_stop(&hdmi.wp);
+err_vid_enable:
+        hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+err_phy_pwr:
+err_phy_cfg:
+err_pll_cfg:
+        dss_pll_disable(&hdmi.pll.pll);
+err_pll_enable:
+        hdmi_power_off_core(dssdev);
+        return -EIO;
+}
+
+static void hdmi_power_off_full(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = hdmi.output.manager;
+
+        hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+
+        dss_mgr_disable(mgr);
+
+        hdmi_wp_video_stop(&hdmi.wp);
+
+        hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+
+        dss_pll_disable(&hdmi.pll.pll);
+
+        hdmi_power_off_core(dssdev);
+}
+
+static int hdmi_display_check_timing(struct omap_dss_device *dssdev,
+                                        struct omap_video_timings *timings)
+{
+        struct omap_dss_device *out = &hdmi.output;
+
+        if (!dispc_mgr_timings_ok(out->dispc_channel, timings))
+                return -EINVAL;
+
+        return 0;
+}
+
+static void hdmi_display_set_timing(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        mutex_lock(&hdmi.lock);
+
+        hdmi.cfg.timings = *timings;
+
+        dispc_set_tv_pclk(timings->pixelclock);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+static void hdmi_display_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        *timings = hdmi.cfg.timings;
+}
+
+static void hdmi_dump_regs(struct seq_file *s)
+{
+        mutex_lock(&hdmi.lock);
+
+        if (hdmi_runtime_get()) {
+                mutex_unlock(&hdmi.lock);
+                return;
+        }
+
+        hdmi_wp_dump(&hdmi.wp, s);
+        hdmi_pll_dump(&hdmi.pll, s);
+        hdmi_phy_dump(&hdmi.phy, s);
+        hdmi4_core_dump(&hdmi.core, s);
+
+        hdmi_runtime_put();
+        mutex_unlock(&hdmi.lock);
+}
+
+static int read_edid(u8 *buf, int len)
+{
+        int r;
+
+        mutex_lock(&hdmi.lock);
+
+        r = hdmi_runtime_get();
+        BUG_ON(r);
+
+        r = hdmi4_read_edid(&hdmi.core,  buf, len);
+
+        hdmi_runtime_put();
+        mutex_unlock(&hdmi.lock);
+
+        return r;
+}
+
+static void hdmi_start_audio_stream(struct omap_hdmi *hd)
+{
+        hdmi_wp_audio_enable(&hd->wp, true);
+        hdmi4_audio_start(&hd->core, &hd->wp);
+}
+
+static void hdmi_stop_audio_stream(struct omap_hdmi *hd)
+{
+        hdmi4_audio_stop(&hd->core, &hd->wp);
+        hdmi_wp_audio_enable(&hd->wp, false);
+}
+
+static int hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out = &hdmi.output;
+        unsigned long flags;
+        int r = 0;
+
+        DSSDBG("ENTER hdmi_display_enable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        if (out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                r = -ENODEV;
+                goto err0;
+        }
+
+        r = hdmi_power_on_full(dssdev);
+        if (r) {
+                DSSERR("failed to power on device\n");
+                goto err0;
+        }
+
+        if (hdmi.audio_configured) {
+                r = hdmi4_audio_config(&hdmi.core, &hdmi.wp, &hdmi.audio_config,
+                                       hdmi.cfg.timings.pixelclock);
+                if (r) {
+                        DSSERR("Error restoring audio configuration: %d", r);
+                        hdmi.audio_abort_cb(&hdmi.pdev->dev);
+                        hdmi.audio_configured = false;
+                }
+        }
+
+        spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+        if (hdmi.audio_configured && hdmi.audio_playing)
+                hdmi_start_audio_stream(&hdmi);
+        hdmi.display_enabled = true;
+        spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+        mutex_unlock(&hdmi.lock);
+        return 0;
+
+err0:
+        mutex_unlock(&hdmi.lock);
+        return r;
+}
+
+static void hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+        unsigned long flags;
+
+        DSSDBG("Enter hdmi_display_disable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+        hdmi_stop_audio_stream(&hdmi);
+        hdmi.display_enabled = false;
+        spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+        hdmi_power_off_full(dssdev);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_core_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        DSSDBG("ENTER omapdss_hdmi_core_enable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        r = hdmi_power_on_core(dssdev);
+        if (r) {
+                DSSERR("failed to power on device\n");
+                goto err0;
+        }
+
+        mutex_unlock(&hdmi.lock);
+        return 0;
+
+err0:
+        mutex_unlock(&hdmi.lock);
+        return r;
+}
+
+static void hdmi_core_disable(struct omap_dss_device *dssdev)
+{
+        DSSDBG("Enter omapdss_hdmi_core_disable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        hdmi_power_off_core(dssdev);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct omap_overlay_manager *mgr;
+        int r;
+
+        r = hdmi_init_regulator();
+        if (r)
+                return r;
+
+        mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+        if (!mgr)
+                return -ENODEV;
+
+        r = dss_mgr_connect(mgr, dssdev);
+        if (r)
+                return r;
+
+        r = omapdss_output_set_device(dssdev, dst);
+        if (r) {
+                DSSERR("failed to connect output to new device: %s\n",
+                                dst->name);
+                dss_mgr_disconnect(mgr, dssdev);
+                return r;
+        }
+
+        return 0;
+}
+
+static void hdmi_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        omapdss_output_unset_device(dssdev);
+
+        if (dssdev->manager)
+                dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static int hdmi_read_edid(struct omap_dss_device *dssdev,
+                u8 *edid, int len)
+{
+        bool need_enable;
+        int r;
+
+        need_enable = hdmi.core_enabled == false;
+
+        if (need_enable) {
+                r = hdmi_core_enable(dssdev);
+                if (r)
+                        return r;
+        }
+
+        r = read_edid(edid, len);
+
+        if (need_enable)
+                hdmi_core_disable(dssdev);
+
+        return r;
+}
+
+static int hdmi_set_infoframe(struct omap_dss_device *dssdev,
+                const struct hdmi_avi_infoframe *avi)
+{
+        hdmi.cfg.infoframe = *avi;
+        return 0;
+}
+
+static int hdmi_set_hdmi_mode(struct omap_dss_device *dssdev,
+                bool hdmi_mode)
+{
+        hdmi.cfg.hdmi_dvi_mode = hdmi_mode ? HDMI_HDMI : HDMI_DVI;
+        return 0;
+}
+
+static const struct omapdss_hdmi_ops hdmi_ops = {
+        .connect                = hdmi_connect,
+        .disconnect             = hdmi_disconnect,
+
+        .enable                 = hdmi_display_enable,
+        .disable                = hdmi_display_disable,
+
+        .check_timings          = hdmi_display_check_timing,
+        .set_timings            = hdmi_display_set_timing,
+        .get_timings            = hdmi_display_get_timings,
+
+        .read_edid              = hdmi_read_edid,
+        .set_infoframe          = hdmi_set_infoframe,
+        .set_hdmi_mode          = hdmi_set_hdmi_mode,
+};
+
+static void hdmi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &hdmi.output;
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_HDMI;
+        out->output_type = OMAP_DISPLAY_TYPE_HDMI;
+        out->name = "hdmi.0";
+        out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+        out->ops.hdmi = &hdmi_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void hdmi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &hdmi.output;
+
+        omapdss_unregister_output(out);
+}
+
+static int hdmi_probe_of(struct platform_device *pdev)
+{
+        struct device_node *node = pdev->dev.of_node;
+        struct device_node *ep;
+        int r;
+
+        ep = omapdss_of_get_first_endpoint(node);
+        if (!ep)
+                return 0;
+
+        r = hdmi_parse_lanes_of(pdev, ep, &hdmi.phy);
+        if (r)
+                goto err;
+
+        of_node_put(ep);
+        return 0;
+
+err:
+        of_node_put(ep);
+        return r;
+}
+
+/* Audio callbacks */
+static int hdmi_audio_startup(struct device *dev,
+                              void (*abort_cb)(struct device *dev))
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        int ret = 0;
+
+        mutex_lock(&hd->lock);
+
+        if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+                ret = -EPERM;
+                goto out;
+        }
+
+        hd->audio_abort_cb = abort_cb;
+
+out:
+        mutex_unlock(&hd->lock);
+
+        return ret;
+}
+
+static int hdmi_audio_shutdown(struct device *dev)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+
+        mutex_lock(&hd->lock);
+        hd->audio_abort_cb = NULL;
+        hd->audio_configured = false;
+        hd->audio_playing = false;
+        mutex_unlock(&hd->lock);
+
+        return 0;
+}
+
+static int hdmi_audio_start(struct device *dev)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        unsigned long flags;
+
+        WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+        spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+        if (hd->display_enabled)
+                hdmi_start_audio_stream(hd);
+        hd->audio_playing = true;
+
+        spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+        return 0;
+}
+
+static void hdmi_audio_stop(struct device *dev)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        unsigned long flags;
+
+        WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+        spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+        if (hd->display_enabled)
+                hdmi_stop_audio_stream(hd);
+        hd->audio_playing = false;
+
+        spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+}
+
+static int hdmi_audio_config(struct device *dev,
+                             struct omap_dss_audio *dss_audio)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        int ret;
+
+        mutex_lock(&hd->lock);
+
+        if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+                ret = -EPERM;
+                goto out;
+        }
+
+        ret = hdmi4_audio_config(&hd->core, &hd->wp, dss_audio,
+                                 hd->cfg.timings.pixelclock);
+        if (!ret) {
+                hd->audio_configured = true;
+                hd->audio_config = *dss_audio;
+        }
+out:
+        mutex_unlock(&hd->lock);
+
+        return ret;
+}
+
+static const struct omap_hdmi_audio_ops hdmi_audio_ops = {
+        .audio_startup = hdmi_audio_startup,
+        .audio_shutdown = hdmi_audio_shutdown,
+        .audio_start = hdmi_audio_start,
+        .audio_stop = hdmi_audio_stop,
+        .audio_config = hdmi_audio_config,
+};
+
+static int hdmi_audio_register(struct device *dev)
+{
+        struct omap_hdmi_audio_pdata pdata = {
+                .dev = dev,
+                .dss_version = omapdss_get_version(),
+                .audio_dma_addr = hdmi_wp_get_audio_dma_addr(&hdmi.wp),
+                .ops = &hdmi_audio_ops,
+        };
+
+        hdmi.audio_pdev = platform_device_register_data(
+                dev, "omap-hdmi-audio", PLATFORM_DEVID_AUTO,
+                &pdata, sizeof(pdata));
+
+        if (IS_ERR(hdmi.audio_pdev))
+                return PTR_ERR(hdmi.audio_pdev);
+
+        return 0;
+}
+
+/* HDMI HW IP initialisation */
+static int hdmi4_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        int r;
+        int irq;
+
+        hdmi.pdev = pdev;
+        dev_set_drvdata(&pdev->dev, &hdmi);
+
+        mutex_init(&hdmi.lock);
+        spin_lock_init(&hdmi.audio_playing_lock);
+
+        if (pdev->dev.of_node) {
+                r = hdmi_probe_of(pdev);
+                if (r)
+                        return r;
+        }
+
+        r = hdmi_wp_init(pdev, &hdmi.wp);
+        if (r)
+                return r;
+
+        r = hdmi_pll_init(pdev, &hdmi.pll, &hdmi.wp);
+        if (r)
+                return r;
+
+        r = hdmi_phy_init(pdev, &hdmi.phy);
+        if (r)
+                goto err;
+
+        r = hdmi4_core_init(pdev, &hdmi.core);
+        if (r)
+                goto err;
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                r = -ENODEV;
+                goto err;
+        }
+
+        r = devm_request_threaded_irq(&pdev->dev, irq,
+                        NULL, hdmi_irq_handler,
+                        IRQF_ONESHOT, "OMAP HDMI", &hdmi.wp);
+        if (r) {
+                DSSERR("HDMI IRQ request failed\n");
+                goto err;
+        }
+
+        pm_runtime_enable(&pdev->dev);
+
+        hdmi_init_output(pdev);
+
+        r = hdmi_audio_register(&pdev->dev);
+        if (r) {
+                DSSERR("Registering HDMI audio failed\n");
+                hdmi_uninit_output(pdev);
+                pm_runtime_disable(&pdev->dev);
+                return r;
+        }
+
+        dss_debugfs_create_file("hdmi", hdmi_dump_regs);
+
+        return 0;
+err:
+        hdmi_pll_uninit(&hdmi.pll);
+        return r;
+}
+
+static void hdmi4_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        if (hdmi.audio_pdev)
+                platform_device_unregister(hdmi.audio_pdev);
+
+        hdmi_uninit_output(pdev);
+
+        hdmi_pll_uninit(&hdmi.pll);
+
+        pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops hdmi4_component_ops = {
+        .bind   = hdmi4_bind,
+        .unbind = hdmi4_unbind,
+};
+
+static int hdmi4_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &hdmi4_component_ops);
+}
+
+static int hdmi4_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &hdmi4_component_ops);
+        return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+        dispc_runtime_put();
+
+        return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+        int r;
+
+        r = dispc_runtime_get();
+        if (r < 0)
+                return r;
+
+        return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+        .runtime_suspend = hdmi_runtime_suspend,
+        .runtime_resume = hdmi_runtime_resume,
+};
+
+static const struct of_device_id hdmi_of_match[] = {
+        { .compatible = "ti,omap4-hdmi", },
+        {},
+};
+
+static struct platform_driver omapdss_hdmihw_driver = {
+        .probe          = hdmi4_probe,
+        .remove         = hdmi4_remove,
+        .driver         = {
+                .name   = "omapdss_hdmi",
+                .pm     = &hdmi_pm_ops,
+                .of_match_table = hdmi_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init hdmi4_init_platform_driver(void)
+{
+        return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi4_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.c
new file mode 100644
index 000000000000..fa72e735dad2
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.c
@@ -0,0 +1,904 @@
+/*
+ * ti_hdmi_4xxx_ip.c
+ *
+ * HDMI TI81xx, TI38xx, TI OMAP4 etc IP driver Library
+ * 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 "HDMICORE"
+
+#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/platform_device.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <sound/asound.h>
+#include <sound/asoundef.h>
+
+#include "hdmi4_core.h"
+#include "dss_features.h"
+
+#define HDMI_CORE_AV            0x500
+
+static inline void __iomem *hdmi_av_base(struct hdmi_core_data *core)
+{
+        return core->base + HDMI_CORE_AV;
+}
+
+static int hdmi_core_ddc_init(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+
+        /* Turn on CLK for DDC */
+        REG_FLD_MOD(base, HDMI_CORE_AV_DPD, 0x7, 2, 0);
+
+        /* IN_PROG */
+        if (REG_GET(base, HDMI_CORE_DDC_STATUS, 4, 4) == 1) {
+                /* Abort transaction */
+                REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0xf, 3, 0);
+                /* IN_PROG */
+                if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+                                        4, 4, 0) != 0) {
+                        DSSERR("Timeout aborting DDC transaction\n");
+                        return -ETIMEDOUT;
+                }
+        }
+
+        /* Clk SCL Devices */
+        REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0xA, 3, 0);
+
+        /* HDMI_CORE_DDC_STATUS_IN_PROG */
+        if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+                                4, 4, 0) != 0) {
+                DSSERR("Timeout starting SCL clock\n");
+                return -ETIMEDOUT;
+        }
+
+        /* Clear FIFO */
+        REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0x9, 3, 0);
+
+        /* HDMI_CORE_DDC_STATUS_IN_PROG */
+        if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+                                4, 4, 0) != 0) {
+                DSSERR("Timeout clearing DDC fifo\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_core_ddc_edid(struct hdmi_core_data *core,
+                u8 *pedid, int ext)
+{
+        void __iomem *base = core->base;
+        u32 i;
+        char checksum;
+        u32 offset = 0;
+
+        /* HDMI_CORE_DDC_STATUS_IN_PROG */
+        if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+                                4, 4, 0) != 0) {
+                DSSERR("Timeout waiting DDC to be ready\n");
+                return -ETIMEDOUT;
+        }
+
+        if (ext % 2 != 0)
+                offset = 0x80;
+
+        /* Load Segment Address Register */
+        REG_FLD_MOD(base, HDMI_CORE_DDC_SEGM, ext / 2, 7, 0);
+
+        /* Load Slave Address Register */
+        REG_FLD_MOD(base, HDMI_CORE_DDC_ADDR, 0xA0 >> 1, 7, 1);
+
+        /* Load Offset Address Register */
+        REG_FLD_MOD(base, HDMI_CORE_DDC_OFFSET, offset, 7, 0);
+
+        /* Load Byte Count */
+        REG_FLD_MOD(base, HDMI_CORE_DDC_COUNT1, 0x80, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_DDC_COUNT2, 0x0, 1, 0);
+
+        /* Set DDC_CMD */
+        if (ext)
+                REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0x4, 3, 0);
+        else
+                REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0x2, 3, 0);
+
+        /* HDMI_CORE_DDC_STATUS_BUS_LOW */
+        if (REG_GET(base, HDMI_CORE_DDC_STATUS, 6, 6) == 1) {
+                DSSERR("I2C Bus Low?\n");
+                return -EIO;
+        }
+        /* HDMI_CORE_DDC_STATUS_NO_ACK */
+        if (REG_GET(base, HDMI_CORE_DDC_STATUS, 5, 5) == 1) {
+                DSSERR("I2C No Ack\n");
+                return -EIO;
+        }
+
+        for (i = 0; i < 0x80; ++i) {
+                int t;
+
+                /* IN_PROG */
+                if (REG_GET(base, HDMI_CORE_DDC_STATUS, 4, 4) == 0) {
+                        DSSERR("operation stopped when reading edid\n");
+                        return -EIO;
+                }
+
+                t = 0;
+                /* FIFO_EMPTY */
+                while (REG_GET(base, HDMI_CORE_DDC_STATUS, 2, 2) == 1) {
+                        if (t++ > 10000) {
+                                DSSERR("timeout reading edid\n");
+                                return -ETIMEDOUT;
+                        }
+                        udelay(1);
+                }
+
+                pedid[i] = REG_GET(base, HDMI_CORE_DDC_DATA, 7, 0);
+        }
+
+        checksum = 0;
+        for (i = 0; i < 0x80; ++i)
+                checksum += pedid[i];
+
+        if (checksum != 0) {
+                DSSERR("E-EDID checksum failed!!\n");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+int hdmi4_read_edid(struct hdmi_core_data *core, u8 *edid, int len)
+{
+        int r, l;
+
+        if (len < 128)
+                return -EINVAL;
+
+        r = hdmi_core_ddc_init(core);
+        if (r)
+                return r;
+
+        r = hdmi_core_ddc_edid(core, edid, 0);
+        if (r)
+                return r;
+
+        l = 128;
+
+        if (len >= 128 * 2 && edid[0x7e] > 0) {
+                r = hdmi_core_ddc_edid(core, edid + 0x80, 1);
+                if (r)
+                        return r;
+                l += 128;
+        }
+
+        return l;
+}
+
+static void hdmi_core_init(struct hdmi_core_video_config *video_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;
+}
+
+static void hdmi_core_powerdown_disable(struct hdmi_core_data *core)
+{
+        DSSDBG("Enter hdmi_core_powerdown_disable\n");
+        REG_FLD_MOD(core->base, HDMI_CORE_SYS_SYS_CTRL1, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_release(struct hdmi_core_data *core)
+{
+        DSSDBG("Enter hdmi_core_swreset_release\n");
+        REG_FLD_MOD(core->base, HDMI_CORE_SYS_SRST, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_assert(struct hdmi_core_data *core)
+{
+        DSSDBG("Enter hdmi_core_swreset_assert\n");
+        REG_FLD_MOD(core->base, HDMI_CORE_SYS_SRST, 0x1, 0, 0);
+}
+
+/* HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_data *core,
+                                struct hdmi_core_video_config *cfg)
+{
+        u32 r = 0;
+        void __iomem *core_sys_base = core->base;
+        void __iomem *core_av_base = hdmi_av_base(core);
+
+        /* sys_ctrl1 default configuration not tunable */
+        r = hdmi_read_reg(core_sys_base, HDMI_CORE_SYS_SYS_CTRL1);
+        r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_VEN_FOLLOWVSYNC, 5, 5);
+        r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_HEN_FOLLOWHSYNC, 4, 4);
+        r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_BSEL_24BITBUS, 2, 2);
+        r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_EDGE_RISINGEDGE, 1, 1);
+        hdmi_write_reg(core_sys_base, HDMI_CORE_SYS_SYS_CTRL1, r);
+
+        REG_FLD_MOD(core_sys_base,
+                        HDMI_CORE_SYS_VID_ACEN, cfg->ip_bus_width, 7, 6);
+
+        /* Vid_Mode */
+        r = hdmi_read_reg(core_sys_base, 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(core_sys_base, HDMI_CORE_SYS_VID_MODE, r);
+
+        /* HDMI_Ctrl */
+        r = hdmi_read_reg(core_av_base, 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(core_av_base, HDMI_CORE_AV_HDMI_CTRL, r);
+
+        /* TMDS_CTRL */
+        REG_FLD_MOD(core_sys_base,
+                        HDMI_CORE_SYS_TMDS_CTRL, cfg->tclk_sel_clkmult, 6, 5);
+}
+
+static void hdmi_core_write_avi_infoframe(struct hdmi_core_data *core,
+        struct hdmi_avi_infoframe *frame)
+{
+        void __iomem *av_base = hdmi_av_base(core);
+        u8 data[HDMI_INFOFRAME_SIZE(AVI)];
+        int i;
+
+        hdmi_avi_infoframe_pack(frame, data, sizeof(data));
+
+        print_hex_dump_debug("AVI: ", DUMP_PREFIX_NONE, 16, 1, data,
+                HDMI_INFOFRAME_SIZE(AVI), false);
+
+        for (i = 0; i < sizeof(data); ++i) {
+                hdmi_write_reg(av_base, HDMI_CORE_AV_AVI_BASE + i * 4,
+                        data[i]);
+        }
+}
+
+static void hdmi_core_av_packet_config(struct hdmi_core_data *core,
+                struct hdmi_core_packet_enable_repeat repeat_cfg)
+{
+        /* enable/repeat the infoframe */
+        hdmi_write_reg(hdmi_av_base(core), 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_av_base(core), 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));
+}
+
+void hdmi4_configure(struct hdmi_core_data *core,
+        struct hdmi_wp_data *wp, struct hdmi_config *cfg)
+{
+        /* HDMI */
+        struct omap_video_timings video_timing;
+        struct hdmi_video_format video_format;
+        /* HDMI core */
+        struct hdmi_core_video_config v_core_cfg;
+        struct hdmi_core_packet_enable_repeat repeat_cfg = { 0 };
+
+        hdmi_core_init(&v_core_cfg);
+
+        hdmi_wp_init_vid_fmt_timings(&video_format, &video_timing, cfg);
+
+        hdmi_wp_video_config_timing(wp, &video_timing);
+
+        /* video config */
+        video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+        hdmi_wp_video_config_format(wp, &video_format);
+
+        hdmi_wp_video_config_interface(wp, &video_timing);
+
+        /*
+         * configure core video part
+         * set software reset in the core
+         */
+        hdmi_core_swreset_assert(core);
+
+        /* power down off */
+        hdmi_core_powerdown_disable(core);
+
+        v_core_cfg.pkt_mode = HDMI_PACKETMODE24BITPERPIXEL;
+        v_core_cfg.hdmi_dvi = cfg->hdmi_dvi_mode;
+
+        hdmi_core_video_config(core, &v_core_cfg);
+
+        /* release software reset in the core */
+        hdmi_core_swreset_release(core);
+
+        if (cfg->hdmi_dvi_mode == HDMI_HDMI) {
+                hdmi_core_write_avi_infoframe(core, &cfg->infoframe);
+
+                /* 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(core, repeat_cfg);
+}
+
+void hdmi4_core_dump(struct hdmi_core_data *core, struct seq_file *s)
+{
+        int i;
+
+#define CORE_REG(i, name) name(i)
+#define DUMPCORE(r) seq_printf(s, "%-35s %08x\n", #r,\
+                hdmi_read_reg(core->base, r))
+#define DUMPCOREAV(r) seq_printf(s, "%-35s %08x\n", #r,\
+                hdmi_read_reg(hdmi_av_base(core), r))
+#define DUMPCOREAV2(i, r) seq_printf(s, "%s[%d]%*s %08x\n", #r, i, \
+                (i < 10) ? 32 - (int)strlen(#r) : 31 - (int)strlen(#r), " ", \
+                hdmi_read_reg(hdmi_av_base(core), CORE_REG(i, r)))
+
+        DUMPCORE(HDMI_CORE_SYS_VND_IDL);
+        DUMPCORE(HDMI_CORE_SYS_DEV_IDL);
+        DUMPCORE(HDMI_CORE_SYS_DEV_IDH);
+        DUMPCORE(HDMI_CORE_SYS_DEV_REV);
+        DUMPCORE(HDMI_CORE_SYS_SRST);
+        DUMPCORE(HDMI_CORE_SYS_SYS_CTRL1);
+        DUMPCORE(HDMI_CORE_SYS_SYS_STAT);
+        DUMPCORE(HDMI_CORE_SYS_SYS_CTRL3);
+        DUMPCORE(HDMI_CORE_SYS_DE_DLY);
+        DUMPCORE(HDMI_CORE_SYS_DE_CTRL);
+        DUMPCORE(HDMI_CORE_SYS_DE_TOP);
+        DUMPCORE(HDMI_CORE_SYS_DE_CNTL);
+        DUMPCORE(HDMI_CORE_SYS_DE_CNTH);
+        DUMPCORE(HDMI_CORE_SYS_DE_LINL);
+        DUMPCORE(HDMI_CORE_SYS_DE_LINH_1);
+        DUMPCORE(HDMI_CORE_SYS_HRES_L);
+        DUMPCORE(HDMI_CORE_SYS_HRES_H);
+        DUMPCORE(HDMI_CORE_SYS_VRES_L);
+        DUMPCORE(HDMI_CORE_SYS_VRES_H);
+        DUMPCORE(HDMI_CORE_SYS_IADJUST);
+        DUMPCORE(HDMI_CORE_SYS_POLDETECT);
+        DUMPCORE(HDMI_CORE_SYS_HWIDTH1);
+        DUMPCORE(HDMI_CORE_SYS_HWIDTH2);
+        DUMPCORE(HDMI_CORE_SYS_VWIDTH);
+        DUMPCORE(HDMI_CORE_SYS_VID_CTRL);
+        DUMPCORE(HDMI_CORE_SYS_VID_ACEN);
+        DUMPCORE(HDMI_CORE_SYS_VID_MODE);
+        DUMPCORE(HDMI_CORE_SYS_VID_BLANK1);
+        DUMPCORE(HDMI_CORE_SYS_VID_BLANK3);
+        DUMPCORE(HDMI_CORE_SYS_VID_BLANK1);
+        DUMPCORE(HDMI_CORE_SYS_DC_HEADER);
+        DUMPCORE(HDMI_CORE_SYS_VID_DITHER);
+        DUMPCORE(HDMI_CORE_SYS_RGB2XVYCC_CT);
+        DUMPCORE(HDMI_CORE_SYS_R2Y_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_R2Y_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_G2Y_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_G2Y_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_B2Y_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_B2Y_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_R2CB_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_R2CB_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_G2CB_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_G2CB_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_B2CB_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_B2CB_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_R2CR_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_R2CR_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_G2CR_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_G2CR_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_B2CR_COEFF_LOW);
+        DUMPCORE(HDMI_CORE_SYS_B2CR_COEFF_UP);
+        DUMPCORE(HDMI_CORE_SYS_RGB_OFFSET_LOW);
+        DUMPCORE(HDMI_CORE_SYS_RGB_OFFSET_UP);
+        DUMPCORE(HDMI_CORE_SYS_Y_OFFSET_LOW);
+        DUMPCORE(HDMI_CORE_SYS_Y_OFFSET_UP);
+        DUMPCORE(HDMI_CORE_SYS_CBCR_OFFSET_LOW);
+        DUMPCORE(HDMI_CORE_SYS_CBCR_OFFSET_UP);
+        DUMPCORE(HDMI_CORE_SYS_INTR_STATE);
+        DUMPCORE(HDMI_CORE_SYS_INTR1);
+        DUMPCORE(HDMI_CORE_SYS_INTR2);
+        DUMPCORE(HDMI_CORE_SYS_INTR3);
+        DUMPCORE(HDMI_CORE_SYS_INTR4);
+        DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK1);
+        DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK2);
+        DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK3);
+        DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK4);
+        DUMPCORE(HDMI_CORE_SYS_INTR_CTRL);
+        DUMPCORE(HDMI_CORE_SYS_TMDS_CTRL);
+
+        DUMPCORE(HDMI_CORE_DDC_ADDR);
+        DUMPCORE(HDMI_CORE_DDC_SEGM);
+        DUMPCORE(HDMI_CORE_DDC_OFFSET);
+        DUMPCORE(HDMI_CORE_DDC_COUNT1);
+        DUMPCORE(HDMI_CORE_DDC_COUNT2);
+        DUMPCORE(HDMI_CORE_DDC_STATUS);
+        DUMPCORE(HDMI_CORE_DDC_CMD);
+        DUMPCORE(HDMI_CORE_DDC_DATA);
+
+        DUMPCOREAV(HDMI_CORE_AV_ACR_CTRL);
+        DUMPCOREAV(HDMI_CORE_AV_FREQ_SVAL);
+        DUMPCOREAV(HDMI_CORE_AV_N_SVAL1);
+        DUMPCOREAV(HDMI_CORE_AV_N_SVAL2);
+        DUMPCOREAV(HDMI_CORE_AV_N_SVAL3);
+        DUMPCOREAV(HDMI_CORE_AV_CTS_SVAL1);
+        DUMPCOREAV(HDMI_CORE_AV_CTS_SVAL2);
+        DUMPCOREAV(HDMI_CORE_AV_CTS_SVAL3);
+        DUMPCOREAV(HDMI_CORE_AV_CTS_HVAL1);
+        DUMPCOREAV(HDMI_CORE_AV_CTS_HVAL2);
+        DUMPCOREAV(HDMI_CORE_AV_CTS_HVAL3);
+        DUMPCOREAV(HDMI_CORE_AV_AUD_MODE);
+        DUMPCOREAV(HDMI_CORE_AV_SPDIF_CTRL);
+        DUMPCOREAV(HDMI_CORE_AV_HW_SPDIF_FS);
+        DUMPCOREAV(HDMI_CORE_AV_SWAP_I2S);
+        DUMPCOREAV(HDMI_CORE_AV_SPDIF_ERTH);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_IN_MAP);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_IN_CTRL);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_CHST0);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_CHST1);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_CHST2);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_CHST4);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_CHST5);
+        DUMPCOREAV(HDMI_CORE_AV_ASRC);
+        DUMPCOREAV(HDMI_CORE_AV_I2S_IN_LEN);
+        DUMPCOREAV(HDMI_CORE_AV_HDMI_CTRL);
+        DUMPCOREAV(HDMI_CORE_AV_AUDO_TXSTAT);
+        DUMPCOREAV(HDMI_CORE_AV_AUD_PAR_BUSCLK_1);
+        DUMPCOREAV(HDMI_CORE_AV_AUD_PAR_BUSCLK_2);
+        DUMPCOREAV(HDMI_CORE_AV_AUD_PAR_BUSCLK_3);
+        DUMPCOREAV(HDMI_CORE_AV_TEST_TXCTRL);
+        DUMPCOREAV(HDMI_CORE_AV_DPD);
+        DUMPCOREAV(HDMI_CORE_AV_PB_CTRL1);
+        DUMPCOREAV(HDMI_CORE_AV_PB_CTRL2);
+        DUMPCOREAV(HDMI_CORE_AV_AVI_TYPE);
+        DUMPCOREAV(HDMI_CORE_AV_AVI_VERS);
+        DUMPCOREAV(HDMI_CORE_AV_AVI_LEN);
+        DUMPCOREAV(HDMI_CORE_AV_AVI_CHSUM);
+
+        for (i = 0; i < HDMI_CORE_AV_AVI_DBYTE_NELEMS; i++)
+                DUMPCOREAV2(i, HDMI_CORE_AV_AVI_DBYTE);
+
+        DUMPCOREAV(HDMI_CORE_AV_SPD_TYPE);
+        DUMPCOREAV(HDMI_CORE_AV_SPD_VERS);
+        DUMPCOREAV(HDMI_CORE_AV_SPD_LEN);
+        DUMPCOREAV(HDMI_CORE_AV_SPD_CHSUM);
+
+        for (i = 0; i < HDMI_CORE_AV_SPD_DBYTE_NELEMS; i++)
+                DUMPCOREAV2(i, HDMI_CORE_AV_SPD_DBYTE);
+
+        DUMPCOREAV(HDMI_CORE_AV_AUDIO_TYPE);
+        DUMPCOREAV(HDMI_CORE_AV_AUDIO_VERS);
+        DUMPCOREAV(HDMI_CORE_AV_AUDIO_LEN);
+        DUMPCOREAV(HDMI_CORE_AV_AUDIO_CHSUM);
+
+        for (i = 0; i < HDMI_CORE_AV_AUD_DBYTE_NELEMS; i++)
+                DUMPCOREAV2(i, HDMI_CORE_AV_AUD_DBYTE);
+
+        DUMPCOREAV(HDMI_CORE_AV_MPEG_TYPE);
+        DUMPCOREAV(HDMI_CORE_AV_MPEG_VERS);
+        DUMPCOREAV(HDMI_CORE_AV_MPEG_LEN);
+        DUMPCOREAV(HDMI_CORE_AV_MPEG_CHSUM);
+
+        for (i = 0; i < HDMI_CORE_AV_MPEG_DBYTE_NELEMS; i++)
+                DUMPCOREAV2(i, HDMI_CORE_AV_MPEG_DBYTE);
+
+        for (i = 0; i < HDMI_CORE_AV_GEN_DBYTE_NELEMS; i++)
+                DUMPCOREAV2(i, HDMI_CORE_AV_GEN_DBYTE);
+
+        DUMPCOREAV(HDMI_CORE_AV_CP_BYTE1);
+
+        for (i = 0; i < HDMI_CORE_AV_GEN2_DBYTE_NELEMS; i++)
+                DUMPCOREAV2(i, HDMI_CORE_AV_GEN2_DBYTE);
+
+        DUMPCOREAV(HDMI_CORE_AV_CEC_ADDR_ID);
+}
+
+static void hdmi_core_audio_config(struct hdmi_core_data *core,
+                                        struct hdmi_core_audio_config *cfg)
+{
+        u32 r;
+        void __iomem *av_base = hdmi_av_base(core);
+
+        /*
+         * Parameters for generation of Audio Clock Recovery packets
+         */
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_N_SVAL1, cfg->n, 7, 0);
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_N_SVAL2, cfg->n >> 8, 7, 0);
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_N_SVAL3, cfg->n >> 16, 7, 0);
+
+        if (cfg->cts_mode == HDMI_AUDIO_CTS_MODE_SW) {
+                REG_FLD_MOD(av_base, HDMI_CORE_AV_CTS_SVAL1, cfg->cts, 7, 0);
+                REG_FLD_MOD(av_base,
+                                HDMI_CORE_AV_CTS_SVAL2, cfg->cts >> 8, 7, 0);
+                REG_FLD_MOD(av_base,
+                                HDMI_CORE_AV_CTS_SVAL3, cfg->cts >> 16, 7, 0);
+        } else {
+                REG_FLD_MOD(av_base, HDMI_CORE_AV_AUD_PAR_BUSCLK_1,
+                                cfg->aud_par_busclk, 7, 0);
+                REG_FLD_MOD(av_base, HDMI_CORE_AV_AUD_PAR_BUSCLK_2,
+                                (cfg->aud_par_busclk >> 8), 7, 0);
+                REG_FLD_MOD(av_base, HDMI_CORE_AV_AUD_PAR_BUSCLK_3,
+                                (cfg->aud_par_busclk >> 16), 7, 0);
+        }
+
+        /* Set ACR clock divisor */
+        REG_FLD_MOD(av_base,
+                        HDMI_CORE_AV_FREQ_SVAL, cfg->mclk_mode, 2, 0);
+
+        r = hdmi_read_reg(av_base, HDMI_CORE_AV_ACR_CTRL);
+        /*
+         * Use TMDS clock for ACR packets. For devices that use
+         * the MCLK, this is the first part of the MCLK initialization.
+         */
+        r = FLD_MOD(r, 0, 2, 2);
+
+        r = FLD_MOD(r, cfg->en_acr_pkt, 1, 1);
+        r = FLD_MOD(r, cfg->cts_mode, 0, 0);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_ACR_CTRL, r);
+
+        /* For devices using MCLK, this completes its initialization. */
+        if (cfg->use_mclk)
+                REG_FLD_MOD(av_base, HDMI_CORE_AV_ACR_CTRL, 1, 2, 2);
+
+        /* Override of SPDIF sample frequency with value in I2S_CHST4 */
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_SPDIF_CTRL,
+                                                cfg->fs_override, 1, 1);
+
+        /*
+         * Set IEC-60958-3 channel status word. It is passed to the IP
+         * just as it is received. The user of the driver is responsible
+         * for its contents.
+         */
+        hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST0,
+                       cfg->iec60958_cfg->status[0]);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST1,
+                       cfg->iec60958_cfg->status[1]);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST2,
+                       cfg->iec60958_cfg->status[2]);
+        /* yes, this is correct: status[3] goes to CHST4 register */
+        hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST4,
+                       cfg->iec60958_cfg->status[3]);
+        /* yes, this is correct: status[4] goes to CHST5 register */
+        hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST5,
+                       cfg->iec60958_cfg->status[4]);
+
+        /* set I2S parameters */
+        r = hdmi_read_reg(av_base, HDMI_CORE_AV_I2S_IN_CTRL);
+        r = FLD_MOD(r, cfg->i2s_cfg.sck_edge_mode, 6, 6);
+        r = FLD_MOD(r, cfg->i2s_cfg.vbit, 4, 4);
+        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(av_base, HDMI_CORE_AV_I2S_IN_CTRL, r);
+
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_I2S_IN_LEN,
+                        cfg->i2s_cfg.in_length_bits, 3, 0);
+
+        /* Audio channels and mode parameters */
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_HDMI_CTRL, cfg->layout, 2, 1);
+        r = hdmi_read_reg(av_base, 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(av_base, HDMI_CORE_AV_AUD_MODE, r);
+
+        /* Audio channel mappings */
+        /* TODO: Make channel mapping dynamic. For now, map channels
+         * in the ALSA order: FL/FR/RL/RR/C/LFE/SL/SR. Remapping is needed as
+         * HDMI speaker order is different. See CEA-861 Section 6.6.2.
+         */
+        hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_IN_MAP, 0x78);
+        REG_FLD_MOD(av_base, HDMI_CORE_AV_SWAP_I2S, 1, 5, 5);
+}
+
+static void hdmi_core_audio_infoframe_cfg(struct hdmi_core_data *core,
+                struct snd_cea_861_aud_if *info_aud)
+{
+        u8 sum = 0, checksum = 0;
+        void __iomem *av_base = hdmi_av_base(core);
+
+        /*
+         * 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(av_base, HDMI_CORE_AV_AUDIO_TYPE, 0x84);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUDIO_VERS, 0x01);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUDIO_LEN, 0x0a);
+        sum += 0x84 + 0x001 + 0x00a;
+
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(0),
+                       info_aud->db1_ct_cc);
+        sum += info_aud->db1_ct_cc;
+
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(1),
+                       info_aud->db2_sf_ss);
+        sum += info_aud->db2_sf_ss;
+
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(2), info_aud->db3);
+        sum += info_aud->db3;
+
+        /*
+         * The OMAP HDMI IP requires to use the 8-channel channel code when
+         * transmitting more than two channels.
+         */
+        if (info_aud->db4_ca != 0x00)
+                info_aud->db4_ca = 0x13;
+
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(3), info_aud->db4_ca);
+        sum += info_aud->db4_ca;
+
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(4),
+                       info_aud->db5_dminh_lsv);
+        sum += info_aud->db5_dminh_lsv;
+
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(5), 0x00);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(6), 0x00);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(7), 0x00);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(8), 0x00);
+        hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(9), 0x00);
+
+        checksum = 0x100 - sum;
+        hdmi_write_reg(av_base,
+                                        HDMI_CORE_AV_AUDIO_CHSUM, checksum);
+
+        /*
+         * TODO: Add MPEG and SPD enable and repeat cfg when EDID parsing
+         * is available.
+         */
+}
+
+int hdmi4_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                struct omap_dss_audio *audio, u32 pclk)
+{
+        struct hdmi_audio_format audio_format;
+        struct hdmi_audio_dma audio_dma;
+        struct hdmi_core_audio_config acore;
+        int err, n, cts, channel_count;
+        unsigned int fs_nr;
+        bool word_length_16b = false;
+
+        if (!audio || !audio->iec || !audio->cea || !core)
+                return -EINVAL;
+
+        acore.iec60958_cfg = audio->iec;
+        /*
+         * In the IEC-60958 status word, check if the audio sample word length
+         * is 16-bit as several optimizations can be performed in such case.
+         */
+        if (!(audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24))
+                if (audio->iec->status[4] & IEC958_AES4_CON_WORDLEN_20_16)
+                        word_length_16b = true;
+
+        /* I2S configuration. See Phillips' specification */
+        if (word_length_16b)
+                acore.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+        else
+                acore.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+        /*
+         * The I2S input word length is twice the lenght given in the IEC-60958
+         * status word. If the word size is greater than
+         * 20 bits, increment by one.
+         */
+        acore.i2s_cfg.in_length_bits = audio->iec->status[4]
+                & IEC958_AES4_CON_WORDLEN;
+        if (audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24)
+                acore.i2s_cfg.in_length_bits++;
+        acore.i2s_cfg.sck_edge_mode = HDMI_AUDIO_I2S_SCK_EDGE_RISING;
+        acore.i2s_cfg.vbit = HDMI_AUDIO_I2S_VBIT_FOR_PCM;
+        acore.i2s_cfg.direction = HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST;
+        acore.i2s_cfg.shift = HDMI_AUDIO_I2S_FIRST_BIT_SHIFT;
+
+        /* convert sample frequency to a number */
+        switch (audio->iec->status[3] & IEC958_AES3_CON_FS) {
+        case IEC958_AES3_CON_FS_32000:
+                fs_nr = 32000;
+                break;
+        case IEC958_AES3_CON_FS_44100:
+                fs_nr = 44100;
+                break;
+        case IEC958_AES3_CON_FS_48000:
+                fs_nr = 48000;
+                break;
+        case IEC958_AES3_CON_FS_88200:
+                fs_nr = 88200;
+                break;
+        case IEC958_AES3_CON_FS_96000:
+                fs_nr = 96000;
+                break;
+        case IEC958_AES3_CON_FS_176400:
+                fs_nr = 176400;
+                break;
+        case IEC958_AES3_CON_FS_192000:
+                fs_nr = 192000;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        err = hdmi_compute_acr(pclk, fs_nr, &n, &cts);
+
+        /* Audio clock regeneration settings */
+        acore.n = n;
+        acore.cts = cts;
+        if (dss_has_feature(FEAT_HDMI_CTS_SWMODE)) {
+                acore.aud_par_busclk = 0;
+                acore.cts_mode = HDMI_AUDIO_CTS_MODE_SW;
+                acore.use_mclk = dss_has_feature(FEAT_HDMI_AUDIO_USE_MCLK);
+        } else {
+                acore.aud_par_busclk = (((128 * 31) - 1) << 8);
+                acore.cts_mode = HDMI_AUDIO_CTS_MODE_HW;
+                acore.use_mclk = true;
+        }
+
+        if (acore.use_mclk)
+                acore.mclk_mode = HDMI_AUDIO_MCLK_128FS;
+
+        /* Audio channels settings */
+        channel_count = (audio->cea->db1_ct_cc &
+                         CEA861_AUDIO_INFOFRAME_DB1CC) + 1;
+
+        switch (channel_count) {
+        case 2:
+                audio_format.active_chnnls_msk = 0x03;
+                break;
+        case 3:
+                audio_format.active_chnnls_msk = 0x07;
+                break;
+        case 4:
+                audio_format.active_chnnls_msk = 0x0f;
+                break;
+        case 5:
+                audio_format.active_chnnls_msk = 0x1f;
+                break;
+        case 6:
+                audio_format.active_chnnls_msk = 0x3f;
+                break;
+        case 7:
+                audio_format.active_chnnls_msk = 0x7f;
+                break;
+        case 8:
+                audio_format.active_chnnls_msk = 0xff;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        /*
+         * the HDMI IP needs to enable four stereo channels when transmitting
+         * more than 2 audio channels.  Similarly, the channel count in the
+         * Audio InfoFrame has to match the sample_present bits (some channels
+         * are padded with zeroes)
+         */
+        if (channel_count == 2) {
+                audio_format.stereo_channels = HDMI_AUDIO_STEREO_ONECHANNEL;
+                acore.i2s_cfg.active_sds = HDMI_AUDIO_I2S_SD0_EN;
+                acore.layout = HDMI_AUDIO_LAYOUT_2CH;
+        } else {
+                audio_format.stereo_channels = HDMI_AUDIO_STEREO_FOURCHANNELS;
+                acore.i2s_cfg.active_sds = HDMI_AUDIO_I2S_SD0_EN |
+                                HDMI_AUDIO_I2S_SD1_EN | HDMI_AUDIO_I2S_SD2_EN |
+                                HDMI_AUDIO_I2S_SD3_EN;
+                acore.layout = HDMI_AUDIO_LAYOUT_8CH;
+                audio->cea->db1_ct_cc = 7;
+        }
+
+        acore.en_spdif = false;
+        /* use sample frequency from channel status word */
+        acore.fs_override = true;
+        /* enable ACR packets */
+        acore.en_acr_pkt = true;
+        /* disable direct streaming digital audio */
+        acore.en_dsd_audio = false;
+        /* use parallel audio interface */
+        acore.en_parallel_aud_input = true;
+
+        /* DMA settings */
+        if (word_length_16b)
+                audio_dma.transfer_size = 0x10;
+        else
+                audio_dma.transfer_size = 0x20;
+        audio_dma.block_size = 0xC0;
+        audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+        audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+        /* audio FIFO format settings */
+        if (word_length_16b) {
+                audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
+                audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
+                audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+        } else {
+                audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_ONESAMPLE;
+                audio_format.sample_size = HDMI_AUDIO_SAMPLE_24BITS;
+                audio_format.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+        }
+        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_ON;
+
+        /* configure DMA and audio FIFO format*/
+        hdmi_wp_audio_config_dma(wp, &audio_dma);
+        hdmi_wp_audio_config_format(wp, &audio_format);
+
+        /* configure the core*/
+        hdmi_core_audio_config(core, &acore);
+
+        /* configure CEA 861 audio infoframe*/
+        hdmi_core_audio_infoframe_cfg(core, audio->cea);
+
+        return 0;
+}
+
+int hdmi4_audio_start(struct hdmi_core_data *core, struct hdmi_wp_data *wp)
+{
+        REG_FLD_MOD(hdmi_av_base(core),
+                    HDMI_CORE_AV_AUD_MODE, true, 0, 0);
+
+        hdmi_wp_audio_core_req_enable(wp, true);
+
+        return 0;
+}
+
+void hdmi4_audio_stop(struct hdmi_core_data *core, struct hdmi_wp_data *wp)
+{
+        REG_FLD_MOD(hdmi_av_base(core),
+                    HDMI_CORE_AV_AUD_MODE, false, 0, 0);
+
+        hdmi_wp_audio_core_req_enable(wp, false);
+}
+
+int hdmi4_core_init(struct platform_device *pdev, struct hdmi_core_data *core)
+{
+        struct resource *res;
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
+        if (!res) {
+                DSSERR("can't get CORE mem resource\n");
+                return -EINVAL;
+        }
+
+        core->base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(core->base)) {
+                DSSERR("can't ioremap CORE\n");
+                return PTR_ERR(core->base);
+        }
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.h b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.h
new file mode 100644
index 000000000000..a069f96ec6f6
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.h
@@ -0,0 +1,273 @@
+/*
+ * HDMI header definition for OMAP4 HDMI core IP
+ *
+ * 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 _HDMI4_CORE_H_
+#define _HDMI4_CORE_H_
+
+#include "hdmi.h"
+
+/* OMAP4 HDMI IP Core System */
+
+#define HDMI_CORE_SYS_VND_IDL                   0x0
+#define HDMI_CORE_SYS_DEV_IDL                   0x8
+#define HDMI_CORE_SYS_DEV_IDH                   0xC
+#define HDMI_CORE_SYS_DEV_REV                   0x10
+#define HDMI_CORE_SYS_SRST                      0x14
+#define HDMI_CORE_SYS_SYS_CTRL1                 0x20
+#define HDMI_CORE_SYS_SYS_STAT                  0x24
+#define HDMI_CORE_SYS_SYS_CTRL3                 0x28
+#define HDMI_CORE_SYS_DCTL                      0x34
+#define HDMI_CORE_SYS_DE_DLY                    0xC8
+#define HDMI_CORE_SYS_DE_CTRL                   0xCC
+#define HDMI_CORE_SYS_DE_TOP                    0xD0
+#define HDMI_CORE_SYS_DE_CNTL                   0xD8
+#define HDMI_CORE_SYS_DE_CNTH                   0xDC
+#define HDMI_CORE_SYS_DE_LINL                   0xE0
+#define HDMI_CORE_SYS_DE_LINH_1                 0xE4
+#define HDMI_CORE_SYS_HRES_L                    0xE8
+#define HDMI_CORE_SYS_HRES_H                    0xEC
+#define HDMI_CORE_SYS_VRES_L                    0xF0
+#define HDMI_CORE_SYS_VRES_H                    0xF4
+#define HDMI_CORE_SYS_IADJUST                   0xF8
+#define HDMI_CORE_SYS_POLDETECT                 0xFC
+#define HDMI_CORE_SYS_HWIDTH1                   0x110
+#define HDMI_CORE_SYS_HWIDTH2                   0x114
+#define HDMI_CORE_SYS_VWIDTH                    0x11C
+#define HDMI_CORE_SYS_VID_CTRL                  0x120
+#define HDMI_CORE_SYS_VID_ACEN                  0x124
+#define HDMI_CORE_SYS_VID_MODE                  0x128
+#define HDMI_CORE_SYS_VID_BLANK1                0x12C
+#define HDMI_CORE_SYS_VID_BLANK2                0x130
+#define HDMI_CORE_SYS_VID_BLANK3                0x134
+#define HDMI_CORE_SYS_DC_HEADER                 0x138
+#define HDMI_CORE_SYS_VID_DITHER                0x13C
+#define HDMI_CORE_SYS_RGB2XVYCC_CT              0x140
+#define HDMI_CORE_SYS_R2Y_COEFF_LOW             0x144
+#define HDMI_CORE_SYS_R2Y_COEFF_UP              0x148
+#define HDMI_CORE_SYS_G2Y_COEFF_LOW             0x14C
+#define HDMI_CORE_SYS_G2Y_COEFF_UP              0x150
+#define HDMI_CORE_SYS_B2Y_COEFF_LOW             0x154
+#define HDMI_CORE_SYS_B2Y_COEFF_UP              0x158
+#define HDMI_CORE_SYS_R2CB_COEFF_LOW            0x15C
+#define HDMI_CORE_SYS_R2CB_COEFF_UP             0x160
+#define HDMI_CORE_SYS_G2CB_COEFF_LOW            0x164
+#define HDMI_CORE_SYS_G2CB_COEFF_UP             0x168
+#define HDMI_CORE_SYS_B2CB_COEFF_LOW            0x16C
+#define HDMI_CORE_SYS_B2CB_COEFF_UP             0x170
+#define HDMI_CORE_SYS_R2CR_COEFF_LOW            0x174
+#define HDMI_CORE_SYS_R2CR_COEFF_UP             0x178
+#define HDMI_CORE_SYS_G2CR_COEFF_LOW            0x17C
+#define HDMI_CORE_SYS_G2CR_COEFF_UP             0x180
+#define HDMI_CORE_SYS_B2CR_COEFF_LOW            0x184
+#define HDMI_CORE_SYS_B2CR_COEFF_UP             0x188
+#define HDMI_CORE_SYS_RGB_OFFSET_LOW            0x18C
+#define HDMI_CORE_SYS_RGB_OFFSET_UP             0x190
+#define HDMI_CORE_SYS_Y_OFFSET_LOW              0x194
+#define HDMI_CORE_SYS_Y_OFFSET_UP               0x198
+#define HDMI_CORE_SYS_CBCR_OFFSET_LOW           0x19C
+#define HDMI_CORE_SYS_CBCR_OFFSET_UP            0x1A0
+#define HDMI_CORE_SYS_INTR_STATE                0x1C0
+#define HDMI_CORE_SYS_INTR1                     0x1C4
+#define HDMI_CORE_SYS_INTR2                     0x1C8
+#define HDMI_CORE_SYS_INTR3                     0x1CC
+#define HDMI_CORE_SYS_INTR4                     0x1D0
+#define HDMI_CORE_SYS_INTR_UNMASK1              0x1D4
+#define HDMI_CORE_SYS_INTR_UNMASK2              0x1D8
+#define HDMI_CORE_SYS_INTR_UNMASK3              0x1DC
+#define HDMI_CORE_SYS_INTR_UNMASK4              0x1E0
+#define HDMI_CORE_SYS_INTR_CTRL                 0x1E4
+#define HDMI_CORE_SYS_TMDS_CTRL                 0x208
+
+/* value definitions for HDMI_CORE_SYS_SYS_CTRL1 fields */
+#define HDMI_CORE_SYS_SYS_CTRL1_VEN_FOLLOWVSYNC 0x1
+#define HDMI_CORE_SYS_SYS_CTRL1_HEN_FOLLOWHSYNC 0x1
+#define HDMI_CORE_SYS_SYS_CTRL1_BSEL_24BITBUS   0x1
+#define HDMI_CORE_SYS_SYS_CTRL1_EDGE_RISINGEDGE 0x1
+
+/* HDMI DDC E-DID */
+#define HDMI_CORE_DDC_ADDR                      0x3B4
+#define HDMI_CORE_DDC_SEGM                      0x3B8
+#define HDMI_CORE_DDC_OFFSET                    0x3BC
+#define HDMI_CORE_DDC_COUNT1                    0x3C0
+#define HDMI_CORE_DDC_COUNT2                    0x3C4
+#define HDMI_CORE_DDC_STATUS                    0x3C8
+#define HDMI_CORE_DDC_CMD                       0x3CC
+#define HDMI_CORE_DDC_DATA                      0x3D0
+
+/* HDMI IP Core Audio Video */
+
+#define HDMI_CORE_AV_ACR_CTRL                   0x4
+#define HDMI_CORE_AV_FREQ_SVAL                  0x8
+#define HDMI_CORE_AV_N_SVAL1                    0xC
+#define HDMI_CORE_AV_N_SVAL2                    0x10
+#define HDMI_CORE_AV_N_SVAL3                    0x14
+#define HDMI_CORE_AV_CTS_SVAL1                  0x18
+#define HDMI_CORE_AV_CTS_SVAL2                  0x1C
+#define HDMI_CORE_AV_CTS_SVAL3                  0x20
+#define HDMI_CORE_AV_CTS_HVAL1                  0x24
+#define HDMI_CORE_AV_CTS_HVAL2                  0x28
+#define HDMI_CORE_AV_CTS_HVAL3                  0x2C
+#define HDMI_CORE_AV_AUD_MODE                   0x50
+#define HDMI_CORE_AV_SPDIF_CTRL                 0x54
+#define HDMI_CORE_AV_HW_SPDIF_FS                0x60
+#define HDMI_CORE_AV_SWAP_I2S                   0x64
+#define HDMI_CORE_AV_SPDIF_ERTH                 0x6C
+#define HDMI_CORE_AV_I2S_IN_MAP                 0x70
+#define HDMI_CORE_AV_I2S_IN_CTRL                0x74
+#define HDMI_CORE_AV_I2S_CHST0                  0x78
+#define HDMI_CORE_AV_I2S_CHST1                  0x7C
+#define HDMI_CORE_AV_I2S_CHST2                  0x80
+#define HDMI_CORE_AV_I2S_CHST4                  0x84
+#define HDMI_CORE_AV_I2S_CHST5                  0x88
+#define HDMI_CORE_AV_ASRC                       0x8C
+#define HDMI_CORE_AV_I2S_IN_LEN                 0x90
+#define HDMI_CORE_AV_HDMI_CTRL                  0xBC
+#define HDMI_CORE_AV_AUDO_TXSTAT                0xC0
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_1           0xCC
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_2           0xD0
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_3           0xD4
+#define HDMI_CORE_AV_TEST_TXCTRL                0xF0
+#define HDMI_CORE_AV_DPD                        0xF4
+#define HDMI_CORE_AV_PB_CTRL1                   0xF8
+#define HDMI_CORE_AV_PB_CTRL2                   0xFC
+#define HDMI_CORE_AV_AVI_BASE                   0x100
+#define HDMI_CORE_AV_AVI_TYPE                   0x100
+#define HDMI_CORE_AV_AVI_VERS                   0x104
+#define HDMI_CORE_AV_AVI_LEN                    0x108
+#define HDMI_CORE_AV_AVI_CHSUM                  0x10C
+#define HDMI_CORE_AV_AVI_DBYTE(n)               (n * 4 + 0x110)
+#define HDMI_CORE_AV_SPD_TYPE                   0x180
+#define HDMI_CORE_AV_SPD_VERS                   0x184
+#define HDMI_CORE_AV_SPD_LEN                    0x188
+#define HDMI_CORE_AV_SPD_CHSUM                  0x18C
+#define HDMI_CORE_AV_SPD_DBYTE(n)               (n * 4 + 0x190)
+#define HDMI_CORE_AV_AUDIO_TYPE                 0x200
+#define HDMI_CORE_AV_AUDIO_VERS                 0x204
+#define HDMI_CORE_AV_AUDIO_LEN                  0x208
+#define HDMI_CORE_AV_AUDIO_CHSUM                0x20C
+#define HDMI_CORE_AV_AUD_DBYTE(n)               (n * 4 + 0x210)
+#define HDMI_CORE_AV_MPEG_TYPE                  0x280
+#define HDMI_CORE_AV_MPEG_VERS                  0x284
+#define HDMI_CORE_AV_MPEG_LEN                   0x288
+#define HDMI_CORE_AV_MPEG_CHSUM                 0x28C
+#define HDMI_CORE_AV_MPEG_DBYTE(n)              (n * 4 + 0x290)
+#define HDMI_CORE_AV_GEN_DBYTE(n)               (n * 4 + 0x300)
+#define HDMI_CORE_AV_CP_BYTE1                   0x37C
+#define HDMI_CORE_AV_GEN2_DBYTE(n)              (n * 4 + 0x380)
+#define HDMI_CORE_AV_CEC_ADDR_ID                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
+
+#define HDMI_CORE_AV_AVI_DBYTE_NELEMS           15
+#define HDMI_CORE_AV_SPD_DBYTE_NELEMS           27
+#define HDMI_CORE_AV_AUD_DBYTE_NELEMS           10
+#define HDMI_CORE_AV_MPEG_DBYTE_NELEMS          27
+#define HDMI_CORE_AV_GEN_DBYTE_NELEMS           31
+#define HDMI_CORE_AV_GEN2_DBYTE_NELEMS          31
+
+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_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
+};
+
+enum hdmi_audio_i2s_config {
+        HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST = 0,
+        HDMI_AUDIO_I2S_LSB_SHIFTED_FIRST = 1,
+        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_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,
+};
+
+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;
+};
+
+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;
+};
+
+int hdmi4_read_edid(struct hdmi_core_data *core, u8 *edid, int len);
+void hdmi4_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                struct hdmi_config *cfg);
+void hdmi4_core_dump(struct hdmi_core_data *core, struct seq_file *s);
+int hdmi4_core_init(struct platform_device *pdev, struct hdmi_core_data *core);
+
+int hdmi4_audio_start(struct hdmi_core_data *core, struct hdmi_wp_data *wp);
+void hdmi4_audio_stop(struct hdmi_core_data *core, struct hdmi_wp_data *wp);
+int hdmi4_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                struct omap_dss_audio *audio, u32 pclk);
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi5.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5.c
new file mode 100644
index 000000000000..a955a2c4c061
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5.c
@@ -0,0 +1,876 @@
+/*
+ * HDMI driver for OMAP5
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * Authors:
+ *      Yong Zhi
+ *      Mythri pk
+ *      Archit Taneja <archit@ti.com>
+ *      Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/component.h>
+#include <video/omapdss.h>
+#include <sound/omap-hdmi-audio.h>
+
+#include "hdmi5_core.h"
+#include "dss.h"
+#include "dss_features.h"
+
+static struct omap_hdmi hdmi;
+
+static int hdmi_runtime_get(void)
+{
+        int r;
+
+        DSSDBG("hdmi_runtime_get\n");
+
+        r = pm_runtime_get_sync(&hdmi.pdev->dev);
+        WARN_ON(r < 0);
+        if (r < 0)
+                return r;
+
+        return 0;
+}
+
+static void hdmi_runtime_put(void)
+{
+        int r;
+
+        DSSDBG("hdmi_runtime_put\n");
+
+        r = pm_runtime_put_sync(&hdmi.pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *data)
+{
+        struct hdmi_wp_data *wp = data;
+        u32 irqstatus;
+
+        irqstatus = hdmi_wp_get_irqstatus(wp);
+        hdmi_wp_set_irqstatus(wp, irqstatus);
+
+        if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
+                        irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+                u32 v;
+                /*
+                 * If we get both connect and disconnect interrupts at the same
+                 * time, turn off the PHY, clear interrupts, and restart, which
+                 * raises connect interrupt if a cable is connected, or nothing
+                 * if cable is not connected.
+                 */
+
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
+
+                /*
+                 * We always get bogus CONNECT & DISCONNECT interrupts when
+                 * setting the PHY to LDOON. To ignore those, we force the RXDET
+                 * line to 0 until the PHY power state has been changed.
+                 */
+                v = hdmi_read_reg(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL);
+                v = FLD_MOD(v, 1, 15, 15); /* FORCE_RXDET_HIGH */
+                v = FLD_MOD(v, 0, 14, 7); /* RXDET_LINE */
+                hdmi_write_reg(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL, v);
+
+                hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
+                                HDMI_IRQ_LINK_DISCONNECT);
+
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+
+                REG_FLD_MOD(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL, 0, 15, 15);
+
+        } else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
+        } else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+                hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+        }
+
+        return IRQ_HANDLED;
+}
+
+static int hdmi_init_regulator(void)
+{
+        int r;
+        struct regulator *reg;
+
+        if (hdmi.vdda_reg != NULL)
+                return 0;
+
+        reg = devm_regulator_get(&hdmi.pdev->dev, "vdda");
+        if (IS_ERR(reg)) {
+                DSSERR("can't get VDDA regulator\n");
+                return PTR_ERR(reg);
+        }
+
+        if (regulator_can_change_voltage(reg)) {
+                r = regulator_set_voltage(reg, 1800000, 1800000);
+                if (r) {
+                        devm_regulator_put(reg);
+                        DSSWARN("can't set the regulator voltage\n");
+                        return r;
+                }
+        }
+
+        hdmi.vdda_reg = reg;
+
+        return 0;
+}
+
+static int hdmi_power_on_core(struct omap_dss_device *dssdev)
+{
+        int r;
+
+        r = regulator_enable(hdmi.vdda_reg);
+        if (r)
+                return r;
+
+        r = hdmi_runtime_get();
+        if (r)
+                goto err_runtime_get;
+
+        /* Make selection of HDMI in DSS */
+        dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+        hdmi.core_enabled = true;
+
+        return 0;
+
+err_runtime_get:
+        regulator_disable(hdmi.vdda_reg);
+
+        return r;
+}
+
+static void hdmi_power_off_core(struct omap_dss_device *dssdev)
+{
+        hdmi.core_enabled = false;
+
+        hdmi_runtime_put();
+        regulator_disable(hdmi.vdda_reg);
+}
+
+static int hdmi_power_on_full(struct omap_dss_device *dssdev)
+{
+        int r;
+        struct omap_video_timings *p;
+        struct omap_overlay_manager *mgr = hdmi.output.manager;
+        struct dss_pll_clock_info hdmi_cinfo = { 0 };
+
+        r = hdmi_power_on_core(dssdev);
+        if (r)
+                return r;
+
+        p = &hdmi.cfg.timings;
+
+        DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
+
+        hdmi_pll_compute(&hdmi.pll, p->pixelclock, &hdmi_cinfo);
+
+        /* disable and clear irqs */
+        hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+        hdmi_wp_set_irqstatus(&hdmi.wp,
+                        hdmi_wp_get_irqstatus(&hdmi.wp));
+
+        r = dss_pll_enable(&hdmi.pll.pll);
+        if (r) {
+                DSSERR("Failed to enable PLL\n");
+                goto err_pll_enable;
+        }
+
+        r = dss_pll_set_config(&hdmi.pll.pll, &hdmi_cinfo);
+        if (r) {
+                DSSERR("Failed to configure PLL\n");
+                goto err_pll_cfg;
+        }
+
+        r = hdmi_phy_configure(&hdmi.phy, hdmi_cinfo.clkdco,
+                hdmi_cinfo.clkout[0]);
+        if (r) {
+                DSSDBG("Failed to start PHY\n");
+                goto err_phy_cfg;
+        }
+
+        r = hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_LDOON);
+        if (r)
+                goto err_phy_pwr;
+
+        hdmi5_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
+
+        /* bypass TV gamma table */
+        dispc_enable_gamma_table(0);
+
+        /* tv size */
+        dss_mgr_set_timings(mgr, p);
+
+        r = hdmi_wp_video_start(&hdmi.wp);
+        if (r)
+                goto err_vid_enable;
+
+        r = dss_mgr_enable(mgr);
+        if (r)
+                goto err_mgr_enable;
+
+        hdmi_wp_set_irqenable(&hdmi.wp,
+                        HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+
+        return 0;
+
+err_mgr_enable:
+        hdmi_wp_video_stop(&hdmi.wp);
+err_vid_enable:
+        hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+err_phy_pwr:
+err_phy_cfg:
+err_pll_cfg:
+        dss_pll_disable(&hdmi.pll.pll);
+err_pll_enable:
+        hdmi_power_off_core(dssdev);
+        return -EIO;
+}
+
+static void hdmi_power_off_full(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = hdmi.output.manager;
+
+        hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+
+        dss_mgr_disable(mgr);
+
+        hdmi_wp_video_stop(&hdmi.wp);
+
+        hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+
+        dss_pll_disable(&hdmi.pll.pll);
+
+        hdmi_power_off_core(dssdev);
+}
+
+static int hdmi_display_check_timing(struct omap_dss_device *dssdev,
+                                        struct omap_video_timings *timings)
+{
+        struct omap_dss_device *out = &hdmi.output;
+
+        /* TODO: proper interlace support */
+        if (timings->interlace)
+                return -EINVAL;
+
+        if (!dispc_mgr_timings_ok(out->dispc_channel, timings))
+                return -EINVAL;
+
+        return 0;
+}
+
+static void hdmi_display_set_timing(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        mutex_lock(&hdmi.lock);
+
+        hdmi.cfg.timings = *timings;
+
+        dispc_set_tv_pclk(timings->pixelclock);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+static void hdmi_display_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        *timings = hdmi.cfg.timings;
+}
+
+static void hdmi_dump_regs(struct seq_file *s)
+{
+        mutex_lock(&hdmi.lock);
+
+        if (hdmi_runtime_get()) {
+                mutex_unlock(&hdmi.lock);
+                return;
+        }
+
+        hdmi_wp_dump(&hdmi.wp, s);
+        hdmi_pll_dump(&hdmi.pll, s);
+        hdmi_phy_dump(&hdmi.phy, s);
+        hdmi5_core_dump(&hdmi.core, s);
+
+        hdmi_runtime_put();
+        mutex_unlock(&hdmi.lock);
+}
+
+static int read_edid(u8 *buf, int len)
+{
+        int r;
+        int idlemode;
+
+        mutex_lock(&hdmi.lock);
+
+        r = hdmi_runtime_get();
+        BUG_ON(r);
+
+        idlemode = REG_GET(hdmi.wp.base, HDMI_WP_SYSCONFIG, 3, 2);
+        /* No-idle mode */
+        REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, 1, 3, 2);
+
+        r = hdmi5_read_edid(&hdmi.core,  buf, len);
+
+        REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, idlemode, 3, 2);
+
+        hdmi_runtime_put();
+        mutex_unlock(&hdmi.lock);
+
+        return r;
+}
+
+static void hdmi_start_audio_stream(struct omap_hdmi *hd)
+{
+        REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, 1, 3, 2);
+        hdmi_wp_audio_enable(&hd->wp, true);
+        hdmi_wp_audio_core_req_enable(&hd->wp, true);
+}
+
+static void hdmi_stop_audio_stream(struct omap_hdmi *hd)
+{
+        hdmi_wp_audio_core_req_enable(&hd->wp, false);
+        hdmi_wp_audio_enable(&hd->wp, false);
+        REG_FLD_MOD(hd->wp.base, HDMI_WP_SYSCONFIG, hd->wp_idlemode, 3, 2);
+}
+
+static int hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out = &hdmi.output;
+        unsigned long flags;
+        int r = 0;
+
+        DSSDBG("ENTER hdmi_display_enable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        if (out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                r = -ENODEV;
+                goto err0;
+        }
+
+        r = hdmi_power_on_full(dssdev);
+        if (r) {
+                DSSERR("failed to power on device\n");
+                goto err0;
+        }
+
+        if (hdmi.audio_configured) {
+                r = hdmi5_audio_config(&hdmi.core, &hdmi.wp, &hdmi.audio_config,
+                                       hdmi.cfg.timings.pixelclock);
+                if (r) {
+                        DSSERR("Error restoring audio configuration: %d", r);
+                        hdmi.audio_abort_cb(&hdmi.pdev->dev);
+                        hdmi.audio_configured = false;
+                }
+        }
+
+        spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+        if (hdmi.audio_configured && hdmi.audio_playing)
+                hdmi_start_audio_stream(&hdmi);
+        hdmi.display_enabled = true;
+        spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+        mutex_unlock(&hdmi.lock);
+        return 0;
+
+err0:
+        mutex_unlock(&hdmi.lock);
+        return r;
+}
+
+static void hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+        unsigned long flags;
+
+        DSSDBG("Enter hdmi_display_disable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+        hdmi_stop_audio_stream(&hdmi);
+        hdmi.display_enabled = false;
+        spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+        hdmi_power_off_full(dssdev);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_core_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        DSSDBG("ENTER omapdss_hdmi_core_enable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        r = hdmi_power_on_core(dssdev);
+        if (r) {
+                DSSERR("failed to power on device\n");
+                goto err0;
+        }
+
+        mutex_unlock(&hdmi.lock);
+        return 0;
+
+err0:
+        mutex_unlock(&hdmi.lock);
+        return r;
+}
+
+static void hdmi_core_disable(struct omap_dss_device *dssdev)
+{
+        DSSDBG("Enter omapdss_hdmi_core_disable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        hdmi_power_off_core(dssdev);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct omap_overlay_manager *mgr;
+        int r;
+
+        r = hdmi_init_regulator();
+        if (r)
+                return r;
+
+        mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+        if (!mgr)
+                return -ENODEV;
+
+        r = dss_mgr_connect(mgr, dssdev);
+        if (r)
+                return r;
+
+        r = omapdss_output_set_device(dssdev, dst);
+        if (r) {
+                DSSERR("failed to connect output to new device: %s\n",
+                                dst->name);
+                dss_mgr_disconnect(mgr, dssdev);
+                return r;
+        }
+
+        return 0;
+}
+
+static void hdmi_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        omapdss_output_unset_device(dssdev);
+
+        if (dssdev->manager)
+                dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static int hdmi_read_edid(struct omap_dss_device *dssdev,
+                u8 *edid, int len)
+{
+        bool need_enable;
+        int r;
+
+        need_enable = hdmi.core_enabled == false;
+
+        if (need_enable) {
+                r = hdmi_core_enable(dssdev);
+                if (r)
+                        return r;
+        }
+
+        r = read_edid(edid, len);
+
+        if (need_enable)
+                hdmi_core_disable(dssdev);
+
+        return r;
+}
+
+static int hdmi_set_infoframe(struct omap_dss_device *dssdev,
+                const struct hdmi_avi_infoframe *avi)
+{
+        hdmi.cfg.infoframe = *avi;
+        return 0;
+}
+
+static int hdmi_set_hdmi_mode(struct omap_dss_device *dssdev,
+                bool hdmi_mode)
+{
+        hdmi.cfg.hdmi_dvi_mode = hdmi_mode ? HDMI_HDMI : HDMI_DVI;
+        return 0;
+}
+
+static const struct omapdss_hdmi_ops hdmi_ops = {
+        .connect                = hdmi_connect,
+        .disconnect             = hdmi_disconnect,
+
+        .enable                 = hdmi_display_enable,
+        .disable                = hdmi_display_disable,
+
+        .check_timings          = hdmi_display_check_timing,
+        .set_timings            = hdmi_display_set_timing,
+        .get_timings            = hdmi_display_get_timings,
+
+        .read_edid              = hdmi_read_edid,
+        .set_infoframe          = hdmi_set_infoframe,
+        .set_hdmi_mode          = hdmi_set_hdmi_mode,
+};
+
+static void hdmi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &hdmi.output;
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_HDMI;
+        out->output_type = OMAP_DISPLAY_TYPE_HDMI;
+        out->name = "hdmi.0";
+        out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+        out->ops.hdmi = &hdmi_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void hdmi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &hdmi.output;
+
+        omapdss_unregister_output(out);
+}
+
+static int hdmi_probe_of(struct platform_device *pdev)
+{
+        struct device_node *node = pdev->dev.of_node;
+        struct device_node *ep;
+        int r;
+
+        ep = omapdss_of_get_first_endpoint(node);
+        if (!ep)
+                return 0;
+
+        r = hdmi_parse_lanes_of(pdev, ep, &hdmi.phy);
+        if (r)
+                goto err;
+
+        of_node_put(ep);
+        return 0;
+
+err:
+        of_node_put(ep);
+        return r;
+}
+
+/* Audio callbacks */
+static int hdmi_audio_startup(struct device *dev,
+                              void (*abort_cb)(struct device *dev))
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        int ret = 0;
+
+        mutex_lock(&hd->lock);
+
+        if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+                ret = -EPERM;
+                goto out;
+        }
+
+        hd->audio_abort_cb = abort_cb;
+
+out:
+        mutex_unlock(&hd->lock);
+
+        return ret;
+}
+
+static int hdmi_audio_shutdown(struct device *dev)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+
+        mutex_lock(&hd->lock);
+        hd->audio_abort_cb = NULL;
+        hd->audio_configured = false;
+        hd->audio_playing = false;
+        mutex_unlock(&hd->lock);
+
+        return 0;
+}
+
+static int hdmi_audio_start(struct device *dev)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        unsigned long flags;
+
+        WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+        spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+        if (hd->display_enabled)
+                hdmi_start_audio_stream(hd);
+        hd->audio_playing = true;
+
+        spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+        return 0;
+}
+
+static void hdmi_audio_stop(struct device *dev)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        unsigned long flags;
+
+        WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+        spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+        if (hd->display_enabled)
+                hdmi_stop_audio_stream(hd);
+        hd->audio_playing = false;
+
+        spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+}
+
+static int hdmi_audio_config(struct device *dev,
+                             struct omap_dss_audio *dss_audio)
+{
+        struct omap_hdmi *hd = dev_get_drvdata(dev);
+        int ret;
+
+        mutex_lock(&hd->lock);
+
+        if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+                ret = -EPERM;
+                goto out;
+        }
+
+        ret = hdmi5_audio_config(&hd->core, &hd->wp, dss_audio,
+                                 hd->cfg.timings.pixelclock);
+
+        if (!ret) {
+                hd->audio_configured = true;
+                hd->audio_config = *dss_audio;
+        }
+out:
+        mutex_unlock(&hd->lock);
+
+        return ret;
+}
+
+static const struct omap_hdmi_audio_ops hdmi_audio_ops = {
+        .audio_startup = hdmi_audio_startup,
+        .audio_shutdown = hdmi_audio_shutdown,
+        .audio_start = hdmi_audio_start,
+        .audio_stop = hdmi_audio_stop,
+        .audio_config = hdmi_audio_config,
+};
+
+static int hdmi_audio_register(struct device *dev)
+{
+        struct omap_hdmi_audio_pdata pdata = {
+                .dev = dev,
+                .dss_version = omapdss_get_version(),
+                .audio_dma_addr = hdmi_wp_get_audio_dma_addr(&hdmi.wp),
+                .ops = &hdmi_audio_ops,
+        };
+
+        hdmi.audio_pdev = platform_device_register_data(
+                dev, "omap-hdmi-audio", PLATFORM_DEVID_AUTO,
+                &pdata, sizeof(pdata));
+
+        if (IS_ERR(hdmi.audio_pdev))
+                return PTR_ERR(hdmi.audio_pdev);
+
+        hdmi_runtime_get();
+        hdmi.wp_idlemode =
+                REG_GET(hdmi.wp.base, HDMI_WP_SYSCONFIG, 3, 2);
+        hdmi_runtime_put();
+
+        return 0;
+}
+
+/* HDMI HW IP initialisation */
+static int hdmi5_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        int r;
+        int irq;
+
+        hdmi.pdev = pdev;
+        dev_set_drvdata(&pdev->dev, &hdmi);
+
+        mutex_init(&hdmi.lock);
+        spin_lock_init(&hdmi.audio_playing_lock);
+
+        if (pdev->dev.of_node) {
+                r = hdmi_probe_of(pdev);
+                if (r)
+                        return r;
+        }
+
+        r = hdmi_wp_init(pdev, &hdmi.wp);
+        if (r)
+                return r;
+
+        r = hdmi_pll_init(pdev, &hdmi.pll, &hdmi.wp);
+        if (r)
+                return r;
+
+        r = hdmi_phy_init(pdev, &hdmi.phy);
+        if (r)
+                goto err;
+
+        r = hdmi5_core_init(pdev, &hdmi.core);
+        if (r)
+                goto err;
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                r = -ENODEV;
+                goto err;
+        }
+
+        r = devm_request_threaded_irq(&pdev->dev, irq,
+                        NULL, hdmi_irq_handler,
+                        IRQF_ONESHOT, "OMAP HDMI", &hdmi.wp);
+        if (r) {
+                DSSERR("HDMI IRQ request failed\n");
+                goto err;
+        }
+
+        pm_runtime_enable(&pdev->dev);
+
+        hdmi_init_output(pdev);
+
+        r = hdmi_audio_register(&pdev->dev);
+        if (r) {
+                DSSERR("Registering HDMI audio failed %d\n", r);
+                hdmi_uninit_output(pdev);
+                pm_runtime_disable(&pdev->dev);
+                return r;
+        }
+
+        dss_debugfs_create_file("hdmi", hdmi_dump_regs);
+
+        return 0;
+err:
+        hdmi_pll_uninit(&hdmi.pll);
+        return r;
+}
+
+static void hdmi5_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        if (hdmi.audio_pdev)
+                platform_device_unregister(hdmi.audio_pdev);
+
+        hdmi_uninit_output(pdev);
+
+        hdmi_pll_uninit(&hdmi.pll);
+
+        pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops hdmi5_component_ops = {
+        .bind   = hdmi5_bind,
+        .unbind = hdmi5_unbind,
+};
+
+static int hdmi5_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &hdmi5_component_ops);
+}
+
+static int hdmi5_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &hdmi5_component_ops);
+        return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+        dispc_runtime_put();
+
+        return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+        int r;
+
+        r = dispc_runtime_get();
+        if (r < 0)
+                return r;
+
+        return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+        .runtime_suspend = hdmi_runtime_suspend,
+        .runtime_resume = hdmi_runtime_resume,
+};
+
+static const struct of_device_id hdmi_of_match[] = {
+        { .compatible = "ti,omap5-hdmi", },
+        { .compatible = "ti,dra7-hdmi", },
+        {},
+};
+
+static struct platform_driver omapdss_hdmihw_driver = {
+        .probe          = hdmi5_probe,
+        .remove         = hdmi5_remove,
+        .driver         = {
+                .name   = "omapdss_hdmi5",
+                .pm     = &hdmi_pm_ops,
+                .of_match_table = hdmi_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init hdmi5_init_platform_driver(void)
+{
+        return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi5_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.c
new file mode 100644
index 000000000000..8ea531d2652c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.c
@@ -0,0 +1,916 @@
+/*
+ * OMAP5 HDMI CORE IP driver library
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * Authors:
+ *      Yong Zhi
+ *      Mythri pk
+ *      Archit Taneja <archit@ti.com>
+ *      Tomi Valkeinen <tomi.valkeinen@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/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <drm/drm_edid.h>
+#include <sound/asound.h>
+#include <sound/asoundef.h>
+
+#include "hdmi5_core.h"
+
+/* only 24 bit color depth used for now */
+static const struct csc_table csc_table_deepcolor[] = {
+        /* HDMI_DEEP_COLOR_24BIT */
+        [0] = { 7036, 0, 0, 32, 0, 7036, 0, 32, 0, 0, 7036, 32, },
+        /* HDMI_DEEP_COLOR_30BIT */
+        [1] = { 7015, 0, 0, 128, 0, 7015, 0, 128, 0, 0, 7015, 128, },
+        /* HDMI_DEEP_COLOR_36BIT */
+        [2] = { 7010, 0, 0, 512, 0, 7010, 0, 512, 0, 0, 7010, 512, },
+        /* FULL RANGE */
+        [3] = { 8192, 0, 0, 0, 0, 8192, 0, 0, 0, 0, 8192, 0, },
+};
+
+static void hdmi_core_ddc_init(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+        const unsigned long long iclk = 266000000;      /* DSS L3 ICLK */
+        const unsigned ss_scl_high = 4000;              /* ns */
+        const unsigned ss_scl_low = 4700;               /* ns */
+        const unsigned fs_scl_high = 600;               /* ns */
+        const unsigned fs_scl_low = 1300;               /* ns */
+        const unsigned sda_hold = 1000;                 /* ns */
+        const unsigned sfr_div = 10;
+        unsigned long long sfr;
+        unsigned v;
+
+        sfr = iclk / sfr_div;   /* SFR_DIV */
+        sfr /= 1000;            /* SFR clock in kHz */
+
+        /* Reset */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SOFTRSTZ, 0, 0, 0);
+        if (hdmi_wait_for_bit_change(base, HDMI_CORE_I2CM_SOFTRSTZ,
+                                0, 0, 1) != 1)
+                DSSERR("HDMI I2CM reset failed\n");
+
+        /* Standard (0) or Fast (1) Mode */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_DIV, 0, 3, 3);
+
+        /* Standard Mode SCL High counter */
+        v = DIV_ROUND_UP_ULL(ss_scl_high * sfr, 1000000);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR,
+                        (v >> 8) & 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR,
+                        v & 0xff, 7, 0);
+
+        /* Standard Mode SCL Low counter */
+        v = DIV_ROUND_UP_ULL(ss_scl_low * sfr, 1000000);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR,
+                        (v >> 8) & 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR,
+                        v & 0xff, 7, 0);
+
+        /* Fast Mode SCL High Counter */
+        v = DIV_ROUND_UP_ULL(fs_scl_high * sfr, 1000000);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR,
+                        (v >> 8) & 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR,
+                        v & 0xff, 7, 0);
+
+        /* Fast Mode SCL Low Counter */
+        v = DIV_ROUND_UP_ULL(fs_scl_low * sfr, 1000000);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR,
+                        (v >> 8) & 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR,
+                        v & 0xff, 7, 0);
+
+        /* SDA Hold Time */
+        v = DIV_ROUND_UP_ULL(sda_hold * sfr, 1000000);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SDA_HOLD_ADDR, v & 0xff, 7, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SLAVE, 0x50, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SEGADDR, 0x30, 6, 0);
+
+        /* NACK_POL to high */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 7, 7);
+
+        /* NACK_MASK to unmasked */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x0, 6, 6);
+
+        /* ARBITRATION_POL to high */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 3, 3);
+
+        /* ARBITRATION_MASK to unmasked */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x0, 2, 2);
+
+        /* DONE_POL to high */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 3, 3);
+
+        /* DONE_MASK to unmasked */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x0, 2, 2);
+}
+
+static void hdmi_core_ddc_uninit(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+
+        /* Mask I2C interrupts */
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 6, 6);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 2, 2);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 2, 2);
+}
+
+static int hdmi_core_ddc_edid(struct hdmi_core_data *core, u8 *pedid, u8 ext)
+{
+        void __iomem *base = core->base;
+        u8 cur_addr;
+        char checksum = 0;
+        const int retries = 1000;
+        u8 seg_ptr = ext / 2;
+        u8 edidbase = ((ext % 2) * 0x80);
+
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_SEGPTR, seg_ptr, 7, 0);
+
+        /*
+         * TODO: We use polling here, although we probably should use proper
+         * interrupts.
+         */
+        for (cur_addr = 0; cur_addr < 128; ++cur_addr) {
+                int i;
+
+                /* clear ERROR and DONE */
+                REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0x3, 1, 0);
+
+                REG_FLD_MOD(base, HDMI_CORE_I2CM_ADDRESS,
+                                edidbase + cur_addr, 7, 0);
+
+                if (seg_ptr)
+                        REG_FLD_MOD(base, HDMI_CORE_I2CM_OPERATION, 1, 1, 1);
+                else
+                        REG_FLD_MOD(base, HDMI_CORE_I2CM_OPERATION, 1, 0, 0);
+
+                for (i = 0; i < retries; ++i) {
+                        u32 stat;
+
+                        stat = REG_GET(base, HDMI_CORE_IH_I2CM_STAT0, 1, 0);
+
+                        /* I2CM_ERROR */
+                        if (stat & 1) {
+                                DSSERR("HDMI I2C Master Error\n");
+                                return -EIO;
+                        }
+
+                        /* I2CM_DONE */
+                        if (stat & (1 << 1))
+                                break;
+
+                        usleep_range(250, 1000);
+                }
+
+                if (i == retries) {
+                        DSSERR("HDMI I2C timeout reading EDID\n");
+                        return -EIO;
+                }
+
+                pedid[cur_addr] = REG_GET(base, HDMI_CORE_I2CM_DATAI, 7, 0);
+                checksum += pedid[cur_addr];
+        }
+
+        return 0;
+
+}
+
+int hdmi5_read_edid(struct hdmi_core_data *core, u8 *edid, int len)
+{
+        int r, n, i;
+        int max_ext_blocks = (len / 128) - 1;
+
+        if (len < 128)
+                return -EINVAL;
+
+        hdmi_core_ddc_init(core);
+
+        r = hdmi_core_ddc_edid(core, edid, 0);
+        if (r)
+                goto out;
+
+        n = edid[0x7e];
+
+        if (n > max_ext_blocks)
+                n = max_ext_blocks;
+
+        for (i = 1; i <= n; i++) {
+                r = hdmi_core_ddc_edid(core, edid + i * EDID_LENGTH, i);
+                if (r)
+                        goto out;
+        }
+
+out:
+        hdmi_core_ddc_uninit(core);
+
+        return r ? r : len;
+}
+
+void hdmi5_core_dump(struct hdmi_core_data *core, struct seq_file *s)
+{
+
+#define DUMPCORE(r) seq_printf(s, "%-35s %08x\n", #r,\
+                hdmi_read_reg(core->base, r))
+
+        DUMPCORE(HDMI_CORE_FC_INVIDCONF);
+        DUMPCORE(HDMI_CORE_FC_INHACTIV0);
+        DUMPCORE(HDMI_CORE_FC_INHACTIV1);
+        DUMPCORE(HDMI_CORE_FC_INHBLANK0);
+        DUMPCORE(HDMI_CORE_FC_INHBLANK1);
+        DUMPCORE(HDMI_CORE_FC_INVACTIV0);
+        DUMPCORE(HDMI_CORE_FC_INVACTIV1);
+        DUMPCORE(HDMI_CORE_FC_INVBLANK);
+        DUMPCORE(HDMI_CORE_FC_HSYNCINDELAY0);
+        DUMPCORE(HDMI_CORE_FC_HSYNCINDELAY1);
+        DUMPCORE(HDMI_CORE_FC_HSYNCINWIDTH0);
+        DUMPCORE(HDMI_CORE_FC_HSYNCINWIDTH1);
+        DUMPCORE(HDMI_CORE_FC_VSYNCINDELAY);
+        DUMPCORE(HDMI_CORE_FC_VSYNCINWIDTH);
+        DUMPCORE(HDMI_CORE_FC_CTRLDUR);
+        DUMPCORE(HDMI_CORE_FC_EXCTRLDUR);
+        DUMPCORE(HDMI_CORE_FC_EXCTRLSPAC);
+        DUMPCORE(HDMI_CORE_FC_CH0PREAM);
+        DUMPCORE(HDMI_CORE_FC_CH1PREAM);
+        DUMPCORE(HDMI_CORE_FC_CH2PREAM);
+        DUMPCORE(HDMI_CORE_FC_AVICONF0);
+        DUMPCORE(HDMI_CORE_FC_AVICONF1);
+        DUMPCORE(HDMI_CORE_FC_AVICONF2);
+        DUMPCORE(HDMI_CORE_FC_AVIVID);
+        DUMPCORE(HDMI_CORE_FC_PRCONF);
+
+        DUMPCORE(HDMI_CORE_MC_CLKDIS);
+        DUMPCORE(HDMI_CORE_MC_SWRSTZREQ);
+        DUMPCORE(HDMI_CORE_MC_FLOWCTRL);
+        DUMPCORE(HDMI_CORE_MC_PHYRSTZ);
+        DUMPCORE(HDMI_CORE_MC_LOCKONCLOCK);
+
+        DUMPCORE(HDMI_CORE_I2CM_SLAVE);
+        DUMPCORE(HDMI_CORE_I2CM_ADDRESS);
+        DUMPCORE(HDMI_CORE_I2CM_DATAO);
+        DUMPCORE(HDMI_CORE_I2CM_DATAI);
+        DUMPCORE(HDMI_CORE_I2CM_OPERATION);
+        DUMPCORE(HDMI_CORE_I2CM_INT);
+        DUMPCORE(HDMI_CORE_I2CM_CTLINT);
+        DUMPCORE(HDMI_CORE_I2CM_DIV);
+        DUMPCORE(HDMI_CORE_I2CM_SEGADDR);
+        DUMPCORE(HDMI_CORE_I2CM_SOFTRSTZ);
+        DUMPCORE(HDMI_CORE_I2CM_SEGPTR);
+        DUMPCORE(HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR);
+        DUMPCORE(HDMI_CORE_I2CM_SDA_HOLD_ADDR);
+}
+
+static void hdmi_core_init(struct hdmi_core_vid_config *video_cfg,
+                        struct hdmi_config *cfg)
+{
+        DSSDBG("hdmi_core_init\n");
+
+        /* video core */
+        video_cfg->data_enable_pol = 1; /* It is always 1*/
+        video_cfg->v_fc_config.timings.hsync_level = cfg->timings.hsync_level;
+        video_cfg->v_fc_config.timings.x_res = cfg->timings.x_res;
+        video_cfg->v_fc_config.timings.hsw = cfg->timings.hsw - 1;
+        video_cfg->v_fc_config.timings.hbp = cfg->timings.hbp;
+        video_cfg->v_fc_config.timings.hfp = cfg->timings.hfp;
+        video_cfg->hblank = cfg->timings.hfp +
+                                cfg->timings.hbp + cfg->timings.hsw - 1;
+        video_cfg->v_fc_config.timings.vsync_level = cfg->timings.vsync_level;
+        video_cfg->v_fc_config.timings.y_res = cfg->timings.y_res;
+        video_cfg->v_fc_config.timings.vsw = cfg->timings.vsw;
+        video_cfg->v_fc_config.timings.vfp = cfg->timings.vfp;
+        video_cfg->v_fc_config.timings.vbp = cfg->timings.vbp;
+        video_cfg->vblank_osc = 0; /* Always 0 - need to confirm */
+        video_cfg->vblank = cfg->timings.vsw +
+                                cfg->timings.vfp + cfg->timings.vbp;
+        video_cfg->v_fc_config.hdmi_dvi_mode = cfg->hdmi_dvi_mode;
+        video_cfg->v_fc_config.timings.interlace = cfg->timings.interlace;
+}
+
+/* DSS_HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_data *core,
+                        struct hdmi_core_vid_config *cfg)
+{
+        void __iomem *base = core->base;
+        unsigned char r = 0;
+        bool vsync_pol, hsync_pol;
+
+        vsync_pol =
+                cfg->v_fc_config.timings.vsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+        hsync_pol =
+                cfg->v_fc_config.timings.hsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+
+        /* Set hsync, vsync and data-enable polarity  */
+        r = hdmi_read_reg(base, HDMI_CORE_FC_INVIDCONF);
+        r = FLD_MOD(r, vsync_pol, 6, 6);
+        r = FLD_MOD(r, hsync_pol, 5, 5);
+        r = FLD_MOD(r, cfg->data_enable_pol, 4, 4);
+        r = FLD_MOD(r, cfg->vblank_osc, 1, 1);
+        r = FLD_MOD(r, cfg->v_fc_config.timings.interlace, 0, 0);
+        hdmi_write_reg(base, HDMI_CORE_FC_INVIDCONF, r);
+
+        /* set x resolution */
+        REG_FLD_MOD(base, HDMI_CORE_FC_INHACTIV1,
+                        cfg->v_fc_config.timings.x_res >> 8, 4, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_INHACTIV0,
+                        cfg->v_fc_config.timings.x_res & 0xFF, 7, 0);
+
+        /* set y resolution */
+        REG_FLD_MOD(base, HDMI_CORE_FC_INVACTIV1,
+                        cfg->v_fc_config.timings.y_res >> 8, 4, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_INVACTIV0,
+                        cfg->v_fc_config.timings.y_res & 0xFF, 7, 0);
+
+        /* set horizontal blanking pixels */
+        REG_FLD_MOD(base, HDMI_CORE_FC_INHBLANK1, cfg->hblank >> 8, 4, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_INHBLANK0, cfg->hblank & 0xFF, 7, 0);
+
+        /* set vertial blanking pixels */
+        REG_FLD_MOD(base, HDMI_CORE_FC_INVBLANK, cfg->vblank, 7, 0);
+
+        /* set horizontal sync offset */
+        REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINDELAY1,
+                        cfg->v_fc_config.timings.hfp >> 8, 4, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINDELAY0,
+                        cfg->v_fc_config.timings.hfp & 0xFF, 7, 0);
+
+        /* set vertical sync offset */
+        REG_FLD_MOD(base, HDMI_CORE_FC_VSYNCINDELAY,
+                        cfg->v_fc_config.timings.vfp, 7, 0);
+
+        /* set horizontal sync pulse width */
+        REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINWIDTH1,
+                        (cfg->v_fc_config.timings.hsw >> 8), 1, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINWIDTH0,
+                        cfg->v_fc_config.timings.hsw & 0xFF, 7, 0);
+
+        /*  set vertical sync pulse width */
+        REG_FLD_MOD(base, HDMI_CORE_FC_VSYNCINWIDTH,
+                        cfg->v_fc_config.timings.vsw, 5, 0);
+
+        /* select DVI mode */
+        REG_FLD_MOD(base, HDMI_CORE_FC_INVIDCONF,
+                        cfg->v_fc_config.hdmi_dvi_mode, 3, 3);
+}
+
+static void hdmi_core_config_video_packetizer(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+        int clr_depth = 0;      /* 24 bit color depth */
+
+        /* COLOR_DEPTH */
+        REG_FLD_MOD(base, HDMI_CORE_VP_PR_CD, clr_depth, 7, 4);
+        /* BYPASS_EN */
+        REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 0 : 1, 6, 6);
+        /* PP_EN */
+        REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 1 : 0, 5, 5);
+        /* YCC422_EN */
+        REG_FLD_MOD(base, HDMI_CORE_VP_CONF, 0, 3, 3);
+        /* PP_STUFFING */
+        REG_FLD_MOD(base, HDMI_CORE_VP_STUFF, clr_depth ? 1 : 0, 1, 1);
+        /* YCC422_STUFFING */
+        REG_FLD_MOD(base, HDMI_CORE_VP_STUFF, 1, 2, 2);
+        /* OUTPUT_SELECTOR */
+        REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 0 : 2, 1, 0);
+}
+
+static void hdmi_core_config_csc(struct hdmi_core_data *core)
+{
+        int clr_depth = 0;      /* 24 bit color depth */
+
+        /* CSC_COLORDEPTH */
+        REG_FLD_MOD(core->base, HDMI_CORE_CSC_SCALE, clr_depth, 7, 4);
+}
+
+static void hdmi_core_config_video_sampler(struct hdmi_core_data *core)
+{
+        int video_mapping = 1;  /* for 24 bit color depth */
+
+        /* VIDEO_MAPPING */
+        REG_FLD_MOD(core->base, HDMI_CORE_TX_INVID0, video_mapping, 4, 0);
+}
+
+static void hdmi_core_write_avi_infoframe(struct hdmi_core_data *core,
+        struct hdmi_avi_infoframe *frame)
+{
+        void __iomem *base = core->base;
+        u8 data[HDMI_INFOFRAME_SIZE(AVI)];
+        u8 *ptr;
+        unsigned y, a, b, s;
+        unsigned c, m, r;
+        unsigned itc, ec, q, sc;
+        unsigned vic;
+        unsigned yq, cn, pr;
+
+        hdmi_avi_infoframe_pack(frame, data, sizeof(data));
+
+        print_hex_dump_debug("AVI: ", DUMP_PREFIX_NONE, 16, 1, data,
+                HDMI_INFOFRAME_SIZE(AVI), false);
+
+        ptr = data + HDMI_INFOFRAME_HEADER_SIZE;
+
+        y = (ptr[0] >> 5) & 0x3;
+        a = (ptr[0] >> 4) & 0x1;
+        b = (ptr[0] >> 2) & 0x3;
+        s = (ptr[0] >> 0) & 0x3;
+
+        c = (ptr[1] >> 6) & 0x3;
+        m = (ptr[1] >> 4) & 0x3;
+        r = (ptr[1] >> 0) & 0x3;
+
+        itc = (ptr[2] >> 7) & 0x1;
+        ec = (ptr[2] >> 4) & 0x7;
+        q = (ptr[2] >> 2) & 0x3;
+        sc = (ptr[2] >> 0) & 0x3;
+
+        vic = ptr[3];
+
+        yq = (ptr[4] >> 6) & 0x3;
+        cn = (ptr[4] >> 4) & 0x3;
+        pr = (ptr[4] >> 0) & 0xf;
+
+        hdmi_write_reg(base, HDMI_CORE_FC_AVICONF0,
+                (a << 6) | (s << 4) | (b << 2) | (y << 0));
+
+        hdmi_write_reg(base, HDMI_CORE_FC_AVICONF1,
+                (c << 6) | (m << 4) | (r << 0));
+
+        hdmi_write_reg(base, HDMI_CORE_FC_AVICONF2,
+                (itc << 7) | (ec << 4) | (q << 2) | (sc << 0));
+
+        hdmi_write_reg(base, HDMI_CORE_FC_AVIVID, vic);
+
+        hdmi_write_reg(base, HDMI_CORE_FC_AVICONF3,
+                (yq << 2) | (cn << 0));
+
+        REG_FLD_MOD(base, HDMI_CORE_FC_PRCONF, pr, 3, 0);
+}
+
+static void hdmi_core_csc_config(struct hdmi_core_data *core,
+                struct csc_table csc_coeff)
+{
+        void __iomem *base = core->base;
+
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A1_MSB, csc_coeff.a1 >> 8 , 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A1_LSB, csc_coeff.a1, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A2_MSB, csc_coeff.a2 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A2_LSB, csc_coeff.a2, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A3_MSB, csc_coeff.a3 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A3_LSB, csc_coeff.a3, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A4_MSB, csc_coeff.a4 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A4_LSB, csc_coeff.a4, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B1_MSB, csc_coeff.b1 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B1_LSB, csc_coeff.b1, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B2_MSB, csc_coeff.b2 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B2_LSB, csc_coeff.b2, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B3_MSB, csc_coeff.b3 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B3_LSB, csc_coeff.b3, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B4_MSB, csc_coeff.b4 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B4_LSB, csc_coeff.b4, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C1_MSB, csc_coeff.c1 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C1_LSB, csc_coeff.c1, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C2_MSB, csc_coeff.c2 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C2_LSB, csc_coeff.c2, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C3_MSB, csc_coeff.c3 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C3_LSB, csc_coeff.c3, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C4_MSB, csc_coeff.c4 >> 8, 6, 0);
+        REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C4_LSB, csc_coeff.c4, 7, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_MC_FLOWCTRL, 0x1, 0, 0);
+}
+
+static void hdmi_core_configure_range(struct hdmi_core_data *core)
+{
+        struct csc_table csc_coeff = { 0 };
+
+        /* support limited range with 24 bit color depth for now */
+        csc_coeff = csc_table_deepcolor[0];
+
+        hdmi_core_csc_config(core, csc_coeff);
+}
+
+static void hdmi_core_enable_video_path(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+
+        DSSDBG("hdmi_core_enable_video_path\n");
+
+        REG_FLD_MOD(base, HDMI_CORE_FC_CTRLDUR, 0x0C, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_EXCTRLDUR, 0x20, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_EXCTRLSPAC, 0x01, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_CH0PREAM, 0x0B, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_CH1PREAM, 0x16, 5, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_CH2PREAM, 0x21, 5, 0);
+        REG_FLD_MOD(base, HDMI_CORE_MC_CLKDIS, 0x00, 0, 0);
+        REG_FLD_MOD(base, HDMI_CORE_MC_CLKDIS, 0x00, 1, 1);
+}
+
+static void hdmi_core_mask_interrupts(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+
+        /* Master IRQ mask */
+        REG_FLD_MOD(base, HDMI_CORE_IH_MUTE, 0x3, 1, 0);
+
+        /* Mask all the interrupts in HDMI core */
+
+        REG_FLD_MOD(base, HDMI_CORE_VP_MASK, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_MASK0, 0xe7, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_MASK1, 0xfb, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_MASK2, 0x3, 1, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_AUD_INT, 0x3, 3, 2);
+        REG_FLD_MOD(base, HDMI_CORE_AUD_GP_MASK, 0x3, 1, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_CEC_MASK, 0x7f, 6, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 6, 6);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 2, 2);
+        REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 2, 2);
+
+        REG_FLD_MOD(base, HDMI_CORE_PHY_MASK0, 0xf3, 7, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+
+        /* Clear all the current interrupt bits */
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_VP_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT0, 0xe7, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT1, 0xfb, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT2, 0x3, 1, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_AS_STAT0, 0x7, 2, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_CEC_STAT0, 0x7f, 6, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0x3, 1, 0);
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+}
+
+static void hdmi_core_enable_interrupts(struct hdmi_core_data *core)
+{
+        /* Unmute interrupts */
+        REG_FLD_MOD(core->base, HDMI_CORE_IH_MUTE, 0x0, 1, 0);
+}
+
+int hdmi5_core_handle_irqs(struct hdmi_core_data *core)
+{
+        void __iomem *base = core->base;
+
+        REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT1, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT2, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_AS_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_CEC_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_VP_STAT0, 0xff, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_IH_I2CMPHY_STAT0, 0xff, 7, 0);
+
+        return 0;
+}
+
+void hdmi5_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                struct hdmi_config *cfg)
+{
+        struct omap_video_timings video_timing;
+        struct hdmi_video_format video_format;
+        struct hdmi_core_vid_config v_core_cfg;
+
+        hdmi_core_mask_interrupts(core);
+
+        hdmi_core_init(&v_core_cfg, cfg);
+
+        hdmi_wp_init_vid_fmt_timings(&video_format, &video_timing, cfg);
+
+        hdmi_wp_video_config_timing(wp, &video_timing);
+
+        /* video config */
+        video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+        hdmi_wp_video_config_format(wp, &video_format);
+
+        hdmi_wp_video_config_interface(wp, &video_timing);
+
+        /* support limited range with 24 bit color depth for now */
+        hdmi_core_configure_range(core);
+        cfg->infoframe.quantization_range = HDMI_QUANTIZATION_RANGE_LIMITED;
+
+        /*
+         * configure core video part, set software reset in the core
+         */
+        v_core_cfg.packet_mode = HDMI_PACKETMODE24BITPERPIXEL;
+
+        hdmi_core_video_config(core, &v_core_cfg);
+
+        hdmi_core_config_video_packetizer(core);
+        hdmi_core_config_csc(core);
+        hdmi_core_config_video_sampler(core);
+
+        if (cfg->hdmi_dvi_mode == HDMI_HDMI)
+                hdmi_core_write_avi_infoframe(core, &cfg->infoframe);
+
+        hdmi_core_enable_video_path(core);
+
+        hdmi_core_enable_interrupts(core);
+}
+
+static void hdmi5_core_audio_config(struct hdmi_core_data *core,
+                        struct hdmi_core_audio_config *cfg)
+{
+        void __iomem *base = core->base;
+        u8 val;
+
+        /* Mute audio before configuring */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0xf, 7, 4);
+
+        /* Set the N parameter */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_N1, cfg->n, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_AUD_N2, cfg->n >> 8, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_AUD_N3, cfg->n >> 16, 3, 0);
+
+        /*
+         * CTS manual mode. Automatic mode is not supported when using audio
+         * parallel interface.
+         */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_CTS3, 1, 4, 4);
+        REG_FLD_MOD(base, HDMI_CORE_AUD_CTS1, cfg->cts, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_AUD_CTS2, cfg->cts >> 8, 7, 0);
+        REG_FLD_MOD(base, HDMI_CORE_AUD_CTS3, cfg->cts >> 16, 3, 0);
+
+        /* Layout of Audio Sample Packets: 2-channel or multichannels */
+        if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH)
+                REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0, 0, 0);
+        else
+                REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 1, 0, 0);
+
+        /* Configure IEC-609580 Validity bits */
+        /* Channel 0 is valid */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, 0, 0, 0);
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, 0, 4, 4);
+
+        if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH)
+                val = 1;
+        else
+                val = 0;
+
+        /* Channels 1, 2 setting */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 1, 1);
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 5, 5);
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 2, 2);
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 6, 6);
+        /* Channel 3 setting */
+        if (cfg->layout == HDMI_AUDIO_LAYOUT_6CH)
+                val = 1;
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 3, 3);
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 7, 7);
+
+        /* Configure IEC-60958 User bits */
+        /* TODO: should be set by user. */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSU, 0, 7, 0);
+
+        /* Configure IEC-60958 Channel Status word */
+        /* CGMSA */
+        val = cfg->iec60958_cfg->status[5] & IEC958_AES5_CON_CGMSA;
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(0), val, 5, 4);
+
+        /* Copyright */
+        val = (cfg->iec60958_cfg->status[0] &
+                        IEC958_AES0_CON_NOT_COPYRIGHT) >> 2;
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(0), val, 0, 0);
+
+        /* Category */
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(1),
+                cfg->iec60958_cfg->status[1]);
+
+        /* PCM audio mode */
+        val = (cfg->iec60958_cfg->status[0] & IEC958_AES0_CON_MODE) >> 6;
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(2), val, 6, 4);
+
+        /* Source number */
+        val = cfg->iec60958_cfg->status[2] & IEC958_AES2_CON_SOURCE;
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(2), val, 3, 0);
+
+        /* Channel number right 0  */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(3), 2, 3, 0);
+        /* Channel number right 1*/
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(3), 4, 7, 4);
+        /* Channel number right 2  */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(4), 6, 3, 0);
+        /* Channel number right 3*/
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(4), 8, 7, 4);
+        /* Channel number left 0  */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(5), 1, 3, 0);
+        /* Channel number left 1*/
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(5), 3, 7, 4);
+        /* Channel number left 2  */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(6), 5, 3, 0);
+        /* Channel number left 3*/
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(6), 7, 7, 4);
+
+        /* Clock accuracy and sample rate */
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(7),
+                cfg->iec60958_cfg->status[3]);
+
+        /* Original sample rate and word length */
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(8),
+                cfg->iec60958_cfg->status[4]);
+
+        /* Enable FIFO empty and full interrupts */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_INT, 3, 3, 2);
+
+        /* Configure GPA */
+        /* select HBR/SPDIF interfaces */
+        if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH) {
+                /* select HBR/SPDIF interfaces */
+                REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+                /* enable two channels in GPA */
+                REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 3, 7, 0);
+        } else if (cfg->layout == HDMI_AUDIO_LAYOUT_6CH) {
+                /* select HBR/SPDIF interfaces */
+                REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+                /* enable six channels in GPA */
+                REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 0x3F, 7, 0);
+        } else {
+                /* select HBR/SPDIF interfaces */
+                REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+                /* enable eight channels in GPA */
+                REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 0xFF, 7, 0);
+        }
+
+        /* disable HBR */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF2, 0, 0, 0);
+        /* enable PCUV */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF2, 1, 1, 1);
+        /* enable GPA FIFO full and empty mask */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_GP_MASK, 3, 1, 0);
+        /* set polarity of GPA FIFO empty interrupts */
+        REG_FLD_MOD(base, HDMI_CORE_AUD_GP_POL, 1, 0, 0);
+
+        /* unmute audio */
+        REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0, 7, 4);
+}
+
+static void hdmi5_core_audio_infoframe_cfg(struct hdmi_core_data *core,
+         struct snd_cea_861_aud_if *info_aud)
+{
+        void __iomem *base = core->base;
+
+        /* channel count and coding type fields in AUDICONF0 are swapped */
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF0,
+                (info_aud->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CC) << 4 |
+                (info_aud->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CT) >> 4);
+
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF1, info_aud->db2_sf_ss);
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF2, info_aud->db4_ca);
+        hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF3,
+          (info_aud->db5_dminh_lsv & CEA861_AUDIO_INFOFRAME_DB5_DM_INH) >> 3 |
+          (info_aud->db5_dminh_lsv & CEA861_AUDIO_INFOFRAME_DB5_LSV));
+}
+
+int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                        struct omap_dss_audio *audio, u32 pclk)
+{
+        struct hdmi_audio_format audio_format;
+        struct hdmi_audio_dma audio_dma;
+        struct hdmi_core_audio_config core_cfg;
+        int err, n, cts, channel_count;
+        unsigned int fs_nr;
+        bool word_length_16b = false;
+
+        if (!audio || !audio->iec || !audio->cea || !core)
+                return -EINVAL;
+
+        core_cfg.iec60958_cfg = audio->iec;
+
+        if (!(audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24) &&
+                (audio->iec->status[4] & IEC958_AES4_CON_WORDLEN_20_16))
+                        word_length_16b = true;
+
+        /* only 16-bit word length supported atm */
+        if (!word_length_16b)
+                return -EINVAL;
+
+        switch (audio->iec->status[3] & IEC958_AES3_CON_FS) {
+        case IEC958_AES3_CON_FS_32000:
+                fs_nr = 32000;
+                break;
+        case IEC958_AES3_CON_FS_44100:
+                fs_nr = 44100;
+                break;
+        case IEC958_AES3_CON_FS_48000:
+                fs_nr = 48000;
+                break;
+        case IEC958_AES3_CON_FS_88200:
+                fs_nr = 88200;
+                break;
+        case IEC958_AES3_CON_FS_96000:
+                fs_nr = 96000;
+                break;
+        case IEC958_AES3_CON_FS_176400:
+                fs_nr = 176400;
+                break;
+        case IEC958_AES3_CON_FS_192000:
+                fs_nr = 192000;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        err = hdmi_compute_acr(pclk, fs_nr, &n, &cts);
+        core_cfg.n = n;
+        core_cfg.cts = cts;
+
+        /* Audio channels settings */
+        channel_count = (audio->cea->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CC)
+                                + 1;
+
+        if (channel_count == 2)
+                core_cfg.layout = HDMI_AUDIO_LAYOUT_2CH;
+        else if (channel_count == 6)
+                core_cfg.layout = HDMI_AUDIO_LAYOUT_6CH;
+        else
+                core_cfg.layout = HDMI_AUDIO_LAYOUT_8CH;
+
+        /* DMA settings */
+        if (word_length_16b)
+                audio_dma.transfer_size = 0x10;
+        else
+                audio_dma.transfer_size = 0x20;
+        audio_dma.block_size = 0xC0;
+        audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+        audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+        /* audio FIFO format settings for 16-bit samples*/
+        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_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
+
+        /* only LPCM atm */
+        audio_format.type = HDMI_AUDIO_TYPE_LPCM;
+
+        /* only allowed option */
+        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_ON;
+
+        /* configure DMA and audio FIFO format*/
+        hdmi_wp_audio_config_dma(wp, &audio_dma);
+        hdmi_wp_audio_config_format(wp, &audio_format);
+
+        /* configure the core */
+        hdmi5_core_audio_config(core, &core_cfg);
+
+        /* configure CEA 861 audio infoframe */
+        hdmi5_core_audio_infoframe_cfg(core, audio->cea);
+
+        return 0;
+}
+
+int hdmi5_core_init(struct platform_device *pdev, struct hdmi_core_data *core)
+{
+        struct resource *res;
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
+        if (!res) {
+                DSSERR("can't get CORE IORESOURCE_MEM HDMI\n");
+                return -EINVAL;
+        }
+
+        core->base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(core->base)) {
+                DSSERR("can't ioremap HDMI core\n");
+                return PTR_ERR(core->base);
+        }
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.h b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.h
new file mode 100644
index 000000000000..f2f1022c5516
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.h
@@ -0,0 +1,304 @@
+/*
+ * HDMI driver definition for TI OMAP5 processors.
+ *
+ * Copyright (C) 2011-2012 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 _HDMI5_CORE_H_
+#define _HDMI5_CORE_H_
+
+#include "hdmi.h"
+
+/* HDMI IP Core System */
+
+/* HDMI Identification */
+#define HDMI_CORE_DESIGN_ID                     0x00000
+#define HDMI_CORE_REVISION_ID                   0x00004
+#define HDMI_CORE_PRODUCT_ID0                   0x00008
+#define HDMI_CORE_PRODUCT_ID1                   0x0000C
+#define HDMI_CORE_CONFIG0_ID                    0x00010
+#define HDMI_CORE_CONFIG1_ID                    0x00014
+#define HDMI_CORE_CONFIG2_ID                    0x00018
+#define HDMI_CORE_CONFIG3_ID                    0x0001C
+
+/* HDMI Interrupt */
+#define HDMI_CORE_IH_FC_STAT0                   0x00400
+#define HDMI_CORE_IH_FC_STAT1                   0x00404
+#define HDMI_CORE_IH_FC_STAT2                   0x00408
+#define HDMI_CORE_IH_AS_STAT0                   0x0040C
+#define HDMI_CORE_IH_PHY_STAT0                  0x00410
+#define HDMI_CORE_IH_I2CM_STAT0                 0x00414
+#define HDMI_CORE_IH_CEC_STAT0                  0x00418
+#define HDMI_CORE_IH_VP_STAT0                   0x0041C
+#define HDMI_CORE_IH_I2CMPHY_STAT0              0x00420
+#define HDMI_CORE_IH_MUTE                       0x007FC
+
+/* HDMI Video Sampler */
+#define HDMI_CORE_TX_INVID0                     0x00800
+#define HDMI_CORE_TX_INSTUFFING                 0x00804
+#define HDMI_CORE_TX_RGYDATA0                   0x00808
+#define HDMI_CORE_TX_RGYDATA1                   0x0080C
+#define HDMI_CORE_TX_RCRDATA0                   0x00810
+#define HDMI_CORE_TX_RCRDATA1                   0x00814
+#define HDMI_CORE_TX_BCBDATA0                   0x00818
+#define HDMI_CORE_TX_BCBDATA1                   0x0081C
+
+/* HDMI Video Packetizer */
+#define HDMI_CORE_VP_STATUS                     0x02000
+#define HDMI_CORE_VP_PR_CD                      0x02004
+#define HDMI_CORE_VP_STUFF                      0x02008
+#define HDMI_CORE_VP_REMAP                      0x0200C
+#define HDMI_CORE_VP_CONF                       0x02010
+#define HDMI_CORE_VP_STAT                       0x02014
+#define HDMI_CORE_VP_INT                        0x02018
+#define HDMI_CORE_VP_MASK                       0x0201C
+#define HDMI_CORE_VP_POL                        0x02020
+
+/* Frame Composer */
+#define HDMI_CORE_FC_INVIDCONF                  0x04000
+#define HDMI_CORE_FC_INHACTIV0                  0x04004
+#define HDMI_CORE_FC_INHACTIV1                  0x04008
+#define HDMI_CORE_FC_INHBLANK0                  0x0400C
+#define HDMI_CORE_FC_INHBLANK1                  0x04010
+#define HDMI_CORE_FC_INVACTIV0                  0x04014
+#define HDMI_CORE_FC_INVACTIV1                  0x04018
+#define HDMI_CORE_FC_INVBLANK                   0x0401C
+#define HDMI_CORE_FC_HSYNCINDELAY0              0x04020
+#define HDMI_CORE_FC_HSYNCINDELAY1              0x04024
+#define HDMI_CORE_FC_HSYNCINWIDTH0              0x04028
+#define HDMI_CORE_FC_HSYNCINWIDTH1              0x0402C
+#define HDMI_CORE_FC_VSYNCINDELAY               0x04030
+#define HDMI_CORE_FC_VSYNCINWIDTH               0x04034
+#define HDMI_CORE_FC_INFREQ0                    0x04038
+#define HDMI_CORE_FC_INFREQ1                    0x0403C
+#define HDMI_CORE_FC_INFREQ2                    0x04040
+#define HDMI_CORE_FC_CTRLDUR                    0x04044
+#define HDMI_CORE_FC_EXCTRLDUR                  0x04048
+#define HDMI_CORE_FC_EXCTRLSPAC                 0x0404C
+#define HDMI_CORE_FC_CH0PREAM                   0x04050
+#define HDMI_CORE_FC_CH1PREAM                   0x04054
+#define HDMI_CORE_FC_CH2PREAM                   0x04058
+#define HDMI_CORE_FC_AVICONF3                   0x0405C
+#define HDMI_CORE_FC_GCP                        0x04060
+#define HDMI_CORE_FC_AVICONF0                   0x04064
+#define HDMI_CORE_FC_AVICONF1                   0x04068
+#define HDMI_CORE_FC_AVICONF2                   0x0406C
+#define HDMI_CORE_FC_AVIVID                     0x04070
+#define HDMI_CORE_FC_AVIETB0                    0x04074
+#define HDMI_CORE_FC_AVIETB1                    0x04078
+#define HDMI_CORE_FC_AVISBB0                    0x0407C
+#define HDMI_CORE_FC_AVISBB1                    0x04080
+#define HDMI_CORE_FC_AVIELB0                    0x04084
+#define HDMI_CORE_FC_AVIELB1                    0x04088
+#define HDMI_CORE_FC_AVISRB0                    0x0408C
+#define HDMI_CORE_FC_AVISRB1                    0x04090
+#define HDMI_CORE_FC_AUDICONF0                  0x04094
+#define HDMI_CORE_FC_AUDICONF1                  0x04098
+#define HDMI_CORE_FC_AUDICONF2                  0x0409C
+#define HDMI_CORE_FC_AUDICONF3                  0x040A0
+#define HDMI_CORE_FC_VSDIEEEID0                 0x040A4
+#define HDMI_CORE_FC_VSDSIZE                    0x040A8
+#define HDMI_CORE_FC_VSDIEEEID1                 0x040C0
+#define HDMI_CORE_FC_VSDIEEEID2                 0x040C4
+#define HDMI_CORE_FC_VSDPAYLOAD(n)              (n * 4 + 0x040C8)
+#define HDMI_CORE_FC_SPDVENDORNAME(n)           (n * 4 + 0x04128)
+#define HDMI_CORE_FC_SPDPRODUCTNAME(n)          (n * 4 + 0x04148)
+#define HDMI_CORE_FC_SPDDEVICEINF               0x04188
+#define HDMI_CORE_FC_AUDSCONF                   0x0418C
+#define HDMI_CORE_FC_AUDSSTAT                   0x04190
+#define HDMI_CORE_FC_AUDSV                      0x04194
+#define HDMI_CORE_FC_AUDSU                      0x04198
+#define HDMI_CORE_FC_AUDSCHNLS(n)               (n * 4 + 0x0419C)
+#define HDMI_CORE_FC_CTRLQHIGH                  0x041CC
+#define HDMI_CORE_FC_CTRLQLOW                   0x041D0
+#define HDMI_CORE_FC_ACP0                       0x041D4
+#define HDMI_CORE_FC_ACP(n)                     ((16-n) * 4 + 0x04208)
+#define HDMI_CORE_FC_ISCR1_0                    0x04248
+#define HDMI_CORE_FC_ISCR1(n)                   ((16-n) * 4 + 0x0424C)
+#define HDMI_CORE_FC_ISCR2(n)                   ((15-n) * 4 + 0x0428C)
+#define HDMI_CORE_FC_DATAUTO0                   0x042CC
+#define HDMI_CORE_FC_DATAUTO1                   0x042D0
+#define HDMI_CORE_FC_DATAUTO2                   0x042D4
+#define HDMI_CORE_FC_DATMAN                     0x042D8
+#define HDMI_CORE_FC_DATAUTO3                   0x042DC
+#define HDMI_CORE_FC_RDRB(n)                    (n * 4 + 0x042E0)
+#define HDMI_CORE_FC_STAT0                      0x04340
+#define HDMI_CORE_FC_INT0                       0x04344
+#define HDMI_CORE_FC_MASK0                      0x04348
+#define HDMI_CORE_FC_POL0                       0x0434C
+#define HDMI_CORE_FC_STAT1                      0x04350
+#define HDMI_CORE_FC_INT1                       0x04354
+#define HDMI_CORE_FC_MASK1                      0x04358
+#define HDMI_CORE_FC_POL1                       0x0435C
+#define HDMI_CORE_FC_STAT2                      0x04360
+#define HDMI_CORE_FC_INT2                       0x04364
+#define HDMI_CORE_FC_MASK2                      0x04368
+#define HDMI_CORE_FC_POL2                       0x0436C
+#define HDMI_CORE_FC_PRCONF                     0x04380
+#define HDMI_CORE_FC_GMD_STAT                   0x04400
+#define HDMI_CORE_FC_GMD_EN                     0x04404
+#define HDMI_CORE_FC_GMD_UP                     0x04408
+#define HDMI_CORE_FC_GMD_CONF                   0x0440C
+#define HDMI_CORE_FC_GMD_HB                     0x04410
+#define HDMI_CORE_FC_GMD_PB(n)                  (n * 4 + 0x04414)
+#define HDMI_CORE_FC_DBGFORCE                   0x04800
+#define HDMI_CORE_FC_DBGAUD0CH0                 0x04804
+#define HDMI_CORE_FC_DBGAUD1CH0                 0x04808
+#define HDMI_CORE_FC_DBGAUD2CH0                 0x0480C
+#define HDMI_CORE_FC_DBGAUD0CH1                 0x04810
+#define HDMI_CORE_FC_DBGAUD1CH1                 0x04814
+#define HDMI_CORE_FC_DBGAUD2CH1                 0x04818
+#define HDMI_CORE_FC_DBGAUD0CH2                 0x0481C
+#define HDMI_CORE_FC_DBGAUD1CH2                 0x04820
+#define HDMI_CORE_FC_DBGAUD2CH2                 0x04824
+#define HDMI_CORE_FC_DBGAUD0CH3                 0x04828
+#define HDMI_CORE_FC_DBGAUD1CH3                 0x0482C
+#define HDMI_CORE_FC_DBGAUD2CH3                 0x04830
+#define HDMI_CORE_FC_DBGAUD0CH4                 0x04834
+#define HDMI_CORE_FC_DBGAUD1CH4                 0x04838
+#define HDMI_CORE_FC_DBGAUD2CH4                 0x0483C
+#define HDMI_CORE_FC_DBGAUD0CH5                 0x04840
+#define HDMI_CORE_FC_DBGAUD1CH5                 0x04844
+#define HDMI_CORE_FC_DBGAUD2CH5                 0x04848
+#define HDMI_CORE_FC_DBGAUD0CH6                 0x0484C
+#define HDMI_CORE_FC_DBGAUD1CH6                 0x04850
+#define HDMI_CORE_FC_DBGAUD2CH6                 0x04854
+#define HDMI_CORE_FC_DBGAUD0CH7                 0x04858
+#define HDMI_CORE_FC_DBGAUD1CH7                 0x0485C
+#define HDMI_CORE_FC_DBGAUD2CH7                 0x04860
+#define HDMI_CORE_FC_DBGTMDS0                   0x04864
+#define HDMI_CORE_FC_DBGTMDS1                   0x04868
+#define HDMI_CORE_FC_DBGTMDS2                   0x0486C
+#define HDMI_CORE_PHY_MASK0                     0x0C018
+#define HDMI_CORE_PHY_I2CM_INT_ADDR             0x0C09C
+#define HDMI_CORE_PHY_I2CM_CTLINT_ADDR          0x0C0A0
+
+/* HDMI Audio */
+#define HDMI_CORE_AUD_CONF0                     0x0C400
+#define HDMI_CORE_AUD_CONF1                     0x0C404
+#define HDMI_CORE_AUD_INT                       0x0C408
+#define HDMI_CORE_AUD_N1                        0x0C800
+#define HDMI_CORE_AUD_N2                        0x0C804
+#define HDMI_CORE_AUD_N3                        0x0C808
+#define HDMI_CORE_AUD_CTS1                      0x0C80C
+#define HDMI_CORE_AUD_CTS2                      0x0C810
+#define HDMI_CORE_AUD_CTS3                      0x0C814
+#define HDMI_CORE_AUD_INCLKFS                   0x0C818
+#define HDMI_CORE_AUD_CC08                      0x0CC08
+#define HDMI_CORE_AUD_GP_CONF0                  0x0D400
+#define HDMI_CORE_AUD_GP_CONF1                  0x0D404
+#define HDMI_CORE_AUD_GP_CONF2                  0x0D408
+#define HDMI_CORE_AUD_D010                      0x0D010
+#define HDMI_CORE_AUD_GP_STAT                   0x0D40C
+#define HDMI_CORE_AUD_GP_INT                    0x0D410
+#define HDMI_CORE_AUD_GP_POL                    0x0D414
+#define HDMI_CORE_AUD_GP_MASK                   0x0D418
+
+/* HDMI Main Controller */
+#define HDMI_CORE_MC_CLKDIS                     0x10004
+#define HDMI_CORE_MC_SWRSTZREQ                  0x10008
+#define HDMI_CORE_MC_FLOWCTRL                   0x10010
+#define HDMI_CORE_MC_PHYRSTZ                    0x10014
+#define HDMI_CORE_MC_LOCKONCLOCK                0x10018
+
+/* HDMI COLOR SPACE CONVERTER */
+#define HDMI_CORE_CSC_CFG                       0x10400
+#define HDMI_CORE_CSC_SCALE                     0x10404
+#define HDMI_CORE_CSC_COEF_A1_MSB               0x10408
+#define HDMI_CORE_CSC_COEF_A1_LSB               0x1040C
+#define HDMI_CORE_CSC_COEF_A2_MSB               0x10410
+#define HDMI_CORE_CSC_COEF_A2_LSB               0x10414
+#define HDMI_CORE_CSC_COEF_A3_MSB               0x10418
+#define HDMI_CORE_CSC_COEF_A3_LSB               0x1041C
+#define HDMI_CORE_CSC_COEF_A4_MSB               0x10420
+#define HDMI_CORE_CSC_COEF_A4_LSB               0x10424
+#define HDMI_CORE_CSC_COEF_B1_MSB               0x10428
+#define HDMI_CORE_CSC_COEF_B1_LSB               0x1042C
+#define HDMI_CORE_CSC_COEF_B2_MSB               0x10430
+#define HDMI_CORE_CSC_COEF_B2_LSB               0x10434
+#define HDMI_CORE_CSC_COEF_B3_MSB               0x10438
+#define HDMI_CORE_CSC_COEF_B3_LSB               0x1043C
+#define HDMI_CORE_CSC_COEF_B4_MSB               0x10440
+#define HDMI_CORE_CSC_COEF_B4_LSB               0x10444
+#define HDMI_CORE_CSC_COEF_C1_MSB               0x10448
+#define HDMI_CORE_CSC_COEF_C1_LSB               0x1044C
+#define HDMI_CORE_CSC_COEF_C2_MSB               0x10450
+#define HDMI_CORE_CSC_COEF_C2_LSB               0x10454
+#define HDMI_CORE_CSC_COEF_C3_MSB               0x10458
+#define HDMI_CORE_CSC_COEF_C3_LSB               0x1045C
+#define HDMI_CORE_CSC_COEF_C4_MSB               0x10460
+#define HDMI_CORE_CSC_COEF_C4_LSB               0x10464
+
+/* HDMI HDCP */
+#define HDMI_CORE_HDCP_MASK                     0x14020
+
+/* HDMI CEC */
+#define HDMI_CORE_CEC_MASK                      0x17408
+
+/* HDMI I2C Master */
+#define HDMI_CORE_I2CM_SLAVE                    0x157C8
+#define HDMI_CORE_I2CM_ADDRESS                  0x157CC
+#define HDMI_CORE_I2CM_DATAO                    0x157D0
+#define HDMI_CORE_I2CM_DATAI                    0X157D4
+#define HDMI_CORE_I2CM_OPERATION                0x157D8
+#define HDMI_CORE_I2CM_INT                      0x157DC
+#define HDMI_CORE_I2CM_CTLINT                   0x157E0
+#define HDMI_CORE_I2CM_DIV                      0x157E4
+#define HDMI_CORE_I2CM_SEGADDR                  0x157E8
+#define HDMI_CORE_I2CM_SOFTRSTZ                 0x157EC
+#define HDMI_CORE_I2CM_SEGPTR                   0x157F0
+#define HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR       0x157F4
+#define HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR       0x157F8
+#define HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR       0x157FC
+#define HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR       0x15800
+#define HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR       0x15804
+#define HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR       0x15808
+#define HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR       0x1580C
+#define HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR       0x15810
+#define HDMI_CORE_I2CM_SDA_HOLD_ADDR            0x15814
+
+enum hdmi_core_packet_mode {
+        HDMI_PACKETMODERESERVEDVALUE = 0,
+        HDMI_PACKETMODE24BITPERPIXEL = 4,
+        HDMI_PACKETMODE30BITPERPIXEL = 5,
+        HDMI_PACKETMODE36BITPERPIXEL = 6,
+        HDMI_PACKETMODE48BITPERPIXEL = 7,
+};
+
+struct hdmi_core_vid_config {
+        struct hdmi_config v_fc_config;
+        enum hdmi_core_packet_mode packet_mode;
+        int data_enable_pol;
+        int vblank_osc;
+        int hblank;
+        int vblank;
+};
+
+struct csc_table {
+        u16 a1, a2, a3, a4;
+        u16 b1, b2, b3, b4;
+        u16 c1, c2, c3, c4;
+};
+
+int hdmi5_read_edid(struct hdmi_core_data *core, u8 *edid, int len);
+void hdmi5_core_dump(struct hdmi_core_data *core, struct seq_file *s);
+int hdmi5_core_handle_irqs(struct hdmi_core_data *core);
+void hdmi5_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                        struct hdmi_config *cfg);
+int hdmi5_core_init(struct platform_device *pdev, struct hdmi_core_data *core);
+
+int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+                        struct omap_dss_audio *audio, u32 pclk);
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_common.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_common.c
new file mode 100644
index 000000000000..1b8fcc6c4ba1
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_common.c
@@ -0,0 +1,148 @@
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <video/omapdss.h>
+
+#include "hdmi.h"
+
+int hdmi_parse_lanes_of(struct platform_device *pdev, struct device_node *ep,
+        struct hdmi_phy_data *phy)
+{
+        struct property *prop;
+        int r, len;
+
+        prop = of_find_property(ep, "lanes", &len);
+        if (prop) {
+                u32 lanes[8];
+
+                if (len / sizeof(u32) != ARRAY_SIZE(lanes)) {
+                        dev_err(&pdev->dev, "bad number of lanes\n");
+                        return -EINVAL;
+                }
+
+                r = of_property_read_u32_array(ep, "lanes", lanes,
+                        ARRAY_SIZE(lanes));
+                if (r) {
+                        dev_err(&pdev->dev, "failed to read lane data\n");
+                        return r;
+                }
+
+                r = hdmi_phy_parse_lanes(phy, lanes);
+                if (r) {
+                        dev_err(&pdev->dev, "failed to parse lane data\n");
+                        return r;
+                }
+        } else {
+                static const u32 default_lanes[] = { 0, 1, 2, 3, 4, 5, 6, 7 };
+
+                r = hdmi_phy_parse_lanes(phy, default_lanes);
+                if (WARN_ON(r)) {
+                        dev_err(&pdev->dev, "failed to parse lane data\n");
+                        return r;
+                }
+        }
+
+        return 0;
+}
+
+int hdmi_compute_acr(u32 pclk, u32 sample_freq, u32 *n, u32 *cts)
+{
+        u32 deep_color;
+        bool deep_color_correct = false;
+
+        if (n == NULL || cts == NULL)
+                return -EINVAL;
+
+        /* TODO: When implemented, query deep color mode here. */
+        deep_color = 100;
+
+        /*
+         * When using deep color, the default N value (as in the HDMI
+         * specification) yields to an non-integer CTS. Hence, we
+         * modify it while keeping the restrictions described in
+         * section 7.2.1 of the HDMI 1.4a specification.
+         */
+        switch (sample_freq) {
+        case 32000:
+        case 48000:
+        case 96000:
+        case 192000:
+                if (deep_color == 125)
+                        if (pclk == 27027000 || pclk == 74250000)
+                                deep_color_correct = true;
+                if (deep_color == 150)
+                        if (pclk == 27027000)
+                                deep_color_correct = true;
+                break;
+        case 44100:
+        case 88200:
+        case 176400:
+                if (deep_color == 125)
+                        if (pclk == 27027000)
+                                deep_color_correct = true;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        if (deep_color_correct) {
+                switch (sample_freq) {
+                case 32000:
+                        *n = 8192;
+                        break;
+                case 44100:
+                        *n = 12544;
+                        break;
+                case 48000:
+                        *n = 8192;
+                        break;
+                case 88200:
+                        *n = 25088;
+                        break;
+                case 96000:
+                        *n = 16384;
+                        break;
+                case 176400:
+                        *n = 50176;
+                        break;
+                case 192000:
+                        *n = 32768;
+                        break;
+                default:
+                        return -EINVAL;
+                }
+        } else {
+                switch (sample_freq) {
+                case 32000:
+                        *n = 4096;
+                        break;
+                case 44100:
+                        *n = 6272;
+                        break;
+                case 48000:
+                        *n = 6144;
+                        break;
+                case 88200:
+                        *n = 12544;
+                        break;
+                case 96000:
+                        *n = 12288;
+                        break;
+                case 176400:
+                        *n = 25088;
+                        break;
+                case 192000:
+                        *n = 24576;
+                        break;
+                default:
+                        return -EINVAL;
+                }
+        }
+        /* Calculate CTS. See HDMI 1.3a or 1.4a specifications */
+        *cts = (pclk/1000) * (*n / 128) * deep_color / (sample_freq / 10);
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_phy.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_phy.c
new file mode 100644
index 000000000000..1f5d19c119ce
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_phy.c
@@ -0,0 +1,247 @@
+/*
+ * HDMI PHY
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated
+ *
+ * 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/kernel.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+struct hdmi_phy_features {
+        bool bist_ctrl;
+        bool ldo_voltage;
+        unsigned long max_phy;
+};
+
+static const struct hdmi_phy_features *phy_feat;
+
+void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s)
+{
+#define DUMPPHY(r) seq_printf(s, "%-35s %08x\n", #r,\
+                hdmi_read_reg(phy->base, r))
+
+        DUMPPHY(HDMI_TXPHY_TX_CTRL);
+        DUMPPHY(HDMI_TXPHY_DIGITAL_CTRL);
+        DUMPPHY(HDMI_TXPHY_POWER_CTRL);
+        DUMPPHY(HDMI_TXPHY_PAD_CFG_CTRL);
+        if (phy_feat->bist_ctrl)
+                DUMPPHY(HDMI_TXPHY_BIST_CONTROL);
+}
+
+int hdmi_phy_parse_lanes(struct hdmi_phy_data *phy, const u32 *lanes)
+{
+        int i;
+
+        for (i = 0; i < 8; i += 2) {
+                u8 lane, pol;
+                int dx, dy;
+
+                dx = lanes[i];
+                dy = lanes[i + 1];
+
+                if (dx < 0 || dx >= 8)
+                        return -EINVAL;
+
+                if (dy < 0 || dy >= 8)
+                        return -EINVAL;
+
+                if (dx & 1) {
+                        if (dy != dx - 1)
+                                return -EINVAL;
+                        pol = 1;
+                } else {
+                        if (dy != dx + 1)
+                                return -EINVAL;
+                        pol = 0;
+                }
+
+                lane = dx / 2;
+
+                phy->lane_function[lane] = i / 2;
+                phy->lane_polarity[lane] = pol;
+        }
+
+        return 0;
+}
+
+static void hdmi_phy_configure_lanes(struct hdmi_phy_data *phy)
+{
+        static const u16 pad_cfg_list[] = {
+                0x0123,
+                0x0132,
+                0x0312,
+                0x0321,
+                0x0231,
+                0x0213,
+                0x1023,
+                0x1032,
+                0x3012,
+                0x3021,
+                0x2031,
+                0x2013,
+                0x1203,
+                0x1302,
+                0x3102,
+                0x3201,
+                0x2301,
+                0x2103,
+                0x1230,
+                0x1320,
+                0x3120,
+                0x3210,
+                0x2310,
+                0x2130,
+        };
+
+        u16 lane_cfg = 0;
+        int i;
+        unsigned lane_cfg_val;
+        u16 pol_val = 0;
+
+        for (i = 0; i < 4; ++i)
+                lane_cfg |= phy->lane_function[i] << ((3 - i) * 4);
+
+        pol_val |= phy->lane_polarity[0] << 0;
+        pol_val |= phy->lane_polarity[1] << 3;
+        pol_val |= phy->lane_polarity[2] << 2;
+        pol_val |= phy->lane_polarity[3] << 1;
+
+        for (i = 0; i < ARRAY_SIZE(pad_cfg_list); ++i)
+                if (pad_cfg_list[i] == lane_cfg)
+                        break;
+
+        if (WARN_ON(i == ARRAY_SIZE(pad_cfg_list)))
+                i = 0;
+
+        lane_cfg_val = i;
+
+        REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, lane_cfg_val, 26, 22);
+        REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, pol_val, 30, 27);
+}
+
+int hdmi_phy_configure(struct hdmi_phy_data *phy, unsigned long hfbitclk,
+        unsigned long lfbitclk)
+{
+        u8 freqout;
+
+        /*
+         * Read address 0 in order to get the SCP reset done completed
+         * Dummy access performed to make sure reset is done
+         */
+        hdmi_read_reg(phy->base, HDMI_TXPHY_TX_CTRL);
+
+        /*
+         * In OMAP5+, the HFBITCLK must be divided by 2 before issuing the
+         * HDMI_PHYPWRCMD_LDOON command.
+        */
+        if (phy_feat->bist_ctrl)
+                REG_FLD_MOD(phy->base, HDMI_TXPHY_BIST_CONTROL, 1, 11, 11);
+
+        /*
+         * If the hfbitclk != lfbitclk, it means the lfbitclk was configured
+         * to be used for TMDS.
+         */
+        if (hfbitclk != lfbitclk)
+                freqout = 0;
+        else if (hfbitclk / 10 < phy_feat->max_phy)
+                freqout = 1;
+        else
+                freqout = 2;
+
+        /*
+         * Write to phy address 0 to configure the clock
+         * use HFBITCLK write HDMI_TXPHY_TX_CONTROL_FREQOUT field
+         */
+        REG_FLD_MOD(phy->base, HDMI_TXPHY_TX_CTRL, freqout, 31, 30);
+
+        /* Write to phy address 1 to start HDMI line (TXVALID and TMDSCLKEN) */
+        hdmi_write_reg(phy->base, HDMI_TXPHY_DIGITAL_CTRL, 0xF0000000);
+
+        /* Setup max LDO voltage */
+        if (phy_feat->ldo_voltage)
+                REG_FLD_MOD(phy->base, HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
+
+        hdmi_phy_configure_lanes(phy);
+
+        return 0;
+}
+
+static const struct hdmi_phy_features omap44xx_phy_feats = {
+        .bist_ctrl      =       false,
+        .ldo_voltage    =       true,
+        .max_phy        =       185675000,
+};
+
+static const struct hdmi_phy_features omap54xx_phy_feats = {
+        .bist_ctrl      =       true,
+        .ldo_voltage    =       false,
+        .max_phy        =       186000000,
+};
+
+static int hdmi_phy_init_features(struct platform_device *pdev)
+{
+        struct hdmi_phy_features *dst;
+        const struct hdmi_phy_features *src;
+
+        dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+        if (!dst) {
+                dev_err(&pdev->dev, "Failed to allocate HDMI PHY Features\n");
+                return -ENOMEM;
+        }
+
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                src = &omap44xx_phy_feats;
+                break;
+
+        case OMAPDSS_VER_OMAP5:
+        case OMAPDSS_VER_DRA7xx:
+                src = &omap54xx_phy_feats;
+                break;
+
+        default:
+                return -ENODEV;
+        }
+
+        memcpy(dst, src, sizeof(*dst));
+        phy_feat = dst;
+
+        return 0;
+}
+
+int hdmi_phy_init(struct platform_device *pdev, struct hdmi_phy_data *phy)
+{
+        int r;
+        struct resource *res;
+
+        r = hdmi_phy_init_features(pdev);
+        if (r)
+                return r;
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy");
+        if (!res) {
+                DSSERR("can't get PHY mem resource\n");
+                return -EINVAL;
+        }
+
+        phy->base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(phy->base)) {
+                DSSERR("can't ioremap TX PHY\n");
+                return PTR_ERR(phy->base);
+        }
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_pll.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_pll.c
new file mode 100644
index 000000000000..06e23a7c432c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_pll.c
@@ -0,0 +1,255 @@
+/*
+ * HDMI PLL
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated
+ *
+ * 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.
+ */
+
+#define DSS_SUBSYS_NAME "HDMIPLL"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+void hdmi_pll_dump(struct hdmi_pll_data *pll, struct seq_file *s)
+{
+#define DUMPPLL(r) seq_printf(s, "%-35s %08x\n", #r,\
+                hdmi_read_reg(pll->base, r))
+
+        DUMPPLL(PLLCTRL_PLL_CONTROL);
+        DUMPPLL(PLLCTRL_PLL_STATUS);
+        DUMPPLL(PLLCTRL_PLL_GO);
+        DUMPPLL(PLLCTRL_CFG1);
+        DUMPPLL(PLLCTRL_CFG2);
+        DUMPPLL(PLLCTRL_CFG3);
+        DUMPPLL(PLLCTRL_SSC_CFG1);
+        DUMPPLL(PLLCTRL_SSC_CFG2);
+        DUMPPLL(PLLCTRL_CFG4);
+}
+
+void hdmi_pll_compute(struct hdmi_pll_data *pll,
+        unsigned long target_tmds, struct dss_pll_clock_info *pi)
+{
+        unsigned long fint, clkdco, clkout;
+        unsigned long target_bitclk, target_clkdco;
+        unsigned long min_dco;
+        unsigned n, m, mf, m2, sd;
+        unsigned long clkin;
+        const struct dss_pll_hw *hw = pll->pll.hw;
+
+        clkin = clk_get_rate(pll->pll.clkin);
+
+        DSSDBG("clkin %lu, target tmds %lu\n", clkin, target_tmds);
+
+        target_bitclk = target_tmds * 10;
+
+        /* Fint */
+        n = DIV_ROUND_UP(clkin, hw->fint_max);
+        fint = clkin / n;
+
+        /* adjust m2 so that the clkdco will be high enough */
+        min_dco = roundup(hw->clkdco_min, fint);
+        m2 = DIV_ROUND_UP(min_dco, target_bitclk);
+        if (m2 == 0)
+                m2 = 1;
+
+        target_clkdco = target_bitclk * m2;
+        m = target_clkdco / fint;
+
+        clkdco = fint * m;
+
+        /* adjust clkdco with fractional mf */
+        if (WARN_ON(target_clkdco - clkdco > fint))
+                mf = 0;
+        else
+                mf = (u32)div_u64(262144ull * (target_clkdco - clkdco), fint);
+
+        if (mf > 0)
+                clkdco += (u32)div_u64((u64)mf * fint, 262144);
+
+        clkout = clkdco / m2;
+
+        /* sigma-delta */
+        sd = DIV_ROUND_UP(fint * m, 250000000);
+
+        DSSDBG("N = %u, M = %u, M.f = %u, M2 = %u, SD = %u\n",
+                n, m, mf, m2, sd);
+        DSSDBG("Fint %lu, clkdco %lu, clkout %lu\n", fint, clkdco, clkout);
+
+        pi->n = n;
+        pi->m = m;
+        pi->mf = mf;
+        pi->mX[0] = m2;
+        pi->sd = sd;
+
+        pi->fint = fint;
+        pi->clkdco = clkdco;
+        pi->clkout[0] = clkout;
+}
+
+static int hdmi_pll_enable(struct dss_pll *dsspll)
+{
+        struct hdmi_pll_data *pll = container_of(dsspll, struct hdmi_pll_data, pll);
+        struct hdmi_wp_data *wp = pll->wp;
+        u16 r = 0;
+
+        dss_ctrl_pll_enable(DSS_PLL_HDMI, true);
+
+        r = hdmi_wp_set_pll_pwr(wp, HDMI_PLLPWRCMD_BOTHON_ALLCLKS);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void hdmi_pll_disable(struct dss_pll *dsspll)
+{
+        struct hdmi_pll_data *pll = container_of(dsspll, struct hdmi_pll_data, pll);
+        struct hdmi_wp_data *wp = pll->wp;
+
+        hdmi_wp_set_pll_pwr(wp, HDMI_PLLPWRCMD_ALLOFF);
+
+        dss_ctrl_pll_enable(DSS_PLL_HDMI, false);
+}
+
+static const struct dss_pll_ops dsi_pll_ops = {
+        .enable = hdmi_pll_enable,
+        .disable = hdmi_pll_disable,
+        .set_config = dss_pll_write_config_type_b,
+};
+
+static const struct dss_pll_hw dss_omap4_hdmi_pll_hw = {
+        .n_max = 255,
+        .m_min = 20,
+        .m_max = 4095,
+        .mX_max = 127,
+        .fint_min = 500000,
+        .fint_max = 2500000,
+
+        .clkdco_min = 500000000,
+        .clkdco_low = 1000000000,
+        .clkdco_max = 2000000000,
+
+        .n_msb = 8,
+        .n_lsb = 1,
+        .m_msb = 20,
+        .m_lsb = 9,
+
+        .mX_msb[0] = 24,
+        .mX_lsb[0] = 18,
+
+        .has_selfreqdco = true,
+};
+
+static const struct dss_pll_hw dss_omap5_hdmi_pll_hw = {
+        .n_max = 255,
+        .m_min = 20,
+        .m_max = 2045,
+        .mX_max = 127,
+        .fint_min = 620000,
+        .fint_max = 2500000,
+
+        .clkdco_min = 750000000,
+        .clkdco_low = 1500000000,
+        .clkdco_max = 2500000000UL,
+
+        .n_msb = 8,
+        .n_lsb = 1,
+        .m_msb = 20,
+        .m_lsb = 9,
+
+        .mX_msb[0] = 24,
+        .mX_lsb[0] = 18,
+
+        .has_selfreqdco = true,
+        .has_refsel = true,
+};
+
+static int dsi_init_pll_data(struct platform_device *pdev, struct hdmi_pll_data *hpll)
+{
+        struct dss_pll *pll = &hpll->pll;
+        struct clk *clk;
+        int r;
+
+        clk = devm_clk_get(&pdev->dev, "sys_clk");
+        if (IS_ERR(clk)) {
+                DSSERR("can't get sys_clk\n");
+                return PTR_ERR(clk);
+        }
+
+        pll->name = "hdmi";
+        pll->id = DSS_PLL_HDMI;
+        pll->base = hpll->base;
+        pll->clkin = clk;
+
+        switch (omapdss_get_version()) {
+        case OMAPDSS_VER_OMAP4430_ES1:
+        case OMAPDSS_VER_OMAP4430_ES2:
+        case OMAPDSS_VER_OMAP4:
+                pll->hw = &dss_omap4_hdmi_pll_hw;
+                break;
+
+        case OMAPDSS_VER_OMAP5:
+        case OMAPDSS_VER_DRA7xx:
+                pll->hw = &dss_omap5_hdmi_pll_hw;
+                break;
+
+        default:
+                return -ENODEV;
+        }
+
+        pll->ops = &dsi_pll_ops;
+
+        r = dss_pll_register(pll);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+int hdmi_pll_init(struct platform_device *pdev, struct hdmi_pll_data *pll,
+        struct hdmi_wp_data *wp)
+{
+        int r;
+        struct resource *res;
+
+        pll->wp = wp;
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "pll");
+        if (!res) {
+                DSSERR("can't get PLL mem resource\n");
+                return -EINVAL;
+        }
+
+        pll->base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(pll->base)) {
+                DSSERR("can't ioremap PLLCTRL\n");
+                return PTR_ERR(pll->base);
+        }
+
+        r = dsi_init_pll_data(pdev, pll);
+        if (r) {
+                DSSERR("failed to init HDMI PLL\n");
+                return r;
+        }
+
+        return 0;
+}
+
+void hdmi_pll_uninit(struct hdmi_pll_data *hpll)
+{
+        struct dss_pll *pll = &hpll->pll;
+
+        dss_pll_unregister(pll);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_wp.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_wp.c
new file mode 100644
index 000000000000..7c544bc56fb5
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_wp.c
@@ -0,0 +1,282 @@
+/*
+ * HDMI wrapper
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated
+ *
+ * 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.
+ */
+
+#define DSS_SUBSYS_NAME "HDMIWP"
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+void hdmi_wp_dump(struct hdmi_wp_data *wp, struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, hdmi_read_reg(wp->base, r))
+
+        DUMPREG(HDMI_WP_REVISION);
+        DUMPREG(HDMI_WP_SYSCONFIG);
+        DUMPREG(HDMI_WP_IRQSTATUS_RAW);
+        DUMPREG(HDMI_WP_IRQSTATUS);
+        DUMPREG(HDMI_WP_IRQENABLE_SET);
+        DUMPREG(HDMI_WP_IRQENABLE_CLR);
+        DUMPREG(HDMI_WP_IRQWAKEEN);
+        DUMPREG(HDMI_WP_PWR_CTRL);
+        DUMPREG(HDMI_WP_DEBOUNCE);
+        DUMPREG(HDMI_WP_VIDEO_CFG);
+        DUMPREG(HDMI_WP_VIDEO_SIZE);
+        DUMPREG(HDMI_WP_VIDEO_TIMING_H);
+        DUMPREG(HDMI_WP_VIDEO_TIMING_V);
+        DUMPREG(HDMI_WP_CLK);
+        DUMPREG(HDMI_WP_AUDIO_CFG);
+        DUMPREG(HDMI_WP_AUDIO_CFG2);
+        DUMPREG(HDMI_WP_AUDIO_CTRL);
+        DUMPREG(HDMI_WP_AUDIO_DATA);
+}
+
+u32 hdmi_wp_get_irqstatus(struct hdmi_wp_data *wp)
+{
+        return hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS);
+}
+
+void hdmi_wp_set_irqstatus(struct hdmi_wp_data *wp, u32 irqstatus)
+{
+        hdmi_write_reg(wp->base, HDMI_WP_IRQSTATUS, irqstatus);
+        /* flush posted write */
+        hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS);
+}
+
+void hdmi_wp_set_irqenable(struct hdmi_wp_data *wp, u32 mask)
+{
+        hdmi_write_reg(wp->base, HDMI_WP_IRQENABLE_SET, mask);
+}
+
+void hdmi_wp_clear_irqenable(struct hdmi_wp_data *wp, u32 mask)
+{
+        hdmi_write_reg(wp->base, HDMI_WP_IRQENABLE_CLR, mask);
+}
+
+/* PHY_PWR_CMD */
+int hdmi_wp_set_phy_pwr(struct hdmi_wp_data *wp, enum hdmi_phy_pwr val)
+{
+        /* Return if already the state */
+        if (REG_GET(wp->base, HDMI_WP_PWR_CTRL, 5, 4) == val)
+                return 0;
+
+        /* Command for power control of HDMI PHY */
+        REG_FLD_MOD(wp->base, HDMI_WP_PWR_CTRL, val, 7, 6);
+
+        /* Status of the power control of HDMI PHY */
+        if (hdmi_wait_for_bit_change(wp->base, 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 */
+int hdmi_wp_set_pll_pwr(struct hdmi_wp_data *wp, enum hdmi_pll_pwr val)
+{
+        /* Command for power control of HDMI PLL */
+        REG_FLD_MOD(wp->base, HDMI_WP_PWR_CTRL, val, 3, 2);
+
+        /* wait till PHY_PWR_STATUS is set */
+        if (hdmi_wait_for_bit_change(wp->base, HDMI_WP_PWR_CTRL, 1, 0, val)
+                        != val) {
+                DSSERR("Failed to set PLL_PWR_STATUS\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+int hdmi_wp_video_start(struct hdmi_wp_data *wp)
+{
+        REG_FLD_MOD(wp->base, HDMI_WP_VIDEO_CFG, true, 31, 31);
+
+        return 0;
+}
+
+void hdmi_wp_video_stop(struct hdmi_wp_data *wp)
+{
+        int i;
+
+        hdmi_write_reg(wp->base, HDMI_WP_IRQSTATUS, HDMI_IRQ_VIDEO_FRAME_DONE);
+
+        REG_FLD_MOD(wp->base, HDMI_WP_VIDEO_CFG, false, 31, 31);
+
+        for (i = 0; i < 50; ++i) {
+                u32 v;
+
+                msleep(20);
+
+                v = hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS_RAW);
+                if (v & HDMI_IRQ_VIDEO_FRAME_DONE)
+                        return;
+        }
+
+        DSSERR("no HDMI FRAMEDONE when disabling output\n");
+}
+
+void hdmi_wp_video_config_format(struct hdmi_wp_data *wp,
+                struct hdmi_video_format *video_fmt)
+{
+        u32 l = 0;
+
+        REG_FLD_MOD(wp->base, 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(wp->base, HDMI_WP_VIDEO_SIZE, l);
+}
+
+void hdmi_wp_video_config_interface(struct hdmi_wp_data *wp,
+                struct omap_video_timings *timings)
+{
+        u32 r;
+        bool vsync_pol, hsync_pol;
+        DSSDBG("Enter hdmi_wp_video_config_interface\n");
+
+        vsync_pol = timings->vsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+        hsync_pol = timings->hsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+
+        r = hdmi_read_reg(wp->base, HDMI_WP_VIDEO_CFG);
+        r = FLD_MOD(r, vsync_pol, 7, 7);
+        r = FLD_MOD(r, hsync_pol, 6, 6);
+        r = FLD_MOD(r, timings->interlace, 3, 3);
+        r = FLD_MOD(r, 1, 1, 0); /* HDMI_TIMING_MASTER_24BIT */
+        hdmi_write_reg(wp->base, HDMI_WP_VIDEO_CFG, r);
+}
+
+void hdmi_wp_video_config_timing(struct hdmi_wp_data *wp,
+                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(wp->base, 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(wp->base, HDMI_WP_VIDEO_TIMING_V, timing_v);
+}
+
+void hdmi_wp_init_vid_fmt_timings(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->packing_mode = HDMI_PACK_10b_RGB_YUV444;
+        video_fmt->y_res = param->timings.y_res;
+        video_fmt->x_res = param->timings.x_res;
+        if (param->timings.interlace)
+                video_fmt->y_res /= 2;
+
+        timings->hbp = param->timings.hbp;
+        timings->hfp = param->timings.hfp;
+        timings->hsw = param->timings.hsw;
+        timings->vbp = param->timings.vbp;
+        timings->vfp = param->timings.vfp;
+        timings->vsw = param->timings.vsw;
+        timings->vsync_level = param->timings.vsync_level;
+        timings->hsync_level = param->timings.hsync_level;
+        timings->interlace = param->timings.interlace;
+}
+
+void hdmi_wp_audio_config_format(struct hdmi_wp_data *wp,
+                struct hdmi_audio_format *aud_fmt)
+{
+        u32 r;
+
+        DSSDBG("Enter hdmi_wp_audio_config_format\n");
+
+        r = hdmi_read_reg(wp->base, HDMI_WP_AUDIO_CFG);
+        if (omapdss_get_version() == OMAPDSS_VER_OMAP4430_ES1 ||
+            omapdss_get_version() == OMAPDSS_VER_OMAP4430_ES2 ||
+            omapdss_get_version() == OMAPDSS_VER_OMAP4) {
+                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(wp->base, HDMI_WP_AUDIO_CFG, r);
+}
+
+void hdmi_wp_audio_config_dma(struct hdmi_wp_data *wp,
+                struct hdmi_audio_dma *aud_dma)
+{
+        u32 r;
+
+        DSSDBG("Enter hdmi_wp_audio_config_dma\n");
+
+        r = hdmi_read_reg(wp->base, 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(wp->base, HDMI_WP_AUDIO_CFG2, r);
+
+        r = hdmi_read_reg(wp->base, 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(wp->base, HDMI_WP_AUDIO_CTRL, r);
+}
+
+int hdmi_wp_audio_enable(struct hdmi_wp_data *wp, bool enable)
+{
+        REG_FLD_MOD(wp->base, HDMI_WP_AUDIO_CTRL, enable, 31, 31);
+
+        return 0;
+}
+
+int hdmi_wp_audio_core_req_enable(struct hdmi_wp_data *wp, bool enable)
+{
+        REG_FLD_MOD(wp->base, HDMI_WP_AUDIO_CTRL, enable, 30, 30);
+
+        return 0;
+}
+
+int hdmi_wp_init(struct platform_device *pdev, struct hdmi_wp_data *wp)
+{
+        struct resource *res;
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "wp");
+        if (!res) {
+                DSSERR("can't get WP mem resource\n");
+                return -EINVAL;
+        }
+        wp->phys_base = res->start;
+
+        wp->base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(wp->base)) {
+                DSSERR("can't ioremap HDMI WP\n");
+                return PTR_ERR(wp->base);
+        }
+
+        return 0;
+}
+
+phys_addr_t hdmi_wp_get_audio_dma_addr(struct hdmi_wp_data *wp)
+{
+        return wp->phys_base + HDMI_WP_AUDIO_DATA;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/manager-sysfs.c b/drivers/video/fbdev/omap2/omapfb/dss/manager-sysfs.c
new file mode 100644
index 000000000000..a7414fb12830
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/manager-sysfs.c
@@ -0,0 +1,531 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "MANAGER"
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t manager_name_show(struct omap_overlay_manager *mgr, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n", mgr->name);
+}
+
+static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
+{
+        struct omap_dss_device *dssdev = mgr->get_device(mgr);
+
+        return snprintf(buf, PAGE_SIZE, "%s\n", dssdev ?
+                        dssdev->name : "<none>");
+}
+
+static int manager_display_match(struct omap_dss_device *dssdev, void *data)
+{
+        const char *str = data;
+
+        return sysfs_streq(dssdev->name, str);
+}
+
+static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        int r = 0;
+        size_t len = size;
+        struct omap_dss_device *dssdev = NULL;
+        struct omap_dss_device *old_dssdev;
+
+        if (buf[size-1] == '\n')
+                --len;
+
+        if (len > 0)
+                dssdev = omap_dss_find_device((void *)buf,
+                        manager_display_match);
+
+        if (len > 0 && dssdev == NULL)
+                return -EINVAL;
+
+        if (dssdev) {
+                DSSDBG("display %s found\n", dssdev->name);
+
+                if (omapdss_device_is_connected(dssdev)) {
+                        DSSERR("new display is already connected\n");
+                        r = -EINVAL;
+                        goto put_device;
+                }
+
+                if (omapdss_device_is_enabled(dssdev)) {
+                        DSSERR("new display is not disabled\n");
+                        r = -EINVAL;
+                        goto put_device;
+                }
+        }
+
+        old_dssdev = mgr->get_device(mgr);
+        if (old_dssdev) {
+                if (omapdss_device_is_enabled(old_dssdev)) {
+                        DSSERR("old display is not disabled\n");
+                        r = -EINVAL;
+                        goto put_device;
+                }
+
+                old_dssdev->driver->disconnect(old_dssdev);
+        }
+
+        if (dssdev) {
+                r = dssdev->driver->connect(dssdev);
+                if (r) {
+                        DSSERR("failed to connect new device\n");
+                        goto put_device;
+                }
+
+                old_dssdev = mgr->get_device(mgr);
+                if (old_dssdev != dssdev) {
+                        DSSERR("failed to connect device to this manager\n");
+                        dssdev->driver->disconnect(dssdev);
+                        goto put_device;
+                }
+
+                r = mgr->apply(mgr);
+                if (r) {
+                        DSSERR("failed to apply dispc config\n");
+                        goto put_device;
+                }
+        }
+
+put_device:
+        if (dssdev)
+                omap_dss_put_device(dssdev);
+
+        return r ? r : size;
+}
+
+static ssize_t manager_default_color_show(struct omap_overlay_manager *mgr,
+                                          char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%#x\n", info.default_color);
+}
+
+static ssize_t manager_default_color_store(struct omap_overlay_manager *mgr,
+                                           const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        u32 color;
+        int r;
+
+        r = kstrtouint(buf, 0, &color);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.default_color = color;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static const char *trans_key_type_str[] = {
+        "gfx-destination",
+        "video-source",
+};
+
+static ssize_t manager_trans_key_type_show(struct omap_overlay_manager *mgr,
+                                           char *buf)
+{
+        enum omap_dss_trans_key_type key_type;
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        key_type = info.trans_key_type;
+        BUG_ON(key_type >= ARRAY_SIZE(trans_key_type_str));
+
+        return snprintf(buf, PAGE_SIZE, "%s\n", trans_key_type_str[key_type]);
+}
+
+static ssize_t manager_trans_key_type_store(struct omap_overlay_manager *mgr,
+                                            const char *buf, size_t size)
+{
+        enum omap_dss_trans_key_type key_type;
+        struct omap_overlay_manager_info info;
+        int r;
+
+        for (key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
+                        key_type < ARRAY_SIZE(trans_key_type_str); key_type++) {
+                if (sysfs_streq(buf, trans_key_type_str[key_type]))
+                        break;
+        }
+
+        if (key_type == ARRAY_SIZE(trans_key_type_str))
+                return -EINVAL;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.trans_key_type = key_type;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_trans_key_value_show(struct omap_overlay_manager *mgr,
+                                            char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%#x\n", info.trans_key);
+}
+
+static ssize_t manager_trans_key_value_store(struct omap_overlay_manager *mgr,
+                                             const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        u32 key_value;
+        int r;
+
+        r = kstrtouint(buf, 0, &key_value);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.trans_key = key_value;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_trans_key_enabled_show(struct omap_overlay_manager *mgr,
+                                              char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.trans_enabled);
+}
+
+static ssize_t manager_trans_key_enabled_store(struct omap_overlay_manager *mgr,
+                                               const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        bool enable;
+        int r;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.trans_enabled = enable;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_alpha_blending_enabled_show(
+                struct omap_overlay_manager *mgr, char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        if(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER))
+                return -ENODEV;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                info.partial_alpha_enabled);
+}
+
+static ssize_t manager_alpha_blending_enabled_store(
+                struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        bool enable;
+        int r;
+
+        if(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER))
+                return -ENODEV;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.partial_alpha_enabled = enable;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
+                char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.cpr_enable);
+}
+
+static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        int r;
+        bool enable;
+
+        if (!dss_has_feature(FEAT_CPR))
+                return -ENODEV;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        mgr->get_manager_info(mgr, &info);
+
+        if (info.cpr_enable == enable)
+                return size;
+
+        info.cpr_enable = enable;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
+                char *buf)
+{
+        struct omap_overlay_manager_info info;
+
+        mgr->get_manager_info(mgr, &info);
+
+        return snprintf(buf, PAGE_SIZE,
+                        "%d %d %d %d %d %d %d %d %d\n",
+                        info.cpr_coefs.rr,
+                        info.cpr_coefs.rg,
+                        info.cpr_coefs.rb,
+                        info.cpr_coefs.gr,
+                        info.cpr_coefs.gg,
+                        info.cpr_coefs.gb,
+                        info.cpr_coefs.br,
+                        info.cpr_coefs.bg,
+                        info.cpr_coefs.bb);
+}
+
+static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager_info info;
+        struct omap_dss_cpr_coefs coefs;
+        int r, i;
+        s16 *arr;
+
+        if (!dss_has_feature(FEAT_CPR))
+                return -ENODEV;
+
+        if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
+                                &coefs.rr, &coefs.rg, &coefs.rb,
+                                &coefs.gr, &coefs.gg, &coefs.gb,
+                                &coefs.br, &coefs.bg, &coefs.bb) != 9)
+                return -EINVAL;
+
+        arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
+                coefs.gr, coefs.gg, coefs.gb,
+                coefs.br, coefs.bg, coefs.bb };
+
+        for (i = 0; i < 9; ++i) {
+                if (arr[i] < -512 || arr[i] > 511)
+                        return -EINVAL;
+        }
+
+        mgr->get_manager_info(mgr, &info);
+
+        info.cpr_coefs = coefs;
+
+        r = mgr->set_manager_info(mgr, &info);
+        if (r)
+                return r;
+
+        r = mgr->apply(mgr);
+        if (r)
+                return r;
+
+        return size;
+}
+
+struct manager_attribute {
+        struct attribute attr;
+        ssize_t (*show)(struct omap_overlay_manager *, char *);
+        ssize_t (*store)(struct omap_overlay_manager *, const char *, size_t);
+};
+
+#define MANAGER_ATTR(_name, _mode, _show, _store) \
+        struct manager_attribute manager_attr_##_name = \
+        __ATTR(_name, _mode, _show, _store)
+
+static MANAGER_ATTR(name, S_IRUGO, manager_name_show, NULL);
+static MANAGER_ATTR(display, S_IRUGO|S_IWUSR,
+                manager_display_show, manager_display_store);
+static MANAGER_ATTR(default_color, S_IRUGO|S_IWUSR,
+                manager_default_color_show, manager_default_color_store);
+static MANAGER_ATTR(trans_key_type, S_IRUGO|S_IWUSR,
+                manager_trans_key_type_show, manager_trans_key_type_store);
+static MANAGER_ATTR(trans_key_value, S_IRUGO|S_IWUSR,
+                manager_trans_key_value_show, manager_trans_key_value_store);
+static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
+                manager_trans_key_enabled_show,
+                manager_trans_key_enabled_store);
+static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
+                manager_alpha_blending_enabled_show,
+                manager_alpha_blending_enabled_store);
+static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
+                manager_cpr_enable_show,
+                manager_cpr_enable_store);
+static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
+                manager_cpr_coef_show,
+                manager_cpr_coef_store);
+
+
+static struct attribute *manager_sysfs_attrs[] = {
+        &manager_attr_name.attr,
+        &manager_attr_display.attr,
+        &manager_attr_default_color.attr,
+        &manager_attr_trans_key_type.attr,
+        &manager_attr_trans_key_value.attr,
+        &manager_attr_trans_key_enabled.attr,
+        &manager_attr_alpha_blending_enabled.attr,
+        &manager_attr_cpr_enable.attr,
+        &manager_attr_cpr_coef.attr,
+        NULL
+};
+
+static ssize_t manager_attr_show(struct kobject *kobj, struct attribute *attr,
+                char *buf)
+{
+        struct omap_overlay_manager *manager;
+        struct manager_attribute *manager_attr;
+
+        manager = container_of(kobj, struct omap_overlay_manager, kobj);
+        manager_attr = container_of(attr, struct manager_attribute, attr);
+
+        if (!manager_attr->show)
+                return -ENOENT;
+
+        return manager_attr->show(manager, buf);
+}
+
+static ssize_t manager_attr_store(struct kobject *kobj, struct attribute *attr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay_manager *manager;
+        struct manager_attribute *manager_attr;
+
+        manager = container_of(kobj, struct omap_overlay_manager, kobj);
+        manager_attr = container_of(attr, struct manager_attribute, attr);
+
+        if (!manager_attr->store)
+                return -ENOENT;
+
+        return manager_attr->store(manager, buf, size);
+}
+
+static const struct sysfs_ops manager_sysfs_ops = {
+        .show = manager_attr_show,
+        .store = manager_attr_store,
+};
+
+static struct kobj_type manager_ktype = {
+        .sysfs_ops = &manager_sysfs_ops,
+        .default_attrs = manager_sysfs_attrs,
+};
+
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+                struct platform_device *pdev)
+{
+        return kobject_init_and_add(&mgr->kobj, &manager_ktype,
+                        &pdev->dev.kobj, "manager%d", mgr->id);
+}
+
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr)
+{
+        kobject_del(&mgr->kobj);
+        kobject_put(&mgr->kobj);
+
+        memset(&mgr->kobj, 0, sizeof(mgr->kobj));
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/manager.c b/drivers/video/fbdev/omap2/omapfb/dss/manager.c
new file mode 100644
index 000000000000..08a67f4f6a20
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/manager.c
@@ -0,0 +1,263 @@
+/*
+ * linux/drivers/video/omap2/dss/manager.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "MANAGER"
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static int num_managers;
+static struct omap_overlay_manager *managers;
+
+int dss_init_overlay_managers(void)
+{
+        int i;
+
+        num_managers = dss_feat_get_num_mgrs();
+
+        managers = kzalloc(sizeof(struct omap_overlay_manager) * num_managers,
+                        GFP_KERNEL);
+
+        BUG_ON(managers == NULL);
+
+        for (i = 0; i < num_managers; ++i) {
+                struct omap_overlay_manager *mgr = &managers[i];
+
+                switch (i) {
+                case 0:
+                        mgr->name = "lcd";
+                        mgr->id = OMAP_DSS_CHANNEL_LCD;
+                        break;
+                case 1:
+                        mgr->name = "tv";
+                        mgr->id = OMAP_DSS_CHANNEL_DIGIT;
+                        break;
+                case 2:
+                        mgr->name = "lcd2";
+                        mgr->id = OMAP_DSS_CHANNEL_LCD2;
+                        break;
+                case 3:
+                        mgr->name = "lcd3";
+                        mgr->id = OMAP_DSS_CHANNEL_LCD3;
+                        break;
+                }
+
+                mgr->caps = 0;
+                mgr->supported_displays =
+                        dss_feat_get_supported_displays(mgr->id);
+                mgr->supported_outputs =
+                        dss_feat_get_supported_outputs(mgr->id);
+
+                INIT_LIST_HEAD(&mgr->overlays);
+        }
+
+        return 0;
+}
+
+int dss_init_overlay_managers_sysfs(struct platform_device *pdev)
+{
+        int i, r;
+
+        for (i = 0; i < num_managers; ++i) {
+                struct omap_overlay_manager *mgr = &managers[i];
+
+                r = dss_manager_kobj_init(mgr, pdev);
+                if (r)
+                        DSSERR("failed to create sysfs file\n");
+        }
+
+        return 0;
+}
+
+void dss_uninit_overlay_managers(void)
+{
+        kfree(managers);
+        managers = NULL;
+        num_managers = 0;
+}
+
+void dss_uninit_overlay_managers_sysfs(struct platform_device *pdev)
+{
+        int i;
+
+        for (i = 0; i < num_managers; ++i) {
+                struct omap_overlay_manager *mgr = &managers[i];
+
+                dss_manager_kobj_uninit(mgr);
+        }
+}
+
+int omap_dss_get_num_overlay_managers(void)
+{
+        return num_managers;
+}
+EXPORT_SYMBOL(omap_dss_get_num_overlay_managers);
+
+struct omap_overlay_manager *omap_dss_get_overlay_manager(int num)
+{
+        if (num >= num_managers)
+                return NULL;
+
+        return &managers[num];
+}
+EXPORT_SYMBOL(omap_dss_get_overlay_manager);
+
+int dss_mgr_simple_check(struct omap_overlay_manager *mgr,
+                const struct omap_overlay_manager_info *info)
+{
+        if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER)) {
+                /*
+                 * OMAP3 supports only graphics source transparency color key
+                 * and alpha blending simultaneously. See TRM 15.4.2.4.2.2
+                 * Alpha Mode.
+                 */
+                if (info->partial_alpha_enabled && info->trans_enabled
+                        && info->trans_key_type != OMAP_DSS_COLOR_KEY_GFX_DST) {
+                        DSSERR("check_manager: illegal transparency key\n");
+                        return -EINVAL;
+                }
+        }
+
+        return 0;
+}
+
+static int dss_mgr_check_zorder(struct omap_overlay_manager *mgr,
+                struct omap_overlay_info **overlay_infos)
+{
+        struct omap_overlay *ovl1, *ovl2;
+        struct omap_overlay_info *info1, *info2;
+
+        list_for_each_entry(ovl1, &mgr->overlays, list) {
+                info1 = overlay_infos[ovl1->id];
+
+                if (info1 == NULL)
+                        continue;
+
+                list_for_each_entry(ovl2, &mgr->overlays, list) {
+                        if (ovl1 == ovl2)
+                                continue;
+
+                        info2 = overlay_infos[ovl2->id];
+
+                        if (info2 == NULL)
+                                continue;
+
+                        if (info1->zorder == info2->zorder) {
+                                DSSERR("overlays %d and %d have the same "
+                                                "zorder %d\n",
+                                        ovl1->id, ovl2->id, info1->zorder);
+                                return -EINVAL;
+                        }
+                }
+        }
+
+        return 0;
+}
+
+int dss_mgr_check_timings(struct omap_overlay_manager *mgr,
+                const struct omap_video_timings *timings)
+{
+        if (!dispc_mgr_timings_ok(mgr->id, timings)) {
+                DSSERR("check_manager: invalid timings\n");
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int dss_mgr_check_lcd_config(struct omap_overlay_manager *mgr,
+                const struct dss_lcd_mgr_config *config)
+{
+        struct dispc_clock_info cinfo = config->clock_info;
+        int dl = config->video_port_width;
+        bool stallmode = config->stallmode;
+        bool fifohandcheck = config->fifohandcheck;
+
+        if (cinfo.lck_div < 1 || cinfo.lck_div > 255)
+                return -EINVAL;
+
+        if (cinfo.pck_div < 1 || cinfo.pck_div > 255)
+                return -EINVAL;
+
+        if (dl != 12 && dl != 16 && dl != 18 && dl != 24)
+                return -EINVAL;
+
+        /* fifohandcheck should be used only with stallmode */
+        if (!stallmode && fifohandcheck)
+                return -EINVAL;
+
+        /*
+         * io pad mode can be only checked by using dssdev connected to the
+         * manager. Ignore checking these for now, add checks when manager
+         * is capable of holding information related to the connected interface
+         */
+
+        return 0;
+}
+
+int dss_mgr_check(struct omap_overlay_manager *mgr,
+                struct omap_overlay_manager_info *info,
+                const struct omap_video_timings *mgr_timings,
+                const struct dss_lcd_mgr_config *lcd_config,
+                struct omap_overlay_info **overlay_infos)
+{
+        struct omap_overlay *ovl;
+        int r;
+
+        if (dss_has_feature(FEAT_ALPHA_FREE_ZORDER)) {
+                r = dss_mgr_check_zorder(mgr, overlay_infos);
+                if (r)
+                        return r;
+        }
+
+        r = dss_mgr_check_timings(mgr, mgr_timings);
+        if (r)
+                return r;
+
+        r = dss_mgr_check_lcd_config(mgr, lcd_config);
+        if (r)
+                return r;
+
+        list_for_each_entry(ovl, &mgr->overlays, list) {
+                struct omap_overlay_info *oi;
+                int r;
+
+                oi = overlay_infos[ovl->id];
+
+                if (oi == NULL)
+                        continue;
+
+                r = dss_ovl_check(ovl, oi, mgr_timings);
+                if (r)
+                        return r;
+        }
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/omapdss-boot-init.c b/drivers/video/fbdev/omap2/omapfb/dss/omapdss-boot-init.c
new file mode 100644
index 000000000000..136d30484d02
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/omapdss-boot-init.c
@@ -0,0 +1,225 @@
+/*
+ * Copyright (C) 2014 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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/>.
+ */
+
+/*
+ * As omapdss panel drivers are omapdss specific, but we want to define the
+ * DT-data in generic manner, we convert the compatible strings of the panel and
+ * encoder nodes from "panel-foo" to "omapdss,panel-foo". This way we can have
+ * both correct DT data and omapdss specific drivers.
+ *
+ * When we get generic panel drivers to the kernel, this file will be removed.
+ */
+
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+
+static struct list_head dss_conv_list __initdata;
+
+static const char prefix[] __initconst = "omapdss,";
+
+struct dss_conv_node {
+        struct list_head list;
+        struct device_node *node;
+        bool root;
+};
+
+static int __init omapdss_count_strings(const struct property *prop)
+{
+        const char *p = prop->value;
+        int l = 0, total = 0;
+        int i;
+
+        for (i = 0; total < prop->length; total += l, p += l, i++)
+                l = strlen(p) + 1;
+
+        return i;
+}
+
+static void __init omapdss_update_prop(struct device_node *node, char *compat,
+        int len)
+{
+        struct property *prop;
+
+        prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+        if (!prop)
+                return;
+
+        prop->name = "compatible";
+        prop->value = compat;
+        prop->length = len;
+
+        of_update_property(node, prop);
+}
+
+static void __init omapdss_prefix_strcpy(char *dst, int dst_len,
+        const char *src, int src_len)
+{
+        size_t total = 0;
+
+        while (total < src_len) {
+                size_t l = strlen(src) + 1;
+
+                strcpy(dst, prefix);
+                dst += strlen(prefix);
+
+                strcpy(dst, src);
+                dst += l;
+
+                src += l;
+                total += l;
+        }
+}
+
+/* prepend compatible property strings with "omapdss," */
+static void __init omapdss_omapify_node(struct device_node *node)
+{
+        struct property *prop;
+        char *new_compat;
+        int num_strs;
+        int new_len;
+
+        prop = of_find_property(node, "compatible", NULL);
+
+        if (!prop || !prop->value)
+                return;
+
+        if (strnlen(prop->value, prop->length) >= prop->length)
+                return;
+
+        /* is it already prefixed? */
+        if (strncmp(prefix, prop->value, strlen(prefix)) == 0)
+                return;
+
+        num_strs = omapdss_count_strings(prop);
+
+        new_len = prop->length + strlen(prefix) * num_strs;
+        new_compat = kmalloc(new_len, GFP_KERNEL);
+
+        omapdss_prefix_strcpy(new_compat, new_len, prop->value, prop->length);
+
+        omapdss_update_prop(node, new_compat, new_len);
+}
+
+static void __init omapdss_add_to_list(struct device_node *node, bool root)
+{
+        struct dss_conv_node *n = kmalloc(sizeof(struct dss_conv_node),
+                GFP_KERNEL);
+        if (n) {
+                n->node = node;
+                n->root = root;
+                list_add(&n->list, &dss_conv_list);
+        }
+}
+
+static bool __init omapdss_list_contains(const struct device_node *node)
+{
+        struct dss_conv_node *n;
+
+        list_for_each_entry(n, &dss_conv_list, list) {
+                if (n->node == node)
+                        return true;
+        }
+
+        return false;
+}
+
+static void __init omapdss_walk_device(struct device_node *node, bool root)
+{
+        struct device_node *n;
+
+        omapdss_add_to_list(node, root);
+
+        /*
+         * of_graph_get_remote_port_parent() prints an error if there is no
+         * port/ports node. To avoid that, check first that there's the node.
+         */
+        n = of_get_child_by_name(node, "ports");
+        if (!n)
+                n = of_get_child_by_name(node, "port");
+        if (!n)
+                return;
+
+        of_node_put(n);
+
+        n = NULL;
+        while ((n = of_graph_get_next_endpoint(node, n)) != NULL) {
+                struct device_node *pn;
+
+                pn = of_graph_get_remote_port_parent(n);
+
+                if (!pn)
+                        continue;
+
+                if (!of_device_is_available(pn) || omapdss_list_contains(pn)) {
+                        of_node_put(pn);
+                        continue;
+                }
+
+                omapdss_walk_device(pn, false);
+        }
+}
+
+static const struct of_device_id omapdss_of_match[] __initconst = {
+        { .compatible = "ti,omap2-dss", },
+        { .compatible = "ti,omap3-dss", },
+        { .compatible = "ti,omap4-dss", },
+        { .compatible = "ti,omap5-dss", },
+        { .compatible = "ti,dra7-dss", },
+        {},
+};
+
+static int __init omapdss_boot_init(void)
+{
+        struct device_node *dss, *child;
+
+        INIT_LIST_HEAD(&dss_conv_list);
+
+        dss = of_find_matching_node(NULL, omapdss_of_match);
+
+        if (dss == NULL || !of_device_is_available(dss))
+                return 0;
+
+        omapdss_walk_device(dss, true);
+
+        for_each_available_child_of_node(dss, child) {
+                if (!of_find_property(child, "compatible", NULL))
+                        continue;
+
+                omapdss_walk_device(child, true);
+        }
+
+        while (!list_empty(&dss_conv_list)) {
+                struct dss_conv_node *n;
+
+                n = list_first_entry(&dss_conv_list, struct dss_conv_node,
+                        list);
+
+                if (!n->root)
+                        omapdss_omapify_node(n->node);
+
+                list_del(&n->list);
+                of_node_put(n->node);
+                kfree(n);
+        }
+
+        return 0;
+}
+
+subsys_initcall(omapdss_boot_init);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/output.c b/drivers/video/fbdev/omap2/omapfb/dss/output.c
new file mode 100644
index 000000000000..16072159bd24
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/output.c
@@ -0,0 +1,267 @@
+/*
+ * Copyright (C) 2012 Texas Instruments Ltd
+ * 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/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static LIST_HEAD(output_list);
+static DEFINE_MUTEX(output_lock);
+
+int omapdss_output_set_device(struct omap_dss_device *out,
+                struct omap_dss_device *dssdev)
+{
+        int r;
+
+        mutex_lock(&output_lock);
+
+        if (out->dst) {
+                DSSERR("output already has device %s connected to it\n",
+                        out->dst->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        if (out->output_type != dssdev->type) {
+                DSSERR("output type and display type don't match\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        out->dst = dssdev;
+        dssdev->src = out;
+
+        mutex_unlock(&output_lock);
+
+        return 0;
+err:
+        mutex_unlock(&output_lock);
+
+        return r;
+}
+EXPORT_SYMBOL(omapdss_output_set_device);
+
+int omapdss_output_unset_device(struct omap_dss_device *out)
+{
+        int r;
+
+        mutex_lock(&output_lock);
+
+        if (!out->dst) {
+                DSSERR("output doesn't have a device connected to it\n");
+                r = -EINVAL;
+                goto err;
+        }
+
+        if (out->dst->state != OMAP_DSS_DISPLAY_DISABLED) {
+                DSSERR("device %s is not disabled, cannot unset device\n",
+                                out->dst->name);
+                r = -EINVAL;
+                goto err;
+        }
+
+        out->dst->src = NULL;
+        out->dst = NULL;
+
+        mutex_unlock(&output_lock);
+
+        return 0;
+err:
+        mutex_unlock(&output_lock);
+
+        return r;
+}
+EXPORT_SYMBOL(omapdss_output_unset_device);
+
+int omapdss_register_output(struct omap_dss_device *out)
+{
+        list_add_tail(&out->list, &output_list);
+        return 0;
+}
+EXPORT_SYMBOL(omapdss_register_output);
+
+void omapdss_unregister_output(struct omap_dss_device *out)
+{
+        list_del(&out->list);
+}
+EXPORT_SYMBOL(omapdss_unregister_output);
+
+struct omap_dss_device *omap_dss_get_output(enum omap_dss_output_id id)
+{
+        struct omap_dss_device *out;
+
+        list_for_each_entry(out, &output_list, list) {
+                if (out->id == id)
+                        return out;
+        }
+
+        return NULL;
+}
+EXPORT_SYMBOL(omap_dss_get_output);
+
+struct omap_dss_device *omap_dss_find_output(const char *name)
+{
+        struct omap_dss_device *out;
+
+        list_for_each_entry(out, &output_list, list) {
+                if (strcmp(out->name, name) == 0)
+                        return omap_dss_get_device(out);
+        }
+
+        return NULL;
+}
+EXPORT_SYMBOL(omap_dss_find_output);
+
+struct omap_dss_device *omap_dss_find_output_by_port_node(struct device_node *port)
+{
+        struct device_node *src_node;
+        struct omap_dss_device *out;
+        u32 reg;
+
+        src_node = dss_of_port_get_parent_device(port);
+        if (!src_node)
+                return NULL;
+
+        reg = dss_of_port_get_port_number(port);
+
+        list_for_each_entry(out, &output_list, list) {
+                if (out->dev->of_node == src_node && out->port_num == reg) {
+                        of_node_put(src_node);
+                        return omap_dss_get_device(out);
+                }
+        }
+
+        of_node_put(src_node);
+
+        return NULL;
+}
+EXPORT_SYMBOL(omap_dss_find_output_by_port_node);
+
+struct omap_dss_device *omapdss_find_output_from_display(struct omap_dss_device *dssdev)
+{
+        while (dssdev->src)
+                dssdev = dssdev->src;
+
+        if (dssdev->id != 0)
+                return omap_dss_get_device(dssdev);
+
+        return NULL;
+}
+EXPORT_SYMBOL(omapdss_find_output_from_display);
+
+struct omap_overlay_manager *omapdss_find_mgr_from_display(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out;
+        struct omap_overlay_manager *mgr;
+
+        out = omapdss_find_output_from_display(dssdev);
+
+        if (out == NULL)
+                return NULL;
+
+        mgr = out->manager;
+
+        omap_dss_put_device(out);
+
+        return mgr;
+}
+EXPORT_SYMBOL(omapdss_find_mgr_from_display);
+
+static const struct dss_mgr_ops *dss_mgr_ops;
+
+int dss_install_mgr_ops(const struct dss_mgr_ops *mgr_ops)
+{
+        if (dss_mgr_ops)
+                return -EBUSY;
+
+        dss_mgr_ops = mgr_ops;
+
+        return 0;
+}
+EXPORT_SYMBOL(dss_install_mgr_ops);
+
+void dss_uninstall_mgr_ops(void)
+{
+        dss_mgr_ops = NULL;
+}
+EXPORT_SYMBOL(dss_uninstall_mgr_ops);
+
+int dss_mgr_connect(struct omap_overlay_manager *mgr,
+                struct omap_dss_device *dst)
+{
+        return dss_mgr_ops->connect(mgr, dst);
+}
+EXPORT_SYMBOL(dss_mgr_connect);
+
+void dss_mgr_disconnect(struct omap_overlay_manager *mgr,
+                struct omap_dss_device *dst)
+{
+        dss_mgr_ops->disconnect(mgr, dst);
+}
+EXPORT_SYMBOL(dss_mgr_disconnect);
+
+void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
+                const struct omap_video_timings *timings)
+{
+        dss_mgr_ops->set_timings(mgr, timings);
+}
+EXPORT_SYMBOL(dss_mgr_set_timings);
+
+void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
+                const struct dss_lcd_mgr_config *config)
+{
+        dss_mgr_ops->set_lcd_config(mgr, config);
+}
+EXPORT_SYMBOL(dss_mgr_set_lcd_config);
+
+int dss_mgr_enable(struct omap_overlay_manager *mgr)
+{
+        return dss_mgr_ops->enable(mgr);
+}
+EXPORT_SYMBOL(dss_mgr_enable);
+
+void dss_mgr_disable(struct omap_overlay_manager *mgr)
+{
+        dss_mgr_ops->disable(mgr);
+}
+EXPORT_SYMBOL(dss_mgr_disable);
+
+void dss_mgr_start_update(struct omap_overlay_manager *mgr)
+{
+        dss_mgr_ops->start_update(mgr);
+}
+EXPORT_SYMBOL(dss_mgr_start_update);
+
+int dss_mgr_register_framedone_handler(struct omap_overlay_manager *mgr,
+                void (*handler)(void *), void *data)
+{
+        return dss_mgr_ops->register_framedone_handler(mgr, handler, data);
+}
+EXPORT_SYMBOL(dss_mgr_register_framedone_handler);
+
+void dss_mgr_unregister_framedone_handler(struct omap_overlay_manager *mgr,
+                void (*handler)(void *), void *data)
+{
+        dss_mgr_ops->unregister_framedone_handler(mgr, handler, data);
+}
+EXPORT_SYMBOL(dss_mgr_unregister_framedone_handler);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/overlay-sysfs.c b/drivers/video/fbdev/omap2/omapfb/dss/overlay-sysfs.c
new file mode 100644
index 000000000000..4cc5ddebfb34
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/overlay-sysfs.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "OVERLAY"
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/platform_device.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t overlay_name_show(struct omap_overlay *ovl, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n", ovl->name);
+}
+
+static ssize_t overlay_manager_show(struct omap_overlay *ovl, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%s\n",
+                        ovl->manager ? ovl->manager->name : "<none>");
+}
+
+static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
+                size_t size)
+{
+        int i, r;
+        struct omap_overlay_manager *mgr = NULL;
+        struct omap_overlay_manager *old_mgr;
+        int len = size;
+
+        if (buf[size-1] == '\n')
+                --len;
+
+        if (len > 0) {
+                for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+                        mgr = omap_dss_get_overlay_manager(i);
+
+                        if (sysfs_streq(buf, mgr->name))
+                                break;
+
+                        mgr = NULL;
+                }
+        }
+
+        if (len > 0 && mgr == NULL)
+                return -EINVAL;
+
+        if (mgr)
+                DSSDBG("manager %s found\n", mgr->name);
+
+        if (mgr == ovl->manager)
+                return size;
+
+        old_mgr = ovl->manager;
+
+        r = dispc_runtime_get();
+        if (r)
+                return r;
+
+        /* detach old manager */
+        if (old_mgr) {
+                r = ovl->unset_manager(ovl);
+                if (r) {
+                        DSSERR("detach failed\n");
+                        goto err;
+                }
+
+                r = old_mgr->apply(old_mgr);
+                if (r)
+                        goto err;
+        }
+
+        if (mgr) {
+                r = ovl->set_manager(ovl, mgr);
+                if (r) {
+                        DSSERR("Failed to attach overlay\n");
+                        goto err;
+                }
+
+                r = mgr->apply(mgr);
+                if (r)
+                        goto err;
+        }
+
+        dispc_runtime_put();
+
+        return size;
+
+err:
+        dispc_runtime_put();
+        return r;
+}
+
+static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+                        info.width, info.height);
+}
+
+static ssize_t overlay_screen_width_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.screen_width);
+}
+
+static ssize_t overlay_position_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+                        info.pos_x, info.pos_y);
+}
+
+static ssize_t overlay_position_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        char *last;
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.pos_x = simple_strtoul(buf, &last, 10);
+        ++last;
+        if (last - buf >= size)
+                return -EINVAL;
+
+        info.pos_y = simple_strtoul(last, &last, 10);
+
+        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_output_size_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+                        info.out_width, info.out_height);
+}
+
+static ssize_t overlay_output_size_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        char *last;
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.out_width = simple_strtoul(buf, &last, 10);
+        ++last;
+        if (last - buf >= size)
+                return -EINVAL;
+
+        info.out_height = simple_strtoul(last, &last, 10);
+
+        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_enabled_show(struct omap_overlay *ovl, char *buf)
+{
+        return snprintf(buf, PAGE_SIZE, "%d\n", ovl->is_enabled(ovl));
+}
+
+static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
+                size_t size)
+{
+        int r;
+        bool enable;
+
+        r = strtobool(buf, &enable);
+        if (r)
+                return r;
+
+        if (enable)
+                r = ovl->enable(ovl);
+        else
+                r = ovl->disable(ovl);
+
+        if (r)
+                return r;
+
+        return size;
+}
+
+static ssize_t overlay_global_alpha_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        info.global_alpha);
+}
+
+static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        u8 alpha;
+        struct omap_overlay_info info;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+                return -ENODEV;
+
+        r = kstrtou8(buf, 0, &alpha);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        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)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n",
+                        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;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+                return -ENODEV;
+
+        r = kstrtou8(buf, 0, &alpha);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.pre_mult_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_zorder_show(struct omap_overlay *ovl, char *buf)
+{
+        struct omap_overlay_info info;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        return snprintf(buf, PAGE_SIZE, "%d\n", info.zorder);
+}
+
+static ssize_t overlay_zorder_store(struct omap_overlay *ovl,
+                const char *buf, size_t size)
+{
+        int r;
+        u8 zorder;
+        struct omap_overlay_info info;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+                return -ENODEV;
+
+        r = kstrtou8(buf, 0, &zorder);
+        if (r)
+                return r;
+
+        ovl->get_overlay_info(ovl, &info);
+
+        info.zorder = zorder;
+
+        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;
+}
+
+struct overlay_attribute {
+        struct attribute attr;
+        ssize_t (*show)(struct omap_overlay *, char *);
+        ssize_t (*store)(struct omap_overlay *, const char *, size_t);
+};
+
+#define OVERLAY_ATTR(_name, _mode, _show, _store) \
+        struct overlay_attribute overlay_attr_##_name = \
+        __ATTR(_name, _mode, _show, _store)
+
+static OVERLAY_ATTR(name, S_IRUGO, overlay_name_show, NULL);
+static OVERLAY_ATTR(manager, S_IRUGO|S_IWUSR,
+                overlay_manager_show, overlay_manager_store);
+static OVERLAY_ATTR(input_size, S_IRUGO, overlay_input_size_show, NULL);
+static OVERLAY_ATTR(screen_width, S_IRUGO, overlay_screen_width_show, NULL);
+static OVERLAY_ATTR(position, S_IRUGO|S_IWUSR,
+                overlay_position_show, overlay_position_store);
+static OVERLAY_ATTR(output_size, S_IRUGO|S_IWUSR,
+                overlay_output_size_show, overlay_output_size_store);
+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 OVERLAY_ATTR(zorder, S_IRUGO|S_IWUSR,
+                overlay_zorder_show, overlay_zorder_store);
+
+static struct attribute *overlay_sysfs_attrs[] = {
+        &overlay_attr_name.attr,
+        &overlay_attr_manager.attr,
+        &overlay_attr_input_size.attr,
+        &overlay_attr_screen_width.attr,
+        &overlay_attr_position.attr,
+        &overlay_attr_output_size.attr,
+        &overlay_attr_enabled.attr,
+        &overlay_attr_global_alpha.attr,
+        &overlay_attr_pre_mult_alpha.attr,
+        &overlay_attr_zorder.attr,
+        NULL
+};
+
+static ssize_t overlay_attr_show(struct kobject *kobj, struct attribute *attr,
+                char *buf)
+{
+        struct omap_overlay *overlay;
+        struct overlay_attribute *overlay_attr;
+
+        overlay = container_of(kobj, struct omap_overlay, kobj);
+        overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+        if (!overlay_attr->show)
+                return -ENOENT;
+
+        return overlay_attr->show(overlay, buf);
+}
+
+static ssize_t overlay_attr_store(struct kobject *kobj, struct attribute *attr,
+                const char *buf, size_t size)
+{
+        struct omap_overlay *overlay;
+        struct overlay_attribute *overlay_attr;
+
+        overlay = container_of(kobj, struct omap_overlay, kobj);
+        overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+        if (!overlay_attr->store)
+                return -ENOENT;
+
+        return overlay_attr->store(overlay, buf, size);
+}
+
+static const struct sysfs_ops overlay_sysfs_ops = {
+        .show = overlay_attr_show,
+        .store = overlay_attr_store,
+};
+
+static struct kobj_type overlay_ktype = {
+        .sysfs_ops = &overlay_sysfs_ops,
+        .default_attrs = overlay_sysfs_attrs,
+};
+
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+                struct platform_device *pdev)
+{
+        return kobject_init_and_add(&ovl->kobj, &overlay_ktype,
+                        &pdev->dev.kobj, "overlay%d", ovl->id);
+}
+
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl)
+{
+        kobject_del(&ovl->kobj);
+        kobject_put(&ovl->kobj);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/overlay.c b/drivers/video/fbdev/omap2/omapfb/dss/overlay.c
new file mode 100644
index 000000000000..2f7cee985cdd
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/overlay.c
@@ -0,0 +1,202 @@
+/*
+ * linux/drivers/video/omap2/dss/overlay.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "OVERLAY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static int num_overlays;
+static struct omap_overlay *overlays;
+
+int omap_dss_get_num_overlays(void)
+{
+        return num_overlays;
+}
+EXPORT_SYMBOL(omap_dss_get_num_overlays);
+
+struct omap_overlay *omap_dss_get_overlay(int num)
+{
+        if (num >= num_overlays)
+                return NULL;
+
+        return &overlays[num];
+}
+EXPORT_SYMBOL(omap_dss_get_overlay);
+
+void dss_init_overlays(struct platform_device *pdev)
+{
+        int i, r;
+
+        num_overlays = dss_feat_get_num_ovls();
+
+        overlays = kzalloc(sizeof(struct omap_overlay) * num_overlays,
+                        GFP_KERNEL);
+
+        BUG_ON(overlays == NULL);
+
+        for (i = 0; i < num_overlays; ++i) {
+                struct omap_overlay *ovl = &overlays[i];
+
+                switch (i) {
+                case 0:
+                        ovl->name = "gfx";
+                        ovl->id = OMAP_DSS_GFX;
+                        break;
+                case 1:
+                        ovl->name = "vid1";
+                        ovl->id = OMAP_DSS_VIDEO1;
+                        break;
+                case 2:
+                        ovl->name = "vid2";
+                        ovl->id = OMAP_DSS_VIDEO2;
+                        break;
+                case 3:
+                        ovl->name = "vid3";
+                        ovl->id = OMAP_DSS_VIDEO3;
+                        break;
+                }
+
+                ovl->caps = dss_feat_get_overlay_caps(ovl->id);
+                ovl->supported_modes =
+                        dss_feat_get_supported_color_modes(ovl->id);
+
+                r = dss_overlay_kobj_init(ovl, pdev);
+                if (r)
+                        DSSERR("failed to create sysfs file\n");
+        }
+}
+
+void dss_uninit_overlays(struct platform_device *pdev)
+{
+        int i;
+
+        for (i = 0; i < num_overlays; ++i) {
+                struct omap_overlay *ovl = &overlays[i];
+                dss_overlay_kobj_uninit(ovl);
+        }
+
+        kfree(overlays);
+        overlays = NULL;
+        num_overlays = 0;
+}
+
+int dss_ovl_simple_check(struct omap_overlay *ovl,
+                const struct omap_overlay_info *info)
+{
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0) {
+                if (info->out_width != 0 && info->width != info->out_width) {
+                        DSSERR("check_overlay: overlay %d doesn't support "
+                                        "scaling\n", ovl->id);
+                        return -EINVAL;
+                }
+
+                if (info->out_height != 0 && info->height != info->out_height) {
+                        DSSERR("check_overlay: overlay %d doesn't support "
+                                        "scaling\n", ovl->id);
+                        return -EINVAL;
+                }
+        }
+
+        if ((ovl->supported_modes & info->color_mode) == 0) {
+                DSSERR("check_overlay: overlay %d doesn't support mode %d\n",
+                                ovl->id, info->color_mode);
+                return -EINVAL;
+        }
+
+        if (info->zorder >= omap_dss_get_num_overlays()) {
+                DSSERR("check_overlay: zorder %d too high\n", info->zorder);
+                return -EINVAL;
+        }
+
+        if (dss_feat_rotation_type_supported(info->rotation_type) == 0) {
+                DSSERR("check_overlay: rotation type %d not supported\n",
+                                info->rotation_type);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
+                const struct omap_video_timings *mgr_timings)
+{
+        u16 outw, outh;
+        u16 dw, dh;
+
+        dw = mgr_timings->x_res;
+        dh = mgr_timings->y_res;
+
+        if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0) {
+                outw = info->width;
+                outh = info->height;
+        } else {
+                if (info->out_width == 0)
+                        outw = info->width;
+                else
+                        outw = info->out_width;
+
+                if (info->out_height == 0)
+                        outh = info->height;
+                else
+                        outh = info->out_height;
+        }
+
+        if (dw < info->pos_x + outw) {
+                DSSERR("overlay %d horizontally not inside the display area "
+                                "(%d + %d >= %d)\n",
+                                ovl->id, info->pos_x, outw, dw);
+                return -EINVAL;
+        }
+
+        if (dh < info->pos_y + outh) {
+                DSSERR("overlay %d vertically not inside the display area "
+                                "(%d + %d >= %d)\n",
+                                ovl->id, info->pos_y, outh, dh);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/*
+ * Checks if replication logic should be used. Only use when overlay is in
+ * RGB12U or RGB16 mode, and video port width interface is 18bpp or 24bpp
+ */
+bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
+                enum omap_color_mode mode)
+{
+        if (mode != OMAP_DSS_COLOR_RGB12U && mode != OMAP_DSS_COLOR_RGB16)
+                return false;
+
+        return config.video_port_width > 16;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/pll.c b/drivers/video/fbdev/omap2/omapfb/dss/pll.c
new file mode 100644
index 000000000000..f974ddcd3b6e
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/pll.c
@@ -0,0 +1,389 @@
+/*
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * 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 "PLL"
+
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/regulator/consumer.h>
+#include <linux/sched.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+#define PLL_CONTROL                     0x0000
+#define PLL_STATUS                      0x0004
+#define PLL_GO                          0x0008
+#define PLL_CONFIGURATION1              0x000C
+#define PLL_CONFIGURATION2              0x0010
+#define PLL_CONFIGURATION3              0x0014
+#define PLL_SSC_CONFIGURATION1          0x0018
+#define PLL_SSC_CONFIGURATION2          0x001C
+#define PLL_CONFIGURATION4              0x0020
+
+static struct dss_pll *dss_plls[4];
+
+int dss_pll_register(struct dss_pll *pll)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dss_plls); ++i) {
+                if (!dss_plls[i]) {
+                        dss_plls[i] = pll;
+                        return 0;
+                }
+        }
+
+        return -EBUSY;
+}
+
+void dss_pll_unregister(struct dss_pll *pll)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dss_plls); ++i) {
+                if (dss_plls[i] == pll) {
+                        dss_plls[i] = NULL;
+                        return;
+                }
+        }
+}
+
+struct dss_pll *dss_pll_find(const char *name)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dss_plls); ++i) {
+                if (dss_plls[i] && strcmp(dss_plls[i]->name, name) == 0)
+                        return dss_plls[i];
+        }
+
+        return NULL;
+}
+
+int dss_pll_enable(struct dss_pll *pll)
+{
+        int r;
+
+        r = clk_prepare_enable(pll->clkin);
+        if (r)
+                return r;
+
+        if (pll->regulator) {
+                r = regulator_enable(pll->regulator);
+                if (r)
+                        goto err_reg;
+        }
+
+        r = pll->ops->enable(pll);
+        if (r)
+                goto err_enable;
+
+        return 0;
+
+err_enable:
+        if (pll->regulator)
+                regulator_disable(pll->regulator);
+err_reg:
+        clk_disable_unprepare(pll->clkin);
+        return r;
+}
+
+void dss_pll_disable(struct dss_pll *pll)
+{
+        pll->ops->disable(pll);
+
+        if (pll->regulator)
+                regulator_disable(pll->regulator);
+
+        clk_disable_unprepare(pll->clkin);
+
+        memset(&pll->cinfo, 0, sizeof(pll->cinfo));
+}
+
+int dss_pll_set_config(struct dss_pll *pll, const struct dss_pll_clock_info *cinfo)
+{
+        int r;
+
+        r = pll->ops->set_config(pll, cinfo);
+        if (r)
+                return r;
+
+        pll->cinfo = *cinfo;
+
+        return 0;
+}
+
+bool dss_pll_hsdiv_calc(const struct dss_pll *pll, unsigned long clkdco,
+                unsigned long out_min, unsigned long out_max,
+                dss_hsdiv_calc_func func, void *data)
+{
+        const struct dss_pll_hw *hw = pll->hw;
+        int m, m_start, m_stop;
+        unsigned long out;
+
+        out_min = out_min ? out_min : 1;
+        out_max = out_max ? out_max : ULONG_MAX;
+
+        m_start = max(DIV_ROUND_UP(clkdco, out_max), 1ul);
+
+        m_stop = min((unsigned)(clkdco / out_min), hw->mX_max);
+
+        for (m = m_start; m <= m_stop; ++m) {
+                out = clkdco / m;
+
+                if (func(m, out, data))
+                        return true;
+        }
+
+        return false;
+}
+
+bool dss_pll_calc(const struct dss_pll *pll, unsigned long clkin,
+                unsigned long pll_min, unsigned long pll_max,
+                dss_pll_calc_func func, void *data)
+{
+        const struct dss_pll_hw *hw = pll->hw;
+        int n, n_start, n_stop;
+        int m, m_start, m_stop;
+        unsigned long fint, clkdco;
+        unsigned long pll_hw_max;
+        unsigned long fint_hw_min, fint_hw_max;
+
+        pll_hw_max = hw->clkdco_max;
+
+        fint_hw_min = hw->fint_min;
+        fint_hw_max = hw->fint_max;
+
+        n_start = max(DIV_ROUND_UP(clkin, fint_hw_max), 1ul);
+        n_stop = min((unsigned)(clkin / fint_hw_min), hw->n_max);
+
+        pll_max = pll_max ? pll_max : ULONG_MAX;
+
+        for (n = n_start; n <= n_stop; ++n) {
+                fint = clkin / n;
+
+                m_start = max(DIV_ROUND_UP(DIV_ROUND_UP(pll_min, fint), 2),
+                                1ul);
+                m_stop = min3((unsigned)(pll_max / fint / 2),
+                                (unsigned)(pll_hw_max / fint / 2),
+                                hw->m_max);
+
+                for (m = m_start; m <= m_stop; ++m) {
+                        clkdco = 2 * m * fint;
+
+                        if (func(n, m, fint, clkdco, data))
+                                return true;
+                }
+        }
+
+        return false;
+}
+
+static int wait_for_bit_change(void __iomem *reg, int bitnum, int value)
+{
+        unsigned long timeout;
+        ktime_t wait;
+        int t;
+
+        /* first busyloop to see if the bit changes right away */
+        t = 100;
+        while (t-- > 0) {
+                if (FLD_GET(readl_relaxed(reg), bitnum, bitnum) == value)
+                        return value;
+        }
+
+        /* then loop for 500ms, sleeping for 1ms in between */
+        timeout = jiffies + msecs_to_jiffies(500);
+        while (time_before(jiffies, timeout)) {
+                if (FLD_GET(readl_relaxed(reg), bitnum, bitnum) == value)
+                        return value;
+
+                wait = ns_to_ktime(1000 * 1000);
+                set_current_state(TASK_UNINTERRUPTIBLE);
+                schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+        }
+
+        return !value;
+}
+
+int dss_pll_wait_reset_done(struct dss_pll *pll)
+{
+        void __iomem *base = pll->base;
+
+        if (wait_for_bit_change(base + PLL_STATUS, 0, 1) != 1)
+                return -ETIMEDOUT;
+        else
+                return 0;
+}
+
+static int dss_wait_hsdiv_ack(struct dss_pll *pll, u32 hsdiv_ack_mask)
+{
+        int t = 100;
+
+        while (t-- > 0) {
+                u32 v = readl_relaxed(pll->base + PLL_STATUS);
+                v &= hsdiv_ack_mask;
+                if (v == hsdiv_ack_mask)
+                        return 0;
+        }
+
+        return -ETIMEDOUT;
+}
+
+int dss_pll_write_config_type_a(struct dss_pll *pll,
+                const struct dss_pll_clock_info *cinfo)
+{
+        const struct dss_pll_hw *hw = pll->hw;
+        void __iomem *base = pll->base;
+        int r = 0;
+        u32 l;
+
+        l = 0;
+        if (hw->has_stopmode)
+                l = FLD_MOD(l, 1, 0, 0);                /* PLL_STOPMODE */
+        l = FLD_MOD(l, cinfo->n - 1, hw->n_msb, hw->n_lsb);     /* PLL_REGN */
+        l = FLD_MOD(l, cinfo->m, hw->m_msb, hw->m_lsb);         /* PLL_REGM */
+        /* M4 */
+        l = FLD_MOD(l, cinfo->mX[0] ? cinfo->mX[0] - 1 : 0,
+                        hw->mX_msb[0], hw->mX_lsb[0]);
+        /* M5 */
+        l = FLD_MOD(l, cinfo->mX[1] ? cinfo->mX[1] - 1 : 0,
+                        hw->mX_msb[1], hw->mX_lsb[1]);
+        writel_relaxed(l, base + PLL_CONFIGURATION1);
+
+        l = 0;
+        /* M6 */
+        l = FLD_MOD(l, cinfo->mX[2] ? cinfo->mX[2] - 1 : 0,
+                        hw->mX_msb[2], hw->mX_lsb[2]);
+        /* M7 */
+        l = FLD_MOD(l, cinfo->mX[3] ? cinfo->mX[3] - 1 : 0,
+                        hw->mX_msb[3], hw->mX_lsb[3]);
+        writel_relaxed(l, base + PLL_CONFIGURATION3);
+
+        l = readl_relaxed(base + PLL_CONFIGURATION2);
+        if (hw->has_freqsel) {
+                u32 f = cinfo->fint < 1000000 ? 0x3 :
+                        cinfo->fint < 1250000 ? 0x4 :
+                        cinfo->fint < 1500000 ? 0x5 :
+                        cinfo->fint < 1750000 ? 0x6 :
+                        0x7;
+
+                l = FLD_MOD(l, f, 4, 1);        /* PLL_FREQSEL */
+        } else if (hw->has_selfreqdco) {
+                u32 f = cinfo->clkdco < hw->clkdco_low ? 0x2 : 0x4;
+
+                l = FLD_MOD(l, f, 3, 1);        /* PLL_SELFREQDCO */
+        }
+        l = FLD_MOD(l, 1, 13, 13);              /* PLL_REFEN */
+        l = FLD_MOD(l, 0, 14, 14);              /* PHY_CLKINEN */
+        l = FLD_MOD(l, 0, 16, 16);              /* M4_CLOCK_EN */
+        l = FLD_MOD(l, 0, 18, 18);              /* M5_CLOCK_EN */
+        l = FLD_MOD(l, 1, 20, 20);              /* HSDIVBYPASS */
+        if (hw->has_refsel)
+                l = FLD_MOD(l, 3, 22, 21);      /* REFSEL = sysclk */
+        l = FLD_MOD(l, 0, 23, 23);              /* M6_CLOCK_EN */
+        l = FLD_MOD(l, 0, 25, 25);              /* M7_CLOCK_EN */
+        writel_relaxed(l, base + PLL_CONFIGURATION2);
+
+        writel_relaxed(1, base + PLL_GO);       /* PLL_GO */
+
+        if (wait_for_bit_change(base + PLL_GO, 0, 0) != 0) {
+                DSSERR("DSS DPLL GO bit not going down.\n");
+                r = -EIO;
+                goto err;
+        }
+
+        if (wait_for_bit_change(base + PLL_STATUS, 1, 1) != 1) {
+                DSSERR("cannot lock DSS DPLL\n");
+                r = -EIO;
+                goto err;
+        }
+
+        l = readl_relaxed(base + PLL_CONFIGURATION2);
+        l = FLD_MOD(l, 1, 14, 14);                      /* PHY_CLKINEN */
+        l = FLD_MOD(l, cinfo->mX[0] ? 1 : 0, 16, 16);   /* M4_CLOCK_EN */
+        l = FLD_MOD(l, cinfo->mX[1] ? 1 : 0, 18, 18);   /* M5_CLOCK_EN */
+        l = FLD_MOD(l, 0, 20, 20);                      /* HSDIVBYPASS */
+        l = FLD_MOD(l, cinfo->mX[2] ? 1 : 0, 23, 23);   /* M6_CLOCK_EN */
+        l = FLD_MOD(l, cinfo->mX[3] ? 1 : 0, 25, 25);   /* M7_CLOCK_EN */
+        writel_relaxed(l, base + PLL_CONFIGURATION2);
+
+        r = dss_wait_hsdiv_ack(pll,
+                (cinfo->mX[0] ? BIT(7) : 0) |
+                (cinfo->mX[1] ? BIT(8) : 0) |
+                (cinfo->mX[2] ? BIT(10) : 0) |
+                (cinfo->mX[3] ? BIT(11) : 0));
+        if (r) {
+                DSSERR("failed to enable HSDIV clocks\n");
+                goto err;
+        }
+
+err:
+        return r;
+}
+
+int dss_pll_write_config_type_b(struct dss_pll *pll,
+                const struct dss_pll_clock_info *cinfo)
+{
+        const struct dss_pll_hw *hw = pll->hw;
+        void __iomem *base = pll->base;
+        u32 l;
+
+        l = 0;
+        l = FLD_MOD(l, cinfo->m, 20, 9);        /* PLL_REGM */
+        l = FLD_MOD(l, cinfo->n - 1, 8, 1);     /* PLL_REGN */
+        writel_relaxed(l, base + PLL_CONFIGURATION1);
+
+        l = readl_relaxed(base + PLL_CONFIGURATION2);
+        l = FLD_MOD(l, 0x0, 12, 12);    /* PLL_HIGHFREQ divide by 2 */
+        l = FLD_MOD(l, 0x1, 13, 13);    /* PLL_REFEN */
+        l = FLD_MOD(l, 0x0, 14, 14);    /* PHY_CLKINEN */
+        if (hw->has_refsel)
+                l = FLD_MOD(l, 0x3, 22, 21);    /* REFSEL = SYSCLK */
+
+        /* PLL_SELFREQDCO */
+        if (cinfo->clkdco > hw->clkdco_low)
+                l = FLD_MOD(l, 0x4, 3, 1);
+        else
+                l = FLD_MOD(l, 0x2, 3, 1);
+        writel_relaxed(l, base + PLL_CONFIGURATION2);
+
+        l = readl_relaxed(base + PLL_CONFIGURATION3);
+        l = FLD_MOD(l, cinfo->sd, 17, 10);      /* PLL_REGSD */
+        writel_relaxed(l, base + PLL_CONFIGURATION3);
+
+        l = readl_relaxed(base + PLL_CONFIGURATION4);
+        l = FLD_MOD(l, cinfo->mX[0], 24, 18);   /* PLL_REGM2 */
+        l = FLD_MOD(l, cinfo->mf, 17, 0);       /* PLL_REGM_F */
+        writel_relaxed(l, base + PLL_CONFIGURATION4);
+
+        writel_relaxed(1, base + PLL_GO);       /* PLL_GO */
+
+        if (wait_for_bit_change(base + PLL_GO, 0, 0) != 0) {
+                DSSERR("DSS DPLL GO bit not going down.\n");
+                return -EIO;
+        }
+
+        if (wait_for_bit_change(base + PLL_STATUS, 1, 1) != 1) {
+                DSSERR("cannot lock DSS DPLL\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/rfbi.c b/drivers/video/fbdev/omap2/omapfb/dss/rfbi.c
new file mode 100644
index 000000000000..aea6a1d0fb20
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/rfbi.c
@@ -0,0 +1,1078 @@
+/*
+ * linux/drivers/video/omap2/dss/rfbi.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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 "RFBI"
+
+#include <linux/kernel.h>
+#include <linux/dma-mapping.h>
+#include <linux/export.h>
+#include <linux/vmalloc.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/kfifo.h>
+#include <linux/ktime.h>
+#include <linux/hrtimer.h>
+#include <linux/seq_file.h>
+#include <linux/semaphore.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+
+struct rfbi_reg { u16 idx; };
+
+#define RFBI_REG(idx)           ((const struct rfbi_reg) { idx })
+
+#define RFBI_REVISION           RFBI_REG(0x0000)
+#define RFBI_SYSCONFIG          RFBI_REG(0x0010)
+#define RFBI_SYSSTATUS          RFBI_REG(0x0014)
+#define RFBI_CONTROL            RFBI_REG(0x0040)
+#define RFBI_PIXEL_CNT          RFBI_REG(0x0044)
+#define RFBI_LINE_NUMBER        RFBI_REG(0x0048)
+#define RFBI_CMD                RFBI_REG(0x004c)
+#define RFBI_PARAM              RFBI_REG(0x0050)
+#define RFBI_DATA               RFBI_REG(0x0054)
+#define RFBI_READ               RFBI_REG(0x0058)
+#define RFBI_STATUS             RFBI_REG(0x005c)
+
+#define RFBI_CONFIG(n)          RFBI_REG(0x0060 + (n)*0x18)
+#define RFBI_ONOFF_TIME(n)      RFBI_REG(0x0064 + (n)*0x18)
+#define RFBI_CYCLE_TIME(n)      RFBI_REG(0x0068 + (n)*0x18)
+#define RFBI_DATA_CYCLE1(n)     RFBI_REG(0x006c + (n)*0x18)
+#define RFBI_DATA_CYCLE2(n)     RFBI_REG(0x0070 + (n)*0x18)
+#define RFBI_DATA_CYCLE3(n)     RFBI_REG(0x0074 + (n)*0x18)
+
+#define RFBI_VSYNC_WIDTH        RFBI_REG(0x0090)
+#define RFBI_HSYNC_WIDTH        RFBI_REG(0x0094)
+
+#define REG_FLD_MOD(idx, val, start, end) \
+        rfbi_write_reg(idx, FLD_MOD(rfbi_read_reg(idx), val, start, end))
+
+enum omap_rfbi_cycleformat {
+        OMAP_DSS_RFBI_CYCLEFORMAT_1_1 = 0,
+        OMAP_DSS_RFBI_CYCLEFORMAT_2_1 = 1,
+        OMAP_DSS_RFBI_CYCLEFORMAT_3_1 = 2,
+        OMAP_DSS_RFBI_CYCLEFORMAT_3_2 = 3,
+};
+
+enum omap_rfbi_datatype {
+        OMAP_DSS_RFBI_DATATYPE_12 = 0,
+        OMAP_DSS_RFBI_DATATYPE_16 = 1,
+        OMAP_DSS_RFBI_DATATYPE_18 = 2,
+        OMAP_DSS_RFBI_DATATYPE_24 = 3,
+};
+
+enum omap_rfbi_parallelmode {
+        OMAP_DSS_RFBI_PARALLELMODE_8 = 0,
+        OMAP_DSS_RFBI_PARALLELMODE_9 = 1,
+        OMAP_DSS_RFBI_PARALLELMODE_12 = 2,
+        OMAP_DSS_RFBI_PARALLELMODE_16 = 3,
+};
+
+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;
+
+        enum omap_rfbi_datatype datatype;
+        enum omap_rfbi_parallelmode parallelmode;
+
+        enum omap_rfbi_te_mode te_mode;
+        int te_enabled;
+
+        void (*framedone_callback)(void *data);
+        void *framedone_callback_data;
+
+        struct omap_dss_device *dssdev[2];
+
+        struct semaphore bus_lock;
+
+        struct omap_video_timings timings;
+        int pixel_size;
+        int data_lines;
+        struct rfbi_timings intf_timings;
+
+        struct omap_dss_device output;
+} rfbi;
+
+static inline void rfbi_write_reg(const struct rfbi_reg idx, u32 val)
+{
+        __raw_writel(val, rfbi.base + idx.idx);
+}
+
+static inline u32 rfbi_read_reg(const struct rfbi_reg idx)
+{
+        return __raw_readl(rfbi.base + idx.idx);
+}
+
+static int rfbi_runtime_get(void)
+{
+        int r;
+
+        DSSDBG("rfbi_runtime_get\n");
+
+        r = pm_runtime_get_sync(&rfbi.pdev->dev);
+        WARN_ON(r < 0);
+        return r < 0 ? r : 0;
+}
+
+static void rfbi_runtime_put(void)
+{
+        int r;
+
+        DSSDBG("rfbi_runtime_put\n");
+
+        r = pm_runtime_put_sync(&rfbi.pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static void rfbi_bus_lock(void)
+{
+        down(&rfbi.bus_lock);
+}
+
+static void rfbi_bus_unlock(void)
+{
+        up(&rfbi.bus_lock);
+}
+
+static void rfbi_write_command(const void *buf, u32 len)
+{
+        switch (rfbi.parallelmode) {
+        case OMAP_DSS_RFBI_PARALLELMODE_8:
+        {
+                const u8 *b = buf;
+                for (; len; len--)
+                        rfbi_write_reg(RFBI_CMD, *b++);
+                break;
+        }
+
+        case OMAP_DSS_RFBI_PARALLELMODE_16:
+        {
+                const u16 *w = buf;
+                BUG_ON(len & 1);
+                for (; len; len -= 2)
+                        rfbi_write_reg(RFBI_CMD, *w++);
+                break;
+        }
+
+        case OMAP_DSS_RFBI_PARALLELMODE_9:
+        case OMAP_DSS_RFBI_PARALLELMODE_12:
+        default:
+                BUG();
+        }
+}
+
+static void rfbi_read_data(void *buf, u32 len)
+{
+        switch (rfbi.parallelmode) {
+        case OMAP_DSS_RFBI_PARALLELMODE_8:
+        {
+                u8 *b = buf;
+                for (; len; len--) {
+                        rfbi_write_reg(RFBI_READ, 0);
+                        *b++ = rfbi_read_reg(RFBI_READ);
+                }
+                break;
+        }
+
+        case OMAP_DSS_RFBI_PARALLELMODE_16:
+        {
+                u16 *w = buf;
+                BUG_ON(len & ~1);
+                for (; len; len -= 2) {
+                        rfbi_write_reg(RFBI_READ, 0);
+                        *w++ = rfbi_read_reg(RFBI_READ);
+                }
+                break;
+        }
+
+        case OMAP_DSS_RFBI_PARALLELMODE_9:
+        case OMAP_DSS_RFBI_PARALLELMODE_12:
+        default:
+                BUG();
+        }
+}
+
+static void rfbi_write_data(const void *buf, u32 len)
+{
+        switch (rfbi.parallelmode) {
+        case OMAP_DSS_RFBI_PARALLELMODE_8:
+        {
+                const u8 *b = buf;
+                for (; len; len--)
+                        rfbi_write_reg(RFBI_PARAM, *b++);
+                break;
+        }
+
+        case OMAP_DSS_RFBI_PARALLELMODE_16:
+        {
+                const u16 *w = buf;
+                BUG_ON(len & 1);
+                for (; len; len -= 2)
+                        rfbi_write_reg(RFBI_PARAM, *w++);
+                break;
+        }
+
+        case OMAP_DSS_RFBI_PARALLELMODE_9:
+        case OMAP_DSS_RFBI_PARALLELMODE_12:
+        default:
+                BUG();
+
+        }
+}
+
+static void rfbi_write_pixels(const void __iomem *buf, int scr_width,
+                u16 x, u16 y,
+                u16 w, u16 h)
+{
+        int start_offset = scr_width * y + x;
+        int horiz_offset = scr_width - w;
+        int i;
+
+        if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_16 &&
+           rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_8) {
+                const u16 __iomem *pd = buf;
+                pd += start_offset;
+
+                for (; h; --h) {
+                        for (i = 0; i < w; ++i) {
+                                const u8 __iomem *b = (const u8 __iomem *)pd;
+                                rfbi_write_reg(RFBI_PARAM, __raw_readb(b+1));
+                                rfbi_write_reg(RFBI_PARAM, __raw_readb(b+0));
+                                ++pd;
+                        }
+                        pd += horiz_offset;
+                }
+        } else if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_24 &&
+           rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_8) {
+                const u32 __iomem *pd = buf;
+                pd += start_offset;
+
+                for (; h; --h) {
+                        for (i = 0; i < w; ++i) {
+                                const u8 __iomem *b = (const u8 __iomem *)pd;
+                                rfbi_write_reg(RFBI_PARAM, __raw_readb(b+2));
+                                rfbi_write_reg(RFBI_PARAM, __raw_readb(b+1));
+                                rfbi_write_reg(RFBI_PARAM, __raw_readb(b+0));
+                                ++pd;
+                        }
+                        pd += horiz_offset;
+                }
+        } else if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_16 &&
+           rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_16) {
+                const u16 __iomem *pd = buf;
+                pd += start_offset;
+
+                for (; h; --h) {
+                        for (i = 0; i < w; ++i) {
+                                rfbi_write_reg(RFBI_PARAM, __raw_readw(pd));
+                                ++pd;
+                        }
+                        pd += horiz_offset;
+                }
+        } else {
+                BUG();
+        }
+}
+
+static int rfbi_transfer_area(struct omap_dss_device *dssdev,
+                void (*callback)(void *data), void *data)
+{
+        u32 l;
+        int r;
+        struct omap_overlay_manager *mgr = rfbi.output.manager;
+        u16 width = rfbi.timings.x_res;
+        u16 height = rfbi.timings.y_res;
+
+        /*BUG_ON(callback == 0);*/
+        BUG_ON(rfbi.framedone_callback != NULL);
+
+        DSSDBG("rfbi_transfer_area %dx%d\n", width, height);
+
+        dss_mgr_set_timings(mgr, &rfbi.timings);
+
+        r = dss_mgr_enable(mgr);
+        if (r)
+                return r;
+
+        rfbi.framedone_callback = callback;
+        rfbi.framedone_callback_data = data;
+
+        rfbi_write_reg(RFBI_PIXEL_CNT, width * height);
+
+        l = rfbi_read_reg(RFBI_CONTROL);
+        l = FLD_MOD(l, 1, 0, 0); /* enable */
+        if (!rfbi.te_enabled)
+                l = FLD_MOD(l, 1, 4, 4); /* ITE */
+
+        rfbi_write_reg(RFBI_CONTROL, l);
+
+        return 0;
+}
+
+static void framedone_callback(void *data)
+{
+        void (*callback)(void *data);
+
+        DSSDBG("FRAMEDONE\n");
+
+        REG_FLD_MOD(RFBI_CONTROL, 0, 0, 0);
+
+        callback = rfbi.framedone_callback;
+        rfbi.framedone_callback = NULL;
+
+        if (callback != NULL)
+                callback(rfbi.framedone_callback_data);
+}
+
+#if 1 /* VERBOSE */
+static void rfbi_print_timings(void)
+{
+        u32 l;
+        u32 time;
+
+        l = rfbi_read_reg(RFBI_CONFIG(0));
+        time = 1000000000 / rfbi.l4_khz;
+        if (l & (1 << 4))
+                time *= 2;
+
+        DSSDBG("Tick time %u ps\n", time);
+        l = rfbi_read_reg(RFBI_ONOFF_TIME(0));
+        DSSDBG("CSONTIME %d, CSOFFTIME %d, WEONTIME %d, WEOFFTIME %d, "
+                "REONTIME %d, REOFFTIME %d\n",
+                l & 0x0f, (l >> 4) & 0x3f, (l >> 10) & 0x0f, (l >> 14) & 0x3f,
+                (l >> 20) & 0x0f, (l >> 24) & 0x3f);
+
+        l = rfbi_read_reg(RFBI_CYCLE_TIME(0));
+        DSSDBG("WECYCLETIME %d, RECYCLETIME %d, CSPULSEWIDTH %d, "
+                "ACCESSTIME %d\n",
+                (l & 0x3f), (l >> 6) & 0x3f, (l >> 12) & 0x3f,
+                (l >> 22) & 0x3f);
+}
+#else
+static void rfbi_print_timings(void) {}
+#endif
+
+
+
+
+static u32 extif_clk_period;
+
+static inline unsigned long round_to_extif_ticks(unsigned long ps, int div)
+{
+        int bus_tick = extif_clk_period * div;
+        return (ps + bus_tick - 1) / bus_tick * bus_tick;
+}
+
+static int calc_reg_timing(struct rfbi_timings *t, int div)
+{
+        t->clk_div = div;
+
+        t->cs_on_time = round_to_extif_ticks(t->cs_on_time, div);
+
+        t->we_on_time = round_to_extif_ticks(t->we_on_time, div);
+        t->we_off_time = round_to_extif_ticks(t->we_off_time, div);
+        t->we_cycle_time = round_to_extif_ticks(t->we_cycle_time, div);
+
+        t->re_on_time = round_to_extif_ticks(t->re_on_time, div);
+        t->re_off_time = round_to_extif_ticks(t->re_off_time, div);
+        t->re_cycle_time = round_to_extif_ticks(t->re_cycle_time, div);
+
+        t->access_time = round_to_extif_ticks(t->access_time, div);
+        t->cs_off_time = round_to_extif_ticks(t->cs_off_time, div);
+        t->cs_pulse_width = round_to_extif_ticks(t->cs_pulse_width, div);
+
+        DSSDBG("[reg]cson %d csoff %d reon %d reoff %d\n",
+               t->cs_on_time, t->cs_off_time, t->re_on_time, t->re_off_time);
+        DSSDBG("[reg]weon %d weoff %d recyc %d wecyc %d\n",
+               t->we_on_time, t->we_off_time, t->re_cycle_time,
+               t->we_cycle_time);
+        DSSDBG("[reg]rdaccess %d cspulse %d\n",
+               t->access_time, t->cs_pulse_width);
+
+        return rfbi_convert_timings(t);
+}
+
+static int calc_extif_timings(struct rfbi_timings *t)
+{
+        u32 max_clk_div;
+        int div;
+
+        rfbi_get_clk_info(&extif_clk_period, &max_clk_div);
+        for (div = 1; div <= max_clk_div; div++) {
+                if (calc_reg_timing(t, div) == 0)
+                        break;
+        }
+
+        if (div <= max_clk_div)
+                return 0;
+
+        DSSERR("can't setup timings\n");
+        return -1;
+}
+
+
+static void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
+{
+        int r;
+
+        if (!t->converted) {
+                r = calc_extif_timings(t);
+                if (r < 0)
+                        DSSERR("Failed to calc timings\n");
+        }
+
+        BUG_ON(!t->converted);
+
+        rfbi_write_reg(RFBI_ONOFF_TIME(rfbi_module), t->tim[0]);
+        rfbi_write_reg(RFBI_CYCLE_TIME(rfbi_module), t->tim[1]);
+
+        /* TIMEGRANULARITY */
+        REG_FLD_MOD(RFBI_CONFIG(rfbi_module),
+                    (t->tim[2] ? 1 : 0), 4, 4);
+
+        rfbi_print_timings();
+}
+
+static int ps_to_rfbi_ticks(int time, int div)
+{
+        unsigned long tick_ps;
+        int ret;
+
+        /* Calculate in picosecs to yield more exact results */
+        tick_ps = 1000000000 / (rfbi.l4_khz) * div;
+
+        ret = (time + tick_ps - 1) / tick_ps;
+
+        return ret;
+}
+
+static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div)
+{
+        *clk_period = 1000000000 / rfbi.l4_khz;
+        *max_clk_div = 2;
+}
+
+static int rfbi_convert_timings(struct rfbi_timings *t)
+{
+        u32 l;
+        int reon, reoff, weon, weoff, cson, csoff, cs_pulse;
+        int actim, recyc, wecyc;
+        int div = t->clk_div;
+
+        if (div <= 0 || div > 2)
+                return -1;
+
+        /* Make sure that after conversion it still holds that:
+         * weoff > weon, reoff > reon, recyc >= reoff, wecyc >= weoff,
+         * csoff > cson, csoff >= max(weoff, reoff), actim > reon
+         */
+        weon = ps_to_rfbi_ticks(t->we_on_time, div);
+        weoff = ps_to_rfbi_ticks(t->we_off_time, div);
+        if (weoff <= weon)
+                weoff = weon + 1;
+        if (weon > 0x0f)
+                return -1;
+        if (weoff > 0x3f)
+                return -1;
+
+        reon = ps_to_rfbi_ticks(t->re_on_time, div);
+        reoff = ps_to_rfbi_ticks(t->re_off_time, div);
+        if (reoff <= reon)
+                reoff = reon + 1;
+        if (reon > 0x0f)
+                return -1;
+        if (reoff > 0x3f)
+                return -1;
+
+        cson = ps_to_rfbi_ticks(t->cs_on_time, div);
+        csoff = ps_to_rfbi_ticks(t->cs_off_time, div);
+        if (csoff <= cson)
+                csoff = cson + 1;
+        if (csoff < max(weoff, reoff))
+                csoff = max(weoff, reoff);
+        if (cson > 0x0f)
+                return -1;
+        if (csoff > 0x3f)
+                return -1;
+
+        l =  cson;
+        l |= csoff << 4;
+        l |= weon  << 10;
+        l |= weoff << 14;
+        l |= reon  << 20;
+        l |= reoff << 24;
+
+        t->tim[0] = l;
+
+        actim = ps_to_rfbi_ticks(t->access_time, div);
+        if (actim <= reon)
+                actim = reon + 1;
+        if (actim > 0x3f)
+                return -1;
+
+        wecyc = ps_to_rfbi_ticks(t->we_cycle_time, div);
+        if (wecyc < weoff)
+                wecyc = weoff;
+        if (wecyc > 0x3f)
+                return -1;
+
+        recyc = ps_to_rfbi_ticks(t->re_cycle_time, div);
+        if (recyc < reoff)
+                recyc = reoff;
+        if (recyc > 0x3f)
+                return -1;
+
+        cs_pulse = ps_to_rfbi_ticks(t->cs_pulse_width, div);
+        if (cs_pulse > 0x3f)
+                return -1;
+
+        l =  wecyc;
+        l |= recyc    << 6;
+        l |= cs_pulse << 12;
+        l |= actim    << 22;
+
+        t->tim[1] = l;
+
+        t->tim[2] = div - 1;
+
+        t->converted = 1;
+
+        return 0;
+}
+
+/* xxx FIX module selection missing */
+static int rfbi_setup_te(enum omap_rfbi_te_mode mode,
+                             unsigned hs_pulse_time, unsigned vs_pulse_time,
+                             int hs_pol_inv, int vs_pol_inv, int extif_div)
+{
+        int hs, vs;
+        int min;
+        u32 l;
+
+        hs = ps_to_rfbi_ticks(hs_pulse_time, 1);
+        vs = ps_to_rfbi_ticks(vs_pulse_time, 1);
+        if (hs < 2)
+                return -EDOM;
+        if (mode == OMAP_DSS_RFBI_TE_MODE_2)
+                min = 2;
+        else /* OMAP_DSS_RFBI_TE_MODE_1 */
+                min = 4;
+        if (vs < min)
+                return -EDOM;
+        if (vs == hs)
+                return -EINVAL;
+        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_write_reg(RFBI_HSYNC_WIDTH, hs);
+        rfbi_write_reg(RFBI_VSYNC_WIDTH, vs);
+
+        l = rfbi_read_reg(RFBI_CONFIG(0));
+        if (hs_pol_inv)
+                l &= ~(1 << 21);
+        else
+                l |= 1 << 21;
+        if (vs_pol_inv)
+                l &= ~(1 << 20);
+        else
+                l |= 1 << 20;
+
+        return 0;
+}
+
+/* xxx FIX module selection missing */
+static int rfbi_enable_te(bool enable, unsigned line)
+{
+        u32 l;
+
+        DSSDBG("te %d line %d mode %d\n", enable, line, rfbi.te_mode);
+        if (line > (1 << 11) - 1)
+                return -EINVAL;
+
+        l = rfbi_read_reg(RFBI_CONFIG(0));
+        l &= ~(0x3 << 2);
+        if (enable) {
+                rfbi.te_enabled = 1;
+                l |= rfbi.te_mode << 2;
+        } else
+                rfbi.te_enabled = 0;
+        rfbi_write_reg(RFBI_CONFIG(0), l);
+        rfbi_write_reg(RFBI_LINE_NUMBER, line);
+
+        return 0;
+}
+
+static int rfbi_configure_bus(int rfbi_module, int bpp, int lines)
+{
+        u32 l;
+        int cycle1 = 0, cycle2 = 0, cycle3 = 0;
+        enum omap_rfbi_cycleformat cycleformat;
+        enum omap_rfbi_datatype datatype;
+        enum omap_rfbi_parallelmode parallelmode;
+
+        switch (bpp) {
+        case 12:
+                datatype = OMAP_DSS_RFBI_DATATYPE_12;
+                break;
+        case 16:
+                datatype = OMAP_DSS_RFBI_DATATYPE_16;
+                break;
+        case 18:
+                datatype = OMAP_DSS_RFBI_DATATYPE_18;
+                break;
+        case 24:
+                datatype = OMAP_DSS_RFBI_DATATYPE_24;
+                break;
+        default:
+                BUG();
+                return 1;
+        }
+        rfbi.datatype = datatype;
+
+        switch (lines) {
+        case 8:
+                parallelmode = OMAP_DSS_RFBI_PARALLELMODE_8;
+                break;
+        case 9:
+                parallelmode = OMAP_DSS_RFBI_PARALLELMODE_9;
+                break;
+        case 12:
+                parallelmode = OMAP_DSS_RFBI_PARALLELMODE_12;
+                break;
+        case 16:
+                parallelmode = OMAP_DSS_RFBI_PARALLELMODE_16;
+                break;
+        default:
+                BUG();
+                return 1;
+        }
+        rfbi.parallelmode = parallelmode;
+
+        if ((bpp % lines) == 0) {
+                switch (bpp / lines) {
+                case 1:
+                        cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_1_1;
+                        break;
+                case 2:
+                        cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_2_1;
+                        break;
+                case 3:
+                        cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_3_1;
+                        break;
+                default:
+                        BUG();
+                        return 1;
+                }
+        } else if ((2 * bpp % lines) == 0) {
+                if ((2 * bpp / lines) == 3)
+                        cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_3_2;
+                else {
+                        BUG();
+                        return 1;
+                }
+        } else {
+                BUG();
+                return 1;
+        }
+
+        switch (cycleformat) {
+        case OMAP_DSS_RFBI_CYCLEFORMAT_1_1:
+                cycle1 = lines;
+                break;
+
+        case OMAP_DSS_RFBI_CYCLEFORMAT_2_1:
+                cycle1 = lines;
+                cycle2 = lines;
+                break;
+
+        case OMAP_DSS_RFBI_CYCLEFORMAT_3_1:
+                cycle1 = lines;
+                cycle2 = lines;
+                cycle3 = lines;
+                break;
+
+        case OMAP_DSS_RFBI_CYCLEFORMAT_3_2:
+                cycle1 = lines;
+                cycle2 = (lines / 2) | ((lines / 2) << 16);
+                cycle3 = (lines << 16);
+                break;
+        }
+
+        REG_FLD_MOD(RFBI_CONTROL, 0, 3, 2); /* clear CS */
+
+        l = 0;
+        l |= FLD_VAL(parallelmode, 1, 0);
+        l |= FLD_VAL(0, 3, 2);          /* TRIGGERMODE: ITE */
+        l |= FLD_VAL(0, 4, 4);          /* TIMEGRANULARITY */
+        l |= FLD_VAL(datatype, 6, 5);
+        /* l |= FLD_VAL(2, 8, 7); */    /* L4FORMAT, 2pix/L4 */
+        l |= FLD_VAL(0, 8, 7);  /* L4FORMAT, 1pix/L4 */
+        l |= FLD_VAL(cycleformat, 10, 9);
+        l |= FLD_VAL(0, 12, 11);        /* UNUSEDBITS */
+        l |= FLD_VAL(0, 16, 16);        /* A0POLARITY */
+        l |= FLD_VAL(0, 17, 17);        /* REPOLARITY */
+        l |= FLD_VAL(0, 18, 18);        /* WEPOLARITY */
+        l |= FLD_VAL(0, 19, 19);        /* CSPOLARITY */
+        l |= FLD_VAL(1, 20, 20);        /* TE_VSYNC_POLARITY */
+        l |= FLD_VAL(1, 21, 21);        /* HSYNCPOLARITY */
+        rfbi_write_reg(RFBI_CONFIG(rfbi_module), l);
+
+        rfbi_write_reg(RFBI_DATA_CYCLE1(rfbi_module), cycle1);
+        rfbi_write_reg(RFBI_DATA_CYCLE2(rfbi_module), cycle2);
+        rfbi_write_reg(RFBI_DATA_CYCLE3(rfbi_module), cycle3);
+
+
+        l = rfbi_read_reg(RFBI_CONTROL);
+        l = FLD_MOD(l, rfbi_module+1, 3, 2); /* Select CSx */
+        l = FLD_MOD(l, 0, 1, 1); /* clear bypass */
+        rfbi_write_reg(RFBI_CONTROL, l);
+
+
+        DSSDBG("RFBI config: bpp %d, lines %d, cycles: 0x%x 0x%x 0x%x\n",
+               bpp, lines, cycle1, cycle2, cycle3);
+
+        return 0;
+}
+
+static int rfbi_configure(struct omap_dss_device *dssdev)
+{
+        return rfbi_configure_bus(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+                        rfbi.data_lines);
+}
+
+static int rfbi_update(struct omap_dss_device *dssdev, void (*callback)(void *),
+                void *data)
+{
+        return rfbi_transfer_area(dssdev, callback, data);
+}
+
+static void rfbi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+{
+        rfbi.timings.x_res = w;
+        rfbi.timings.y_res = h;
+}
+
+static void rfbi_set_pixel_size(struct omap_dss_device *dssdev, int pixel_size)
+{
+        rfbi.pixel_size = pixel_size;
+}
+
+static void rfbi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+        rfbi.data_lines = data_lines;
+}
+
+static void rfbi_set_interface_timings(struct omap_dss_device *dssdev,
+                struct rfbi_timings *timings)
+{
+        rfbi.intf_timings = *timings;
+}
+
+static void rfbi_dump_regs(struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, rfbi_read_reg(r))
+
+        if (rfbi_runtime_get())
+                return;
+
+        DUMPREG(RFBI_REVISION);
+        DUMPREG(RFBI_SYSCONFIG);
+        DUMPREG(RFBI_SYSSTATUS);
+        DUMPREG(RFBI_CONTROL);
+        DUMPREG(RFBI_PIXEL_CNT);
+        DUMPREG(RFBI_LINE_NUMBER);
+        DUMPREG(RFBI_CMD);
+        DUMPREG(RFBI_PARAM);
+        DUMPREG(RFBI_DATA);
+        DUMPREG(RFBI_READ);
+        DUMPREG(RFBI_STATUS);
+
+        DUMPREG(RFBI_CONFIG(0));
+        DUMPREG(RFBI_ONOFF_TIME(0));
+        DUMPREG(RFBI_CYCLE_TIME(0));
+        DUMPREG(RFBI_DATA_CYCLE1(0));
+        DUMPREG(RFBI_DATA_CYCLE2(0));
+        DUMPREG(RFBI_DATA_CYCLE3(0));
+
+        DUMPREG(RFBI_CONFIG(1));
+        DUMPREG(RFBI_ONOFF_TIME(1));
+        DUMPREG(RFBI_CYCLE_TIME(1));
+        DUMPREG(RFBI_DATA_CYCLE1(1));
+        DUMPREG(RFBI_DATA_CYCLE2(1));
+        DUMPREG(RFBI_DATA_CYCLE3(1));
+
+        DUMPREG(RFBI_VSYNC_WIDTH);
+        DUMPREG(RFBI_HSYNC_WIDTH);
+
+        rfbi_runtime_put();
+#undef DUMPREG
+}
+
+static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = rfbi.output.manager;
+        struct dss_lcd_mgr_config mgr_config;
+
+        mgr_config.io_pad_mode = DSS_IO_PAD_MODE_RFBI;
+
+        mgr_config.stallmode = true;
+        /* Do we need fifohandcheck for RFBI? */
+        mgr_config.fifohandcheck = false;
+
+        mgr_config.video_port_width = rfbi.pixel_size;
+        mgr_config.lcden_sig_polarity = 0;
+
+        dss_mgr_set_lcd_config(mgr, &mgr_config);
+
+        /*
+         * Set rfbi.timings with default values, the x_res and y_res fields
+         * are expected to be already configured by the panel driver via
+         * omapdss_rfbi_set_size()
+         */
+        rfbi.timings.hsw = 1;
+        rfbi.timings.hfp = 1;
+        rfbi.timings.hbp = 1;
+        rfbi.timings.vsw = 1;
+        rfbi.timings.vfp = 0;
+        rfbi.timings.vbp = 0;
+
+        rfbi.timings.interlace = false;
+        rfbi.timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        rfbi.timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        rfbi.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        rfbi.timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+        rfbi.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE;
+
+        dss_mgr_set_timings(mgr, &rfbi.timings);
+}
+
+static int rfbi_display_enable(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out = &rfbi.output;
+        int r;
+
+        if (out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                return -ENODEV;
+        }
+
+        r = rfbi_runtime_get();
+        if (r)
+                return r;
+
+        r = dss_mgr_register_framedone_handler(out->manager,
+                        framedone_callback, NULL);
+        if (r) {
+                DSSERR("can't get FRAMEDONE irq\n");
+                goto err1;
+        }
+
+        rfbi_config_lcd_manager(dssdev);
+
+        rfbi_configure_bus(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+                        rfbi.data_lines);
+
+        rfbi_set_timings(dssdev->phy.rfbi.channel, &rfbi.intf_timings);
+
+        return 0;
+err1:
+        rfbi_runtime_put();
+        return r;
+}
+
+static void rfbi_display_disable(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out = &rfbi.output;
+
+        dss_mgr_unregister_framedone_handler(out->manager,
+                        framedone_callback, NULL);
+
+        rfbi_runtime_put();
+}
+
+static int rfbi_init_display(struct omap_dss_device *dssdev)
+{
+        rfbi.dssdev[dssdev->phy.rfbi.channel] = dssdev;
+        return 0;
+}
+
+static void rfbi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &rfbi.output;
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_DBI;
+        out->output_type = OMAP_DISPLAY_TYPE_DBI;
+        out->name = "rfbi.0";
+        out->dispc_channel = OMAP_DSS_CHANNEL_LCD;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void rfbi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &rfbi.output;
+
+        omapdss_unregister_output(out);
+}
+
+/* RFBI HW IP initialisation */
+static int rfbi_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        u32 rev;
+        struct resource *rfbi_mem;
+        struct clk *clk;
+        int r;
+
+        rfbi.pdev = pdev;
+
+        sema_init(&rfbi.bus_lock, 1);
+
+        rfbi_mem = platform_get_resource(rfbi.pdev, IORESOURCE_MEM, 0);
+        if (!rfbi_mem) {
+                DSSERR("can't get IORESOURCE_MEM RFBI\n");
+                return -EINVAL;
+        }
+
+        rfbi.base = devm_ioremap(&pdev->dev, rfbi_mem->start,
+                                 resource_size(rfbi_mem));
+        if (!rfbi.base) {
+                DSSERR("can't ioremap RFBI\n");
+                return -ENOMEM;
+        }
+
+        clk = clk_get(&pdev->dev, "ick");
+        if (IS_ERR(clk)) {
+                DSSERR("can't get ick\n");
+                return PTR_ERR(clk);
+        }
+
+        rfbi.l4_khz = clk_get_rate(clk) / 1000;
+
+        clk_put(clk);
+
+        pm_runtime_enable(&pdev->dev);
+
+        r = rfbi_runtime_get();
+        if (r)
+                goto err_runtime_get;
+
+        msleep(10);
+
+        rev = rfbi_read_reg(RFBI_REVISION);
+        dev_dbg(&pdev->dev, "OMAP RFBI rev %d.%d\n",
+               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+        rfbi_runtime_put();
+
+        dss_debugfs_create_file("rfbi", rfbi_dump_regs);
+
+        rfbi_init_output(pdev);
+
+        return 0;
+
+err_runtime_get:
+        pm_runtime_disable(&pdev->dev);
+        return r;
+}
+
+static void rfbi_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        rfbi_uninit_output(pdev);
+
+        pm_runtime_disable(&pdev->dev);
+
+        return 0;
+}
+
+static const struct component_ops rfbi_component_ops = {
+        .bind   = rfbi_bind,
+        .unbind = rfbi_unbind,
+};
+
+static int rfbi_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &rfbi_component_ops);
+}
+
+static int rfbi_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &rfbi_component_ops);
+        return 0;
+}
+
+static int rfbi_runtime_suspend(struct device *dev)
+{
+        dispc_runtime_put();
+
+        return 0;
+}
+
+static int rfbi_runtime_resume(struct device *dev)
+{
+        int r;
+
+        r = dispc_runtime_get();
+        if (r < 0)
+                return r;
+
+        return 0;
+}
+
+static const struct dev_pm_ops rfbi_pm_ops = {
+        .runtime_suspend = rfbi_runtime_suspend,
+        .runtime_resume = rfbi_runtime_resume,
+};
+
+static struct platform_driver omap_rfbihw_driver = {
+        .probe          = rfbi_probe,
+        .remove         = rfbi_remove,
+        .driver         = {
+                .name   = "omapdss_rfbi",
+                .pm     = &rfbi_pm_ops,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init rfbi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_rfbihw_driver);
+}
+
+void rfbi_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_rfbihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/sdi.c b/drivers/video/fbdev/omap2/omapfb/dss/sdi.c
new file mode 100644
index 000000000000..d747cc6b59e1
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/sdi.c
@@ -0,0 +1,454 @@
+/*
+ * linux/drivers/video/omap2/dss/sdi.c
+ *
+ * Copyright (C) 2009 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/>.
+ */
+
+#define DSS_SUBSYS_NAME "SDI"
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/regulator/consumer.h>
+#include <linux/export.h>
+#include <linux/platform_device.h>
+#include <linux/string.h>
+#include <linux/of.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+
+static struct {
+        struct platform_device *pdev;
+
+        bool update_enabled;
+        struct regulator *vdds_sdi_reg;
+
+        struct dss_lcd_mgr_config mgr_config;
+        struct omap_video_timings timings;
+        int datapairs;
+
+        struct omap_dss_device output;
+
+        bool port_initialized;
+} sdi;
+
+struct sdi_clk_calc_ctx {
+        unsigned long pck_min, pck_max;
+
+        unsigned long fck;
+        struct dispc_clock_info dispc_cinfo;
+};
+
+static bool dpi_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+                unsigned long pck, void *data)
+{
+        struct sdi_clk_calc_ctx *ctx = data;
+
+        ctx->dispc_cinfo.lck_div = lckd;
+        ctx->dispc_cinfo.pck_div = pckd;
+        ctx->dispc_cinfo.lck = lck;
+        ctx->dispc_cinfo.pck = pck;
+
+        return true;
+}
+
+static bool dpi_calc_dss_cb(unsigned long fck, void *data)
+{
+        struct sdi_clk_calc_ctx *ctx = data;
+
+        ctx->fck = fck;
+
+        return dispc_div_calc(fck, ctx->pck_min, ctx->pck_max,
+                        dpi_calc_dispc_cb, ctx);
+}
+
+static int sdi_calc_clock_div(unsigned long pclk,
+                unsigned long *fck,
+                struct dispc_clock_info *dispc_cinfo)
+{
+        int i;
+        struct sdi_clk_calc_ctx ctx;
+
+        /*
+         * DSS fclk gives us very few possibilities, so finding a good pixel
+         * clock may not be possible. We try multiple times to find the clock,
+         * each time widening the pixel clock range we look for, up to
+         * +/- 1MHz.
+         */
+
+        for (i = 0; i < 10; ++i) {
+                bool ok;
+
+                memset(&ctx, 0, sizeof(ctx));
+                if (pclk > 1000 * i * i * i)
+                        ctx.pck_min = max(pclk - 1000 * i * i * i, 0lu);
+                else
+                        ctx.pck_min = 0;
+                ctx.pck_max = pclk + 1000 * i * i * i;
+
+                ok = dss_div_calc(pclk, ctx.pck_min, dpi_calc_dss_cb, &ctx);
+                if (ok) {
+                        *fck = ctx.fck;
+                        *dispc_cinfo = ctx.dispc_cinfo;
+                        return 0;
+                }
+        }
+
+        return -EINVAL;
+}
+
+static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = sdi.output.manager;
+
+        sdi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+
+        sdi.mgr_config.stallmode = false;
+        sdi.mgr_config.fifohandcheck = false;
+
+        sdi.mgr_config.video_port_width = 24;
+        sdi.mgr_config.lcden_sig_polarity = 1;
+
+        dss_mgr_set_lcd_config(mgr, &sdi.mgr_config);
+}
+
+static int sdi_display_enable(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out = &sdi.output;
+        struct omap_video_timings *t = &sdi.timings;
+        unsigned long fck;
+        struct dispc_clock_info dispc_cinfo;
+        unsigned long pck;
+        int r;
+
+        if (out->manager == NULL) {
+                DSSERR("failed to enable display: no output/manager\n");
+                return -ENODEV;
+        }
+
+        r = regulator_enable(sdi.vdds_sdi_reg);
+        if (r)
+                goto err_reg_enable;
+
+        r = dispc_runtime_get();
+        if (r)
+                goto err_get_dispc;
+
+        /* 15.5.9.1.2 */
+        t->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+        t->sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+
+        r = sdi_calc_clock_div(t->pixelclock, &fck, &dispc_cinfo);
+        if (r)
+                goto err_calc_clock_div;
+
+        sdi.mgr_config.clock_info = dispc_cinfo;
+
+        pck = fck / dispc_cinfo.lck_div / dispc_cinfo.pck_div;
+
+        if (pck != t->pixelclock) {
+                DSSWARN("Could not find exact pixel clock. Requested %d Hz, got %lu Hz\n",
+                        t->pixelclock, pck);
+
+                t->pixelclock = pck;
+        }
+
+
+        dss_mgr_set_timings(out->manager, t);
+
+        r = dss_set_fck_rate(fck);
+        if (r)
+                goto err_set_dss_clock_div;
+
+        sdi_config_lcd_manager(dssdev);
+
+        /*
+         * LCLK and PCLK divisors are located in shadow registers, and we
+         * normally write them to DISPC registers when enabling the output.
+         * However, SDI uses pck-free as source clock for its PLL, and pck-free
+         * is affected by the divisors. And as we need the PLL before enabling
+         * the output, we need to write the divisors early.
+         *
+         * It seems just writing to the DISPC register is enough, and we don't
+         * need to care about the shadow register mechanism for pck-free. The
+         * exact reason for this is unknown.
+         */
+        dispc_mgr_set_clock_div(out->manager->id, &sdi.mgr_config.clock_info);
+
+        dss_sdi_init(sdi.datapairs);
+        r = dss_sdi_enable();
+        if (r)
+                goto err_sdi_enable;
+        mdelay(2);
+
+        r = dss_mgr_enable(out->manager);
+        if (r)
+                goto err_mgr_enable;
+
+        return 0;
+
+err_mgr_enable:
+        dss_sdi_disable();
+err_sdi_enable:
+err_set_dss_clock_div:
+err_calc_clock_div:
+        dispc_runtime_put();
+err_get_dispc:
+        regulator_disable(sdi.vdds_sdi_reg);
+err_reg_enable:
+        return r;
+}
+
+static void sdi_display_disable(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = sdi.output.manager;
+
+        dss_mgr_disable(mgr);
+
+        dss_sdi_disable();
+
+        dispc_runtime_put();
+
+        regulator_disable(sdi.vdds_sdi_reg);
+}
+
+static void sdi_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        sdi.timings = *timings;
+}
+
+static void sdi_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        *timings = sdi.timings;
+}
+
+static int sdi_check_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        struct omap_overlay_manager *mgr = sdi.output.manager;
+
+        if (mgr && !dispc_mgr_timings_ok(mgr->id, timings))
+                return -EINVAL;
+
+        if (timings->pixelclock == 0)
+                return -EINVAL;
+
+        return 0;
+}
+
+static void sdi_set_datapairs(struct omap_dss_device *dssdev, int datapairs)
+{
+        sdi.datapairs = datapairs;
+}
+
+static int sdi_init_regulator(void)
+{
+        struct regulator *vdds_sdi;
+
+        if (sdi.vdds_sdi_reg)
+                return 0;
+
+        vdds_sdi = devm_regulator_get(&sdi.pdev->dev, "vdds_sdi");
+        if (IS_ERR(vdds_sdi)) {
+                if (PTR_ERR(vdds_sdi) != -EPROBE_DEFER)
+                        DSSERR("can't get VDDS_SDI regulator\n");
+                return PTR_ERR(vdds_sdi);
+        }
+
+        sdi.vdds_sdi_reg = vdds_sdi;
+
+        return 0;
+}
+
+static int sdi_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct omap_overlay_manager *mgr;
+        int r;
+
+        r = sdi_init_regulator();
+        if (r)
+                return r;
+
+        mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+        if (!mgr)
+                return -ENODEV;
+
+        r = dss_mgr_connect(mgr, dssdev);
+        if (r)
+                return r;
+
+        r = omapdss_output_set_device(dssdev, dst);
+        if (r) {
+                DSSERR("failed to connect output to new device: %s\n",
+                                dst->name);
+                dss_mgr_disconnect(mgr, dssdev);
+                return r;
+        }
+
+        return 0;
+}
+
+static void sdi_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        omapdss_output_unset_device(dssdev);
+
+        if (dssdev->manager)
+                dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_sdi_ops sdi_ops = {
+        .connect = sdi_connect,
+        .disconnect = sdi_disconnect,
+
+        .enable = sdi_display_enable,
+        .disable = sdi_display_disable,
+
+        .check_timings = sdi_check_timings,
+        .set_timings = sdi_set_timings,
+        .get_timings = sdi_get_timings,
+
+        .set_datapairs = sdi_set_datapairs,
+};
+
+static void sdi_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &sdi.output;
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_SDI;
+        out->output_type = OMAP_DISPLAY_TYPE_SDI;
+        out->name = "sdi.0";
+        out->dispc_channel = OMAP_DSS_CHANNEL_LCD;
+        /* We have SDI only on OMAP3, where it's on port 1 */
+        out->port_num = 1;
+        out->ops.sdi = &sdi_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void sdi_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &sdi.output;
+
+        omapdss_unregister_output(out);
+}
+
+static int sdi_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        sdi.pdev = pdev;
+
+        sdi_init_output(pdev);
+
+        return 0;
+}
+
+static void sdi_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        sdi_uninit_output(pdev);
+}
+
+static const struct component_ops sdi_component_ops = {
+        .bind   = sdi_bind,
+        .unbind = sdi_unbind,
+};
+
+static int sdi_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &sdi_component_ops);
+}
+
+static int sdi_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &sdi_component_ops);
+        return 0;
+}
+
+static struct platform_driver omap_sdi_driver = {
+        .probe          = sdi_probe,
+        .remove         = sdi_remove,
+        .driver         = {
+                .name   = "omapdss_sdi",
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init sdi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_sdi_driver);
+}
+
+void sdi_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_sdi_driver);
+}
+
+int sdi_init_port(struct platform_device *pdev, struct device_node *port)
+{
+        struct device_node *ep;
+        u32 datapairs;
+        int r;
+
+        ep = omapdss_of_get_next_endpoint(port, NULL);
+        if (!ep)
+                return 0;
+
+        r = of_property_read_u32(ep, "datapairs", &datapairs);
+        if (r) {
+                DSSERR("failed to parse datapairs\n");
+                goto err_datapairs;
+        }
+
+        sdi.datapairs = datapairs;
+
+        of_node_put(ep);
+
+        sdi.pdev = pdev;
+
+        sdi_init_output(pdev);
+
+        sdi.port_initialized = true;
+
+        return 0;
+
+err_datapairs:
+        of_node_put(ep);
+
+        return r;
+}
+
+void sdi_uninit_port(struct device_node *port)
+{
+        if (!sdi.port_initialized)
+                return;
+
+        sdi_uninit_output(sdi.pdev);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/venc.c b/drivers/video/fbdev/omap2/omapfb/dss/venc.c
new file mode 100644
index 000000000000..26e0ee30adf8
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/venc.c
@@ -0,0 +1,997 @@
+/*
+ * linux/drivers/video/omap2/dss/venc.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * VENC settings from TI's DSS driver
+ *
+ * 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 "VENC"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+/* Venc registers */
+#define VENC_REV_ID                             0x00
+#define VENC_STATUS                             0x04
+#define VENC_F_CONTROL                          0x08
+#define VENC_VIDOUT_CTRL                        0x10
+#define VENC_SYNC_CTRL                          0x14
+#define VENC_LLEN                               0x1C
+#define VENC_FLENS                              0x20
+#define VENC_HFLTR_CTRL                         0x24
+#define VENC_CC_CARR_WSS_CARR                   0x28
+#define VENC_C_PHASE                            0x2C
+#define VENC_GAIN_U                             0x30
+#define VENC_GAIN_V                             0x34
+#define VENC_GAIN_Y                             0x38
+#define VENC_BLACK_LEVEL                        0x3C
+#define VENC_BLANK_LEVEL                        0x40
+#define VENC_X_COLOR                            0x44
+#define VENC_M_CONTROL                          0x48
+#define VENC_BSTAMP_WSS_DATA                    0x4C
+#define VENC_S_CARR                             0x50
+#define VENC_LINE21                             0x54
+#define VENC_LN_SEL                             0x58
+#define VENC_L21__WC_CTL                        0x5C
+#define VENC_HTRIGGER_VTRIGGER                  0x60
+#define VENC_SAVID__EAVID                       0x64
+#define VENC_FLEN__FAL                          0x68
+#define VENC_LAL__PHASE_RESET                   0x6C
+#define VENC_HS_INT_START_STOP_X                0x70
+#define VENC_HS_EXT_START_STOP_X                0x74
+#define VENC_VS_INT_START_X                     0x78
+#define VENC_VS_INT_STOP_X__VS_INT_START_Y      0x7C
+#define VENC_VS_INT_STOP_Y__VS_EXT_START_X      0x80
+#define VENC_VS_EXT_STOP_X__VS_EXT_START_Y      0x84
+#define VENC_VS_EXT_STOP_Y                      0x88
+#define VENC_AVID_START_STOP_X                  0x90
+#define VENC_AVID_START_STOP_Y                  0x94
+#define VENC_FID_INT_START_X__FID_INT_START_Y   0xA0
+#define VENC_FID_INT_OFFSET_Y__FID_EXT_START_X  0xA4
+#define VENC_FID_EXT_START_Y__FID_EXT_OFFSET_Y  0xA8
+#define VENC_TVDETGP_INT_START_STOP_X           0xB0
+#define VENC_TVDETGP_INT_START_STOP_Y           0xB4
+#define VENC_GEN_CTRL                           0xB8
+#define VENC_OUTPUT_CONTROL                     0xC4
+#define VENC_OUTPUT_TEST                        0xC8
+#define VENC_DAC_B__DAC_C                       0xC8
+
+struct venc_config {
+        u32 f_control;
+        u32 vidout_ctrl;
+        u32 sync_ctrl;
+        u32 llen;
+        u32 flens;
+        u32 hfltr_ctrl;
+        u32 cc_carr_wss_carr;
+        u32 c_phase;
+        u32 gain_u;
+        u32 gain_v;
+        u32 gain_y;
+        u32 black_level;
+        u32 blank_level;
+        u32 x_color;
+        u32 m_control;
+        u32 bstamp_wss_data;
+        u32 s_carr;
+        u32 line21;
+        u32 ln_sel;
+        u32 l21__wc_ctl;
+        u32 htrigger_vtrigger;
+        u32 savid__eavid;
+        u32 flen__fal;
+        u32 lal__phase_reset;
+        u32 hs_int_start_stop_x;
+        u32 hs_ext_start_stop_x;
+        u32 vs_int_start_x;
+        u32 vs_int_stop_x__vs_int_start_y;
+        u32 vs_int_stop_y__vs_ext_start_x;
+        u32 vs_ext_stop_x__vs_ext_start_y;
+        u32 vs_ext_stop_y;
+        u32 avid_start_stop_x;
+        u32 avid_start_stop_y;
+        u32 fid_int_start_x__fid_int_start_y;
+        u32 fid_int_offset_y__fid_ext_start_x;
+        u32 fid_ext_start_y__fid_ext_offset_y;
+        u32 tvdetgp_int_start_stop_x;
+        u32 tvdetgp_int_start_stop_y;
+        u32 gen_ctrl;
+};
+
+/* from TRM */
+static const struct venc_config venc_config_pal_trm = {
+        .f_control                              = 0,
+        .vidout_ctrl                            = 1,
+        .sync_ctrl                              = 0x40,
+        .llen                                   = 0x35F, /* 863 */
+        .flens                                  = 0x270, /* 624 */
+        .hfltr_ctrl                             = 0,
+        .cc_carr_wss_carr                       = 0x2F7225ED,
+        .c_phase                                = 0,
+        .gain_u                                 = 0x111,
+        .gain_v                                 = 0x181,
+        .gain_y                                 = 0x140,
+        .black_level                            = 0x3B,
+        .blank_level                            = 0x3B,
+        .x_color                                = 0x7,
+        .m_control                              = 0x2,
+        .bstamp_wss_data                        = 0x3F,
+        .s_carr                                 = 0x2A098ACB,
+        .line21                                 = 0,
+        .ln_sel                                 = 0x01290015,
+        .l21__wc_ctl                            = 0x0000F603,
+        .htrigger_vtrigger                      = 0,
+
+        .savid__eavid                           = 0x06A70108,
+        .flen__fal                              = 0x00180270,
+        .lal__phase_reset                       = 0x00040135,
+        .hs_int_start_stop_x                    = 0x00880358,
+        .hs_ext_start_stop_x                    = 0x000F035F,
+        .vs_int_start_x                         = 0x01A70000,
+        .vs_int_stop_x__vs_int_start_y          = 0x000001A7,
+        .vs_int_stop_y__vs_ext_start_x          = 0x01AF0000,
+        .vs_ext_stop_x__vs_ext_start_y          = 0x000101AF,
+        .vs_ext_stop_y                          = 0x00000025,
+        .avid_start_stop_x                      = 0x03530083,
+        .avid_start_stop_y                      = 0x026C002E,
+        .fid_int_start_x__fid_int_start_y       = 0x0001008A,
+        .fid_int_offset_y__fid_ext_start_x      = 0x002E0138,
+        .fid_ext_start_y__fid_ext_offset_y      = 0x01380001,
+
+        .tvdetgp_int_start_stop_x               = 0x00140001,
+        .tvdetgp_int_start_stop_y               = 0x00010001,
+        .gen_ctrl                               = 0x00FF0000,
+};
+
+/* from TRM */
+static const struct venc_config venc_config_ntsc_trm = {
+        .f_control                              = 0,
+        .vidout_ctrl                            = 1,
+        .sync_ctrl                              = 0x8040,
+        .llen                                   = 0x359,
+        .flens                                  = 0x20C,
+        .hfltr_ctrl                             = 0,
+        .cc_carr_wss_carr                       = 0x043F2631,
+        .c_phase                                = 0,
+        .gain_u                                 = 0x102,
+        .gain_v                                 = 0x16C,
+        .gain_y                                 = 0x12F,
+        .black_level                            = 0x43,
+        .blank_level                            = 0x38,
+        .x_color                                = 0x7,
+        .m_control                              = 0x1,
+        .bstamp_wss_data                        = 0x38,
+        .s_carr                                 = 0x21F07C1F,
+        .line21                                 = 0,
+        .ln_sel                                 = 0x01310011,
+        .l21__wc_ctl                            = 0x0000F003,
+        .htrigger_vtrigger                      = 0,
+
+        .savid__eavid                           = 0x069300F4,
+        .flen__fal                              = 0x0016020C,
+        .lal__phase_reset                       = 0x00060107,
+        .hs_int_start_stop_x                    = 0x008E0350,
+        .hs_ext_start_stop_x                    = 0x000F0359,
+        .vs_int_start_x                         = 0x01A00000,
+        .vs_int_stop_x__vs_int_start_y          = 0x020701A0,
+        .vs_int_stop_y__vs_ext_start_x          = 0x01AC0024,
+        .vs_ext_stop_x__vs_ext_start_y          = 0x020D01AC,
+        .vs_ext_stop_y                          = 0x00000006,
+        .avid_start_stop_x                      = 0x03480078,
+        .avid_start_stop_y                      = 0x02060024,
+        .fid_int_start_x__fid_int_start_y       = 0x0001008A,
+        .fid_int_offset_y__fid_ext_start_x      = 0x01AC0106,
+        .fid_ext_start_y__fid_ext_offset_y      = 0x01060006,
+
+        .tvdetgp_int_start_stop_x               = 0x00140001,
+        .tvdetgp_int_start_stop_y               = 0x00010001,
+        .gen_ctrl                               = 0x00F90000,
+};
+
+static const struct venc_config venc_config_pal_bdghi = {
+        .f_control                              = 0,
+        .vidout_ctrl                            = 0,
+        .sync_ctrl                              = 0,
+        .hfltr_ctrl                             = 0,
+        .x_color                                = 0,
+        .line21                                 = 0,
+        .ln_sel                                 = 21,
+        .htrigger_vtrigger                      = 0,
+        .tvdetgp_int_start_stop_x               = 0x00140001,
+        .tvdetgp_int_start_stop_y               = 0x00010001,
+        .gen_ctrl                               = 0x00FB0000,
+
+        .llen                                   = 864-1,
+        .flens                                  = 625-1,
+        .cc_carr_wss_carr                       = 0x2F7625ED,
+        .c_phase                                = 0xDF,
+        .gain_u                                 = 0x111,
+        .gain_v                                 = 0x181,
+        .gain_y                                 = 0x140,
+        .black_level                            = 0x3e,
+        .blank_level                            = 0x3e,
+        .m_control                              = 0<<2 | 1<<1,
+        .bstamp_wss_data                        = 0x42,
+        .s_carr                                 = 0x2a098acb,
+        .l21__wc_ctl                            = 0<<13 | 0x16<<8 | 0<<0,
+        .savid__eavid                           = 0x06A70108,
+        .flen__fal                              = 23<<16 | 624<<0,
+        .lal__phase_reset                       = 2<<17 | 310<<0,
+        .hs_int_start_stop_x                    = 0x00920358,
+        .hs_ext_start_stop_x                    = 0x000F035F,
+        .vs_int_start_x                         = 0x1a7<<16,
+        .vs_int_stop_x__vs_int_start_y          = 0x000601A7,
+        .vs_int_stop_y__vs_ext_start_x          = 0x01AF0036,
+        .vs_ext_stop_x__vs_ext_start_y          = 0x27101af,
+        .vs_ext_stop_y                          = 0x05,
+        .avid_start_stop_x                      = 0x03530082,
+        .avid_start_stop_y                      = 0x0270002E,
+        .fid_int_start_x__fid_int_start_y       = 0x0005008A,
+        .fid_int_offset_y__fid_ext_start_x      = 0x002E0138,
+        .fid_ext_start_y__fid_ext_offset_y      = 0x01380005,
+};
+
+const struct omap_video_timings omap_dss_pal_timings = {
+        .x_res          = 720,
+        .y_res          = 574,
+        .pixelclock     = 13500000,
+        .hsw            = 64,
+        .hfp            = 12,
+        .hbp            = 68,
+        .vsw            = 5,
+        .vfp            = 5,
+        .vbp            = 41,
+
+        .interlace      = true,
+};
+EXPORT_SYMBOL(omap_dss_pal_timings);
+
+const struct omap_video_timings omap_dss_ntsc_timings = {
+        .x_res          = 720,
+        .y_res          = 482,
+        .pixelclock     = 13500000,
+        .hsw            = 64,
+        .hfp            = 16,
+        .hbp            = 58,
+        .vsw            = 6,
+        .vfp            = 6,
+        .vbp            = 31,
+
+        .interlace      = true,
+};
+EXPORT_SYMBOL(omap_dss_ntsc_timings);
+
+static struct {
+        struct platform_device *pdev;
+        void __iomem *base;
+        struct mutex venc_lock;
+        u32 wss_data;
+        struct regulator *vdda_dac_reg;
+
+        struct clk      *tv_dac_clk;
+
+        struct omap_video_timings timings;
+        enum omap_dss_venc_type type;
+        bool invert_polarity;
+
+        struct omap_dss_device output;
+} venc;
+
+static inline void venc_write_reg(int idx, u32 val)
+{
+        __raw_writel(val, venc.base + idx);
+}
+
+static inline u32 venc_read_reg(int idx)
+{
+        u32 l = __raw_readl(venc.base + idx);
+        return l;
+}
+
+static void venc_write_config(const struct venc_config *config)
+{
+        DSSDBG("write venc conf\n");
+
+        venc_write_reg(VENC_LLEN, config->llen);
+        venc_write_reg(VENC_FLENS, config->flens);
+        venc_write_reg(VENC_CC_CARR_WSS_CARR, config->cc_carr_wss_carr);
+        venc_write_reg(VENC_C_PHASE, config->c_phase);
+        venc_write_reg(VENC_GAIN_U, config->gain_u);
+        venc_write_reg(VENC_GAIN_V, config->gain_v);
+        venc_write_reg(VENC_GAIN_Y, config->gain_y);
+        venc_write_reg(VENC_BLACK_LEVEL, config->black_level);
+        venc_write_reg(VENC_BLANK_LEVEL, config->blank_level);
+        venc_write_reg(VENC_M_CONTROL, config->m_control);
+        venc_write_reg(VENC_BSTAMP_WSS_DATA, config->bstamp_wss_data |
+                        venc.wss_data);
+        venc_write_reg(VENC_S_CARR, config->s_carr);
+        venc_write_reg(VENC_L21__WC_CTL, config->l21__wc_ctl);
+        venc_write_reg(VENC_SAVID__EAVID, config->savid__eavid);
+        venc_write_reg(VENC_FLEN__FAL, config->flen__fal);
+        venc_write_reg(VENC_LAL__PHASE_RESET, config->lal__phase_reset);
+        venc_write_reg(VENC_HS_INT_START_STOP_X, config->hs_int_start_stop_x);
+        venc_write_reg(VENC_HS_EXT_START_STOP_X, config->hs_ext_start_stop_x);
+        venc_write_reg(VENC_VS_INT_START_X, config->vs_int_start_x);
+        venc_write_reg(VENC_VS_INT_STOP_X__VS_INT_START_Y,
+                       config->vs_int_stop_x__vs_int_start_y);
+        venc_write_reg(VENC_VS_INT_STOP_Y__VS_EXT_START_X,
+                       config->vs_int_stop_y__vs_ext_start_x);
+        venc_write_reg(VENC_VS_EXT_STOP_X__VS_EXT_START_Y,
+                       config->vs_ext_stop_x__vs_ext_start_y);
+        venc_write_reg(VENC_VS_EXT_STOP_Y, config->vs_ext_stop_y);
+        venc_write_reg(VENC_AVID_START_STOP_X, config->avid_start_stop_x);
+        venc_write_reg(VENC_AVID_START_STOP_Y, config->avid_start_stop_y);
+        venc_write_reg(VENC_FID_INT_START_X__FID_INT_START_Y,
+                       config->fid_int_start_x__fid_int_start_y);
+        venc_write_reg(VENC_FID_INT_OFFSET_Y__FID_EXT_START_X,
+                       config->fid_int_offset_y__fid_ext_start_x);
+        venc_write_reg(VENC_FID_EXT_START_Y__FID_EXT_OFFSET_Y,
+                       config->fid_ext_start_y__fid_ext_offset_y);
+
+        venc_write_reg(VENC_DAC_B__DAC_C,  venc_read_reg(VENC_DAC_B__DAC_C));
+        venc_write_reg(VENC_VIDOUT_CTRL, config->vidout_ctrl);
+        venc_write_reg(VENC_HFLTR_CTRL, config->hfltr_ctrl);
+        venc_write_reg(VENC_X_COLOR, config->x_color);
+        venc_write_reg(VENC_LINE21, config->line21);
+        venc_write_reg(VENC_LN_SEL, config->ln_sel);
+        venc_write_reg(VENC_HTRIGGER_VTRIGGER, config->htrigger_vtrigger);
+        venc_write_reg(VENC_TVDETGP_INT_START_STOP_X,
+                       config->tvdetgp_int_start_stop_x);
+        venc_write_reg(VENC_TVDETGP_INT_START_STOP_Y,
+                       config->tvdetgp_int_start_stop_y);
+        venc_write_reg(VENC_GEN_CTRL, config->gen_ctrl);
+        venc_write_reg(VENC_F_CONTROL, config->f_control);
+        venc_write_reg(VENC_SYNC_CTRL, config->sync_ctrl);
+}
+
+static void venc_reset(void)
+{
+        int t = 1000;
+
+        venc_write_reg(VENC_F_CONTROL, 1<<8);
+        while (venc_read_reg(VENC_F_CONTROL) & (1<<8)) {
+                if (--t == 0) {
+                        DSSERR("Failed to reset venc\n");
+                        return;
+                }
+        }
+
+#ifdef CONFIG_FB_OMAP2_DSS_SLEEP_AFTER_VENC_RESET
+        /* the magical sleep that makes things work */
+        /* XXX more info? What bug this circumvents? */
+        msleep(20);
+#endif
+}
+
+static int venc_runtime_get(void)
+{
+        int r;
+
+        DSSDBG("venc_runtime_get\n");
+
+        r = pm_runtime_get_sync(&venc.pdev->dev);
+        WARN_ON(r < 0);
+        return r < 0 ? r : 0;
+}
+
+static void venc_runtime_put(void)
+{
+        int r;
+
+        DSSDBG("venc_runtime_put\n");
+
+        r = pm_runtime_put_sync(&venc.pdev->dev);
+        WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static const struct venc_config *venc_timings_to_config(
+                struct omap_video_timings *timings)
+{
+        if (memcmp(&omap_dss_pal_timings, timings, sizeof(*timings)) == 0)
+                return &venc_config_pal_trm;
+
+        if (memcmp(&omap_dss_ntsc_timings, timings, sizeof(*timings)) == 0)
+                return &venc_config_ntsc_trm;
+
+        BUG();
+        return NULL;
+}
+
+static int venc_power_on(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = venc.output.manager;
+        u32 l;
+        int r;
+
+        r = venc_runtime_get();
+        if (r)
+                goto err0;
+
+        venc_reset();
+        venc_write_config(venc_timings_to_config(&venc.timings));
+
+        dss_set_venc_output(venc.type);
+        dss_set_dac_pwrdn_bgz(1);
+
+        l = 0;
+
+        if (venc.type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+                l |= 1 << 1;
+        else /* S-Video */
+                l |= (1 << 0) | (1 << 2);
+
+        if (venc.invert_polarity == false)
+                l |= 1 << 3;
+
+        venc_write_reg(VENC_OUTPUT_CONTROL, l);
+
+        dss_mgr_set_timings(mgr, &venc.timings);
+
+        r = regulator_enable(venc.vdda_dac_reg);
+        if (r)
+                goto err1;
+
+        r = dss_mgr_enable(mgr);
+        if (r)
+                goto err2;
+
+        return 0;
+
+err2:
+        regulator_disable(venc.vdda_dac_reg);
+err1:
+        venc_write_reg(VENC_OUTPUT_CONTROL, 0);
+        dss_set_dac_pwrdn_bgz(0);
+
+        venc_runtime_put();
+err0:
+        return r;
+}
+
+static void venc_power_off(struct omap_dss_device *dssdev)
+{
+        struct omap_overlay_manager *mgr = venc.output.manager;
+
+        venc_write_reg(VENC_OUTPUT_CONTROL, 0);
+        dss_set_dac_pwrdn_bgz(0);
+
+        dss_mgr_disable(mgr);
+
+        regulator_disable(venc.vdda_dac_reg);
+
+        venc_runtime_put();
+}
+
+static int venc_display_enable(struct omap_dss_device *dssdev)
+{
+        struct omap_dss_device *out = &venc.output;
+        int r;
+
+        DSSDBG("venc_display_enable\n");
+
+        mutex_lock(&venc.venc_lock);
+
+        if (out->manager == NULL) {
+                DSSERR("Failed to enable display: no output/manager\n");
+                r = -ENODEV;
+                goto err0;
+        }
+
+        r = venc_power_on(dssdev);
+        if (r)
+                goto err0;
+
+        venc.wss_data = 0;
+
+        mutex_unlock(&venc.venc_lock);
+
+        return 0;
+err0:
+        mutex_unlock(&venc.venc_lock);
+        return r;
+}
+
+static void venc_display_disable(struct omap_dss_device *dssdev)
+{
+        DSSDBG("venc_display_disable\n");
+
+        mutex_lock(&venc.venc_lock);
+
+        venc_power_off(dssdev);
+
+        mutex_unlock(&venc.venc_lock);
+}
+
+static void venc_set_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        DSSDBG("venc_set_timings\n");
+
+        mutex_lock(&venc.venc_lock);
+
+        /* Reset WSS data when the TV standard changes. */
+        if (memcmp(&venc.timings, timings, sizeof(*timings)))
+                venc.wss_data = 0;
+
+        venc.timings = *timings;
+
+        dispc_set_tv_pclk(13500000);
+
+        mutex_unlock(&venc.venc_lock);
+}
+
+static int venc_check_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        DSSDBG("venc_check_timings\n");
+
+        if (memcmp(&omap_dss_pal_timings, timings, sizeof(*timings)) == 0)
+                return 0;
+
+        if (memcmp(&omap_dss_ntsc_timings, timings, sizeof(*timings)) == 0)
+                return 0;
+
+        return -EINVAL;
+}
+
+static void venc_get_timings(struct omap_dss_device *dssdev,
+                struct omap_video_timings *timings)
+{
+        mutex_lock(&venc.venc_lock);
+
+        *timings = venc.timings;
+
+        mutex_unlock(&venc.venc_lock);
+}
+
+static u32 venc_get_wss(struct omap_dss_device *dssdev)
+{
+        /* Invert due to VENC_L21_WC_CTL:INV=1 */
+        return (venc.wss_data >> 8) ^ 0xfffff;
+}
+
+static int venc_set_wss(struct omap_dss_device *dssdev, u32 wss)
+{
+        const struct venc_config *config;
+        int r;
+
+        DSSDBG("venc_set_wss\n");
+
+        mutex_lock(&venc.venc_lock);
+
+        config = venc_timings_to_config(&venc.timings);
+
+        /* Invert due to VENC_L21_WC_CTL:INV=1 */
+        venc.wss_data = (wss ^ 0xfffff) << 8;
+
+        r = venc_runtime_get();
+        if (r)
+                goto err;
+
+        venc_write_reg(VENC_BSTAMP_WSS_DATA, config->bstamp_wss_data |
+                        venc.wss_data);
+
+        venc_runtime_put();
+
+err:
+        mutex_unlock(&venc.venc_lock);
+
+        return r;
+}
+
+static void venc_set_type(struct omap_dss_device *dssdev,
+                enum omap_dss_venc_type type)
+{
+        mutex_lock(&venc.venc_lock);
+
+        venc.type = type;
+
+        mutex_unlock(&venc.venc_lock);
+}
+
+static void venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
+                bool invert_polarity)
+{
+        mutex_lock(&venc.venc_lock);
+
+        venc.invert_polarity = invert_polarity;
+
+        mutex_unlock(&venc.venc_lock);
+}
+
+static int venc_init_regulator(void)
+{
+        struct regulator *vdda_dac;
+
+        if (venc.vdda_dac_reg != NULL)
+                return 0;
+
+        if (venc.pdev->dev.of_node)
+                vdda_dac = devm_regulator_get(&venc.pdev->dev, "vdda");
+        else
+                vdda_dac = devm_regulator_get(&venc.pdev->dev, "vdda_dac");
+
+        if (IS_ERR(vdda_dac)) {
+                if (PTR_ERR(vdda_dac) != -EPROBE_DEFER)
+                        DSSERR("can't get VDDA_DAC regulator\n");
+                return PTR_ERR(vdda_dac);
+        }
+
+        venc.vdda_dac_reg = vdda_dac;
+
+        return 0;
+}
+
+static void venc_dump_regs(struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, venc_read_reg(r))
+
+        if (venc_runtime_get())
+                return;
+
+        DUMPREG(VENC_F_CONTROL);
+        DUMPREG(VENC_VIDOUT_CTRL);
+        DUMPREG(VENC_SYNC_CTRL);
+        DUMPREG(VENC_LLEN);
+        DUMPREG(VENC_FLENS);
+        DUMPREG(VENC_HFLTR_CTRL);
+        DUMPREG(VENC_CC_CARR_WSS_CARR);
+        DUMPREG(VENC_C_PHASE);
+        DUMPREG(VENC_GAIN_U);
+        DUMPREG(VENC_GAIN_V);
+        DUMPREG(VENC_GAIN_Y);
+        DUMPREG(VENC_BLACK_LEVEL);
+        DUMPREG(VENC_BLANK_LEVEL);
+        DUMPREG(VENC_X_COLOR);
+        DUMPREG(VENC_M_CONTROL);
+        DUMPREG(VENC_BSTAMP_WSS_DATA);
+        DUMPREG(VENC_S_CARR);
+        DUMPREG(VENC_LINE21);
+        DUMPREG(VENC_LN_SEL);
+        DUMPREG(VENC_L21__WC_CTL);
+        DUMPREG(VENC_HTRIGGER_VTRIGGER);
+        DUMPREG(VENC_SAVID__EAVID);
+        DUMPREG(VENC_FLEN__FAL);
+        DUMPREG(VENC_LAL__PHASE_RESET);
+        DUMPREG(VENC_HS_INT_START_STOP_X);
+        DUMPREG(VENC_HS_EXT_START_STOP_X);
+        DUMPREG(VENC_VS_INT_START_X);
+        DUMPREG(VENC_VS_INT_STOP_X__VS_INT_START_Y);
+        DUMPREG(VENC_VS_INT_STOP_Y__VS_EXT_START_X);
+        DUMPREG(VENC_VS_EXT_STOP_X__VS_EXT_START_Y);
+        DUMPREG(VENC_VS_EXT_STOP_Y);
+        DUMPREG(VENC_AVID_START_STOP_X);
+        DUMPREG(VENC_AVID_START_STOP_Y);
+        DUMPREG(VENC_FID_INT_START_X__FID_INT_START_Y);
+        DUMPREG(VENC_FID_INT_OFFSET_Y__FID_EXT_START_X);
+        DUMPREG(VENC_FID_EXT_START_Y__FID_EXT_OFFSET_Y);
+        DUMPREG(VENC_TVDETGP_INT_START_STOP_X);
+        DUMPREG(VENC_TVDETGP_INT_START_STOP_Y);
+        DUMPREG(VENC_GEN_CTRL);
+        DUMPREG(VENC_OUTPUT_CONTROL);
+        DUMPREG(VENC_OUTPUT_TEST);
+
+        venc_runtime_put();
+
+#undef DUMPREG
+}
+
+static int venc_get_clocks(struct platform_device *pdev)
+{
+        struct clk *clk;
+
+        if (dss_has_feature(FEAT_VENC_REQUIRES_TV_DAC_CLK)) {
+                clk = devm_clk_get(&pdev->dev, "tv_dac_clk");
+                if (IS_ERR(clk)) {
+                        DSSERR("can't get tv_dac_clk\n");
+                        return PTR_ERR(clk);
+                }
+        } else {
+                clk = NULL;
+        }
+
+        venc.tv_dac_clk = clk;
+
+        return 0;
+}
+
+static int venc_connect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        struct omap_overlay_manager *mgr;
+        int r;
+
+        r = venc_init_regulator();
+        if (r)
+                return r;
+
+        mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+        if (!mgr)
+                return -ENODEV;
+
+        r = dss_mgr_connect(mgr, dssdev);
+        if (r)
+                return r;
+
+        r = omapdss_output_set_device(dssdev, dst);
+        if (r) {
+                DSSERR("failed to connect output to new device: %s\n",
+                                dst->name);
+                dss_mgr_disconnect(mgr, dssdev);
+                return r;
+        }
+
+        return 0;
+}
+
+static void venc_disconnect(struct omap_dss_device *dssdev,
+                struct omap_dss_device *dst)
+{
+        WARN_ON(dst != dssdev->dst);
+
+        if (dst != dssdev->dst)
+                return;
+
+        omapdss_output_unset_device(dssdev);
+
+        if (dssdev->manager)
+                dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_atv_ops venc_ops = {
+        .connect = venc_connect,
+        .disconnect = venc_disconnect,
+
+        .enable = venc_display_enable,
+        .disable = venc_display_disable,
+
+        .check_timings = venc_check_timings,
+        .set_timings = venc_set_timings,
+        .get_timings = venc_get_timings,
+
+        .set_type = venc_set_type,
+        .invert_vid_out_polarity = venc_invert_vid_out_polarity,
+
+        .set_wss = venc_set_wss,
+        .get_wss = venc_get_wss,
+};
+
+static void venc_init_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &venc.output;
+
+        out->dev = &pdev->dev;
+        out->id = OMAP_DSS_OUTPUT_VENC;
+        out->output_type = OMAP_DISPLAY_TYPE_VENC;
+        out->name = "venc.0";
+        out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+        out->ops.atv = &venc_ops;
+        out->owner = THIS_MODULE;
+
+        omapdss_register_output(out);
+}
+
+static void venc_uninit_output(struct platform_device *pdev)
+{
+        struct omap_dss_device *out = &venc.output;
+
+        omapdss_unregister_output(out);
+}
+
+static int venc_probe_of(struct platform_device *pdev)
+{
+        struct device_node *node = pdev->dev.of_node;
+        struct device_node *ep;
+        u32 channels;
+        int r;
+
+        ep = omapdss_of_get_first_endpoint(node);
+        if (!ep)
+                return 0;
+
+        venc.invert_polarity = of_property_read_bool(ep, "ti,invert-polarity");
+
+        r = of_property_read_u32(ep, "ti,channels", &channels);
+        if (r) {
+                dev_err(&pdev->dev,
+                        "failed to read property 'ti,channels': %d\n", r);
+                goto err;
+        }
+
+        switch (channels) {
+        case 1:
+                venc.type = OMAP_DSS_VENC_TYPE_COMPOSITE;
+                break;
+        case 2:
+                venc.type = OMAP_DSS_VENC_TYPE_SVIDEO;
+                break;
+        default:
+                dev_err(&pdev->dev, "bad channel propert '%d'\n", channels);
+                r = -EINVAL;
+                goto err;
+        }
+
+        of_node_put(ep);
+
+        return 0;
+err:
+        of_node_put(ep);
+
+        return 0;
+}
+
+/* VENC HW IP initialisation */
+static int venc_bind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+        u8 rev_id;
+        struct resource *venc_mem;
+        int r;
+
+        venc.pdev = pdev;
+
+        mutex_init(&venc.venc_lock);
+
+        venc.wss_data = 0;
+
+        venc_mem = platform_get_resource(venc.pdev, IORESOURCE_MEM, 0);
+        if (!venc_mem) {
+                DSSERR("can't get IORESOURCE_MEM VENC\n");
+                return -EINVAL;
+        }
+
+        venc.base = devm_ioremap(&pdev->dev, venc_mem->start,
+                                 resource_size(venc_mem));
+        if (!venc.base) {
+                DSSERR("can't ioremap VENC\n");
+                return -ENOMEM;
+        }
+
+        r = venc_get_clocks(pdev);
+        if (r)
+                return r;
+
+        pm_runtime_enable(&pdev->dev);
+
+        r = venc_runtime_get();
+        if (r)
+                goto err_runtime_get;
+
+        rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
+        dev_dbg(&pdev->dev, "OMAP VENC rev %d\n", rev_id);
+
+        venc_runtime_put();
+
+        if (pdev->dev.of_node) {
+                r = venc_probe_of(pdev);
+                if (r) {
+                        DSSERR("Invalid DT data\n");
+                        goto err_probe_of;
+                }
+        }
+
+        dss_debugfs_create_file("venc", venc_dump_regs);
+
+        venc_init_output(pdev);
+
+        return 0;
+
+err_probe_of:
+err_runtime_get:
+        pm_runtime_disable(&pdev->dev);
+        return r;
+}
+
+static void venc_unbind(struct device *dev, struct device *master, void *data)
+{
+        struct platform_device *pdev = to_platform_device(dev);
+
+        venc_uninit_output(pdev);
+
+        pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops venc_component_ops = {
+        .bind   = venc_bind,
+        .unbind = venc_unbind,
+};
+
+static int venc_probe(struct platform_device *pdev)
+{
+        return component_add(&pdev->dev, &venc_component_ops);
+}
+
+static int venc_remove(struct platform_device *pdev)
+{
+        component_del(&pdev->dev, &venc_component_ops);
+        return 0;
+}
+
+static int venc_runtime_suspend(struct device *dev)
+{
+        if (venc.tv_dac_clk)
+                clk_disable_unprepare(venc.tv_dac_clk);
+
+        dispc_runtime_put();
+
+        return 0;
+}
+
+static int venc_runtime_resume(struct device *dev)
+{
+        int r;
+
+        r = dispc_runtime_get();
+        if (r < 0)
+                return r;
+
+        if (venc.tv_dac_clk)
+                clk_prepare_enable(venc.tv_dac_clk);
+
+        return 0;
+}
+
+static const struct dev_pm_ops venc_pm_ops = {
+        .runtime_suspend = venc_runtime_suspend,
+        .runtime_resume = venc_runtime_resume,
+};
+
+static const struct of_device_id venc_of_match[] = {
+        { .compatible = "ti,omap2-venc", },
+        { .compatible = "ti,omap3-venc", },
+        { .compatible = "ti,omap4-venc", },
+        {},
+};
+
+static struct platform_driver omap_venchw_driver = {
+        .probe          = venc_probe,
+        .remove         = venc_remove,
+        .driver         = {
+                .name   = "omapdss_venc",
+                .pm     = &venc_pm_ops,
+                .of_match_table = venc_of_match,
+                .suppress_bind_attrs = true,
+        },
+};
+
+int __init venc_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_venchw_driver);
+}
+
+void venc_uninit_platform_driver(void)
+{
+        platform_driver_unregister(&omap_venchw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/video-pll.c b/drivers/video/fbdev/omap2/omapfb/dss/video-pll.c
new file mode 100644
index 000000000000..b1ec59e42940
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/video-pll.c
@@ -0,0 +1,211 @@
+/*
+* Copyright (C) 2014 Texas Instruments Ltd
+*
+* 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.
+*
+* 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/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+struct dss_video_pll {
+        struct dss_pll pll;
+
+        struct device *dev;
+
+        void __iomem *clkctrl_base;
+};
+
+#define REG_MOD(reg, val, start, end) \
+        writel_relaxed(FLD_MOD(readl_relaxed(reg), val, start, end), reg)
+
+static void dss_dpll_enable_scp_clk(struct dss_video_pll *vpll)
+{
+        REG_MOD(vpll->clkctrl_base, 1, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dss_dpll_disable_scp_clk(struct dss_video_pll *vpll)
+{
+        REG_MOD(vpll->clkctrl_base, 0, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dss_dpll_power_enable(struct dss_video_pll *vpll)
+{
+        REG_MOD(vpll->clkctrl_base, 2, 31, 30); /* PLL_POWER_ON_ALL */
+
+        /*
+         * DRA7x PLL CTRL's PLL_PWR_STATUS seems to always return 0,
+         * so we have to use fixed delay here.
+         */
+        msleep(1);
+}
+
+static void dss_dpll_power_disable(struct dss_video_pll *vpll)
+{
+        REG_MOD(vpll->clkctrl_base, 0, 31, 30); /* PLL_POWER_OFF */
+}
+
+static int dss_video_pll_enable(struct dss_pll *pll)
+{
+        struct dss_video_pll *vpll = container_of(pll, struct dss_video_pll, pll);
+        int r;
+
+        r = dss_runtime_get();
+        if (r)
+                return r;
+
+        dss_ctrl_pll_enable(pll->id, true);
+
+        dss_dpll_enable_scp_clk(vpll);
+
+        r = dss_pll_wait_reset_done(pll);
+        if (r)
+                goto err_reset;
+
+        dss_dpll_power_enable(vpll);
+
+        return 0;
+
+err_reset:
+        dss_dpll_disable_scp_clk(vpll);
+        dss_ctrl_pll_enable(pll->id, false);
+        dss_runtime_put();
+
+        return r;
+}
+
+static void dss_video_pll_disable(struct dss_pll *pll)
+{
+        struct dss_video_pll *vpll = container_of(pll, struct dss_video_pll, pll);
+
+        dss_dpll_power_disable(vpll);
+
+        dss_dpll_disable_scp_clk(vpll);
+
+        dss_ctrl_pll_enable(pll->id, false);
+
+        dss_runtime_put();
+}
+
+static const struct dss_pll_ops dss_pll_ops = {
+        .enable = dss_video_pll_enable,
+        .disable = dss_video_pll_disable,
+        .set_config = dss_pll_write_config_type_a,
+};
+
+static const struct dss_pll_hw dss_dra7_video_pll_hw = {
+        .n_max = (1 << 8) - 1,
+        .m_max = (1 << 12) - 1,
+        .mX_max = (1 << 5) - 1,
+        .fint_min = 500000,
+        .fint_max = 2500000,
+        .clkdco_max = 1800000000,
+
+        .n_msb = 8,
+        .n_lsb = 1,
+        .m_msb = 20,
+        .m_lsb = 9,
+
+        .mX_msb[0] = 25,
+        .mX_lsb[0] = 21,
+        .mX_msb[1] = 30,
+        .mX_lsb[1] = 26,
+
+        .has_refsel = true,
+};
+
+struct dss_pll *dss_video_pll_init(struct platform_device *pdev, int id,
+        struct regulator *regulator)
+{
+        const char * const reg_name[] = { "pll1", "pll2" };
+        const char * const clkctrl_name[] = { "pll1_clkctrl", "pll2_clkctrl" };
+        const char * const clkin_name[] = { "video1_clk", "video2_clk" };
+
+        struct resource *res;
+        struct dss_video_pll *vpll;
+        void __iomem *pll_base, *clkctrl_base;
+        struct clk *clk;
+        struct dss_pll *pll;
+        int r;
+
+        /* PLL CONTROL */
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM, reg_name[id]);
+        if (!res) {
+                dev_err(&pdev->dev,
+                        "missing platform resource data for pll%d\n", id);
+                return ERR_PTR(-ENODEV);
+        }
+
+        pll_base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(pll_base)) {
+                dev_err(&pdev->dev, "failed to ioremap pll%d reg_name\n", id);
+                return ERR_CAST(pll_base);
+        }
+
+        /* CLOCK CONTROL */
+
+        res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+                clkctrl_name[id]);
+        if (!res) {
+                dev_err(&pdev->dev,
+                        "missing platform resource data for pll%d\n", id);
+                return ERR_PTR(-ENODEV);
+        }
+
+        clkctrl_base = devm_ioremap_resource(&pdev->dev, res);
+        if (IS_ERR(clkctrl_base)) {
+                dev_err(&pdev->dev, "failed to ioremap pll%d clkctrl\n", id);
+                return ERR_CAST(clkctrl_base);
+        }
+
+        /* CLKIN */
+
+        clk = devm_clk_get(&pdev->dev, clkin_name[id]);
+        if (IS_ERR(clk)) {
+                DSSERR("can't get video pll clkin\n");
+                return ERR_CAST(clk);
+        }
+
+        vpll = devm_kzalloc(&pdev->dev, sizeof(*vpll), GFP_KERNEL);
+        if (!vpll)
+                return ERR_PTR(-ENOMEM);
+
+        vpll->dev = &pdev->dev;
+        vpll->clkctrl_base = clkctrl_base;
+
+        pll = &vpll->pll;
+
+        pll->name = id == 0 ? "video0" : "video1";
+        pll->id = id == 0 ? DSS_PLL_VIDEO1 : DSS_PLL_VIDEO2;
+        pll->clkin = clk;
+        pll->regulator = regulator;
+        pll->base = pll_base;
+        pll->hw = &dss_dra7_video_pll_hw;
+        pll->ops = &dss_pll_ops;
+
+        r = dss_pll_register(pll);
+        if (r)
+                return ERR_PTR(r);
+
+        return pll;
+}
+
+void dss_video_pll_uninit(struct dss_pll *pll)
+{
+        dss_pll_unregister(pll);
+}
diff --git a/drivers/video/fbdev/omap2/vrfb.c b/drivers/video/fbdev/omap2/omapfb/vrfb.c
index f346b02eee1d..f346b02eee1d 100644
--- a/drivers/video/fbdev/omap2/vrfb.c
+++ b/drivers/video/fbdev/omap2/omapfb/vrfb.c
diff --git a/drivers/video/fbdev/pxafb.c b/drivers/video/fbdev/pxafb.c
index 94813af97f09..33b2bb315a2a 100644
--- a/drivers/video/fbdev/pxafb.c
+++ b/drivers/video/fbdev/pxafb.c
@@ -55,6 +55,9 @@
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 #include <linux/console.h>
+#include <linux/of_graph.h>
+#include <video/of_display_timing.h>
+#include <video/videomode.h>
 
 #include <mach/hardware.h>
 #include <asm/io.h>
@@ -457,7 +460,7 @@ static int pxafb_adjust_timing(struct pxafb_info *fbi,
 static int pxafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
         struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
-        struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
+        struct pxafb_mach_info *inf = fbi->inf;
         int err;
 
         if (inf->fixed_modes) {
@@ -1230,7 +1233,7 @@ static unsigned int __smart_timing(unsigned time_ns, unsigned long lcd_clk)
 static void setup_smart_timing(struct pxafb_info *fbi,
                                 struct fb_var_screeninfo *var)
 {
-        struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
+        struct pxafb_mach_info *inf = fbi->inf;
         struct pxafb_mode_info *mode = &inf->modes[0];
         unsigned long lclk = clk_get_rate(fbi->clk);
         unsigned t1, t2, t3, t4;
@@ -1258,14 +1261,13 @@ static void setup_smart_timing(struct pxafb_info *fbi,
 static int pxafb_smart_thread(void *arg)
 {
         struct pxafb_info *fbi = arg;
-        struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
+        struct pxafb_mach_info *inf = fbi->inf;
 
         if (!inf->smart_update) {
                 pr_err("%s: not properly initialized, thread terminated\n",
                                 __func__);
                 return -EINVAL;
         }
-        inf = dev_get_platdata(fbi->dev);
 
         pr_debug("%s(): task starting\n", __func__);
 
@@ -1788,11 +1790,11 @@ decode_mode:
                 fbi->video_mem_size = video_mem_size;
 }
 
-static struct pxafb_info *pxafb_init_fbinfo(struct device *dev)
+static struct pxafb_info *pxafb_init_fbinfo(struct device *dev,
+                                            struct pxafb_mach_info *inf)
 {
         struct pxafb_info *fbi;
         void *addr;
-        struct pxafb_mach_info *inf = dev_get_platdata(dev);
 
         /* Alloc the pxafb_info and pseudo_palette in one step */
         fbi = kmalloc(sizeof(struct pxafb_info) + sizeof(u32) * 16, GFP_KERNEL);
@@ -1801,6 +1803,7 @@ static struct pxafb_info *pxafb_init_fbinfo(struct device *dev)
 
         memset(fbi, 0, sizeof(struct pxafb_info));
         fbi->dev = dev;
+        fbi->inf = inf;
 
         fbi->clk = clk_get(dev, NULL);
         if (IS_ERR(fbi->clk)) {
@@ -1852,10 +1855,9 @@ static struct pxafb_info *pxafb_init_fbinfo(struct device *dev)
 }
 
 #ifdef CONFIG_FB_PXA_PARAMETERS
-static int parse_opt_mode(struct device *dev, const char *this_opt)
+static int parse_opt_mode(struct device *dev, const char *this_opt,
+                          struct pxafb_mach_info *inf)
 {
-        struct pxafb_mach_info *inf = dev_get_platdata(dev);
-
         const char *name = this_opt+5;
         unsigned int namelen = strlen(name);
         int res_specified = 0, bpp_specified = 0;
@@ -1911,9 +1913,9 @@ done:
         return 0;
 }
 
-static int parse_opt(struct device *dev, char *this_opt)
+static int parse_opt(struct device *dev, char *this_opt,
+                     struct pxafb_mach_info *inf)
 {
-        struct pxafb_mach_info *inf = dev_get_platdata(dev);
         struct pxafb_mode_info *mode = &inf->modes[0];
         char s[64];
 
@@ -1922,7 +1924,7 @@ static int parse_opt(struct device *dev, char *this_opt)
         if (!strncmp(this_opt, "vmem:", 5)) {
                 video_mem_size = memparse(this_opt + 5, NULL);
         } else if (!strncmp(this_opt, "mode:", 5)) {
-                return parse_opt_mode(dev, this_opt);
+                return parse_opt_mode(dev, this_opt, inf);
         } else if (!strncmp(this_opt, "pixclock:", 9)) {
                 mode->pixclock = simple_strtoul(this_opt+9, NULL, 0);
                 sprintf(s, "pixclock: %ld\n", mode->pixclock);
@@ -2011,7 +2013,8 @@ static int parse_opt(struct device *dev, char *this_opt)
         return 0;
 }
 
-static int pxafb_parse_options(struct device *dev, char *options)
+static int pxafb_parse_options(struct device *dev, char *options,
+                               struct pxafb_mach_info *inf)
 {
         char *this_opt;
         int ret;
@@ -2023,7 +2026,7 @@ static int pxafb_parse_options(struct device *dev, char *options)
 
         /* could be made table driven or similar?... */
         while ((this_opt = strsep(&options, ",")) != NULL) {
-                ret = parse_opt(dev, this_opt);
+                ret = parse_opt(dev, this_opt, inf);
                 if (ret)
                         return ret;
         }
@@ -2092,22 +2095,180 @@ static void pxafb_check_options(struct device *dev, struct pxafb_mach_info *inf)
 #define pxafb_check_options(...)        do {} while (0)
 #endif
 
+#if defined(CONFIG_OF)
+static const char * const lcd_types[] = {
+        "unknown", "mono-stn", "mono-dstn", "color-stn", "color-dstn",
+        "color-tft", "smart-panel", NULL
+};
+
+static int of_get_pxafb_display(struct device *dev, struct device_node *disp,
+                                struct pxafb_mach_info *info, u32 bus_width)
+{
+        struct display_timings *timings;
+        struct videomode vm;
+        int i, ret = -EINVAL;
+        const char *s;
+
+        ret = of_property_read_string(disp, "lcd-type", &s);
+        if (ret)
+                s = "color-tft";
+
+        for (i = 0; lcd_types[i]; i++)
+                if (!strcmp(s, lcd_types[i]))
+                        break;
+        if (!i || !lcd_types[i]) {
+                dev_err(dev, "lcd-type %s is unknown\n", s);
+                return -EINVAL;
+        }
+        info->lcd_conn |= LCD_CONN_TYPE(i);
+        info->lcd_conn |= LCD_CONN_WIDTH(bus_width);
+
+        timings = of_get_display_timings(disp);
+        if (!timings)
+                goto out;
+
+        ret = -ENOMEM;
+        info->modes = kmalloc_array(timings->num_timings,
+                                    sizeof(info->modes[0]), GFP_KERNEL);
+        if (!info->modes)
+                goto out;
+        info->num_modes = timings->num_timings;
+
+        for (i = 0; i < timings->num_timings; i++) {
+                ret = videomode_from_timings(timings, &vm, i);
+                if (ret) {
+                        dev_err(dev, "videomode_from_timings %d failed: %d\n",
+                                i, ret);
+                        goto out;
+                }
+                if (vm.flags & DISPLAY_FLAGS_PIXDATA_POSEDGE)
+                        info->lcd_conn |= LCD_PCLK_EDGE_RISE;
+                if (vm.flags & DISPLAY_FLAGS_PIXDATA_NEGEDGE)
+                        info->lcd_conn |= LCD_PCLK_EDGE_FALL;
+                if (vm.flags & DISPLAY_FLAGS_DE_HIGH)
+                        info->lcd_conn |= LCD_BIAS_ACTIVE_HIGH;
+                if (vm.flags & DISPLAY_FLAGS_DE_LOW)
+                        info->lcd_conn |= LCD_BIAS_ACTIVE_LOW;
+                if (vm.flags & DISPLAY_FLAGS_HSYNC_HIGH)
+                        info->modes[i].sync |= FB_SYNC_HOR_HIGH_ACT;
+                if (vm.flags & DISPLAY_FLAGS_VSYNC_HIGH)
+                        info->modes[i].sync |= FB_SYNC_VERT_HIGH_ACT;
+
+                info->modes[i].pixclock = 1000000000UL / (vm.pixelclock / 1000);
+                info->modes[i].xres = vm.hactive;
+                info->modes[i].yres = vm.vactive;
+                info->modes[i].hsync_len = vm.hsync_len;
+                info->modes[i].left_margin = vm.hback_porch;
+                info->modes[i].right_margin = vm.hfront_porch;
+                info->modes[i].vsync_len = vm.vsync_len;
+                info->modes[i].upper_margin = vm.vback_porch;
+                info->modes[i].lower_margin = vm.vfront_porch;
+        }
+        ret = 0;
+
+out:
+        display_timings_release(timings);
+        return ret;
+}
+
+static int of_get_pxafb_mode_info(struct device *dev,
+                                  struct pxafb_mach_info *info)
+{
+        struct device_node *display, *np;
+        u32 bus_width;
+        int ret, i;
+
+        np = of_graph_get_next_endpoint(dev->of_node, NULL);
+        if (!np) {
+                dev_err(dev, "could not find endpoint\n");
+                return -EINVAL;
+        }
+        ret = of_property_read_u32(np, "bus-width", &bus_width);
+        if (ret) {
+                dev_err(dev, "no bus-width specified: %d\n", ret);
+                return ret;
+        }
+
+        display = of_graph_get_remote_port_parent(np);
+        of_node_put(np);
+        if (!display) {
+                dev_err(dev, "no display defined\n");
+                return -EINVAL;
+        }
+
+        ret = of_get_pxafb_display(dev, display, info, bus_width);
+        of_node_put(display);
+        if (ret)
+                return ret;
+
+        for (i = 0; i < info->num_modes; i++)
+                info->modes[i].bpp = bus_width;
+
+        return 0;
+}
+
+static struct pxafb_mach_info *of_pxafb_of_mach_info(struct device *dev)
+{
+        int ret;
+        struct pxafb_mach_info *info;
+
+        if (!dev->of_node)
+                return NULL;
+        info = devm_kzalloc(dev, sizeof(*info), GFP_KERNEL);
+        if (!info)
+                return ERR_PTR(-ENOMEM);
+        ret = of_get_pxafb_mode_info(dev, info);
+        if (ret) {
+                kfree(info->modes);
+                return ERR_PTR(ret);
+        }
+
+        /*
+         * On purpose, neither lccrX registers nor video memory size can be
+         * specified through device-tree, they are considered more a debug hack
+         * available through command line.
+         */
+        return info;
+}
+#else
+static struct pxafb_mach_info *of_pxafb_of_mach_info(struct device *dev)
+{
+        return NULL;
+}
+#endif
+
 static int pxafb_probe(struct platform_device *dev)
 {
         struct pxafb_info *fbi;
-        struct pxafb_mach_info *inf;
+        struct pxafb_mach_info *inf, *pdata;
         struct resource *r;
-        int irq, ret;
+        int i, irq, ret;
 
         dev_dbg(&dev->dev, "pxafb_probe\n");
 
-        inf = dev_get_platdata(&dev->dev);
         ret = -ENOMEM;
-        fbi = NULL;
+        pdata = dev_get_platdata(&dev->dev);
+        inf = devm_kmalloc(&dev->dev, sizeof(*inf), GFP_KERNEL);
         if (!inf)
                 goto failed;
 
-        ret = pxafb_parse_options(&dev->dev, g_options);
+        if (pdata) {
+                *inf = *pdata;
+                inf->modes =
+                        devm_kmalloc_array(&dev->dev, pdata->num_modes,
+                                           sizeof(inf->modes[0]), GFP_KERNEL);
+                if (!inf->modes)
+                        goto failed;
+                for (i = 0; i < inf->num_modes; i++)
+                        inf->modes[i] = pdata->modes[i];
+        }
+
+        if (!pdata)
+                inf = of_pxafb_of_mach_info(&dev->dev);
+        if (IS_ERR_OR_NULL(inf))
+                goto failed;
+
+        ret = pxafb_parse_options(&dev->dev, g_options, inf);
         if (ret < 0)
                 goto failed;
 
@@ -2125,7 +2286,7 @@ static int pxafb_probe(struct platform_device *dev)
                 goto failed;
         }
 
-        fbi = pxafb_init_fbinfo(&dev->dev);
+        fbi = pxafb_init_fbinfo(&dev->dev, inf);
         if (!fbi) {
                 /* only reason for pxafb_init_fbinfo to fail is kmalloc */
                 dev_err(&dev->dev, "Failed to initialize framebuffer device\n");
@@ -2299,11 +2460,20 @@ static int pxafb_remove(struct platform_device *dev)
         return 0;
 }
 
+static const struct of_device_id pxafb_of_dev_id[] = {
+        { .compatible = "marvell,pxa270-lcdc", },
+        { .compatible = "marvell,pxa300-lcdc", },
+        { .compatible = "marvell,pxa2xx-lcdc", },
+        { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, pxafb_of_dev_id);
+
 static struct platform_driver pxafb_driver = {
         .probe          = pxafb_probe,
         .remove         = pxafb_remove,
         .driver         = {
                 .name   = "pxa2xx-fb",
+                .of_match_table = pxafb_of_dev_id,
 #ifdef CONFIG_PM
                 .pm     = &pxafb_pm_ops,
 #endif
diff --git a/drivers/video/fbdev/pxafb.h b/drivers/video/fbdev/pxafb.h
index 26ba9fa3f737..5dc414e26fc8 100644
--- a/drivers/video/fbdev/pxafb.h
+++ b/drivers/video/fbdev/pxafb.h
@@ -167,6 +167,8 @@ struct pxafb_info {
 
         void (*lcd_power)(int, struct fb_var_screeninfo *);
         void (*backlight_power)(int);
+
+        struct pxafb_mach_info  *inf;
 };
 
 #define TO_INF(ptr,member) container_of(ptr,struct pxafb_info,member)
diff --git a/drivers/video/fbdev/riva/fbdev.c b/drivers/video/fbdev/riva/fbdev.c
index f1ad2747064b..2ef26ad99341 100644
--- a/drivers/video/fbdev/riva/fbdev.c
+++ b/drivers/video/fbdev/riva/fbdev.c
@@ -1765,6 +1765,7 @@ static int riva_get_EDID_i2c(struct fb_info *info)
         int i;
 
         NVTRACE_ENTER();
+        par->riva.LockUnlock(&par->riva, 0);
         riva_create_i2c_busses(par);
         for (i = 0; i < 3; i++) {
                 if (!par->chan[i].par)
diff --git a/drivers/video/fbdev/sh_mobile_hdmi.c b/drivers/video/fbdev/sh_mobile_hdmi.c
deleted file mode 100644
index 7c72a3f02056..000000000000
--- a/drivers/video/fbdev/sh_mobile_hdmi.c
+++ /dev/null
@@ -1,1489 +0,0 @@
-/*
- * SH-Mobile High-Definition Multimedia Interface (HDMI) driver
- * for SLISHDMI13T and SLIPHDMIT IP cores
- *
- * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/clk.h>
-#include <linux/console.h>
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/workqueue.h>
-#include <sound/soc.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"
-
-/* HDMI Core Control Register (HTOP0) */
-#define HDMI_SYSTEM_CTRL                        0x00 /* System control */
-#define HDMI_L_R_DATA_SWAP_CTRL_RPKT            0x01 /* L/R data swap control,
-                                                        bits 19..16 of 20-bit N for Audio Clock Regeneration packet */
-#define HDMI_20_BIT_N_FOR_AUDIO_RPKT_15_8       0x02 /* bits 15..8 of 20-bit N for Audio Clock Regeneration packet */
-#define HDMI_20_BIT_N_FOR_AUDIO_RPKT_7_0        0x03 /* bits 7..0 of 20-bit N for Audio Clock Regeneration packet */
-#define HDMI_SPDIF_AUDIO_SAMP_FREQ_CTS          0x04 /* SPDIF audio sampling frequency,
-                                                        bits 19..16 of Internal CTS */
-#define HDMI_INTERNAL_CTS_15_8                  0x05 /* bits 15..8 of Internal CTS */
-#define HDMI_INTERNAL_CTS_7_0                   0x06 /* bits 7..0 of Internal CTS */
-#define HDMI_EXTERNAL_CTS_19_16                 0x07 /* External CTS */
-#define HDMI_EXTERNAL_CTS_15_8                  0x08 /* External CTS */
-#define HDMI_EXTERNAL_CTS_7_0                   0x09 /* External CTS */
-#define HDMI_AUDIO_SETTING_1                    0x0A /* Audio setting.1 */
-#define HDMI_AUDIO_SETTING_2                    0x0B /* Audio setting.2 */
-#define HDMI_I2S_AUDIO_SET                      0x0C /* I2S audio setting */
-#define HDMI_DSD_AUDIO_SET                      0x0D /* DSD audio setting */
-#define HDMI_DEBUG_MONITOR_1                    0x0E /* Debug monitor.1 */
-#define HDMI_DEBUG_MONITOR_2                    0x0F /* Debug monitor.2 */
-#define HDMI_I2S_INPUT_PIN_SWAP                 0x10 /* I2S input pin swap */
-#define HDMI_AUDIO_STATUS_BITS_SETTING_1        0x11 /* Audio status bits setting.1 */
-#define HDMI_AUDIO_STATUS_BITS_SETTING_2        0x12 /* Audio status bits setting.2 */
-#define HDMI_CATEGORY_CODE                      0x13 /* Category code */
-#define HDMI_SOURCE_NUM_AUDIO_WORD_LEN          0x14 /* Source number/Audio word length */
-#define HDMI_AUDIO_VIDEO_SETTING_1              0x15 /* Audio/Video setting.1 */
-#define HDMI_VIDEO_SETTING_1                    0x16 /* Video setting.1 */
-#define HDMI_DEEP_COLOR_MODES                   0x17 /* Deep Color Modes */
-
-/* 12 16- and 10-bit Color space conversion parameters: 0x18..0x2f */
-#define HDMI_COLOR_SPACE_CONVERSION_PARAMETERS  0x18
-
-#define HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS      0x30 /* External video parameter settings */
-#define HDMI_EXTERNAL_H_TOTAL_7_0               0x31 /* External horizontal total (LSB) */
-#define HDMI_EXTERNAL_H_TOTAL_11_8              0x32 /* External horizontal total (MSB) */
-#define HDMI_EXTERNAL_H_BLANK_7_0               0x33 /* External horizontal blank (LSB) */
-#define HDMI_EXTERNAL_H_BLANK_9_8               0x34 /* External horizontal blank (MSB) */
-#define HDMI_EXTERNAL_H_DELAY_7_0               0x35 /* External horizontal delay (LSB) */
-#define HDMI_EXTERNAL_H_DELAY_9_8               0x36 /* External horizontal delay (MSB) */
-#define HDMI_EXTERNAL_H_DURATION_7_0            0x37 /* External horizontal duration (LSB) */
-#define HDMI_EXTERNAL_H_DURATION_9_8            0x38 /* External horizontal duration (MSB) */
-#define HDMI_EXTERNAL_V_TOTAL_7_0               0x39 /* External vertical total (LSB) */
-#define HDMI_EXTERNAL_V_TOTAL_9_8               0x3A /* External vertical total (MSB) */
-#define HDMI_AUDIO_VIDEO_SETTING_2              0x3B /* Audio/Video setting.2 */
-#define HDMI_EXTERNAL_V_BLANK                   0x3D /* External vertical blank */
-#define HDMI_EXTERNAL_V_DELAY                   0x3E /* External vertical delay */
-#define HDMI_EXTERNAL_V_DURATION                0x3F /* External vertical duration */
-#define HDMI_CTRL_PKT_MANUAL_SEND_CONTROL       0x40 /* Control packet manual send control */
-#define HDMI_CTRL_PKT_AUTO_SEND                 0x41 /* Control packet auto send with VSYNC control */
-#define HDMI_AUTO_CHECKSUM_OPTION               0x42 /* Auto checksum option */
-#define HDMI_VIDEO_SETTING_2                    0x45 /* Video setting.2 */
-#define HDMI_OUTPUT_OPTION                      0x46 /* Output option */
-#define HDMI_SLIPHDMIT_PARAM_OPTION             0x51 /* SLIPHDMIT parameter option */
-#define HDMI_HSYNC_PMENT_AT_EMB_7_0             0x52 /* HSYNC placement at embedded sync (LSB) */
-#define HDMI_HSYNC_PMENT_AT_EMB_15_8            0x53 /* HSYNC placement at embedded sync (MSB) */
-#define HDMI_VSYNC_PMENT_AT_EMB_7_0             0x54 /* VSYNC placement at embedded sync (LSB) */
-#define HDMI_VSYNC_PMENT_AT_EMB_14_8            0x55 /* VSYNC placement at embedded sync (MSB) */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_1         0x56 /* SLIPHDMIT parameter settings.1 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_2         0x57 /* SLIPHDMIT parameter settings.2 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_3         0x58 /* SLIPHDMIT parameter settings.3 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_5         0x59 /* SLIPHDMIT parameter settings.5 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_6         0x5A /* SLIPHDMIT parameter settings.6 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_7         0x5B /* SLIPHDMIT parameter settings.7 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_8         0x5C /* SLIPHDMIT parameter settings.8 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_9         0x5D /* SLIPHDMIT parameter settings.9 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_10        0x5E /* SLIPHDMIT parameter settings.10 */
-#define HDMI_CTRL_PKT_BUF_INDEX                 0x5F /* Control packet buffer index */
-#define HDMI_CTRL_PKT_BUF_ACCESS_HB0            0x60 /* Control packet data buffer access window - HB0 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_HB1            0x61 /* Control packet data buffer access window - HB1 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_HB2            0x62 /* Control packet data buffer access window - HB2 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB0            0x63 /* Control packet data buffer access window - PB0 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB1            0x64 /* Control packet data buffer access window - PB1 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB2            0x65 /* Control packet data buffer access window - PB2 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB3            0x66 /* Control packet data buffer access window - PB3 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB4            0x67 /* Control packet data buffer access window - PB4 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB5            0x68 /* Control packet data buffer access window - PB5 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB6            0x69 /* Control packet data buffer access window - PB6 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB7            0x6A /* Control packet data buffer access window - PB7 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB8            0x6B /* Control packet data buffer access window - PB8 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB9            0x6C /* Control packet data buffer access window - PB9 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB10           0x6D /* Control packet data buffer access window - PB10 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB11           0x6E /* Control packet data buffer access window - PB11 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB12           0x6F /* Control packet data buffer access window - PB12 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB13           0x70 /* Control packet data buffer access window - PB13 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB14           0x71 /* Control packet data buffer access window - PB14 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB15           0x72 /* Control packet data buffer access window - PB15 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB16           0x73 /* Control packet data buffer access window - PB16 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB17           0x74 /* Control packet data buffer access window - PB17 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB18           0x75 /* Control packet data buffer access window - PB18 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB19           0x76 /* Control packet data buffer access window - PB19 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB20           0x77 /* Control packet data buffer access window - PB20 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB21           0x78 /* Control packet data buffer access window - PB21 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB22           0x79 /* Control packet data buffer access window - PB22 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB23           0x7A /* Control packet data buffer access window - PB23 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB24           0x7B /* Control packet data buffer access window - PB24 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB25           0x7C /* Control packet data buffer access window - PB25 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB26           0x7D /* Control packet data buffer access window - PB26 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB27           0x7E /* Control packet data buffer access window - PB27 */
-#define HDMI_EDID_KSV_FIFO_ACCESS_WINDOW        0x80 /* EDID/KSV FIFO access window */
-#define HDMI_DDC_BUS_ACCESS_FREQ_CTRL_7_0       0x81 /* DDC bus access frequency control (LSB) */
-#define HDMI_DDC_BUS_ACCESS_FREQ_CTRL_15_8      0x82 /* DDC bus access frequency control (MSB) */
-#define HDMI_INTERRUPT_MASK_1                   0x92 /* Interrupt mask.1 */
-#define HDMI_INTERRUPT_MASK_2                   0x93 /* Interrupt mask.2 */
-#define HDMI_INTERRUPT_STATUS_1                 0x94 /* Interrupt status.1 */
-#define HDMI_INTERRUPT_STATUS_2                 0x95 /* Interrupt status.2 */
-#define HDMI_INTERRUPT_MASK_3                   0x96 /* Interrupt mask.3 */
-#define HDMI_INTERRUPT_MASK_4                   0x97 /* Interrupt mask.4 */
-#define HDMI_INTERRUPT_STATUS_3                 0x98 /* Interrupt status.3 */
-#define HDMI_INTERRUPT_STATUS_4                 0x99 /* Interrupt status.4 */
-#define HDMI_SOFTWARE_HDCP_CONTROL_1            0x9A /* Software HDCP control.1 */
-#define HDMI_FRAME_COUNTER                      0x9C /* Frame counter */
-#define HDMI_FRAME_COUNTER_FOR_RI_CHECK         0x9D /* Frame counter for Ri check */
-#define HDMI_HDCP_CONTROL                       0xAF /* HDCP control */
-#define HDMI_RI_FRAME_COUNT_REGISTER            0xB2 /* Ri frame count register */
-#define HDMI_DDC_BUS_CONTROL                    0xB7 /* DDC bus control */
-#define HDMI_HDCP_STATUS                        0xB8 /* HDCP status */
-#define HDMI_SHA0                               0xB9 /* sha0 */
-#define HDMI_SHA1                               0xBA /* sha1 */
-#define HDMI_SHA2                               0xBB /* sha2 */
-#define HDMI_SHA3                               0xBC /* sha3 */
-#define HDMI_SHA4                               0xBD /* sha4 */
-#define HDMI_BCAPS_READ                         0xBE /* BCAPS read / debug */
-#define HDMI_AKSV_BKSV_7_0_MONITOR              0xBF /* AKSV/BKSV[7:0] monitor */
-#define HDMI_AKSV_BKSV_15_8_MONITOR             0xC0 /* AKSV/BKSV[15:8] monitor */
-#define HDMI_AKSV_BKSV_23_16_MONITOR            0xC1 /* AKSV/BKSV[23:16] monitor */
-#define HDMI_AKSV_BKSV_31_24_MONITOR            0xC2 /* AKSV/BKSV[31:24] monitor */
-#define HDMI_AKSV_BKSV_39_32_MONITOR            0xC3 /* AKSV/BKSV[39:32] monitor */
-#define HDMI_EDID_SEGMENT_POINTER               0xC4 /* EDID segment pointer */
-#define HDMI_EDID_WORD_ADDRESS                  0xC5 /* EDID word address */
-#define HDMI_EDID_DATA_FIFO_ADDRESS             0xC6 /* EDID data FIFO address */
-#define HDMI_NUM_OF_HDMI_DEVICES                0xC7 /* Number of HDMI devices */
-#define HDMI_HDCP_ERROR_CODE                    0xC8 /* HDCP error code */
-#define HDMI_100MS_TIMER_SET                    0xC9 /* 100ms timer setting */
-#define HDMI_5SEC_TIMER_SET                     0xCA /* 5sec timer setting */
-#define HDMI_RI_READ_COUNT                      0xCB /* Ri read count */
-#define HDMI_AN_SEED                            0xCC /* An seed */
-#define HDMI_MAX_NUM_OF_RCIVRS_ALLOWED          0xCD /* Maximum number of receivers allowed */
-#define HDMI_HDCP_MEMORY_ACCESS_CONTROL_1       0xCE /* HDCP memory access control.1 */
-#define HDMI_HDCP_MEMORY_ACCESS_CONTROL_2       0xCF /* HDCP memory access control.2 */
-#define HDMI_HDCP_CONTROL_2                     0xD0 /* HDCP Control 2 */
-#define HDMI_HDCP_KEY_MEMORY_CONTROL            0xD2 /* HDCP Key Memory Control */
-#define HDMI_COLOR_SPACE_CONV_CONFIG_1          0xD3 /* Color space conversion configuration.1 */
-#define HDMI_VIDEO_SETTING_3                    0xD4 /* Video setting.3 */
-#define HDMI_RI_7_0                             0xD5 /* Ri[7:0] */
-#define HDMI_RI_15_8                            0xD6 /* Ri[15:8] */
-#define HDMI_PJ                                 0xD7 /* Pj */
-#define HDMI_SHA_RD                             0xD8 /* sha_rd */
-#define HDMI_RI_7_0_SAVED                       0xD9 /* Ri[7:0] saved */
-#define HDMI_RI_15_8_SAVED                      0xDA /* Ri[15:8] saved */
-#define HDMI_PJ_SAVED                           0xDB /* Pj saved */
-#define HDMI_NUM_OF_DEVICES                     0xDC /* Number of devices */
-#define HDMI_HOT_PLUG_MSENS_STATUS              0xDF /* Hot plug/MSENS status */
-#define HDMI_BCAPS_WRITE                        0xE0 /* bcaps */
-#define HDMI_BSTAT_7_0                          0xE1 /* bstat[7:0] */
-#define HDMI_BSTAT_15_8                         0xE2 /* bstat[15:8] */
-#define HDMI_BKSV_7_0                           0xE3 /* bksv[7:0] */
-#define HDMI_BKSV_15_8                          0xE4 /* bksv[15:8] */
-#define HDMI_BKSV_23_16                         0xE5 /* bksv[23:16] */
-#define HDMI_BKSV_31_24                         0xE6 /* bksv[31:24] */
-#define HDMI_BKSV_39_32                         0xE7 /* bksv[39:32] */
-#define HDMI_AN_7_0                             0xE8 /* An[7:0] */
-#define HDMI_AN_15_8                            0xE9 /* An [15:8] */
-#define HDMI_AN_23_16                           0xEA /* An [23:16] */
-#define HDMI_AN_31_24                           0xEB /* An [31:24] */
-#define HDMI_AN_39_32                           0xEC /* An [39:32] */
-#define HDMI_AN_47_40                           0xED /* An [47:40] */
-#define HDMI_AN_55_48                           0xEE /* An [55:48] */
-#define HDMI_AN_63_56                           0xEF /* An [63:56] */
-#define HDMI_PRODUCT_ID                         0xF0 /* Product ID */
-#define HDMI_REVISION_ID                        0xF1 /* Revision ID */
-#define HDMI_TEST_MODE                          0xFE /* Test mode */
-
-/* HDMI Control Register (HTOP1) */
-#define HDMI_HTOP1_TEST_MODE                    0x0000 /* Test mode */
-#define HDMI_HTOP1_VIDEO_INPUT                  0x0008 /* VideoInput */
-#define HDMI_HTOP1_CORE_RSTN                    0x000C /* CoreResetn */
-#define HDMI_HTOP1_PLLBW                        0x0018 /* PLLBW */
-#define HDMI_HTOP1_CLK_TO_PHY                   0x001C /* Clk to Phy */
-#define HDMI_HTOP1_VIDEO_INPUT2                 0x0020 /* VideoInput2 */
-#define HDMI_HTOP1_TISEMP0_1                    0x0024 /* tisemp0-1 */
-#define HDMI_HTOP1_TISEMP2_C                    0x0028 /* tisemp2-c */
-#define HDMI_HTOP1_TISIDRV                      0x002C /* tisidrv */
-#define HDMI_HTOP1_TISEN                        0x0034 /* tisen */
-#define HDMI_HTOP1_TISDREN                      0x0038 /* tisdren  */
-#define HDMI_HTOP1_CISRANGE                     0x003C /* cisrange  */
-#define HDMI_HTOP1_ENABLE_SELECTOR              0x0040 /* Enable Selector */
-#define HDMI_HTOP1_MACRO_RESET                  0x0044 /* Macro reset */
-#define HDMI_HTOP1_PLL_CALIBRATION              0x0048 /* PLL calibration */
-#define HDMI_HTOP1_RE_CALIBRATION               0x004C /* Re-calibration */
-#define HDMI_HTOP1_CURRENT                      0x0050 /* Current */
-#define HDMI_HTOP1_PLL_LOCK_DETECT              0x0054 /* PLL lock detect */
-#define HDMI_HTOP1_PHY_TEST_MODE                0x0058 /* PHY Test Mode */
-#define HDMI_HTOP1_CLK_SET                      0x0080 /* Clock Set */
-#define HDMI_HTOP1_DDC_FAIL_SAFE                0x0084 /* DDC fail safe */
-#define HDMI_HTOP1_PRBS                         0x0088 /* PRBS */
-#define HDMI_HTOP1_EDID_AINC_CONTROL            0x008C /* EDID ainc Control */
-#define HDMI_HTOP1_HTOP_DCL_MODE                0x00FC /* Deep Coloer Mode */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF0           0x0100 /* Deep Color:FRC COEF0 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF1           0x0104 /* Deep Color:FRC COEF1 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF2           0x0108 /* Deep Color:FRC COEF2 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF3           0x010C /* Deep Color:FRC COEF3 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF0_C         0x0110 /* Deep Color:FRC COEF0C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF1_C         0x0114 /* Deep Color:FRC COEF1C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF2_C         0x0118 /* Deep Color:FRC COEF2C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF3_C         0x011C /* Deep Color:FRC COEF3C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_MODE            0x0120 /* Deep Color:FRC Mode */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START1         0x0124 /* Deep Color:Rect Start1 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE1          0x0128 /* Deep Color:Rect Size1 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START2         0x012C /* Deep Color:Rect Start2 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE2          0x0130 /* Deep Color:Rect Size2 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START3         0x0134 /* Deep Color:Rect Start3 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE3          0x0138 /* Deep Color:Rect Size3 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START4         0x013C /* Deep Color:Rect Start4 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE4          0x0140 /* Deep Color:Rect Size4 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_1       0x0144 /* Deep Color:Fil Para Y1_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_2       0x0148 /* Deep Color:Fil Para Y1_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_1      0x014C /* Deep Color:Fil Para CB1_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_2      0x0150 /* Deep Color:Fil Para CB1_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_1      0x0154 /* Deep Color:Fil Para CR1_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_2      0x0158 /* Deep Color:Fil Para CR1_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_1       0x015C /* Deep Color:Fil Para Y2_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_2       0x0160 /* Deep Color:Fil Para Y2_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_1      0x0164 /* Deep Color:Fil Para CB2_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_2      0x0168 /* Deep Color:Fil Para CB2_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_1      0x016C /* Deep Color:Fil Para CR2_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_2      0x0170 /* Deep Color:Fil Para CR2_2 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_Y1         0x0174 /* Deep Color:Cor Para Y1 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CB1        0x0178 /* Deep Color:Cor Para CB1 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CR1        0x017C /* Deep Color:Cor Para CR1 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_Y2         0x0180 /* Deep Color:Cor Para Y2 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CB2        0x0184 /* Deep Color:Cor Para CB2 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CR2        0x0188 /* Deep Color:Cor Para CR2 */
-#define HDMI_HTOP1_EDID_DATA_READ               0x0200 /* EDID Data Read 128Byte:0x03FC */
-
-enum hotplug_state {
-        HDMI_HOTPLUG_DISCONNECTED,
-        HDMI_HOTPLUG_CONNECTED,
-        HDMI_HOTPLUG_EDID_DONE,
-};
-
-struct sh_hdmi {
-        struct sh_mobile_lcdc_entity entity;
-
-        void __iomem *base;
-        void __iomem *htop1;
-        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;
-        int irq;
-        struct clk *hdmi_clk;
-        struct device *dev;
-        struct delayed_work edid_work;
-        struct fb_videomode mode;
-        struct fb_monspecs monspec;
-
-        /* register access functions */
-        void (*write)(struct sh_hdmi *hdmi, u8 data, u8 reg);
-        u8 (*read)(struct sh_hdmi *hdmi, u8 reg);
-};
-
-#define entity_to_sh_hdmi(e)    container_of(e, struct sh_hdmi, entity)
-
-static void __hdmi_write8(struct sh_hdmi *hdmi, u8 data, u8 reg)
-{
-        iowrite8(data, hdmi->base + reg);
-}
-
-static u8 __hdmi_read8(struct sh_hdmi *hdmi, u8 reg)
-{
-        return ioread8(hdmi->base + reg);
-}
-
-static void __hdmi_write32(struct sh_hdmi *hdmi, u8 data, u8 reg)
-{
-        iowrite32((u32)data, hdmi->base + (reg * 4));
-        udelay(100);
-}
-
-static u8 __hdmi_read32(struct sh_hdmi *hdmi, u8 reg)
-{
-        return (u8)ioread32(hdmi->base + (reg * 4));
-}
-
-static void hdmi_write(struct sh_hdmi *hdmi, u8 data, u8 reg)
-{
-        hdmi->write(hdmi, data, reg);
-}
-
-static u8 hdmi_read(struct sh_hdmi *hdmi, u8 reg)
-{
-        return hdmi->read(hdmi, reg);
-}
-
-static void hdmi_bit_set(struct sh_hdmi *hdmi, u8 mask, u8 data, u8 reg)
-{
-        u8 val = hdmi_read(hdmi, reg);
-
-        val &= ~mask;
-        val |= (data & mask);
-
-        hdmi_write(hdmi, val, reg);
-}
-
-static void hdmi_htop1_write(struct sh_hdmi *hdmi, u32 data, u32 reg)
-{
-        iowrite32(data, hdmi->htop1 + reg);
-        udelay(100);
-}
-
-static u32 hdmi_htop1_read(struct sh_hdmi *hdmi, u32 reg)
-{
-        return ioread32(hdmi->htop1 + 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 sh_hdmi_external_video_param(struct sh_hdmi *hdmi)
-{
-        struct fb_videomode *mode = &hdmi->mode;
-        u16 htotal, hblank, hdelay, vtotal, vblank, vdelay, voffset;
-        u8 sync = 0;
-
-        htotal = mode->xres + mode->right_margin + mode->left_margin
-               + mode->hsync_len;
-        hdelay = mode->hsync_len + mode->left_margin;
-        hblank = mode->right_margin + hdelay;
-
-        /*
-         * Vertical timing looks a bit different in Figure 18,
-         * but let's try the same first by setting offset = 0
-         */
-        vtotal = mode->yres + mode->upper_margin + mode->lower_margin
-               + mode->vsync_len;
-        vdelay = mode->vsync_len + mode->upper_margin;
-        vblank = mode->lower_margin + vdelay;
-        voffset = min(mode->upper_margin / 2, 6U);
-
-        /*
-         * [3]: VSYNC polarity: Positive
-         * [2]: HSYNC polarity: Positive
-         * [1]: Interlace/Progressive: Progressive
-         * [0]: External video settings enable: used.
-         */
-        if (mode->sync & FB_SYNC_HOR_HIGH_ACT)
-                sync |= 4;
-        if (mode->sync & FB_SYNC_VERT_HIGH_ACT)
-                sync |= 8;
-
-        dev_dbg(hdmi->dev, "H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
-                htotal, hblank, hdelay, mode->hsync_len,
-                vtotal, vblank, vdelay, mode->vsync_len, sync);
-
-        hdmi_write(hdmi, sync | (voffset << 4), HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
-
-        hdmi_write(hdmi, htotal, HDMI_EXTERNAL_H_TOTAL_7_0);
-        hdmi_write(hdmi, htotal >> 8, HDMI_EXTERNAL_H_TOTAL_11_8);
-
-        hdmi_write(hdmi, hblank, HDMI_EXTERNAL_H_BLANK_7_0);
-        hdmi_write(hdmi, hblank >> 8, HDMI_EXTERNAL_H_BLANK_9_8);
-
-        hdmi_write(hdmi, hdelay, HDMI_EXTERNAL_H_DELAY_7_0);
-        hdmi_write(hdmi, hdelay >> 8, HDMI_EXTERNAL_H_DELAY_9_8);
-
-        hdmi_write(hdmi, mode->hsync_len, HDMI_EXTERNAL_H_DURATION_7_0);
-        hdmi_write(hdmi, mode->hsync_len >> 8, HDMI_EXTERNAL_H_DURATION_9_8);
-
-        hdmi_write(hdmi, vtotal, HDMI_EXTERNAL_V_TOTAL_7_0);
-        hdmi_write(hdmi, vtotal >> 8, HDMI_EXTERNAL_V_TOTAL_9_8);
-
-        hdmi_write(hdmi, vblank, HDMI_EXTERNAL_V_BLANK);
-
-        hdmi_write(hdmi, vdelay, HDMI_EXTERNAL_V_DELAY);
-
-        hdmi_write(hdmi, mode->vsync_len, HDMI_EXTERNAL_V_DURATION);
-
-        /* 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);
-}
-
-/**
- * sh_hdmi_video_config()
- */
-static void sh_hdmi_video_config(struct sh_hdmi *hdmi)
-{
-        /*
-         * [7:4]: Audio sampling frequency: 48kHz
-         * [3:1]: Input video format: RGB and YCbCr 4:4:4 (Y on Green)
-         * [0]: Internal/External DE select: internal
-         */
-        hdmi_write(hdmi, 0x20, HDMI_AUDIO_VIDEO_SETTING_1);
-
-        /*
-         * [7:6]: Video output format: RGB 4:4:4
-         * [5:4]: Input video data width: 8 bit
-         * [3:1]: EAV/SAV location: channel 1
-         * [0]: Video input color space: RGB
-         */
-        hdmi_write(hdmi, 0x34, HDMI_VIDEO_SETTING_1);
-
-        /*
-         * [7:6]: Together with bit [6] of HDMI_AUDIO_VIDEO_SETTING_2, which is
-         * left at 0 by default, this configures 24bpp and sets the Color Depth
-         * (CD) field in the General Control Packet
-         */
-        hdmi_write(hdmi, 0x20, HDMI_DEEP_COLOR_MODES);
-}
-
-/**
- * sh_hdmi_audio_config()
- */
-static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
-{
-        u8 data;
-        struct sh_mobile_hdmi_info *pdata = dev_get_platdata(hdmi->dev);
-
-        /*
-         * [7:4] L/R data swap control
-         * [3:0] appropriate N[19:16]
-         */
-        hdmi_write(hdmi, 0x00, HDMI_L_R_DATA_SWAP_CTRL_RPKT);
-        /* appropriate N[15:8] */
-        hdmi_write(hdmi, 0x18, HDMI_20_BIT_N_FOR_AUDIO_RPKT_15_8);
-        /* appropriate N[7:0] */
-        hdmi_write(hdmi, 0x00, HDMI_20_BIT_N_FOR_AUDIO_RPKT_7_0);
-
-        /* [7:4] 48 kHz SPDIF not used */
-        hdmi_write(hdmi, 0x20, HDMI_SPDIF_AUDIO_SAMP_FREQ_CTS);
-
-        /*
-         * [6:5] set required down sampling rate if required
-         * [4:3] set required audio source
-         */
-        switch (pdata->flags & HDMI_SND_SRC_MASK) {
-        default:
-                /* fall through */
-        case HDMI_SND_SRC_I2S:
-                data = 0x0 << 3;
-                break;
-        case HDMI_SND_SRC_SPDIF:
-                data = 0x1 << 3;
-                break;
-        case HDMI_SND_SRC_DSD:
-                data = 0x2 << 3;
-                break;
-        case HDMI_SND_SRC_HBR:
-                data = 0x3 << 3;
-                break;
-        }
-        hdmi_write(hdmi, data, HDMI_AUDIO_SETTING_1);
-
-        /* [3:0] set sending channel number for channel status */
-        hdmi_write(hdmi, 0x40, HDMI_AUDIO_SETTING_2);
-
-        /*
-         * [5:2] set valid I2S source input pin
-         * [1:0] set input I2S source mode
-         */
-        hdmi_write(hdmi, 0x04, HDMI_I2S_AUDIO_SET);
-
-        /* [7:4] set valid DSD source input pin */
-        hdmi_write(hdmi, 0x00, HDMI_DSD_AUDIO_SET);
-
-        /* [7:0] set appropriate I2S input pin swap settings if required */
-        hdmi_write(hdmi, 0x00, HDMI_I2S_INPUT_PIN_SWAP);
-
-        /*
-         * [7] set validity bit for channel status
-         * [3:0] set original sample frequency for channel status
-         */
-        hdmi_write(hdmi, 0x00, HDMI_AUDIO_STATUS_BITS_SETTING_1);
-
-        /*
-         * [7] set value for channel status
-         * [6] set value for channel status
-         * [5] set copyright bit for channel status
-         * [4:2] set additional information for channel status
-         * [1:0] set clock accuracy for channel status
-         */
-        hdmi_write(hdmi, 0x00, HDMI_AUDIO_STATUS_BITS_SETTING_2);
-
-        /* [7:0] set category code for channel status */
-        hdmi_write(hdmi, 0x00, HDMI_CATEGORY_CODE);
-
-        /*
-         * [7:4] set source number for channel status
-         * [3:0] set word length for channel status
-         */
-        hdmi_write(hdmi, 0x00, HDMI_SOURCE_NUM_AUDIO_WORD_LEN);
-
-        /* [7:4] set sample frequency for channel status */
-        hdmi_write(hdmi, 0x20, HDMI_AUDIO_VIDEO_SETTING_1);
-}
-
-/**
- * sh_hdmi_phy_config() - configure the HDMI PHY for the used video mode
- */
-static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
-{
-        if (hdmi->mode.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->mode.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);
-        }
-}
-
-/**
- * sh_hdmi_avi_infoframe_setup() - Auxiliary Video Information InfoFrame CONTROL PACKET
- */
-static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
-{
-        u8 vic;
-
-        /* AVI InfoFrame */
-        hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_INDEX);
-
-        /* Packet Type = 0x82 */
-        hdmi_write(hdmi, 0x82, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-
-        /* Version = 0x02 */
-        hdmi_write(hdmi, 0x02, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-
-        /* Length = 13 (0x0D) */
-        hdmi_write(hdmi, 0x0D, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-        /* N. A. Checksum */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0);
-
-        /*
-         * Y = RGB
-         * A0 = No Data
-         * B = Bar Data not valid
-         * S = No Data
-         */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
-
-        /*
-         * [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);
-
-        /*
-         * ITC = No Data
-         * EC = xvYCC601
-         * Q = Default (depends on video format)
-         * SC = No Known non_uniform Scaling
-         */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB3);
-
-        /*
-         * VIC 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
-         */
-        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);
-
-        /* Line Number of End of Top Bar (lower 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB6);
-
-        /* Line Number of End of Top Bar (upper 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB7);
-
-        /* Line Number of Start of Bottom Bar (lower 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB8);
-
-        /* Line Number of Start of Bottom Bar (upper 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB9);
-
-        /* Pixel Number of End of Left Bar (lower 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
-
-        /* Pixel Number of End of Left Bar (upper 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB11);
-
-        /* Pixel Number of Start of Right Bar (lower 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB12);
-
-        /* Pixel Number of Start of Right Bar (upper 8 bits) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB13);
-}
-
-/**
- * sh_hdmi_audio_infoframe_setup() - Audio InfoFrame of CONTROL PACKET
- */
-static void sh_hdmi_audio_infoframe_setup(struct sh_hdmi *hdmi)
-{
-        /* Audio InfoFrame */
-        hdmi_write(hdmi, 0x08, HDMI_CTRL_PKT_BUF_INDEX);
-
-        /* Packet Type = 0x84 */
-        hdmi_write(hdmi, 0x84, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-
-        /* Version Number = 0x01 */
-        hdmi_write(hdmi, 0x01, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-
-        /* 0 Length = 10 (0x0A) */
-        hdmi_write(hdmi, 0x0A, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-        /* n. a. Checksum */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0);
-
-        /* Audio Channel Count = Refer to Stream Header */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
-
-        /* Refer to Stream Header */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
-
-        /* Format depends on coding type (i.e. CT0...CT3) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB3);
-
-        /* Speaker Channel Allocation = Front Right + Front Left */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
-
-        /* Level Shift Value = 0 dB, Down - mix is permitted or no information */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
-
-        /* Reserved (0) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB6);
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB7);
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB8);
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB9);
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
-}
-
-/**
- * sh_hdmi_configure() - Initialise HDMI for output
- */
-static void sh_hdmi_configure(struct sh_hdmi *hdmi)
-{
-        /* Configure video format */
-        sh_hdmi_video_config(hdmi);
-
-        /* Configure audio format */
-        sh_hdmi_audio_config(hdmi);
-
-        /* Configure PHY */
-        sh_hdmi_phy_config(hdmi);
-
-        /* Auxiliary Video Information (AVI) InfoFrame */
-        sh_hdmi_avi_infoframe_setup(hdmi);
-
-        /* Audio InfoFrame */
-        sh_hdmi_audio_infoframe_setup(hdmi);
-
-        /*
-         * Control packet auto send with VSYNC control: auto send
-         * General control, Gamut metadata, ISRC, and ACP packets
-         */
-        hdmi_write(hdmi, 0x8E, HDMI_CTRL_PKT_AUTO_SEND);
-
-        /* FIXME */
-        msleep(10);
-
-        /* PS mode b->d, reset PLLA and PLLB */
-        hdmi_bit_set(hdmi, 0xFC, 0x4C, HDMI_SYSTEM_CTRL);
-
-        udelay(10);
-
-        hdmi_bit_set(hdmi, 0xFC, 0x40, HDMI_SYSTEM_CTRL);
-}
-
-static unsigned long sh_hdmi_rate_error(struct sh_hdmi *hdmi,
-                const struct fb_videomode *mode,
-                unsigned long *hdmi_rate, unsigned long *parent_rate)
-{
-        unsigned long target = PICOS2KHZ(mode->pixclock) * 1000, rate_error;
-        struct sh_mobile_hdmi_info *pdata = dev_get_platdata(hdmi->dev);
-
-        *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 sh_mobile_lcdc_chan *ch = hdmi->entity.lcdc;
-        const struct fb_videomode *mode, *found = 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;
-        bool use_edid_mode = false;
-        u8 edid[128];
-        char *forced;
-        int i;
-
-        /* Read EDID */
-        dev_dbg(hdmi->dev, "Read back EDID code:");
-        for (i = 0; i < 128; i++) {
-                edid[i] = (hdmi->htop1) ?
-                        (u8)hdmi_htop1_read(hdmi, HDMI_HTOP1_EDID_DATA_READ + (i * 4)) :
-                        hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
-#ifdef DEBUG
-                if ((i % 16) == 0) {
-                        printk(KERN_CONT "\n");
-                        printk(KERN_DEBUG "%02X | %02X", i, edid[i]);
-                } else {
-                        printk(KERN_CONT " %02X", edid[i]);
-                }
-#endif
-        }
-#ifdef DEBUG
-        printk(KERN_CONT "\n");
-#endif
-
-        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 */
-                if (ch && ch->notify &&
-                    ch->notify(ch, SH_MOBILE_LCDC_EVENT_DISPLAY_MODE, mode,
-                               NULL)) {
-                        scanning = true;
-                        preferred_bad = true;
-                        continue;
-                }
-
-                found = mode;
-                found_rate_error = rate_error;
-                use_edid_mode = true;
-        }
-
-        /*
-         * TODO 1: if no default mode is present, postpone running the config
-         * until after the LCDC channel is initialized.
-         * TODO 2: consider registering the HDMI platform device from the LCDC
-         * driver.
-         */
-        if (!found && hdmi->entity.def_mode.xres != 0) {
-                found = &hdmi->entity.def_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", use_edid_mode ? "EDID" : "default",
-                hdmi->preprogrammed_vic ? "VIC" : "external", found->xres,
-                found->yres, found->refresh, PICOS2KHZ(found->pixclock) * 1000,
-                found_rate_error);
-
-        hdmi->mode = *found;
-        sh_hdmi_external_video_param(hdmi);
-
-        return 0;
-}
-
-static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
-{
-        struct sh_hdmi *hdmi = dev_id;
-        u8 status1, status2, mask1, mask2;
-
-        /* mode_b and PLLA and PLLB reset */
-        hdmi_bit_set(hdmi, 0xFC, 0x2C, HDMI_SYSTEM_CTRL);
-
-        /* How long shall reset be held? */
-        udelay(10);
-
-        /* mode_b and PLLA and PLLB reset release */
-        hdmi_bit_set(hdmi, 0xFC, 0x20, HDMI_SYSTEM_CTRL);
-
-        status1 = hdmi_read(hdmi, HDMI_INTERRUPT_STATUS_1);
-        status2 = hdmi_read(hdmi, HDMI_INTERRUPT_STATUS_2);
-
-        mask1 = hdmi_read(hdmi, HDMI_INTERRUPT_MASK_1);
-        mask2 = hdmi_read(hdmi, HDMI_INTERRUPT_MASK_2);
-
-        /* Correct would be to ack only set bits, but the datasheet requires 0xff */
-        hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_1);
-        hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_2);
-
-        if (printk_ratelimit())
-                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;
-        } else if (status1 & 0xc0) {
-                u8 msens;
-
-                /* Datasheet specifies 10ms... */
-                udelay(500);
-
-                msens = hdmi_read(hdmi, HDMI_HOT_PLUG_MSENS_STATUS);
-                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);
-                        /* 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) {
-                                hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
-                                schedule_delayed_work(&hdmi->edid_work, 0);
-                        }
-                        /* display_off will switch back to mode_a */
-                }
-        } else if (status1 & 2) {
-                /* EDID error interrupt: retry */
-                /* Set EDID word address  */
-                hdmi_write(hdmi, hdmi->edid_block_addr, HDMI_EDID_WORD_ADDRESS);
-                /* Set 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);
-                schedule_delayed_work(&hdmi->edid_work, msecs_to_jiffies(10));
-        }
-
-        return IRQ_HANDLED;
-}
-
-static int sh_hdmi_display_on(struct sh_mobile_lcdc_entity *entity)
-{
-        struct sh_hdmi *hdmi = entity_to_sh_hdmi(entity);
-
-        dev_dbg(hdmi->dev, "%s(%p): state %x\n", __func__, hdmi,
-                hdmi->hp_state);
-
-        /*
-         * 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
-         */
-        if (hdmi->hp_state == HDMI_HOTPLUG_EDID_DONE) {
-                /* PS mode d->e. All functions are active */
-                hdmi_bit_set(hdmi, 0xFC, 0x80, HDMI_SYSTEM_CTRL);
-                dev_dbg(hdmi->dev, "HDMI running\n");
-        }
-
-        return hdmi->hp_state == HDMI_HOTPLUG_DISCONNECTED
-                ? SH_MOBILE_LCDC_DISPLAY_DISCONNECTED
-                : SH_MOBILE_LCDC_DISPLAY_CONNECTED;
-}
-
-static void sh_hdmi_display_off(struct sh_mobile_lcdc_entity *entity)
-{
-        struct sh_hdmi *hdmi = entity_to_sh_hdmi(entity);
-
-        dev_dbg(hdmi->dev, "%s(%p)\n", __func__, hdmi);
-        /* PS mode e->a */
-        hdmi_bit_set(hdmi, 0xFC, 0x10, HDMI_SYSTEM_CTRL);
-}
-
-static const struct sh_mobile_lcdc_entity_ops sh_hdmi_ops = {
-        .display_on = sh_hdmi_display_on,
-        .display_off = sh_hdmi_display_off,
-};
-
-/**
- * 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 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_lcdc_chan *ch = hdmi->entity.lcdc;
-        int ret;
-
-        dev_dbg(hdmi->dev, "%s(%p): begin, hotplug status %d\n", __func__, hdmi,
-                hdmi->hp_state);
-
-        if (hdmi->hp_state == HDMI_HOTPLUG_CONNECTED) {
-                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 (ch && ch->notify)
-                        ch->notify(ch, SH_MOBILE_LCDC_EVENT_DISPLAY_CONNECT,
-                                   &hdmi->mode, &hdmi->monspec);
-        } else {
-                hdmi->monspec.modedb_len = 0;
-                fb_destroy_modedb(hdmi->monspec.modedb);
-                hdmi->monspec.modedb = NULL;
-
-                if (ch && ch->notify)
-                        ch->notify(ch, SH_MOBILE_LCDC_EVENT_DISPLAY_DISCONNECT,
-                                   NULL, NULL);
-
-                ret = 0;
-        }
-
-out:
-        if (ret < 0 && ret != -EAGAIN)
-                hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
-
-        dev_dbg(hdmi->dev, "%s(%p): end\n", __func__, hdmi);
-}
-
-static void sh_hdmi_htop1_init(struct sh_hdmi *hdmi)
-{
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_MODE);
-        hdmi_htop1_write(hdmi, 0x0000000b, 0x0010);
-        hdmi_htop1_write(hdmi, 0x00006710, HDMI_HTOP1_HTOP_DCL_FRC_MODE);
-        hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_1);
-        hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_2);
-        hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_1);
-        hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_2);
-        hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_1);
-        hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_2);
-        hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_1);
-        hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_2);
-        hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_1);
-        hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_2);
-        hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_1);
-        hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_2);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_Y1);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CB1);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CR1);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_Y2);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CB2);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CR2);
-        hdmi_htop1_write(hdmi, 0x00000008, HDMI_HTOP1_CURRENT);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_TISEMP0_1);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_TISEMP2_C);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_PHY_TEST_MODE);
-        hdmi_htop1_write(hdmi, 0x00000081, HDMI_HTOP1_TISIDRV);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_PLLBW);
-        hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISEN);
-        hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISDREN);
-        hdmi_htop1_write(hdmi, 0x00000003, HDMI_HTOP1_ENABLE_SELECTOR);
-        hdmi_htop1_write(hdmi, 0x00000001, HDMI_HTOP1_MACRO_RESET);
-        hdmi_htop1_write(hdmi, 0x00000016, HDMI_HTOP1_CISRANGE);
-        msleep(100);
-        hdmi_htop1_write(hdmi, 0x00000001, HDMI_HTOP1_ENABLE_SELECTOR);
-        msleep(100);
-        hdmi_htop1_write(hdmi, 0x00000003, HDMI_HTOP1_ENABLE_SELECTOR);
-        hdmi_htop1_write(hdmi, 0x00000001, HDMI_HTOP1_MACRO_RESET);
-        hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISEN);
-        hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISDREN);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_VIDEO_INPUT);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_CLK_TO_PHY);
-        hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_VIDEO_INPUT2);
-        hdmi_htop1_write(hdmi, 0x0000000a, HDMI_HTOP1_CLK_SET);
-}
-
-static int __init sh_hdmi_probe(struct platform_device *pdev)
-{
-        struct sh_mobile_hdmi_info *pdata = dev_get_platdata(&pdev->dev);
-        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-        struct resource *htop1_res;
-        int irq = platform_get_irq(pdev, 0), ret;
-        struct sh_hdmi *hdmi;
-        long rate;
-
-        if (!res || !pdata || irq < 0)
-                return -ENODEV;
-
-        htop1_res = NULL;
-        if (pdata->flags & HDMI_HAS_HTOP1) {
-                htop1_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-                if (!htop1_res) {
-                        dev_err(&pdev->dev, "htop1 needs register base\n");
-                        return -EINVAL;
-                }
-        }
-
-        hdmi = devm_kzalloc(&pdev->dev, sizeof(*hdmi), GFP_KERNEL);
-        if (!hdmi) {
-                dev_err(&pdev->dev, "Cannot allocate device data\n");
-                return -ENOMEM;
-        }
-
-        hdmi->dev = &pdev->dev;
-        hdmi->entity.owner = THIS_MODULE;
-        hdmi->entity.ops = &sh_hdmi_ops;
-        hdmi->irq = irq;
-
-        hdmi->hdmi_clk = clk_get(&pdev->dev, "ick");
-        if (IS_ERR(hdmi->hdmi_clk)) {
-                ret = PTR_ERR(hdmi->hdmi_clk);
-                dev_err(&pdev->dev, "Unable to get clock: %d\n", ret);
-                return ret;
-        }
-
-        /* select register access functions */
-        if (pdata->flags & HDMI_32BIT_REG) {
-                hdmi->write     = __hdmi_write32;
-                hdmi->read      = __hdmi_read32;
-        } else {
-                hdmi->write     = __hdmi_write8;
-                hdmi->read      = __hdmi_read8;
-        }
-
-        /* 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);
-
-        if (rate < 0) {
-                ret = rate;
-                goto erate;
-        }
-
-        ret = clk_prepare_enable(hdmi->hdmi_clk);
-        if (ret < 0) {
-                dev_err(hdmi->dev, "Cannot enable clock: %d\n", ret);
-                goto erate;
-        }
-
-        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");
-                ret = -EBUSY;
-                goto ereqreg;
-        }
-
-        hdmi->base = ioremap(res->start, resource_size(res));
-        if (!hdmi->base) {
-                dev_err(&pdev->dev, "HDMI register region already claimed\n");
-                ret = -ENOMEM;
-                goto emap;
-        }
-
-        platform_set_drvdata(pdev, &hdmi->entity);
-
-        INIT_DELAYED_WORK(&hdmi->edid_work, sh_hdmi_edid_work_fn);
-
-        pm_runtime_enable(&pdev->dev);
-        pm_runtime_get_sync(&pdev->dev);
-
-        /* init interrupt polarity */
-        if (pdata->flags & HDMI_OUTPUT_PUSH_PULL)
-                hdmi_bit_set(hdmi, 0x02, 0x02, HDMI_SYSTEM_CTRL);
-
-        if (pdata->flags & HDMI_OUTPUT_POLARITY_HI)
-                hdmi_bit_set(hdmi, 0x01, 0x01, HDMI_SYSTEM_CTRL);
-
-        /* enable htop1 register if needed */
-        if (htop1_res) {
-                hdmi->htop1 = ioremap(htop1_res->start, resource_size(htop1_res));
-                if (!hdmi->htop1) {
-                        dev_err(&pdev->dev, "control register region already claimed\n");
-                        ret = -ENOMEM;
-                        goto emap_htop1;
-                }
-                sh_hdmi_htop1_init(hdmi);
-        }
-
-        /* 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);
-        if (ret < 0) {
-                dev_err(&pdev->dev, "Unable to request irq: %d\n", ret);
-                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;
-        }
-
-        return 0;
-
-ecodec:
-        free_irq(irq, hdmi);
-ereqirq:
-        if (hdmi->htop1)
-                iounmap(hdmi->htop1);
-emap_htop1:
-        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_unprepare(hdmi->hdmi_clk);
-erate:
-        clk_put(hdmi->hdmi_clk);
-
-        return ret;
-}
-
-static int __exit sh_hdmi_remove(struct platform_device *pdev)
-{
-        struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
-        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-        snd_soc_unregister_codec(&pdev->dev);
-
-        /* No new work will be scheduled, wait for running ISR */
-        free_irq(hdmi->irq, hdmi);
-        /* Wait for already scheduled work */
-        cancel_delayed_work_sync(&hdmi->edid_work);
-        pm_runtime_put(&pdev->dev);
-        pm_runtime_disable(&pdev->dev);
-        clk_disable_unprepare(hdmi->hdmi_clk);
-        clk_put(hdmi->hdmi_clk);
-        if (hdmi->htop1)
-                iounmap(hdmi->htop1);
-        iounmap(hdmi->base);
-        release_mem_region(res->start, resource_size(res));
-
-        return 0;
-}
-
-static int sh_hdmi_suspend(struct device *dev)
-{
-        struct platform_device *pdev = to_platform_device(dev);
-        struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
-
-        disable_irq(hdmi->irq);
-        /* Wait for already scheduled work */
-        cancel_delayed_work_sync(&hdmi->edid_work);
-        return 0;
-}
-
-static int sh_hdmi_resume(struct device *dev)
-{
-        struct platform_device *pdev = to_platform_device(dev);
-        struct sh_mobile_hdmi_info *pdata = dev_get_platdata(dev);
-        struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
-
-        /* Re-init interrupt polarity */
-        if (pdata->flags & HDMI_OUTPUT_PUSH_PULL)
-                hdmi_bit_set(hdmi, 0x02, 0x02, HDMI_SYSTEM_CTRL);
-
-        if (pdata->flags & HDMI_OUTPUT_POLARITY_HI)
-                hdmi_bit_set(hdmi, 0x01, 0x01, HDMI_SYSTEM_CTRL);
-
-        /* Re-init htop1 */
-        if (hdmi->htop1)
-                sh_hdmi_htop1_init(hdmi);
-
-        /* Now it's safe to enable interrupts again */
-        enable_irq(hdmi->irq);
-        return 0;
-}
-
-static const struct dev_pm_ops sh_hdmi_pm_ops = {
-        .suspend        = sh_hdmi_suspend,
-        .resume         = sh_hdmi_resume,
-};
-
-static struct platform_driver sh_hdmi_driver = {
-        .remove         = __exit_p(sh_hdmi_remove),
-        .driver = {
-                .name   = "sh-mobile-hdmi",
-                .pm     = &sh_hdmi_pm_ops,
-        },
-};
-
-module_platform_driver_probe(sh_hdmi_driver, sh_hdmi_probe);
-
-MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
-MODULE_DESCRIPTION("SuperH / ARM-shmobile HDMI driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/fbdev/simplefb.c b/drivers/video/fbdev/simplefb.c
index 52c5c7e63b52..48ccf6db62a2 100644
--- a/drivers/video/fbdev/simplefb.c
+++ b/drivers/video/fbdev/simplefb.c
@@ -28,7 +28,10 @@
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/clk-provider.h>
+#include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/parser.h>
+#include <linux/regulator/consumer.h>
 
 static struct fb_fix_screeninfo simplefb_fix = {
         .id             = "simple",
@@ -174,6 +177,10 @@ struct simplefb_par {
         int clk_count;
         struct clk **clks;
 #endif
+#if defined CONFIG_OF && defined CONFIG_REGULATOR
+        u32 regulator_count;
+        struct regulator **regulators;
+#endif
 };
 
 #if defined CONFIG_OF && defined CONFIG_COMMON_CLK
@@ -269,6 +276,110 @@ static int simplefb_clocks_init(struct simplefb_par *par,
 static void simplefb_clocks_destroy(struct simplefb_par *par) { }
 #endif
 
+#if defined CONFIG_OF && defined CONFIG_REGULATOR
+
+#define SUPPLY_SUFFIX "-supply"
+
+/*
+ * Regulator handling code.
+ *
+ * Here we handle the num-supplies and vin*-supply properties of our
+ * "simple-framebuffer" dt node. This is necessary so that we can make sure
+ * that any regulators needed by the display hardware that the bootloader
+ * set up for us (and for which it provided a simplefb dt node), stay up,
+ * for the life of the simplefb driver.
+ *
+ * When the driver unloads, we cleanly disable, and then release the
+ * regulators.
+ *
+ * We only complain about errors here, no action is taken as the most likely
+ * error can only happen due to a mismatch between the bootloader which set
+ * up simplefb, and the regulator definitions in the device tree. Chances are
+ * that there are no adverse effects, and if there are, a clean teardown of
+ * the fb probe will not help us much either. So just complain and carry on,
+ * and hope that the user actually gets a working fb at the end of things.
+ */
+static int simplefb_regulators_init(struct simplefb_par *par,
+        struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct property *prop;
+        struct regulator *regulator;
+        const char *p;
+        int count = 0, i = 0, ret;
+
+        if (dev_get_platdata(&pdev->dev) || !np)
+                return 0;
+
+        /* Count the number of regulator supplies */
+        for_each_property_of_node(np, prop) {
+                p = strstr(prop->name, SUPPLY_SUFFIX);
+                if (p && p != prop->name)
+                        count++;
+        }
+
+        if (!count)
+                return 0;
+
+        par->regulators = devm_kcalloc(&pdev->dev, count,
+                                       sizeof(struct regulator *), GFP_KERNEL);
+        if (!par->regulators)
+                return -ENOMEM;
+
+        /* Get all the regulators */
+        for_each_property_of_node(np, prop) {
+                char name[32]; /* 32 is max size of property name */
+
+                p = strstr(prop->name, SUPPLY_SUFFIX);
+                if (!p || p == prop->name)
+                        continue;
+
+                strlcpy(name, prop->name,
+                        strlen(prop->name) - strlen(SUPPLY_SUFFIX) + 1);
+                regulator = devm_regulator_get_optional(&pdev->dev, name);
+                if (IS_ERR(regulator)) {
+                        if (PTR_ERR(regulator) == -EPROBE_DEFER)
+                                return -EPROBE_DEFER;
+                        dev_err(&pdev->dev, "regulator %s not found: %ld\n",
+                                name, PTR_ERR(regulator));
+                        continue;
+                }
+                par->regulators[i++] = regulator;
+        }
+        par->regulator_count = i;
+
+        /* Enable all the regulators */
+        for (i = 0; i < par->regulator_count; i++) {
+                ret = regulator_enable(par->regulators[i]);
+                if (ret) {
+                        dev_err(&pdev->dev,
+                                "failed to enable regulator %d: %d\n",
+                                i, ret);
+                        devm_regulator_put(par->regulators[i]);
+                        par->regulators[i] = NULL;
+                }
+        }
+
+        return 0;
+}
+
+static void simplefb_regulators_destroy(struct simplefb_par *par)
+{
+        int i;
+
+        if (!par->regulators)
+                return;
+
+        for (i = 0; i < par->regulator_count; i++)
+                if (par->regulators[i])
+                        regulator_disable(par->regulators[i]);
+}
+#else
+static int simplefb_regulators_init(struct simplefb_par *par,
+        struct platform_device *pdev) { return 0; }
+static void simplefb_regulators_destroy(struct simplefb_par *par) { }
+#endif
+
 static int simplefb_probe(struct platform_device *pdev)
 {
         int ret;
@@ -340,6 +451,10 @@ static int simplefb_probe(struct platform_device *pdev)
         if (ret < 0)
                 goto error_unmap;
 
+        ret = simplefb_regulators_init(par, pdev);
+        if (ret < 0)
+                goto error_clocks;
+
         dev_info(&pdev->dev, "framebuffer at 0x%lx, 0x%x bytes, mapped to 0x%p\n",
                              info->fix.smem_start, info->fix.smem_len,
                              info->screen_base);
@@ -351,13 +466,15 @@ static int simplefb_probe(struct platform_device *pdev)
         ret = register_framebuffer(info);
         if (ret < 0) {
                 dev_err(&pdev->dev, "Unable to register simplefb: %d\n", ret);
-                goto error_clocks;
+                goto error_regulators;
         }
 
         dev_info(&pdev->dev, "fb%d: simplefb registered!\n", info->node);
 
         return 0;
 
+error_regulators:
+        simplefb_regulators_destroy(par);
 error_clocks:
         simplefb_clocks_destroy(par);
 error_unmap:
@@ -373,6 +490,7 @@ static int simplefb_remove(struct platform_device *pdev)
         struct simplefb_par *par = info->par;
 
         unregister_framebuffer(info);
+        simplefb_regulators_destroy(par);
         simplefb_clocks_destroy(par);
         framebuffer_release(info);
 
diff --git a/drivers/video/fbdev/sm712fb.c b/drivers/video/fbdev/sm712fb.c
index 629bfa2d2f51..86ae1d4556fc 100644
--- a/drivers/video/fbdev/sm712fb.c
+++ b/drivers/video/fbdev/sm712fb.c
@@ -28,9 +28,7 @@
 #include <linux/console.h>
 #include <linux/screen_info.h>
 
-#ifdef CONFIG_PM
 #include <linux/pm.h>
-#endif
 
 #include "sm712.h"
 
@@ -1545,8 +1543,7 @@ static void smtcfb_pci_remove(struct pci_dev *pdev)
         pci_disable_device(pdev);
 }
 
-#ifdef CONFIG_PM
-static int smtcfb_pci_suspend(struct device *device)
+static int __maybe_unused smtcfb_pci_suspend(struct device *device)
 {
         struct pci_dev *pdev = to_pci_dev(device);
         struct smtcfb_info *sfb;
@@ -1569,7 +1566,7 @@ static int smtcfb_pci_suspend(struct device *device)
         return 0;
 }
 
-static int smtcfb_pci_resume(struct device *device)
+static int __maybe_unused smtcfb_pci_resume(struct device *device)
 {
         struct pci_dev *pdev = to_pci_dev(device);
         struct smtcfb_info *sfb;
@@ -1610,20 +1607,13 @@ static int smtcfb_pci_resume(struct device *device)
 }
 
 static SIMPLE_DEV_PM_OPS(sm7xx_pm_ops, smtcfb_pci_suspend, smtcfb_pci_resume);
-#define SM7XX_PM_OPS (&sm7xx_pm_ops)
-
-#else  /* !CONFIG_PM */
-
-#define SM7XX_PM_OPS NULL
-
-#endif /* !CONFIG_PM */
 
 static struct pci_driver smtcfb_driver = {
         .name = "smtcfb",
         .id_table = smtcfb_pci_table,
         .probe = smtcfb_pci_probe,
         .remove = smtcfb_pci_remove,
-        .driver.pm  = SM7XX_PM_OPS,
+        .driver.pm  = &sm7xx_pm_ops,
 };
 
 static int __init sm712fb_init(void)
diff --git a/include/linux/fb.h b/include/linux/fb.h
index 3d003805aac3..55433f86f0a3 100644
--- a/include/linux/fb.h
+++ b/include/linux/fb.h
@@ -175,9 +175,27 @@ struct fb_blit_caps {
         u32 flags;
 };
 
+#ifdef CONFIG_FB_NOTIFY
 extern int fb_register_client(struct notifier_block *nb);
 extern int fb_unregister_client(struct notifier_block *nb);
 extern int fb_notifier_call_chain(unsigned long val, void *v);
+#else
+static inline int fb_register_client(struct notifier_block *nb)
+{
+        return 0;
+};
+
+static inline int fb_unregister_client(struct notifier_block *nb)
+{
+        return 0;
+};
+
+static inline int fb_notifier_call_chain(unsigned long val, void *v)
+{
+        return 0;
+};
+#endif
+
 /*
  * Pixmap structure definition
  *
diff --git a/include/video/omapdss.h b/include/video/omapdss.h
index f001a356fd98..295b41e20d8e 100644
--- a/include/video/omapdss.h
+++ b/include/video/omapdss.h
@@ -87,6 +87,7 @@ enum omap_channel {
         OMAP_DSS_CHANNEL_DIGIT  = 1,
         OMAP_DSS_CHANNEL_LCD2   = 2,
         OMAP_DSS_CHANNEL_LCD3   = 3,
+        OMAP_DSS_CHANNEL_WB     = 4,
 };
 
 enum omap_color_mode {
@@ -367,14 +368,12 @@ struct omap_video_timings {
         enum omap_dss_signal_edge sync_pclk_edge;
 };
 
-#ifdef CONFIG_OMAP2_DSS_VENC
 /* Hardcoded timings for tv modes. Venc only uses these to
  * identify the mode, and does not actually use the configs
  * itself. However, the configs should be something that
  * a normal monitor can also show */
 extern const struct omap_video_timings omap_dss_pal_timings;
 extern const struct omap_video_timings omap_dss_ntsc_timings;
-#endif
 
 struct omap_dss_cpr_coefs {
         s16 rr, rg, rb;
@@ -866,8 +865,6 @@ void omap_video_timings_to_videomode(const struct omap_video_timings *ovt,
 
 int dss_feat_get_num_mgrs(void);
 int dss_feat_get_num_ovls(void);
-enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
-enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane);
 
 
diff --git a/include/video/sh_mobile_hdmi.h b/include/video/sh_mobile_hdmi.h
deleted file mode 100644
index 63d20efa254a..000000000000
--- a/include/video/sh_mobile_hdmi.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * SH-Mobile High-Definition Multimedia Interface (HDMI)
- *
- * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef SH_MOBILE_HDMI_H
-#define SH_MOBILE_HDMI_H
-
-struct sh_mobile_lcdc_chan_cfg;
-struct device;
-struct clk;
-
-/*
- * flags format
- *
- * 0x00000CBA
- *
- * A: Audio source select
- * B: Int output option
- * C: Chip specific option
- */
-
-/* Audio source select */
-#define HDMI_SND_SRC_MASK       (0xF << 0)
-#define HDMI_SND_SRC_I2S        (0 << 0) /* default */
-#define HDMI_SND_SRC_SPDIF      (1 << 0)
-#define HDMI_SND_SRC_DSD        (2 << 0)
-#define HDMI_SND_SRC_HBR        (3 << 0)
-
-/* Int output option */
-#define HDMI_OUTPUT_PUSH_PULL   (1 << 4) /* System control : output mode */
-#define HDMI_OUTPUT_POLARITY_HI (1 << 5) /* System control : output polarity */
-
-/* Chip specific option */
-#define HDMI_32BIT_REG          (1 << 8)
-#define HDMI_HAS_HTOP1          (1 << 9)
-
-struct sh_mobile_hdmi_info {
-        unsigned int                     flags;
-        long (*clk_optimize_parent)(unsigned long target, unsigned long *best_freq,
-                                    unsigned long *parent_freq);
-};
-
-#endif