1 files changed, 199 insertions, 324 deletions

diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index d8df17a7d5f..ce7783b63f6 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -26,18 +26,22 @@
 
 #include "global.h"
 
-static struct fb_var_screeninfo default_var;
 static char *viafb_name = "Via";
 static u32 pseudo_pal[17];
 
 /* video mode */
-static char *viafb_mode = "640x480";
-static char *viafb_mode1 = "640x480";
+static char *viafb_mode;
+static char *viafb_mode1;
+static int viafb_bpp = 32;
+static int viafb_bpp1 = 32;
+
+static unsigned int viafb_second_offset;
+static int viafb_second_size;
 
 static int viafb_accel = 1;
 
 /* Added for specifying active devices.*/
-char *viafb_active_dev = "";
+char *viafb_active_dev;
 
 /*Added for specify lcd output port*/
 char *viafb_lcd_port = "";
@@ -50,18 +54,78 @@ static void apply_second_mode_setting(struct fb_var_screeninfo
 	*sec_var);
 static void retrieve_device_setting(struct viafb_ioctl_setting
 	*setting_info);
+static int viafb_pan_display(struct fb_var_screeninfo *var,
+	struct fb_info *info);
 
 static struct fb_ops viafb_ops;
 
 
+static void viafb_fill_var_color_info(struct fb_var_screeninfo *var, u8 depth)
+{
+	var->grayscale = 0;
+	var->red.msb_right = 0;
+	var->green.msb_right = 0;
+	var->blue.msb_right = 0;
+	var->transp.offset = 0;
+	var->transp.length = 0;
+	var->transp.msb_right = 0;
+	var->nonstd = 0;
+	switch (depth) {
+	case 8:
+		var->bits_per_pixel = 8;
+		var->red.offset = 0;
+		var->green.offset = 0;
+		var->blue.offset = 0;
+		var->red.length = 8;
+		var->green.length = 8;
+		var->blue.length = 8;
+		break;
+	case 15:
+		var->bits_per_pixel = 16;
+		var->red.offset = 10;
+		var->green.offset = 5;
+		var->blue.offset = 0;
+		var->red.length = 5;
+		var->green.length = 5;
+		var->blue.length = 5;
+		break;
+	case 16:
+		var->bits_per_pixel = 16;
+		var->red.offset = 11;
+		var->green.offset = 5;
+		var->blue.offset = 0;
+		var->red.length = 5;
+		var->green.length = 6;
+		var->blue.length = 5;
+		break;
+	case 24:
+		var->bits_per_pixel = 32;
+		var->red.offset = 16;
+		var->green.offset = 8;
+		var->blue.offset = 0;
+		var->red.length = 8;
+		var->green.length = 8;
+		var->blue.length = 8;
+		break;
+	case 30:
+		var->bits_per_pixel = 32;
+		var->red.offset = 20;
+		var->green.offset = 10;
+		var->blue.offset = 0;
+		var->red.length = 10;
+		var->green.length = 10;
+		var->blue.length = 10;
+		break;
+	}
+}
+
 static void viafb_update_fix(struct fb_info *info)
 {
 	u32 bpp = info->var.bits_per_pixel;
 
 	info->fix.visual =
 		bpp == 8 ? FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
-	info->fix.line_length =
-		((info->var.xres_virtual + 7) & ~7) * bpp / 8;
+	info->fix.line_length = (info->var.xres_virtual * bpp / 8 + 7) & ~7;
 }
 
 static void viafb_setup_fixinfo(struct fb_fix_screeninfo *fix,
@@ -75,6 +139,7 @@ static void viafb_setup_fixinfo(struct fb_fix_screeninfo *fix,
 
 	fix->type = FB_TYPE_PACKED_PIXELS;
 	fix->type_aux = 0;
+	fix->visual = FB_VISUAL_TRUECOLOR;
 
 	fix->xpanstep = fix->ywrapstep = 0;
 	fix->ypanstep = 1;
@@ -97,9 +162,10 @@ static int viafb_release(struct fb_info *info, int user)
 static int viafb_check_var(struct fb_var_screeninfo *var,
 	struct fb_info *info)
 {
-	int vmode_index, htotal, vtotal;
+	int htotal, vtotal, depth;
+	struct VideoModeTable *vmode_entry;
 	struct viafb_par *ppar = info->par;
-	u32 long_refresh;
+	u32 long_refresh, line;
 
 	DEBUG_MSG(KERN_INFO "viafb_check_var!\n");
 	/* Sanity check */
@@ -107,26 +173,36 @@ static int viafb_check_var(struct fb_var_screeninfo *var,
 	if (var->vmode & FB_VMODE_INTERLACED || var->vmode & FB_VMODE_DOUBLE)
 		return -EINVAL;
 
-	vmode_index = viafb_get_mode_index(var->xres, var->yres);
-	if (vmode_index == VIA_RES_INVALID) {
+	vmode_entry = viafb_get_mode(var->xres, var->yres);
+	if (!vmode_entry) {
 		DEBUG_MSG(KERN_INFO
 			  "viafb: Mode %dx%dx%d not supported!!\n",
 			  var->xres, var->yres, var->bits_per_pixel);
 		return -EINVAL;
 	}
 
-	if (24 == var->bits_per_pixel)
-		var->bits_per_pixel = 32;
+	depth = fb_get_color_depth(var, &info->fix);
+	if (!depth)
+		depth = var->bits_per_pixel;
 
-	if (var->bits_per_pixel != 8 && var->bits_per_pixel != 16 &&
-		var->bits_per_pixel != 32)
+	if (depth < 0 || depth > 32)
 		return -EINVAL;
+	else if (!depth)
+		depth = 24;
+	else if (depth == 15 && viafb_dual_fb && ppar->iga_path == IGA1)
+		depth = 15;
+	else if (depth == 30)
+		depth = 30;
+	else if (depth <= 8)
+		depth = 8;
+	else if (depth <= 16)
+		depth = 16;
+	else
+		depth = 24;
 
-	if ((var->xres_virtual * (var->bits_per_pixel >> 3)) & 0x1F)
-		/*32 pixel alignment */
-		var->xres_virtual = (var->xres_virtual + 31) & ~31;
-	if (var->xres_virtual * var->yres_virtual * var->bits_per_pixel / 8 >
-		ppar->memsize)
+	viafb_fill_var_color_info(var, depth);
+	line = (var->xres_virtual * var->bits_per_pixel / 8 + 7) & ~7;
+	if (line * var->yres_virtual > ppar->memsize)
 		return -EINVAL;
 
 	/* Based on var passed in to calculate the refresh,
@@ -142,7 +218,7 @@ static int viafb_check_var(struct fb_var_screeninfo *var,
 	viafb_refresh = viafb_get_refresh(var->xres, var->yres, long_refresh);
 
 	/* Adjust var according to our driver's own table */
-	viafb_fill_var_timing_info(var, viafb_refresh, vmode_index);
+	viafb_fill_var_timing_info(var, viafb_refresh, vmode_entry);
 	if (info->var.accel_flags & FB_ACCELF_TEXT &&
 		!ppar->shared->engine_mmio)
 		info->var.accel_flags = 0;
@@ -153,40 +229,45 @@ static int viafb_check_var(struct fb_var_screeninfo *var,
 static int viafb_set_par(struct fb_info *info)
 {
 	struct viafb_par *viapar = info->par;
-	int vmode_index;
-	int vmode_index1 = 0;
+	struct VideoModeTable *vmode_entry, *vmode_entry1 = NULL;
 	DEBUG_MSG(KERN_INFO "viafb_set_par!\n");
 
 	viapar->depth = fb_get_color_depth(&info->var, &info->fix);
-	viafb_update_device_setting(info->var.xres, info->var.yres,
-			      info->var.bits_per_pixel, viafb_refresh, 0);
+	viafb_update_device_setting(viafbinfo->var.xres, viafbinfo->var.yres,
+		viafbinfo->var.bits_per_pixel, viafb_refresh, 0);
 
-	vmode_index = viafb_get_mode_index(info->var.xres, info->var.yres);
-
-	if (viafb_SAMM_ON == 1) {
+	vmode_entry = viafb_get_mode(viafbinfo->var.xres, viafbinfo->var.yres);
+	if (viafb_dual_fb) {
+		vmode_entry1 = viafb_get_mode(viafbinfo1->var.xres,
+			viafbinfo1->var.yres);
+		viafb_update_device_setting(viafbinfo1->var.xres,
+			viafbinfo1->var.yres, viafbinfo1->var.bits_per_pixel,
+			viafb_refresh1, 1);
+	} else if (viafb_SAMM_ON == 1) {
 		DEBUG_MSG(KERN_INFO
 		"viafb_second_xres = %d, viafb_second_yres = %d, bpp = %d\n",
 			  viafb_second_xres, viafb_second_yres, viafb_bpp1);
-		vmode_index1 = viafb_get_mode_index(viafb_second_xres,
+		vmode_entry1 = viafb_get_mode(viafb_second_xres,
 			viafb_second_yres);
-		DEBUG_MSG(KERN_INFO "->viafb_SAMM_ON: index=%d\n",
-			vmode_index1);
 
 		viafb_update_device_setting(viafb_second_xres,
 			viafb_second_yres, viafb_bpp1, viafb_refresh1, 1);
 	}
 
-	if (vmode_index != VIA_RES_INVALID) {
-		viafb_setmode(vmode_index, info->var.xres, info->var.yres,
-			info->var.bits_per_pixel, vmode_index1,
-			viafb_second_xres, viafb_second_yres, viafb_bpp1);
-
+	if (vmode_entry) {
 		viafb_update_fix(info);
-		viafb_bpp = info->var.bits_per_pixel;
+		if (viafb_dual_fb && viapar->iga_path == IGA2)
+			viafb_bpp1 = info->var.bits_per_pixel;
+		else
+			viafb_bpp = info->var.bits_per_pixel;
+
 		if (info->var.accel_flags & FB_ACCELF_TEXT)
 			info->flags &= ~FBINFO_HWACCEL_DISABLED;
 		else
 			info->flags |= FBINFO_HWACCEL_DISABLED;
+		viafb_setmode(vmode_entry, info->var.bits_per_pixel,
+			vmode_entry1, viafb_bpp1);
+		viafb_pan_display(&info->var, info);
 	}
 
 	return 0;
@@ -196,234 +277,52 @@ static int viafb_set_par(struct fb_info *info)
 static int viafb_setcolreg(unsigned regno, unsigned red, unsigned green,
 unsigned blue, unsigned transp, struct fb_info *info)
 {
-	u8 sr1a, sr1b, cr67, cr6a, rev = 0, shift = 10;
-	unsigned cmap_entries = (info->var.bits_per_pixel == 8) ? 256 : 16;
-	DEBUG_MSG(KERN_INFO "viafb_setcolreg!\n");
-	if (regno >= cmap_entries)
-		return 1;
-	if (UNICHROME_CLE266 == viaparinfo->chip_info->gfx_chip_name) {
-		/*
-		 * Read PCI bus 0,dev 0,function 0,index 0xF6 to get chip rev.
-		 */
-		outl(0x80000000 | (0xf6 & ~3), (unsigned long)0xCF8);
-		rev = (inl((unsigned long)0xCFC) >> ((0xf6 & 3) * 8)) & 0xff;
-	}
-	switch (info->var.bits_per_pixel) {
-	case 8:
-		outb(0x1A, 0x3C4);
-		sr1a = inb(0x3C5);
-		outb(0x1B, 0x3C4);
-		sr1b = inb(0x3C5);
-		outb(0x67, 0x3D4);
-		cr67 = inb(0x3D5);
-		outb(0x6A, 0x3D4);
-		cr6a = inb(0x3D5);
-
-		/* Map the 3C6/7/8/9 to the IGA2 */
-		outb(0x1A, 0x3C4);
-		outb(sr1a | 0x01, 0x3C5);
-		/* Second Display Engine colck always on */
-		outb(0x1B, 0x3C4);
-		outb(sr1b | 0x80, 0x3C5);
-		/* Second Display Color Depth 8 */
-		outb(0x67, 0x3D4);
-		outb(cr67 & 0x3F, 0x3D5);
-		outb(0x6A, 0x3D4);
-		/* Second Display Channel Reset CR6A[6]) */
-		outb(cr6a & 0xBF, 0x3D5);
-		/* Second Display Channel Enable CR6A[7] */
-		outb(cr6a | 0x80, 0x3D5);
-		/* Second Display Channel stop reset) */
-		outb(cr6a | 0x40, 0x3D5);
-
-		/* Bit mask of palette */
-		outb(0xFF, 0x3c6);
-		/* Write one register of IGA2 */
-		outb(regno, 0x3C8);
-		if (UNICHROME_CLE266 == viaparinfo->chip_info->gfx_chip_name &&
-			rev >= 15) {
-			shift = 8;
-			viafb_write_reg_mask(CR6A, VIACR, BIT5, BIT5);
-			viafb_write_reg_mask(SR15, VIASR, BIT7, BIT7);
-		} else {
-			shift = 10;
-			viafb_write_reg_mask(CR6A, VIACR, 0, BIT5);
-			viafb_write_reg_mask(SR15, VIASR, 0, BIT7);
-		}
-		outb(red >> shift, 0x3C9);
-		outb(green >> shift, 0x3C9);
-		outb(blue >> shift, 0x3C9);
-
-		/* Map the 3C6/7/8/9 to the IGA1 */
-		outb(0x1A, 0x3C4);
-		outb(sr1a & 0xFE, 0x3C5);
-		/* Bit mask of palette */
-		outb(0xFF, 0x3c6);
-		/* Write one register of IGA1 */
-		outb(regno, 0x3C8);
-		outb(red >> shift, 0x3C9);
-		outb(green >> shift, 0x3C9);
-		outb(blue >> shift, 0x3C9);
-
-		outb(0x1A, 0x3C4);
-		outb(sr1a, 0x3C5);
-		outb(0x1B, 0x3C4);
-		outb(sr1b, 0x3C5);
-		outb(0x67, 0x3D4);
-		outb(cr67, 0x3D5);
-		outb(0x6A, 0x3D4);
-		outb(cr6a, 0x3D5);
-		break;
-	case 16:
-		((u32 *) info->pseudo_palette)[regno] = (red & 0xF800) |
-		    ((green & 0xFC00) >> 5) | ((blue & 0xF800) >> 11);
-		break;
-	case 32:
-		((u32 *) info->pseudo_palette)[regno] =
-		    ((transp & 0xFF00) << 16) |
-		    ((red & 0xFF00) << 8) |
-		    ((green & 0xFF00)) | ((blue & 0xFF00) >> 8);
-		break;
-	}
-
-	return 0;
+	struct viafb_par *viapar = info->par;
+	u32 r, g, b;
 
-}
+	if (info->fix.visual == FB_VISUAL_PSEUDOCOLOR) {
+		if (regno > 255)
+			return -EINVAL;
 
-/*CALLED BY: fb_set_cmap */
-/*           fb_set_var, pass 256 colors */
-/*CALLED BY: fb_set_cmap */
-/*           fbcon_set_palette, pass 16 colors */
-static int viafb_setcmap(struct fb_cmap *cmap, struct fb_info *info)
-{
-	u32 len = cmap->len;
-	u32 i;
-	u16 *pred = cmap->red;
-	u16 *pgreen = cmap->green;
-	u16 *pblue = cmap->blue;
-	u16 *ptransp = cmap->transp;
-	u8 sr1a, sr1b, cr67, cr6a, rev = 0, shift = 10;
-	if (len > 256)
-		return 1;
-	if (UNICHROME_CLE266 == viaparinfo->chip_info->gfx_chip_name) {
-		/*
-		 * Read PCI bus 0, dev 0, function 0, index 0xF6 to get chip
-		 * rev.
-		 */
-		outl(0x80000000 | (0xf6 & ~3), (unsigned long)0xCF8);
-		rev = (inl((unsigned long)0xCFC) >> ((0xf6 & 3) * 8)) & 0xff;
-	}
-	switch (info->var.bits_per_pixel) {
-	case 8:
-		outb(0x1A, 0x3C4);
-		sr1a = inb(0x3C5);
-		outb(0x1B, 0x3C4);
-		sr1b = inb(0x3C5);
-		outb(0x67, 0x3D4);
-		cr67 = inb(0x3D5);
-		outb(0x6A, 0x3D4);
-		cr6a = inb(0x3D5);
-		/* Map the 3C6/7/8/9 to the IGA2 */
-		outb(0x1A, 0x3C4);
-		outb(sr1a | 0x01, 0x3C5);
-		outb(0x1B, 0x3C4);
-		/* Second Display Engine colck always on */
-		outb(sr1b | 0x80, 0x3C5);
-		outb(0x67, 0x3D4);
-		/* Second Display Color Depth 8 */
-		outb(cr67 & 0x3F, 0x3D5);
-		outb(0x6A, 0x3D4);
-		/* Second Display Channel Reset CR6A[6]) */
-		outb(cr6a & 0xBF, 0x3D5);
-		/* Second Display Channel Enable CR6A[7] */
-		outb(cr6a | 0x80, 0x3D5);
-		/* Second Display Channel stop reset) */
-		outb(cr6a | 0xC0, 0x3D5);
-
-		/* Bit mask of palette */
-		outb(0xFF, 0x3c6);
-		outb(0x00, 0x3C8);
-		if (UNICHROME_CLE266 == viaparinfo->chip_info->gfx_chip_name &&
-			rev >= 15) {
-			shift = 8;
-			viafb_write_reg_mask(CR6A, VIACR, BIT5, BIT5);
-			viafb_write_reg_mask(SR15, VIASR, BIT7, BIT7);
-		} else {
-			shift = 10;
-			viafb_write_reg_mask(CR6A, VIACR, 0, BIT5);
-			viafb_write_reg_mask(SR15, VIASR, 0, BIT7);
-		}
-		for (i = 0; i < len; i++) {
-			outb((*(pred + i)) >> shift, 0x3C9);
-			outb((*(pgreen + i)) >> shift, 0x3C9);
-			outb((*(pblue + i)) >> shift, 0x3C9);
-		}
+		if (!viafb_dual_fb || viapar->iga_path == IGA1)
+			viafb_set_primary_color_register(regno, red >> 8,
+				green >> 8, blue >> 8);
 
-		outb(0x1A, 0x3C4);
-		/* Map the 3C6/7/8/9 to the IGA1 */
-		outb(sr1a & 0xFE, 0x3C5);
-		/* Bit mask of palette */
-		outb(0xFF, 0x3c6);
-		outb(0x00, 0x3C8);
-		for (i = 0; i < len; i++) {
-			outb((*(pred + i)) >> shift, 0x3C9);
-			outb((*(pgreen + i)) >> shift, 0x3C9);
-			outb((*(pblue + i)) >> shift, 0x3C9);
-		}
+		if (!viafb_dual_fb || viapar->iga_path == IGA2)
+			viafb_set_secondary_color_register(regno, red >> 8,
+				green >> 8, blue >> 8);
+	} else {
+		if (regno > 15)
+			return -EINVAL;
 
-		outb(0x1A, 0x3C4);
-		outb(sr1a, 0x3C5);
-		outb(0x1B, 0x3C4);
-		outb(sr1b, 0x3C5);
-		outb(0x67, 0x3D4);
-		outb(cr67, 0x3D5);
-		outb(0x6A, 0x3D4);
-		outb(cr6a, 0x3D5);
-		break;
-	case 16:
-		if (len > 17)
-			return 0;	/* Because static u32 pseudo_pal[17]; */
-		for (i = 0; i < len; i++)
-			((u32 *) info->pseudo_palette)[i] =
-			    (*(pred + i) & 0xF800) |
-			    ((*(pgreen + i) & 0xFC00) >> 5) |
-			    ((*(pblue + i) & 0xF800) >> 11);
-		break;
-	case 32:
-		if (len > 17)
-			return 0;
-		if (ptransp) {
-			for (i = 0; i < len; i++)
-				((u32 *) info->pseudo_palette)[i] =
-				    ((*(ptransp + i) & 0xFF00) << 16) |
-				    ((*(pred + i) & 0xFF00) << 8) |
-				    ((*(pgreen + i) & 0xFF00)) |
-				    ((*(pblue + i) & 0xFF00) >> 8);
-		} else {
-			for (i = 0; i < len; i++)
-				((u32 *) info->pseudo_palette)[i] =
-				    0x00000000 |
-				    ((*(pred + i) & 0xFF00) << 8) |
-				    ((*(pgreen + i) & 0xFF00)) |
-				    ((*(pblue + i) & 0xFF00) >> 8);
-		}
-		break;
+		r = (red >> (16 - info->var.red.length))
+			<< info->var.red.offset;
+		b = (blue >> (16 - info->var.blue.length))
+			<< info->var.blue.offset;
+		g = (green >> (16 - info->var.green.length))
+			<< info->var.green.offset;
+		((u32 *) info->pseudo_palette)[regno] = r | g | b;
 	}
+
 	return 0;
 }
 
 static int viafb_pan_display(struct fb_var_screeninfo *var,
 	struct fb_info *info)
 {
-	unsigned int offset;
-
-	DEBUG_MSG(KERN_INFO "viafb_pan_display!\n");
-
-	offset = (var->xoffset + (var->yoffset * var->xres_virtual)) *
-	    var->bits_per_pixel / 16;
+	struct viafb_par *viapar = info->par;
+	u32 vram_addr = (var->yoffset * var->xres_virtual + var->xoffset)
+		* (var->bits_per_pixel / 8) + viapar->vram_addr;
+
+	DEBUG_MSG(KERN_DEBUG "viafb_pan_display, address = %d\n", vram_addr);
+	if (!viafb_dual_fb) {
+		viafb_set_primary_address(vram_addr);
+		viafb_set_secondary_address(vram_addr);
+	} else if (viapar->iga_path == IGA1)
+		viafb_set_primary_address(vram_addr);
+	else
+		viafb_set_secondary_address(vram_addr);
 
-	DEBUG_MSG(KERN_INFO "\nviafb_pan_display,offset =%d ", offset);
-	viafb_set_primary_address(offset);
 	return 0;
 }
 
@@ -477,6 +376,7 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
 	u32 gpu32;
 
 	DEBUG_MSG(KERN_INFO "viafb_ioctl: 0x%X !!\n", cmd);
+	printk(KERN_WARNING "viafb_ioctl: Please avoid this interface as it is unstable and might change or vanish at any time!\n");
 	memset(&u, 0, sizeof(u));
 
 	switch (cmd) {
@@ -872,7 +772,9 @@ static int viafb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 	if (info->flags & FBINFO_HWACCEL_DISABLED || info != viafbinfo)
 		return -ENODEV;
 
-	if (chip_name == UNICHROME_CLE266 && viapar->iga_path == IGA2)
+	/* LCD ouput does not support hw cursors (at least on VN896) */
+	if ((chip_name == UNICHROME_CLE266 && viapar->iga_path == IGA2) ||
+		viafb_LCD_ON)
 		return -ENODEV;
 
 	viafb_show_hw_cursor(info, HW_Cursor_OFF);
@@ -1014,23 +916,6 @@ static int viafb_sync(struct fb_info *info)
 	return 0;
 }
 
-int viafb_get_mode_index(int hres, int vres)
-{
-	u32 i;
-	DEBUG_MSG(KERN_INFO "viafb_get_mode_index!\n");
-
-	for (i = 0; i < NUM_TOTAL_MODETABLE; i++)
-		if (CLE266Modes[i].mode_array &&
-			CLE266Modes[i].crtc[0].crtc.hor_addr == hres &&
-			CLE266Modes[i].crtc[0].crtc.ver_addr == vres)
-			break;
-
-	if (i == NUM_TOTAL_MODETABLE)
-		return VIA_RES_INVALID;
-
-	return CLE266Modes[i].ModeIndex;
-}
-
 static void check_available_device_to_enable(int device_id)
 {
 	int device_num = 0;
@@ -1329,7 +1214,7 @@ static void retrieve_device_setting(struct viafb_ioctl_setting
 	setting_info->lcd_attributes.lcd_mode = viafb_lcd_mode;
 }
 
-static void parse_active_dev(void)
+static int parse_active_dev(void)
 {
 	viafb_CRT_ON = STATE_OFF;
 	viafb_DVI_ON = STATE_OFF;
@@ -1340,60 +1225,63 @@ static void parse_active_dev(void)
 	   IGA path to devices in SAMM case. */
 	/*    Note: The previous of active_dev is primary device,
 	   and the following is secondary device. */
-	if (!strncmp(viafb_active_dev, "CRT+DVI", 7)) {
+	if (!viafb_active_dev) {
+		viafb_CRT_ON = STATE_ON;
+		viafb_SAMM_ON = STATE_OFF;
+	} else if (!strcmp(viafb_active_dev, "CRT+DVI")) {
 		/* CRT+DVI */
 		viafb_CRT_ON = STATE_ON;
 		viafb_DVI_ON = STATE_ON;
 		viafb_primary_dev = CRT_Device;
-	} else if (!strncmp(viafb_active_dev, "DVI+CRT", 7)) {
+	} else if (!strcmp(viafb_active_dev, "DVI+CRT")) {
 		/* DVI+CRT */
 		viafb_CRT_ON = STATE_ON;
 		viafb_DVI_ON = STATE_ON;
 		viafb_primary_dev = DVI_Device;
-	} else if (!strncmp(viafb_active_dev, "CRT+LCD", 7)) {
+	} else if (!strcmp(viafb_active_dev, "CRT+LCD")) {
 		/* CRT+LCD */
 		viafb_CRT_ON = STATE_ON;
 		viafb_LCD_ON = STATE_ON;
 		viafb_primary_dev = CRT_Device;
-	} else if (!strncmp(viafb_active_dev, "LCD+CRT", 7)) {
+	} else if (!strcmp(viafb_active_dev, "LCD+CRT")) {
 		/* LCD+CRT */
 		viafb_CRT_ON = STATE_ON;
 		viafb_LCD_ON = STATE_ON;
 		viafb_primary_dev = LCD_Device;
-	} else if (!strncmp(viafb_active_dev, "DVI+LCD", 7)) {
+	} else if (!strcmp(viafb_active_dev, "DVI+LCD")) {
 		/* DVI+LCD */
 		viafb_DVI_ON = STATE_ON;
 		viafb_LCD_ON = STATE_ON;
 		viafb_primary_dev = DVI_Device;
-	} else if (!strncmp(viafb_active_dev, "LCD+DVI", 7)) {
+	} else if (!strcmp(viafb_active_dev, "LCD+DVI")) {
 		/* LCD+DVI */
 		viafb_DVI_ON = STATE_ON;
 		viafb_LCD_ON = STATE_ON;
 		viafb_primary_dev = LCD_Device;
-	} else if (!strncmp(viafb_active_dev, "LCD+LCD2", 8)) {
+	} else if (!strcmp(viafb_active_dev, "LCD+LCD2")) {
 		viafb_LCD_ON = STATE_ON;
 		viafb_LCD2_ON = STATE_ON;
 		viafb_primary_dev = LCD_Device;
-	} else if (!strncmp(viafb_active_dev, "LCD2+LCD", 8)) {
+	} else if (!strcmp(viafb_active_dev, "LCD2+LCD")) {
 		viafb_LCD_ON = STATE_ON;
 		viafb_LCD2_ON = STATE_ON;
 		viafb_primary_dev = LCD2_Device;
-	} else if (!strncmp(viafb_active_dev, "CRT", 3)) {
+	} else if (!strcmp(viafb_active_dev, "CRT")) {
 		/* CRT only */
 		viafb_CRT_ON = STATE_ON;
 		viafb_SAMM_ON = STATE_OFF;
-	} else if (!strncmp(viafb_active_dev, "DVI", 3)) {
+	} else if (!strcmp(viafb_active_dev, "DVI")) {
 		/* DVI only */
 		viafb_DVI_ON = STATE_ON;
 		viafb_SAMM_ON = STATE_OFF;
-	} else if (!strncmp(viafb_active_dev, "LCD", 3)) {
+	} else if (!strcmp(viafb_active_dev, "LCD")) {
 		/* LCD only */
 		viafb_LCD_ON = STATE_ON;
 		viafb_SAMM_ON = STATE_OFF;
-	} else {
-		viafb_CRT_ON = STATE_ON;
-		viafb_SAMM_ON = STATE_OFF;
-	}
+	} else
+		return -EINVAL;
+
+	return 0;
 }
 
 static int parse_port(char *opt_str, int *output_interface)
@@ -1822,35 +1710,37 @@ static void viafb_remove_proc(struct proc_dir_entry *viafb_entry)
 	remove_proc_entry("viafb", NULL);
 }
 
-static void parse_mode(const char *str, u32 *xres, u32 *yres)
+static int parse_mode(const char *str, u32 *xres, u32 *yres)
 {
 	char *ptr;
 
+	if (!str) {
+		*xres = 640;
+		*yres = 480;
+		return 0;
+	}
+
 	*xres = simple_strtoul(str, &ptr, 10);
 	if (ptr[0] != 'x')
-		goto out_default;
+		return -EINVAL;
 
 	*yres = simple_strtoul(&ptr[1], &ptr, 10);
 	if (ptr[0])
-		goto out_default;
-
-	return;
+		return -EINVAL;
 
-out_default:
-	printk(KERN_WARNING "viafb received invalid mode string: %s\n", str);
-	*xres = 640;
-	*yres = 480;
+	return 0;
 }
 
 static int __devinit via_pci_probe(struct pci_dev *pdev,
 				   const struct pci_device_id *ent)
 {
 	u32 default_xres, default_yres;
-	int vmode_index;
+	struct VideoModeTable *vmode_entry;
+	struct fb_var_screeninfo default_var;
 	u32 viafb_par_length;
 
 	DEBUG_MSG(KERN_INFO "VIAFB PCI Probe!!\n");
-
+	memset(&default_var, 0, sizeof(default_var));
 	viafb_par_length = ALIGN(sizeof(struct viafb_par), BITS_PER_LONG/8);
 
 	/* Allocate fb_info and ***_par here, also including some other needed
@@ -1876,7 +1766,6 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 
 	if (viafb_dual_fb)
 		viafb_SAMM_ON = 1;
-	parse_active_dev();
 	parse_lcd_port();
 	parse_dvi_port();
 
@@ -1925,9 +1814,7 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 	}
 
 	parse_mode(viafb_mode, &default_xres, &default_yres);
-	vmode_index = viafb_get_mode_index(default_xres, default_yres);
-	DEBUG_MSG(KERN_INFO "0->index=%d\n", vmode_index);
-
+	vmode_entry = viafb_get_mode(default_xres, default_yres);
 	if (viafb_SAMM_ON == 1) {
 		parse_mode(viafb_mode1, &viafb_second_xres,
 			&viafb_second_yres);
@@ -1946,19 +1833,6 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 			viafb_second_virtual_yres = viafb_second_yres;
 	}
 
-	switch (viafb_bpp) {
-	case 0 ... 8:
-		viafb_bpp = 8;
-		break;
-	case 9 ... 16:
-		viafb_bpp = 16;
-		break;
-	case 17 ... 32:
-		viafb_bpp = 32;
-		break;
-	default:
-		viafb_bpp = 8;
-	}
 	default_var.xres = default_xres;
 	default_var.yres = default_yres;
 	switch (default_xres) {
@@ -1971,8 +1845,6 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 	}
 	default_var.yres_virtual = default_yres;
 	default_var.bits_per_pixel = viafb_bpp;
-	if (default_var.bits_per_pixel == 15)
-		default_var.bits_per_pixel = 16;
 	default_var.pixclock =
 	    viafb_get_pixclock(default_xres, default_yres, viafb_refresh);
 	default_var.left_margin = (default_xres >> 3) & 0xf8;
@@ -1981,6 +1853,8 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 	default_var.lower_margin = 4;
 	default_var.hsync_len = default_var.left_margin;
 	default_var.vsync_len = 4;
+	viafb_setup_fixinfo(&viafbinfo->fix, viaparinfo);
+	viafbinfo->var = default_var;
 
 	if (viafb_dual_fb) {
 		viafbinfo1 = framebuffer_alloc(viafb_par_length, &pdev->dev);
@@ -2015,8 +1889,6 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 		default_var.yres = viafb_second_yres;
 		default_var.xres_virtual = viafb_second_virtual_xres;
 		default_var.yres_virtual = viafb_second_virtual_yres;
-		if (viafb_bpp1 != viafb_bpp)
-			viafb_bpp1 = viafb_bpp;
 		default_var.bits_per_pixel = viafb_bpp1;
 		default_var.pixclock =
 		    viafb_get_pixclock(viafb_second_xres, viafb_second_yres,
@@ -2036,9 +1908,7 @@ static int __devinit via_pci_probe(struct pci_dev *pdev,
 			&viafbinfo1->fix);
 	}
 
-	viafb_setup_fixinfo(&viafbinfo->fix, viaparinfo);
-	viafb_check_var(&default_var, viafbinfo);
-	viafbinfo->var = default_var;
+	viafb_check_var(&viafbinfo->var, viafbinfo);
 	viafb_update_fix(viafbinfo);
 	viaparinfo->depth = fb_get_color_depth(&viafbinfo->var,
 		&viafbinfo->fix);
@@ -2196,12 +2066,20 @@ static struct pci_driver viafb_driver = {
 
 static int __init viafb_init(void)
 {
+	u32 dummy;
 #ifndef MODULE
 	char *option = NULL;
 	if (fb_get_options("viafb", &option))
 		return -ENODEV;
 	viafb_setup(option);
 #endif
+	if (parse_mode(viafb_mode, &dummy, &dummy)
+		|| parse_mode(viafb_mode1, &dummy, &dummy)
+		|| viafb_bpp < 0 || viafb_bpp > 32
+		|| viafb_bpp1 < 0 || viafb_bpp1 > 32
+		|| parse_active_dev())
+		return -EINVAL;
+
 	printk(KERN_INFO
        "VIA Graphics Intergration Chipset framebuffer %d.%d initializing\n",
 	       VERSION_MAJOR, VERSION_MINOR);
@@ -2229,15 +2107,12 @@ static struct fb_ops viafb_ops = {
 	.fb_cursor = viafb_cursor,
 	.fb_ioctl = viafb_ioctl,
 	.fb_sync = viafb_sync,
-	.fb_setcmap = viafb_setcmap,
 };
 
 module_init(viafb_init);
 module_exit(viafb_exit);
 
 #ifdef MODULE
-module_param(viafb_memsize, int, S_IRUSR);
-
 module_param(viafb_mode, charp, S_IRUSR);
 MODULE_PARM_DESC(viafb_mode, "Set resolution (default=640x480)");