--- linux-2.6.9-rc1.orig/drivers/video/riva/fbdev.c	2004-08-24 20:34:24.000000000 +0200
+++ linux-2.6.9-rc1/drivers/video/riva/fbdev.c	2004-08-29 15:38:48.000000000 +0200
@@ -203,6 +203,7 @@
  * ------------------------------------------------------------------------- */
 
 /* command line data, set in rivafb_setup() */
+static u32 pseudo_palette[17];
 static int flatpanel __initdata = -1; /* Autodetect later */
 static int forceCRTC __initdata = -1;
 #ifdef CONFIG_MTRR
@@ -448,6 +451,7 @@
 		
 		for (j = 0; j < w/2; j++) {
 			tmp = 0;
+
 #if defined (__BIG_ENDIAN)
 			tmp = (b & (1 << 31)) ? fg << 16 : bg << 16;
 			b <<= 1;
@@ -514,7 +518,7 @@
 		       unsigned char *green, unsigned char *blue)
 {
 	
-	VGA_WR08(chip->PDIO, 0x3c7, regnum);
+	VGA_WR08(chip->PDIO, 0x3c8, regnum);
 	*red = VGA_RD08(chip->PDIO, 0x3c9);
 	*green = VGA_RD08(chip->PDIO, 0x3c9);
 	*blue = VGA_RD08(chip->PDIO, 0x3c9);
@@ -1006,7 +1010,7 @@
 			par->state.flags |= VGA_SAVE_CMAP;
 		save_vga(&par->state);
 #endif
-		riva_common_setup(par);
+
 		RivaGetConfig(&par->riva, par->Chipset);
 		/* vgaHWunlock() + riva unlock (0x7F) */
 		CRTCout(par, 0x11, 0xFF);
@@ -1043,8 +1047,7 @@
 
 static int rivafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
-	struct fb_videomode *mode;
-	struct riva_par *par = (struct riva_par *) info->par;
+	struct fb_monspecs *specs = &info->monspecs;
 	int nom, den;		/* translating from pixels->bytes */
 	int mode_valid = 0;
 	
@@ -1061,9 +1064,6 @@
 		/* fall through */
 	case 16:
 		var->bits_per_pixel = 16;
-		/* The Riva128 supports RGB555 only */
-		if (par->riva.Architecture == NV_ARCH_03)
-			var->green.length = 5;
 		if (var->green.length == 5) {
 			/* 0rrrrrgg gggbbbbb */
 			var->red.offset = 10;
@@ -1106,21 +1106,62 @@
 			mode_valid = 1;
 	}
 
-	/* calculate modeline if supported by monitor */
-	if (!mode_valid && info->monspecs.gtf) {
-		if (!fb_get_mode(FB_MAXTIMINGS, 0, var, info))
-			mode_valid = 1;
+	/* find best mode from modedb */
+	if (!mode_valid && specs->modedb_len) {
+		int i, best, best_refresh, best_x, best_y, diff_x, diff_y;
+
+		best_refresh = best = best_x = best_y = 0;
+		diff_x = diff_y = -1;
+
+		for (i = 0; i < specs->modedb_len; i++) {
+			if (var->xres <= specs->modedb[i].xres &&
+			    !(specs->modedb[i].flag & FB_MODE_IS_CALCULATED) &&
+			    specs->modedb[i].xres - var->xres < diff_x) {
+				best_x = specs->modedb[i].xres;
+				diff_x = best_x - var->xres;
+			}
+			if (!diff_x) break;
+		}
+
+		if (diff_x != -1) {
+			for (i = 0; i < specs->modedb_len; i++) {
+				if (best_x == specs->modedb[i].xres &&
+				    var->yres <= specs->modedb[i].yres &&
+				    !(specs->modedb[i].flag &
+				      FB_MODE_IS_CALCULATED) &&
+				    specs->modedb[i].yres-var->yres < diff_y) {
+					best_y = specs->modedb[i].yres;
+					diff_y = best_y - var->yres;
+				}
+				if (!diff_y) break;
+			}
+		}
+
+		if (diff_y != -1) {
+			for (i = 0; i < specs->modedb_len; i++) {
+				if (best_x == specs->modedb[i].xres &&
+				    best_y == specs->modedb[i].yres &&
+				    !(specs->modedb[i].flag &
+				      FB_MODE_IS_CALCULATED) &&
+				    specs->modedb[i].refresh > best_refresh) {
+					best_refresh=specs->modedb[i].refresh;
+					best = i;
+				}
+			}
 	}
 
-	if (!mode_valid) {
-		mode = fb_find_best_mode(var, &info->monspecs.modelist);
-		if (mode) {
-			riva_update_var(var, mode);
+		if (best_refresh) {
+			riva_update_var(var, &specs->modedb[best]);
 			mode_valid = 1;
 		}
 	}
 
-	if (!mode_valid && !list_empty(&info->monspecs.modelist))
+	/* calculate modeline if supported by monitor */
+	if (!mode_valid && info->monspecs.gtf) {
+		if (!fb_get_mode(FB_MAXTIMINGS, 0, var, info))
+			mode_valid = 1;
+	}
+	if (!mode_valid && info->monspecs.modedb_len)
 		return -EINVAL;
 
 	if (var->xres_virtual < var->xres)
@@ -1155,6 +1196,12 @@
 	struct riva_par *par = (struct riva_par *) info->par;
 
 	NVTRACE_ENTER();
+	riva_common_setup(par);
+	RivaGetConfig(&par->riva, par->Chipset);
+	/* vgaHWunlock() + riva unlock (0x7F) */
+	CRTCout(par, 0x11, 0xFF);
+	par->riva.LockUnlock(&par->riva, 0);
+
 	riva_load_video_mode(info);
 	riva_setup_accel(info);
 	
@@ -1162,7 +1209,6 @@
 	info->fix.line_length = (info->var.xres_virtual * (info->var.bits_per_pixel >> 3));
 	info->fix.visual = (info->var.bits_per_pixel == 8) ?
 				FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_DIRECTCOLOR;
-
 	NVTRACE_LEAVE();
 	return 0;
 }
@@ -1291,31 +1337,6 @@
 		    (red * 77 + green * 151 + blue * 28) >> 8;
 	}
 
-	if (regno < 16 && info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
-		((u32 *) info->pseudo_palette)[regno] =
-			(regno << info->var.red.offset) |
-			(regno << info->var.green.offset) |
-			(regno << info->var.blue.offset);
-		/*
-		 * The Riva128 2D engine requires color information in
-		 * TrueColor format even if framebuffer is in DirectColor
-		 */
-		if (par->riva.Architecture == NV_ARCH_03) {
-			switch (info->var.bits_per_pixel) {
-			case 16:
-				par->palette[regno] = ((red & 0xf800) >> 1) |
-					((green & 0xf800) >> 6) |
-					((blue & 0xf800) >> 11);
-				break;
-			case 32:
-				par->palette[regno] = ((red & 0xff00) << 8) |
-					((green & 0xff00)) |
-					((blue & 0xff00) >> 8);
-				break;
-			}
-		}
-	}
-
 	switch (info->var.bits_per_pixel) {
 	case 8:
 		/* "transparent" stuff is completely ignored. */
@@ -1323,27 +1344,45 @@
 		break;
 	case 16:
 		if (info->var.green.length == 5) {
-			for (i = 0; i < 8; i++) {
+			if (regno < 16) {
+				/* 0rrrrrgg gggbbbbb */
+				((u32 *)info->pseudo_palette)[regno] =
+					((red & 0xf800) >> 1) |
+					((green & 0xf800) >> 6) |
+					((blue & 0xf800) >> 11);
+			}
+			for (i = 0; i < 8; i++) 
 				riva_wclut(chip, regno*8+i, red >> 8,
 					   green >> 8, blue >> 8);
-			}
 		} else {
 			u8 r, g, b;
 
+			if (regno < 16) {
+				/* rrrrrggg gggbbbbb */
+				((u32 *)info->pseudo_palette)[regno] =
+					((red & 0xf800) >> 0) |
+					((green & 0xf800) >> 5) |
+					((blue & 0xf800) >> 11);
+			}
 			if (regno < 32) {
 				for (i = 0; i < 8; i++) {
-					riva_wclut(chip, regno*8+i,
-						   red >> 8, green >> 8,
-						   blue >> 8);
+					riva_wclut(chip, regno*8+i, red >> 8, 
+						   green >> 8, blue >> 8);
+				}
 				}
+			for (i = 0; i < 4; i++) {
+				riva_rclut(chip, regno*2+i, &r, &g, &b);
+				riva_wclut(chip, regno*4+i, r, green >> 8, b);
 			}
-			riva_rclut(chip, regno*4, &r, &g, &b);
-			for (i = 0; i < 4; i++)
-				riva_wclut(chip, regno*4+i, r,
-					   green >> 8, b);
 		}
 		break;
 	case 32:
+		if (regno < 16) {
+			((u32 *)info->pseudo_palette)[regno] =
+				((red & 0xff00) << 8) |
+				((green & 0xff00)) | ((blue & 0xff00) >> 8);
+			
+		}
 		riva_wclut(chip, regno, red >> 8, green >> 8, blue >> 8);
 		break;
 	default:
@@ -1372,12 +1411,8 @@
 
 	if (info->var.bits_per_pixel == 8)
 		color = rect->color;
-	else {
-		if (par->riva.Architecture != NV_ARCH_03)
-			color = ((u32 *)info->pseudo_palette)[rect->color];
 		else
-			color = par->palette[rect->color];
-	}
+		color = ((u32 *)info->pseudo_palette)[rect->color];
 
 	switch (rect->rop) {
 	case ROP_XOR:
@@ -1473,17 +1508,15 @@
 		bgx = image->bg_color;
 		break;
 	case 16:
-	case 32:
-		if (par->riva.Architecture != NV_ARCH_03) {
 			fgx = ((u32 *)info->pseudo_palette)[image->fg_color];
 			bgx = ((u32 *)info->pseudo_palette)[image->bg_color];
-		} else {
-			fgx = par->palette[image->fg_color];
-			bgx = par->palette[image->bg_color];
-		}
 		if (info->var.green.length == 6)
 			convert_bgcolor_16(&bgx);	
 		break;
+	case 32:
+		fgx = ((u32 *)info->pseudo_palette)[image->fg_color];
+		bgx = ((u32 *)info->pseudo_palette)[image->bg_color];
+		break;
 	}
 
 	RIVA_FIFO_FREE(par->riva, Bitmap, 7);
@@ -1652,7 +1685,6 @@
 static int __devinit riva_set_fbinfo(struct fb_info *info)
 {
 	unsigned int cmap_len;
-	struct riva_par *par = (struct riva_par *) info->par;
 
 	NVTRACE_ENTER();
 	info->flags = FBINFO_DEFAULT
@@ -1665,8 +1697,7 @@
 	info->var = rivafb_default_var;
 	info->fix.visual = (info->var.bits_per_pixel == 8) ?
 				FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_DIRECTCOLOR;
-
-	info->pseudo_palette = par->pseudo_palette;
+	info->pseudo_palette = pseudo_palette;
 
 	cmap_len = riva_get_cmap_len(&info->var);
 	fb_alloc_cmap(&info->cmap, cmap_len, 0);	
@@ -1788,8 +1819,6 @@
 	struct riva_par *par = (struct riva_par *) info->par;
 
 	fb_edid_to_monspecs(par->EDID, &info->monspecs);
-	fb_videomode_to_modelist(info->monspecs.modedb, info->monspecs.modedb_len,
-				 &info->monspecs.modelist);
 	riva_update_default_var(var, info);
 
 	/* if user specified flatpanel, we respect that */
@@ -1881,7 +1910,6 @@
 
 	default_par->Chipset = (pd->vendor << 16) | pd->device;
 	printk(KERN_INFO PFX "nVidia device/chipset %X\n",default_par->Chipset);
-	
 #ifdef CONFIG_PCI_NAMES
 	printk(KERN_INFO PFX "%s\n", pd->pretty_name);
 #endif
@@ -1991,10 +2019,6 @@
 		goto err_out_iounmap_fb;
 	}
 
-	fb_destroy_modedb(info->monspecs.modedb);
-	info->monspecs.modedb_len = 0;
-	info->monspecs.modedb = NULL;
-
 	if (register_framebuffer(info) < 0) {
 		printk(KERN_ERR PFX
 			"error registering riva framebuffer\n");
@@ -2069,6 +2093,7 @@
 		iounmap((caddr_t)par->riva.PRAMIN);
 	pci_release_regions(pd);
 	pci_disable_device(pd);
+	fb_destroy_modedb(info->monspecs.modedb);
 	kfree(info->pixmap.addr);
 	kfree(par);
 	kfree(info);