diff --git a/drivers/video/i810/i810_dvt.c b/drivers/video/i810/i810_dvt.c
index 27fa703a2e0..b4b3670667a 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,