[patch] a patch to update g-d-s to vesafb-tng-0.9-rc4-r3

vesafb-tng-update.patch (text/plain), 24.07 KB, created by Michal Januszewski (RETIRED) on 2004-09-10 13:47:10 UTC

diff -Naur linux-2.6.8-gentoo-r4/Documentation/fb/vesafb.txt linux-2.6.8-gentoo-r4-vesafb-tng-update/Documentation/fb/vesafb.txt
--- linux-2.6.8-gentoo-r4/Documentation/fb/vesafb.txt	2004-09-10 22:25:37.000000000 +0200
+++ linux-2.6.8-gentoo-r4-vesafb-tng-update/Documentation/fb/vesafb.txt	2004-09-10 22:32:18.877399896 +0200
@@ -246,21 +246,13 @@
  o vesafb_probe
    - get service thread's PID (started earlier from fbmem.c)
    - call vesafb_vbe_init
+   - find a matching VBE mode
    - try to find the specified mode in vesa_modes modedb
-   - if the previous step failed or was skipped:
-     - try to find a matching mode in the VBE modedb - identify VBE mode ID
-     - try to find a matching mode in the EDID modedb
-     - if the previous step failed or was skipped:
-       - try to calculate mode timings with GTF
+   - try to find the specified mode in the EDID modedb
+   - try to calculate timings with the GTF
    - low level setup - request_mem_region, ioremap, etc.
    - setup /proc/fb<x>/modes and /proc/fb<x>/vbe_info
 
-3. Used hacks
- 
- o info->var.reserved[0] holds the VBE mode ID
- o info->var.reserved[1] holds a pointer to the VBE mode data in vesafb's
-   mode database.
-
 Have fun!
 
 --
diff -Naur linux-2.6.8-gentoo-r4/drivers/video/vesafb-thread.c linux-2.6.8-gentoo-r4-vesafb-tng-update/drivers/video/vesafb-thread.c
--- linux-2.6.8-gentoo-r4/drivers/video/vesafb-thread.c	2004-09-10 22:25:37.000000000 +0200
+++ linux-2.6.8-gentoo-r4-vesafb-tng-update/drivers/video/vesafb-thread.c	2004-09-10 22:32:18.886398528 +0200
@@ -381,18 +381,9 @@
 		addr -= BUFFER+REAL_MEM;					 			\
 		vbe_pib(task->res)->str = (u32) (task->res + addr);					\
 													\
-	/* this should never happen: someone was insane enough to put the data somewhere in the RAM;	\
-	   we need to copy as much of it as possible to our buffer */					\
+	/* this should never happen: someone was insane enough to put the data somewhere in the RAM */	\
 	} else {											\
-		res = strlcpy((char*) (((int)&(vbe_pib(task->res)->oem_data)) + oem_offset),		\
-			      (char*) addr, sizeof(vbe_pib(task->res)->oem_data) - oem_offset);		\
-													\
-		vbe_pib(task->res)->str = ((u32)&(vbe_pib(task->res)->oem_data)) + oem_offset;		\
-													\
-		oem_offset += res+1;									\
-		if (oem_offset > sizeof(vbe_pib(task->res)->oem_data)) {				\
-			oem_offset = sizeof(vbe_pib(task->res)->oem_data);				\
-		}											\
+		vbe_pib(task->res)->str = (u32) "";							\
 	}												\
 }
 
@@ -400,7 +391,7 @@
 {
 	struct vesafb_task *task;
 	struct list_head *node, *next;
-	int addr, oem_offset = 0, res;
+	int addr, res;
 	
 	down(&vesafb_sem);
 	list_for_each_safe(node, next, &vesafb_task_list) {
@@ -430,8 +421,7 @@
 					vesafb_get_string(oem_product_rev_ptr);
 				}
 		
-				/* this is basically the same as vesafb_get_string; the third part
-				   is different though, so that's what causes all this mess */
+				/* this is basically the same as vesafb_get_string */
 				addr = ((vbe_pib(task->res)->mode_list_ptr & 0xffff0000) >> 12) +
 					(vbe_pib(task->res)->mode_list_ptr & 0x0000ffff);
 			
@@ -444,6 +434,7 @@
 					vbe_pib(task->res)->mode_list_ptr = (u32) (task->res + addr);
 				} else {
 					res = 0;
+					printk(KERN_WARNING "vesafb: warning, copying modelist from somewhere in RAM!\n");
 					while (*(u16*)(addr+res) != 0xffff && 
 					       res < (sizeof(vbe_pib(task->res)->reserved) - 2) )
 					{
diff -Naur linux-2.6.8-gentoo-r4/drivers/video/vesafb-tng.c linux-2.6.8-gentoo-r4-vesafb-tng-update/drivers/video/vesafb-tng.c
--- linux-2.6.8-gentoo-r4/drivers/video/vesafb-tng.c	2004-09-10 22:25:37.000000000 +0200
+++ linux-2.6.8-gentoo-r4-vesafb-tng-update/drivers/video/vesafb-tng.c	2004-09-10 22:32:18.889398072 +0200
@@ -33,6 +33,9 @@
 #define dac_reg	(0x3c8)
 #define dac_val	(0x3c9)
 
+#define VESAFB_NEED_EXACT_RES 1
+#define VESAFB_NEED_EXACT_DEPTH 2
+
 /* --------------------------------------------------------------------- */
 
 static struct fb_var_screeninfo vesafb_defined __initdata = {
@@ -75,7 +78,7 @@
 static unsigned short			maxhf          __initdata = 0; /* maximum horizontal frequency */
 static int 				gtf            __initdata = 0; /* forces use of the GTF */
 static char				*mode_option   __initdata = NULL;
-static unsigned short			vbemode	       __initdata = 0;
+static unsigned short			vbemode	                  = 0;
 
 extern int vesafb_pid;			/* PID of the vesafb service thread */
 
@@ -87,6 +90,7 @@
 #define vesafb_wait_for_task(task)	{ while (task->done == 0) { schedule(); } }
 
 extern void vesafb_queue_task(struct vesafb_task *task);
+static int vesafb_find_vbe_mode(int xres, int yres, int bpp, unsigned char flags);
 
 /* --------------------------------------------------------------------- */
 
@@ -221,27 +225,40 @@
 	struct vesafb_par *par = (struct vesafb_par *) info->par;
 	struct vesafb_task *mytask;
 	struct vesafb_crtc_info_block *crtc = NULL;	
-	struct vesafb_mode_info_block *mode = (void*)info->var.reserved[1];
-	int err = 0;
-	
-	/* sanity check */
-	if (info->var.reserved[0] == 0xffff)
-		return -EINVAL;
+	struct vesafb_mode_info_block *mode = NULL;
+	int err = 0, i;
 
+	/* has the VBE mode number been specified? */
+	if (vbemode > 0) {
+		for (i = 0; i < vbe_modes_cnt; i++) {
+			if (vbe_modes[i].mode_id == vbemode) {
+				mode = &vbe_modes[i];
+				break;
+			}
+		}
+	} else {
+		i = vesafb_find_vbe_mode(info->var.xres, info->var.yres, 
+					 info->var.bits_per_pixel, VESAFB_NEED_EXACT_RES | VESAFB_NEED_EXACT_DEPTH);
+		if (i > 0)
+			mode = &vbe_modes[i];
+	}
+
+	if (mode == NULL)
+		return -EINVAL;
+	
 	vesafb_create_task (mytask);
 
 	mytask->regs.eax = 0x4f02;
-	mytask->regs.ebx = (u16)info->var.reserved[0] | 0x4000;	/* use LFB */
+	mytask->regs.ebx = mode->mode_id | 0x4000;	/* use LFB */
 
-	if (vbe_ib.vbe_version >= 0x0300 && !nocrtc && 
-	    info->var.reserved[2] != 0xdeadbeef) {
+	if (vbe_ib.vbe_version >= 0x0300 && !nocrtc && !vbemode) {
 
 		mytask->regs.ebx |= 0x0800; /* use CRTC data */
 		crtc = kmalloc(sizeof(struct vesafb_crtc_info_block), GFP_KERNEL);
 	
 		if (!crtc) {
 			err = -ENOMEM;
-			goto sp_end;		
+			goto out;
 		}
 		crtc->horiz_start = info->var.xres + info->var.right_margin;
 		crtc->horiz_end	  = crtc->horiz_start + info->var.hsync_len;
@@ -277,11 +294,15 @@
 	vesafb_queue_task (mytask);
 	vesafb_wait_for_task(mytask);
 
-	if (mytask->regs.eax != 0x004f) {
+	if ((mytask->regs.eax & 0xffff) != 0x004f) {
 		printk(KERN_ERR "vesafb: mode switch failed (eax: 0x%lx)\n", mytask->regs.eax);
 		err = -EINVAL;
-		goto sp_end;
+		goto out;
 	}
+
+	/* the VBE mode number is valid only the first time the mode is set */
+	if (vbemode)
+		vbemode = 0;
 	
 	if (vbe_ib.capabilities & VESAFB_CAP_CAN_SWITCH_DAC && mode->bits_per_pixel <= 8) {
 		mytask->done = 0;
@@ -304,20 +325,28 @@
 		
 	info->fix.visual = (info->var.bits_per_pixel == 8) ? FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
 	info->fix.line_length = mode->bytes_per_scan_line;
-	par->vbe_mode = info->var.reserved[0];
+	par->vbe_mode = mode->mode_id;
 
-	DPRINTK("set new mode %dx%d-%d\n", info->var.xres, info->var.yres, info->var.bits_per_pixel);
+	DPRINTK("set new mode %dx%d-%d (0x%x)\n", info->var.xres, info->var.yres, info->var.bits_per_pixel, mode->mode_id);
 
-sp_end:
-	if (crtc != NULL)
+out:	if (crtc != NULL)
 		kfree(crtc);
 	kfree(mytask);
 
 	return err;
 }
 
-void vesafb_setup_var(struct fb_var_screeninfo *var, struct vesafb_mode_info_block *mode)
+static void vesafb_setup_var(struct fb_var_screeninfo *var, struct fb_info *info, 
+			     struct vesafb_mode_info_block *mode)
 {
+	var->xres = mode->x_res;
+	var->yres = mode->y_res;
+	var->xres_virtual = mode->x_res;
+	var->yres_virtual = info->fix.smem_len / mode->bytes_per_scan_line;
+	var->xoffset = 0;
+	var->yoffset = 0;
+	var->bits_per_pixel = mode->bits_per_pixel;
+
 	if (var->bits_per_pixel == 15)
 		var->bits_per_pixel = 16;
 
@@ -362,7 +391,7 @@
 	}
 }
 
-int inline vesafb_check_limits(struct fb_var_screeninfo *var, struct fb_info *info)
+static int inline vesafb_check_limits(struct fb_var_screeninfo *var, struct fb_info *info)
 {	
 	if (mon_limits)
 		return fb_validate_mode(var, info);
@@ -370,59 +399,34 @@
 		return 0;
 }
 
-int vesafb_find_vbe_mode(int xres, int yres, int bpp, unsigned char flags)
+static int vesafb_find_vbe_mode(int xres, int yres, int bpp, unsigned char flags)
 {
-	int match = -1;
-	int i;
+	int i, match = -1, h = 0, d = 0x7fffffff;
 	
-	DPRINTK("looking for mode: %dx%d-%d\n", xres, yres, bpp);
-
-	/* first try to find the exact mode the user wants to set.. */
 	for (i = 0; i < vbe_modes_cnt; i++) {
 
-		if (vbe_modes[i].x_res == xres && vbe_modes[i].y_res == yres) {
-	
-			int h = bpp - vbe_modes[i].bits_per_pixel;
-			
-			/* ok, we've got an exact match */
-			if (h == 0)
-				return i;
-					
-			if (match == -1 || (bpp - vbe_modes[match].bits_per_pixel) > h)
-				match = i;
-		}
-	}
-	
-	/* .. and if this fails look for similar modes */
-	if (match == -1 && flags) {
-	
-		unsigned int min = 0xffffffff; /* just a big number */
-		unsigned int d;
-		
-		DPRINTK("mode not found (1st pass)\n"); 
-			
-		for (i = 0; i < vbe_modes_cnt; i++) {
+		h = abs(vbe_modes[i].x_res - xres) + abs(vbe_modes[i].y_res - yres) + 
+		    (bpp - vbe_modes[i].bits_per_pixel);
 
-			if (vbe_modes[i].y_res < yres || vbe_modes[i].x_res < xres)
-				continue;
+		if (h == 0)
+			return i;
 
-			d = vbe_modes[i].y_res - yres + vbe_modes[i].x_res - xres;
-					
-			if (d < min) {
-				min = d;
-				match = i;
-			}
-		
-			if (d == min && (bpp - vbe_modes[match].bits_per_pixel) > 
-	  		    (bpp - vbe_modes[i].bits_per_pixel))
-				match = i;
+		if (h < d) {
+			d = h;
+			match = i;
 		}
 	}
 
+	if (!(flags & VESAFB_NEED_EXACT_DEPTH) && d <= 24)
+		return match;
+
+	if (flags & VESAFB_NEED_EXACT_RES)
+		return -1;
+
 	return match;
 }
 
-int vesafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+static int vesafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	int match = -1;
 	
@@ -433,21 +437,17 @@
 	if (var->vmode & (FB_VMODE_INTERLACED | FB_VMODE_DOUBLE))
 		return -EINVAL;
 	
-	match = vesafb_find_vbe_mode(var->xres, var->yres, var->bits_per_pixel, (vbe_ib.vbe_version >= 0x300) ? 1 : 0);
+	match = vesafb_find_vbe_mode(var->xres, var->yres, var->bits_per_pixel, VESAFB_NEED_EXACT_RES);
 
 	if (match == -1) {
 		printk(KERN_ERR "vesafb: mode %dx%d-%d@%d not found\n", var->xres, var->yres, var->bits_per_pixel,
 				(int)(PICOS2KHZ(info->var.pixclock) /
 				((info->var.xres + info->var.right_margin + info->var.hsync_len + info->var.left_margin) *
 				(info->var.yres + info->var.lower_margin + info->var.vsync_len + info->var.upper_margin))) * 1000);
-		var->reserved[0] = 0xffff;
 		return -EINVAL;
 	} else {
 		var->bits_per_pixel = vbe_modes[match].bits_per_pixel;
-		var->reserved[0] = (u32)vbe_modes[match].mode_id;
-		var->reserved[1] = (u32)(&vbe_modes[match]);
-		var->reserved[2] = 0x0;
-		vesafb_setup_var(var, &vbe_modes[match]);
+		vesafb_setup_var(var, info, &vbe_modes[match]);
 	
 		DPRINTK("found mode 0x%x (%dx%d-%dbpp)\n",
 			vbe_modes[match].mode_id, vbe_modes[match].x_res, vbe_modes[match].y_res, 
@@ -589,9 +589,8 @@
 {
 	struct vesafb_task *mytask;
 	u16 *mode = 0;
-	int off = 0;
-	int i;
-	
+	int i, off = 0;
+
 	vesafb_create_task (mytask);
 	mytask->regs.eax = 0x4f00;
 	mytask->type = VESAFB_TASK_GETVBE_IB;
@@ -605,7 +604,7 @@
 		return 1;
 	}
 	
-	if (mytask->regs.eax != 0x004F) {
+	if ((mytask->regs.eax & 0xffff) != 0x004f) {
 		printk(KERN_ERR "vesafb: Getting mode info block failed (eax=0x%x)\n",(u32)mytask->regs.eax);
 		kfree(mytask);
 		return 1;	
@@ -660,7 +659,7 @@
 	vesafb_queue_task(mytask);
 	vesafb_wait_for_task(mytask);
 
-	if (mytask->regs.eax != 0x004f || mytask->regs.es < 0xc000) {
+	if ((mytask->regs.eax & 0xffff) != 0x004f || mytask->regs.es < 0xc000) {
 		pmi_setpal = ypan = 0;
 	} else {
 		printk(KERN_INFO "vesafb: protected mode interface info at %04x:%04x\n", (u16)mytask->regs.es, (u16)mytask->regs.edi);
@@ -689,7 +688,7 @@
 	}
 
 	if (noedid || vbe_ib.vbe_version < 0x0300)
-		goto vi_1;
+		goto set_monspecs;
 	
 	mytask->regs.eax = 0x4f15;
 	mytask->regs.ebx = 0;
@@ -699,8 +698,8 @@
 	vesafb_queue_task(mytask);
 	vesafb_wait_for_task(mytask);
 
-	if (mytask->regs.eax != 0x004f)
-		goto vi_1;
+	if ((mytask->regs.eax & 0xffff) != 0x004f)
+		goto set_monspecs;
 
 	if ((mytask->regs.ebx & 0x3) == 3) {
 		printk(KERN_INFO "vesafb: hardware supports both DCC1 and DCC2 transfers\n");
@@ -710,7 +709,7 @@
 		printk(KERN_INFO "vesafb: hardware supports DCC1 transfers\n");
 	} else {
 		printk(KERN_INFO "vesafb: hardware doesn't support DCC transfers\n");
-		goto vi_1;
+		goto set_monspecs;
 	}
 	
 	mytask->regs.eax = 0x4f15;
@@ -722,7 +721,7 @@
 	vesafb_queue_task(mytask);
 	vesafb_wait_for_task(mytask);
 
-	if (mytask->regs.eax == 0x004F) {
+	if ((mytask->regs.eax & 0xffff) == 0x004f) {
 
 		mon_limits = !fb_get_monitor_limits(mytask->res, &info->monspecs);
 
@@ -735,7 +734,7 @@
 	}
 	kfree(mytask->res);
 	
-vi_1:
+set_monspecs:
 	if (maxclk)
 		info->monspecs.dclkmax = maxclk * 1000000;
 
@@ -761,13 +760,172 @@
 	return 0;
 }
 
+static int __init decode_mode(u32 *xres, u32 *yres, u32 *bpp, u32 *refresh)
+{
+	int len = strlen(mode_option), i, err = 0;
+	u8 res_specified = 0, bpp_specified = 0, refresh_specified = 0, 
+	   yres_specified = 0;
+
+	for (i = len-1; i >= 0; i--) {
+ 		switch (mode_option[i]) {
+		case '@':
+    			len = i;
+    			if (!refresh_specified && !bpp_specified &&
+			    !yres_specified) {
+				*refresh = simple_strtoul(&mode_option[i+1], NULL, 0);
+				refresh_specified = 1;
+			} else
+				goto out;
+		    	break;
+		case '-':
+			len = i;
+		    	if (!bpp_specified && !yres_specified) {
+			    	*bpp = simple_strtoul(&mode_option[i+1], NULL, 0);
+				bpp_specified = 1;
+		    	} else
+				goto out;
+		    	break;
+		case 'x':
+			if (!yres_specified) {
+				*yres = simple_strtoul(&mode_option[i+1], NULL, 0);
+				yres_specified = 1;
+		    	} else
+				goto out;
+		    	break;
+		case '0'...'9':
+			break;
+		default:
+			goto out;
+	    	}
+	}
+
+	if (i < 0 && yres_specified) {
+		*xres = simple_strtoul(mode_option, NULL, 0);
+	   	res_specified = 1;
+	}
+
+out:	if (!res_specified || !yres_specified) {
+		printk(KERN_ERR "vesafb: invalid resolution, %s not specified\n",
+				(!res_specified) ? "width" : "height");
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static int __init vesafb_init_set_mode(struct fb_info *info)
+{
+	char buf[32];
+	int i, modeid, refresh = 0;
+	u8 refresh_specified = 0;
+
+	if (!mode_option)
+		mode_option = CONFIG_FB_VESA_DEFAULT_MODE;
+
+	if (vbemode > 0) {
+		for (i = 0; i < vbe_modes_cnt; i++) {
+			if (vbe_modes[i].mode_id == vbemode) {
+				info->var.vmode = FB_VMODE_NONINTERLACED;
+				info->var.sync = FB_SYNC_VERT_HIGH_ACT;
+				vesafb_setup_var(&info->var, info, &vbe_modes[i]);
+				fb_get_mode(FB_VSYNCTIMINGS | FB_IGNOREMON, 60, &info->var, info);
+				return i;
+			}
+		}
+
+		printk(KERN_INFO "specified VBE mode %d not found\n",vbemode);
+	}
+
+	if (decode_mode(&info->var.xres, &info->var.yres, 
+			&info->var.bits_per_pixel, &refresh))
+		return -EINVAL;
+
+	if (refresh)
+		refresh_specified = 1;
+	else	
+		refresh = 60;
+	
+	modeid = vesafb_find_vbe_mode(info->var.xres, info->var.yres, info->var.bits_per_pixel, 0);
+
+	if (modeid == -1) {
+		return -EINVAL;	
+	} else {
+		info->var.vmode = FB_VMODE_NONINTERLACED;
+		info->var.sync = FB_SYNC_VERT_HIGH_ACT;
+		vesafb_setup_var(&info->var, info, &vbe_modes[modeid]);
+	}
+
+	if (gtf)
+		goto timings_gtf;
+
+	sprintf(buf, "%dx%d-%d@%d", info->var.xres, info->var.yres, info->var.bits_per_pixel, refresh);
+	i = fb_find_mode(&info->var, info, buf, vesa_modes, VESA_MODEDB_SIZE, NULL, 0);
+	DPRINTK("fb_find_mode returned %d\n", i);
+
+	if (i == 1 || (i == 2 && vbe_ib.vbe_version < 0x0300))
+		return modeid;
+
+	if (edid_modes != NULL && !gtf) {
+		DPRINTK("looking for EDID modes\n");
+		
+		for (i = 0; i < edid_modes_cnt; i++) {
+
+			if (edid_modes[i].xres == info->var.xres && 
+			    edid_modes[i].yres == info->var.yres && 
+			    abs(edid_modes[i].refresh - refresh) < 5) {
+
+				info->var.pixclock = edid_modes[i].pixclock;
+			    	info->var.left_margin = edid_modes[i].left_margin;
+				info->var.right_margin = edid_modes[i].right_margin;
+				info->var.upper_margin = edid_modes[i].upper_margin;
+				info->var.lower_margin = edid_modes[i].lower_margin;
+				info->var.hsync_len = edid_modes[i].hsync_len;
+				info->var.vsync_len = edid_modes[i].vsync_len;
+				info->var.sync = edid_modes[i].sync;
+				info->var.vmode = edid_modes[i].vmode;
+				DPRINTK("using EDID-provided mode\n");
+				return modeid;
+			}
+		}
+	}
+
+timings_gtf:
+	if (refresh_specified)
+		i = FB_VSYNCTIMINGS;
+	else
+		i = FB_MAXTIMINGS;
+		
+	if (vbe_ib.vbe_version < 0x0300) { 
+		i = FB_VSYNCTIMINGS | FB_IGNOREMON;
+		refresh = 60;
+	} 
+	
+	if (!mon_limits)
+		i |= FB_IGNOREMON;
+	
+	if (!fb_get_mode(i, refresh, &info->var, info))
+		return modeid;
+
+	printk(KERN_WARNING "vesafb: trying maximum allowed refresh rate\n");
+
+	if (i & FB_VSYNCTIMINGS && refresh > 60 && mon_limits) {
+		if (!fb_get_mode(FB_MAXTIMINGS, refresh, &info->var, info))
+			return modeid;
+	}
+
+	printk(KERN_WARNING "vesafb: using default BIOS refresh rate\n");
+	vbemode = vbe_modes[modeid].mode_id;
+
+	return modeid;
+}
+
 static int __init vesafb_probe(struct device *device)
 {
 	char entry[16];
 	struct platform_device *dev = to_platform_device(device);
 	struct fb_info *info;
 	int err = 0, i;
-
+	
 	vesafb_info = info = framebuffer_alloc(sizeof(struct vesafb_par) + sizeof(u32) * 256, &dev->dev);
 	
 	if (!info)
@@ -776,18 +934,18 @@
 	if (vesafb_pid)
 		vesafb_serv_thread = find_task_by_pid(vesafb_pid);
 	else {
-		printk(KERN_ERR "vesafb: vesafb thread not running - returning..\n");
+		printk(KERN_ERR "vesafb: vesafb thread not running\n");
 		framebuffer_release(info);
 		return -EINVAL;
 	}
 
 	if (vesafb_vbe_init(info)) {
-		printk(KERN_ERR "vesafb: vbe_init failed - returning..\n");
+		printk(KERN_ERR "vesafb: vbe_init failed\n");
 		err = -EINVAL;
-		goto pr_err;
+		goto out;
 	}
 
-	vesafb_fix.smem_len = vbe_ib.total_memory * 65536;
+	vesafb_fix.smem_len  = vbe_ib.total_memory * 65536;
 	vesafb_fix.ypanstep  = ypan     ? 1 : 0;
 	vesafb_fix.ywrapstep = (ypan>1) ? 1 : 0;
 
@@ -811,162 +969,14 @@
 
 	if (fb_alloc_cmap(&info->cmap, 256, 0) < 0) {
 		err = -ENXIO;
-		goto pr_err;
+		goto out;
 	}	
 
-	if (!mode_option)
-		mode_option = CONFIG_FB_VESA_DEFAULT_MODE;
-
-	if (vbemode > 0) {
-		for (i = 0; i < vbe_modes_cnt; i++) {
-			if (vbe_modes[i].mode_id == vbemode) {
-				info->var.xres = vbe_modes[i].x_res;
-				info->var.yres = vbe_modes[i].y_res;
-				info->var.xres_virtual = vbe_modes[i].x_res;
-				info->var.xoffset = 0;
-				info->var.yoffset = 0;
-				info->var.bits_per_pixel = vbe_modes[i].bits_per_pixel;
-				info->var.reserved[0] = (u32)vbe_modes[i].mode_id;
-				info->var.reserved[1] = (u32)(&vbe_modes[i]);
-				info->var.reserved[2] = 0xdeadbeef;
-				info->var.vmode = FB_VMODE_NONINTERLACED;
-				info->var.sync = FB_SYNC_VERT_HIGH_ACT;
-				vesafb_setup_var(&info->var, &vbe_modes[i]);
-				fb_get_mode(FB_MAXTIMINGS, 60, &info->var, info);
-				goto pr_end;
-			}
-		}
+	i = vesafb_init_set_mode(info);
+	if (i < 0)
+		goto out_cmap;
 
-		printk(KERN_INFO "specified VBE mode %d not found\n",vbemode);
-	}
-
-	if (gtf)
-		goto pr_manual;
-
-	i = fb_find_mode(&info->var, info, mode_option, vesa_modes, 33, NULL, 0);
-
-	DPRINTK("fb_find_mode returned %d\n", i);
-	
-	if (i == 0 || i >= 3) 
-pr_manual:	
-	{
-		int match = -1;
-		unsigned int len = strlen(mode_option);
-		unsigned int xres = 0, yres = 0, bpp = 8, refresh = 60;
-		unsigned char res_specified = 0, bpp_specified = 0, refresh_specified = 0, yres_specified = 0;
-	
-		for (i = len-1; i >= 0; i--) {
-	    		switch (mode_option[i]) {
-				case '@':
-		    			len = i;
-		    			if (!refresh_specified && !bpp_specified &&
-					    !yres_specified) {
-						refresh = simple_strtoul(&mode_option[i+1], NULL, 0);
-						refresh_specified = 1;
-		    			} else
-						goto pr_modedone;
-		    			break;
-				case '-':
-		    			len = i;
-		    			if (!bpp_specified && !yres_specified) {
-					    	bpp = simple_strtoul(&mode_option[i+1], NULL, 0);
-						bpp_specified = 1;
-		    			} else
-						goto pr_modedone;
-		    			break;
-				case 'x':
-		    			if (!yres_specified) {
-						yres = simple_strtoul(&mode_option[i+1], NULL, 0);
-						yres_specified = 1;
-		    			} else
-						goto pr_modedone;
-		    			break;
-				case '0'...'9':
-		    			break;
-				default:
-		    			goto pr_modedone;
-	    		}
-		}
-
-		if (i < 0 && yres_specified) {
-	    		xres = simple_strtoul(mode_option, NULL, 0);
-	    		res_specified = 1;
-		}
-
-pr_modedone:	if (!res_specified || !yres_specified) {
-			printk(KERN_ERR "vesafb: invalid resolution, %s not specified\n",
-					(!res_specified) ? "width" : "height");
-			err = -EINVAL;
-			goto pr_err1;
-		}
-	
-		match = vesafb_find_vbe_mode(xres, yres, bpp, (vbe_ib.vbe_version >= 0x300) ? 1 : 0);
-		
-		if (match == -1) {
-			printk(KERN_ERR "vesafb: no matching VBE mode found\n");
-			err = -EINVAL;
-			goto pr_err1;
-		}
-		
-		info->var.xres = xres;
-		info->var.yres = yres;
-		info->var.xres_virtual = xres;
-		info->var.xoffset = 0;
-		info->var.yoffset = 0;
-		info->var.bits_per_pixel = vbe_modes[match].bits_per_pixel;
-		info->var.reserved[0] = (u32)vbe_modes[match].mode_id;
-		info->var.reserved[1] = (u32)(&vbe_modes[match]);
-		info->var.reserved[2] = 0x0;
-		info->var.vmode = FB_VMODE_NONINTERLACED;
-		info->var.sync = FB_SYNC_VERT_HIGH_ACT;
-		vesafb_setup_var(&info->var, &vbe_modes[match]);
-
-		if (edid_modes != NULL && !gtf) {
-		
-			DPRINTK("looking for EDID modes\n");
-		
-			for (i = 0; i < edid_modes_cnt; i++) {
-
-				if (edid_modes[i].xres == xres && edid_modes[i].yres == yres && 
-				    edid_modes[i].refresh - refresh < 5 && edid_modes[i].refresh - refresh > -5) {
-
-					info->var.pixclock = edid_modes[i].pixclock;
-				    	info->var.left_margin = edid_modes[i].left_margin;
-					info->var.right_margin = edid_modes[i].right_margin;
-					info->var.upper_margin = edid_modes[i].upper_margin;
-					info->var.lower_margin = edid_modes[i].lower_margin;
-					info->var.hsync_len = edid_modes[i].hsync_len;
-					info->var.vsync_len = edid_modes[i].vsync_len;
-					info->var.sync = edid_modes[i].sync;
-					info->var.vmode = edid_modes[i].vmode;
-					DPRINTK("using EDID-provided mode\n");
-					goto pr_end;
-				}
-			}
-		}
-		
-		if (refresh_specified)
-			i = FB_VSYNCTIMINGS;
-		else
-			i = FB_MAXTIMINGS;
-			
-		if (vbe_ib.vbe_version < 0x0300) { 
-			i = FB_VSYNCTIMINGS | FB_IGNOREMON;
-			refresh = 60;
-		} 
-	
-		if (!mon_limits)
-			i |= FB_IGNOREMON;
-		
-		if (fb_get_mode(i, refresh, &info->var, info) != 0) {
-			printk(KERN_ERR "vesafb: fb_get_mode failed, try a different refresh rate.\n");
-			err = -EINVAL;
-			goto pr_err1;
-		}
-	}
-pr_end:
-	info->var.yres_virtual = info->fix.smem_len / ((struct vesafb_mode_info_block*)info->var.reserved[1])->bytes_per_scan_line;
-	info->fix.smem_start = ((struct vesafb_mode_info_block*)info->var.reserved[1])->phys_base_ptr;
+	info->fix.smem_start = vbe_modes[i].phys_base_ptr;
 
 	if (ypan && info->var.yres_virtual > info->var.yres) {
 		printk(KERN_INFO "vesafb: scrolling: %s using protected mode interface, yres_virtual=%d\n",
@@ -993,7 +1003,7 @@
 		       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
 			info->fix.smem_len, info->fix.smem_start);
 		err = -EIO;
-		goto pr_err2;
+		goto out_mem;
  	}
 
 	/* request failure does not faze us, as vgacon probably has this
@@ -1016,7 +1026,7 @@
 	if (register_framebuffer(info) < 0) {
 		printk(KERN_ERR "vesafb: failed to register framebuffer device\n");
 		err = -EINVAL;
-		goto pr_err2;
+		goto out_mem;
 	}
 
   	printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
@@ -1035,11 +1045,11 @@
 	fb_destroy_modedb(edid_modes);
 	return 0;
 
-pr_err2:
+out_mem:
 	release_mem_region(info->fix.smem_start, info->fix.smem_len);
-pr_err1:
+out_cmap:
 	fb_dealloc_cmap(&info->cmap);
-pr_err:	
+out:	
 	framebuffer_release(info);
 	vesafb_info = NULL;
 
@@ -1145,5 +1155,5 @@
 #endif /* MODULE */
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Micha³ Januszewski");
-MODULE_DESCRIPTION("Framebuffer driver for VBE2.0-compliant graphic boards");
+MODULE_AUTHOR("Michal Januszewski");
+MODULE_DESCRIPTION("Framebuffer driver for VBE2.0+ compliant graphic boards");