patch-2.4.9 linux/drivers/video/controlfb.c

• Lines: 1653
• Date: Sun Aug 12 10:51:42 2001
• Orig file: v2.4.8/linux/drivers/video/controlfb.c
• Orig date: Fri Feb 9 11:30:23 2001

diff -u --recursive --new-file v2.4.8/linux/drivers/video/controlfb.c linux/drivers/video/controlfb.c
@@ -3,6 +3,9 @@
  *
  *  Created 12 July 1998 by Dan Jacobowitz <dan@debian.org>
  *  Copyright (C) 1998 Dan Jacobowitz
+ *  Copyright (C) 2001 Takashi Oe
+ *
+ *  Mmap code by Michel Lanners <mlan@cpu.lu>
  *
  *  Frame buffer structure from:
  *    drivers/video/chipsfb.c -- frame buffer device for
@@ -62,26 +65,40 @@
 	int	xres, yres;
 	int	vxres, vyres;
 	int	xoffset, yoffset;
+	int	pitch;
+	struct control_regvals	regvals;
+	unsigned long sync;
+	unsigned char ctrl;
 };
 
 #define DIRTY(z) ((x)->z != (y)->z)
+#define DIRTY_CMAP(z) (memcmp(&((x)->z), &((y)->z), sizeof((y)->z)))
 static inline int PAR_EQUAL(struct fb_par_control *x, struct fb_par_control *y)
 {
-	return (!DIRTY(vmode) && !DIRTY(cmode) && !DIRTY(xres)
-		&& !DIRTY(yres) && !DIRTY(vxres) && !DIRTY(vyres)
-		&& !DIRTY(xoffset) && !DIRTY(yoffset));
+	int i, results;
+
+	results = 1;
+	for (i = 0; i < 3; i++)
+		results &= !DIRTY(regvals.clock_params[i]);
+	if (!results)
+		return 0;
+	for (i = 0; i < 16; i++)
+		results &= !DIRTY(regvals.regs[i]);
+	if (!results)
+		return 0;
+	return (!DIRTY(cmode) && !DIRTY(xres) && !DIRTY(yres)
+		&& !DIRTY(vxres) && !DIRTY(vyres));
 }
 static inline int VAR_MATCH(struct fb_var_screeninfo *x, struct fb_var_screeninfo *y)
 {
 	return (!DIRTY(bits_per_pixel) && !DIRTY(xres)
 		&& !DIRTY(yres) && !DIRTY(xres_virtual)
-		&& !DIRTY(yres_virtual));
+		&& !DIRTY(yres_virtual)
+		&& !DIRTY_CMAP(red) && !DIRTY_CMAP(green) && !DIRTY_CMAP(blue));
 }
 
 struct fb_info_control {
 	struct fb_info			info;
-/*	struct fb_fix_screeninfo	fix;
-	struct fb_var_screeninfo	var;*/
 	struct display			display;
 	struct fb_par_control		par;
 	struct {
@@ -93,12 +110,16 @@
 	
 	struct control_regs	*control_regs;
 	unsigned long		control_regs_phys;
+	unsigned long		control_regs_size;
 	
 	__u8			*frame_buffer;
 	unsigned long		frame_buffer_phys;
-	
-	int			sense, control_use_bank2;
+	unsigned long		fb_orig_base;
+	unsigned long		fb_orig_size;
+
+	int			control_use_bank2;
 	unsigned long		total_vram;
+	unsigned char		vram_attr;
 	union {
 #ifdef FBCON_HAS_CFB16
 		u16 cfb16[16];
@@ -109,61 +130,96 @@
 	} fbcon_cmap;
 };
 
+/* control register access macro */
+#define CNTRL_REG(INFO,REG) (&(((INFO)->control_regs-> ## REG).r))
+
+
 /******************** Prototypes for exported functions ********************/
+/*
+ * struct fb_ops
+ */
 static int control_get_fix(struct fb_fix_screeninfo *fix, int con,
-			 struct fb_info *info);
+	struct fb_info *info);
 static int control_get_var(struct fb_var_screeninfo *var, int con,
-			 struct fb_info *info);
+	struct fb_info *info);
 static int control_set_var(struct fb_var_screeninfo *var, int con,
-			 struct fb_info *info);
+	struct fb_info *info);
 static int control_pan_display(struct fb_var_screeninfo *var, int con,
-			     struct fb_info *info);
+	struct fb_info *info);
 static int control_get_cmap(struct fb_cmap *cmap, int kspc, int con,
-			  struct fb_info *info);
+	struct fb_info *info);
 static int control_set_cmap(struct fb_cmap *cmap, int kspc, int con,
-			  struct fb_info *info);
+	struct fb_info *info);
 static int control_mmap(struct fb_info *info, struct file *file,
-                         struct vm_area_struct *vma);
+	struct vm_area_struct *vma);
 
+/*
+ * low level fbcon ops
+ */
+static int controlfb_switch(int con, struct fb_info *info);
+static int controlfb_updatevar(int con, struct fb_info *info);
+static void controlfb_blank(int blank_mode, struct fb_info *info);
 
+/*
+ * low level cmap set/get ops
+ */
 static int controlfb_getcolreg(u_int regno, u_int *red, u_int *green,
-			     u_int *blue, u_int *transp, struct fb_info *info);
+	u_int *blue, u_int *transp, struct fb_info *info);
 static int controlfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
-			     u_int transp, struct fb_info *info);
-
-/******************** Prototypes for internal functions ********************/
-static void control_par_to_fix(struct fb_par_control *par, struct fb_fix_screeninfo *fix,
-	struct fb_info_control *p);
-static void do_install_cmap(int con, struct fb_info *info);
-static void control_set_dispsw(struct display *disp, int cmode, struct fb_info_control *p);
-
-/************************* Internal variables *****************************/
-static int currcon = 0;
-static int par_set = 0;
-static char fontname[40] __initdata = { 0 };
-static int default_vmode = VMODE_NVRAM;
-static int default_cmode = CMODE_NVRAM;
+	u_int transp, struct fb_info *info);
 
 /*
- * Exported functions
+ * inititialization
  */
 int control_init(void);
 void control_setup(char *);
 
-static void control_of_init(struct device_node *dp);
-static int read_control_sense(struct fb_info_control *p);
-static inline int control_vram_reqd(int video_mode, int color_mode);
+/*
+ * low level fbcon revc ops
+ */
+static void control_cfb16_revc(struct display *p, int xx, int yy);
+static void control_cfb32_revc(struct display *p, int xx, int yy);
+
+
+/******************** Prototypes for internal functions **********************/
+
+static void do_install_cmap(struct display *disp, struct fb_info *info);
 static void set_control_clock(unsigned char *params);
-static void control_set_hardware(struct fb_info_control *p, struct fb_par_control *par);
-static inline void control_par_to_var(struct fb_par_control *par, struct fb_var_screeninfo *var);
+static int init_control(struct fb_info_control *p);
+static void control_set_hardware(struct fb_info_control *p,
+	struct fb_par_control *par);
+static int control_of_init(struct device_node *dp);
+static void control_par_to_fix(struct fb_par_control *par,
+	struct fb_fix_screeninfo *fix, struct fb_info_control *p);
+static void control_set_dispsw(struct display *disp, int cmode,
+	struct fb_info_control *p);
+static void find_vram_size(struct fb_info_control *p);
+static int read_control_sense(struct fb_info_control *p);
+static int calc_clock_params(unsigned long clk, unsigned char *param);
 static int control_var_to_par(struct fb_var_screeninfo *var,
 	struct fb_par_control *par, const struct fb_info *fb_info);
-
-static void control_init_info(struct fb_info *info, struct fb_info_control *p);
+static inline void control_par_to_var(struct fb_par_control *par,
+	struct fb_var_screeninfo *var);
+static void control_par_to_fix(struct fb_par_control *par,
+	struct fb_fix_screeninfo *fix, struct fb_info_control *p);
 static void control_par_to_display(struct fb_par_control *par,
-  struct display *disp, struct fb_fix_screeninfo *fix, struct fb_info_control *p);
+	struct display *disp, struct fb_fix_screeninfo *fix,
+	struct fb_info_control *p);
+static void control_set_dispsw(struct display *disp, int cmode,
+	struct fb_info_control *p);
+static void control_init_info(struct fb_info *info, struct fb_info_control *p);
+static void control_cleanup(void);
+
+
+/************************** Internal variables *******************************/
+
+static int currcon;
+static struct fb_info_control *control_fb;
+
+static char fontname[40] __initdata = { 0 };
+static int default_vmode __initdata = VMODE_NVRAM;
+static int default_cmode __initdata = CMODE_NVRAM;
 
-static int controlfb_updatevar(int con, struct fb_info *info);
 
 static struct fb_ops controlfb_ops = {
 	owner:		THIS_MODULE,
@@ -177,7 +233,6 @@
 };
 
 
-
 /********************  The functions for controlfb_ops ********************/
 
 #ifdef MODULE
@@ -185,18 +240,16 @@
 {
 	struct device_node *dp;
 
-	printk("Loading...\n");
 	dp = find_devices("control");
-	if (dp != 0)
-		control_of_init(dp);
-	else
-		printk("Failed.\n");
-	printk("Done.\n");
+	if (dp != 0 && !control_of_init(dp))
+		return 0;
+
+	return -ENXIO;
 }
 
 void cleanup_module(void)
 {
-	/* FIXME: clean up and release regions */
+	control_cleanup();
 }
 #endif
 
@@ -207,8 +260,6 @@
 {
 	struct fb_info_control *p = (struct fb_info_control *) info;
 
-	if(!par_set)
-		printk(KERN_ERR "control_get_fix called with unset par!\n");
 	if(con == -1) {
 		control_par_to_fix(&p->par, fix, p);
 	} else {
@@ -225,8 +276,6 @@
 {
 	struct fb_info_control *p = (struct fb_info_control *) info;
 
-	if(!par_set)
-		printk(KERN_ERR "control_get_var called with unset par!\n");
 	if(con == -1) {
 		control_par_to_var(&p->par, var);
 	} else {
@@ -235,8 +284,10 @@
 	return 0;
 }
 
-/* Sets everything according to var */
-/* No longer safe for use in console switching */
+
+/*
+ * Sets everything according to var
+ */
 static int control_set_var(struct fb_var_screeninfo *var, int con,
 			 struct fb_info *info)
 {
@@ -246,10 +297,10 @@
 	int depthchange, err;
 	int activate = var->activate;
 
-	disp = (con >= 0) ? &fb_display[con] : info->disp;
-
 	if((err = control_var_to_par(var, &par, info))) {
-		printk (KERN_ERR "control_set_var: error calling control_var_to_par: %d.\n", err);
+		if (con < 0)
+			printk (KERN_ERR "control_set_var: error calling"
+					 " control_var_to_par: %d.\n", err);
 		return err;
 	}
 	
@@ -258,8 +309,8 @@
 	if ((activate & FB_ACTIVATE_MASK) != FB_ACTIVATE_NOW)
 		return 0;
 
-/* I know, we want to use fb_display[con], but grab certain info from p->var instead. */
-/* [above no longer true] */
+	disp = (con >= 0) ? &fb_display[con] : info->disp;
+
 	depthchange = (disp->var.bits_per_pixel != var->bits_per_pixel);
 	if(!VAR_MATCH(&disp->var, var)) {
 		struct fb_fix_screeninfo	fix;
@@ -269,32 +320,54 @@
 			(*info->changevar)(con);
 	} else
 		disp->var = *var;
-	/*p->disp = *disp;*/
 
-	
-	if(con == currcon || con == -1) {
+	if(con == currcon) {
 		control_set_hardware(p, &par);
+		if(depthchange) {
+			if((err = fb_alloc_cmap(&disp->cmap, 0, 0)))
+				return err;
+			do_install_cmap(disp, info);
+		}
 	}
-	if(depthchange) {
-		if((err = fb_alloc_cmap(&disp->cmap, 0, 0)))
-			return err;
-		do_install_cmap(con, info);
-	}
+
 	return 0;
 }
 
+
+/*
+ * Set screen start address according to var offset values
+ */
+static inline void set_screen_start(int xoffset, int yoffset,
+	struct fb_info_control *p)
+{
+	struct fb_par_control *par = &p->par;
+
+	par->xoffset = xoffset;
+	par->yoffset = yoffset;
+	out_le32(CNTRL_REG(p,start_addr),
+		 par->yoffset * par->pitch + (par->xoffset << par->cmode));
+}
+
+
 static int control_pan_display(struct fb_var_screeninfo *var, int con,
 			     struct fb_info *info)
 {
+	unsigned int xoffset, hstep;
 	struct fb_info_control *p = (struct fb_info_control *)info;
 	struct fb_par_control *par = &p->par;
-	
-	if (var->xoffset != 0 || var->yoffset+var->yres > var->yres_virtual)
+
+	/*
+	 * make sure start addr will be 32-byte aligned
+	 */
+	hstep = 0x1f >> par->cmode;
+	xoffset = (var->xoffset + hstep) & ~hstep;
+
+	if (xoffset+par->xres > par->vxres ||
+	    var->yoffset+par->yres > par->vyres)
 		return -EINVAL;
-	fb_display[con].var.yoffset =  par->yoffset = var->yoffset;
-	if(con == currcon)
-		out_le32(&p->control_regs->start_addr.r,
-		    par->yoffset * (par->vxres << par->cmode));
+
+	set_screen_start(xoffset, var->yoffset, p);
+
 	return 0;
 }
 
@@ -315,11 +388,10 @@
 static int control_set_cmap(struct fb_cmap *cmap, int kspc, int con,
 			 struct fb_info *info)
 {
-	struct display *disp = &fb_display[con];
-	int err;
+	struct display *disp = (con < 0)? info->disp: &fb_display[con];
+	int err, size = disp->var.bits_per_pixel == 16 ? 32 : 256;
 
-	if (disp->cmap.len == 0) {
-		int size = disp->var.bits_per_pixel == 16 ? 32 : 256;
+	if (disp->cmap.len != size) {
 		err = fb_alloc_cmap(&disp->cmap, size, 0);
 		if (err)
 			return err;
@@ -330,7 +402,9 @@
 	return 0;
 }
 
-/* Private mmap since we want to have a different caching on the framebuffer
+
+/*
+ * Private mmap since we want to have a different caching on the framebuffer
  * for controlfb.
  * Note there's no locking in here; it's done in fb_mmap() in fbmem.c.
  */
@@ -363,6 +437,9 @@
                pgprot_val(vma->vm_page_prot) |= _PAGE_WRITETHRU;
        }
        start &= PAGE_MASK;
+       if ((vma->vm_end - vma->vm_start + off) > len)
+       		return -EINVAL;
+       off += start;
        vma->vm_pgoff = off >> PAGE_SHIFT;
        if (io_remap_page_range(vma->vm_start, off,
            vma->vm_end - vma->vm_start, vma->vm_page_prot))
@@ -372,77 +449,80 @@
 }
 
 
-/********************  End of controlfb_ops implementation  ********************/
-/* (new one that is) */
+/********************  End of controlfb_ops implementation  ******************/
 
+/*
+ * low level fbcon ops
+ */
 
 static int controlfb_switch(int con, struct fb_info *info)
 {
 	struct fb_info_control	*p = (struct fb_info_control *)info;
 	struct fb_par_control	par;
-	int oldcon = currcon;
 
-	if (fb_display[currcon].cmap.len)
+	if (currcon >= 0 && fb_display[currcon].cmap.len)
 		fb_get_cmap(&fb_display[currcon].cmap, 1, controlfb_getcolreg,
 			    info);
 	currcon = con;
 
+	fb_display[con].var.activate = FB_ACTIVATE_NOW;
 	control_var_to_par(&fb_display[con].var, &par, info);
 	control_set_hardware(p, &par);
 	control_set_dispsw(&fb_display[con], par.cmode, p);
+	do_install_cmap(&fb_display[con], info);
 
-	if(fb_display[oldcon].var.yoffset != fb_display[con].var.yoffset)
-		controlfb_updatevar(con, info);
-
-	do_install_cmap(con, info);
 	return 1;
 }
 
+
 static int controlfb_updatevar(int con, struct fb_info *info)
 {
-	struct fb_info_control	*p = (struct fb_info_control *)info;
+	struct fb_var_screeninfo *var = &fb_display[con].var;
+	struct fb_info_control *p = (struct fb_info_control *) info;
+
+	set_screen_start(var->xoffset, var->yoffset, p);
 
-	if(con != currcon)
-		return 0;
-	/* imsttfb blanks the unused bottom of the screen here...hmm. */
-	out_le32(&p->control_regs->start_addr.r,
-	    fb_display[con].var.yoffset * fb_display[con].line_length);
-	
 	return 0;
 }
 
+
 static void controlfb_blank(int blank_mode, struct fb_info *info)
 {
-/*
- *  Blank the screen if blank_mode != 0, else unblank. If blank == NULL
- *  then the caller blanks by setting the CLUT (Color Look Up Table) to all
- *  black. Return 0 if blanking succeeded, != 0 if un-/blanking failed due
- *  to e.g. a video mode which doesn't support it. Implements VESA suspend
- *  and powerdown modes on hardware that supports disabling hsync/vsync:
- *    blank_mode == 2: suspend vsync
- *    blank_mode == 3: suspend hsync
- *    blank_mode == 4: powerdown
- */
-/* A blank_mode of 1+VESA_NO_BLANKING or 1+VESA_POWERDOWN act alike... */
 	struct fb_info_control *p = (struct fb_info_control *) info;
-	int	ctrl;
-
-	if(blank_mode == 1+VESA_NO_BLANKING)
-		blank_mode = 1+VESA_POWERDOWN;
-	ctrl = ld_le32(&p->control_regs->ctrl.r) | 0x33;
-	if (blank_mode)
-		--blank_mode;
-	if (blank_mode & VESA_VSYNC_SUSPEND)
-		ctrl &= ~3;
-	if (blank_mode & VESA_HSYNC_SUSPEND)
-		ctrl &= ~0x30;
-	out_le32(&p->control_regs->ctrl.r, ctrl);
+	unsigned ctrl;
 
-/* TODO: Figure out how the heck to powerdown this thing! */
+	ctrl = ld_le32(CNTRL_REG(p,ctrl));
+	if (blank_mode > 0)
+		switch (blank_mode - 1) {
+		case VESA_VSYNC_SUSPEND:
+			ctrl &= ~3;
+			break;
+		case VESA_HSYNC_SUSPEND:
+			ctrl &= ~0x30;
+			break;
+		case VESA_POWERDOWN:
+			ctrl &= ~0x33;
+			/* fall through */
+		case VESA_NO_BLANKING:
+			ctrl |= 0x400;
+			break;
+		default:
+			break;
+		}
+	else {
+		ctrl &= ~0x400;
+		ctrl |= 0x33;
+	}
+	out_le32(CNTRL_REG(p,ctrl), ctrl);
 
-    return;
+	return;
 }
 
+
+/*
+ * low level cmap set/get ops
+ */
+
 static int controlfb_getcolreg(u_int regno, u_int *red, u_int *green,
 			     u_int *blue, u_int *transp, struct fb_info *info)
 {
@@ -497,33 +577,25 @@
 	return 0;
 }
 
-static void do_install_cmap(int con, struct fb_info *info)
+static void do_install_cmap(struct display *disp, struct fb_info *info)
 {
-	if (con != currcon)
-		return;
-	if (fb_display[con].cmap.len)
-		fb_set_cmap(&fb_display[con].cmap, 1, controlfb_setcolreg,
+	if (disp->cmap.len)
+		fb_set_cmap(&disp->cmap, 1, controlfb_setcolreg,
 			    info);
 	else {
-		int size = fb_display[con].var.bits_per_pixel == 16 ? 32 : 256;
+		int size = disp->var.bits_per_pixel == 16 ? 32 : 256;
 		fb_set_cmap(fb_default_cmap(size), 1, controlfb_setcolreg,
 			    info);
 	}
 }
 
-static inline int control_vram_reqd(int video_mode, int color_mode)
-{
-	return (control_reg_init[video_mode-1]->vres
-		* control_reg_init[video_mode-1]->hres << color_mode)
-	       + control_reg_init[video_mode-1]->offset[color_mode];
-}
 
 static void set_control_clock(unsigned char *params)
 {
+#ifdef CONFIG_ADB_CUDA
 	struct adb_request req;
 	int i;
 
-#ifdef CONFIG_ADB_CUDA
 	for (i = 0; i < 3; ++i) {
 		cuda_request(&req, NULL, 5, CUDA_PACKET, CUDA_GET_SET_IIC,
 			     0x50, i + 1, params[i]);
@@ -534,128 +606,137 @@
 }
 
 
-static void __init init_control(struct fb_info_control *p)
+/*
+ * finish off the driver initialization and register
+ */
+static int __init init_control(struct fb_info_control *p)
 {
-	struct fb_par_control parstruct;
-	struct fb_par_control *par = &parstruct;
+	int full, sense, vmode, cmode, vyres;
 	struct fb_var_screeninfo var;
 
-	p->sense = read_control_sense(p);
-	printk(KERN_INFO "Monitor sense value = 0x%x, ", p->sense);
+	printk(KERN_INFO "controlfb: ");
+
+	full = p->total_vram == 0x400000;
+
 	/* Try to pick a video mode out of NVRAM if we have one. */
-	if (default_vmode == VMODE_NVRAM) {
-		par->vmode = nvram_read_byte(NV_VMODE);
-		if(par->vmode <= 0 || par->vmode > VMODE_MAX || !control_reg_init[par->vmode - 1])
-			par->vmode = VMODE_CHOOSE;
-		if(par->vmode == VMODE_CHOOSE)
-			par->vmode = mac_map_monitor_sense(p->sense);
-		if(!control_reg_init[par->vmode - 1])
-			par->vmode = VMODE_640_480_60;
+	if (default_cmode == CMODE_NVRAM){
+		cmode = nvram_read_byte(NV_CMODE);
+		if(cmode < CMODE_8 || cmode > CMODE_32)
+			cmode = CMODE_8;
 	} else
-		par->vmode=default_vmode;
+		cmode=default_cmode;
+
+	if (default_vmode == VMODE_NVRAM) {
+		vmode = nvram_read_byte(NV_VMODE);
+		if (vmode < 1 || vmode > VMODE_MAX ||
+		    control_mac_modes[vmode - 1].m[full] < cmode) {
+			sense = read_control_sense(p);
+			printk("Monitor sense value = 0x%x, ", sense);
+			vmode = mac_map_monitor_sense(sense);
+			if (control_mac_modes[vmode - 1].m[full] < cmode)
+				vmode = VMODE_640_480_60;
+		}
+	} else {
+		vmode=default_vmode;
+		if (control_mac_modes[vmode - 1].m[full] < cmode) {
+			if (cmode > CMODE_8)
+				cmode--;
+			else
+				vmode = VMODE_640_480_60;
+		}
+	}
+
+	if (mac_vmode_to_var(vmode, cmode, &var) < 0) {
+		/* This shouldn't happen! */
+		printk("mac_vmode_to_var(%d, %d,) failed\n", vmode, cmode);
+try_again:
+		vmode = VMODE_640_480_60;
+		cmode = CMODE_8;
+		if (mac_vmode_to_var(vmode, cmode, &var) < 0) {
+			printk(KERN_ERR "controlfb: mac_vmode_to_var() failed\n");
+			return -ENXIO;
+		}
+		printk(KERN_INFO "controlfb: ");
+	}
+	printk("using video mode %d and color mode %d.\n", vmode, cmode);
+
+	vyres = (p->total_vram - CTRLFB_OFF) / (var.xres << cmode);
+	if (vyres > var.yres)
+		var.yres_virtual = vyres;
 
-	if (default_cmode == CMODE_NVRAM){
-		par->cmode = nvram_read_byte(NV_CMODE);
-		if(par->cmode < CMODE_8 || par->cmode > CMODE_32)
-			par->cmode = CMODE_8;}
-	else
-		par->cmode=default_cmode;
-	/*
-	 * Reduce the pixel size if we don't have enough VRAM.
-	 */
-	while(par->cmode > CMODE_8 && control_vram_reqd(par->vmode, par->cmode) > p->total_vram)
-		par->cmode--;
-	
-	printk("using video mode %d and color mode %d.\n", par->vmode, par->cmode);
-	
-	par->vxres = par->xres = control_reg_init[par->vmode - 1]->hres;
-	par->yres = control_reg_init[par->vmode - 1]->vres;
-	par->vyres = p->total_vram / (par->vxres << par->cmode);
-	par->xoffset = par->yoffset = 0;
-	
 	control_init_info(&p->info, p);
-	
-	par_set = 1;	/* Debug */
-	
-	control_par_to_var(par, &var);
+	currcon = -1;
 	var.activate = FB_ACTIVATE_NOW;
-	control_set_var(&var, -1, &p->info);
-	
-	p->info.flags = FBINFO_FLAG_DEFAULT;
-	if (register_framebuffer(&p->info) < 0) {
-		kfree(p);
-		return;
+
+	if (control_set_var(&var, -1, &p->info) < 0) {
+		if (vmode != VMODE_640_480_60 || cmode != CMODE_8)
+			goto try_again;
+		printk(KERN_ERR "controlfb: initilization failed\n");
+		return -ENXIO;
 	}
+
+	p->info.flags = FBINFO_FLAG_DEFAULT;
+	if (register_framebuffer(&p->info) < 0)
+		return -ENXIO;
 	
 	printk(KERN_INFO "fb%d: control display adapter\n", GET_FB_IDX(p->info.node));	
+
+	return 0;
 }
 
-#define STORE_D2(a,d) \
+#define RADACAL_WRITE(a,d) \
 	out_8(&p->cmap_regs->addr, (a)); \
-	out_8(&p->cmap_regs->d2,   (d))
+	out_8(&p->cmap_regs->dat,   (d))
 
 /* Now how about actually saying, Make it so! */
 /* Some things in here probably don't need to be done each time. */
 static void control_set_hardware(struct fb_info_control *p, struct fb_par_control *par)
 {
-	struct control_regvals	*init;
+	struct control_regvals	*r;
 	volatile struct preg	*rp;
-	int			flags, ctrl, i;
-	int			vmode, cmode;
-	
-	if(PAR_EQUAL(&p->par, par))
+	int			i, cmode;
+
+	if (PAR_EQUAL(&p->par, par)) {
+		/*
+		 * check if only xoffset or yoffset differs.
+		 * this prevents flickers in typical VT switch case.
+		 */
+		if (p->par.xoffset != par->xoffset ||
+		    p->par.yoffset != par->yoffset)
+			set_screen_start(par->xoffset, par->yoffset, p);
+			
 		return;
+	}
 	
 	p->par = *par;
-	
-	vmode = p->par.vmode;
 	cmode = p->par.cmode;
+	r = &par->regvals;
 	
-	init = control_reg_init[vmode - 1];
+	/* Turn off display */
+	out_le32(CNTRL_REG(p,ctrl), 0x400 | par->ctrl);
 	
-	if (control_vram_reqd(vmode, cmode) > 0x200000)
-		flags = 0x51;
-	else if (p->control_use_bank2)
-		flags = 0x39;
-	else
-		flags = 0x31;
-	if (vmode >= VMODE_1280_960_75 && cmode >= CMODE_16)
-		ctrl = 0x7f;
-	else
-		ctrl = 0x3b;
-
-	/* Initialize display timing registers */
-	out_le32(&p->control_regs->ctrl.r, 0x43b);
-	
-	set_control_clock(init->clock_params);
+	set_control_clock(r->clock_params);
 	
-	STORE_D2(0x20, init->radacal_ctrl[cmode]);
-	STORE_D2(0x21, p->control_use_bank2 ? 0 : 1);
-	STORE_D2(0x10, 0);
-	STORE_D2(0x11, 0);
+	RADACAL_WRITE(0x20, r->radacal_ctrl);
+	RADACAL_WRITE(0x21, p->control_use_bank2 ? 0 : 1);
+	RADACAL_WRITE(0x10, 0);
+	RADACAL_WRITE(0x11, 0);
 
 	rp = &p->control_regs->vswin;
 	for (i = 0; i < 16; ++i, ++rp)
-		out_le32(&rp->r, init->regs[i]);
+		out_le32(&rp->r, r->regs[i]);
 	
-	out_le32(&p->control_regs->pitch.r, par->vxres << cmode);
-	out_le32(&p->control_regs->mode.r, init->mode[cmode]);
-	out_le32(&p->control_regs->flags.r, flags);
-	out_le32(&p->control_regs->start_addr.r,
-	    par->yoffset * (par->vxres << cmode));
-	out_le32(&p->control_regs->reg18.r, 0x1e5);
-	out_le32(&p->control_regs->reg19.r, 0);
-
-	for (i = 0; i < 16; ++i) {
-		controlfb_setcolreg(color_table[i], default_red[i]<<8,
-				    default_grn[i]<<8, default_blu[i]<<8,
-				    0, (struct fb_info *)p);
-	}
-/* Does the above need to be here each time? -- danj */
+	out_le32(CNTRL_REG(p,pitch), par->pitch);
+	out_le32(CNTRL_REG(p,mode), r->mode);
+	out_le32(CNTRL_REG(p,vram_attr), p->vram_attr);
+	out_le32(CNTRL_REG(p,start_addr), par->yoffset * par->pitch
+		 + (par->xoffset << cmode));
+	out_le32(CNTRL_REG(p,rfrcnt), 0x1e5);
+	out_le32(CNTRL_REG(p,intr_ena), 0);
 
 	/* Turn on display */
-	out_le32(&p->control_regs->ctrl.r, ctrl);
-	
+	out_le32(CNTRL_REG(p,ctrl), par->ctrl);
+
 #ifdef CONFIG_FB_COMPAT_XPMAC
 	/* And let the world know the truth. */
 	if (!console_fb_info || console_fb_info == &p->info) {
@@ -663,12 +744,11 @@
 		display_info.width = p->par.xres;
 		display_info.depth = (cmode == CMODE_32) ? 32 :
 			((cmode == CMODE_16) ? 16 : 8);
-		display_info.pitch = p->par.vxres << p->par.cmode;
-		display_info.mode = vmode;
+		display_info.pitch = p->par.pitch;
+		display_info.mode = p->par.vmode;
 		strncpy(display_info.name, "control",
 			sizeof(display_info.name));
-		display_info.fb_address = p->frame_buffer_phys
-			 + control_reg_init[vmode-1]->offset[cmode];
+		display_info.fb_address = p->frame_buffer_phys + CTRLFB_OFF;
 		display_info.cmap_adr_address = p->cmap_regs_phys;
 		display_info.cmap_data_address = p->cmap_regs_phys + 0x30;
 		display_info.disp_reg_address = p->control_regs_phys;
@@ -677,99 +757,173 @@
 #endif /* CONFIG_FB_COMPAT_XPMAC */
 }
 
+
+/*
+ * Called from fbmem.c for probing & intializing
+ */
 int __init control_init(void)
 {
 	struct device_node *dp;
 
 	dp = find_devices("control");
-	if (dp != 0)
-		control_of_init(dp);
-	return 0;
+	if (dp != 0 && !control_of_init(dp))
+		return 0;
+
+	return -ENXIO;
+}
+
+
+/* Work out which banks of VRAM we have installed. */
+/* danj: I guess the card just ignores writes to nonexistant VRAM... */
+
+static void __init find_vram_size(struct fb_info_control *p)
+{
+	int bank1, bank2;
+
+	/*
+	 * Set VRAM in 2MB (bank 1) mode
+	 * VRAM Bank 2 will be accessible through offset 0x600000 if present
+	 * and VRAM Bank 1 will not respond at that offset even if present
+	 */
+	out_le32(CNTRL_REG(p,vram_attr), 0x31);
+
+	out_8(&p->frame_buffer[0x600000], 0xb3);
+	out_8(&p->frame_buffer[0x600001], 0x71);
+	asm volatile("eieio; dcbf 0,%0" : : "r" (&p->frame_buffer[0x600000])
+					: "memory" );
+	mb();
+	asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0x600000])
+					: "memory" );
+	mb();
+
+	bank2 = (in_8(&p->frame_buffer[0x600000]) == 0xb3)
+		&& (in_8(&p->frame_buffer[0x600001]) == 0x71);
+
+	/*
+	 * Set VRAM in 2MB (bank 2) mode
+	 * VRAM Bank 1 will be accessible through offset 0x000000 if present
+	 * and VRAM Bank 2 will not respond at that offset even if present
+	 */
+	out_le32(CNTRL_REG(p,vram_attr), 0x39);
+
+	out_8(&p->frame_buffer[0], 0x5a);
+	out_8(&p->frame_buffer[1], 0xc7);
+	asm volatile("eieio; dcbf 0,%0" : : "r" (&p->frame_buffer[0])
+					: "memory" );
+	mb();
+	asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0])
+					: "memory" );
+	mb();
+
+	bank1 = (in_8(&p->frame_buffer[0]) == 0x5a)
+		&& (in_8(&p->frame_buffer[1]) == 0xc7);
+
+	if (bank2) {
+		if (!bank1) {
+			/*
+			 * vram bank 2 only
+			 */
+			p->control_use_bank2 = 1;
+			p->vram_attr = 0x39;
+			p->frame_buffer += 0x600000;
+			p->frame_buffer_phys += 0x600000;
+		} else {
+			/*
+			 * 4 MB vram
+			 */
+			p->vram_attr = 0x51;
+		}
+	} else {
+		/*
+		 * vram bank 1 only
+		 */
+		p->vram_attr = 0x31;
+	}
+
+        p->total_vram = (bank1 + bank2) * 0x200000;
+
+	printk(KERN_INFO "controlfb: VRAM Total = %dMB "
+			"(%dMB @ bank 1, %dMB @ bank 2)\n",
+			(bank1 + bank2) << 1, bank1 << 1, bank2 << 1);
 }
 
-static void __init control_of_init(struct device_node *dp)
+
+/*
+ * find "control" and initialize
+ */
+static int __init control_of_init(struct device_node *dp)
 {
 	struct fb_info_control	*p;
-	unsigned long		addr, size;
-	int			i, bank1, bank2;
+	unsigned long		addr;
+	int			i;
 
+	if (control_fb) {
+		printk(KERN_ERR "controlfb: only one control is supported\n");
+		return -ENXIO;
+	}
 	if(dp->n_addrs != 2) {
 		printk(KERN_ERR "expecting 2 address for control (got %d)", dp->n_addrs);
-		return;
+		return -ENXIO;
 	}
 	p = kmalloc(sizeof(*p), GFP_ATOMIC);
 	if (p == 0)
-		return;
+		return -ENXIO;
+	control_fb = p;	/* save it for cleanups */
 	memset(p, 0, sizeof(*p));
 
 	/* Map in frame buffer and registers */
 	for (i = 0; i < dp->n_addrs; ++i) {
 		addr = dp->addrs[i].address;
-		size = dp->addrs[i].size;
-		/* Let's assume we can request either all or nothing */
-		if (!request_mem_region(addr, size, "controlfb")) {
-		    kfree(p);
-		    return;
-		}
-		if (size >= 0x800000) {
+		if (dp->addrs[i].size >= 0x800000) {
+			p->fb_orig_base = addr;
+			p->fb_orig_size = dp->addrs[i].size;
 			/* use the big-endian aperture (??) */
-			addr += 0x800000;
-			/* map at most 8MB for the frame buffer */
-			p->frame_buffer_phys = addr;
-			p->frame_buffer = __ioremap(addr, 0x800000, _PAGE_WRITETHRU);
+			p->frame_buffer_phys = addr + 0x800000;
 		} else {
 			p->control_regs_phys = addr;
-			p->control_regs = ioremap(addr, size);
+			p->control_regs_size = dp->addrs[i].size;
 		}
 	}
+
+	if (!p->fb_orig_base ||
+	    !request_mem_region(p->fb_orig_base,p->fb_orig_size,"controlfb")) {
+		p->fb_orig_base = 0;
+		goto error_out;
+	}
+	/* map at most 8MB for the frame buffer */
+	p->frame_buffer = __ioremap(p->frame_buffer_phys, 0x800000,
+				    _PAGE_WRITETHRU);
+
+	if (!p->control_regs_phys ||
+	    !request_mem_region(p->control_regs_phys, p->control_regs_size,
+	    "controlfb regs")) {
+		p->control_regs_phys = 0;
+		goto error_out;
+	}
+	p->control_regs = ioremap(p->control_regs_phys, p->control_regs_size);
+
 	p->cmap_regs_phys = 0xf301b000;	 /* XXX not in prom? */
-	request_mem_region(p->cmap_regs_phys, 0x1000, "controlfb cmap");
+	if (!request_mem_region(p->cmap_regs_phys, 0x1000, "controlfb cmap")) {
+		p->cmap_regs_phys = 0;
+		goto error_out;
+	}
 	p->cmap_regs = ioremap(p->cmap_regs_phys, 0x1000);
 
-	/* Work out which banks of VRAM we have installed. */
-	/* According to Andrew Fyfe <bandr@best.com>, the VRAM behaves like so: */
-	/* afyfe: observations from an 8500:
-	 * - with 2M vram in bank 1, it appears at offsets 0, 2M and 4M
-	 * - with 2M vram in bank 2, it appears only at offset 6M
-	 * - with 4M vram, it appears only as a 4M block at offset 0.
-	 */
+	if (!p->cmap_regs || !p->control_regs || !p->frame_buffer)
+		goto error_out;
 
-	/* We know there is something at 2M if there is something at 0M. */
-	out_8(&p->frame_buffer[0x200000], 0xa5);
-	out_8(&p->frame_buffer[0x200001], 0x38);
-	asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0x200000]) : "memory" );
+	find_vram_size(p);
+	if (!p->total_vram)
+		goto error_out;
 
-	out_8(&p->frame_buffer[0], 0x5a);
-	out_8(&p->frame_buffer[1], 0xc7);
-	asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0]) : "memory" );
+	if (init_control(p) < 0)
+		goto error_out;
 
-	bank1 =  (in_8(&p->frame_buffer[0x000000]) == 0x5a)
-		&& (in_8(&p->frame_buffer[0x000001]) == 0xc7);
-	bank2 =  (in_8(&p->frame_buffer[0x200000]) == 0xa5)
-		&& (in_8(&p->frame_buffer[0x200001]) == 0x38);
-
-	if(bank2 && !bank1)
-		printk(KERN_INFO "controlfb: Found memory at 2MB but not at 0!  Please contact dan@debian.org\n");
-
-	if(!bank1) {
-		out_8(&p->frame_buffer[0x600000], 0xa5);
-		out_8(&p->frame_buffer[0x600001], 0x38);
-		asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0x600000]) : "memory" );
-		bank2 = (in_8(&p->frame_buffer[0x600000]) == 0xa5)
-			&& (in_8(&p->frame_buffer[0x600001]) == 0x38);
-		/* If we don't have bank 1 installed, we hope we have bank 2 :-) */
-		p->control_use_bank2 = 1;
-		p->frame_buffer += 0x600000;
-		p->frame_buffer_phys += 0x600000;
-	}
-	
-	p->total_vram = (bank1 + bank2) * 0x200000;
-	
-	printk(KERN_INFO "controlfb: Memory bank 1 %s, bank 2 %s, total VRAM %dMB\n",
-		bank1 ? "present" : "absent", bank2 ? "present" : "absent",
-		2 * (bank1 + bank2));
+	return 0;
 
-	init_control(p);
+error_out:
+	control_cleanup();
+	return -ENXIO;
 }
 
 /*
@@ -781,181 +935,234 @@
 {
 	int sense;
 
-	out_le32(&p->control_regs->mon_sense.r, 7);	/* drive all lines high */
+	out_le32(CNTRL_REG(p,mon_sense), 7);	/* drive all lines high */
 	__delay(200);
-	out_le32(&p->control_regs->mon_sense.r, 077);	/* turn off drivers */
+	out_le32(CNTRL_REG(p,mon_sense), 077);	/* turn off drivers */
 	__delay(2000);
-	sense = (in_le32(&p->control_regs->mon_sense.r) & 0x1c0) << 2;
+	sense = (in_le32(CNTRL_REG(p,mon_sense)) & 0x1c0) << 2;
 
 	/* drive each sense line low in turn and collect the other 2 */
-	out_le32(&p->control_regs->mon_sense.r, 033);	/* drive A low */
+	out_le32(CNTRL_REG(p,mon_sense), 033);	/* drive A low */
 	__delay(2000);
-	sense |= (in_le32(&p->control_regs->mon_sense.r) & 0xc0) >> 2;
-	out_le32(&p->control_regs->mon_sense.r, 055);	/* drive B low */
+	sense |= (in_le32(CNTRL_REG(p,mon_sense)) & 0xc0) >> 2;
+	out_le32(CNTRL_REG(p,mon_sense), 055);	/* drive B low */
 	__delay(2000);
-	sense |= ((in_le32(&p->control_regs->mon_sense.r) & 0x100) >> 5)
-		| ((in_le32(&p->control_regs->mon_sense.r) & 0x40) >> 4);
-	out_le32(&p->control_regs->mon_sense.r, 066);	/* drive C low */
+	sense |= ((in_le32(CNTRL_REG(p,mon_sense)) & 0x100) >> 5)
+		| ((in_le32(CNTRL_REG(p,mon_sense)) & 0x40) >> 4);
+	out_le32(CNTRL_REG(p,mon_sense), 066);	/* drive C low */
 	__delay(2000);
-	sense |= (in_le32(&p->control_regs->mon_sense.r) & 0x180) >> 7;
+	sense |= (in_le32(CNTRL_REG(p,mon_sense)) & 0x180) >> 7;
 
-	out_le32(&p->control_regs->mon_sense.r, 077);	/* turn off drivers */
+	out_le32(CNTRL_REG(p,mon_sense), 077);	/* turn off drivers */
 	
 	return sense;
 }
 
-/***********************  Various translation functions  ***********************/
-#if 1
-/* This routine takes a user-supplied var, and picks the best vmode/cmode from it. */
-static int control_var_to_par(struct fb_var_screeninfo *var,
-	struct fb_par_control *par, const struct fb_info *fb_info)
-{
-	int xres = var->xres;
-	int yres = var->yres;
-	int bpp = var->bits_per_pixel;
-	struct fb_info_control *p = (struct fb_info_control *) fb_info;
+/**********************  Various translation functions  **********************/
 
-    /*
-     *  Get the video params out of 'var'. If a value doesn't fit, round it up,
-     *  if it's too big, return -EINVAL.
-     *
-     *  Suggestion: Round up in the following order: bits_per_pixel, xres,
-     *  yres, xres_virtual, yres_virtual, xoffset, yoffset, grayscale,
-     *  bitfields, horizontal timing, vertical timing.
-     */
-	/* swiped by jonh from atyfb.c */
-	if (xres <= 640 && yres <= 480)
-		par->vmode = VMODE_640_480_67;		/* 640x480, 67Hz */
-	else if (xres <= 640 && yres <= 870)
-		par->vmode = VMODE_640_870_75P;		/* 640x870, 75Hz (portrait) */
-	else if (xres <= 800 && yres <= 600)
-		par->vmode = VMODE_800_600_75;		/* 800x600, 75Hz */
-	else if (xres <= 832 && yres <= 624)
-		par->vmode = VMODE_832_624_75;		/* 832x624, 75Hz */
-	else if (xres <= 1024 && yres <= 768)
-		par->vmode = VMODE_1024_768_75;		/* 1024x768, 75Hz */
-	else if (xres <= 1152 && yres <= 870)
-		par->vmode = VMODE_1152_870_75;		/* 1152x870, 75Hz */
-	else if (xres <= 1280 && yres <= 960)
-		par->vmode = VMODE_1280_960_75;		/* 1280x960, 75Hz */
-	else if (xres <= 1280 && yres <= 1024)
-		par->vmode = VMODE_1280_1024_75;	/* 1280x1024, 75Hz */
-	else {
-		printk(KERN_ERR "Bad x/y res in var_to_par\n");
-		return -EINVAL;
-	}
+#define CONTROL_PIXCLOCK_BASE	256016
+#define CONTROL_PIXCLOCK_MIN	5000	/* ~ 200 MHz dot clock */
 
-	xres = control_reg_init[par->vmode-1]->hres;
-	yres = control_reg_init[par->vmode-1]->vres;
+/*
+ * calculate the clock paramaters to be sent to CUDA according to given
+ * pixclock in pico second.
+ */
+static int calc_clock_params(unsigned long clk, unsigned char *param)
+{
+	unsigned long p0, p1, p2, k, l, m, n, min;
 
-	par->xres = xres;
-	par->yres = yres;
+	if (clk > (CONTROL_PIXCLOCK_BASE << 3))
+		return 1;
 
-	if (var->xres_virtual <= xres)
-		par->vxres = xres;
-	else if(var->xres_virtual > xres) {
-		par->vxres = xres;
-	} else	/* NotReached at present */
-		par->vxres = (var->xres_virtual+7) & ~7;
+	p2 = ((clk << 4) < CONTROL_PIXCLOCK_BASE)? 3: 2;
+	l = clk << p2;
+	p0 = 0;
+	p1 = 0;
+	for (k = 1, min = l; k < 32; k++) {
+		unsigned long rem;
+
+		m = CONTROL_PIXCLOCK_BASE * k;
+		n = m / l;
+		rem = m % l;
+		if (n && (n < 128) && rem < min) {
+			p0 = k;
+			p1 = n;
+			min = rem;
+		}
+	}
+	if (!p0 || !p1)
+		return 1;
 
-	if (var->yres_virtual <= yres)
-		par->vyres = yres;
-	else
-		par->vyres = var->yres_virtual;
+	param[0] = p0;
+	param[1] = p1;
+	param[2] = p2;
 
-	if (var->xoffset > 0 || var->yoffset+yres > par->vyres) {
-		printk(KERN_ERR "Bad offsets in var_to_par\n");
-		return -EINVAL;
-	}
+	return 0;
+}
 
-	par->xoffset = (var->xoffset+7) & ~7;
-	par->yoffset = var->yoffset;
 
+/*
+ * This routine takes a user-supplied var, and picks the best vmode/cmode
+ * from it.
+ */
 
- 	if (bpp <= 8)
+static int control_var_to_par(struct fb_var_screeninfo *var,
+	struct fb_par_control *par, const struct fb_info *fb_info)
+{
+	int cmode, piped_diff, hstep;
+	unsigned hperiod, hssync, hsblank, hesync, heblank, piped, heq, hlfln,
+		 hserr, vperiod, vssync, vesync, veblank, vsblank, vswin, vewin;
+	unsigned long pixclock;
+	struct fb_info_control *p = (struct fb_info_control *) fb_info;
+	struct control_regvals *r = &par->regvals;
+
+	switch (var->bits_per_pixel) {
+	case 8:
 		par->cmode = CMODE_8;
-	else if (bpp <= 16)
+		if (p->total_vram > 0x200000) {
+			r->mode = 3;
+			r->radacal_ctrl = 0x20;
+			piped_diff = 13;
+		} else {
+			r->mode = 2;
+			r->radacal_ctrl = 0x10;
+			piped_diff = 9;
+		}
+		break;
+	case 15:
+	case 16:
 		par->cmode = CMODE_16;
-	else if (bpp <= 32)
+		if (p->total_vram > 0x200000) {
+			r->mode = 2;
+			r->radacal_ctrl = 0x24;
+			piped_diff = 5;
+		} else {
+			r->mode = 1;
+			r->radacal_ctrl = 0x14;
+			piped_diff = 3;
+		}
+		break;
+	case 32:
 		par->cmode = CMODE_32;
-	else {
-		printk(KERN_ERR "Bad bpp in var_to_par\n");
+		if (p->total_vram > 0x200000) {
+			r->mode = 1;
+			r->radacal_ctrl = 0x28;
+		} else {
+			r->mode = 0;
+			r->radacal_ctrl = 0x18;
+		}
+		piped_diff = 1;
+		break;
+	default:
 		return -EINVAL;
 	}
 
-	if (control_vram_reqd(par->vmode, par->cmode) > p->total_vram) {
-		printk(KERN_ERR "Too much VRAM required for vmode %d cmode %d.\n", par->vmode, par->cmode);
-		return -EINVAL;
-	}
+	/*
+	 * adjust xres and vxres so that the corresponding memory widths are
+	 * 32-byte aligned
+	 */
+	hstep = 31 >> par->cmode;
+	par->xres = (var->xres + hstep) & ~hstep;
+	par->vxres = (var->xres_virtual + hstep) & ~hstep;
+	par->xoffset = (var->xoffset + hstep) & ~hstep;
+	if (par->vxres < par->xres)
+		par->vxres = par->xres;
+	par->pitch = par->vxres << par->cmode;
 
-	/* Check if we know about the wanted video mode */
-	if (control_reg_init[par->vmode - 1] == NULL) {
-		printk(KERN_ERR "init is null in control_var_to_par().\n");
-		/* I'm not sure if control has any specific requirements --	*/
-		/* if we have a regvals struct, we're good to go?		*/
-		return -EINVAL;
-	}
+	par->yres = var->yres;
+	par->vyres = var->yres_virtual;
+	par->yoffset = var->yoffset;
+	if (par->vyres < par->yres)
+		par->vyres = par->yres;
 
-	return 0;
-}
-#else
-/* This routine takes a user-supplied var, and picks the best vmode/cmode from it. */
-static int control_var_to_par(struct fb_var_screeninfo *var,
-	struct fb_par_control *par, const struct fb_info *fb_info)
-{
-	struct fb_info_control *p = (struct fb_info_control *) fb_info;
-	
-	if(mac_var_to_vmode(var, &par->vmode, &par->cmode) != 0)
-		return -EINVAL;
-	par->xres = par->vxres = vmode_attrs[par->vmode - 1].hres;
-	par->yres = par->vyres = vmode_attrs[par->vmode - 1].vres;
-	par->xoffset = par->yoffset = 0;
-	
-	if (control_vram_reqd(par->vmode, par->cmode) > p->total_vram)
+	par->sync = var->sync;
+
+	if (par->pitch * par->vyres + CTRLFB_OFF > p->total_vram)
 		return -EINVAL;
 
-	/* Check if we know about the wanted video mode */
-	if(!control_reg_init[par->vmode-1]) {
-		/* I'm not sure if control has any specific requirements --	*/
-		/* if we have a regvals struct, we're good to go?		*/
+	if (par->xoffset + par->xres > par->vxres)
+		par->xoffset = par->vxres - par->xres;
+	if (par->yoffset + par->yres > par->vyres)
+		par->yoffset = par->vyres - par->yres;
+
+	pixclock = (var->pixclock < CONTROL_PIXCLOCK_MIN)? CONTROL_PIXCLOCK_MIN:
+		   var->pixclock;
+	if (calc_clock_params(pixclock, r->clock_params))
 		return -EINVAL;
-	}
+
+	hperiod = ((var->left_margin + par->xres + var->right_margin
+		    + var->hsync_len) >> 1) - 2;
+	hssync = hperiod + 1;
+	hsblank = hssync - (var->right_margin >> 1);
+	hesync = (var->hsync_len >> 1) - 1;
+	heblank = (var->left_margin >> 1) + hesync;
+	piped = heblank - piped_diff;
+	heq = var->hsync_len >> 2;
+	hlfln = (hperiod+2) >> 1;
+	hserr = hssync-hesync;
+	vperiod = (var->vsync_len + var->lower_margin + par->yres
+		   + var->upper_margin) << 1;
+	vssync = vperiod - 2;
+	vesync = (var->vsync_len << 1) - vperiod + vssync;
+	veblank = (var->upper_margin << 1) + vesync;
+	vsblank = vssync - (var->lower_margin << 1);
+	vswin = (vsblank+vssync) >> 1;
+	vewin = (vesync+veblank) >> 1;
+
+	r->regs[0] = vswin;
+	r->regs[1] = vsblank;
+	r->regs[2] = veblank;
+	r->regs[3] = vewin;
+	r->regs[4] = vesync;
+	r->regs[5] = vssync;
+	r->regs[6] = vperiod;
+	r->regs[7] = piped;
+	r->regs[8] = hperiod;
+	r->regs[9] = hsblank;
+	r->regs[10] = heblank;
+	r->regs[11] = hesync;
+	r->regs[12] = hssync;
+	r->regs[13] = heq;
+	r->regs[14] = hlfln;
+	r->regs[15] = hserr;
+
+	if (par->xres >= 1280 && par->cmode >= CMODE_16)
+		par->ctrl = 0x7f;
+	else
+		par->ctrl = 0x3b;
+
+	if (mac_var_to_vmode(var, &par->vmode, &cmode))
+		par->vmode = 0;
+
 	return 0;
 }
-#endif
 
-/***********  Convert hardware data in par to an fb_var_screeninfo ***********/
+
+/*
+ * Convert hardware data in par to an fb_var_screeninfo
+ */
 
 static void control_par_to_var(struct fb_par_control *par, struct fb_var_screeninfo *var)
 {
 	struct control_regints *rv;
 	
-	rv = (struct control_regints *) control_reg_init[par->vmode - 1]->regs;
+	rv = (struct control_regints *) par->regvals.regs;
 	
 	memset(var, 0, sizeof(*var));
-	var->xres = control_reg_init[par->vmode - 1]->hres;
-	var->yres = control_reg_init[par->vmode - 1]->vres;
+	var->xres = par->xres;
+	var->yres = par->yres;
 	var->xres_virtual = par->vxres;
 	var->yres_virtual = par->vyres;
 	var->xoffset = par->xoffset;
 	var->yoffset = par->yoffset;
-	var->grayscale = 0;
 	
-	if(par->cmode != CMODE_8 && par->cmode != CMODE_16 && par->cmode != CMODE_32) {
-		printk(KERN_ERR "Bad color mode in control_par_to_var()!\n");
-		par->cmode = CMODE_8;
-	}
 	switch(par->cmode) {
+	default:
 	case CMODE_8:
 		var->bits_per_pixel = 8;
-		var->red.offset = 0;
 		var->red.length = 8;
-		var->green.offset = 0;
 		var->green.length = 8;
-		var->blue.offset = 0;
 		var->blue.length = 8;
-		var->transp.offset = 0;
-		var->transp.length = 0;
 		break;
 	case CMODE_16:	/* RGB 555 */
 		var->bits_per_pixel = 16;
@@ -963,10 +1170,7 @@
 		var->red.length = 5;
 		var->green.offset = 5;
 		var->green.length = 5;
-		var->blue.offset = 0;
 		var->blue.length = 5;
-		var->transp.offset = 0;
-		var->transp.length = 0;
 		break;
 	case CMODE_32:	/* RGB 888 */
 		var->bits_per_pixel = 32;
@@ -974,67 +1178,41 @@
 		var->red.length = 8;
 		var->green.offset = 8;
 		var->green.length = 8;
-		var->blue.offset = 0;
 		var->blue.length = 8;
 		var->transp.offset = 24;
 		var->transp.length = 8;
 		break;
 	}
-	var->red.msb_right = 0;
-	var->green.msb_right = 0;
-	var->blue.msb_right = 0;
-	var->transp.msb_right = 0;
-	var->nonstd = 0;
-	var->activate = 0;
 	var->height = -1;
 	var->width = -1;
 	var->vmode = FB_VMODE_NONINTERLACED;
 
-	var->left_margin = (rv->heblank - rv->hesync)
-		<< ((par->vmode > 18) ? 2 : 1);
-	var->right_margin = (rv->hssync - rv->hsblank)
-		<< ((par->vmode > 18) ? 2 : 1);
-	var->hsync_len = (rv->hperiod + 2 - rv->hssync + rv->hesync)
-		<< ((par->vmode > 18) ? 2 : 1);
+	var->left_margin = (rv->heblank - rv->hesync) << 1;
+	var->right_margin = (rv->hssync - rv->hsblank) << 1;
+	var->hsync_len = (rv->hperiod + 2 - rv->hssync + rv->hesync) << 1;
 
 	var->upper_margin = (rv->veblank - rv->vesync) >> 1;
 	var->lower_margin = (rv->vssync - rv->vsblank) >> 1;
 	var->vsync_len = (rv->vperiod - rv->vssync + rv->vesync) >> 1;
 
-	/* Acording to macmodes.c... */
-	if((par->vmode >= 9 && par->vmode <= 12) ||
-	   (par->vmode >= 16 && par->vmode <= 18) ||
-	   (par->vmode == 20))
-	{
-		var->sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT;
-	} else {
-		var->sync = 0;          /* I suppose */
-	}
+	var->sync = par->sync;
 
-    /* The reason these are both here: with my revised margin calculations, */
-    /* these SHOULD both give the same answer for each mode.  Some day I    */
-    /* will sit down and check the rest.  Works perfectly for vmode 13.     */
-
-#if 0
-/* jonh's pixclocks...*/
-	/* no long long support in the kernel :-( */
-	/* this splittig trick will work if xres > 232 */
-	var->pixclock = 1000000000/
-	(var->left_margin+var->xres+var->right_margin+var->hsync_len);
-	var->pixclock *= 1000;
-	var->pixclock /= vmode_attrs[par->vmode-1].vfreq*
-	 (var->upper_margin+var->yres+var->lower_margin+var->vsync_len);
-#else
-/* danj's */
-	/* 10^12 * clock_params[0] / (3906400 * clock_params[1] * 2^clock_params[2]) */
-	/* (10^12 * clock_params[0] / (3906400 * clock_params[1])) >> clock_params[2] */
+	/*
+	 * 10^12 * clock_params[0] / (3906400 * clock_params[1]
+	 *			      * 2^clock_params[2])
+	 * (10^12 * clock_params[0] / (3906400 * clock_params[1]))
+	 * >> clock_params[2]
+	 */
 	/* (255990.17 * clock_params[0] / clock_params[1]) >> clock_params[2] */
-	var->pixclock = 255990 * control_reg_init[par->vmode-1]->clock_params[0];
-	var->pixclock /= control_reg_init[par->vmode-1]->clock_params[1];
-	var->pixclock >>= control_reg_init[par->vmode-1]->clock_params[2];
-#endif
+	var->pixclock = CONTROL_PIXCLOCK_BASE * par->regvals.clock_params[0];
+	var->pixclock /= par->regvals.clock_params[1];
+	var->pixclock >>= par->regvals.clock_params[2];
 }
 
+
+/*
+ * init fix according to given par
+ */
 static void control_par_to_fix(struct fb_par_control *par, struct fb_fix_screeninfo *fix,
 	struct fb_info_control *p)
 {
@@ -1044,52 +1222,41 @@
 	fix->mmio_len = sizeof(struct control_regs);
 	fix->type = FB_TYPE_PACKED_PIXELS;
 	
+	fix->xpanstep = 32 >> par->cmode;
 	fix->ypanstep = 1;
-	/*
-		fix->type_aux = 0;
-		fix->ywrapstep = 0;
-		fix->ypanstep = 0;
-		fix->xpanstep = 0;
-	*/
-
-	fix->smem_start = (p->frame_buffer_phys
-		+ control_reg_init[par->vmode-1]->offset[par->cmode]);
-	fix->smem_len = p->total_vram - control_reg_init[par->vmode-1]->offset[par->cmode];
+
+	fix->smem_start = p->frame_buffer_phys + CTRLFB_OFF;
+	fix->smem_len = p->total_vram - CTRLFB_OFF;
 	fix->visual = (par->cmode == CMODE_8) ?
 		FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_DIRECTCOLOR;
-	fix->line_length = par->vxres << par->cmode;
+	fix->line_length = par->pitch;
 }
 
-/* We never initialize any display except for p->disp.
-   And p->disp is already memset to 0.  So no memset here.
-   [Found by Takashi Oe]
-*/
+/*
+ * initialize a portion of struct display which low level driver is responsible
+ * for.
+ */
 static void control_par_to_display(struct fb_par_control *par,
   struct display *disp, struct fb_fix_screeninfo *fix, struct fb_info_control *p)
 {
-	/* memset(disp, 0, sizeof(*disp)); */
 	disp->type = fix->type;
 	disp->can_soft_blank = 1;
 	disp->scrollmode = SCROLL_YNOMOVE | SCROLL_YNOPARTIAL;
 	disp->ypanstep = fix->ypanstep;
 	disp->ywrapstep = fix->ywrapstep;
-#if 0
-		disp->type_aux = fix->type_aux;
-		disp->cmap.red = NULL;	/* ??? danj */
-		disp->cmap.green = NULL;
-		disp->cmap.blue = NULL;
-		disp->cmap.transp = NULL;
-			/* Yeah, I realize I just set 0 = 0. */
-#endif
 
 	control_par_to_var(par, &disp->var);
-	disp->screen_base = (char *) p->frame_buffer
-		 + control_reg_init[par->vmode-1]->offset[par->cmode];
+	disp->screen_base = (char *) p->frame_buffer + CTRLFB_OFF;
 	disp->visual = fix->visual;
 	disp->line_length = fix->line_length;
 	control_set_dispsw(disp, par->cmode, p);
 }
 
+
+/*
+ * our own _revc() routines since generic routines don't work for DIRECT Color
+ * devices like control
+ */
 static void control_cfb16_revc(struct display *p, int xx, int yy)
 {
     u8 *dest;
@@ -1164,6 +1331,9 @@
 };
 
 
+/*
+ * Set struct dispsw according to given cmode
+ */
 static void control_set_dispsw(struct display *disp, int cmode, struct fb_info_control *p)
 {
 	switch (cmode) {
@@ -1190,6 +1360,10 @@
 	}
 }
 
+
+/*
+ * Set misc info vars for this driver
+ */
 static void __init control_init_info(struct fb_info *info, struct fb_info_control *p)
 {
 	strcpy(info->modename, "control");
@@ -1203,7 +1377,36 @@
 	info->blank = &controlfb_blank;
 }
 
-/* Parse user speficied options (`video=controlfb:') */
+
+static void control_cleanup(void)
+{
+	struct fb_info_control	*p = control_fb;
+
+	if (!p)
+		return;
+
+	if (p->cmap_regs)
+		iounmap(p->cmap_regs);
+	if (p->control_regs)
+		iounmap(p->control_regs);
+	if (p->frame_buffer) {
+		if (p->control_use_bank2)
+			p->frame_buffer -= 0x600000;
+		iounmap(p->frame_buffer);
+	}
+	if (p->cmap_regs_phys)
+		release_mem_region(p->cmap_regs_phys, 0x1000);
+	if (p->control_regs_phys)
+		release_mem_region(p->control_regs_phys, p->control_regs_size);
+	if (p->fb_orig_base)
+		release_mem_region(p->fb_orig_base, p->fb_orig_size);
+	kfree(p);
+}
+
+
+/*
+ * Parse user speficied options (`video=controlfb:')
+ */
 void __init control_setup(char *options)
 {
 	char *this_opt;
@@ -1223,11 +1426,11 @@
 					break;
 			memcpy(fontname, this_opt + 5, i);
 			fontname[i] = 0;
-		}
-		if (!strncmp(this_opt, "vmode:", 6)) {
+		} else if (!strncmp(this_opt, "vmode:", 6)) {
 			int vmode = simple_strtoul(this_opt+6, NULL, 0);
-		if (vmode > 0 && vmode <= VMODE_MAX)
-			default_vmode = vmode;
+			if (vmode > 0 && vmode <= VMODE_MAX &&
+			    control_mac_modes[vmode - 1].m[1] >= 0)
+				default_vmode = vmode;
 		} else if (!strncmp(this_opt, "cmode:", 6)) {
 			int depth = simple_strtoul(this_opt+6, NULL, 0);
 			switch (depth) {
@@ -1252,20 +1455,3 @@
 	}
 }
 
-#if 0
-static int controlfb_pan_display(struct fb_var_screeninfo *var,
-			   struct controlfb_par *par,
-			   const struct fb_info *fb_info)
-{
-    /*
-     *  Pan (or wrap, depending on the `vmode' field) the display using the
-     *  `xoffset' and `yoffset' fields of the `var' structure.
-     *  If the values don't fit, return -EINVAL.
-     */
-
-	FUNCID;
-
-    return 0;
-}
-
-#endif