1 files changed, 29 insertions, 24 deletions

diff --git a/drivers/video/i810/i810_dvt.c b/drivers/video/i810/i810_dvt.c
index 27fa703a2e0a..b4b3670667ab 100644
--- a/drivers/video/i810/i810_dvt.c
+++ b/drivers/video/i810/i810_dvt.c
@@ -212,24 +212,29 @@ inline void round_off_yres(u32 *xres, u32 *yres)
         *yres = (*xres * 3) >> 2;
 }
 
-void i810fb_encode_registers(const struct fb_var_screeninfo *var,
-                             struct i810fb_par *par, u32 xres, u32 yres)
+static int i810fb_find_best_mode(u32 xres, u32 yres, u32 pixclock)
 { 
         u32 diff = 0, diff_best = 0xFFFFFFFF, i = 0, i_best = 0; 
-        u8 hfl;
+        u8 hfl = (u8) ((xres >> 3) - 1);
 
-        hfl = (u8) ((xres >> 3) - 1);
         for (i = 0; i < ARRAY_SIZE(std_modes); i++) { 
                 if (std_modes[i].cr01 == hfl) { 
-                        if (std_modes[i].pixclock <= par->regs.pixclock)
-                                diff = par->regs.pixclock - 
-                                        std_modes[i].pixclock;
+                        if (std_modes[i].pixclock <= pixclock)
+                                diff = pixclock - std_modes[i].pixclock;
                         if (diff < diff_best) {  
                                 i_best = i;
                                 diff_best = diff;
                         }
                 }
         }
+        return i_best;
+}
+
+void i810fb_encode_registers(const struct fb_var_screeninfo *var,
+                             struct i810fb_par *par, u32 xres, u32 yres)
+{
+        u32 i_best = i810fb_find_best_mode(xres, yres, par->regs.pixclock);
+
         par->regs = std_modes[i_best];
 
         /* overlay */
@@ -239,36 +244,36 @@ void i810fb_encode_registers(const struct fb_var_screeninfo *var,
 
 void i810fb_fill_var_timings(struct fb_var_screeninfo *var)
 {
-        struct i810fb_par par;
         u32 total, xres, yres;
+        u32 mode, pixclock;
 
         xres = var->xres;
         yres = var->yres;
         
-        par.regs.pixclock = 1000000000/var->pixclock;
-        i810fb_encode_registers(var, &par, xres, yres);
+        pixclock = 1000000000 / var->pixclock;
+        mode = i810fb_find_best_mode(xres, yres, pixclock);
         
-        total = ((par.regs.cr00 | (par.regs.cr35 & 1) << 8) + 3) << 3;
+        total = (std_modes[mode].cr00 | (std_modes[mode].cr35 & 1) << 8) + 3;
+        total <<= 3;
         
-        var->pixclock = 1000000000/par.regs.pixclock;
-        var->right_margin = (par.regs.cr04 << 3) - xres;
-        var->hsync_len = ((par.regs.cr05 & 0x1F) - 
-                               (par.regs.cr04 & 0x1F)) << 3;
+        var->pixclock = 1000000000 / std_modes[mode].pixclock;
+        var->right_margin = (std_modes[mode].cr04 << 3) - xres;
+        var->hsync_len = ((std_modes[mode].cr05 & 0x1F) -
+                         (std_modes[mode].cr04 & 0x1F)) << 3;
         var->left_margin = (total - (xres + var->right_margin + 
                                      var->hsync_len));
         var->sync = FB_SYNC_ON_GREEN;
-        if (~(par.regs.msr & (1 << 6)))
+        if (~(std_modes[mode].msr & (1 << 6)))
                 var->sync |= FB_SYNC_HOR_HIGH_ACT;
-        if (~(par.regs.msr & (1 << 7)))
+        if (~(std_modes[mode].msr & (1 << 7)))
                 var->sync |= FB_SYNC_VERT_HIGH_ACT;
 
-
-        total = ((par.regs.cr06 | (par.regs.cr30 & 0x0F)  << 8)) + 2;
-        var->lower_margin = (par.regs.cr10 | 
-                             (par.regs.cr32 & 0x0F) << 8) - yres;
-        var->vsync_len = (par.regs.cr11 & 0x0F) - (var->lower_margin & 0x0F);
-        var->upper_margin = total - (yres + var->lower_margin + 
-                                     var->vsync_len);
+        total = (std_modes[mode].cr06 | (std_modes[mode].cr30 & 0xF)  << 8) + 2;
+        var->lower_margin = (std_modes[mode].cr10 |
+                            (std_modes[mode].cr32 & 0x0F) << 8) - yres;
+        var->vsync_len = (std_modes[mode].cr11 & 0x0F) -
+                         (var->lower_margin & 0x0F);
+        var->upper_margin = total - (yres + var->lower_margin + var->vsync_len);
 }
 
 u32 i810_get_watermark(struct fb_var_screeninfo *var,