staging: xgifb: main: move functions to avoid forward declarations
authorAaro Koskinen <aaro.koskinen@iki.fi>
Mon, 18 Apr 2011 19:14:05 +0000 (22:14 +0300)
committerGreg Kroah-Hartman <gregkh@suse.de>
Tue, 26 Apr 2011 00:26:27 +0000 (17:26 -0700)
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
drivers/staging/xgifb/XGI_main_26.c

index c9764a7..1b20e55 100644 (file)
@@ -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,
index 8de6829..1350681 100644 (file)
@@ -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;