1 files changed, 48 insertions, 59 deletions

diff --git a/drivers/video/via/hw.c b/drivers/video/via/hw.c
index d7b9a9f32dee..6845c82db3cb 100644
--- a/drivers/video/via/hw.c
+++ b/drivers/video/via/hw.c
@@ -1467,49 +1467,40 @@ void viafb_set_vclock(u32 clk, int set_iga)
         via_write_misc_reg_mask(0x0C, 0x0C); /* select external clock */
 }
 
-void viafb_fill_crtc_timing(struct VideoModeTable *video_mode, int bpp_byte,
-        int set_iga)
+static struct display_timing var_to_timing(const struct fb_var_screeninfo *var)
 {
-        struct crt_mode_table *crt_table = video_mode->crtc;
-        struct display_timing crt_reg;
-        int i;
-        int index = 0;
-        int h_addr, v_addr;
-        u32 clock, refresh = viafb_refresh;
-
-        if (viafb_SAMM_ON && set_iga == IGA2)
-                refresh = viafb_refresh1;
-
-        for (i = 0; i < video_mode->mode_array; i++) {
-                index = i;
+        struct display_timing timing;
+
+        timing.hor_addr = var->xres;
+        timing.hor_sync_start = timing.hor_addr + var->right_margin;
+        timing.hor_sync_end = timing.hor_sync_start + var->hsync_len;
+        timing.hor_total = timing.hor_sync_end + var->left_margin;
+        timing.hor_blank_start = timing.hor_addr;
+        timing.hor_blank_end = timing.hor_total;
+        timing.ver_addr = var->yres;
+        timing.ver_sync_start = timing.ver_addr + var->lower_margin;
+        timing.ver_sync_end = timing.ver_sync_start + var->vsync_len;
+        timing.ver_total = timing.ver_sync_end + var->upper_margin;
+        timing.ver_blank_start = timing.ver_addr;
+        timing.ver_blank_end = timing.ver_total;
+        return timing;
+}
 
-                if (crt_table[i].refresh_rate == refresh)
-                        break;
-        }
+void viafb_fill_crtc_timing(const struct fb_var_screeninfo *var, int iga)
+{
+        struct display_timing crt_reg = var_to_timing(var);
 
-        crt_reg = crt_table[index].crtc;
-        crt_reg.hor_blank_end += crt_reg.hor_blank_start;
-        crt_reg.hor_sync_end += crt_reg.hor_sync_start;
-        crt_reg.ver_blank_end += crt_reg.ver_blank_start;
-        crt_reg.ver_sync_end += crt_reg.ver_sync_start;
-        h_addr = crt_reg.hor_addr;
-        v_addr = crt_reg.ver_addr;
-        if (set_iga == IGA1)
+        if (iga == IGA1)
                 via_set_primary_timing(&crt_reg);
-        else if (set_iga == IGA2)
+        else if (iga == IGA2)
                 via_set_secondary_timing(&crt_reg);
 
-        viafb_load_fetch_count_reg(h_addr, bpp_byte, set_iga);
-
-        /* load FIFO */
-        if ((viaparinfo->chip_info->gfx_chip_name != UNICHROME_CLE266)
-            && (viaparinfo->chip_info->gfx_chip_name != UNICHROME_K400))
-                viafb_load_FIFO_reg(set_iga, h_addr, v_addr);
-
-        clock = crt_reg.hor_total * crt_reg.ver_total
-                * crt_table[index].refresh_rate;
-        viafb_set_vclock(clock, set_iga);
+        viafb_load_fetch_count_reg(var->xres, var->bits_per_pixel / 8, iga);
+        if (viaparinfo->chip_info->gfx_chip_name != UNICHROME_CLE266
+                && viaparinfo->chip_info->gfx_chip_name != UNICHROME_K400)
+                viafb_load_FIFO_reg(iga, var->xres, var->yres);
 
+        viafb_set_vclock(PICOS2KHZ(var->pixclock) * 1000, iga);
 }
 
 void __devinit viafb_init_chip_info(int chip_type)
@@ -1788,6 +1779,7 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
         u8 value, index, mask;
         struct crt_mode_table *crt_timing;
         struct crt_mode_table *crt_timing1 = NULL;
+        struct fb_var_screeninfo var2;
 
         device_screen_off();
         crt_timing = vmode_tbl->crtc;
@@ -1894,17 +1886,24 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
 
         /* Clear On Screen */
 
+        if (viafb_dual_fb) {
+                var2 = viafbinfo1->var;
+        } else if (viafb_SAMM_ON) {
+                viafb_fill_var_timing_info(&var2, viafb_get_best_mode(
+                        vmode_tbl1->crtc->crtc.hor_addr,
+                        vmode_tbl1->crtc->crtc.ver_addr, viafb_refresh1));
+                var2.bits_per_pixel = viafbinfo->var.bits_per_pixel;
+        }
+
         /* CRT set mode */
         if (viafb_CRT_ON) {
-                if (viafb_SAMM_ON &&
-                        viaparinfo->shared->iga2_devices & VIA_CRT) {
-                        viafb_fill_crtc_timing(vmode_tbl1, video_bpp1 / 8,
-                                IGA2);
-                } else {
-                        viafb_fill_crtc_timing(vmode_tbl, video_bpp / 8,
+                if (viaparinfo->shared->iga2_devices & VIA_CRT
+                        && viafb_SAMM_ON)
+                        viafb_fill_crtc_timing(&var2, IGA2);
+                else
+                        viafb_fill_crtc_timing(&viafbinfo->var,
                                 (viaparinfo->shared->iga1_devices & VIA_CRT)
                                 ? IGA1 : IGA2);
-                }
 
                 /* Patch if set_hres is not 8 alignment (1366) to viafb_setmode
                 to 8 alignment (1368),there is several pixels (2 pixels)
@@ -1918,22 +1917,12 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
         }
 
         if (viafb_DVI_ON) {
-                if (viafb_SAMM_ON &&
-                        (viaparinfo->tmds_setting_info->iga_path == IGA2)) {
-                        viafb_dvi_set_mode(viafb_get_mode
-                                     (viaparinfo->tmds_setting_info->h_active,
-                                      viaparinfo->tmds_setting_info->
-                                      v_active),
-                                     video_bpp1, viaparinfo->
-                                     tmds_setting_info->iga_path);
-                } else {
-                        viafb_dvi_set_mode(viafb_get_mode
-                                     (viaparinfo->tmds_setting_info->h_active,
-                                      viaparinfo->
-                                      tmds_setting_info->v_active),
-                                     video_bpp, viaparinfo->
-                                     tmds_setting_info->iga_path);
-                }
+                if (viaparinfo->shared->tmds_setting_info.iga_path == IGA2
+                        && viafb_SAMM_ON)
+                        viafb_dvi_set_mode(&var2, IGA2);
+                else
+                        viafb_dvi_set_mode(&viafbinfo->var,
+                                viaparinfo->tmds_setting_info->iga_path);
         }
 
         if (viafb_LCD_ON) {