1 files changed, 25 insertions, 2 deletions

diff --git a/drivers/video/backlight/tosa_lcd.c b/drivers/video/backlight/tosa_lcd.c
index 57a26649f1a5..b7fbc75a62fc 100644
--- a/drivers/video/backlight/tosa_lcd.c
+++ b/drivers/video/backlight/tosa_lcd.c
@@ -39,6 +39,7 @@ struct tosa_lcd_data {
         struct i2c_client *i2c;
 
         int lcd_power;
+        bool is_vga;
 };
 
 static int tosa_tg_send(struct spi_device *spi, int adrs, uint8_t data)
@@ -81,8 +82,12 @@ static void tosa_lcd_tg_init(struct tosa_lcd_data *data)
 static void tosa_lcd_tg_on(struct tosa_lcd_data *data)
 {
         struct spi_device *spi = data->spi;
-        const int value = TG_REG0_COLOR | TG_REG0_UD | TG_REG0_LR;
-        tosa_tg_send(spi, TG_PNLCTL, value | TG_REG0_VQV); /* this depends on mode */
+        int value = TG_REG0_COLOR | TG_REG0_UD | TG_REG0_LR;
+
+        if (data->is_vga)
+                value |= TG_REG0_VQV;
+
+        tosa_tg_send(spi, TG_PNLCTL, value);
 
         /* TG LCD pannel power up */
         tosa_tg_send(spi, TG_PINICTL,0x4);
@@ -142,9 +147,25 @@ static int tosa_lcd_get_power(struct lcd_device *lcd)
         return data->lcd_power;
 }
 
+static int tosa_lcd_set_mode(struct lcd_device *lcd, struct fb_videomode *mode)
+{
+        struct tosa_lcd_data *data = lcd_get_data(lcd);
+
+        if (mode->xres == 320 || mode->yres == 320)
+                data->is_vga = false;
+        else
+                data->is_vga = true;
+
+        if (POWER_IS_ON(data->lcd_power))
+                tosa_lcd_tg_on(data);
+
+        return 0;
+}
+
 static struct lcd_ops tosa_lcd_ops = {
         .set_power = tosa_lcd_set_power,
         .get_power = tosa_lcd_get_power,
+        .set_mode = tosa_lcd_set_mode,
 };
 
 static int __devinit tosa_lcd_probe(struct spi_device *spi)
@@ -156,6 +177,8 @@ static int __devinit tosa_lcd_probe(struct spi_device *spi)
         if (!data)
                 return -ENOMEM;
 
+        data->is_vga = true; /* defaut to VGA mode */
+
         /*
          * bits_per_word cannot be configured in platform data
          */