29 files changed, 1688 insertions, 1485 deletions

diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index bbeee4ba2488..a1e9406b5afa 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -1505,7 +1505,7 @@ config FB_SIS_315
 
 config FB_VIA
        tristate "VIA UniChrome (Pro) and Chrome9 display support"
-       depends on FB && PCI
+       depends on FB && PCI && X86
        select FB_CFB_FILLRECT
        select FB_CFB_COPYAREA
        select FB_CFB_IMAGEBLIT
diff --git a/drivers/video/console/Kconfig b/drivers/video/console/Kconfig
index 8e8f18d29d7a..5a35f22372b9 100644
--- a/drivers/video/console/Kconfig
+++ b/drivers/video/console/Kconfig
@@ -6,7 +6,7 @@ menu "Console display driver support"
 
 config VGA_CONSOLE
         bool "VGA text console" if EMBEDDED || !X86
-        depends on !ARCH_ACORN && !ARCH_EBSA110 && !4xx && !8xx && !SPARC && !M68K && !PARISC && !FRV && !ARCH_VERSATILE && !SUPERH && !BLACKFIN && !AVR32 && !MN10300
+        depends on !4xx && !8xx && !SPARC && !M68K && !PARISC && !FRV && !SUPERH && !BLACKFIN && !AVR32 && !MN10300 && (!ARM || ARCH_FOOTBRIDGE || ARCH_INTEGRATOR || ARCH_NETWINDER)
         default y
         help
           Saying Y here will allow you to use Linux in text mode through a
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index 43f0639b1c10..5c363d026f64 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -40,6 +40,12 @@
  */
 #define DEBUG_VAR 1
 
+#if defined(CONFIG_BACKLIGHT_CLASS_DEVICE) || \
+        (defined(CONFIG_BACKLIGHT_CLASS_DEVICE_MODULE) && \
+                defined(CONFIG_FB_IMX_MODULE))
+#define PWMR_BACKLIGHT_AVAILABLE
+#endif
+
 #define DRIVER_NAME "imx-fb"
 
 #define LCDC_SSA        0x00
@@ -175,7 +181,9 @@ struct imxfb_info {
 
         struct imx_fb_videomode *mode;
         int                     num_modes;
+#ifdef PWMR_BACKLIGHT_AVAILABLE
         struct backlight_device *bl;
+#endif
 
         void (*lcd_power)(int);
         void (*backlight_power)(int);
@@ -450,8 +458,7 @@ static int imxfb_set_par(struct fb_info *info)
         return 0;
 }
 
-
-
+#ifdef PWMR_BACKLIGHT_AVAILABLE
 static int imxfb_bl_get_brightness(struct backlight_device *bl)
 {
         struct imxfb_info *fbi = bl_get_data(bl);
@@ -516,6 +523,7 @@ static void imxfb_exit_backlight(struct imxfb_info *fbi)
         if (fbi->bl)
                 backlight_device_unregister(fbi->bl);
 }
+#endif
 
 static void imxfb_enable_controller(struct imxfb_info *fbi)
 {
@@ -647,6 +655,9 @@ static int imxfb_activate_var(struct fb_var_screeninfo *var, struct fb_info *inf
                         fbi->regs + LCDC_SIZE);
 
         writel(fbi->pcr, fbi->regs + LCDC_PCR);
+#ifndef PWMR_BACKLIGHT_AVAILABLE
+        writel(fbi->pwmr, fbi->regs + LCDC_PWMR);
+#endif
         writel(fbi->lscr1, fbi->regs + LCDC_LSCR1);
         writel(fbi->dmacr, fbi->regs + LCDC_DMACR);
 
@@ -847,7 +858,9 @@ static int __init imxfb_probe(struct platform_device *pdev)
 
         imxfb_enable_controller(fbi);
         fbi->pdev = pdev;
+#ifdef PWMR_BACKLIGHT_AVAILABLE
         imxfb_init_backlight(fbi);
+#endif
 
         return 0;
 
@@ -885,7 +898,9 @@ static int __devexit imxfb_remove(struct platform_device *pdev)
 
         imxfb_disable_controller(fbi);
 
+#ifdef PWMR_BACKLIGHT_AVAILABLE
         imxfb_exit_backlight(fbi);
+#endif
         unregister_framebuffer(info);
 
         pdata = pdev->dev.platform_data;
diff --git a/drivers/video/omap/lcd_apollon.c b/drivers/video/omap/lcd_apollon.c
index 2be94eb3bbf5..10459d8bd9a0 100644
--- a/drivers/video/omap/lcd_apollon.c
+++ b/drivers/video/omap/lcd_apollon.c
@@ -25,7 +25,6 @@
 #include <linux/platform_device.h>
 
 #include <mach/gpio.h>
-#include <plat/mux.h>
 
 #include "omapfb.h"
 
@@ -34,8 +33,6 @@
 static int apollon_panel_init(struct lcd_panel *panel,
                                 struct omapfb_device *fbdev)
 {
-        /* configure LCD PWR_EN */
-        omap_cfg_reg(M21_242X_GPIO11);
         return 0;
 }
 
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index aaf5d308a046..e1c765d11419 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -28,12 +28,13 @@
 #include <linux/fb.h>
 #include <linux/interrupt.h>
 #include <linux/gpio.h>
-#include <linux/completion.h>
 #include <linux/workqueue.h>
 #include <linux/slab.h>
+#include <linux/regulator/consumer.h>
 #include <linux/mutex.h>
 
 #include <plat/display.h>
+#include <plat/nokia-dsi-panel.h>
 
 /* DSI Virtual channel. Hardcoded for now. */
 #define TCH 0
@@ -62,11 +63,136 @@
 #define DCS_GET_ID2             0xdb
 #define DCS_GET_ID3             0xdc
 
-/* #define TAAL_USE_ESD_CHECK */
 #define TAAL_ESD_CHECK_PERIOD   msecs_to_jiffies(5000)
 
+static irqreturn_t taal_te_isr(int irq, void *data);
+static void taal_te_timeout_work_callback(struct work_struct *work);
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable);
 
+struct panel_regulator {
+        struct regulator *regulator;
+        const char *name;
+        int min_uV;
+        int max_uV;
+};
+
+static void free_regulators(struct panel_regulator *regulators, int n)
+{
+        int i;
+
+        for (i = 0; i < n; i++) {
+                /* disable/put in reverse order */
+                regulator_disable(regulators[n - i - 1].regulator);
+                regulator_put(regulators[n - i - 1].regulator);
+        }
+}
+
+static int init_regulators(struct omap_dss_device *dssdev,
+                        struct panel_regulator *regulators, int n)
+{
+        int r, i, v;
+
+        for (i = 0; i < n; i++) {
+                struct regulator *reg;
+
+                reg = regulator_get(&dssdev->dev, regulators[i].name);
+                if (IS_ERR(reg)) {
+                        dev_err(&dssdev->dev, "failed to get regulator %s\n",
+                                regulators[i].name);
+                        r = PTR_ERR(reg);
+                        goto err;
+                }
+
+                /* FIXME: better handling of fixed vs. variable regulators */
+                v = regulator_get_voltage(reg);
+                if (v < regulators[i].min_uV || v > regulators[i].max_uV) {
+                        r = regulator_set_voltage(reg, regulators[i].min_uV,
+                                                regulators[i].max_uV);
+                        if (r) {
+                                dev_err(&dssdev->dev,
+                                        "failed to set regulator %s voltage\n",
+                                        regulators[i].name);
+                                regulator_put(reg);
+                                goto err;
+                        }
+                }
+
+                r = regulator_enable(reg);
+                if (r) {
+                        dev_err(&dssdev->dev, "failed to enable regulator %s\n",
+                                regulators[i].name);
+                        regulator_put(reg);
+                        goto err;
+                }
+
+                regulators[i].regulator = reg;
+        }
+
+        return 0;
+
+err:
+        free_regulators(regulators, i);
+
+        return r;
+}
+
+/**
+ * struct panel_config - panel configuration
+ * @name: panel name
+ * @type: panel type
+ * @timings: panel resolution
+ * @sleep: various panel specific delays, passed to msleep() if non-zero
+ * @reset_sequence: reset sequence timings, passed to udelay() if non-zero
+ * @regulators: array of panel regulators
+ * @num_regulators: number of regulators in the array
+ */
+struct panel_config {
+        const char *name;
+        int type;
+
+        struct omap_video_timings timings;
+
+        struct {
+                unsigned int sleep_in;
+                unsigned int sleep_out;
+                unsigned int hw_reset;
+                unsigned int enable_te;
+        } sleep;
+
+        struct {
+                unsigned int high;
+                unsigned int low;
+        } reset_sequence;
+
+        struct panel_regulator *regulators;
+        int num_regulators;
+};
+
+enum {
+        PANEL_TAAL,
+};
+
+static struct panel_config panel_configs[] = {
+        {
+                .name           = "taal",
+                .type           = PANEL_TAAL,
+                .timings        = {
+                        .x_res          = 864,
+                        .y_res          = 480,
+                },
+                .sleep          = {
+                        .sleep_in       = 5,
+                        .sleep_out      = 5,
+                        .hw_reset       = 5,
+                        .enable_te      = 100, /* possible panel bug */
+                },
+                .reset_sequence = {
+                        .high           = 10,
+                        .low            = 10,
+                },
+        },
+};
+
 struct taal_data {
         struct mutex lock;
 
@@ -84,8 +210,15 @@ struct taal_data {
         bool mirror;
 
         bool te_enabled;
-        bool use_ext_te;
-        struct completion te_completion;
+
+        atomic_t do_update;
+        struct {
+                u16 x;
+                u16 y;
+                u16 w;
+                u16 h;
+        } update_region;
+        struct delayed_work te_timeout_work;
 
         bool use_dsi_bl;
 
@@ -96,8 +229,16 @@ struct taal_data {
 
         struct workqueue_struct *esd_wq;
         struct delayed_work esd_work;
+
+        struct panel_config *panel_config;
 };
 
+static inline struct nokia_dsi_panel_data
+*get_panel_data(const struct omap_dss_device *dssdev)
+{
+        return (struct nokia_dsi_panel_data *) dssdev->data;
+}
+
 static void taal_esd_work(struct work_struct *work);
 
 static void hw_guard_start(struct taal_data *td, int guard_msec)
@@ -159,7 +300,8 @@ static int taal_sleep_in(struct taal_data *td)
 
         hw_guard_start(td, 120);
 
-        msleep(5);
+        if (td->panel_config->sleep.sleep_in)
+                msleep(td->panel_config->sleep.sleep_in);
 
         return 0;
 }
@@ -176,7 +318,8 @@ static int taal_sleep_out(struct taal_data *td)
 
         hw_guard_start(td, 120);
 
-        msleep(5);
+        if (td->panel_config->sleep.sleep_out)
+                msleep(td->panel_config->sleep.sleep_out);
 
         return 0;
 }
@@ -279,6 +422,7 @@ static int taal_bl_update_status(struct backlight_device *dev)
 {
         struct omap_dss_device *dssdev = dev_get_drvdata(&dev->dev);
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
         int level;
 
@@ -290,24 +434,26 @@ static int taal_bl_update_status(struct backlight_device *dev)
 
         dev_dbg(&dssdev->dev, "update brightness to %d\n", level);
 
+        mutex_lock(&td->lock);
+
         if (td->use_dsi_bl) {
                 if (td->enabled) {
                         dsi_bus_lock();
                         r = taal_dcs_write_1(DCS_BRIGHTNESS, level);
                         dsi_bus_unlock();
-                        if (r)
-                                return r;
+                } else {
+                        r = 0;
                 }
         } else {
-                if (!dssdev->set_backlight)
-                        return -EINVAL;
-
-                r = dssdev->set_backlight(dssdev, level);
-                if (r)
-                        return r;
+                if (!panel_data->set_backlight)
+                        r = -EINVAL;
+                else
+                        r = panel_data->set_backlight(dssdev, level);
         }
 
-        return 0;
+        mutex_unlock(&td->lock);
+
+        return r;
 }
 
 static int taal_bl_get_intensity(struct backlight_device *dev)
@@ -344,16 +490,6 @@ static void taal_get_resolution(struct omap_dss_device *dssdev,
         }
 }
 
-static irqreturn_t taal_te_isr(int irq, void *data)
-{
-        struct omap_dss_device *dssdev = data;
-        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-
-        complete_all(&td->te_completion);
-
-        return IRQ_HANDLED;
-}
-
 static ssize_t taal_num_errors_show(struct device *dev,
                 struct device_attribute *attr, char *buf)
 {
@@ -362,6 +498,8 @@ static ssize_t taal_num_errors_show(struct device *dev,
         u8 errors;
         int r;
 
+        mutex_lock(&td->lock);
+
         if (td->enabled) {
                 dsi_bus_lock();
                 r = taal_dcs_read_1(DCS_READ_NUM_ERRORS, &errors);
@@ -370,6 +508,8 @@ static ssize_t taal_num_errors_show(struct device *dev,
                 r = -ENODEV;
         }
 
+        mutex_unlock(&td->lock);
+
         if (r)
                 return r;
 
@@ -384,6 +524,8 @@ static ssize_t taal_hw_revision_show(struct device *dev,
         u8 id1, id2, id3;
         int r;
 
+        mutex_lock(&td->lock);
+
         if (td->enabled) {
                 dsi_bus_lock();
                 r = taal_get_id(&id1, &id2, &id3);
@@ -392,6 +534,8 @@ static ssize_t taal_hw_revision_show(struct device *dev,
                 r = -ENODEV;
         }
 
+        mutex_unlock(&td->lock);
+
         if (r)
                 return r;
 
@@ -441,6 +585,8 @@ static ssize_t store_cabc_mode(struct device *dev,
         if (i == ARRAY_SIZE(cabc_modes))
                 return -EINVAL;
 
+        mutex_lock(&td->lock);
+
         if (td->enabled) {
                 dsi_bus_lock();
                 if (!td->cabc_broken)
@@ -450,6 +596,8 @@ static ssize_t store_cabc_mode(struct device *dev,
 
         td->cabc_mode = i;
 
+        mutex_unlock(&td->lock);
+
         return count;
 }
 
@@ -488,47 +636,93 @@ static struct attribute_group taal_attr_group = {
         .attrs = taal_attrs,
 };
 
+static void taal_hw_reset(struct omap_dss_device *dssdev)
+{
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+
+        if (panel_data->reset_gpio == -1)
+                return;
+
+        gpio_set_value(panel_data->reset_gpio, 1);
+        if (td->panel_config->reset_sequence.high)
+                udelay(td->panel_config->reset_sequence.high);
+        /* reset the panel */
+        gpio_set_value(panel_data->reset_gpio, 0);
+        /* assert reset */
+        if (td->panel_config->reset_sequence.low)
+                udelay(td->panel_config->reset_sequence.low);
+        gpio_set_value(panel_data->reset_gpio, 1);
+        /* wait after releasing reset */
+        if (td->panel_config->sleep.hw_reset)
+                msleep(td->panel_config->sleep.hw_reset);
+}
+
 static int taal_probe(struct omap_dss_device *dssdev)
 {
         struct backlight_properties props;
         struct taal_data *td;
         struct backlight_device *bldev;
-        int r;
-
-        const struct omap_video_timings taal_panel_timings = {
-                .x_res          = 864,
-                .y_res          = 480,
-        };
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
+        struct panel_config *panel_config = NULL;
+        int r, i;
 
         dev_dbg(&dssdev->dev, "probe\n");
 
+        if (!panel_data || !panel_data->name) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(panel_configs); i++) {
+                if (strcmp(panel_data->name, panel_configs[i].name) == 0) {
+                        panel_config = &panel_configs[i];
+                        break;
+                }
+        }
+
+        if (!panel_config) {
+                r = -EINVAL;
+                goto err;
+        }
+
         dssdev->panel.config = OMAP_DSS_LCD_TFT;
-        dssdev->panel.timings = taal_panel_timings;
+        dssdev->panel.timings = panel_config->timings;
         dssdev->ctrl.pixel_size = 24;
 
         td = kzalloc(sizeof(*td), GFP_KERNEL);
         if (!td) {
                 r = -ENOMEM;
-                goto err0;
+                goto err;
         }
         td->dssdev = dssdev;
+        td->panel_config = panel_config;
 
         mutex_init(&td->lock);
 
+        atomic_set(&td->do_update, 0);
+
+        r = init_regulators(dssdev, panel_config->regulators,
+                        panel_config->num_regulators);
+        if (r)
+                goto err_reg;
+
         td->esd_wq = create_singlethread_workqueue("taal_esd");
         if (td->esd_wq == NULL) {
                 dev_err(&dssdev->dev, "can't create ESD workqueue\n");
                 r = -ENOMEM;
-                goto err1;
+                goto err_wq;
         }
         INIT_DELAYED_WORK_DEFERRABLE(&td->esd_work, taal_esd_work);
 
         dev_set_drvdata(&dssdev->dev, td);
 
+        taal_hw_reset(dssdev);
+
         /* if no platform set_backlight() defined, presume DSI backlight
          * control */
         memset(&props, 0, sizeof(struct backlight_properties));
-        if (!dssdev->set_backlight)
+        if (!panel_data->set_backlight)
                 td->use_dsi_bl = true;
 
         if (td->use_dsi_bl)
@@ -539,7 +733,7 @@ static int taal_probe(struct omap_dss_device *dssdev)
                                           &taal_bl_ops, &props);
         if (IS_ERR(bldev)) {
                 r = PTR_ERR(bldev);
-                goto err2;
+                goto err_bl;
         }
 
         td->bldev = bldev;
@@ -553,13 +747,13 @@ static int taal_probe(struct omap_dss_device *dssdev)
 
         taal_bl_update_status(bldev);
 
-        if (dssdev->phy.dsi.ext_te) {
-                int gpio = dssdev->phy.dsi.ext_te_gpio;
+        if (panel_data->use_ext_te) {
+                int gpio = panel_data->ext_te_gpio;
 
                 r = gpio_request(gpio, "taal irq");
                 if (r) {
                         dev_err(&dssdev->dev, "GPIO request failed\n");
-                        goto err3;
+                        goto err_gpio;
                 }
 
                 gpio_direction_input(gpio);
@@ -571,49 +765,52 @@ static int taal_probe(struct omap_dss_device *dssdev)
                 if (r) {
                         dev_err(&dssdev->dev, "IRQ request failed\n");
                         gpio_free(gpio);
-                        goto err3;
+                        goto err_irq;
                 }
 
-                init_completion(&td->te_completion);
+                INIT_DELAYED_WORK_DEFERRABLE(&td->te_timeout_work,
+                                        taal_te_timeout_work_callback);
 
-                td->use_ext_te = true;
+                dev_dbg(&dssdev->dev, "Using GPIO TE\n");
         }
 
         r = sysfs_create_group(&dssdev->dev.kobj, &taal_attr_group);
         if (r) {
                 dev_err(&dssdev->dev, "failed to create sysfs files\n");
-                goto err4;
+                goto err_sysfs;
         }
 
         return 0;
-err4:
-        if (td->use_ext_te) {
-                int gpio = dssdev->phy.dsi.ext_te_gpio;
-                free_irq(gpio_to_irq(gpio), dssdev);
-                gpio_free(gpio);
-        }
-err3:
+err_sysfs:
+        if (panel_data->use_ext_te)
+                free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
+err_irq:
+        if (panel_data->use_ext_te)
+                gpio_free(panel_data->ext_te_gpio);
+err_gpio:
         backlight_device_unregister(bldev);
-err2:
-        cancel_delayed_work_sync(&td->esd_work);
+err_bl:
         destroy_workqueue(td->esd_wq);
-err1:
+err_wq:
+        free_regulators(panel_config->regulators, panel_config->num_regulators);
+err_reg:
         kfree(td);
-err0:
+err:
         return r;
 }
 
 static void taal_remove(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         struct backlight_device *bldev;
 
         dev_dbg(&dssdev->dev, "remove\n");
 
         sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
 
-        if (td->use_ext_te) {
-                int gpio = dssdev->phy.dsi.ext_te_gpio;
+        if (panel_data->use_ext_te) {
+                int gpio = panel_data->ext_te_gpio;
                 free_irq(gpio_to_irq(gpio), dssdev);
                 gpio_free(gpio);
         }
@@ -623,9 +820,15 @@ static void taal_remove(struct omap_dss_device *dssdev)
         taal_bl_update_status(bldev);
         backlight_device_unregister(bldev);
 
-        cancel_delayed_work_sync(&td->esd_work);
+        cancel_delayed_work(&td->esd_work);
         destroy_workqueue(td->esd_wq);
 
+        /* reset, to be sure that the panel is in a valid state */
+        taal_hw_reset(dssdev);
+
+        free_regulators(td->panel_config->regulators,
+                        td->panel_config->num_regulators);
+
         kfree(td);
 }
 
@@ -635,23 +838,14 @@ static int taal_power_on(struct omap_dss_device *dssdev)
         u8 id1, id2, id3;
         int r;
 
-        if (dssdev->platform_enable) {
-                r = dssdev->platform_enable(dssdev);
-                if (r)
-                        return r;
-        }
-
-        /* it seems we have to wait a bit until taal is ready */
-        msleep(5);
-
-        dsi_bus_lock();
-
         r = omapdss_dsi_display_enable(dssdev);
         if (r) {
                 dev_err(&dssdev->dev, "failed to enable DSI\n");
                 goto err0;
         }
 
+        taal_hw_reset(dssdev);
+
         omapdss_dsi_vc_enable_hs(TCH, false);
 
         r = taal_sleep_out(td);
@@ -662,34 +856,47 @@ static int taal_power_on(struct omap_dss_device *dssdev)
         if (r)
                 goto err;
 
-        /* on early revisions CABC is broken */
-        if (id2 == 0x00 || id2 == 0xff || id2 == 0x81)
+        /* on early Taal revisions CABC is broken */
+        if (td->panel_config->type == PANEL_TAAL &&
+                (id2 == 0x00 || id2 == 0xff || id2 == 0x81))
                 td->cabc_broken = true;
 
-        taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
-        taal_dcs_write_1(DCS_CTRL_DISPLAY, (1<<2) | (1<<5)); /* BL | BCTRL */
+        r = taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
+        if (r)
+                goto err;
+
+        r = taal_dcs_write_1(DCS_CTRL_DISPLAY,
+                        (1<<2) | (1<<5));       /* BL | BCTRL */
+        if (r)
+                goto err;
+
+        r = taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+        if (r)
+                goto err;
 
-        taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+        r = taal_set_addr_mode(td->rotate, td->mirror);
+        if (r)
+                goto err;
 
-        taal_set_addr_mode(td->rotate, td->mirror);
-        if (!td->cabc_broken)
-                taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+        if (!td->cabc_broken) {
+                r = taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+                if (r)
+                        goto err;
+        }
 
-        taal_dcs_write_0(DCS_DISPLAY_ON);
+        r = taal_dcs_write_0(DCS_DISPLAY_ON);
+        if (r)
+                goto err;
 
         r = _taal_enable_te(dssdev, td->te_enabled);
         if (r)
                 goto err;
 
-#ifdef TAAL_USE_ESD_CHECK
-        queue_delayed_work(td->esd_wq, &td->esd_work, TAAL_ESD_CHECK_PERIOD);
-#endif
-
         td->enabled = 1;
 
         if (!td->intro_printed) {
-                dev_info(&dssdev->dev, "revision %02x.%02x.%02x\n",
-                                id1, id2, id3);
+                dev_info(&dssdev->dev, "%s panel revision %02x.%02x.%02x\n",
+                        td->panel_config->name, id1, id2, id3);
                 if (td->cabc_broken)
                         dev_info(&dssdev->dev,
                                         "old Taal version, CABC disabled\n");
@@ -698,46 +905,44 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 
         omapdss_dsi_vc_enable_hs(TCH, true);
 
-        dsi_bus_unlock();
-
         return 0;
 err:
+        dev_err(&dssdev->dev, "error while enabling panel, issuing HW reset\n");
+
+        taal_hw_reset(dssdev);
+
         omapdss_dsi_display_disable(dssdev);
 err0:
-        dsi_bus_unlock();
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
         return r;
 }
 
 static void taal_power_off(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        int r;
 
-        dsi_bus_lock();
-
-        cancel_delayed_work(&td->esd_work);
-
-        taal_dcs_write_0(DCS_DISPLAY_OFF);
-        taal_sleep_in(td);
+        r = taal_dcs_write_0(DCS_DISPLAY_OFF);
+        if (!r) {
+                r = taal_sleep_in(td);
+                /* HACK: wait a bit so that the message goes through */
+                msleep(10);
+        }
 
-        /* wait a bit so that the message goes through */
-        msleep(10);
+        if (r) {
+                dev_err(&dssdev->dev,
+                                "error disabling panel, issuing HW reset\n");
+                taal_hw_reset(dssdev);
+        }
 
         omapdss_dsi_display_disable(dssdev);
 
-        if (dssdev->platform_disable)
-                dssdev->platform_disable(dssdev);
-
         td->enabled = 0;
-
-        dsi_bus_unlock();
 }
 
 static int taal_enable(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         dev_dbg(&dssdev->dev, "enable\n");
@@ -749,10 +954,19 @@ static int taal_enable(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        dsi_bus_lock();
+
         r = taal_power_on(dssdev);
+
+        dsi_bus_unlock();
+
         if (r)
                 goto err;
 
+        if (panel_data->use_esd_check)
+                queue_delayed_work(td->esd_wq, &td->esd_work,
+                                TAAL_ESD_CHECK_PERIOD);
+
         dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
         mutex_unlock(&td->lock);
@@ -772,9 +986,15 @@ static void taal_disable(struct omap_dss_device *dssdev)
 
         mutex_lock(&td->lock);
 
+        cancel_delayed_work(&td->esd_work);
+
+        dsi_bus_lock();
+
         if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
                 taal_power_off(dssdev);
 
+        dsi_bus_unlock();
+
         dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
 
         mutex_unlock(&td->lock);
@@ -794,7 +1014,14 @@ static int taal_suspend(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        cancel_delayed_work(&td->esd_work);
+
+        dsi_bus_lock();
+
         taal_power_off(dssdev);
+
+        dsi_bus_unlock();
+
         dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
         mutex_unlock(&td->lock);
@@ -808,6 +1035,7 @@ err:
 static int taal_resume(struct omap_dss_device *dssdev)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         dev_dbg(&dssdev->dev, "resume\n");
@@ -819,8 +1047,20 @@ static int taal_resume(struct omap_dss_device *dssdev)
                 goto err;
         }
 
+        dsi_bus_lock();
+
         r = taal_power_on(dssdev);
-        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        dsi_bus_unlock();
+
+        if (r) {
+                dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+        } else {
+                dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+                if (panel_data->use_esd_check)
+                        queue_delayed_work(td->esd_wq, &td->esd_work,
+                                        TAAL_ESD_CHECK_PERIOD);
+        }
 
         mutex_unlock(&td->lock);
 
@@ -837,10 +1077,52 @@ static void taal_framedone_cb(int err, void *data)
         dsi_bus_unlock();
 }
 
+static irqreturn_t taal_te_isr(int irq, void *data)
+{
+        struct omap_dss_device *dssdev = data;
+        struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        int old;
+        int r;
+
+        old = atomic_cmpxchg(&td->do_update, 1, 0);
+
+        if (old) {
+                cancel_delayed_work(&td->te_timeout_work);
+
+                r = omap_dsi_update(dssdev, TCH,
+                                td->update_region.x,
+                                td->update_region.y,
+                                td->update_region.w,
+                                td->update_region.h,
+                                taal_framedone_cb, dssdev);
+                if (r)
+                        goto err;
+        }
+
+        return IRQ_HANDLED;
+err:
+        dev_err(&dssdev->dev, "start update failed\n");
+        dsi_bus_unlock();
+        return IRQ_HANDLED;
+}
+
+static void taal_te_timeout_work_callback(struct work_struct *work)
+{
+        struct taal_data *td = container_of(work, struct taal_data,
+                                        te_timeout_work.work);
+        struct omap_dss_device *dssdev = td->dssdev;
+
+        dev_err(&dssdev->dev, "TE not received for 250ms!\n");
+
+        atomic_set(&td->do_update, 0);
+        dsi_bus_unlock();
+}
+
 static int taal_update(struct omap_dss_device *dssdev,
                                     u16 x, u16 y, u16 w, u16 h)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
         dev_dbg(&dssdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
@@ -853,7 +1135,7 @@ static int taal_update(struct omap_dss_device *dssdev,
                 goto err;
         }
 
-        r = omap_dsi_prepare_update(dssdev, &x, &y, &w, &h);
+        r = omap_dsi_prepare_update(dssdev, &x, &y, &w, &h, true);
         if (r)
                 goto err;
 
@@ -861,10 +1143,21 @@ static int taal_update(struct omap_dss_device *dssdev,
         if (r)
                 goto err;
 
-        r = omap_dsi_update(dssdev, TCH, x, y, w, h,
-                        taal_framedone_cb, dssdev);
-        if (r)
-                goto err;
+        if (td->te_enabled && panel_data->use_ext_te) {
+                td->update_region.x = x;
+                td->update_region.y = y;
+                td->update_region.w = w;
+                td->update_region.h = h;
+                barrier();
+                schedule_delayed_work(&td->te_timeout_work,
+                                msecs_to_jiffies(250));
+                atomic_set(&td->do_update, 1);
+        } else {
+                r = omap_dsi_update(dssdev, TCH, x, y, w, h,
+                                taal_framedone_cb, dssdev);
+                if (r)
+                        goto err;
+        }
 
         /* note: no bus_unlock here. unlock is in framedone_cb */
         mutex_unlock(&td->lock);
@@ -894,20 +1187,19 @@ static int taal_sync(struct omap_dss_device *dssdev)
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 {
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         int r;
 
-        td->te_enabled = enable;
-
         if (enable)
                 r = taal_dcs_write_1(DCS_TEAR_ON, 0);
         else
                 r = taal_dcs_write_0(DCS_TEAR_OFF);
 
-        omapdss_dsi_enable_te(dssdev, enable);
+        if (!panel_data->use_ext_te)
+                omapdss_dsi_enable_te(dssdev, enable);
 
-        /* XXX for some reason, DSI TE breaks if we don't wait here.
-         * Panel bug? Needs more studying */
-        msleep(100);
+        if (td->panel_config->sleep.enable_te)
+                msleep(td->panel_config->sleep.enable_te);
 
         return r;
 }
@@ -918,10 +1210,26 @@ static int taal_enable_te(struct omap_dss_device *dssdev, bool enable)
         int r;
 
         mutex_lock(&td->lock);
+
+        if (td->te_enabled == enable)
+                goto end;
+
         dsi_bus_lock();
 
-        r = _taal_enable_te(dssdev, enable);
+        if (td->enabled) {
+                r = _taal_enable_te(dssdev, enable);
+                if (r)
+                        goto err;
+        }
+
+        td->te_enabled = enable;
+
+        dsi_bus_unlock();
+end:
+        mutex_unlock(&td->lock);
 
+        return 0;
+err:
         dsi_bus_unlock();
         mutex_unlock(&td->lock);
 
@@ -948,6 +1256,10 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
         dev_dbg(&dssdev->dev, "rotate %d\n", rotate);
 
         mutex_lock(&td->lock);
+
+        if (td->rotate == rotate)
+                goto end;
+
         dsi_bus_lock();
 
         if (td->enabled) {
@@ -959,6 +1271,7 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
         td->rotate = rotate;
 
         dsi_bus_unlock();
+end:
         mutex_unlock(&td->lock);
         return 0;
 err:
@@ -987,6 +1300,10 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
         dev_dbg(&dssdev->dev, "mirror %d\n", enable);
 
         mutex_lock(&td->lock);
+
+        if (td->mirror == enable)
+                goto end;
+
         dsi_bus_lock();
         if (td->enabled) {
                 r = taal_set_addr_mode(td->rotate, enable);
@@ -997,6 +1314,7 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
         td->mirror = enable;
 
         dsi_bus_unlock();
+end:
         mutex_unlock(&td->lock);
         return 0;
 err:
@@ -1024,23 +1342,30 @@ static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
         int r;
 
         mutex_lock(&td->lock);
+
+        if (!td->enabled) {
+                r = -ENODEV;
+                goto err1;
+        }
+
         dsi_bus_lock();
 
         r = taal_dcs_read_1(DCS_GET_ID1, &id1);
         if (r)
-                goto err;
+                goto err2;
         r = taal_dcs_read_1(DCS_GET_ID2, &id2);
         if (r)
-                goto err;
+                goto err2;
         r = taal_dcs_read_1(DCS_GET_ID3, &id3);
         if (r)
-                goto err;
+                goto err2;
 
         dsi_bus_unlock();
         mutex_unlock(&td->lock);
         return 0;
-err:
+err2:
         dsi_bus_unlock();
+err1:
         mutex_unlock(&td->lock);
         return r;
 }
@@ -1128,6 +1453,7 @@ static void taal_esd_work(struct work_struct *work)
         struct taal_data *td = container_of(work, struct taal_data,
                         esd_work.work);
         struct omap_dss_device *dssdev = td->dssdev;
+        struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
         u8 state1, state2;
         int r;
 
@@ -1168,7 +1494,7 @@ static void taal_esd_work(struct work_struct *work)
         }
         /* Self-diagnostics result is also shown on TE GPIO line. We need
          * to re-enable TE after self diagnostics */
-        if (td->use_ext_te && td->te_enabled) {
+        if (td->te_enabled && panel_data->use_ext_te) {
                 r = taal_dcs_write_1(DCS_TEAR_ON, 0);
                 if (r)
                         goto err;
@@ -1184,6 +1510,7 @@ err:
         dev_err(&dssdev->dev, "performing LCD reset\n");
 
         taal_power_off(dssdev);
+        taal_hw_reset(dssdev);
         taal_power_on(dssdev);
 
         dsi_bus_unlock();
diff --git a/drivers/video/omap2/displays/panel-toppoly-tdo35s.c b/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
index fa434ca6e4b7..e320e67d06f3 100644
--- a/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
+++ b/drivers/video/omap2/displays/panel-toppoly-tdo35s.c
@@ -73,8 +73,12 @@ static void toppoly_tdo_panel_power_off(struct omap_dss_device *dssdev)
 
 static int toppoly_tdo_panel_probe(struct omap_dss_device *dssdev)
 {
-        dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-                OMAP_DSS_LCD_IHS;
+        dssdev->panel.config = OMAP_DSS_LCD_TFT |
+                               OMAP_DSS_LCD_IVS |
+                               OMAP_DSS_LCD_IHS |
+                               OMAP_DSS_LCD_IPC |
+                               OMAP_DSS_LCD_ONOFF;
+
         dssdev->panel.timings = toppoly_tdo_panel_timings;
 
         return 0;
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index e777e352dbcd..5ecdc0004094 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -31,6 +31,7 @@
 #include <linux/seq_file.h>
 #include <linux/delay.h>
 #include <linux/workqueue.h>
+#include <linux/hardirq.h>
 
 #include <plat/sram.h>
 #include <plat/clock.h>
@@ -335,7 +336,7 @@ void dispc_save_context(void)
 void dispc_restore_context(void)
 {
         RR(SYSCONFIG);
-        RR(IRQENABLE);
+        /*RR(IRQENABLE);*/
         /*RR(CONTROL);*/
         RR(CONFIG);
         RR(DEFAULT_COLOR0);
@@ -472,6 +473,15 @@ void dispc_restore_context(void)
 
         /* enable last, because LCD & DIGIT enable are here */
         RR(CONTROL);
+
+        /* clear spurious SYNC_LOST_DIGIT interrupts */
+        dispc_write_reg(DISPC_IRQSTATUS, DISPC_IRQ_SYNC_LOST_DIGIT);
+
+        /*
+         * enable last so IRQs won't trigger before
+         * the context is fully restored
+         */
+        RR(IRQENABLE);
 }
 
 #undef SR
@@ -3019,7 +3029,7 @@ void dispc_fake_vsync_irq(void)
         u32 irqstatus = DISPC_IRQ_VSYNC;
         int i;
 
-        local_irq_disable();
+        WARN_ON(!in_interrupt());
 
         for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
                 struct omap_dispc_isr_data *isr_data;
@@ -3031,8 +3041,6 @@ void dispc_fake_vsync_irq(void)
                 if (isr_data->mask & irqstatus)
                         isr_data->isr(isr_data->arg, irqstatus);
         }
-
-        local_irq_enable();
 }
 #endif
 
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index ef8c8529dda2..22dd7a474f79 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -82,6 +82,9 @@ static ssize_t display_upd_mode_store(struct device *dev,
         int val, r;
         enum omap_dss_update_mode mode;
 
+        if (!dssdev->driver->set_update_mode)
+                return -EINVAL;
+
         val = simple_strtoul(buf, NULL, 10);
 
         switch (val) {
@@ -343,7 +346,6 @@ int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
         case OMAP_DISPLAY_TYPE_VENC:
         case OMAP_DISPLAY_TYPE_SDI:
                 return 24;
-                return 24;
         default:
                 BUG();
         }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 3af207b2bde3..b3fa3a7db911 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -165,6 +165,14 @@ struct dsi_reg { u16 idx; };
 #define DSI_CIO_IRQ_ERRCONTENTIONLP1_3  (1 << 25)
 #define DSI_CIO_IRQ_ULPSACTIVENOT_ALL0  (1 << 30)
 #define DSI_CIO_IRQ_ULPSACTIVENOT_ALL1  (1 << 31)
+#define DSI_CIO_IRQ_ERROR_MASK \
+        (DSI_CIO_IRQ_ERRSYNCESC1 | DSI_CIO_IRQ_ERRSYNCESC2 | \
+         DSI_CIO_IRQ_ERRSYNCESC3 | DSI_CIO_IRQ_ERRESC1 | DSI_CIO_IRQ_ERRESC2 | \
+         DSI_CIO_IRQ_ERRESC3 | DSI_CIO_IRQ_ERRCONTROL1 | \
+         DSI_CIO_IRQ_ERRCONTROL2 | DSI_CIO_IRQ_ERRCONTROL3 | \
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_1 | DSI_CIO_IRQ_ERRCONTENTIONLP1_1 | \
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_2 | DSI_CIO_IRQ_ERRCONTENTIONLP1_2 | \
+         DSI_CIO_IRQ_ERRCONTENTIONLP0_3 | DSI_CIO_IRQ_ERRCONTENTIONLP1_3)
 
 #define DSI_DT_DCS_SHORT_WRITE_0        0x05
 #define DSI_DT_DCS_SHORT_WRITE_1        0x15
@@ -232,13 +240,15 @@ static struct
         unsigned pll_locked;
 
         struct completion bta_completion;
+        void (*bta_callback)(void);
 
         int update_channel;
         struct dsi_update_region update_region;
 
         bool te_enabled;
 
-        struct work_struct framedone_work;
+        struct workqueue_struct *workqueue;
+
         void (*framedone_callback)(int, void *);
         void *framedone_data;
 
@@ -509,9 +519,13 @@ void dsi_irq_handler(void)
                 dss_collect_irq_stats(vcstatus, dsi.irq_stats.vc_irqs[i]);
 #endif
 
-                if (vcstatus & DSI_VC_IRQ_BTA)
+                if (vcstatus & DSI_VC_IRQ_BTA) {
                         complete(&dsi.bta_completion);
 
+                        if (dsi.bta_callback)
+                                dsi.bta_callback();
+                }
+
                 if (vcstatus & DSI_VC_IRQ_ERROR_MASK) {
                         DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
                                        i, vcstatus);
@@ -536,8 +550,12 @@ void dsi_irq_handler(void)
                 /* flush posted write */
                 dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
 
-                DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
-                print_irq_status_cio(ciostatus);
+                if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
+                        DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
+                        print_irq_status_cio(ciostatus);
+                } else if (debug_irq) {
+                        print_irq_status_cio(ciostatus);
+                }
         }
 
         dsi_write_reg(DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
@@ -584,11 +602,8 @@ static void _dsi_initialize_irq(void)
         for (i = 0; i < 4; ++i)
                 dsi_write_reg(DSI_VC_IRQENABLE(i), l);
 
-        /* XXX zonda responds incorrectly, causing control error:
-           Exit from LP-ESC mode to LP11 uses wrong transition states on the
-           data lines LP0 and LN0. */
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE,
-                        -1 & (~DSI_CIO_IRQ_ERRCONTROL2));
+        l = DSI_CIO_IRQ_ERROR_MASK;
+        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, l);
 }
 
 static u32 dsi_get_errors(void)
@@ -1098,6 +1113,7 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
         if (wait_for_bit_change(DSI_PLL_STATUS, 0, 1) != 1) {
                 DSSERR("PLL not coming out of reset.\n");
                 r = -ENODEV;
+                dispc_pck_free_enable(0);
                 goto err1;
         }
 
@@ -1740,42 +1756,52 @@ static void dsi_vc_initial_config(int channel)
         dsi.vc[channel].mode = DSI_VC_MODE_L4;
 }
 
-static void dsi_vc_config_l4(int channel)
+static int dsi_vc_config_l4(int channel)
 {
         if (dsi.vc[channel].mode == DSI_VC_MODE_L4)
-                return;
+                return 0;
 
         DSSDBGF("%d", channel);
 
         dsi_vc_enable(channel, 0);
 
-        if (REG_GET(DSI_VC_CTRL(channel), 15, 15)) /* VC_BUSY */
+        /* VC_BUSY */
+        if (wait_for_bit_change(DSI_VC_CTRL(channel), 15, 0) != 0) {
                 DSSERR("vc(%d) busy when trying to config for L4\n", channel);
+                return -EIO;
+        }
 
         REG_FLD_MOD(DSI_VC_CTRL(channel), 0, 1, 1); /* SOURCE, 0 = L4 */
 
         dsi_vc_enable(channel, 1);
 
         dsi.vc[channel].mode = DSI_VC_MODE_L4;
+
+        return 0;
 }
 
-static void dsi_vc_config_vp(int channel)
+static int dsi_vc_config_vp(int channel)
 {
         if (dsi.vc[channel].mode == DSI_VC_MODE_VP)
-                return;
+                return 0;
 
         DSSDBGF("%d", channel);
 
         dsi_vc_enable(channel, 0);
 
-        if (REG_GET(DSI_VC_CTRL(channel), 15, 15)) /* VC_BUSY */
+        /* VC_BUSY */
+        if (wait_for_bit_change(DSI_VC_CTRL(channel), 15, 0) != 0) {
                 DSSERR("vc(%d) busy when trying to config for VP\n", channel);
+                return -EIO;
+        }
 
         REG_FLD_MOD(DSI_VC_CTRL(channel), 1, 1, 1); /* SOURCE, 1 = video port */
 
         dsi_vc_enable(channel, 1);
 
         dsi.vc[channel].mode = DSI_VC_MODE_VP;
+
+        return 0;
 }
 
 
@@ -1854,19 +1880,19 @@ static u16 dsi_vc_flush_receive_data(int channel)
                 u32 val;
                 u8 dt;
                 val = dsi_read_reg(DSI_VC_SHORT_PACKET_HEADER(channel));
-                DSSDBG("\trawval %#08x\n", val);
+                DSSERR("\trawval %#08x\n", val);
                 dt = FLD_GET(val, 5, 0);
                 if (dt == DSI_DT_RX_ACK_WITH_ERR) {
                         u16 err = FLD_GET(val, 23, 8);
                         dsi_show_rx_ack_with_err(err);
                 } else if (dt == DSI_DT_RX_SHORT_READ_1) {
-                        DSSDBG("\tDCS short response, 1 byte: %#x\n",
+                        DSSERR("\tDCS short response, 1 byte: %#x\n",
                                         FLD_GET(val, 23, 8));
                 } else if (dt == DSI_DT_RX_SHORT_READ_2) {
-                        DSSDBG("\tDCS short response, 2 byte: %#x\n",
+                        DSSERR("\tDCS short response, 2 byte: %#x\n",
                                         FLD_GET(val, 23, 8));
                 } else if (dt == DSI_DT_RX_DCS_LONG_READ) {
-                        DSSDBG("\tDCS long response, len %d\n",
+                        DSSERR("\tDCS long response, len %d\n",
                                         FLD_GET(val, 23, 8));
                         dsi_vc_flush_long_data(channel);
                 } else {
@@ -2087,6 +2113,13 @@ int dsi_vc_dcs_write(int channel, u8 *data, int len)
         if (r)
                 goto err;
 
+        if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {    /* RX_FIFO_NOT_EMPTY */
+                DSSERR("rx fifo not empty after write, dumping data:\n");
+                dsi_vc_flush_receive_data(channel);
+                r = -EIO;
+                goto err;
+        }
+
         return 0;
 err:
         DSSERR("dsi_vc_dcs_write(ch %d, cmd 0x%02x, len %d) failed\n",
@@ -2233,11 +2266,12 @@ int dsi_vc_dcs_read_1(int channel, u8 dcs_cmd, u8 *data)
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_1);
 
-int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u16 *data)
+int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u8 *data1, u8 *data2)
 {
+        u8 buf[2];
         int r;
 
-        r = dsi_vc_dcs_read(channel, dcs_cmd, (u8 *)data, 2);
+        r = dsi_vc_dcs_read(channel, dcs_cmd, buf, 2);
 
         if (r < 0)
                 return r;
@@ -2245,231 +2279,122 @@ int dsi_vc_dcs_read_2(int channel, u8 dcs_cmd, u16 *data)
         if (r != 2)
                 return -EIO;
 
+        *data1 = buf[0];
+        *data2 = buf[1];
+
         return 0;
 }
 EXPORT_SYMBOL(dsi_vc_dcs_read_2);
 
 int dsi_vc_set_max_rx_packet_size(int channel, u16 len)
 {
-        int r;
-        r = dsi_vc_send_short(channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
+        return dsi_vc_send_short(channel, DSI_DT_SET_MAX_RET_PKG_SIZE,
                         len, 0);
-
-        if (r)
-                return r;
-
-        r = dsi_vc_send_bta_sync(channel);
-
-        return r;
 }
 EXPORT_SYMBOL(dsi_vc_set_max_rx_packet_size);
 
-static void dsi_set_lp_rx_timeout(unsigned long ns)
+static void dsi_set_lp_rx_timeout(unsigned ticks, bool x4, bool x16)
 {
-        u32 r;
-        unsigned x4, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
 
-        /* ticks in DSI_FCK */
+        BUG_ON(ticks > 0x1fff);
 
+        /* ticks in DSI_FCK */
         fck = dsi_fclk_rate();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x4 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 4;
-                x4 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x4 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (4 * 16);
-                x4 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("LP_TX_TO over limit, setting it to max\n");
-                ticks = 0x1fff;
-                x4 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING2);
         r = FLD_MOD(r, 1, 15, 15);      /* LP_RX_TO */
-        r = FLD_MOD(r, x16, 14, 14);    /* LP_RX_TO_X16 */
-        r = FLD_MOD(r, x4, 13, 13);     /* LP_RX_TO_X4 */
+        r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);    /* LP_RX_TO_X16 */
+        r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);     /* LP_RX_TO_X4 */
         r = FLD_MOD(r, ticks, 12, 0);   /* LP_RX_COUNTER */
         dsi_write_reg(DSI_TIMING2, r);
 
-        DSSDBG("LP_RX_TO %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x4 ? " x4" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("LP_RX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_ta_timeout(unsigned long ns)
+static void dsi_set_ta_timeout(unsigned ticks, bool x8, bool x16)
 {
-        u32 r;
-        unsigned x8, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
+
+        BUG_ON(ticks > 0x1fff);
 
         /* ticks in DSI_FCK */
         fck = dsi_fclk_rate();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x8 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 8;
-                x8 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x8 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (8 * 16);
-                x8 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("TA_TO over limit, setting it to max\n");
-                ticks = 0x1fff;
-                x8 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING1);
         r = FLD_MOD(r, 1, 31, 31);      /* TA_TO */
-        r = FLD_MOD(r, x16, 30, 30);    /* TA_TO_X16 */
-        r = FLD_MOD(r, x8, 29, 29);     /* TA_TO_X8 */
+        r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);    /* TA_TO_X16 */
+        r = FLD_MOD(r, x8 ? 1 : 0, 29, 29);     /* TA_TO_X8 */
         r = FLD_MOD(r, ticks, 28, 16);  /* TA_TO_COUNTER */
         dsi_write_reg(DSI_TIMING1, r);
 
-        DSSDBG("TA_TO %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x8 ? " x8" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1);
+
+        DSSDBG("TA_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x8 ? " x8" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_stop_state_counter(unsigned long ns)
+static void dsi_set_stop_state_counter(unsigned ticks, bool x4, bool x16)
 {
-        u32 r;
-        unsigned x4, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
 
-        /* ticks in DSI_FCK */
+        BUG_ON(ticks > 0x1fff);
 
+        /* ticks in DSI_FCK */
         fck = dsi_fclk_rate();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x4 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 4;
-                x4 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x4 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (4 * 16);
-                x4 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("STOP_STATE_COUNTER_IO over limit, "
-                                "setting it to max\n");
-                ticks = 0x1fff;
-                x4 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING1);
         r = FLD_MOD(r, 1, 15, 15);      /* FORCE_TX_STOP_MODE_IO */
-        r = FLD_MOD(r, x16, 14, 14);    /* STOP_STATE_X16_IO */
-        r = FLD_MOD(r, x4, 13, 13);     /* STOP_STATE_X4_IO */
+        r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);    /* STOP_STATE_X16_IO */
+        r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);     /* STOP_STATE_X4_IO */
         r = FLD_MOD(r, ticks, 12, 0);   /* STOP_STATE_COUNTER_IO */
         dsi_write_reg(DSI_TIMING1, r);
 
-        DSSDBG("STOP_STATE_COUNTER %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x4 ? " x4" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("STOP_STATE_COUNTER %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_set_hs_tx_timeout(unsigned long ns)
+static void dsi_set_hs_tx_timeout(unsigned ticks, bool x4, bool x16)
 {
-        u32 r;
-        unsigned x4, x16;
         unsigned long fck;
-        unsigned long ticks;
+        unsigned long total_ticks;
+        u32 r;
 
-        /* ticks in TxByteClkHS */
+        BUG_ON(ticks > 0x1fff);
 
+        /* ticks in TxByteClkHS */
         fck = dsi_get_txbyteclkhs();
-        ticks = (fck / 1000 / 1000) * ns / 1000;
-        x4 = 0;
-        x16 = 0;
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 4;
-                x4 = 1;
-                x16 = 0;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / 16;
-                x4 = 0;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                ticks = (fck / 1000 / 1000) * ns / 1000 / (4 * 16);
-                x4 = 1;
-                x16 = 1;
-        }
-
-        if (ticks > 0x1fff) {
-                DSSWARN("HS_TX_TO over limit, setting it to max\n");
-                ticks = 0x1fff;
-                x4 = 1;
-                x16 = 1;
-        }
 
         r = dsi_read_reg(DSI_TIMING2);
         r = FLD_MOD(r, 1, 31, 31);      /* HS_TX_TO */
-        r = FLD_MOD(r, x16, 30, 30);    /* HS_TX_TO_X16 */
-        r = FLD_MOD(r, x4, 29, 29);     /* HS_TX_TO_X8 (4 really) */
+        r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);    /* HS_TX_TO_X16 */
+        r = FLD_MOD(r, x4 ? 1 : 0, 29, 29);     /* HS_TX_TO_X8 (4 really) */
         r = FLD_MOD(r, ticks, 28, 16);  /* HS_TX_TO_COUNTER */
         dsi_write_reg(DSI_TIMING2, r);
 
-        DSSDBG("HS_TX_TO %lu ns (%#lx ticks%s%s)\n",
-                        (ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1) * 1000) /
-                        (fck / 1000 / 1000),
-                        ticks, x4 ? " x4" : "", x16 ? " x16" : "");
+        total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+        DSSDBG("HS_TX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+                        total_ticks,
+                        ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+                        (total_ticks * 1000) / (fck / 1000 / 1000));
 }
 static int dsi_proto_config(struct omap_dss_device *dssdev)
 {
@@ -2487,10 +2412,10 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
                         DSI_FIFO_SIZE_32);
 
         /* XXX what values for the timeouts? */
-        dsi_set_stop_state_counter(1000);
-        dsi_set_ta_timeout(6400000);
-        dsi_set_lp_rx_timeout(48000);
-        dsi_set_hs_tx_timeout(1000000);
+        dsi_set_stop_state_counter(0x1000, false, false);
+        dsi_set_ta_timeout(0x1fff, true, true);
+        dsi_set_lp_rx_timeout(0x1fff, true, true);
+        dsi_set_hs_tx_timeout(0x1fff, true, true);
 
         switch (dssdev->ctrl.pixel_size) {
         case 16:
@@ -2759,6 +2684,7 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
         unsigned packet_payload;
         unsigned packet_len;
         u32 l;
+        int r;
         const unsigned channel = dsi.update_channel;
         /* line buffer is 1024 x 24bits */
         /* XXX: for some reason using full buffer size causes considerable TX
@@ -2809,8 +2735,9 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 
         dsi_perf_mark_start();
 
-        schedule_delayed_work(&dsi.framedone_timeout_work,
+        r = queue_delayed_work(dsi.workqueue, &dsi.framedone_timeout_work,
                         msecs_to_jiffies(250));
+        BUG_ON(r == 0);
 
         dss_start_update(dssdev);
 
@@ -2834,62 +2761,70 @@ static void dsi_te_timeout(unsigned long arg)
 }
 #endif
 
-static void dsi_framedone_timeout_work_callback(struct work_struct *work)
+static void dsi_handle_framedone(int error)
 {
-        int r;
         const int channel = dsi.update_channel;
 
-        DSSERR("Framedone not received for 250ms!\n");
+        cancel_delayed_work(&dsi.framedone_timeout_work);
+
+        dsi_vc_disable_bta_irq(channel);
 
         /* SIDLEMODE back to smart-idle */
         dispc_enable_sidle();
 
+        dsi.bta_callback = NULL;
+
         if (dsi.te_enabled) {
                 /* enable LP_RX_TO again after the TE */
                 REG_FLD_MOD(DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
         }
 
-        /* Send BTA after the frame. We need this for the TE to work, as TE
-         * trigger is only sent for BTAs without preceding packet. Thus we need
-         * to BTA after the pixel packets so that next BTA will cause TE
-         * trigger.
-         *
-         * This is not needed when TE is not in use, but we do it anyway to
-         * make sure that the transfer has been completed. It would be more
-         * optimal, but more complex, to wait only just before starting next
-         * transfer. */
-        r = dsi_vc_send_bta_sync(channel);
-        if (r)
-                DSSERR("BTA after framedone failed\n");
-
         /* RX_FIFO_NOT_EMPTY */
         if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
                 DSSERR("Received error during frame transfer:\n");
                 dsi_vc_flush_receive_data(channel);
+                if (!error)
+                        error = -EIO;
         }
 
-        dsi.framedone_callback(-ETIMEDOUT, dsi.framedone_data);
+        dsi.framedone_callback(error, dsi.framedone_data);
+
+        if (!error)
+                dsi_perf_show("DISPC");
 }
 
-static void dsi_framedone_irq_callback(void *data, u32 mask)
+static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 {
-        /* Note: We get FRAMEDONE when DISPC has finished sending pixels and
-         * turns itself off. However, DSI still has the pixels in its buffers,
-         * and is sending the data.
-         */
+        /* XXX While extremely unlikely, we could get FRAMEDONE interrupt after
+         * 250ms which would conflict with this timeout work. What should be
+         * done is first cancel the transfer on the HW, and then cancel the
+         * possibly scheduled framedone work. However, cancelling the transfer
+         * on the HW is buggy, and would probably require resetting the whole
+         * DSI */
 
-        /* SIDLEMODE back to smart-idle */
-        dispc_enable_sidle();
+        DSSERR("Framedone not received for 250ms!\n");
 
-        schedule_work(&dsi.framedone_work);
+        dsi_handle_framedone(-ETIMEDOUT);
 }
 
-static void dsi_handle_framedone(void)
+static void dsi_framedone_bta_callback(void)
+{
+        dsi_handle_framedone(0);
+
+#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
+        dispc_fake_vsync_irq();
+#endif
+}
+
+static void dsi_framedone_irq_callback(void *data, u32 mask)
 {
-        int r;
         const int channel = dsi.update_channel;
+        int r;
 
-        DSSDBG("FRAMEDONE\n");
+        /* Note: We get FRAMEDONE when DISPC has finished sending pixels and
+         * turns itself off. However, DSI still has the pixels in its buffers,
+         * and is sending the data.
+         */
 
         if (dsi.te_enabled) {
                 /* enable LP_RX_TO again after the TE */
@@ -2904,37 +2839,30 @@ static void dsi_handle_framedone(void)
          * This is not needed when TE is not in use, but we do it anyway to
          * make sure that the transfer has been completed. It would be more
          * optimal, but more complex, to wait only just before starting next
-         * transfer. */
-        r = dsi_vc_send_bta_sync(channel);
-        if (r)
-                DSSERR("BTA after framedone failed\n");
-
-        /* RX_FIFO_NOT_EMPTY */
-        if (REG_GET(DSI_VC_CTRL(channel), 20, 20)) {
-                DSSERR("Received error during frame transfer:\n");
-                dsi_vc_flush_receive_data(channel);
-        }
-
-#ifdef CONFIG_OMAP2_DSS_FAKE_VSYNC
-        dispc_fake_vsync_irq();
-#endif
-}
-
-static void dsi_framedone_work_callback(struct work_struct *work)
-{
-        DSSDBGF();
+         * transfer.
+         *
+         * Also, as there's no interrupt telling when the transfer has been
+         * done and the channel could be reconfigured, the only way is to
+         * busyloop until TE_SIZE is zero. With BTA we can do this
+         * asynchronously.
+         * */
 
-        cancel_delayed_work_sync(&dsi.framedone_timeout_work);
+        dsi.bta_callback = dsi_framedone_bta_callback;
 
-        dsi_handle_framedone();
+        barrier();
 
-        dsi_perf_show("DISPC");
+        dsi_vc_enable_bta_irq(channel);
 
-        dsi.framedone_callback(0, dsi.framedone_data);
+        r = dsi_vc_send_bta(channel);
+        if (r) {
+                DSSERR("BTA after framedone failed\n");
+                dsi_handle_framedone(-EIO);
+        }
 }
 
 int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
-                                    u16 *x, u16 *y, u16 *w, u16 *h)
+                                    u16 *x, u16 *y, u16 *w, u16 *h,
+                                    bool enlarge_update_area)
 {
         u16 dw, dh;
 
@@ -2958,7 +2886,8 @@ int omap_dsi_prepare_update(struct omap_dss_device *dssdev,
         dsi_perf_mark_setup();
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-                dss_setup_partial_planes(dssdev, x, y, w, h);
+                dss_setup_partial_planes(dssdev, x, y, w, h,
+                                enlarge_update_area);
                 dispc_set_lcd_size(*w, *h);
         }
 
@@ -2973,6 +2902,12 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 {
         dsi.update_channel = channel;
 
+        /* OMAP DSS cannot send updates of odd widths.
+         * omap_dsi_prepare_update() makes the widths even, but add a BUG_ON
+         * here to make sure we catch erroneous updates. Otherwise we'll only
+         * see rather obscure HW error happening, as DSS halts. */
+        BUG_ON(x % 2 == 1);
+
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
                 dsi.framedone_callback = callback;
                 dsi.framedone_data = data;
@@ -2985,7 +2920,12 @@ int omap_dsi_update(struct omap_dss_device *dssdev,
 
                 dsi_update_screen_dispc(dssdev, x, y, w, h);
         } else {
-                dsi_update_screen_l4(dssdev, x, y, w, h);
+                int r;
+
+                r = dsi_update_screen_l4(dssdev, x, y, w, h);
+                if (r)
+                        return r;
+
                 dsi_perf_show("L4");
                 callback(0, data);
         }
@@ -3048,8 +2988,10 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
         cinfo.regm3 = dssdev->phy.dsi.div.regm3;
         cinfo.regm4 = dssdev->phy.dsi.div.regm4;
         r = dsi_calc_clock_rates(&cinfo);
-        if (r)
+        if (r) {
+                DSSERR("Failed to calc dsi clocks\n");
                 return r;
+        }
 
         r = dsi_pll_set_clock_div(&cinfo);
         if (r) {
@@ -3147,6 +3089,13 @@ err0:
 
 static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev)
 {
+        /* disable interface */
+        dsi_if_enable(0);
+        dsi_vc_enable(0, 0);
+        dsi_vc_enable(1, 0);
+        dsi_vc_enable(2, 0);
+        dsi_vc_enable(3, 0);
+
         dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
         dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
         dsi_complexio_uninit();
@@ -3257,7 +3206,7 @@ void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
         burst_size_bytes = 16 * 32 / 8;
 
         *fifo_high = fifo_size - burst_size_bytes;
-        *fifo_low = fifo_size - burst_size_bytes * 8;
+        *fifo_low = fifo_size - burst_size_bytes * 2;
 }
 
 int dsi_init_display(struct omap_dss_device *dssdev)
@@ -3274,6 +3223,18 @@ int dsi_init_display(struct omap_dss_device *dssdev)
         return 0;
 }
 
+void dsi_wait_dsi1_pll_active(void)
+{
+        if (wait_for_bit_change(DSI_PLL_STATUS, 7, 1) != 1)
+                DSSERR("DSI1 PLL clock not active\n");
+}
+
+void dsi_wait_dsi2_pll_active(void)
+{
+        if (wait_for_bit_change(DSI_PLL_STATUS, 8, 1) != 1)
+                DSSERR("DSI2 PLL clock not active\n");
+}
+
 int dsi_init(struct platform_device *pdev)
 {
         u32 rev;
@@ -3292,7 +3253,10 @@ int dsi_init(struct platform_device *pdev)
         mutex_init(&dsi.lock);
         sema_init(&dsi.bus_lock, 1);
 
-        INIT_WORK(&dsi.framedone_work, dsi_framedone_work_callback);
+        dsi.workqueue = create_singlethread_workqueue("dsi");
+        if (dsi.workqueue == NULL)
+                return -ENOMEM;
+
         INIT_DELAYED_WORK_DEFERRABLE(&dsi.framedone_timeout_work,
                         dsi_framedone_timeout_work_callback);
 
@@ -3328,6 +3292,7 @@ int dsi_init(struct platform_device *pdev)
 err2:
         iounmap(dsi.base);
 err1:
+        destroy_workqueue(dsi.workqueue);
         return r;
 }
 
@@ -3335,6 +3300,8 @@ void dsi_exit(void)
 {
         iounmap(dsi.base);
 
+        destroy_workqueue(dsi.workqueue);
+
         DSSDBG("omap_dsi_exit\n");
 }
 
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 24b18258654f..77c3621c9171 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -265,6 +265,9 @@ void dss_select_dispc_clk_source(enum dss_clk_source clk_src)
 
         b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
 
+        if (clk_src == DSS_SRC_DSI1_PLL_FCLK)
+                dsi_wait_dsi1_pll_active();
+
         REG_FLD_MOD(DSS_CONTROL, b, 0, 0);      /* DISPC_CLK_SWITCH */
 
         dss.dispc_clk_source = clk_src;
@@ -279,6 +282,9 @@ void dss_select_dsi_clk_source(enum dss_clk_source clk_src)
 
         b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
 
+        if (clk_src == DSS_SRC_DSI2_PLL_FCLK)
+                dsi_wait_dsi2_pll_active();
+
         REG_FLD_MOD(DSS_CONTROL, b, 1, 1);      /* DSI_CLK_SWITCH */
 
         dss.dsi_clk_source = clk_src;
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index 786f433fd571..5c7940d5f282 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -199,7 +199,8 @@ int dss_init_overlay_managers(struct platform_device *pdev);
 void dss_uninit_overlay_managers(struct platform_device *pdev);
 int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl);
 void dss_setup_partial_planes(struct omap_dss_device *dssdev,
-                                u16 *x, u16 *y, u16 *w, u16 *h);
+                                u16 *x, u16 *y, u16 *w, u16 *h,
+                                bool enlarge_update_area);
 void dss_start_update(struct omap_dss_device *dssdev);
 
 /* overlay */
@@ -281,6 +282,8 @@ void dsi_pll_uninit(void);
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
                 u32 fifo_size, enum omap_burst_size *burst_size,
                 u32 *fifo_low, u32 *fifo_high);
+void dsi_wait_dsi1_pll_active(void);
+void dsi_wait_dsi2_pll_active(void);
 #else
 static inline int dsi_init(struct platform_device *pdev)
 {
@@ -289,6 +292,12 @@ static inline int dsi_init(struct platform_device *pdev)
 static inline void dsi_exit(void)
 {
 }
+static inline void dsi_wait_dsi1_pll_active(void)
+{
+}
+static inline void dsi_wait_dsi2_pll_active(void)
+{
+}
 #endif
 
 /* DPI */
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 9e1fbe531bf0..6a649ab5539e 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -440,6 +440,10 @@ struct manager_cache_data {
 
         /* manual update region */
         u16 x, y, w, h;
+
+        /* enlarge the update area if the update area contains scaled
+         * overlays */
+        bool enlarge_update_area;
 };
 
 static struct {
@@ -525,7 +529,7 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
         int i;
         struct omap_dss_device *dssdev = mgr->device;
 
-        if (!dssdev)
+        if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
         if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
@@ -596,11 +600,14 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
         int r;
         int i;
 
-        if (!ovl->manager || !ovl->manager->device)
+        if (!ovl->manager)
                 return 0;
 
         dssdev = ovl->manager->device;
 
+        if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
         if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
                 irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
                 channel = OMAP_DSS_CHANNEL_DIGIT;
@@ -718,6 +725,7 @@ static int configure_overlay(enum omap_plane plane)
         u16 x, y, w, h;
         u32 paddr;
         int r;
+        u16 orig_w, orig_h, orig_outw, orig_outh;
 
         DSSDBGF("%d", plane);
 
@@ -738,8 +746,16 @@ static int configure_overlay(enum omap_plane plane)
         outh = c->out_height == 0 ? c->height : c->out_height;
         paddr = c->paddr;
 
+        orig_w = w;
+        orig_h = h;
+        orig_outw = outw;
+        orig_outh = outh;
+
         if (c->manual_update && mc->do_manual_update) {
                 unsigned bpp;
+                unsigned scale_x_m = w, scale_x_d = outw;
+                unsigned scale_y_m = h, scale_y_d = outh;
+
                 /* If the overlay is outside the update region, disable it */
                 if (!rectangle_intersects(mc->x, mc->y, mc->w, mc->h,
                                         x, y, outw, outh)) {
@@ -770,38 +786,47 @@ static int configure_overlay(enum omap_plane plane)
                         BUG();
                 }
 
-                if (dispc_is_overlay_scaled(c)) {
-                        /* If the overlay is scaled, the update area has
-                         * already been enlarged to cover the whole overlay. We
-                         * only need to adjust x/y here */
-                        x = c->pos_x - mc->x;
-                        y = c->pos_y - mc->y;
+                if (mc->x > c->pos_x) {
+                        x = 0;
+                        outw -= (mc->x - c->pos_x);
+                        paddr += (mc->x - c->pos_x) *
+                                scale_x_m / scale_x_d * bpp / 8;
                 } else {
-                        if (mc->x > c->pos_x) {
-                                x = 0;
-                                w -= (mc->x - c->pos_x);
-                                paddr += (mc->x - c->pos_x) * bpp / 8;
-                        } else {
-                                x = c->pos_x - mc->x;
-                        }
-
-                        if (mc->y > c->pos_y) {
-                                y = 0;
-                                h -= (mc->y - c->pos_y);
-                                paddr += (mc->y - c->pos_y) * c->screen_width *
-                                        bpp / 8;
-                        } else {
-                                y = c->pos_y - mc->y;
-                        }
-
-                        if (mc->w < (x+w))
-                                w -= (x+w) - (mc->w);
+                        x = c->pos_x - mc->x;
+                }
 
-                        if (mc->h < (y+h))
-                                h -= (y+h) - (mc->h);
+                if (mc->y > c->pos_y) {
+                        y = 0;
+                        outh -= (mc->y - c->pos_y);
+                        paddr += (mc->y - c->pos_y) *
+                                scale_y_m / scale_y_d *
+                                c->screen_width * bpp / 8;
+                } else {
+                        y = c->pos_y - mc->y;
+                }
 
-                        outw = w;
-                        outh = h;
+                if (mc->w < (x + outw))
+                        outw -= (x + outw) - (mc->w);
+
+                if (mc->h < (y + outh))
+                        outh -= (y + outh) - (mc->h);
+
+                w = w * outw / orig_outw;
+                h = h * outh / orig_outh;
+
+                /* YUV mode overlay's input width has to be even and the
+                 * algorithm above may adjust the width to be odd.
+                 *
+                 * Here we adjust the width if needed, preferring to increase
+                 * the width if the original width was bigger.
+                 */
+                if ((w & 1) &&
+                                (c->color_mode == OMAP_DSS_COLOR_YUV2 ||
+                                 c->color_mode == OMAP_DSS_COLOR_UYVY)) {
+                        if (orig_w > w)
+                                w += 1;
+                        else
+                                w -= 1;
                 }
         }
 
@@ -960,7 +985,7 @@ static void make_even(u16 *x, u16 *w)
 /* Configure dispc for partial update. Return possibly modified update
  * area */
 void dss_setup_partial_planes(struct omap_dss_device *dssdev,
-                u16 *xi, u16 *yi, u16 *wi, u16 *hi)
+                u16 *xi, u16 *yi, u16 *wi, u16 *hi, bool enlarge_update_area)
 {
         struct overlay_cache_data *oc;
         struct manager_cache_data *mc;
@@ -969,6 +994,7 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
         int i;
         u16 x, y, w, h;
         unsigned long flags;
+        bool area_changed;
 
         x = *xi;
         y = *yi;
@@ -989,73 +1015,91 @@ void dss_setup_partial_planes(struct omap_dss_device *dssdev,
 
         spin_lock_irqsave(&dss_cache.lock, flags);
 
-        /* We need to show the whole overlay if it is scaled. So look for
-         * those, and make the update area larger if found.
-         * Also mark the overlay cache dirty */
-        for (i = 0; i < num_ovls; ++i) {
-                unsigned x1, y1, x2, y2;
-                unsigned outw, outh;
+        /*
+         * Execute the outer loop until the inner loop has completed
+         * once without increasing the update area. This will ensure that
+         * all scaled overlays end up completely within the update area.
+         */
+        do {
+                area_changed = false;
 
-                oc = &dss_cache.overlay_cache[i];
+                /* We need to show the whole overlay if it is scaled. So look
+                 * for those, and make the update area larger if found.
+                 * Also mark the overlay cache dirty */
+                for (i = 0; i < num_ovls; ++i) {
+                        unsigned x1, y1, x2, y2;
+                        unsigned outw, outh;
 
-                if (oc->channel != mgr->id)
-                        continue;
+                        oc = &dss_cache.overlay_cache[i];
 
-                oc->dirty = true;
+                        if (oc->channel != mgr->id)
+                                continue;
 
-                if (!oc->enabled)
-                        continue;
+                        oc->dirty = true;
 
-                if (!dispc_is_overlay_scaled(oc))
-                        continue;
+                        if (!enlarge_update_area)
+                                continue;
 
-                outw = oc->out_width == 0 ? oc->width : oc->out_width;
-                outh = oc->out_height == 0 ? oc->height : oc->out_height;
+                        if (!oc->enabled)
+                                continue;
 
-                /* is the overlay outside the update region? */
-                if (!rectangle_intersects(x, y, w, h,
-                                        oc->pos_x, oc->pos_y,
-                                        outw, outh))
-                        continue;
+                        if (!dispc_is_overlay_scaled(oc))
+                                continue;
 
-                /* if the overlay totally inside the update region? */
-                if (rectangle_subset(oc->pos_x, oc->pos_y, outw, outh,
-                                        x, y, w, h))
-                        continue;
+                        outw = oc->out_width == 0 ?
+                                oc->width : oc->out_width;
+                        outh = oc->out_height == 0 ?
+                                oc->height : oc->out_height;
+
+                        /* is the overlay outside the update region? */
+                        if (!rectangle_intersects(x, y, w, h,
+                                                oc->pos_x, oc->pos_y,
+                                                outw, outh))
+                                continue;
 
-                if (x > oc->pos_x)
-                        x1 = oc->pos_x;
-                else
-                        x1 = x;
+                        /* if the overlay totally inside the update region? */
+                        if (rectangle_subset(oc->pos_x, oc->pos_y, outw, outh,
+                                                x, y, w, h))
+                                continue;
 
-                if (y > oc->pos_y)
-                        y1 = oc->pos_y;
-                else
-                        y1 = y;
+                        if (x > oc->pos_x)
+                                x1 = oc->pos_x;
+                        else
+                                x1 = x;
 
-                if ((x + w) < (oc->pos_x + outw))
-                        x2 = oc->pos_x + outw;
-                else
-                        x2 = x + w;
+                        if (y > oc->pos_y)
+                                y1 = oc->pos_y;
+                        else
+                                y1 = y;
 
-                if ((y + h) < (oc->pos_y + outh))
-                        y2 = oc->pos_y + outh;
-                else
-                        y2 = y + h;
+                        if ((x + w) < (oc->pos_x + outw))
+                                x2 = oc->pos_x + outw;
+                        else
+                                x2 = x + w;
 
-                x = x1;
-                y = y1;
-                w = x2 - x1;
-                h = y2 - y1;
+                        if ((y + h) < (oc->pos_y + outh))
+                                y2 = oc->pos_y + outh;
+                        else
+                                y2 = y + h;
 
-                make_even(&x, &w);
+                        x = x1;
+                        y = y1;
+                        w = x2 - x1;
+                        h = y2 - y1;
 
-                DSSDBG("changing upd area due to ovl(%d) scaling %d,%d %dx%d\n",
+                        make_even(&x, &w);
+
+                        DSSDBG("changing upd area due to ovl(%d) "
+                               "scaling %d,%d %dx%d\n",
                                 i, x, y, w, h);
-        }
+
+                        area_changed = true;
+                }
+        } while (area_changed);
 
         mc = &dss_cache.manager_cache[mgr->id];
         mc->do_manual_update = true;
+        mc->enlarge_update_area = enlarge_update_area;
         mc->x = x;
         mc->y = y;
         mc->w = w;
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 82336583adef..244dca81a399 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -65,7 +65,7 @@ static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
                 for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
                         mgr = omap_dss_get_overlay_manager(i);
 
-                        if (strncmp(buf, mgr->name, len) == 0)
+                        if (sysfs_streq(buf, mgr->name))
                                 break;
 
                         mgr = NULL;
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index cc23f53cc62d..bbe62464e92d 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -886,7 +886,7 @@ int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
                 return -EINVAL;
 
         if (dssdev->manager->caps & OMAP_DSS_OVL_MGR_CAP_DISPC) {
-                dss_setup_partial_planes(dssdev, x, y, w, h);
+                dss_setup_partial_planes(dssdev, x, y, w, h, true);
                 dispc_set_lcd_size(*w, *h);
         }
 
diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index 9c7361871d78..6f435450987e 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -34,12 +34,37 @@
 
 #include "omapfb.h"
 
+static u8 get_mem_idx(struct omapfb_info *ofbi)
+{
+        if (ofbi->id == ofbi->region->id)
+                return 0;
+
+        return OMAPFB_MEM_IDX_ENABLED | ofbi->region->id;
+}
+
+static struct omapfb2_mem_region *get_mem_region(struct omapfb_info *ofbi,
+                                                 u8 mem_idx)
+{
+        struct omapfb2_device *fbdev = ofbi->fbdev;
+
+        if (mem_idx & OMAPFB_MEM_IDX_ENABLED)
+                mem_idx &= OMAPFB_MEM_IDX_MASK;
+        else
+                mem_idx = ofbi->id;
+
+        if (mem_idx >= fbdev->num_fbs)
+                return NULL;
+
+        return &fbdev->regions[mem_idx];
+}
+
 static int omapfb_setup_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omap_overlay *ovl;
-        struct omap_overlay_info info;
+        struct omap_overlay_info old_info;
+        struct omapfb2_mem_region *old_rg, *new_rg;
         int r = 0;
 
         DBG("omapfb_setup_plane\n");
@@ -52,36 +77,106 @@ static int omapfb_setup_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
         /* XXX uses only the first overlay */
         ovl = ofbi->overlays[0];
 
-        if (pi->enabled && !ofbi->region.size) {
+        old_rg = ofbi->region;
+        new_rg = get_mem_region(ofbi, pi->mem_idx);
+        if (!new_rg) {
+                r = -EINVAL;
+                goto out;
+        }
+
+        /* Take the locks in a specific order to keep lockdep happy */
+        if (old_rg->id < new_rg->id) {
+                omapfb_get_mem_region(old_rg);
+                omapfb_get_mem_region(new_rg);
+        } else if (new_rg->id < old_rg->id) {
+                omapfb_get_mem_region(new_rg);
+                omapfb_get_mem_region(old_rg);
+        } else
+                omapfb_get_mem_region(old_rg);
+
+        if (pi->enabled && !new_rg->size) {
                 /*
                  * This plane's memory was freed, can't enable it
                  * until it's reallocated.
                  */
                 r = -EINVAL;
-                goto out;
+                goto put_mem;
         }
 
-        ovl->get_overlay_info(ovl, &info);
+        ovl->get_overlay_info(ovl, &old_info);
 
-        info.pos_x = pi->pos_x;
-        info.pos_y = pi->pos_y;
-        info.out_width = pi->out_width;
-        info.out_height = pi->out_height;
-        info.enabled = pi->enabled;
+        if (old_rg != new_rg) {
+                ofbi->region = new_rg;
+                set_fb_fix(fbi);
+        }
 
-        r = ovl->set_overlay_info(ovl, &info);
-        if (r)
-                goto out;
+        if (pi->enabled) {
+                struct omap_overlay_info info;
 
-        if (ovl->manager) {
-                r = ovl->manager->apply(ovl->manager);
+                r = omapfb_setup_overlay(fbi, ovl, pi->pos_x, pi->pos_y,
+                        pi->out_width, pi->out_height);
                 if (r)
-                        goto out;
+                        goto undo;
+
+                ovl->get_overlay_info(ovl, &info);
+
+                if (!info.enabled) {
+                        info.enabled = pi->enabled;
+                        r = ovl->set_overlay_info(ovl, &info);
+                        if (r)
+                                goto undo;
+                }
+        } else {
+                struct omap_overlay_info info;
+
+                ovl->get_overlay_info(ovl, &info);
+
+                info.enabled = pi->enabled;
+                info.pos_x = pi->pos_x;
+                info.pos_y = pi->pos_y;
+                info.out_width = pi->out_width;
+                info.out_height = pi->out_height;
+
+                r = ovl->set_overlay_info(ovl, &info);
+                if (r)
+                        goto undo;
         }
 
-out:
-        if (r)
-                dev_err(fbdev->dev, "setup_plane failed\n");
+        if (ovl->manager)
+                ovl->manager->apply(ovl->manager);
+
+        /* Release the locks in a specific order to keep lockdep happy */
+        if (old_rg->id > new_rg->id) {
+                omapfb_put_mem_region(old_rg);
+                omapfb_put_mem_region(new_rg);
+        } else if (new_rg->id > old_rg->id) {
+                omapfb_put_mem_region(new_rg);
+                omapfb_put_mem_region(old_rg);
+        } else
+                omapfb_put_mem_region(old_rg);
+
+        return 0;
+
+ undo:
+        if (old_rg != new_rg) {
+                ofbi->region = old_rg;
+                set_fb_fix(fbi);
+        }
+
+        ovl->set_overlay_info(ovl, &old_info);
+ put_mem:
+        /* Release the locks in a specific order to keep lockdep happy */
+        if (old_rg->id > new_rg->id) {
+                omapfb_put_mem_region(old_rg);
+                omapfb_put_mem_region(new_rg);
+        } else if (new_rg->id > old_rg->id) {
+                omapfb_put_mem_region(new_rg);
+                omapfb_put_mem_region(old_rg);
+        } else
+                omapfb_put_mem_region(old_rg);
+ out:
+        dev_err(fbdev->dev, "setup_plane failed\n");
+
         return r;
 }
 
@@ -92,8 +187,8 @@ static int omapfb_query_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
         if (ofbi->num_overlays != 1) {
                 memset(pi, 0, sizeof(*pi));
         } else {
-                struct omap_overlay_info *ovli;
                 struct omap_overlay *ovl;
+                struct omap_overlay_info *ovli;
 
                 ovl = ofbi->overlays[0];
                 ovli = &ovl->info;
@@ -103,6 +198,7 @@ static int omapfb_query_plane(struct fb_info *fbi, struct omapfb_plane_info *pi)
                 pi->enabled = ovli->enabled;
                 pi->channel_out = 0; /* xxx */
                 pi->mirror = 0;
+                pi->mem_idx = get_mem_idx(ofbi);
                 pi->out_width = ovli->out_width;
                 pi->out_height = ovli->out_height;
         }
@@ -115,7 +211,7 @@ static int omapfb_setup_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omapfb2_mem_region *rg;
-        int r, i;
+        int r = 0, i;
         size_t size;
 
         if (mi->type > OMAPFB_MEMTYPE_MAX)
@@ -123,22 +219,44 @@ static int omapfb_setup_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
 
         size = PAGE_ALIGN(mi->size);
 
-        rg = &ofbi->region;
+        rg = ofbi->region;
 
-        for (i = 0; i < ofbi->num_overlays; i++) {
-                if (ofbi->overlays[i]->info.enabled)
-                        return -EBUSY;
+        down_write_nested(&rg->lock, rg->id);
+        atomic_inc(&rg->lock_count);
+
+        if (atomic_read(&rg->map_count)) {
+                r = -EBUSY;
+                goto out;
+        }
+
+        for (i = 0; i < fbdev->num_fbs; i++) {
+                struct omapfb_info *ofbi2 = FB2OFB(fbdev->fbs[i]);
+                int j;
+
+                if (ofbi2->region != rg)
+                        continue;
+
+                for (j = 0; j < ofbi2->num_overlays; j++) {
+                        if (ofbi2->overlays[j]->info.enabled) {
+                                r = -EBUSY;
+                                goto out;
+                        }
+                }
         }
 
         if (rg->size != size || rg->type != mi->type) {
                 r = omapfb_realloc_fbmem(fbi, size, mi->type);
                 if (r) {
                         dev_err(fbdev->dev, "realloc fbmem failed\n");
-                        return r;
+                        goto out;
                 }
         }
 
-        return 0;
+ out:
+        atomic_dec(&rg->lock_count);
+        up_write(&rg->lock);
+
+        return r;
 }
 
 static int omapfb_query_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
@@ -146,12 +264,14 @@ static int omapfb_query_mem(struct fb_info *fbi, struct omapfb_mem_info *mi)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_mem_region *rg;
 
-        rg = &ofbi->region;
+        rg = omapfb_get_mem_region(ofbi->region);
         memset(mi, 0, sizeof(*mi));
 
         mi->size = rg->size;
         mi->type = rg->type;
 
+        omapfb_put_mem_region(rg);
+
         return 0;
 }
 
@@ -490,6 +610,7 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
                 struct omapfb_vram_info         vram_info;
                 struct omapfb_tearsync_info     tearsync_info;
                 struct omapfb_display_info      display_info;
+                u32                             crt;
         } p;
 
         int r = 0;
@@ -648,6 +769,17 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
                         r = -EFAULT;
                 break;
 
+        case FBIO_WAITFORVSYNC:
+                if (get_user(p.crt, (__u32 __user *)arg)) {
+                        r = -EFAULT;
+                        break;
+                }
+                if (p.crt != 0) {
+                        r = -ENODEV;
+                        break;
+                }
+                /* FALLTHROUGH */
+
         case OMAPFB_WAITFORVSYNC:
                 DBG("ioctl WAITFORVSYNC\n");
                 if (!display) {
@@ -738,7 +870,7 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
                         break;
                 }
 
-                if (!display->driver->enable_te) {
+                if (!display || !display->driver->enable_te) {
                         r = -ENODEV;
                         break;
                 }
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 4b4506da96da..04034d410d6d 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -157,7 +157,7 @@ static void fill_fb(struct fb_info *fbi)
 
 static unsigned omapfb_get_vrfb_offset(const struct omapfb_info *ofbi, int rot)
 {
-        const struct vrfb *vrfb = &ofbi->region.vrfb;
+        const struct vrfb *vrfb = &ofbi->region->vrfb;
         unsigned offset;
 
         switch (rot) {
@@ -185,27 +185,27 @@ static unsigned omapfb_get_vrfb_offset(const struct omapfb_info *ofbi, int rot)
 static u32 omapfb_get_region_rot_paddr(const struct omapfb_info *ofbi, int rot)
 {
         if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB) {
-                return ofbi->region.vrfb.paddr[rot]
+                return ofbi->region->vrfb.paddr[rot]
                         + omapfb_get_vrfb_offset(ofbi, rot);
         } else {
-                return ofbi->region.paddr;
+                return ofbi->region->paddr;
         }
 }
 
 static u32 omapfb_get_region_paddr(const struct omapfb_info *ofbi)
 {
         if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
-                return ofbi->region.vrfb.paddr[0];
+                return ofbi->region->vrfb.paddr[0];
         else
-                return ofbi->region.paddr;
+                return ofbi->region->paddr;
 }
 
 static void __iomem *omapfb_get_region_vaddr(const struct omapfb_info *ofbi)
 {
         if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
-                return ofbi->region.vrfb.vaddr[0];
+                return ofbi->region->vrfb.vaddr[0];
         else
-                return ofbi->region.vaddr;
+                return ofbi->region->vaddr;
 }
 
 static struct omapfb_colormode omapfb_colormodes[] = {
@@ -450,7 +450,7 @@ static int check_vrfb_fb_size(unsigned long region_size,
 static int check_fb_size(const struct omapfb_info *ofbi,
                 struct fb_var_screeninfo *var)
 {
-        unsigned long max_frame_size = ofbi->region.size;
+        unsigned long max_frame_size = ofbi->region->size;
         int bytespp = var->bits_per_pixel >> 3;
         unsigned long line_size = var->xres_virtual * bytespp;
 
@@ -497,7 +497,7 @@ static int check_fb_size(const struct omapfb_info *ofbi,
 static int setup_vrfb_rotation(struct fb_info *fbi)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        struct omapfb2_mem_region *rg = &ofbi->region;
+        struct omapfb2_mem_region *rg = ofbi->region;
         struct vrfb *vrfb = &rg->vrfb;
         struct fb_var_screeninfo *var = &fbi->var;
         struct fb_fix_screeninfo *fix = &fbi->fix;
@@ -558,9 +558,9 @@ static int setup_vrfb_rotation(struct fb_info *fbi)
                 return r;
 
         /* used by open/write in fbmem.c */
-        fbi->screen_base = ofbi->region.vrfb.vaddr[0];
+        fbi->screen_base = ofbi->region->vrfb.vaddr[0];
 
-        fix->smem_start = ofbi->region.vrfb.paddr[0];
+        fix->smem_start = ofbi->region->vrfb.paddr[0];
 
         switch (var->nonstd) {
         case OMAPFB_COLOR_YUV422:
@@ -599,7 +599,7 @@ void set_fb_fix(struct fb_info *fbi)
         struct fb_fix_screeninfo *fix = &fbi->fix;
         struct fb_var_screeninfo *var = &fbi->var;
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        struct omapfb2_mem_region *rg = &ofbi->region;
+        struct omapfb2_mem_region *rg = ofbi->region;
 
         DBG("set_fb_fix\n");
 
@@ -668,8 +668,7 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
 
         DBG("check_fb_var %d\n", ofbi->id);
 
-        if (ofbi->region.size == 0)
-                return 0;
+        WARN_ON(!atomic_read(&ofbi->region->lock_count));
 
         r = fb_mode_to_dss_mode(var, &mode);
         if (r) {
@@ -684,13 +683,14 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
                 }
         }
 
-        if (var->rotate < 0 || var->rotate > 3)
+        if (var->rotate > 3)
                 return -EINVAL;
 
         if (check_fb_res_bounds(var))
                 return -EINVAL;
 
-        if (check_fb_size(ofbi, var))
+        /* When no memory is allocated ignore the size check */
+        if (ofbi->region->size != 0 && check_fb_size(ofbi, var))
                 return -EINVAL;
 
         if (var->xres + var->xoffset > var->xres_virtual)
@@ -822,9 +822,43 @@ static unsigned calc_rotation_offset_vrfb(const struct fb_var_screeninfo *var,
         return offset;
 }
 
+static void omapfb_calc_addr(const struct omapfb_info *ofbi,
+                             const struct fb_var_screeninfo *var,
+                             const struct fb_fix_screeninfo *fix,
+                             int rotation, u32 *paddr, void __iomem **vaddr)
+{
+        u32 data_start_p;
+        void __iomem *data_start_v;
+        int offset;
+
+        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB) {
+                data_start_p = omapfb_get_region_rot_paddr(ofbi, rotation);
+                data_start_v = NULL;
+        } else {
+                data_start_p = omapfb_get_region_paddr(ofbi);
+                data_start_v = omapfb_get_region_vaddr(ofbi);
+        }
+
+        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
+                offset = calc_rotation_offset_vrfb(var, fix, rotation);
+        else
+                offset = calc_rotation_offset_dma(var, fix, rotation);
+
+        data_start_p += offset;
+        data_start_v += offset;
+
+        if (offset)
+                DBG("offset %d, %d = %d\n",
+                    var->xoffset, var->yoffset, offset);
+
+        DBG("paddr %x, vaddr %p\n", data_start_p, data_start_v);
+
+        *paddr = data_start_p;
+        *vaddr = data_start_v;
+}
 
 /* setup overlay according to the fb */
-static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
+int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
                 u16 posx, u16 posy, u16 outw, u16 outh)
 {
         int r = 0;
@@ -832,9 +866,8 @@ static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
         struct fb_var_screeninfo *var = &fbi->var;
         struct fb_fix_screeninfo *fix = &fbi->fix;
         enum omap_color_mode mode = 0;
-        int offset;
-        u32 data_start_p;
-        void __iomem *data_start_v;
+        u32 data_start_p = 0;
+        void __iomem *data_start_v = NULL;
         struct omap_overlay_info info;
         int xres, yres;
         int screen_width;
@@ -842,6 +875,8 @@ static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
         int rotation = var->rotate;
         int i;
 
+        WARN_ON(!atomic_read(&ofbi->region->lock_count));
+
         for (i = 0; i < ofbi->num_overlays; i++) {
                 if (ovl != ofbi->overlays[i])
                         continue;
@@ -861,28 +896,9 @@ static int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
                 yres = var->yres;
         }
 
-
-        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB) {
-                data_start_p = omapfb_get_region_rot_paddr(ofbi, rotation);
-                data_start_v = NULL;
-        } else {
-                data_start_p = omapfb_get_region_paddr(ofbi);
-                data_start_v = omapfb_get_region_vaddr(ofbi);
-        }
-
-        if (ofbi->rotation_type == OMAP_DSS_ROT_VRFB)
-                offset = calc_rotation_offset_vrfb(var, fix, rotation);
-        else
-                offset = calc_rotation_offset_dma(var, fix, rotation);
-
-        data_start_p += offset;
-        data_start_v += offset;
-
-        if (offset)
-                DBG("offset %d, %d = %d\n",
-                                var->xoffset, var->yoffset, offset);
-
-        DBG("paddr %x, vaddr %p\n", data_start_p, data_start_v);
+        if (ofbi->region->size)
+                omapfb_calc_addr(ofbi, var, fix, rotation,
+                                 &data_start_p, &data_start_v);
 
         r = fb_mode_to_dss_mode(var, &mode);
         if (r) {
@@ -954,12 +970,14 @@ int omapfb_apply_changes(struct fb_info *fbi, int init)
                 fill_fb(fbi);
 #endif
 
+        WARN_ON(!atomic_read(&ofbi->region->lock_count));
+
         for (i = 0; i < ofbi->num_overlays; i++) {
                 ovl = ofbi->overlays[i];
 
                 DBG("apply_changes, fb %d, ovl %d\n", ofbi->id, ovl->id);
 
-                if (ofbi->region.size == 0) {
+                if (ofbi->region->size == 0) {
                         /* the fb is not available. disable the overlay */
                         omapfb_overlay_enable(ovl, 0);
                         if (!init && ovl->manager)
@@ -1007,36 +1025,48 @@ err:
  * DO NOT MODIFY PAR */
 static int omapfb_check_var(struct fb_var_screeninfo *var, struct fb_info *fbi)
 {
+        struct omapfb_info *ofbi = FB2OFB(fbi);
         int r;
 
         DBG("check_var(%d)\n", FB2OFB(fbi)->id);
 
+        omapfb_get_mem_region(ofbi->region);
+
         r = check_fb_var(fbi, var);
 
+        omapfb_put_mem_region(ofbi->region);
+
         return r;
 }
 
 /* set the video mode according to info->var */
 static int omapfb_set_par(struct fb_info *fbi)
 {
+        struct omapfb_info *ofbi = FB2OFB(fbi);
         int r;
 
         DBG("set_par(%d)\n", FB2OFB(fbi)->id);
 
+        omapfb_get_mem_region(ofbi->region);
+
         set_fb_fix(fbi);
 
         r = setup_vrfb_rotation(fbi);
         if (r)
-                return r;
+                goto out;
 
         r = omapfb_apply_changes(fbi, 0);
 
+ out:
+        omapfb_put_mem_region(ofbi->region);
+
         return r;
 }
 
 static int omapfb_pan_display(struct fb_var_screeninfo *var,
                 struct fb_info *fbi)
 {
+        struct omapfb_info *ofbi = FB2OFB(fbi);
         struct fb_var_screeninfo new_var;
         int r;
 
@@ -1052,23 +1082,31 @@ static int omapfb_pan_display(struct fb_var_screeninfo *var,
 
         fbi->var = new_var;
 
+        omapfb_get_mem_region(ofbi->region);
+
         r = omapfb_apply_changes(fbi, 0);
 
+        omapfb_put_mem_region(ofbi->region);
+
         return r;
 }
 
 static void mmap_user_open(struct vm_area_struct *vma)
 {
-        struct omapfb_info *ofbi = (struct omapfb_info *)vma->vm_private_data;
+        struct omapfb2_mem_region *rg = vma->vm_private_data;
 
-        atomic_inc(&ofbi->map_count);
+        omapfb_get_mem_region(rg);
+        atomic_inc(&rg->map_count);
+        omapfb_put_mem_region(rg);
 }
 
 static void mmap_user_close(struct vm_area_struct *vma)
 {
-        struct omapfb_info *ofbi = (struct omapfb_info *)vma->vm_private_data;
+        struct omapfb2_mem_region *rg = vma->vm_private_data;
 
-        atomic_dec(&ofbi->map_count);
+        omapfb_get_mem_region(rg);
+        atomic_dec(&rg->map_count);
+        omapfb_put_mem_region(rg);
 }
 
 static struct vm_operations_struct mmap_user_ops = {
@@ -1080,9 +1118,11 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 {
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct fb_fix_screeninfo *fix = &fbi->fix;
+        struct omapfb2_mem_region *rg;
         unsigned long off;
         unsigned long start;
         u32 len;
+        int r = -EINVAL;
 
         if (vma->vm_end - vma->vm_start == 0)
                 return 0;
@@ -1090,12 +1130,14 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
                 return -EINVAL;
         off = vma->vm_pgoff << PAGE_SHIFT;
 
+        rg = omapfb_get_mem_region(ofbi->region);
+
         start = omapfb_get_region_paddr(ofbi);
         len = fix->smem_len;
         if (off >= len)
-                return -EINVAL;
+                goto error;
         if ((vma->vm_end - vma->vm_start + off) > len)
-                return -EINVAL;
+                goto error;
 
         off += start;
 
@@ -1105,13 +1147,25 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
         vma->vm_flags |= VM_IO | VM_RESERVED;
         vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
         vma->vm_ops = &mmap_user_ops;
-        vma->vm_private_data = ofbi;
+        vma->vm_private_data = rg;
         if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
-                             vma->vm_end - vma->vm_start, vma->vm_page_prot))
-                return -EAGAIN;
+                               vma->vm_end - vma->vm_start,
+                               vma->vm_page_prot)) {
+                r = -EAGAIN;
+                goto error;
+        }
+
         /* vm_ops.open won't be called for mmap itself. */
-        atomic_inc(&ofbi->map_count);
+        atomic_inc(&rg->map_count);
+
+        omapfb_put_mem_region(rg);
+
         return 0;
+
+ error:
+        omapfb_put_mem_region(ofbi->region);
+
+        return r;
 }
 
 /* Store a single color palette entry into a pseudo palette or the hardware
@@ -1154,11 +1208,6 @@ static int _setcolreg(struct fb_info *fbi, u_int regno, u_int red, u_int green,
                 if (r != 0)
                         break;
 
-                if (regno < 0) {
-                        r = -EINVAL;
-                        break;
-                }
-
                 if (regno < 16) {
                         u16 pal;
                         pal = ((red >> (16 - var->red.length)) <<
@@ -1217,6 +1266,9 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
         int do_update = 0;
         int r = 0;
 
+        if (!display)
+                return -EINVAL;
+
         omapfb_lock(fbdev);
 
         switch (blank) {
@@ -1300,7 +1352,9 @@ static void omapfb_free_fbmem(struct fb_info *fbi)
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omapfb2_mem_region *rg;
 
-        rg = &ofbi->region;
+        rg = ofbi->region;
+
+        WARN_ON(atomic_read(&rg->map_count));
 
         if (rg->paddr)
                 if (omap_vram_free(rg->paddr, rg->size))
@@ -1355,8 +1409,15 @@ static int omapfb_alloc_fbmem(struct fb_info *fbi, unsigned long size,
         void __iomem *vaddr;
         int r;
 
-        rg = &ofbi->region;
-        memset(rg, 0, sizeof(*rg));
+        rg = ofbi->region;
+
+        rg->paddr = 0;
+        rg->vaddr = NULL;
+        memset(&rg->vrfb, 0, sizeof rg->vrfb);
+        rg->size = 0;
+        rg->type = 0;
+        rg->alloc = false;
+        rg->map = false;
 
         size = PAGE_ALIGN(size);
 
@@ -1609,7 +1670,7 @@ static int omapfb_allocate_all_fbs(struct omapfb2_device *fbdev)
         for (i = 0; i < fbdev->num_fbs; i++) {
                 struct omapfb_info *ofbi = FB2OFB(fbdev->fbs[i]);
                 struct omapfb2_mem_region *rg;
-                rg = &ofbi->region;
+                rg = ofbi->region;
 
                 DBG("region%d phys %08x virt %p size=%lu\n",
                                 i,
@@ -1626,7 +1687,7 @@ int omapfb_realloc_fbmem(struct fb_info *fbi, unsigned long size, int type)
         struct omapfb_info *ofbi = FB2OFB(fbi);
         struct omapfb2_device *fbdev = ofbi->fbdev;
         struct omap_dss_device *display = fb2display(fbi);
-        struct omapfb2_mem_region *rg = &ofbi->region;
+        struct omapfb2_mem_region *rg = ofbi->region;
         unsigned long old_size = rg->size;
         unsigned long old_paddr = rg->paddr;
         int old_type = rg->type;
@@ -1709,7 +1770,7 @@ static int omapfb_fb_init(struct omapfb2_device *fbdev, struct fb_info *fbi)
         fbi->flags = FBINFO_FLAG_DEFAULT;
         fbi->pseudo_palette = fbdev->pseudo_palette;
 
-        if (ofbi->region.size == 0) {
+        if (ofbi->region->size == 0) {
                 clear_fb_info(fbi);
                 return 0;
         }
@@ -1871,6 +1932,10 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
                 ofbi->fbdev = fbdev;
                 ofbi->id = i;
 
+                ofbi->region = &fbdev->regions[i];
+                ofbi->region->id = i;
+                init_rwsem(&ofbi->region->lock);
+
                 /* assign these early, so that fb alloc can use them */
                 ofbi->rotation_type = def_vrfb ? OMAP_DSS_ROT_VRFB :
                         OMAP_DSS_ROT_DMA;
@@ -1900,7 +1965,13 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
 
         /* setup fb_infos */
         for (i = 0; i < fbdev->num_fbs; i++) {
-                r = omapfb_fb_init(fbdev, fbdev->fbs[i]);
+                struct fb_info *fbi = fbdev->fbs[i];
+                struct omapfb_info *ofbi = FB2OFB(fbi);
+
+                omapfb_get_mem_region(ofbi->region);
+                r = omapfb_fb_init(fbdev, fbi);
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r) {
                         dev_err(fbdev->dev, "failed to setup fb_info\n");
                         return r;
@@ -1921,20 +1992,19 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
         DBG("framebuffers registered\n");
 
         for (i = 0; i < fbdev->num_fbs; i++) {
-                r = omapfb_apply_changes(fbdev->fbs[i], 1);
+                struct fb_info *fbi = fbdev->fbs[i];
+                struct omapfb_info *ofbi = FB2OFB(fbi);
+
+                omapfb_get_mem_region(ofbi->region);
+                r = omapfb_apply_changes(fbi, 1);
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r) {
                         dev_err(fbdev->dev, "failed to change mode\n");
                         return r;
                 }
         }
 
-        DBG("create sysfs for fbs\n");
-        r = omapfb_create_sysfs(fbdev);
-        if (r) {
-                dev_err(fbdev->dev, "failed to create sysfs entries\n");
-                return r;
-        }
-
         /* Enable fb0 */
         if (fbdev->num_fbs > 0) {
                 struct omapfb_info *ofbi = FB2OFB(fbdev->fbs[0]);
@@ -1968,11 +2038,11 @@ static int omapfb_mode_to_timings(const char *mode_str,
 #ifdef CONFIG_OMAP2_DSS_VENC
         if (strcmp(mode_str, "pal") == 0) {
                 *timings = omap_dss_pal_timings;
-                *bpp = 0;
+                *bpp = 24;
                 return 0;
         } else if (strcmp(mode_str, "ntsc") == 0) {
                 *timings = omap_dss_ntsc_timings;
-                *bpp = 0;
+                *bpp = 24;
                 return 0;
         }
 #endif
@@ -2220,6 +2290,13 @@ static int omapfb_probe(struct platform_device *pdev)
                 }
         }
 
+        DBG("create sysfs for fbs\n");
+        r = omapfb_create_sysfs(fbdev);
+        if (r) {
+                dev_err(fbdev->dev, "failed to create sysfs entries\n");
+                goto cleanup;
+        }
+
         return 0;
 
 cleanup:
diff --git a/drivers/video/omap2/omapfb/omapfb-sysfs.c b/drivers/video/omap2/omapfb/omapfb-sysfs.c
index 5179219128bd..6f9c72cd6bb0 100644
--- a/drivers/video/omap2/omapfb/omapfb-sysfs.c
+++ b/drivers/video/omap2/omapfb/omapfb-sysfs.c
@@ -49,6 +49,7 @@ static ssize_t store_rotate_type(struct device *dev,
 {
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
+        struct omapfb2_mem_region *rg;
         enum omap_dss_rotation_type rot_type;
         int r;
 
@@ -64,9 +65,11 @@ static ssize_t store_rotate_type(struct device *dev,
         if (rot_type == ofbi->rotation_type)
                 goto out;
 
-        if (ofbi->region.size) {
+        rg = omapfb_get_mem_region(ofbi->region);
+
+        if (rg->size) {
                 r = -EBUSY;
-                goto out;
+                goto put_region;
         }
 
         ofbi->rotation_type = rot_type;
@@ -75,6 +78,8 @@ static ssize_t store_rotate_type(struct device *dev,
          * Since the VRAM for this FB is not allocated at the moment we don't
          * need to do any further parameter checking at this point.
          */
+put_region:
+        omapfb_put_mem_region(rg);
 out:
         unlock_fb_info(fbi);
 
@@ -97,7 +102,7 @@ static ssize_t store_mirror(struct device *dev,
 {
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
-        bool mirror;
+        unsigned long mirror;
         int r;
         struct fb_var_screeninfo new_var;
 
@@ -111,6 +116,8 @@ static ssize_t store_mirror(struct device *dev,
 
         ofbi->mirror = mirror;
 
+        omapfb_get_mem_region(ofbi->region);
+
         memcpy(&new_var, &fbi->var, sizeof(new_var));
         r = check_fb_var(fbi, &new_var);
         if (r)
@@ -125,6 +132,8 @@ static ssize_t store_mirror(struct device *dev,
 
         r = count;
 out:
+        omapfb_put_mem_region(ofbi->region);
+
         unlock_fb_info(fbi);
 
         return r;
@@ -263,11 +272,15 @@ static ssize_t store_overlays(struct device *dev, struct device_attribute *attr,
 
                 DBG("detaching %d\n", ofbi->overlays[i]->id);
 
+                omapfb_get_mem_region(ofbi->region);
+
                 omapfb_overlay_enable(ovl, 0);
 
                 if (ovl->manager)
                         ovl->manager->apply(ovl->manager);
 
+                omapfb_put_mem_region(ofbi->region);
+
                 for (t = i + 1; t < ofbi->num_overlays; t++) {
                         ofbi->rotation[t-1] = ofbi->rotation[t];
                         ofbi->overlays[t-1] = ofbi->overlays[t];
@@ -300,7 +313,12 @@ static ssize_t store_overlays(struct device *dev, struct device_attribute *attr,
         }
 
         if (added) {
+                omapfb_get_mem_region(ofbi->region);
+
                 r = omapfb_apply_changes(fbi, 0);
+
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r)
                         goto out;
         }
@@ -388,7 +406,12 @@ static ssize_t store_overlays_rotate(struct device *dev,
                 for (i = 0; i < num_ovls; ++i)
                         ofbi->rotation[i] = rotation[i];
 
+                omapfb_get_mem_region(ofbi->region);
+
                 r = omapfb_apply_changes(fbi, 0);
+
+                omapfb_put_mem_region(ofbi->region);
+
                 if (r)
                         goto out;
 
@@ -408,7 +431,7 @@ static ssize_t show_size(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
 
-        return snprintf(buf, PAGE_SIZE, "%lu\n", ofbi->region.size);
+        return snprintf(buf, PAGE_SIZE, "%lu\n", ofbi->region->size);
 }
 
 static ssize_t store_size(struct device *dev, struct device_attribute *attr,
@@ -416,6 +439,8 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
 {
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
+        struct omapfb2_device *fbdev = ofbi->fbdev;
+        struct omapfb2_mem_region *rg;
         unsigned long size;
         int r;
         int i;
@@ -425,15 +450,33 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
         if (!lock_fb_info(fbi))
                 return -ENODEV;
 
-        for (i = 0; i < ofbi->num_overlays; i++) {
-                if (ofbi->overlays[i]->info.enabled) {
-                        r = -EBUSY;
-                        goto out;
+        rg = ofbi->region;
+
+        down_write_nested(&rg->lock, rg->id);
+        atomic_inc(&rg->lock_count);
+
+        if (atomic_read(&rg->map_count)) {
+                r = -EBUSY;
+                goto out;
+        }
+
+        for (i = 0; i < fbdev->num_fbs; i++) {
+                struct omapfb_info *ofbi2 = FB2OFB(fbdev->fbs[i]);
+                int j;
+
+                if (ofbi2->region != rg)
+                        continue;
+
+                for (j = 0; j < ofbi2->num_overlays; j++) {
+                        if (ofbi2->overlays[j]->info.enabled) {
+                                r = -EBUSY;
+                                goto out;
+                        }
                 }
         }
 
-        if (size != ofbi->region.size) {
-                r = omapfb_realloc_fbmem(fbi, size, ofbi->region.type);
+        if (size != ofbi->region->size) {
+                r = omapfb_realloc_fbmem(fbi, size, ofbi->region->type);
                 if (r) {
                         dev_err(dev, "realloc fbmem failed\n");
                         goto out;
@@ -442,6 +485,9 @@ static ssize_t store_size(struct device *dev, struct device_attribute *attr,
 
         r = count;
 out:
+        atomic_dec(&rg->lock_count);
+        up_write(&rg->lock);
+
         unlock_fb_info(fbi);
 
         return r;
@@ -453,7 +499,7 @@ static ssize_t show_phys(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
 
-        return snprintf(buf, PAGE_SIZE, "%0x\n", ofbi->region.paddr);
+        return snprintf(buf, PAGE_SIZE, "%0x\n", ofbi->region->paddr);
 }
 
 static ssize_t show_virt(struct device *dev,
@@ -462,7 +508,7 @@ static ssize_t show_virt(struct device *dev,
         struct fb_info *fbi = dev_get_drvdata(dev);
         struct omapfb_info *ofbi = FB2OFB(fbi);
 
-        return snprintf(buf, PAGE_SIZE, "%p\n", ofbi->region.vaddr);
+        return snprintf(buf, PAGE_SIZE, "%p\n", ofbi->region->vaddr);
 }
 
 static struct device_attribute omapfb_attrs[] = {
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index cd54fdbfd8bb..1305fc9880ba 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -27,6 +27,8 @@
 #define DEBUG
 #endif
 
+#include <linux/rwsem.h>
+
 #include <plat/display.h>
 
 #ifdef DEBUG
@@ -44,6 +46,7 @@ extern unsigned int omapfb_debug;
 #define OMAPFB_MAX_OVL_PER_FB 3
 
 struct omapfb2_mem_region {
+        int             id;
         u32             paddr;
         void __iomem    *vaddr;
         struct vrfb     vrfb;
@@ -51,13 +54,15 @@ struct omapfb2_mem_region {
         u8              type;           /* OMAPFB_PLANE_MEM_* */
         bool            alloc;          /* allocated by the driver */
         bool            map;            /* kernel mapped by the driver */
+        atomic_t        map_count;
+        struct rw_semaphore lock;
+        atomic_t        lock_count;
 };
 
 /* appended to fb_info */
 struct omapfb_info {
         int id;
-        struct omapfb2_mem_region region;
-        atomic_t map_count;
+        struct omapfb2_mem_region *region;
         int num_overlays;
         struct omap_overlay *overlays[OMAPFB_MAX_OVL_PER_FB];
         struct omapfb2_device *fbdev;
@@ -76,6 +81,7 @@ struct omapfb2_device {
 
         unsigned num_fbs;
         struct fb_info *fbs[10];
+        struct omapfb2_mem_region regions[10];
 
         unsigned num_displays;
         struct omap_dss_device *displays[10];
@@ -117,6 +123,9 @@ int omapfb_update_window(struct fb_info *fbi,
 int dss_mode_to_fb_mode(enum omap_color_mode dssmode,
                         struct fb_var_screeninfo *var);
 
+int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
+                u16 posx, u16 posy, u16 outw, u16 outh);
+
 /* find the display connected to this fb, if any */
 static inline struct omap_dss_device *fb2display(struct fb_info *fbi)
 {
@@ -148,8 +157,24 @@ static inline int omapfb_overlay_enable(struct omap_overlay *ovl,
         struct omap_overlay_info info;
 
         ovl->get_overlay_info(ovl, &info);
+        if (info.enabled == enable)
+                return 0;
         info.enabled = enable;
         return ovl->set_overlay_info(ovl, &info);
 }
 
+static inline struct omapfb2_mem_region *
+omapfb_get_mem_region(struct omapfb2_mem_region *rg)
+{
+        down_read_nested(&rg->lock, rg->id);
+        atomic_inc(&rg->lock_count);
+        return rg;
+}
+
+static inline void omapfb_put_mem_region(struct omapfb2_mem_region *rg)
+{
+        atomic_dec(&rg->lock_count);
+        up_read(&rg->lock);
+}
+
 #endif
diff --git a/drivers/video/via/chip.h b/drivers/video/via/chip.h
index d9b6e06e0700..ef1f3de2e052 100644
--- a/drivers/video/via/chip.h
+++ b/drivers/video/via/chip.h
@@ -160,7 +160,6 @@ struct lvds_setting_information {
         int v_active;
         int bpp;
         int refresh_rate;
-        int get_lcd_size_method;
         int lcd_panel_id;
         int lcd_panel_hres;
         int lcd_panel_vres;
diff --git a/drivers/video/via/hw.c b/drivers/video/via/hw.c
index b996803ae2c1..7dcb4d5bb9c3 100644
--- a/drivers/video/via/hw.c
+++ b/drivers/video/via/hw.c
@@ -23,143 +23,341 @@
 #include "global.h"
 
 static struct pll_map pll_value[] = {
-        {CLK_25_175M, CLE266_PLL_25_175M, K800_PLL_25_175M,
-         CX700_25_175M, VX855_25_175M},
-        {CLK_29_581M, CLE266_PLL_29_581M, K800_PLL_29_581M,
-         CX700_29_581M, VX855_29_581M},
-        {CLK_26_880M, CLE266_PLL_26_880M, K800_PLL_26_880M,
-         CX700_26_880M, VX855_26_880M},
-        {CLK_31_490M, CLE266_PLL_31_490M, K800_PLL_31_490M,
-         CX700_31_490M, VX855_31_490M},
-        {CLK_31_500M, CLE266_PLL_31_500M, K800_PLL_31_500M,
-         CX700_31_500M, VX855_31_500M},
-        {CLK_31_728M, CLE266_PLL_31_728M, K800_PLL_31_728M,
-         CX700_31_728M, VX855_31_728M},
-        {CLK_32_668M, CLE266_PLL_32_668M, K800_PLL_32_668M,
-         CX700_32_668M, VX855_32_668M},
-        {CLK_36_000M, CLE266_PLL_36_000M, K800_PLL_36_000M,
-         CX700_36_000M, VX855_36_000M},
-        {CLK_40_000M, CLE266_PLL_40_000M, K800_PLL_40_000M,
-         CX700_40_000M, VX855_40_000M},
-        {CLK_41_291M, CLE266_PLL_41_291M, K800_PLL_41_291M,
-         CX700_41_291M, VX855_41_291M},
-        {CLK_43_163M, CLE266_PLL_43_163M, K800_PLL_43_163M,
-         CX700_43_163M, VX855_43_163M},
-        {CLK_45_250M, CLE266_PLL_45_250M, K800_PLL_45_250M,
-         CX700_45_250M, VX855_45_250M},
-        {CLK_46_000M, CLE266_PLL_46_000M, K800_PLL_46_000M,
-         CX700_46_000M, VX855_46_000M},
-        {CLK_46_996M, CLE266_PLL_46_996M, K800_PLL_46_996M,
-         CX700_46_996M, VX855_46_996M},
-        {CLK_48_000M, CLE266_PLL_48_000M, K800_PLL_48_000M,
-         CX700_48_000M, VX855_48_000M},
-        {CLK_48_875M, CLE266_PLL_48_875M, K800_PLL_48_875M,
-         CX700_48_875M, VX855_48_875M},
-        {CLK_49_500M, CLE266_PLL_49_500M, K800_PLL_49_500M,
-         CX700_49_500M, VX855_49_500M},
-        {CLK_52_406M, CLE266_PLL_52_406M, K800_PLL_52_406M,
-         CX700_52_406M, VX855_52_406M},
-        {CLK_52_977M, CLE266_PLL_52_977M, K800_PLL_52_977M,
-         CX700_52_977M, VX855_52_977M},
-        {CLK_56_250M, CLE266_PLL_56_250M, K800_PLL_56_250M,
-         CX700_56_250M, VX855_56_250M},
-        {CLK_57_275M, 0, 0, 0, VX855_57_275M},
-        {CLK_60_466M, CLE266_PLL_60_466M, K800_PLL_60_466M,
-         CX700_60_466M, VX855_60_466M},
-        {CLK_61_500M, CLE266_PLL_61_500M, K800_PLL_61_500M,
-         CX700_61_500M, VX855_61_500M},
-        {CLK_65_000M, CLE266_PLL_65_000M, K800_PLL_65_000M,
-         CX700_65_000M, VX855_65_000M},
-        {CLK_65_178M, CLE266_PLL_65_178M, K800_PLL_65_178M,
-         CX700_65_178M, VX855_65_178M},
-        {CLK_66_750M, CLE266_PLL_66_750M, K800_PLL_66_750M,
-         CX700_66_750M, VX855_66_750M},
-        {CLK_68_179M, CLE266_PLL_68_179M, K800_PLL_68_179M,
-         CX700_68_179M, VX855_68_179M},
-        {CLK_69_924M, CLE266_PLL_69_924M, K800_PLL_69_924M,
-         CX700_69_924M, VX855_69_924M},
-        {CLK_70_159M, CLE266_PLL_70_159M, K800_PLL_70_159M,
-         CX700_70_159M, VX855_70_159M},
-        {CLK_72_000M, CLE266_PLL_72_000M, K800_PLL_72_000M,
-         CX700_72_000M, VX855_72_000M},
-        {CLK_78_750M, CLE266_PLL_78_750M, K800_PLL_78_750M,
-         CX700_78_750M, VX855_78_750M},
-        {CLK_80_136M, CLE266_PLL_80_136M, K800_PLL_80_136M,
-         CX700_80_136M, VX855_80_136M},
-        {CLK_83_375M, CLE266_PLL_83_375M, K800_PLL_83_375M,
-         CX700_83_375M, VX855_83_375M},
-        {CLK_83_950M, CLE266_PLL_83_950M, K800_PLL_83_950M,
-         CX700_83_950M, VX855_83_950M},
-        {CLK_84_750M, CLE266_PLL_84_750M, K800_PLL_84_750M,
-         CX700_84_750M, VX855_84_750M},
-        {CLK_85_860M, CLE266_PLL_85_860M, K800_PLL_85_860M,
-         CX700_85_860M, VX855_85_860M},
-        {CLK_88_750M, CLE266_PLL_88_750M, K800_PLL_88_750M,
-         CX700_88_750M, VX855_88_750M},
-        {CLK_94_500M, CLE266_PLL_94_500M, K800_PLL_94_500M,
-         CX700_94_500M, VX855_94_500M},
-        {CLK_97_750M, CLE266_PLL_97_750M, K800_PLL_97_750M,
-         CX700_97_750M, VX855_97_750M},
-        {CLK_101_000M, CLE266_PLL_101_000M, K800_PLL_101_000M,
-         CX700_101_000M, VX855_101_000M},
-        {CLK_106_500M, CLE266_PLL_106_500M, K800_PLL_106_500M,
-         CX700_106_500M, VX855_106_500M},
-        {CLK_108_000M, CLE266_PLL_108_000M, K800_PLL_108_000M,
-         CX700_108_000M, VX855_108_000M},
-        {CLK_113_309M, CLE266_PLL_113_309M, K800_PLL_113_309M,
-         CX700_113_309M, VX855_113_309M},
-        {CLK_118_840M, CLE266_PLL_118_840M, K800_PLL_118_840M,
-         CX700_118_840M, VX855_118_840M},
-        {CLK_119_000M, CLE266_PLL_119_000M, K800_PLL_119_000M,
-         CX700_119_000M, VX855_119_000M},
-        {CLK_121_750M, CLE266_PLL_121_750M, K800_PLL_121_750M,
-         CX700_121_750M, 0},
-        {CLK_125_104M, CLE266_PLL_125_104M, K800_PLL_125_104M,
-         CX700_125_104M, 0},
-        {CLK_133_308M, CLE266_PLL_133_308M, K800_PLL_133_308M,
-         CX700_133_308M, 0},
-        {CLK_135_000M, CLE266_PLL_135_000M, K800_PLL_135_000M,
-         CX700_135_000M, VX855_135_000M},
-        {CLK_136_700M, CLE266_PLL_136_700M, K800_PLL_136_700M,
-         CX700_136_700M, VX855_136_700M},
-        {CLK_138_400M, CLE266_PLL_138_400M, K800_PLL_138_400M,
-         CX700_138_400M, VX855_138_400M},
-        {CLK_146_760M, CLE266_PLL_146_760M, K800_PLL_146_760M,
-         CX700_146_760M, VX855_146_760M},
-        {CLK_153_920M, CLE266_PLL_153_920M, K800_PLL_153_920M,
-         CX700_153_920M, VX855_153_920M},
-        {CLK_156_000M, CLE266_PLL_156_000M, K800_PLL_156_000M,
-         CX700_156_000M, VX855_156_000M},
-        {CLK_157_500M, CLE266_PLL_157_500M, K800_PLL_157_500M,
-         CX700_157_500M, VX855_157_500M},
-        {CLK_162_000M, CLE266_PLL_162_000M, K800_PLL_162_000M,
-         CX700_162_000M, VX855_162_000M},
-        {CLK_187_000M, CLE266_PLL_187_000M, K800_PLL_187_000M,
-         CX700_187_000M, VX855_187_000M},
-        {CLK_193_295M, CLE266_PLL_193_295M, K800_PLL_193_295M,
-         CX700_193_295M, VX855_193_295M},
-        {CLK_202_500M, CLE266_PLL_202_500M, K800_PLL_202_500M,
-         CX700_202_500M, VX855_202_500M},
-        {CLK_204_000M, CLE266_PLL_204_000M, K800_PLL_204_000M,
-         CX700_204_000M, VX855_204_000M},
-        {CLK_218_500M, CLE266_PLL_218_500M, K800_PLL_218_500M,
-         CX700_218_500M, VX855_218_500M},
-        {CLK_234_000M, CLE266_PLL_234_000M, K800_PLL_234_000M,
-         CX700_234_000M, VX855_234_000M},
-        {CLK_267_250M, CLE266_PLL_267_250M, K800_PLL_267_250M,
-         CX700_267_250M, VX855_267_250M},
-        {CLK_297_500M, CLE266_PLL_297_500M, K800_PLL_297_500M,
-         CX700_297_500M, VX855_297_500M},
-        {CLK_74_481M, CLE266_PLL_74_481M, K800_PLL_74_481M,
-         CX700_74_481M, VX855_74_481M},
-        {CLK_172_798M, CLE266_PLL_172_798M, K800_PLL_172_798M,
-         CX700_172_798M, VX855_172_798M},
-        {CLK_122_614M, CLE266_PLL_122_614M, K800_PLL_122_614M,
-         CX700_122_614M, VX855_122_614M},
-        {CLK_74_270M, CLE266_PLL_74_270M, K800_PLL_74_270M,
-         CX700_74_270M, 0},
-        {CLK_148_500M, CLE266_PLL_148_500M, K800_PLL_148_500M,
-         CX700_148_500M, VX855_148_500M}
+        {25175000,
+                {99, 7, 3},
+                {85, 3, 4},     /* ignoring bit difference: 0x00008000 */
+                {141, 5, 4},
+                {141, 5, 4} },
+        {29581000,
+                {33, 4, 2},
+                {66, 2, 4},     /* ignoring bit difference: 0x00808000 */
+                {166, 5, 4},    /* ignoring bit difference: 0x00008000 */
+                {165, 5, 4} },
+        {26880000,
+                {15, 4, 1},
+                {30, 2, 3},     /* ignoring bit difference: 0x00808000 */
+                {150, 5, 4},
+                {150, 5, 4} },
+        {31500000,
+                {53, 3, 3},     /* ignoring bit difference: 0x00008000 */
+                {141, 4, 4},    /* ignoring bit difference: 0x00008000 */
+                {176, 5, 4},
+                {176, 5, 4} },
+        {31728000,
+                {31, 7, 1},
+                {177, 5, 4},    /* ignoring bit difference: 0x00008000 */
+                {177, 5, 4},
+                {142, 4, 4} },
+        {32688000,
+                {73, 4, 3},
+                {146, 4, 4},    /* ignoring bit difference: 0x00008000 */
+                {183, 5, 4},
+                {146, 4, 4} },
+        {36000000,
+                {101, 5, 3},    /* ignoring bit difference: 0x00008000 */
+                {161, 4, 4},    /* ignoring bit difference: 0x00008000 */
+                {202, 5, 4},
+                {161, 4, 4} },
+        {40000000,
+                {89, 4, 3},
+                {89, 4, 3},     /* ignoring bit difference: 0x00008000 */
+                {112, 5, 3},
+                {112, 5, 3} },
+        {41291000,
+                {23, 4, 1},
+                {69, 3, 3},     /* ignoring bit difference: 0x00008000 */
+                {115, 5, 3},
+                {115, 5, 3} },
+        {43163000,
+                {121, 5, 3},
+                {121, 5, 3},    /* ignoring bit difference: 0x00008000 */
+                {121, 5, 3},
+                {121, 5, 3} },
+        {45250000,
+                {127, 5, 3},
+                {127, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {127, 5, 3},
+                {127, 5, 3} },
+        {46000000,
+                {90, 7, 2},
+                {103, 4, 3},    /* ignoring bit difference: 0x00008000 */
+                {129, 5, 3},
+                {103, 4, 3} },
+        {46996000,
+                {105, 4, 3},    /* ignoring bit difference: 0x00008000 */
+                {131, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {131, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {105, 4, 3} },
+        {48000000,
+                {67, 20, 0},
+                {134, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {134, 5, 3},
+                {134, 5, 3} },
+        {48875000,
+                {99, 29, 0},
+                {82, 3, 3},     /* ignoring bit difference: 0x00808000 */
+                {82, 3, 3},     /* ignoring bit difference: 0x00808000 */
+                {137, 5, 3} },
+        {49500000,
+                {83, 6, 2},
+                {83, 3, 3},     /* ignoring bit difference: 0x00008000 */
+                {138, 5, 3},
+                {83, 3, 3} },
+        {52406000,
+                {117, 4, 3},
+                {117, 4, 3},    /* ignoring bit difference: 0x00008000 */
+                {117, 4, 3},
+                {88, 3, 3} },
+        {52977000,
+                {37, 5, 1},
+                {148, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {148, 5, 3},
+                {148, 5, 3} },
+        {56250000,
+                {55, 7, 1},     /* ignoring bit difference: 0x00008000 */
+                {126, 4, 3},    /* ignoring bit difference: 0x00008000 */
+                {157, 5, 3},
+                {157, 5, 3} },
+        {57275000,
+                {0, 0, 0},
+                {2, 2, 0},
+                {2, 2, 0},
+                {157, 5, 3} },  /* ignoring bit difference: 0x00808000 */
+        {60466000,
+                {76, 9, 1},
+                {169, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {169, 5, 3},    /* FIXED: old = {72, 2, 3} */
+                {169, 5, 3} },
+        {61500000,
+                {86, 20, 0},
+                {172, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {172, 5, 3},
+                {172, 5, 3} },
+        {65000000,
+                {109, 6, 2},    /* ignoring bit difference: 0x00008000 */
+                {109, 3, 3},    /* ignoring bit difference: 0x00008000 */
+                {109, 3, 3},
+                {109, 3, 3} },
+        {65178000,
+                {91, 5, 2},
+                {182, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {109, 3, 3},
+                {182, 5, 3} },
+        {66750000,
+                {75, 4, 2},
+                {150, 4, 3},    /* ignoring bit difference: 0x00808000 */
+                {150, 4, 3},
+                {112, 3, 3} },
+        {68179000,
+                {19, 4, 0},
+                {114, 3, 3},    /* ignoring bit difference: 0x00008000 */
+                {190, 5, 3},
+                {191, 5, 3} },
+        {69924000,
+                {83, 17, 0},
+                {195, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {195, 5, 3},
+                {195, 5, 3} },
+        {70159000,
+                {98, 20, 0},
+                {196, 5, 3},    /* ignoring bit difference: 0x00808000 */
+                {196, 5, 3},
+                {195, 5, 3} },
+        {72000000,
+                {121, 24, 0},
+                {161, 4, 3},    /* ignoring bit difference: 0x00808000 */
+                {161, 4, 3},
+                {161, 4, 3} },
+        {78750000,
+                {33, 3, 1},
+                {66, 3, 2},     /* ignoring bit difference: 0x00008000 */
+                {110, 5, 2},
+                {110, 5, 2} },
+        {80136000,
+                {28, 5, 0},
+                {68, 3, 2},     /* ignoring bit difference: 0x00008000 */
+                {112, 5, 2},
+                {112, 5, 2} },
+        {83375000,
+                {93, 2, 3},
+                {93, 4, 2},     /* ignoring bit difference: 0x00800000 */
+                {93, 4, 2},     /* ignoring bit difference: 0x00800000 */
+                {117, 5, 2} },
+        {83950000,
+                {41, 7, 0},
+                {117, 5, 2},    /* ignoring bit difference: 0x00008000 */
+                {117, 5, 2},
+                {117, 5, 2} },
+        {84750000,
+                {118, 5, 2},
+                {118, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {118, 5, 2},
+                {118, 5, 2} },
+        {85860000,
+                {84, 7, 1},
+                {120, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {120, 5, 2},
+                {118, 5, 2} },
+        {88750000,
+                {31, 5, 0},
+                {124, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {174, 7, 2},    /* ignoring bit difference: 0x00808000 */
+                {124, 5, 2} },
+        {94500000,
+                {33, 5, 0},
+                {132, 5, 2},    /* ignoring bit difference: 0x00008000 */
+                {132, 5, 2},
+                {132, 5, 2} },
+        {97750000,
+                {82, 6, 1},
+                {137, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {137, 5, 2},
+                {137, 5, 2} },
+        {101000000,
+                {127, 9, 1},
+                {141, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {141, 5, 2},
+                {141, 5, 2} },
+        {106500000,
+                {119, 4, 2},
+                {119, 4, 2},    /* ignoring bit difference: 0x00808000 */
+                {119, 4, 2},
+                {149, 5, 2} },
+        {108000000,
+                {121, 4, 2},
+                {121, 4, 2},    /* ignoring bit difference: 0x00808000 */
+                {151, 5, 2},
+                {151, 5, 2} },
+        {113309000,
+                {95, 12, 0},
+                {95, 3, 2},     /* ignoring bit difference: 0x00808000 */
+                {95, 3, 2},
+                {159, 5, 2} },
+        {118840000,
+                {83, 5, 1},
+                {166, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {166, 5, 2},
+                {166, 5, 2} },
+        {119000000,
+                {108, 13, 0},
+                {133, 4, 2},    /* ignoring bit difference: 0x00808000 */
+                {133, 4, 2},
+                {167, 5, 2} },
+        {121750000,
+                {85, 5, 1},
+                {170, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {68, 2, 2},
+                {0, 0, 0} },
+        {125104000,
+                {53, 6, 0},     /* ignoring bit difference: 0x00008000 */
+                {106, 3, 2},    /* ignoring bit difference: 0x00008000 */
+                {175, 5, 2},
+                {0, 0, 0} },
+        {135000000,
+                {94, 5, 1},
+                {28, 3, 0},     /* ignoring bit difference: 0x00804000 */
+                {151, 4, 2},
+                {189, 5, 2} },
+        {136700000,
+                {115, 12, 0},
+                {191, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {191, 5, 2},
+                {191, 5, 2} },
+        {138400000,
+                {87, 9, 0},
+                {116, 3, 2},    /* ignoring bit difference: 0x00808000 */
+                {116, 3, 2},
+                {194, 5, 2} },
+        {146760000,
+                {103, 5, 1},
+                {206, 5, 2},    /* ignoring bit difference: 0x00808000 */
+                {206, 5, 2},
+                {206, 5, 2} },
+        {153920000,
+                {86, 8, 0},
+                {86, 4, 1},     /* ignoring bit difference: 0x00808000 */
+                {86, 4, 1},
+                {86, 4, 1} },   /* FIXED: old = {84, 2, 1} */
+        {156000000,
+                {109, 5, 1},
+                {109, 5, 1},    /* ignoring bit difference: 0x00808000 */
+                {109, 5, 1},
+                {108, 5, 1} },
+        {157500000,
+                {55, 5, 0},     /* ignoring bit difference: 0x00008000 */
+                {22, 2, 0},     /* ignoring bit difference: 0x00802000 */
+                {110, 5, 1},
+                {110, 5, 1} },
+        {162000000,
+                {113, 5, 1},
+                {113, 5, 1},    /* ignoring bit difference: 0x00808000 */
+                {113, 5, 1},
+                {113, 5, 1} },
+        {187000000,
+                {118, 9, 0},
+                {131, 5, 1},    /* ignoring bit difference: 0x00808000 */
+                {131, 5, 1},
+                {131, 5, 1} },
+        {193295000,
+                {108, 8, 0},
+                {81, 3, 1},     /* ignoring bit difference: 0x00808000 */
+                {135, 5, 1},
+                {135, 5, 1} },
+        {202500000,
+                {99, 7, 0},
+                {85, 3, 1},     /* ignoring bit difference: 0x00808000 */
+                {142, 5, 1},
+                {142, 5, 1} },
+        {204000000,
+                {100, 7, 0},
+                {143, 5, 1},    /* ignoring bit difference: 0x00808000 */
+                {143, 5, 1},
+                {143, 5, 1} },
+        {218500000,
+                {92, 6, 0},
+                {153, 5, 1},    /* ignoring bit difference: 0x00808000 */
+                {153, 5, 1},
+                {153, 5, 1} },
+        {234000000,
+                {98, 6, 0},
+                {98, 3, 1},     /* ignoring bit difference: 0x00008000 */
+                {98, 3, 1},
+                {164, 5, 1} },
+        {267250000,
+                {112, 6, 0},
+                {112, 3, 1},    /* ignoring bit difference: 0x00808000 */
+                {187, 5, 1},
+                {187, 5, 1} },
+        {297500000,
+                {102, 5, 0},    /* ignoring bit difference: 0x00008000 */
+                {166, 4, 1},    /* ignoring bit difference: 0x00008000 */
+                {208, 5, 1},
+                {208, 5, 1} },
+        {74481000,
+                {26, 5, 0},
+                {125, 3, 3},    /* ignoring bit difference: 0x00808000 */
+                {208, 5, 3},
+                {209, 5, 3} },
+        {172798000,
+                {121, 5, 1},
+                {121, 5, 1},    /* ignoring bit difference: 0x00808000 */
+                {121, 5, 1},
+                {121, 5, 1} },
+        {122614000,
+                {60, 7, 0},
+                {137, 4, 2},    /* ignoring bit difference: 0x00808000 */
+                {137, 4, 2},
+                {172, 5, 2} },
+        {74270000,
+                {83, 8, 1},
+                {208, 5, 3},
+                {208, 5, 3},
+                {0, 0, 0} },
+        {148500000,
+                {83, 8, 0},
+                {208, 5, 2},
+                {166, 4, 2},
+                {208, 5, 2} }
 };
 
 static struct fifo_depth_select display_fifo_depth_reg = {
@@ -1360,40 +1558,70 @@ void viafb_load_FIFO_reg(int set_iga, int hor_active, int ver_active)
 
 }
 
+static u32 cle266_encode_pll(struct pll_config pll)
+{
+        return (pll.multiplier << 8)
+                | (pll.rshift << 6)
+                | pll.divisor;
+}
+
+static u32 k800_encode_pll(struct pll_config pll)
+{
+        return ((pll.divisor - 2) << 16)
+                | (pll.rshift << 10)
+                | (pll.multiplier - 2);
+}
+
+static u32 vx855_encode_pll(struct pll_config pll)
+{
+        return (pll.divisor << 16)
+                | (pll.rshift << 10)
+                | pll.multiplier;
+}
+
 u32 viafb_get_clk_value(int clk)
 {
-        int i;
+        u32 value = 0;
+        int i = 0;
 
-        for (i = 0; i < NUM_TOTAL_PLL_TABLE; i++) {
-                if (clk == pll_value[i].clk) {
-                        switch (viaparinfo->chip_info->gfx_chip_name) {
-                        case UNICHROME_CLE266:
-                        case UNICHROME_K400:
-                                return pll_value[i].cle266_pll;
-
-                        case UNICHROME_K800:
-                        case UNICHROME_PM800:
-                        case UNICHROME_CN700:
-                                return pll_value[i].k800_pll;
-
-                        case UNICHROME_CX700:
-                        case UNICHROME_K8M890:
-                        case UNICHROME_P4M890:
-                        case UNICHROME_P4M900:
-                        case UNICHROME_VX800:
-                                return pll_value[i].cx700_pll;
-                        case UNICHROME_VX855:
-                                return pll_value[i].vx855_pll;
-                        }
+        while (i < NUM_TOTAL_PLL_TABLE && clk != pll_value[i].clk)
+                i++;
+
+        if (i == NUM_TOTAL_PLL_TABLE) {
+                printk(KERN_WARNING "viafb_get_clk_value: PLL lookup failed!");
+        } else {
+                switch (viaparinfo->chip_info->gfx_chip_name) {
+                case UNICHROME_CLE266:
+                case UNICHROME_K400:
+                        value = cle266_encode_pll(pll_value[i].cle266_pll);
+                        break;
+
+                case UNICHROME_K800:
+                case UNICHROME_PM800:
+                case UNICHROME_CN700:
+                        value = k800_encode_pll(pll_value[i].k800_pll);
+                        break;
+
+                case UNICHROME_CX700:
+                case UNICHROME_CN750:
+                case UNICHROME_K8M890:
+                case UNICHROME_P4M890:
+                case UNICHROME_P4M900:
+                case UNICHROME_VX800:
+                        value = k800_encode_pll(pll_value[i].cx700_pll);
+                        break;
+
+                case UNICHROME_VX855:
+                        value = vx855_encode_pll(pll_value[i].vx855_pll);
+                        break;
                 }
         }
 
-        DEBUG_MSG(KERN_INFO "Can't find match PLL value\n\n");
-        return 0;
+        return value;
 }
 
 /* Set VCLK*/
-void viafb_set_vclock(u32 CLK, int set_iga)
+void viafb_set_vclock(u32 clk, int set_iga)
 {
         /* H.W. Reset : ON */
         viafb_write_reg_mask(CR17, VIACR, 0x00, BIT7);
@@ -1403,26 +1631,23 @@ void viafb_set_vclock(u32 CLK, int set_iga)
                 switch (viaparinfo->chip_info->gfx_chip_name) {
                 case UNICHROME_CLE266:
                 case UNICHROME_K400:
-                        viafb_write_reg(SR46, VIASR, CLK / 0x100);
-                        viafb_write_reg(SR47, VIASR, CLK % 0x100);
+                        via_write_reg(VIASR, SR46, (clk & 0x00FF));
+                        via_write_reg(VIASR, SR47, (clk & 0xFF00) >> 8);
                         break;
 
                 case UNICHROME_K800:
                 case UNICHROME_PM800:
                 case UNICHROME_CN700:
                 case UNICHROME_CX700:
+                case UNICHROME_CN750:
                 case UNICHROME_K8M890:
                 case UNICHROME_P4M890:
                 case UNICHROME_P4M900:
                 case UNICHROME_VX800:
                 case UNICHROME_VX855:
-                        viafb_write_reg(SR44, VIASR, CLK / 0x10000);
-                        DEBUG_MSG(KERN_INFO "\nSR44=%x", CLK / 0x10000);
-                        viafb_write_reg(SR45, VIASR, (CLK & 0xFFFF) / 0x100);
-                        DEBUG_MSG(KERN_INFO "\nSR45=%x",
-                                  (CLK & 0xFFFF) / 0x100);
-                        viafb_write_reg(SR46, VIASR, CLK % 0x100);
-                        DEBUG_MSG(KERN_INFO "\nSR46=%x", CLK % 0x100);
+                        via_write_reg(VIASR, SR44, (clk & 0x0000FF));
+                        via_write_reg(VIASR, SR45, (clk & 0x00FF00) >> 8);
+                        via_write_reg(VIASR, SR46, (clk & 0xFF0000) >> 16);
                         break;
                 }
         }
@@ -1432,22 +1657,23 @@ void viafb_set_vclock(u32 CLK, int set_iga)
                 switch (viaparinfo->chip_info->gfx_chip_name) {
                 case UNICHROME_CLE266:
                 case UNICHROME_K400:
-                        viafb_write_reg(SR44, VIASR, CLK / 0x100);
-                        viafb_write_reg(SR45, VIASR, CLK % 0x100);
+                        via_write_reg(VIASR, SR44, (clk & 0x00FF));
+                        via_write_reg(VIASR, SR45, (clk & 0xFF00) >> 8);
                         break;
 
                 case UNICHROME_K800:
                 case UNICHROME_PM800:
                 case UNICHROME_CN700:
                 case UNICHROME_CX700:
+                case UNICHROME_CN750:
                 case UNICHROME_K8M890:
                 case UNICHROME_P4M890:
                 case UNICHROME_P4M900:
                 case UNICHROME_VX800:
                 case UNICHROME_VX855:
-                        viafb_write_reg(SR4A, VIASR, CLK / 0x10000);
-                        viafb_write_reg(SR4B, VIASR, (CLK & 0xFFFF) / 0x100);
-                        viafb_write_reg(SR4C, VIASR, CLK % 0x100);
+                        via_write_reg(VIASR, SR4A, (clk & 0x0000FF));
+                        via_write_reg(VIASR, SR4B, (clk & 0x00FF00) >> 8);
+                        via_write_reg(VIASR, SR4C, (clk & 0xFF0000) >> 16);
                         break;
                 }
         }
@@ -1791,8 +2017,6 @@ void viafb_init_chip_info(int chip_type)
         viafb_set_iga_path();
 
         viaparinfo->lvds_setting_info->display_method = viafb_lcd_dsp_method;
-        viaparinfo->lvds_setting_info->get_lcd_size_method =
-                GET_LCD_SIZE_BY_USER_SETTING;
         viaparinfo->lvds_setting_info->lcd_mode = viafb_lcd_mode;
         viaparinfo->lvds_setting_info2->display_method =
                 viaparinfo->lvds_setting_info->display_method;
@@ -1946,13 +2170,6 @@ static void init_tmds_chip_info(void)
 
 static void init_lvds_chip_info(void)
 {
-        if (viafb_lcd_panel_id > LCD_PANEL_ID_MAXIMUM)
-                viaparinfo->lvds_setting_info->get_lcd_size_method =
-                    GET_LCD_SIZE_BY_VGA_BIOS;
-        else
-                viaparinfo->lvds_setting_info->get_lcd_size_method =
-                    GET_LCD_SIZE_BY_USER_SETTING;
-
         viafb_lvds_trasmitter_identify();
         viafb_init_lcd_size();
         viafb_init_lvds_output_interface(&viaparinfo->chip_info->lvds_chip_info,
diff --git a/drivers/video/via/hw.h b/drivers/video/via/hw.h
index a109de379816..c44399895294 100644
--- a/drivers/video/via/hw.h
+++ b/drivers/video/via/hw.h
@@ -700,12 +700,18 @@ struct _lcd_scaling_factor {
         struct _lcd_ver_scaling_factor lcd_ver_scaling_factor;
 };
 
+struct pll_config {
+        u16 multiplier;
+        u8 divisor;
+        u8 rshift;
+};
+
 struct pll_map {
         u32 clk;
-        u32 cle266_pll;
-        u32 k800_pll;
-        u32 cx700_pll;
-        u32 vx855_pll;
+        struct pll_config cle266_pll;
+        struct pll_config k800_pll;
+        struct pll_config cx700_pll;
+        struct pll_config vx855_pll;
 };
 
 struct rgbLUT {
diff --git a/drivers/video/via/ioctl.h b/drivers/video/via/ioctl.h
index c430fa23008a..6010d10b59e8 100644
--- a/drivers/video/via/ioctl.h
+++ b/drivers/video/via/ioctl.h
@@ -35,11 +35,9 @@
 #define VIAFB_GET_SAMM_INFO             0x56494107      /* 'VIA\07' */
 #define VIAFB_TURN_ON_OUTPUT_DEVICE     0x56494108      /* 'VIA\08' */
 #define VIAFB_TURN_OFF_OUTPUT_DEVICE    0x56494109      /* 'VIA\09' */
-#define VIAFB_SET_DEVICE                0x5649410A
 #define VIAFB_GET_DEVICE                0x5649410B
 #define VIAFB_GET_DRIVER_VERSION        0x56494112      /* 'VIA\12' */
 #define VIAFB_GET_CHIP_INFO             0x56494113      /* 'VIA\13' */
-#define VIAFB_SET_DEVICE_INFO           0x56494114
 #define VIAFB_GET_DEVICE_INFO           0x56494115
 
 #define VIAFB_GET_DEVICE_SUPPORT        0x56494118
@@ -50,7 +48,6 @@
 #define VIAFB_GET_GAMMA_LUT             0x56494124
 #define VIAFB_SET_GAMMA_LUT             0x56494125
 #define VIAFB_GET_GAMMA_SUPPORT_STATE   0x56494126
-#define VIAFB_SET_SECOND_MODE           0x56494129
 #define VIAFB_SYNC_SURFACE              0x56494130
 #define VIAFB_GET_DRIVER_CAPS           0x56494131
 #define VIAFB_GET_IGA_SCALING_INFO      0x56494132
diff --git a/drivers/video/via/lcd.c b/drivers/video/via/lcd.c
index 2ab0f156439a..fc25ae30c5f6 100644
--- a/drivers/video/via/lcd.c
+++ b/drivers/video/via/lcd.c
@@ -75,8 +75,6 @@ static void check_diport_of_integrated_lvds(
 static struct display_timing lcd_centering_timging(struct display_timing
                                             mode_crt_reg,
                                            struct display_timing panel_crt_reg);
-static void viafb_load_scaling_factor_for_p4m900(int set_hres,
-        int set_vres, int panel_hres, int panel_vres);
 
 static int check_lvds_chip(int device_id_subaddr, int device_id)
 {
@@ -89,33 +87,8 @@ static int check_lvds_chip(int device_id_subaddr, int device_id)
 void viafb_init_lcd_size(void)
 {
         DEBUG_MSG(KERN_INFO "viafb_init_lcd_size()\n");
-        DEBUG_MSG(KERN_INFO
-                "viaparinfo->lvds_setting_info->get_lcd_size_method %d\n",
-                viaparinfo->lvds_setting_info->get_lcd_size_method);
 
-        switch (viaparinfo->lvds_setting_info->get_lcd_size_method) {
-        case GET_LCD_SIZE_BY_SYSTEM_BIOS:
-                break;
-        case GET_LCD_SZIE_BY_HW_STRAPPING:
-                break;
-        case GET_LCD_SIZE_BY_VGA_BIOS:
-                DEBUG_MSG(KERN_INFO "Get LCD Size method by VGA BIOS !!\n");
-                fp_id_to_vindex(viafb_lcd_panel_id);
-                DEBUG_MSG(KERN_INFO "LCD Panel_ID = %d\n",
-                          viaparinfo->lvds_setting_info->lcd_panel_id);
-                break;
-        case GET_LCD_SIZE_BY_USER_SETTING:
-                DEBUG_MSG(KERN_INFO "Get LCD Size method by user setting !!\n");
-                fp_id_to_vindex(viafb_lcd_panel_id);
-                DEBUG_MSG(KERN_INFO "LCD Panel_ID = %d\n",
-                          viaparinfo->lvds_setting_info->lcd_panel_id);
-                break;
-        default:
-                DEBUG_MSG(KERN_INFO "viafb_init_lcd_size fail\n");
-                viaparinfo->lvds_setting_info->lcd_panel_id =
-                        LCD_PANEL_ID1_800X600;
-                fp_id_to_vindex(LCD_PANEL_ID1_800X600);
-        }
+        fp_id_to_vindex(viafb_lcd_panel_id);
         viaparinfo->lvds_setting_info2->lcd_panel_id =
                 viaparinfo->lvds_setting_info->lcd_panel_id;
         viaparinfo->lvds_setting_info2->lcd_panel_hres =
@@ -437,14 +410,9 @@ static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
 
         /* LCD Scaling Enable */
         viafb_write_reg_mask(CR79, VIACR, 0x07, BIT0 + BIT1 + BIT2);
-        if (UNICHROME_P4M900 == viaparinfo->chip_info->gfx_chip_name) {
-                viafb_load_scaling_factor_for_p4m900(set_hres, set_vres,
-                                               panel_hres, panel_vres);
-                return;
-        }
 
         /* Check if expansion for horizontal */
-        if (set_hres != panel_hres) {
+        if (set_hres < panel_hres) {
                 /* Load Horizontal Scaling Factor */
                 switch (viaparinfo->chip_info->gfx_chip_name) {
                 case UNICHROME_CLE266:
@@ -464,6 +432,10 @@ static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
                 case UNICHROME_CX700:
                 case UNICHROME_K8M890:
                 case UNICHROME_P4M890:
+                case UNICHROME_P4M900:
+                case UNICHROME_CN750:
+                case UNICHROME_VX800:
+                case UNICHROME_VX855:
                         reg_value =
                             K800_LCD_HOR_SCF_FORMULA(set_hres, panel_hres);
                         /* Horizontal scaling enabled */
@@ -483,7 +455,7 @@ static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
         }
 
         /* Check if expansion for vertical */
-        if (set_vres != panel_vres) {
+        if (set_vres < panel_vres) {
                 /* Load Vertical Scaling Factor */
                 switch (viaparinfo->chip_info->gfx_chip_name) {
                 case UNICHROME_CLE266:
@@ -503,6 +475,10 @@ static void load_lcd_scaling(int set_hres, int set_vres, int panel_hres,
                 case UNICHROME_CX700:
                 case UNICHROME_K8M890:
                 case UNICHROME_P4M890:
+                case UNICHROME_P4M900:
+                case UNICHROME_CN750:
+                case UNICHROME_VX800:
+                case UNICHROME_VX855:
                         reg_value =
                             K800_LCD_VER_SCF_FORMULA(set_vres, panel_vres);
                         /* Vertical scaling enabled */
@@ -648,9 +624,8 @@ void viafb_lcd_set_mode(struct crt_mode_table *mode_crt_table,
                                  (mode_crt_reg, panel_crt_reg), IGA1);
         } else {
                 /* Expansion */
-                if ((plvds_setting_info->display_method ==
-                     LCD_EXPANDSION) & ((set_hres != panel_hres)
-                                        || (set_vres != panel_vres))) {
+                if (plvds_setting_info->display_method == LCD_EXPANDSION
+                        && (set_hres < panel_hres || set_vres < panel_vres)) {
                         /* expansion timing IGA2 loaded panel set timing*/
                         viafb_load_crtc_timing(panel_crt_reg, IGA2);
                         DEBUG_MSG(KERN_INFO "viafb_load_crtc_timing!!\n");
@@ -1139,69 +1114,3 @@ bool viafb_lcd_get_mobile_state(bool *mobile)
                 return false;
         }
 }
-
-static void viafb_load_scaling_factor_for_p4m900(int set_hres,
-        int set_vres, int panel_hres, int panel_vres)
-{
-        int h_scaling_factor;
-        int v_scaling_factor;
-        u8 cra2 = 0;
-        u8 cr77 = 0;
-        u8 cr78 = 0;
-        u8 cr79 = 0;
-        u8 cr9f = 0;
-        /* Check if expansion for horizontal */
-        if (set_hres < panel_hres) {
-                /* Load Horizontal Scaling Factor */
-
-                /* For VIA_K8M800 or later chipsets. */
-                h_scaling_factor =
-                    K800_LCD_HOR_SCF_FORMULA(set_hres, panel_hres);
-                /* HSCaleFactor[1:0] at CR9F[1:0] */
-                cr9f = h_scaling_factor & 0x0003;
-                /* HSCaleFactor[9:2] at CR77[7:0] */
-                cr77 = (h_scaling_factor & 0x03FC) >> 2;
-                /* HSCaleFactor[11:10] at CR79[5:4] */
-                cr79 = (h_scaling_factor & 0x0C00) >> 10;
-                cr79 <<= 4;
-
-                /* Horizontal scaling enabled */
-                cra2 = 0xC0;
-
-                DEBUG_MSG(KERN_INFO "Horizontal Scaling value = %d\n",
-                          h_scaling_factor);
-        } else {
-                /* Horizontal scaling disabled */
-                cra2 = 0x00;
-        }
-
-        /* Check if expansion for vertical */
-        if (set_vres < panel_vres) {
-                /* Load Vertical Scaling Factor */
-
-                /* For VIA_K8M800 or later chipsets. */
-                v_scaling_factor =
-                    K800_LCD_VER_SCF_FORMULA(set_vres, panel_vres);
-
-                /* Vertical scaling enabled */
-                cra2 |= 0x08;
-                /* VSCaleFactor[0] at CR79[3] */
-                cr79 |= ((v_scaling_factor & 0x0001) << 3);
-                /* VSCaleFactor[8:1] at CR78[7:0] */
-                cr78 |= (v_scaling_factor & 0x01FE) >> 1;
-                /* VSCaleFactor[10:9] at CR79[7:6] */
-                cr79 |= ((v_scaling_factor & 0x0600) >> 9) << 6;
-
-                DEBUG_MSG(KERN_INFO "Vertical Scaling value = %d\n",
-                          v_scaling_factor);
-        } else {
-                /* Vertical scaling disabled */
-                cra2 |= 0x00;
-        }
-
-        viafb_write_reg_mask(CRA2, VIACR, cra2, BIT3 + BIT6 + BIT7);
-        viafb_write_reg_mask(CR77, VIACR, cr77, 0xFF);
-        viafb_write_reg_mask(CR78, VIACR, cr78, 0xFF);
-        viafb_write_reg_mask(CR79, VIACR, cr79, 0xF8);
-        viafb_write_reg_mask(CR9F, VIACR, cr9f, BIT0 + BIT1);
-}
diff --git a/drivers/video/via/lcd.h b/drivers/video/via/lcd.h
index 9762ec62b495..b348efc360b8 100644
--- a/drivers/video/via/lcd.h
+++ b/drivers/video/via/lcd.h
@@ -28,11 +28,6 @@
 #define     VT3271_DEVICE_ID_REG        0x02
 #define     VT3271_DEVICE_ID            0x71
 
-#define     GET_LCD_SIZE_BY_SYSTEM_BIOS     0x01
-#define     GET_LCD_SIZE_BY_VGA_BIOS        0x02
-#define     GET_LCD_SZIE_BY_HW_STRAPPING    0x03
-#define     GET_LCD_SIZE_BY_USER_SETTING    0x04
-
 /* Definition DVI Panel ID*/
 /* Resolution: 640x480,   Channel: single, Dithering: Enable */
 #define     LCD_PANEL_ID0_640X480       0x00
diff --git a/drivers/video/via/share.h b/drivers/video/via/share.h
index 7f0de7f006ad..2cbe1031b421 100644
--- a/drivers/video/via/share.h
+++ b/drivers/video/via/share.h
@@ -631,7 +631,6 @@
 #define CLK_25_175M     25175000
 #define CLK_26_880M     26880000
 #define CLK_29_581M     29581000
-#define CLK_31_490M     31490000
 #define CLK_31_500M     31500000
 #define CLK_31_728M     31728000
 #define CLK_32_668M     32688000
@@ -676,7 +675,6 @@
 #define CLK_119_000M    119000000
 #define CLK_121_750M    121750000       /* 121.704MHz */
 #define CLK_125_104M    125104000
-#define CLK_133_308M    133308000
 #define CLK_135_000M    135000000
 #define CLK_136_700M    136700000
 #define CLK_138_400M    138400000
@@ -699,313 +697,6 @@
 #define CLK_172_798M    172798000
 #define CLK_122_614M    122614000
 
-/* CLE266 PLL value
-*/
-#define CLE266_PLL_25_175M     0x0000C763
-#define CLE266_PLL_26_880M     0x0000440F
-#define CLE266_PLL_29_581M     0x00008421
-#define CLE266_PLL_31_490M     0x00004721
-#define CLE266_PLL_31_500M     0x0000C3B5
-#define CLE266_PLL_31_728M     0x0000471F
-#define CLE266_PLL_32_668M     0x0000C449
-#define CLE266_PLL_36_000M     0x0000C5E5
-#define CLE266_PLL_40_000M     0x0000C459
-#define CLE266_PLL_41_291M     0x00004417
-#define CLE266_PLL_43_163M     0x0000C579
-#define CLE266_PLL_45_250M     0x0000C57F       /* 45.46MHz */
-#define CLE266_PLL_46_000M     0x0000875A
-#define CLE266_PLL_46_996M     0x0000C4E9
-#define CLE266_PLL_48_000M     0x00001443
-#define CLE266_PLL_48_875M     0x00001D63
-#define CLE266_PLL_49_500M     0x00008653
-#define CLE266_PLL_52_406M     0x0000C475
-#define CLE266_PLL_52_977M     0x00004525
-#define CLE266_PLL_56_250M     0x000047B7
-#define CLE266_PLL_60_466M     0x0000494C
-#define CLE266_PLL_61_500M     0x00001456
-#define CLE266_PLL_65_000M     0x000086ED
-#define CLE266_PLL_65_178M     0x0000855B
-#define CLE266_PLL_66_750M     0x0000844B       /* 67.116MHz */
-#define CLE266_PLL_68_179M     0x00000413
-#define CLE266_PLL_69_924M     0x00001153
-#define CLE266_PLL_70_159M     0x00001462
-#define CLE266_PLL_72_000M     0x00001879
-#define CLE266_PLL_74_270M     0x00004853
-#define CLE266_PLL_78_750M     0x00004321
-#define CLE266_PLL_80_136M     0x0000051C
-#define CLE266_PLL_83_375M     0x0000C25D
-#define CLE266_PLL_83_950M     0x00000729
-#define CLE266_PLL_84_750M     0x00008576       /* 84.537MHz */
-#define CLE266_PLL_85_860M     0x00004754
-#define CLE266_PLL_88_750M     0x0000051F
-#define CLE266_PLL_94_500M     0x00000521
-#define CLE266_PLL_97_750M     0x00004652
-#define CLE266_PLL_101_000M    0x0000497F
-#define CLE266_PLL_106_500M    0x00008477       /* 106.491463 MHz */
-#define CLE266_PLL_108_000M    0x00008479
-#define CLE266_PLL_113_309M    0x00000C5F
-#define CLE266_PLL_118_840M    0x00004553
-#define CLE266_PLL_119_000M    0x00000D6C
-#define CLE266_PLL_121_750M    0x00004555       /* 121.704MHz */
-#define CLE266_PLL_125_104M    0x000006B5
-#define CLE266_PLL_133_308M    0x0000465F
-#define CLE266_PLL_135_000M    0x0000455E
-#define CLE266_PLL_136_700M    0x00000C73
-#define CLE266_PLL_138_400M    0x00000957
-#define CLE266_PLL_146_760M    0x00004567
-#define CLE266_PLL_148_500M    0x00000853
-#define CLE266_PLL_153_920M    0x00000856
-#define CLE266_PLL_156_000M    0x0000456D
-#define CLE266_PLL_157_500M    0x000005B7
-#define CLE266_PLL_162_000M    0x00004571
-#define CLE266_PLL_187_000M    0x00000976
-#define CLE266_PLL_193_295M    0x0000086C
-#define CLE266_PLL_202_500M    0x00000763
-#define CLE266_PLL_204_000M    0x00000764
-#define CLE266_PLL_218_500M    0x0000065C
-#define CLE266_PLL_234_000M    0x00000662
-#define CLE266_PLL_267_250M    0x00000670
-#define CLE266_PLL_297_500M    0x000005E6
-#define CLE266_PLL_74_481M     0x0000051A
-#define CLE266_PLL_172_798M    0x00004579
-#define CLE266_PLL_122_614M    0x0000073C
-
-/* K800 PLL value
-*/
-#define K800_PLL_25_175M     0x00539001
-#define K800_PLL_26_880M     0x001C8C80
-#define K800_PLL_29_581M     0x00409080
-#define K800_PLL_31_490M     0x006F9001
-#define K800_PLL_31_500M     0x008B9002
-#define K800_PLL_31_728M     0x00AF9003
-#define K800_PLL_32_668M     0x00909002
-#define K800_PLL_36_000M     0x009F9002
-#define K800_PLL_40_000M     0x00578C02
-#define K800_PLL_41_291M     0x00438C01
-#define K800_PLL_43_163M     0x00778C03
-#define K800_PLL_45_250M     0x007D8C83 /* 45.46MHz */
-#define K800_PLL_46_000M     0x00658C02
-#define K800_PLL_46_996M     0x00818C83
-#define K800_PLL_48_000M     0x00848C83
-#define K800_PLL_48_875M     0x00508C81
-#define K800_PLL_49_500M     0x00518C01
-#define K800_PLL_52_406M     0x00738C02
-#define K800_PLL_52_977M     0x00928C83
-#define K800_PLL_56_250M     0x007C8C02
-#define K800_PLL_60_466M     0x00A78C83
-#define K800_PLL_61_500M     0x00AA8C83
-#define K800_PLL_65_000M     0x006B8C01
-#define K800_PLL_65_178M     0x00B48C83
-#define K800_PLL_66_750M     0x00948C82 /* 67.116MHz */
-#define K800_PLL_68_179M     0x00708C01
-#define K800_PLL_69_924M     0x00C18C83
-#define K800_PLL_70_159M     0x00C28C83
-#define K800_PLL_72_000M     0x009F8C82
-#define K800_PLL_74_270M     0x00ce0c03
-#define K800_PLL_78_750M     0x00408801
-#define K800_PLL_80_136M     0x00428801
-#define K800_PLL_83_375M     0x005B0882
-#define K800_PLL_83_950M     0x00738803
-#define K800_PLL_84_750M     0x00748883 /* 84.477MHz */
-#define K800_PLL_85_860M     0x00768883
-#define K800_PLL_88_750M     0x007A8883
-#define K800_PLL_94_500M     0x00828803
-#define K800_PLL_97_750M     0x00878883
-#define K800_PLL_101_000M    0x008B8883
-#define K800_PLL_106_500M    0x00758882 /* 106.491463 MHz */
-#define K800_PLL_108_000M    0x00778882
-#define K800_PLL_113_309M    0x005D8881
-#define K800_PLL_118_840M    0x00A48883
-#define K800_PLL_119_000M    0x00838882
-#define K800_PLL_121_750M    0x00A88883 /* 121.704MHz */
-#define K800_PLL_125_104M    0x00688801
-#define K800_PLL_133_308M    0x005D8801
-#define K800_PLL_135_000M    0x001A4081
-#define K800_PLL_136_700M    0x00BD8883
-#define K800_PLL_138_400M    0x00728881
-#define K800_PLL_146_760M    0x00CC8883
-#define K800_PLL_148_500M    0x00ce0803
-#define K800_PLL_153_920M    0x00548482
-#define K800_PLL_156_000M    0x006B8483
-#define K800_PLL_157_500M    0x00142080
-#define K800_PLL_162_000M    0x006F8483
-#define K800_PLL_187_000M    0x00818483
-#define K800_PLL_193_295M    0x004F8481
-#define K800_PLL_202_500M    0x00538481
-#define K800_PLL_204_000M    0x008D8483
-#define K800_PLL_218_500M    0x00978483
-#define K800_PLL_234_000M    0x00608401
-#define K800_PLL_267_250M    0x006E8481
-#define K800_PLL_297_500M    0x00A48402
-#define K800_PLL_74_481M     0x007B8C81
-#define K800_PLL_172_798M    0x00778483
-#define K800_PLL_122_614M    0x00878882
-
-/* PLL for VT3324 */
-#define CX700_25_175M     0x008B1003
-#define CX700_26_719M     0x00931003
-#define CX700_26_880M     0x00941003
-#define CX700_29_581M     0x00A49003
-#define CX700_31_490M     0x00AE1003
-#define CX700_31_500M     0x00AE1003
-#define CX700_31_728M     0x00AF1003
-#define CX700_32_668M     0x00B51003
-#define CX700_36_000M     0x00C81003
-#define CX700_40_000M     0x006E0C03
-#define CX700_41_291M     0x00710C03
-#define CX700_43_163M     0x00770C03
-#define CX700_45_250M     0x007D0C03    /* 45.46MHz */
-#define CX700_46_000M     0x007F0C03
-#define CX700_46_996M     0x00818C83
-#define CX700_48_000M     0x00840C03
-#define CX700_48_875M     0x00508C81
-#define CX700_49_500M     0x00880C03
-#define CX700_52_406M     0x00730C02
-#define CX700_52_977M     0x00920C03
-#define CX700_56_250M     0x009B0C03
-#define CX700_60_466M     0x00460C00
-#define CX700_61_500M     0x00AA0C03
-#define CX700_65_000M     0x006B0C01
-#define CX700_65_178M     0x006B0C01
-#define CX700_66_750M     0x00940C02    /*67.116MHz */
-#define CX700_68_179M     0x00BC0C03
-#define CX700_69_924M     0x00C10C03
-#define CX700_70_159M     0x00C20C03
-#define CX700_72_000M     0x009F0C02
-#define CX700_74_270M     0x00CE0C03
-#define CX700_74_481M     0x00CE0C03
-#define CX700_78_750M     0x006C0803
-#define CX700_80_136M     0x006E0803
-#define CX700_83_375M     0x005B0882
-#define CX700_83_950M     0x00730803
-#define CX700_84_750M     0x00740803    /* 84.537Mhz */
-#define CX700_85_860M     0x00760803
-#define CX700_88_750M     0x00AC8885
-#define CX700_94_500M     0x00820803
-#define CX700_97_750M     0x00870803
-#define CX700_101_000M    0x008B0803
-#define CX700_106_500M    0x00750802
-#define CX700_108_000M    0x00950803
-#define CX700_113_309M    0x005D0801
-#define CX700_118_840M    0x00A40803
-#define CX700_119_000M    0x00830802
-#define CX700_121_750M    0x00420800    /* 121.704MHz */
-#define CX700_125_104M    0x00AD0803
-#define CX700_133_308M    0x00930802
-#define CX700_135_000M    0x00950802
-#define CX700_136_700M    0x00BD0803
-#define CX700_138_400M    0x00720801
-#define CX700_146_760M    0x00CC0803
-#define CX700_148_500M    0x00a40802
-#define CX700_153_920M    0x00540402
-#define CX700_156_000M    0x006B0403
-#define CX700_157_500M    0x006C0403
-#define CX700_162_000M    0x006F0403
-#define CX700_172_798M    0x00770403
-#define CX700_187_000M    0x00810403
-#define CX700_193_295M    0x00850403
-#define CX700_202_500M    0x008C0403
-#define CX700_204_000M    0x008D0403
-#define CX700_218_500M    0x00970403
-#define CX700_234_000M    0x00600401
-#define CX700_267_250M    0x00B90403
-#define CX700_297_500M    0x00CE0403
-#define CX700_122_614M    0x00870802
-
-/* PLL for VX855 */
-#define VX855_22_000M     0x007B1005
-#define VX855_25_175M     0x008D1005
-#define VX855_26_719M     0x00961005
-#define VX855_26_880M     0x00961005
-#define VX855_27_000M     0x00971005
-#define VX855_29_581M     0x00A51005
-#define VX855_29_829M     0x00641003
-#define VX855_31_490M     0x00B01005
-#define VX855_31_500M     0x00B01005
-#define VX855_31_728M     0x008E1004
-#define VX855_32_668M     0x00921004
-#define VX855_36_000M     0x00A11004
-#define VX855_40_000M     0x00700C05
-#define VX855_41_291M     0x00730C05
-#define VX855_43_163M     0x00790C05
-#define VX855_45_250M     0x007F0C05      /* 45.46MHz */
-#define VX855_46_000M     0x00670C04
-#define VX855_46_996M     0x00690C04
-#define VX855_48_000M     0x00860C05
-#define VX855_48_875M     0x00890C05
-#define VX855_49_500M     0x00530C03
-#define VX855_52_406M     0x00580C03
-#define VX855_52_977M     0x00940C05
-#define VX855_56_250M     0x009D0C05
-#define VX855_57_275M     0x009D8C85    /* Used by XO panel */
-#define VX855_60_466M     0x00A90C05
-#define VX855_61_500M     0x00AC0C05
-#define VX855_65_000M     0x006D0C03
-#define VX855_65_178M     0x00B60C05
-#define VX855_66_750M     0x00700C03    /*67.116MHz */
-#define VX855_67_295M     0x00BC0C05
-#define VX855_68_179M     0x00BF0C05
-#define VX855_68_369M     0x00BF0C05
-#define VX855_69_924M     0x00C30C05
-#define VX855_70_159M     0x00C30C05
-#define VX855_72_000M     0x00A10C04
-#define VX855_73_023M     0x00CC0C05
-#define VX855_74_481M     0x00D10C05
-#define VX855_78_750M     0x006E0805
-#define VX855_79_466M     0x006F0805
-#define VX855_80_136M     0x00700805
-#define VX855_81_627M     0x00720805
-#define VX855_83_375M     0x00750805
-#define VX855_83_527M     0x00750805
-#define VX855_83_950M     0x00750805
-#define VX855_84_537M     0x00760805
-#define VX855_84_750M     0x00760805     /* 84.537Mhz */
-#define VX855_85_500M     0x00760805        /* 85.909080 MHz*/
-#define VX855_85_860M     0x00760805
-#define VX855_85_909M     0x00760805
-#define VX855_88_750M     0x007C0805
-#define VX855_89_489M     0x007D0805
-#define VX855_94_500M     0x00840805
-#define VX855_96_648M     0x00870805
-#define VX855_97_750M     0x00890805
-#define VX855_101_000M    0x008D0805
-#define VX855_106_500M    0x00950805
-#define VX855_108_000M    0x00970805
-#define VX855_110_125M    0x00990805
-#define VX855_112_000M    0x009D0805
-#define VX855_113_309M    0x009F0805
-#define VX855_115_000M    0x00A10805
-#define VX855_118_840M    0x00A60805
-#define VX855_119_000M    0x00A70805
-#define VX855_121_750M    0x00AA0805       /* 121.704MHz */
-#define VX855_122_614M    0x00AC0805
-#define VX855_126_266M    0x00B10805
-#define VX855_130_250M    0x00B60805      /* 130.250 */
-#define VX855_135_000M    0x00BD0805
-#define VX855_136_700M    0x00BF0805
-#define VX855_137_750M    0x00C10805
-#define VX855_138_400M    0x00C20805
-#define VX855_144_300M    0x00CA0805
-#define VX855_146_760M    0x00CE0805
-#define VX855_148_500M    0x00D00805
-#define VX855_153_920M    0x00540402
-#define VX855_156_000M    0x006C0405
-#define VX855_156_867M    0x006E0405
-#define VX855_157_500M    0x006E0405
-#define VX855_162_000M    0x00710405
-#define VX855_172_798M    0x00790405
-#define VX855_187_000M    0x00830405
-#define VX855_193_295M    0x00870405
-#define VX855_202_500M    0x008E0405
-#define VX855_204_000M    0x008F0405
-#define VX855_218_500M    0x00990405
-#define VX855_229_500M    0x00A10405
-#define VX855_234_000M    0x00A40405
-#define VX855_267_250M    0x00BB0405
-#define VX855_297_500M    0x00D00405
-#define VX855_339_500M    0x00770005
-#define VX855_340_772M    0x00770005
-
 
 /* Definition CRTC Timing Index */
 #define H_TOTAL_INDEX               0
diff --git a/drivers/video/via/via-core.c b/drivers/video/via/via-core.c
index e8cfe8392110..66f403033111 100644
--- a/drivers/video/via/via-core.c
+++ b/drivers/video/via/via-core.c
@@ -64,7 +64,7 @@ static inline int viafb_mmio_read(int reg)
  */
 static u32 viafb_enabled_ints;
 
-static void viafb_int_init(void)
+static void __devinit viafb_int_init(void)
 {
         viafb_enabled_ints = 0;
 
@@ -489,7 +489,7 @@ out_unmap:
         return ret;
 }
 
-static void __devexit via_pci_teardown_mmio(struct viafb_dev *vdev)
+static void via_pci_teardown_mmio(struct viafb_dev *vdev)
 {
         iounmap(vdev->fbmem);
         iounmap(vdev->engine_mmio);
@@ -548,7 +548,7 @@ static int __devinit via_setup_subdevs(struct viafb_dev *vdev)
         return 0;
 }
 
-static void __devexit via_teardown_subdevs(void)
+static void via_teardown_subdevs(void)
 {
         int i;
 
@@ -613,22 +613,24 @@ static void __devexit via_pci_remove(struct pci_dev *pdev)
 static struct pci_device_id via_pci_table[] __devinitdata = {
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_CLE266_DID),
           .driver_data = UNICHROME_CLE266 },
-        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_PM800_DID),
-          .driver_data = UNICHROME_PM800 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_K400_DID),
           .driver_data = UNICHROME_K400 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_K800_DID),
           .driver_data = UNICHROME_K800 },
-        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_P4M890_DID),
+        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_PM800_DID),
+          .driver_data = UNICHROME_PM800 },
+        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_CN700_DID),
           .driver_data = UNICHROME_CN700 },
-        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_K8M890_DID),
-          .driver_data = UNICHROME_K8M890 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_CX700_DID),
           .driver_data = UNICHROME_CX700 },
-        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_P4M900_DID),
-          .driver_data = UNICHROME_P4M900 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_CN750_DID),
           .driver_data = UNICHROME_CN750 },
+        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_K8M890_DID),
+          .driver_data = UNICHROME_K8M890 },
+        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_P4M890_DID),
+          .driver_data = UNICHROME_P4M890 },
+        { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_P4M900_DID),
+          .driver_data = UNICHROME_P4M900 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_VX800_DID),
           .driver_data = UNICHROME_VX800 },
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, UNICHROME_VX855_DID),
diff --git a/drivers/video/via/via-gpio.c b/drivers/video/via/via-gpio.c
index 595516aea691..39acb37e7a1d 100644
--- a/drivers/video/via/via-gpio.c
+++ b/drivers/video/via/via-gpio.c
@@ -73,7 +73,7 @@ struct viafb_gpio_cfg {
         struct gpio_chip gpio_chip;
         struct viafb_dev *vdev;
         struct viafb_gpio *active_gpios[VIAFB_NUM_GPIOS];
-        char *gpio_names[VIAFB_NUM_GPIOS];
+        const char *gpio_names[VIAFB_NUM_GPIOS];
 };
 
 /*
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index 1082541358f0..bdd0e4130f4e 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -49,11 +49,6 @@ char *viafb_active_dev;
 char *viafb_lcd_port = "";
 char *viafb_dvi_port = "";
 
-static void viafb_set_device(struct device_t active_dev);
-static int apply_device_setting(struct viafb_ioctl_setting setting_info,
-                         struct fb_info *info);
-static void apply_second_mode_setting(struct fb_var_screeninfo
-        *sec_var);
 static void retrieve_device_setting(struct viafb_ioctl_setting
         *setting_info);
 static int viafb_pan_display(struct fb_var_screeninfo *var,
@@ -221,9 +216,9 @@ static int viafb_check_var(struct fb_var_screeninfo *var,
 
         /* Adjust var according to our driver's own table */
         viafb_fill_var_timing_info(var, viafb_refresh, vmode_entry);
-        if (info->var.accel_flags & FB_ACCELF_TEXT &&
+        if (var->accel_flags & FB_ACCELF_TEXT &&
                 !ppar->shared->vdev->engine_mmio)
-                info->var.accel_flags = 0;
+                var->accel_flags = 0;
 
         return 0;
 }
@@ -234,6 +229,7 @@ static int viafb_set_par(struct fb_info *info)
         struct VideoModeTable *vmode_entry, *vmode_entry1 = NULL;
         DEBUG_MSG(KERN_INFO "viafb_set_par!\n");
 
+        viafb_update_fix(info);
         viapar->depth = fb_get_color_depth(&info->var, &info->fix);
         viafb_update_device_setting(viafbinfo->var.xres, viafbinfo->var.yres,
                 viafbinfo->var.bits_per_pixel, viafb_refresh, 0);
@@ -257,7 +253,6 @@ static int viafb_set_par(struct fb_info *info)
         }
 
         if (vmode_entry) {
-                viafb_update_fix(info);
                 if (viafb_dual_fb && viapar->iga_path == IGA2)
                         viafb_bpp1 = info->var.bits_per_pixel;
                 else
@@ -478,13 +473,6 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
                 if (gpu32 & LCD_Device)
                         viafb_lcd_disable();
                 break;
-        case VIAFB_SET_DEVICE:
-                if (copy_from_user(&u.active_dev, (void *)argp,
-                        sizeof(u.active_dev)))
-                        return -EFAULT;
-                viafb_set_device(u.active_dev);
-                viafb_set_par(info);
-                break;
         case VIAFB_GET_DEVICE:
                 u.active_dev.crt = viafb_CRT_ON;
                 u.active_dev.dvi = viafb_DVI_ON;
@@ -527,21 +515,6 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
 
                 break;
 
-        case VIAFB_SET_DEVICE_INFO:
-                if (copy_from_user(&u.viafb_setting,
-                        argp, sizeof(u.viafb_setting)))
-                        return -EFAULT;
-                if (apply_device_setting(u.viafb_setting, info) < 0)
-                        return -EINVAL;
-
-                break;
-
-        case VIAFB_SET_SECOND_MODE:
-                if (copy_from_user(&u.sec_var, argp, sizeof(u.sec_var)))
-                        return -EFAULT;
-                apply_second_mode_setting(&u.sec_var);
-                break;
-
         case VIAFB_GET_DEVICE_INFO:
 
                 retrieve_device_setting(&u.viafb_setting);
@@ -913,112 +886,6 @@ static int viafb_sync(struct fb_info *info)
         return 0;
 }
 
-static void check_available_device_to_enable(int device_id)
-{
-        int device_num = 0;
-
-        /* Initialize: */
-        viafb_CRT_ON = STATE_OFF;
-        viafb_DVI_ON = STATE_OFF;
-        viafb_LCD_ON = STATE_OFF;
-        viafb_LCD2_ON = STATE_OFF;
-        viafb_DeviceStatus = None_Device;
-
-        if ((device_id & CRT_Device) && (device_num < MAX_ACTIVE_DEV_NUM)) {
-                viafb_CRT_ON = STATE_ON;
-                device_num++;
-                viafb_DeviceStatus |= CRT_Device;
-        }
-
-        if ((device_id & DVI_Device) && (device_num < MAX_ACTIVE_DEV_NUM)) {
-                viafb_DVI_ON = STATE_ON;
-                device_num++;
-                viafb_DeviceStatus |= DVI_Device;
-        }
-
-        if ((device_id & LCD_Device) && (device_num < MAX_ACTIVE_DEV_NUM)) {
-                viafb_LCD_ON = STATE_ON;
-                device_num++;
-                viafb_DeviceStatus |= LCD_Device;
-        }
-
-        if ((device_id & LCD2_Device) && (device_num < MAX_ACTIVE_DEV_NUM)) {
-                viafb_LCD2_ON = STATE_ON;
-                device_num++;
-                viafb_DeviceStatus |= LCD2_Device;
-        }
-
-        if (viafb_DeviceStatus == None_Device) {
-                /* Use CRT as default active device: */
-                viafb_CRT_ON = STATE_ON;
-                viafb_DeviceStatus = CRT_Device;
-        }
-        DEBUG_MSG(KERN_INFO "Device Status:%x", viafb_DeviceStatus);
-}
-
-static void viafb_set_device(struct device_t active_dev)
-{
-        /* Check available device to enable: */
-        int device_id = None_Device;
-        if (active_dev.crt)
-                device_id |= CRT_Device;
-        if (active_dev.dvi)
-                device_id |= DVI_Device;
-        if (active_dev.lcd)
-                device_id |= LCD_Device;
-
-        check_available_device_to_enable(device_id);
-
-        /* Check property of LCD: */
-        if (viafb_LCD_ON) {
-                if (active_dev.lcd_dsp_cent) {
-                        viaparinfo->lvds_setting_info->display_method =
-                                viafb_lcd_dsp_method = LCD_CENTERING;
-                } else {
-                        viaparinfo->lvds_setting_info->display_method =
-                                viafb_lcd_dsp_method = LCD_EXPANDSION;
-                }
-
-                if (active_dev.lcd_mode == LCD_SPWG) {
-                        viaparinfo->lvds_setting_info->lcd_mode =
-                                viafb_lcd_mode = LCD_SPWG;
-                } else {
-                        viaparinfo->lvds_setting_info->lcd_mode =
-                                viafb_lcd_mode = LCD_OPENLDI;
-                }
-
-                if (active_dev.lcd_panel_id <= LCD_PANEL_ID_MAXIMUM) {
-                        viafb_lcd_panel_id = active_dev.lcd_panel_id;
-                        viafb_init_lcd_size();
-                }
-        }
-
-        /* Check property of mode: */
-        if (!active_dev.xres1)
-                viafb_second_xres = 640;
-        else
-                viafb_second_xres = active_dev.xres1;
-        if (!active_dev.yres1)
-                viafb_second_yres = 480;
-        else
-                viafb_second_yres = active_dev.yres1;
-        if (active_dev.bpp != 0)
-                viafb_bpp = active_dev.bpp;
-        if (active_dev.bpp1 != 0)
-                viafb_bpp1 = active_dev.bpp1;
-        if (active_dev.refresh != 0)
-                viafb_refresh = active_dev.refresh;
-        if (active_dev.refresh1 != 0)
-                viafb_refresh1 = active_dev.refresh1;
-        if ((active_dev.samm == STATE_OFF) || (active_dev.samm == STATE_ON))
-                viafb_SAMM_ON = active_dev.samm;
-        viafb_primary_dev = active_dev.primary_dev;
-
-        via_set_primary_address(0);
-        via_set_secondary_address(viafb_SAMM_ON ? viafb_second_offset : 0);
-        viafb_set_iga_path();
-}
-
 static int get_primary_device(void)
 {
         int primary_device = 0;
@@ -1060,124 +927,6 @@ static int get_primary_device(void)
         return primary_device;
 }
 
-static void apply_second_mode_setting(struct fb_var_screeninfo
-        *sec_var)
-{
-        u32 htotal, vtotal, long_refresh;
-
-        htotal = sec_var->xres + sec_var->left_margin +
-                sec_var->right_margin + sec_var->hsync_len;
-        vtotal = sec_var->yres + sec_var->upper_margin +
-                sec_var->lower_margin + sec_var->vsync_len;
-        if ((sec_var->xres_virtual * (sec_var->bits_per_pixel >> 3)) & 0x1F) {
-                /*Is 32 bytes alignment? */
-                /*32 pixel alignment */
-                sec_var->xres_virtual = (sec_var->xres_virtual + 31) & ~31;
-        }
-
-        htotal = sec_var->xres + sec_var->left_margin +
-                sec_var->right_margin + sec_var->hsync_len;
-        vtotal = sec_var->yres + sec_var->upper_margin +
-                sec_var->lower_margin + sec_var->vsync_len;
-        long_refresh = 1000000000UL / sec_var->pixclock * 1000;
-        long_refresh /= (htotal * vtotal);
-
-        viafb_second_xres = sec_var->xres;
-        viafb_second_yres = sec_var->yres;
-        viafb_second_virtual_xres = sec_var->xres_virtual;
-        viafb_second_virtual_yres = sec_var->yres_virtual;
-        viafb_bpp1 = sec_var->bits_per_pixel;
-        viafb_refresh1 = viafb_get_refresh(sec_var->xres, sec_var->yres,
-                long_refresh);
-}
-
-static int apply_device_setting(struct viafb_ioctl_setting setting_info,
-        struct fb_info *info)
-{
-        int need_set_mode = 0;
-        DEBUG_MSG(KERN_INFO "apply_device_setting\n");
-
-        if (setting_info.device_flag) {
-                need_set_mode = 1;
-                check_available_device_to_enable(setting_info.device_status);
-        }
-
-        /* Unlock LCD's operation according to LCD flag
-           and check if the setting value is valid. */
-        /* If the value is valid, apply the new setting value to the device. */
-        if (viafb_LCD_ON) {
-                if (setting_info.lcd_operation_flag & OP_LCD_CENTERING) {
-                        need_set_mode = 1;
-                        if (setting_info.lcd_attributes.display_center) {
-                                /* Centering */
-                                viaparinfo->lvds_setting_info->display_method =
-                                    LCD_CENTERING;
-                                viafb_lcd_dsp_method = LCD_CENTERING;
-                                viaparinfo->lvds_setting_info2->display_method =
-                                    viafb_lcd_dsp_method = LCD_CENTERING;
-                        } else {
-                                /* expandsion */
-                                viaparinfo->lvds_setting_info->display_method =
-                                    LCD_EXPANDSION;
-                                viafb_lcd_dsp_method = LCD_EXPANDSION;
-                                viaparinfo->lvds_setting_info2->display_method =
-                                    LCD_EXPANDSION;
-                                viafb_lcd_dsp_method = LCD_EXPANDSION;
-                        }
-                }
-
-                if (setting_info.lcd_operation_flag & OP_LCD_MODE) {
-                        need_set_mode = 1;
-                        if (setting_info.lcd_attributes.lcd_mode ==
-                                LCD_SPWG) {
-                                viaparinfo->lvds_setting_info->lcd_mode =
-                                        viafb_lcd_mode = LCD_SPWG;
-                        } else {
-                                viaparinfo->lvds_setting_info->lcd_mode =
-                                        viafb_lcd_mode = LCD_OPENLDI;
-                        }
-                        viaparinfo->lvds_setting_info2->lcd_mode =
-                            viaparinfo->lvds_setting_info->lcd_mode;
-                }
-
-                if (setting_info.lcd_operation_flag & OP_LCD_PANEL_ID) {
-                        need_set_mode = 1;
-                        if (setting_info.lcd_attributes.panel_id <=
-                            LCD_PANEL_ID_MAXIMUM) {
-                                viafb_lcd_panel_id =
-                                    setting_info.lcd_attributes.panel_id;
-                                viafb_init_lcd_size();
-                        }
-                }
-        }
-
-        if (0 != (setting_info.samm_status & OP_SAMM)) {
-                setting_info.samm_status =
-                    setting_info.samm_status & (~OP_SAMM);
-                if (setting_info.samm_status == 0
-                    || setting_info.samm_status == 1) {
-                        viafb_SAMM_ON = setting_info.samm_status;
-
-                        if (viafb_SAMM_ON)
-                                viafb_primary_dev = setting_info.primary_device;
-
-                        via_set_primary_address(0);
-                        via_set_secondary_address(viafb_SAMM_ON ?
-                                viafb_second_offset : 0);
-                        viafb_set_iga_path();
-                }
-                need_set_mode = 1;
-        }
-
-        if (!need_set_mode) {
-                ;
-        } else {
-                viafb_set_iga_path();
-                viafb_set_par(info);
-        }
-        return true;
-}
-
 static void retrieve_device_setting(struct viafb_ioctl_setting
         *setting_info)
 {
@@ -1776,10 +1525,6 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
         parse_lcd_port();
         parse_dvi_port();
 
-        /* for dual-fb must viafb_SAMM_ON=1 and viafb_dual_fb=1 */
-        if (!viafb_SAMM_ON)
-                viafb_dual_fb = 0;
-
         viafb_init_chip_info(vdev->chip_type);
         /*
          * The framebuffer will have been successfully mapped by
@@ -1823,30 +1568,13 @@ int __devinit via_fb_pci_probe(struct viafb_dev *vdev)
                 parse_mode(viafb_mode1, &viafb_second_xres,
                         &viafb_second_yres);
 
-                if (0 == viafb_second_virtual_xres) {
-                        switch (viafb_second_xres) {
-                        case 1400:
-                                viafb_second_virtual_xres = 1408;
-                                break;
-                        default:
-                                viafb_second_virtual_xres = viafb_second_xres;
-                                break;
-                        }
-                }
-                if (0 == viafb_second_virtual_yres)
-                        viafb_second_virtual_yres = viafb_second_yres;
+                viafb_second_virtual_xres = viafb_second_xres;
+                viafb_second_virtual_yres = viafb_second_yres;
         }
 
         default_var.xres = default_xres;
         default_var.yres = default_yres;
-        switch (default_xres) {
-        case 1400:
-                default_var.xres_virtual = 1408;
-                break;
-        default:
-                default_var.xres_virtual = default_xres;
-                break;
-        }
+        default_var.xres_virtual = default_xres;
         default_var.yres_virtual = default_yres;
         default_var.bits_per_pixel = viafb_bpp;
         default_var.pixclock =
diff --git a/drivers/video/w100fb.c b/drivers/video/w100fb.c
index e66b8b19ce5d..d8b12c32e3ef 100644
--- a/drivers/video/w100fb.c
+++ b/drivers/video/w100fb.c
@@ -858,9 +858,9 @@ unsigned long w100fb_gpio_read(int port)
 void w100fb_gpio_write(int port, unsigned long value)
 {
         if (port==W100_GPIO_PORT_A)
-                value = writel(value, remapped_regs + mmGPIO_DATA);
+                writel(value, remapped_regs + mmGPIO_DATA);
         else
-                value = writel(value, remapped_regs + mmGPIO_DATA2);
+                writel(value, remapped_regs + mmGPIO_DATA2);
 }
 EXPORT_SYMBOL(w100fb_gpio_read);
 EXPORT_SYMBOL(w100fb_gpio_write);