2 files changed, 51 insertions, 14 deletions

diff --git a/drivers/media/video/gspca/m5602/m5602_mt9m111.c b/drivers/media/video/gspca/m5602/m5602_mt9m111.c
index edf502041b80..7986ef074c55 100644
--- a/drivers/media/video/gspca/m5602/m5602_mt9m111.c
+++ b/drivers/media/video/gspca/m5602/m5602_mt9m111.c
@@ -269,6 +269,10 @@ int mt9m111_start(struct sd *sd)
 {
         int i, err = 0;
         u8 data[2];
+        struct cam *cam = &sd->gspca_dev.cam;
+
+        int width = cam->cam_mode[sd->gspca_dev.curr_mode].width;
+        int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
 
         for (i = 0; i < ARRAY_SIZE(start_mt9m111) && !err; i++) {
                 if (start_mt9m111[i][0] == BRIDGE) {
@@ -282,6 +286,53 @@ int mt9m111_start(struct sd *sd)
                                 start_mt9m111[i][1], data, 2);
                 }
         }
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height >> 8) & 0xff);
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height & 0xff));
+        if (err < 0)
+                return err;
+
+        for (i = 0; i < 2 && !err; i++)
+                err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 2);
+        if (err < 0)
+                return err;
+
+        for (i = 0; i < 2 && !err; i++)
+                err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, 0);
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA,
+                                 (width >> 8) & 0xff);
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, width & 0xff);
+        if (err < 0)
+                return err;
+
+        err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
+        if (err < 0)
+                return err;
+
+        switch (width) {
+        case 640:
+                PDEBUG(D_V4L2, "Configuring camera for VGA mode");
+                break;
+        }
         return err;
 }
 
diff --git a/drivers/media/video/gspca/m5602/m5602_mt9m111.h b/drivers/media/video/gspca/m5602/m5602_mt9m111.h
index 6598dd9c438e..7c8c700bab95 100644
--- a/drivers/media/video/gspca/m5602/m5602_mt9m111.h
+++ b/drivers/media/video/gspca/m5602/m5602_mt9m111.h
@@ -275,20 +275,6 @@ static const unsigned char start_mt9m111[][4] =
         {BRIDGE, M5602_XB_VSYNC_PARA, 0x00, 0x00},
         {BRIDGE, M5602_XB_VSYNC_PARA, 0x00, 0x00},
         {BRIDGE, M5602_XB_VSYNC_PARA, 0x00, 0x00},
-        {BRIDGE, M5602_XB_VSYNC_PARA, 0x01, 0x00},
-        {BRIDGE, M5602_XB_VSYNC_PARA, 0xe0, 0x00}, /* 480 */
-        {BRIDGE, M5602_XB_VSYNC_PARA, 0x00, 0x00},
-        {BRIDGE, M5602_XB_VSYNC_PARA, 0x00, 0x00},
-        {BRIDGE, M5602_XB_SIG_INI, 0x00, 0x00},
-        {BRIDGE, M5602_XB_SIG_INI, 0x02, 0x00},
-        {BRIDGE, M5602_XB_HSYNC_PARA, 0x00, 0x00},
-        {BRIDGE, M5602_XB_HSYNC_PARA, 0x00, 0x00},
-        {BRIDGE, M5602_XB_HSYNC_PARA, 0x02, 0x00}, /* 639*/
-        {BRIDGE, M5602_XB_HSYNC_PARA, 0x7f, 0x00},
-        {BRIDGE, M5602_XB_SIG_INI, 0x00, 0x00},
-        {BRIDGE, M5602_XB_SEN_CLK_DIV, 0x00, 0x00},
-        {BRIDGE, M5602_XB_SEN_CLK_CTRL, 0xb0, 0x00},
 };
 
-
 #endif