staging: xgifb: main: move functions to avoid forward declarations

Move functions so that all forward declarations of internal functions
can be deleted.

Signed-off-by: Aaro Koskinen <aaro.koskinen@iki.fi>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

drivers/staging/xgifb/XGI_main.h

@@ -651,24 +651,6 @@ static unsigned char filter_tb;
 
 /* ---------------------- Routine prototypes ------------------------- */
 
-/* Interface used by the world */
-#ifndef MODULE
-XGIINITSTATIC int __init XGIfb_setup(char *options);
-#endif
-
-/* Interface to the low level console driver */
-
-
-
-/* fbdev routines */
-XGIINITSTATIC int __init xgifb_init(void);
-static int      XGIfb_set_par(struct fb_info *info);
-static int      XGIfb_blank(int blank,
-                            struct fb_info *info);
-/*static int 	XGIfb_mmap(struct fb_info *info, struct file *file,
-		           struct vm_area_struct *vma);
-*/
-
 /*
 extern int	XGIfb_mode_rate_to_dclock(VB_DEVICE_INFO *XGI_Pr,
 			      struct xgi_hw_device_info *HwDeviceExtension,
@@ -683,29 +665,6 @@ extern int      XGIfb_mode_rate_to_ddata(VB_DEVICE_INFO *XGI_Pr, struct xgi_hw_d
 extern unsigned char XGI_SearchModeID(unsigned short ModeNo,
 				unsigned short *ModeIdIndex,
 				struct vb_device_info *);
-static int      XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
-			      struct fb_info *info);
-
-/* Internal general routines */
-static void     XGIfb_search_mode(const char *name);
-static int      XGIfb_validate_mode(int modeindex);
-static u8       XGIfb_search_refresh_rate(unsigned int rate);
-static int      XGIfb_setcolreg(unsigned regno, unsigned red, unsigned green,
-			unsigned blue, unsigned transp,
-			struct fb_info *fb_info);
-static int      XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
-		      	struct fb_info *info);
-static void     XGIfb_pre_setmode(void);
-static void     XGIfb_post_setmode(void);
-
-/* Chipset-dependent internal routines */
-
-
-static int      XGIfb_get_dram_size(void);
-static void     XGIfb_detect_VB(void);
-static void     XGIfb_get_VB_type(void);
-static int      XGIfb_has_VB(void);
-
 
 /* Internal routines to access PCI configuration space */
 unsigned char XGIfb_query_VGA_config_space(struct xgi_hw_device_info *pXGIhw_ext,

drivers/staging/xgifb/XGI_main_26.c

@@ -50,7 +50,6 @@
 #define GPIOG_EN    (1<<6)
 #define GPIOG_WRITE (1<<6)
 #define GPIOG_READ  (1<<1)
-int XGIfb_GetXG21DefaultLVDSModeIdx(void);
 
 #define XGIFB_ROM_SIZE	65536
 
@@ -480,6 +479,33 @@ unsigned char XGIfb_query_north_bridge_space(struct xgi_hw_device_info *pXGIhw_e
 */
 /* ------------------ Internal helper routines ----------------- */
 
+int XGIfb_GetXG21DefaultLVDSModeIdx(void)
+{
+
+	int found_mode = 0;
+	int XGIfb_mode_idx = 0;
+
+	found_mode = 0;
+	while ((XGIbios_mode[XGIfb_mode_idx].mode_no != 0)
+			&& (XGIbios_mode[XGIfb_mode_idx].xres
+					<= XGI21_LCDCapList[0].LVDSHDE)) {
+		if ((XGIbios_mode[XGIfb_mode_idx].xres
+				== XGI21_LCDCapList[0].LVDSHDE)
+				&& (XGIbios_mode[XGIfb_mode_idx].yres
+						== XGI21_LCDCapList[0].LVDSVDE)
+				&& (XGIbios_mode[XGIfb_mode_idx].bpp == 8)) {
+			XGIfb_mode_no = XGIbios_mode[XGIfb_mode_idx].mode_no;
+			found_mode = 1;
+			break;
+		}
+		XGIfb_mode_idx++;
+	}
+	if (!found_mode)
+		XGIfb_mode_idx = 0;
+
+	return XGIfb_mode_idx;
+}
+
 static void XGIfb_search_mode(const char *name)
 {
 	int i = 0, j = 0, l;
@@ -603,33 +629,6 @@ static int XGIfb_GetXG21LVDSData(void)
 	return 0;
 }
 
-int XGIfb_GetXG21DefaultLVDSModeIdx(void)
-{
-
-	int found_mode = 0;
-	int XGIfb_mode_idx = 0;
-
-	found_mode = 0;
-	while ((XGIbios_mode[XGIfb_mode_idx].mode_no != 0)
-			&& (XGIbios_mode[XGIfb_mode_idx].xres
-					<= XGI21_LCDCapList[0].LVDSHDE)) {
-		if ((XGIbios_mode[XGIfb_mode_idx].xres
-				== XGI21_LCDCapList[0].LVDSHDE)
-				&& (XGIbios_mode[XGIfb_mode_idx].yres
-						== XGI21_LCDCapList[0].LVDSVDE)
-				&& (XGIbios_mode[XGIfb_mode_idx].bpp == 8)) {
-			XGIfb_mode_no = XGIbios_mode[XGIfb_mode_idx].mode_no;
-			found_mode = 1;
-			break;
-		}
-		XGIfb_mode_idx++;
-	}
-	if (!found_mode)
-		XGIfb_mode_idx = 0;
-
-	return XGIfb_mode_idx;
-}
-
 static int XGIfb_validate_mode(int myindex)
 {
 	u16 xres, yres;
@@ -978,134 +977,352 @@ static void XGIfb_bpp_to_var(struct fb_var_screeninfo *var)
 	}
 }
 
-static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
-		struct fb_info *info)
-{
+/* --------------------- SetMode routines ------------------------- */
 
-	unsigned int htotal = var->left_margin + var->xres + var->right_margin
-			+ var->hsync_len;
-	unsigned int vtotal = var->upper_margin + var->yres + var->lower_margin
-			+ var->vsync_len;
-#if defined(__powerpc__)
-	u8 sr_data, cr_data;
-#endif
-	unsigned int drate = 0, hrate = 0;
-	int found_mode = 0;
-	int old_mode;
-	/* unsigned char reg, reg1; */
+static void XGIfb_pre_setmode(void)
+{
+	u8 cr30 = 0, cr31 = 0;
 
-	DEBUGPRN("Inside do_set_var");
-	/* printk(KERN_DEBUG "XGIfb:var->yres=%d, var->upper_margin=%d, var->lower_margin=%d, var->vsync_len=%d\n", var->yres, var->upper_margin, var->lower_margin, var->vsync_len); */
+	cr31 = xgifb_reg_get(XGICR, 0x31);
+	cr31 &= ~0x60;
 
-	info->var.xres_virtual = var->xres_virtual;
-	info->var.yres_virtual = var->yres_virtual;
-	info->var.bits_per_pixel = var->bits_per_pixel;
+	switch (xgi_video_info.disp_state & DISPTYPE_DISP2) {
+	case DISPTYPE_CRT2:
+		cr30 = (XGI_VB_OUTPUT_CRT2 | XGI_SIMULTANEOUS_VIEW_ENABLE);
+		cr31 |= XGI_DRIVER_MODE;
+		break;
+	case DISPTYPE_LCD:
+		cr30 = (XGI_VB_OUTPUT_LCD | XGI_SIMULTANEOUS_VIEW_ENABLE);
+		cr31 |= XGI_DRIVER_MODE;
+		break;
+	case DISPTYPE_TV:
+		if (xgi_video_info.TV_type == TVMODE_HIVISION)
+			cr30 = (XGI_VB_OUTPUT_HIVISION
+					| XGI_SIMULTANEOUS_VIEW_ENABLE);
+		else if (xgi_video_info.TV_plug == TVPLUG_SVIDEO)
+			cr30 = (XGI_VB_OUTPUT_SVIDEO
+					| XGI_SIMULTANEOUS_VIEW_ENABLE);
+		else if (xgi_video_info.TV_plug == TVPLUG_COMPOSITE)
+			cr30 = (XGI_VB_OUTPUT_COMPOSITE
+					| XGI_SIMULTANEOUS_VIEW_ENABLE);
+		else if (xgi_video_info.TV_plug == TVPLUG_SCART)
+			cr30 = (XGI_VB_OUTPUT_SCART
+					| XGI_SIMULTANEOUS_VIEW_ENABLE);
+		cr31 |= XGI_DRIVER_MODE;
 
-	if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_NONINTERLACED)
-		vtotal <<= 1;
-	else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE)
-		vtotal <<= 2;
-	else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED) {
-		/* vtotal <<= 1; */
-		/* var->yres <<= 1; */
+		if (XGIfb_tvmode == 1 || xgi_video_info.TV_type == TVMODE_PAL)
+			cr31 |= 0x01;
+		else
+			cr31 &= ~0x01;
+		break;
+	default: /* disable CRT2 */
+		cr30 = 0x00;
+		cr31 |= (XGI_DRIVER_MODE | XGI_VB_OUTPUT_DISABLE);
 	}
 
-	if (!htotal || !vtotal) {
-		DPRINTK("XGIfb: Invalid 'var' information\n");
-		return -EINVAL;
-	} printk(KERN_DEBUG "XGIfb: var->pixclock=%d, htotal=%d, vtotal=%d\n",
-			var->pixclock, htotal, vtotal);
+	xgifb_reg_set(XGICR, IND_XGI_SCRATCH_REG_CR30, cr30);
+	xgifb_reg_set(XGICR, IND_XGI_SCRATCH_REG_CR31, cr31);
+	xgifb_reg_set(XGICR, IND_XGI_SCRATCH_REG_CR33, (XGIfb_rate_idx & 0x0F));
+}
 
-	if (var->pixclock && htotal && vtotal) {
-		drate = 1000000000 / var->pixclock;
-		hrate = (drate * 1000) / htotal;
-		xgi_video_info.refresh_rate = (unsigned int) (hrate * 2
-				/ vtotal);
-	} else {
-		xgi_video_info.refresh_rate = 60;
+static void XGIfb_post_setmode(void)
+{
+	u8 reg;
+	unsigned char doit = 1;
+	/*
+	xgifb_reg_set(XGISR,IND_XGI_PASSWORD,XGI_PASSWORD);
+	xgifb_reg_set(XGICR, 0x13, 0x00);
+	xgifb_reg_and_or(XGISR,0x0E, 0xF0, 0x01);
+	*test*
+	*/
+	if (xgi_video_info.video_bpp == 8) {
+		/* TW: We can't switch off CRT1 on LVDS/Chrontel in 8bpp Modes */
+		if ((xgi_video_info.hasVB == HASVB_LVDS)
+				|| (xgi_video_info.hasVB == HASVB_LVDS_CHRONTEL)) {
+			doit = 0;
+		}
+		/* TW: We can't switch off CRT1 on 301B-DH in 8bpp Modes if using LCD */
+		if (xgi_video_info.disp_state & DISPTYPE_LCD)
+			doit = 0;
 	}
 
-	printk(KERN_DEBUG "XGIfb: Change mode to %dx%dx%d-%dHz\n",
-			var->xres, var->yres, var->bits_per_pixel, xgi_video_info.refresh_rate);
+	/* TW: We can't switch off CRT1 if bridge is in slave mode */
+	if (xgi_video_info.hasVB != HASVB_NONE) {
+		reg = xgifb_reg_get(XGIPART1, 0x00);
 
-	old_mode = xgifb_mode_idx;
-	xgifb_mode_idx = 0;
+		if ((reg & 0x50) == 0x10)
+			doit = 0;
 
-	while ((XGIbios_mode[xgifb_mode_idx].mode_no != 0)
-			&& (XGIbios_mode[xgifb_mode_idx].xres <= var->xres)) {
-		if ((XGIbios_mode[xgifb_mode_idx].xres == var->xres)
-				&& (XGIbios_mode[xgifb_mode_idx].yres
-						== var->yres)
-				&& (XGIbios_mode[xgifb_mode_idx].bpp
-						== var->bits_per_pixel)) {
-			XGIfb_mode_no = XGIbios_mode[xgifb_mode_idx].mode_no;
-			found_mode = 1;
-			break;
-		}
-		xgifb_mode_idx++;
+	} else {
+		XGIfb_crt1off = 0;
 	}
 
-	if (found_mode)
-		xgifb_mode_idx = XGIfb_validate_mode(xgifb_mode_idx);
+	reg = xgifb_reg_get(XGICR, 0x17);
+	if ((XGIfb_crt1off) && (doit))
+		reg &= ~0x80;
 	else
-		xgifb_mode_idx = -1;
+		reg |= 0x80;
+	xgifb_reg_set(XGICR, 0x17, reg);
 
-	if (xgifb_mode_idx < 0) {
-		printk(KERN_ERR "XGIfb: Mode %dx%dx%d not supported\n", var->xres,
-				var->yres, var->bits_per_pixel);
-		xgifb_mode_idx = old_mode;
-		return -EINVAL;
-	}
+	xgifb_reg_and(XGISR, IND_XGI_RAMDAC_CONTROL, ~0x04);
 
-	if (XGIfb_search_refresh_rate(xgi_video_info.refresh_rate) == 0) {
-		XGIfb_rate_idx = XGIbios_mode[xgifb_mode_idx].rate_idx;
-		xgi_video_info.refresh_rate = 60;
-	}
+	if ((xgi_video_info.disp_state & DISPTYPE_TV) && (xgi_video_info.hasVB
+			== HASVB_301)) {
 
-	if (isactive) {
+		reg = xgifb_reg_get(XGIPART4, 0x01);
 
-		XGIfb_pre_setmode();
-		if (XGISetModeNew(&XGIhw_ext, XGIfb_mode_no) == 0) {
-			printk(KERN_ERR "XGIfb: Setting mode[0x%x] failed\n", XGIfb_mode_no);
-			return -EINVAL;
-		}
-		info->fix.line_length = ((info->var.xres_virtual
-				* info->var.bits_per_pixel) >> 6);
+		if (reg < 0xB0) { /* Set filter for XGI301 */
 
-		xgifb_reg_set(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
+			switch (xgi_video_info.video_width) {
+			case 320:
+				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 4 : 12;
+				break;
+			case 640:
+				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 5 : 13;
+				break;
+			case 720:
+				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 6 : 14;
+				break;
+			case 800:
+				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 7 : 15;
+				break;
+			default:
+				filter = -1;
+				break;
+			}
 
-		xgifb_reg_set(XGICR, 0x13, (info->fix.line_length & 0x00ff));
-		xgifb_reg_set(XGISR, 0x0E, (info->fix.line_length & 0xff00) >> 8);
+			xgifb_reg_or(XGIPART1, XGIfb_CRT2_write_enable, 0x01);
 
-		XGIfb_post_setmode();
+			if (xgi_video_info.TV_type == TVMODE_NTSC) {
 
-		DPRINTK("XGIfb: Set new mode: %dx%dx%d-%d\n",
-				XGIbios_mode[xgifb_mode_idx].xres,
-				XGIbios_mode[xgifb_mode_idx].yres,
-				XGIbios_mode[xgifb_mode_idx].bpp,
-				xgi_video_info.refresh_rate);
+				xgifb_reg_and(XGIPART2, 0x3a, 0x1f);
 
-		xgi_video_info.video_bpp = XGIbios_mode[xgifb_mode_idx].bpp;
-		xgi_video_info.video_vwidth = info->var.xres_virtual;
-		xgi_video_info.video_width = XGIbios_mode[xgifb_mode_idx].xres;
-		xgi_video_info.video_vheight = info->var.yres_virtual;
-		xgi_video_info.video_height = XGIbios_mode[xgifb_mode_idx].yres;
-		xgi_video_info.org_x = xgi_video_info.org_y = 0;
-		xgi_video_info.video_linelength = info->var.xres_virtual
-				* (xgi_video_info.video_bpp >> 3);
-		switch (xgi_video_info.video_bpp) {
-		case 8:
-			xgi_video_info.DstColor = 0x0000;
-			xgi_video_info.XGI310_AccelDepth = 0x00000000;
-			xgi_video_info.video_cmap_len = 256;
-#if defined(__powerpc__)
-			cr_data = xgifb_reg_get(XGICR, 0x4D);
-			xgifb_reg_set(XGICR, 0x4D, (cr_data & 0xE0));
-#endif
-			break;
-		case 16:
-			xgi_video_info.DstColor = 0x8000;
-			xgi_video_info.XGI310_AccelDepth = 0x00010000;
+				if (xgi_video_info.TV_plug == TVPLUG_SVIDEO) {
+
+					xgifb_reg_and(XGIPART2, 0x30, 0xdf);
+
+				} else if (xgi_video_info.TV_plug
+						== TVPLUG_COMPOSITE) {
+
+					xgifb_reg_or(XGIPART2, 0x30, 0x20);
+
+					switch (xgi_video_info.video_width) {
+					case 640:
+						xgifb_reg_set(XGIPART2, 0x35, 0xEB);
+						xgifb_reg_set(XGIPART2, 0x36, 0x04);
+						xgifb_reg_set(XGIPART2, 0x37, 0x25);
+						xgifb_reg_set(XGIPART2, 0x38, 0x18);
+						break;
+					case 720:
+						xgifb_reg_set(XGIPART2, 0x35, 0xEE);
+						xgifb_reg_set(XGIPART2, 0x36, 0x0C);
+						xgifb_reg_set(XGIPART2, 0x37, 0x22);
+						xgifb_reg_set(XGIPART2, 0x38, 0x08);
+						break;
+					case 800:
+						xgifb_reg_set(XGIPART2, 0x35, 0xEB);
+						xgifb_reg_set(XGIPART2, 0x36, 0x15);
+						xgifb_reg_set(XGIPART2, 0x37, 0x25);
+						xgifb_reg_set(XGIPART2, 0x38, 0xF6);
+						break;
+					}
+				}
+
+			} else if (xgi_video_info.TV_type == TVMODE_PAL) {
+
+				xgifb_reg_and(XGIPART2, 0x3A, 0x1F);
+
+				if (xgi_video_info.TV_plug == TVPLUG_SVIDEO) {
+
+					xgifb_reg_and(XGIPART2, 0x30, 0xDF);
+
+				} else if (xgi_video_info.TV_plug
+						== TVPLUG_COMPOSITE) {
+
+					xgifb_reg_or(XGIPART2, 0x30, 0x20);
+
+					switch (xgi_video_info.video_width) {
+					case 640:
+						xgifb_reg_set(XGIPART2, 0x35, 0xF1);
+						xgifb_reg_set(XGIPART2, 0x36, 0xF7);
+						xgifb_reg_set(XGIPART2, 0x37, 0x1F);
+						xgifb_reg_set(XGIPART2, 0x38, 0x32);
+						break;
+					case 720:
+						xgifb_reg_set(XGIPART2, 0x35, 0xF3);
+						xgifb_reg_set(XGIPART2, 0x36, 0x00);
+						xgifb_reg_set(XGIPART2, 0x37, 0x1D);
+						xgifb_reg_set(XGIPART2, 0x38, 0x20);
+						break;
+					case 800:
+						xgifb_reg_set(XGIPART2, 0x35, 0xFC);
+						xgifb_reg_set(XGIPART2, 0x36, 0xFB);
+						xgifb_reg_set(XGIPART2, 0x37, 0x14);
+						xgifb_reg_set(XGIPART2, 0x38, 0x2A);
+						break;
+					}
+				}
+			}
+
+			if ((filter >= 0) && (filter <= 7)) {
+				DPRINTK("FilterTable[%d]-%d: %02x %02x %02x %02x\n", filter_tb, filter,
+						XGI_TV_filter[filter_tb].filter[filter][0],
+						XGI_TV_filter[filter_tb].filter[filter][1],
+						XGI_TV_filter[filter_tb].filter[filter][2],
+						XGI_TV_filter[filter_tb].filter[filter][3]
+				);
+				xgifb_reg_set(
+						XGIPART2,
+						0x35,
+						(XGI_TV_filter[filter_tb].filter[filter][0]));
+				xgifb_reg_set(
+						XGIPART2,
+						0x36,
+						(XGI_TV_filter[filter_tb].filter[filter][1]));
+				xgifb_reg_set(
+						XGIPART2,
+						0x37,
+						(XGI_TV_filter[filter_tb].filter[filter][2]));
+				xgifb_reg_set(
+						XGIPART2,
+						0x38,
+						(XGI_TV_filter[filter_tb].filter[filter][3]));
+			}
+
+		}
+
+	}
+
+}
+
+static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
+		struct fb_info *info)
+{
+
+	unsigned int htotal = var->left_margin + var->xres + var->right_margin
+			+ var->hsync_len;
+	unsigned int vtotal = var->upper_margin + var->yres + var->lower_margin
+			+ var->vsync_len;
+#if defined(__powerpc__)
+	u8 sr_data, cr_data;
+#endif
+	unsigned int drate = 0, hrate = 0;
+	int found_mode = 0;
+	int old_mode;
+	/* unsigned char reg, reg1; */
+
+	DEBUGPRN("Inside do_set_var");
+	/* printk(KERN_DEBUG "XGIfb:var->yres=%d, var->upper_margin=%d, var->lower_margin=%d, var->vsync_len=%d\n", var->yres, var->upper_margin, var->lower_margin, var->vsync_len); */
+
+	info->var.xres_virtual = var->xres_virtual;
+	info->var.yres_virtual = var->yres_virtual;
+	info->var.bits_per_pixel = var->bits_per_pixel;
+
+	if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_NONINTERLACED)
+		vtotal <<= 1;
+	else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE)
+		vtotal <<= 2;
+	else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED) {
+		/* vtotal <<= 1; */
+		/* var->yres <<= 1; */
+	}
+
+	if (!htotal || !vtotal) {
+		DPRINTK("XGIfb: Invalid 'var' information\n");
+		return -EINVAL;
+	} printk(KERN_DEBUG "XGIfb: var->pixclock=%d, htotal=%d, vtotal=%d\n",
+			var->pixclock, htotal, vtotal);
+
+	if (var->pixclock && htotal && vtotal) {
+		drate = 1000000000 / var->pixclock;
+		hrate = (drate * 1000) / htotal;
+		xgi_video_info.refresh_rate = (unsigned int) (hrate * 2
+				/ vtotal);
+	} else {
+		xgi_video_info.refresh_rate = 60;
+	}
+
+	printk(KERN_DEBUG "XGIfb: Change mode to %dx%dx%d-%dHz\n",
+			var->xres, var->yres, var->bits_per_pixel, xgi_video_info.refresh_rate);
+
+	old_mode = xgifb_mode_idx;
+	xgifb_mode_idx = 0;
+
+	while ((XGIbios_mode[xgifb_mode_idx].mode_no != 0)
+			&& (XGIbios_mode[xgifb_mode_idx].xres <= var->xres)) {
+		if ((XGIbios_mode[xgifb_mode_idx].xres == var->xres)
+				&& (XGIbios_mode[xgifb_mode_idx].yres
+						== var->yres)
+				&& (XGIbios_mode[xgifb_mode_idx].bpp
+						== var->bits_per_pixel)) {
+			XGIfb_mode_no = XGIbios_mode[xgifb_mode_idx].mode_no;
+			found_mode = 1;
+			break;
+		}
+		xgifb_mode_idx++;
+	}
+
+	if (found_mode)
+		xgifb_mode_idx = XGIfb_validate_mode(xgifb_mode_idx);
+	else
+		xgifb_mode_idx = -1;
+
+	if (xgifb_mode_idx < 0) {
+		printk(KERN_ERR "XGIfb: Mode %dx%dx%d not supported\n", var->xres,
+				var->yres, var->bits_per_pixel);
+		xgifb_mode_idx = old_mode;
+		return -EINVAL;
+	}
+
+	if (XGIfb_search_refresh_rate(xgi_video_info.refresh_rate) == 0) {
+		XGIfb_rate_idx = XGIbios_mode[xgifb_mode_idx].rate_idx;
+		xgi_video_info.refresh_rate = 60;
+	}
+
+	if (isactive) {
+
+		XGIfb_pre_setmode();
+		if (XGISetModeNew(&XGIhw_ext, XGIfb_mode_no) == 0) {
+			printk(KERN_ERR "XGIfb: Setting mode[0x%x] failed\n", XGIfb_mode_no);
+			return -EINVAL;
+		}
+		info->fix.line_length = ((info->var.xres_virtual
+				* info->var.bits_per_pixel) >> 6);
+
+		xgifb_reg_set(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
+
+		xgifb_reg_set(XGICR, 0x13, (info->fix.line_length & 0x00ff));
+		xgifb_reg_set(XGISR, 0x0E, (info->fix.line_length & 0xff00) >> 8);
+
+		XGIfb_post_setmode();
+
+		DPRINTK("XGIfb: Set new mode: %dx%dx%d-%d\n",
+				XGIbios_mode[xgifb_mode_idx].xres,
+				XGIbios_mode[xgifb_mode_idx].yres,
+				XGIbios_mode[xgifb_mode_idx].bpp,
+				xgi_video_info.refresh_rate);
+
+		xgi_video_info.video_bpp = XGIbios_mode[xgifb_mode_idx].bpp;
+		xgi_video_info.video_vwidth = info->var.xres_virtual;
+		xgi_video_info.video_width = XGIbios_mode[xgifb_mode_idx].xres;
+		xgi_video_info.video_vheight = info->var.yres_virtual;
+		xgi_video_info.video_height = XGIbios_mode[xgifb_mode_idx].yres;
+		xgi_video_info.org_x = xgi_video_info.org_y = 0;
+		xgi_video_info.video_linelength = info->var.xres_virtual
+				* (xgi_video_info.video_bpp >> 3);
+		switch (xgi_video_info.video_bpp) {
+		case 8:
+			xgi_video_info.DstColor = 0x0000;
+			xgi_video_info.XGI310_AccelDepth = 0x00000000;
+			xgi_video_info.video_cmap_len = 256;
+#if defined(__powerpc__)
+			cr_data = xgifb_reg_get(XGICR, 0x4D);
+			xgifb_reg_set(XGICR, 0x4D, (cr_data & 0xE0));
+#endif
+			break;
+		case 16:
+			xgi_video_info.DstColor = 0x8000;
+			xgi_video_info.XGI310_AccelDepth = 0x00010000;
 #if defined(__powerpc__)
 			cr_data = xgifb_reg_get(XGICR, 0x4D);
 			xgifb_reg_set(XGICR, 0x4D, ((cr_data & 0xE0) | 0x0B));
@@ -1249,6 +1466,41 @@ static int XGIfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 	return 0;
 }
 
+/* ----------- FBDev related routines for all series ---------- */
+
+static int XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
+		struct fb_info *info)
+{
+	DEBUGPRN("inside get_fix");
+	memset(fix, 0, sizeof(struct fb_fix_screeninfo));
+
+	strcpy(fix->id, myid);
+
+	fix->smem_start = xgi_video_info.video_base;
+
+	fix->smem_len = xgi_video_info.video_size;
+
+	fix->type = video_type;
+	fix->type_aux = 0;
+	if (xgi_video_info.video_bpp == 8)
+		fix->visual = FB_VISUAL_PSEUDOCOLOR;
+	else
+		fix->visual = FB_VISUAL_DIRECTCOLOR;
+	fix->xpanstep = 0;
+#ifdef XGIFB_PAN
+	if (XGIfb_ypan)
+		fix->ypanstep = 1;
+#endif
+	fix->ywrapstep = 0;
+	fix->line_length = xgi_video_info.video_linelength;
+	fix->mmio_start = xgi_video_info.mmio_base;
+	fix->mmio_len = xgi_video_info.mmio_size;
+	fix->accel = FB_ACCEL_XGI_XABRE;
+
+	DEBUGPRN("end of get_fix");
+	return 0;
+}
+
 static int XGIfb_set_par(struct fb_info *info)
 {
 	int err;
@@ -1460,41 +1712,6 @@ static int XGIfb_blank(int blank, struct fb_info *info)
 	return 0;
 }
 
-/* ----------- FBDev related routines for all series ---------- */
-
-static int XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
-		struct fb_info *info)
-{
-	DEBUGPRN("inside get_fix");
-	memset(fix, 0, sizeof(struct fb_fix_screeninfo));
-
-	strcpy(fix->id, myid);
-
-	fix->smem_start = xgi_video_info.video_base;
-
-	fix->smem_len = xgi_video_info.video_size;
-
-	fix->type = video_type;
-	fix->type_aux = 0;
-	if (xgi_video_info.video_bpp == 8)
-		fix->visual = FB_VISUAL_PSEUDOCOLOR;
-	else
-		fix->visual = FB_VISUAL_DIRECTCOLOR;
-	fix->xpanstep = 0;
-#ifdef XGIFB_PAN
-	if (XGIfb_ypan)
-		fix->ypanstep = 1;
-#endif
-	fix->ywrapstep = 0;
-	fix->line_length = xgi_video_info.video_linelength;
-	fix->mmio_start = xgi_video_info.mmio_base;
-	fix->mmio_len = xgi_video_info.mmio_size;
-	fix->accel = FB_ACCEL_XGI_XABRE;
-
-	DEBUGPRN("end of get_fix");
-	return 0;
-}
-
 static struct fb_ops XGIfb_ops = {
 	.owner = THIS_MODULE,
 	.fb_open = XGIfb_open,
@@ -1676,25 +1893,6 @@ static void XGIfb_detect_VB(void)
 	}
 }
 
-static void XGIfb_get_VB_type(void)
-{
-	u8 reg;
-
-	if (!XGIfb_has_VB()) {
-		reg = xgifb_reg_get(XGICR, IND_XGI_SCRATCH_REG_CR37);
-		switch ((reg & XGI_EXTERNAL_CHIP_MASK) >> 1) {
-		case XGI310_EXTERNAL_CHIP_LVDS:
-			xgi_video_info.hasVB = HASVB_LVDS;
-			break;
-		case XGI310_EXTERNAL_CHIP_LVDS_CHRONTEL:
-			xgi_video_info.hasVB = HASVB_LVDS_CHRONTEL;
-			break;
-		default:
-			break;
-		}
-	}
-}
-
 static int XGIfb_has_VB(void)
 {
 	u8 vb_chipid;
@@ -1714,6 +1912,25 @@ static int XGIfb_has_VB(void)
 	return 1;
 }
 
+static void XGIfb_get_VB_type(void)
+{
+	u8 reg;
+
+	if (!XGIfb_has_VB()) {
+		reg = xgifb_reg_get(XGICR, IND_XGI_SCRATCH_REG_CR37);
+		switch ((reg & XGI_EXTERNAL_CHIP_MASK) >> 1) {
+		case XGI310_EXTERNAL_CHIP_LVDS:
+			xgi_video_info.hasVB = HASVB_LVDS;
+			break;
+		case XGI310_EXTERNAL_CHIP_LVDS_CHRONTEL:
+			xgi_video_info.hasVB = HASVB_LVDS_CHRONTEL;
+			break;
+		default:
+			break;
+		}
+	}
+}
+
 /* ------------------ Sensing routines ------------------ */
 
 /* TW: Determine and detect attached devices on XGI30x */
@@ -1836,224 +2053,6 @@ void XGI_Sense30x(void)
 	xgifb_reg_set(XGIPART4, 0x0d, backupP4_0d);
 }
 
-/* --------------------- SetMode routines ------------------------- */
-
-static void XGIfb_pre_setmode(void)
-{
-	u8 cr30 = 0, cr31 = 0;
-
-	cr31 = xgifb_reg_get(XGICR, 0x31);
-	cr31 &= ~0x60;
-
-	switch (xgi_video_info.disp_state & DISPTYPE_DISP2) {
-	case DISPTYPE_CRT2:
-		cr30 = (XGI_VB_OUTPUT_CRT2 | XGI_SIMULTANEOUS_VIEW_ENABLE);
-		cr31 |= XGI_DRIVER_MODE;
-		break;
-	case DISPTYPE_LCD:
-		cr30 = (XGI_VB_OUTPUT_LCD | XGI_SIMULTANEOUS_VIEW_ENABLE);
-		cr31 |= XGI_DRIVER_MODE;
-		break;
-	case DISPTYPE_TV:
-		if (xgi_video_info.TV_type == TVMODE_HIVISION)
-			cr30 = (XGI_VB_OUTPUT_HIVISION
-					| XGI_SIMULTANEOUS_VIEW_ENABLE);
-		else if (xgi_video_info.TV_plug == TVPLUG_SVIDEO)
-			cr30 = (XGI_VB_OUTPUT_SVIDEO
-					| XGI_SIMULTANEOUS_VIEW_ENABLE);
-		else if (xgi_video_info.TV_plug == TVPLUG_COMPOSITE)
-			cr30 = (XGI_VB_OUTPUT_COMPOSITE
-					| XGI_SIMULTANEOUS_VIEW_ENABLE);
-		else if (xgi_video_info.TV_plug == TVPLUG_SCART)
-			cr30 = (XGI_VB_OUTPUT_SCART
-					| XGI_SIMULTANEOUS_VIEW_ENABLE);
-		cr31 |= XGI_DRIVER_MODE;
-
-		if (XGIfb_tvmode == 1 || xgi_video_info.TV_type == TVMODE_PAL)
-			cr31 |= 0x01;
-		else
-			cr31 &= ~0x01;
-		break;
-	default: /* disable CRT2 */
-		cr30 = 0x00;
-		cr31 |= (XGI_DRIVER_MODE | XGI_VB_OUTPUT_DISABLE);
-	}
-
-	xgifb_reg_set(XGICR, IND_XGI_SCRATCH_REG_CR30, cr30);
-	xgifb_reg_set(XGICR, IND_XGI_SCRATCH_REG_CR31, cr31);
-	xgifb_reg_set(XGICR, IND_XGI_SCRATCH_REG_CR33, (XGIfb_rate_idx & 0x0F));
-}
-
-static void XGIfb_post_setmode(void)
-{
-	u8 reg;
-	unsigned char doit = 1;
-	/*
-	xgifb_reg_set(XGISR,IND_XGI_PASSWORD,XGI_PASSWORD);
-	xgifb_reg_set(XGICR, 0x13, 0x00);
-	xgifb_reg_and_or(XGISR,0x0E, 0xF0, 0x01);
-	*test*
-	*/
-	if (xgi_video_info.video_bpp == 8) {
-		/* TW: We can't switch off CRT1 on LVDS/Chrontel in 8bpp Modes */
-		if ((xgi_video_info.hasVB == HASVB_LVDS)
-				|| (xgi_video_info.hasVB == HASVB_LVDS_CHRONTEL)) {
-			doit = 0;
-		}
-		/* TW: We can't switch off CRT1 on 301B-DH in 8bpp Modes if using LCD */
-		if (xgi_video_info.disp_state & DISPTYPE_LCD)
-			doit = 0;
-	}
-
-	/* TW: We can't switch off CRT1 if bridge is in slave mode */
-	if (xgi_video_info.hasVB != HASVB_NONE) {
-		reg = xgifb_reg_get(XGIPART1, 0x00);
-
-		if ((reg & 0x50) == 0x10)
-			doit = 0;
-
-	} else {
-		XGIfb_crt1off = 0;
-	}
-
-	reg = xgifb_reg_get(XGICR, 0x17);
-	if ((XGIfb_crt1off) && (doit))
-		reg &= ~0x80;
-	else
-		reg |= 0x80;
-	xgifb_reg_set(XGICR, 0x17, reg);
-
-	xgifb_reg_and(XGISR, IND_XGI_RAMDAC_CONTROL, ~0x04);
-
-	if ((xgi_video_info.disp_state & DISPTYPE_TV) && (xgi_video_info.hasVB
-			== HASVB_301)) {
-
-		reg = xgifb_reg_get(XGIPART4, 0x01);
-
-		if (reg < 0xB0) { /* Set filter for XGI301 */
-
-			switch (xgi_video_info.video_width) {
-			case 320:
-				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 4 : 12;
-				break;
-			case 640:
-				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 5 : 13;
-				break;
-			case 720:
-				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 6 : 14;
-				break;
-			case 800:
-				filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 7 : 15;
-				break;
-			default:
-				filter = -1;
-				break;
-			}
-
-			xgifb_reg_or(XGIPART1, XGIfb_CRT2_write_enable, 0x01);
-
-			if (xgi_video_info.TV_type == TVMODE_NTSC) {
-
-				xgifb_reg_and(XGIPART2, 0x3a, 0x1f);
-
-				if (xgi_video_info.TV_plug == TVPLUG_SVIDEO) {
-
-					xgifb_reg_and(XGIPART2, 0x30, 0xdf);
-
-				} else if (xgi_video_info.TV_plug
-						== TVPLUG_COMPOSITE) {
-
-					xgifb_reg_or(XGIPART2, 0x30, 0x20);
-
-					switch (xgi_video_info.video_width) {
-					case 640:
-						xgifb_reg_set(XGIPART2, 0x35, 0xEB);
-						xgifb_reg_set(XGIPART2, 0x36, 0x04);
-						xgifb_reg_set(XGIPART2, 0x37, 0x25);
-						xgifb_reg_set(XGIPART2, 0x38, 0x18);
-						break;
-					case 720:
-						xgifb_reg_set(XGIPART2, 0x35, 0xEE);
-						xgifb_reg_set(XGIPART2, 0x36, 0x0C);
-						xgifb_reg_set(XGIPART2, 0x37, 0x22);
-						xgifb_reg_set(XGIPART2, 0x38, 0x08);
-						break;
-					case 800:
-						xgifb_reg_set(XGIPART2, 0x35, 0xEB);
-						xgifb_reg_set(XGIPART2, 0x36, 0x15);
-						xgifb_reg_set(XGIPART2, 0x37, 0x25);
-						xgifb_reg_set(XGIPART2, 0x38, 0xF6);
-						break;
-					}
-				}
-
-			} else if (xgi_video_info.TV_type == TVMODE_PAL) {
-
-				xgifb_reg_and(XGIPART2, 0x3A, 0x1F);
-
-				if (xgi_video_info.TV_plug == TVPLUG_SVIDEO) {
-
-					xgifb_reg_and(XGIPART2, 0x30, 0xDF);
-
-				} else if (xgi_video_info.TV_plug
-						== TVPLUG_COMPOSITE) {
-
-					xgifb_reg_or(XGIPART2, 0x30, 0x20);
-
-					switch (xgi_video_info.video_width) {
-					case 640:
-						xgifb_reg_set(XGIPART2, 0x35, 0xF1);
-						xgifb_reg_set(XGIPART2, 0x36, 0xF7);
-						xgifb_reg_set(XGIPART2, 0x37, 0x1F);
-						xgifb_reg_set(XGIPART2, 0x38, 0x32);
-						break;
-					case 720:
-						xgifb_reg_set(XGIPART2, 0x35, 0xF3);
-						xgifb_reg_set(XGIPART2, 0x36, 0x00);
-						xgifb_reg_set(XGIPART2, 0x37, 0x1D);
-						xgifb_reg_set(XGIPART2, 0x38, 0x20);
-						break;
-					case 800:
-						xgifb_reg_set(XGIPART2, 0x35, 0xFC);
-						xgifb_reg_set(XGIPART2, 0x36, 0xFB);
-						xgifb_reg_set(XGIPART2, 0x37, 0x14);
-						xgifb_reg_set(XGIPART2, 0x38, 0x2A);
-						break;
-					}
-				}
-			}
-
-			if ((filter >= 0) && (filter <= 7)) {
-				DPRINTK("FilterTable[%d]-%d: %02x %02x %02x %02x\n", filter_tb, filter,
-						XGI_TV_filter[filter_tb].filter[filter][0],
-						XGI_TV_filter[filter_tb].filter[filter][1],
-						XGI_TV_filter[filter_tb].filter[filter][2],
-						XGI_TV_filter[filter_tb].filter[filter][3]
-				);
-				xgifb_reg_set(
-						XGIPART2,
-						0x35,
-						(XGI_TV_filter[filter_tb].filter[filter][0]));
-				xgifb_reg_set(
-						XGIPART2,
-						0x36,
-						(XGI_TV_filter[filter_tb].filter[filter][1]));
-				xgifb_reg_set(
-						XGIPART2,
-						0x37,
-						(XGI_TV_filter[filter_tb].filter[filter][2]));
-				xgifb_reg_set(
-						XGIPART2,
-						0x38,
-						(XGI_TV_filter[filter_tb].filter[filter][3]));
-			}
-
-		}
-
-	}
-
-}
-
 XGIINITSTATIC int __init XGIfb_setup(char *options)
 {
 	char *this_opt;